文章目录

  • 1. 问题建模
  • 2. QP问题
  • 3. 格式转换
    • 构建 P P P q q q矩阵
    • 等式约束
  • 4. 使用OSQP求解问题

1. 问题建模

  假设已知一条参考轨迹为 r 0 , r 1 , . . . , r i , . . . , r n r_0,r_1,...,r_i,...,r_n r0,r1,...,ri,...,rn,这条轨迹可以任意生成,因为其中可能有直角拐点或者锐角拐点,机器人无法直接沿着该轨迹移动。因此,需要生成一条满足机器人运动学约束、尽可能平滑的实际可行的轨迹 x 0 , x 1 , . . . , x i , . . . x n x_0,x_1,...,x_i,...x_n x0,x1,...,xi,...xn,这也是我们需要优化的变量,这里轨迹 { r i } \{r_i\} {ri} { x i } \{x_i\} {xi}具有相同的长度。由此建立下面的代价函数( cost   function \textbf{cost function} cost function ):

J = w x ∑ i = 0 n − 1 ( x i − r i ) 2 + w x ′ ∑ i = 0 n − 1 ( x i ′ ) 2 + w x ′ ′ ∑ i = 0 n − 1 ( x i ′ ′ ) 2 + w x ′ ′ ′ ∑ i = 0 n − 1 ( x i ′ ′ ′ ) 2 \mathcal{J}=w_x\sum^{n-1}_{i=0}(x_i-r_i)^2+w_{x'}\sum^{n-1}_{i=0}(x'_i)^2+w_{x''}\sum^{n-1}_{i=0}(x''_i)^2+w_{x'''}\sum^{n-1}_{i=0}(x'''_i)^2 J=wxi=0n1(xiri)2+wxi=0n1(xi)2+wxi=0n1(xi)2+wxi=0n1(xi)2

  • w x ∑ i = 0 n − 1 ( x i − r i ) 2 w_x\sum^{n-1}_{i=0}(x_i-r_i)^2 wxi=0n1(xiri)2:最小化生成轨迹和参考轨迹之间的距离,也即让生成轨迹尽可能的沿着参考轨迹,其中 w x w_x wx为权重。
  • w x ′ ∑ i = 0 n − 1 ( x i ′ ) 2 w_{x'}\sum^{n-1}_{i=0}(x'_i)^2 wxi=0n1(xi)2:最小化轨迹的一阶导,其中 w x ′ w_x' wx为权重。通俗点讲, { x } \{x\} {x}为轨迹时, { x ′ } \{x'\} {x}为速度,这一项的目的是最小化速度。就像开车的时候,沿直线运动轨迹最短,能耗最小,乱打方向盘会让轨迹扭曲,加大能耗和移动时间。
  • w x ∑ i = 0 n − 1 ( x i − r i ) 2 w_x\sum^{n-1}_{i=0}(x_i-r_i)^2 wxi=0n1(xiri)2:最小化轨迹的二阶导,其中 w x ′ ′ w_x'' wx为权重。通俗点讲,就是最小化加速度。
  • w x ∑ i = 0 n − 1 ( x i − r i ) 2 w_x\sum^{n-1}_{i=0}(x_i-r_i)^2 wxi=0n1(xiri)2:最小化轨迹的三阶导,其中 w x ′ ′ ′ w_x''' wx为权重。通俗点讲,就是最小化加加速度(专业术语应该叫$\textbf{Jerk})。

2. QP问题

  常见格式如下:

  其中 P {P} P是一个 n x n nxn nxn矩阵, x x x n n n维向量, q q q m x n mxn mxn的矩阵。

  为什么要转换成QP格式?

  因为QP问题一定是可求解的,而且目前各种求解器都支持求解QP问题,速度相对于非线性优化问题快了很多很多很多。

3. 格式转换

  下面将我们的问题转换成QP格式,主要就是构建 P \mathcal{P} P矩阵和 q \mathcal{q} q矩阵,下面的内容主要参考这篇文章规划控制:Piecewise Jerk Path Optimizer的python实现。这里将参考文章种的一维拓展到二维,即 x = { x 0 , x 1 , . . . , x n } x=\{x_0,x_1, ..., x_n\} x={x0,x1,...,xn},其中每个状态 x i = { p x i , p y i , v x i , v y i , a x i , a y i } x_i=\{p_{xi},p_{yi},v_{xi},v_{yi},a_{xi},a_{yi}\} xi={pxi,pyi,vxi,vyi,axi,ayi},分别对应在 x y xy xy轴上的位置 p x p_x px p y p_y py、速度 v x v_x vx v y v_y vy和加速度 a x a_x ax a y a_y ay

  为了简化QP问题的规模,这里假设优化变量 x x x包含 n n n个状态(分别对应参考轨迹 r r r的长度),且满足如下关系

  • x i ′ = x i + 1 − x i Δ t x'_i=\frac{x_{i+1}-x_i}{\Delta{t}} xi=Δtxi+1xi
  • x i ′ ′ = x i + 1 ′ − x i ′ Δ t x''_i=\frac{x'_{i+1}-x'_i}{\Delta{t}} xi=Δtxi+1xi
  • x i ′ ′ ′ = x i + 1 ′ ′ − x i ′ ′ Δ t x'''_i=\frac{x''_{i+1}-x''_i}{\Delta{t}} xi=Δtxi+1xi

  将上面最后一个公式带入代价函数 J \mathcal{J} J中,并将其展开可得:
