基于二次规划的路径规划算法

二次规划问题

二次规划问题标准形式：
$$\begin{array}{rl}& minimise:f(x)=\frac{1}{2}{x}^{T}Px+q^{T}x\\ & subjectto:l\le Ax\le u\end{array}$$

Apollo使用osqp进行二次规划问题的求解。第一行为代价函数，第二行为约束条件。二次优化的目标就是在满足约束条件的基础上，找到变量的最有估值使得代价函数的值最小。二次规划只在代价函数为凸函数的时候能够收敛到最优解，因此这需要P矩阵为半正定矩阵，这是非常重要的一个条件。这反映在Apollo中的规划算法则为需要进行求解的空间为凸空间，这样二次规划才能收敛到一条最优路径。

定义优化变量

路径规划一般是在Frenet坐标系中进行的。s为沿着参考线的方向，l为垂直于坐标系的方向。如图所示，将障碍物分别投影到SL坐标系上。在s方向上以间隔$△s$作为一个间隔点，从$s_0,s_1,s_2$一直到$s_{n-1}$,构成了规划的路径。选取每个间隔点的$l,\dot{l},\ddot{l}$作为优化变量：

$$\begin{array}{r}{l}_0\\ {\dot{l}}_0\\ {\ddot{l}}_0\end{array}\stackrel{△s}{⟶}\begin{array}{r}{l}_1\\ {\dot{l}}_1\\ {\ddot{l}}_1\end{array}\stackrel{△s}{⟶}\begin{array}{r}{l}_2\\ {\dot{l}}_2\\ {\ddot{l}}_2\end{array}......\begin{array}{r}{l}_{n-2}\\ {\dot{l}}_{n-2}\\ {\ddot{l}}_{n-2}\end{array}\stackrel{△s}{⟶}\begin{array}{r}{l}_{n-1}\\ {\dot{l}}_{n-1}\\ {\ddot{l}}_{n-1}\end{array}$$
如此，就构成了优化变量x:
x = { l 0 , l 1 , . . . , l n − 1 , l ˙ 0 , l ˙ 1 , . . . , l ˙ n − 1 , l ¨ 0 , l ¨ 1 , . . . , l ¨ n − 1 } x = \{l_0, l_1, ..., l_{n-1},\dot{l}_0, \dot{l}_1, ..., \dot{l}_{n-1},\ddot{l}_0, \ddot{l}_1, ..., \ddot{l}_{n-1}\} x={l0​,l1​,...,ln−1​,l˙0​,l˙1​,...,l˙n−1​,l¨0​,l¨1​,...,l¨n−1​}

定义目标函数

对于目标函数的设计，我们需要明确以下目标：

• 确保安全、礼貌的驾驶，尽可能贴近车道中心线行驶：$|l_i|\downarrow$
• 确保舒适的体感，尽可能降低横向速度/加速度/加加速度：$|l_i|\downarrow ,|{\dot{l}}_i|\downarrow ,|{\ddot{l}}_i|\downarrow$
• 确保终点接近参考终点（这个往往用在靠边停车场景之）：$l_{end}=l_{ref}$

最后会得到以下目标函数：
$$\begin{array}{rl}minf& =\sum _{i=0}^{n-1}(w_{ref}(l_i-l_{i\_ref}{)}^2+w_l{l}_i^2+w_{dl}{\dot{l}}_i^2+w_{ddl}{\ddot{l}}_i^2+w_{dddl}{\stackrel{...}{l}}_i^2\\ & +w_{end-l}(l_{n-1}-l_{end}{)}^2+w_{end-dl}({\dot{l}}_{n-1}-{\dot{l}}_{end}{)}^2+w_{end-ddl}({\ddot{l}}_{n-1}-{\ddot{l}}_{end}{)}^2\end{array}$$
其中：${\stackrel{...}{l}}_i=\frac{{\ddot{l}}_{i+1}-{\ddot{l}}_i}{△s}=\frac{({\ddot{l}}_{i+1}{)}^2+({\ddot{l}}_i{)}^2-2{\ddot{l}}_{i+1}{\ddot{l}}_i}{△s^2}$、$w_l，w_{dl}，w_{ddl}，w_{dddl}是对优化变量的权重$、$w_{end-l},w_{end-dl},w_{end-ddl}是对偏移终点的权重$。

为了方便后续矩阵编写，将目标函数按阶次整理如下：
​$$\begin{array}{rl}minf& =\sum _{i=0}^{n-1}{w}_l{l}_i+\sum _{i=0}^{n-1}{w}_{ref}(l_i-l_{i\_ref}{)}^2+w_{l_{end}}(l_{n-1}-l_{end})\\ & +\sum _{i=0}^{n-1}{w}_{\dot{l}}{\dot{l}}_i+w_{{\dot{l}}_{end}}({\dot{l}}_{n-1}-{\dot{l}}_{end})\\ & +\sum _{i=0}^{n-1}{w}_{\ddot{l}}{\ddot{l}}_i+w_{{\ddot{l}}_{end}}(d{\dot{l}}_{n-1}-{\ddot{l}}_{end})\\ & +\sum _{i=0}^{n-2}{w}_{{\stackrel{...}{l}}_i}{\stackrel{...}{l}}_i^2\end{array}$$

上式中${\sum }_{i=0}^{n-2}{\stackrel{...}{l}}_i^2$展开为：
$$\sum _{i=0}^{n-2}{\stackrel{...}{l}}_i^2=\frac{({\ddot{l}}_0{)}^2}{△s^2}+2\sum _{i=0}^{n-2}\frac{({\ddot{l}}_i{)}^2}{△s^2}+\frac{({\ddot{l}}_{n-1}{)}^2}{△s^2}-2\sum _{i=0}^{n-2}\frac{{\ddot{l}}_i{\ddot{l}}_{i+1}}{△s^2}$$

进一步对目标函数展开合并，且移除常量项可得：
$$\begin{array}{rl}minf& =\sum _{i=0}^{n-1}{w}_l{l}_i^2+\sum _{i=0}^{n-1}{w}_{ref}(l_i-l_{i\_ref}{)}^2+w_{l_{end}}(l_{n-1}-l_{end}{)}^2\\ & +\sum _{i=0}^{n-1}{w}_{\dot{l}}{\dot{l}}_i^2+w_{{\dot{l}}_{end}}({\dot{l}}_{n-1}-{\dot{l}}_{end})\\ & +\sum _{i=0}^{n-1}{w}_{\ddot{l}}{\ddot{l}}_i+w_{{\ddot{l}}_{end}}({\ddot{l}}_{n-1}-{\ddot{l}}_{end})\\ & +w_{\stackrel{...}{l}}[\frac{({\ddot{l}}_0{)}^2}{△s^2}+2\sum _{i=0}^{n-2}\frac{({\ddot{l}}_i{)}^2}{△s^2}+\frac{({\ddot{l}}_{n-1}{)}^2}{△s^2}-2\sum _{i=0}^{n-2}\frac{{\ddot{l}}_i{\ddot{l}}_{i+1}}{△s^2}]\end{array}$$

要满足的约束条件

• 主车必须在道路边界内，同时不能和障碍物有碰撞：
  $$l_i\in (l_{min}^i,l_{max}^i)$$

• 根据当前状态，主车的横向速度/加速度/加加速度有特定运动学限制：
  $${\dot{l}}_i\in ({\dot{l}}_{min}^i,{\dot{l}}_{max}^i),{\ddot{l}}_i\in ({\ddot{l}}_{min}^i,{\ddot{l}}_{max}^i),{\stackrel{...}{l}}_i\in ({\stackrel{...}{l}}_{min}^i,{\stackrel{...}{l}}_{max}^i)$$

• 连续性约束：
  $$\begin{array}{rl}{\ddot{l}}_{i+1}& ={\ddot{l}}_i+{\int }_0^{△s}{\stackrel{...}{l}}_{i\to i+1}ds={\ddot{l}}_i+{\stackrel{...}{l}}_{i\to i+1}△s\\ {\dot{l}}_{i+1}& ={\dot{l}}_i+{\int }_0^{△s}{\ddot{l}}_{i\to i+1}ds={\dot{l}}_i+{\ddot{l}}_i△s+\frac{1}{2}{\stackrel{...}{l}}_{i\to i+1}△s^2\\ l_{i+1}& =l_i+{\int }_0^{△s}{\dot{l}}_{i\to i+1}ds=l_i+{\dot{l}}_i△s+\frac{1}{2}{\ddot{l}}_i△s^2+\frac{1}{6}{\stackrel{...}{l}}_{i\to i+1}△s^3\end{array}$$

• 起点约束：
  $$l_0=l_{init},{\dot{l}}_0={\dot{l}}_{init},{\ddot{l}}_0={\ddot{l}}_{init}$$

参考文献

1. PnC进阶(9.0版)¹
2. 非线性规划 拉格朗日乘子法²
3. 规划模块TASK之PIECEWISE_JERK_PATH_OPTIMIZER³