J = w x ∑ i = 0 n − 1 [ ( x i ) 2 + ( r i ) 2 − 2 x i r i ] + w x ′ ∑ i = 0 n − 1 ( x i ′ ) 2 + w x ′ ′ ∑ i = 0 n − 1 ( x i ′ ′ ) 2 + w x ′ ′ ′ Δ s 2 ∑ i = 0 n − 2 [ ( x i + 1 ′ ′ ) 2 + ( x i ′ ′ ) 2 − 2 x i + 1 ′ ′ x i ′ ′ ] \mathcal{J}=w_x\sum^{n-1}_{i=0}[(x_i)^2+(r_i)^2-2x_ir_i]+w_{x'}\sum^{n-1}_{i=0}(x'_i)^2+w_{x''}\sum^{n-1}_{i=0}(x''_i)^2+\frac{w_{x'''}}{\Delta s^2}\sum^{n-2}_{i=0}[(x''_{i+1})^2+(x''_i)^2-2x''_{i+1}x''_i] J=wxi=0n1[(xi)2+(ri)22xiri]+wxi=0n1(xi)2+wxi=0n1(xi)2+Δs2wxi=0n2[(xi+1)2+(xi)22xi+1xi]

去掉其中的常数项(和优化变量 x x x无关的项)可得:

J = w x ∑ i = 0 n − 1 [ ( x i ) 2 − 2 x i r i ] + w x ′ ∑ i = 0 n − 1 ( x i ′ ) 2 + ( w x ′ ′ + 2 w x ′ ′ ′ Δ s 2 ) ∑ i = 0 n − 1 ( x i ′ ′ ) 2 − w x ′ ′ ′ Δ s 2 ( x 0 ′ ′ + x n − 1 ′ ′ ) − 2 w x ′ ′ ′ Δ s 2 ∑ i = 0 n − 2 ( x i + 1 ′ ′ x i ′ ′ ) \mathcal{J}=w_x\sum^{n-1}_{i=0}[(x_i)^2-2x_ir_i]+w_{x'}\sum^{n-1}_{i=0}(x'_i)^2+(w_{x''}+\frac{2w_{x'''}}{\Delta s^2})\sum^{n-1}_{i=0}(x''_i)^2-\frac{w_{x'''}}{\Delta s^2}(x''_0+x''_{n-1})-\frac{2w_{x'''}}{\Delta s^2}\sum^{n-2}_{i=0}(x''_{i+1}x''_i) J=wxi=0n1[(xi)22xiri]+wxi=0n1(xi)2+(wx+Δs22wx)i=0n1(xi)2Δs2wx(x0+xn1)Δs22wxi=0n2(xi+1xi)

其中

构建 P P P q q q矩阵

等式约束

  为了让生成的轨迹满足机器人的移动状态,需要将机器人的运动模型转换成对应的等式约束,即 l = A x = u l =Ax = u l=Ax=u。这里考虑如下运动模型:

转换成对应矩阵形式,此处以2维空间为例,即 x = { x 0 , x 1 , . . . , x n } x=\{x_0,x_1, ..., x_n\} x={x0,x1,...,xn},其中每个状态 x i = { p x i , p y i , v x i , v y i , a x i , a y i } x_i=\{p_{xi},p_{yi},v_{xi},v_{yi},a_{xi},a_{yi}\} xi={pxi,pyi,vxi,vyi,axi,ayi},分别对应在 x y xy xy轴上的位置 p x p_x px p y p_y py、速度 v x v_x vx v y v_y vy和加速度 a x a_x ax a y a_y ay

4. 使用OSQP求解问题

  效果如下,红色点是设置的路径点,灰色是b样条拟合的路径,绿色是优化的结果。

  直接上代码

import osqp
import numpy as np
import time
from scipy.interpolate import *
from scipy import sparse
import matplotlib.pyplot as plt


class PathOptimizer:
    def __init__(self, points, ts, acc, d='2d'):
        self.ctrl_points = points
        self.ctrl_ts = ts
        self.acc_max = acc
        self.dimension = d

        # 时间分配
        self.seg_length = self.acc_max * self.ctrl_ts * self.ctrl_ts
        self.total_length = 0
        for i in range(self.ctrl_points.shape[0] - 1):
            self.total_length += np.linalg.norm(self.ctrl_points[i + 1, :] - self.ctrl_points[i, :])
        self.total_time = self.total_length/self.seg_length * self.ctrl_ts * 1.2

        # 生成控制点B样条函数
        time_list = np.linspace(0, self.total_time, self.ctrl_points.shape[0])
        self.px_spline = InterpolatedUnivariateSpline(time_list, self.ctrl_points[:, 0])
        self.py_spline = InterpolatedUnivariateSpline(time_list, self.ctrl_points[:, 1])

        # 对B样条优化参考轨迹进行均匀采样
        self.seg_time_list = np.arange(0, self.total_time, self.ctrl_ts)
        self.px = self.px_spline(self.seg_time_list)
        self.py = self.py_spline(self.seg_time_list)

        # 优化问题权重参数
        self.weight_pos = 5
        self.weight_vel = 1
        self.weight_acc = 1
        self.weight_jerk = 0.1

        start_time = time.perf_counter()
        self.smooth_path = self.osqpSmooth()
        print('OSQP in ' + self.dimension + ' Cost time: ' + str(time.perf_counter() - start_time))
        pass

    def osqpSmooth(self):
        px0, py0 = self.px, self.py
        vx0 = vy0 = vz0 = np.zeros(self.px.shape)
        ax0 = ay0 = az0 = np.zeros(self.px.shape)

        # x(0), x(1), ..., x(n - 1), x'(0), x'(1), ..., x'(n - 1), x"(0), x"(1), ..., x"(n - 1)
        x0_total = np.hstack((px0, py0, vx0, vy0, ax0, ay0))


        n = int(x0_total.shape[0] / 3)   # 每种级别状态有几个变量,即x共n个,x'共n个,x"也是n个
        Q_x = self.weight_pos * np.eye(n)
        Q_dx = self.weight_vel * np.eye(n)
        Q_zeros = np.zeros((n, n))
        w_ddl = self.weight_acc
        w_dddl = self.weight_jerk

        if n % 2 != 0:
            print('x0 input error.')
            return
        n_part = int(n/2)   # 每部分有n_part个数据,即x_x(0), x_x(1), ..., x_x(n-1), x_y(0), x_y(1), ..., x_y(n-1), ...

        Q_ddx_part = (w_ddl + 2 * w_dddl / self.ctrl_ts / self.ctrl_ts) * np.eye(n_part) \
                     - 2 * w_dddl / self.ctrl_ts / self.ctrl_ts * np.eye(n_part, k=-1)
        Q_ddx_part[0][0] = w_ddl + w_dddl / self.ctrl_ts / self.ctrl_ts
        Q_ddx_part[n_part-1][n_part-1] = w_ddl + w_dddl / self.ctrl_ts / self.ctrl_ts

        np_zeros = np.zeros(Q_ddx_part.shape)
        Q_ddx_l = np.vstack((Q_ddx_part, np_zeros))
        Q_ddx_r = np.vstack((np_zeros, Q_ddx_part))
        Q_ddx = np.hstack((Q_ddx_l, Q_ddx_r))


        Q_total = sparse.csc_matrix(np.block([[Q_x, Q_zeros, Q_zeros],
                                              [Q_zeros, Q_dx, Q_zeros],
                                              [Q_zeros, Q_zeros, Q_ddx]
                                              ]))

        p_total = - x0_total
        p_total[:n] = self.weight_pos * p_total[:n]

        # 动力学模型,所以是等式约束
        # x(i+1) = x(i) + x'(i)△t
        # x'(i+1) = x'(i) + x"(i)△t
        AI_part = np.eye(n_part-1, n_part) - np.eye(n_part-1, n_part, k=1)  # 计算同阶变量之间插值 (n-1) x n维
        AT_part = self.ctrl_ts * np.eye(n_part-1, n_part)   # 时间矩阵 (n-1) x n维
        AZ_part = np.zeros([n_part-1, n_part])  # 全0矩阵 (n-1) x n维

        # 起点为第一个点
        A_init = np.zeros([4, x0_total.shape[0]])
        A_init[0, 0] = A_init[1, n_part] = 1
        A_init[2, n_part-1] = A_init[3, 2*n_part-1] = 1

        A_l_init = A_u_init = np.array([x0_total[0], x0_total[n_part],
                                        x0_total[n_part-1], x0_total[2*n_part-1]])

        A_eq = sparse.csc_matrix(np.block([
            [A_init],
            [AI_part, AZ_part, AT_part, AZ_part, AZ_part, AZ_part],
            [AZ_part, AI_part, AZ_part, AT_part, AZ_part, AZ_part],
            [AZ_part, AZ_part, AI_part, AZ_part, AT_part, AZ_part],
            [AZ_part, AZ_part, AZ_part, AI_part, AZ_part, AT_part]
        ]))
        A_leq = A_ueq = np.zeros(A_eq.shape[0])
        A_leq[:4] = A_ueq[:4] = A_l_init


        # Create an OSQP object
        prob = osqp.OSQP()
        prob.setup(Q_total, p_total, A_eq, A_leq, A_ueq, alpha=1.0)
        res = prob.solve()

        return res.x[:]


class PathGenerator:
    def __init__(self):
        self.ctrl_points = np.array([[0.5, 0.5],
                                     [0.5, 1],
                                     [1.5, 1.],
                                     [2., 2.],
                                     [2.5, 2.5]])

        self.ctrl_ts = 0.1  # 控制时间s
        self.acc_max = 2    # 米/秒

        self.ref_path = PathOptimizer(self.ctrl_points, self.ctrl_ts, self.acc_max)


if __name__ == '__main__':
    pg = PathGenerator()

    fig = plt.figure()

    ax = plt.axes()
    ax.scatter(pg.ctrl_points[:, 0], pg.ctrl_points[:, 1], color='red')
    ax.plot(pg.ref_path.px, pg.ref_path.py, color='gray')

    seg_num = pg.ref_path.px.shape[0]
    ax.plot(pg.ref_path.smooth_path[:seg_num], pg.ref_path.smooth_path[seg_num:2*seg_num], color='green')

    plt.show()