自动驾驶决策规划算法-路径决策算法：二次规划

本文为学习自动驾驶决策规划算法第二章第四节(中) 路径二次规划算法》的学习笔记。

1 二次型

二次型的形式为
$\begin{equation} \frac{1}{2}\boldsymbol{x}^TH\boldsymbol{x}+f^T\boldsymbol{x} \end{equation}$
约束
$\begin{equation} A_{eq}\boldsymbol{x}=b_{eq} \end{equation}$
或
$\begin{equation} lb≤A_1\boldsymbol{x}≤ub \end{equation}$
可以转化为 $A\boldsymbol{x}≤b$ 的形式：
$\begin{equation} A_{eq}\boldsymbol{x}=b_{eq}\implies b_{eq}≤A_{eq}\boldsymbol{x}≤b_{eq}\implies \begin{cases} A_{eq}\boldsymbol{x}≤b_{eq}\\ -A_{eq}\boldsymbol{x}≤-b_{eq} \end{cases}\implies \begin{bmatrix} A_{eq} \\ -A_{eq} \end{bmatrix}\boldsymbol{x}≤ \begin{bmatrix} b_{eq} \\ -b_{eq} \end{bmatrix} \end{equation}$
同理有
$\begin{equation} lb≤A_1\boldsymbol{x}≤ub\implies\begin{bmatrix} A_{1} \\ -A_{1} \end{bmatrix}\boldsymbol{x}≤ \begin{bmatrix} ub \\ -lb \end{bmatrix} \end{equation}$

2 轻决策与重决策

上一章节动态规划实际上属于决策算法，比如绕障碍时从左边绕还是右边绕，为二次规划开辟了凸空间。
在这里插入图片描述
决策算法分重决策和轻决策，重决策是基于人给定的规则，轻决策基于代价函数。
重决策：
优点：
计算量小；
在感知不强的情况下仍然能做决策；
缺点：
场景太多，无法完全覆盖；
人给出的决策所开辟的凸空间未必满足约束，二次规划搜不到解：
在这里插入图片描述
轻决策根据设计的代价函数，在离散空间上搜索最优路径，开辟凸空间；
优点：
无认为规则，可以处理复杂场景；
缺点：
复杂场景计算量很大；
依赖预测（速度规划时会涉及）；
要求周围环境全知（感知、定位要求高）；
代价函数设计/最优解未必符合人的驾驶习惯；

3 二次规划算法

在这里插入图片描述

如图，动态规划的粗解决策了绕行的方向，图中 $l_{min1},l_{min2},l_{min3}\dots$ 与 $l_{max1},l_{max2},l_{max3}\dots$ 构成凸空间，在该凸空间中使用二次规划求解路径：
已知 $s_i$ 和 $l_{mini},l_{maxi}]$ 求 $l = f (s)$ 满足：
$f (s)$ 二阶导数连续；
$l_{mini}≤f(s_i)≤l_{maxi}$ ；
$f (s)$ 尽可能平滑（各阶导数的平方尽可能小）；
$f (s)$ 尽可能在凸空间的中间；（Apollo）

3.1 分段加加速度优化法

假设连接 $l_i$ 和 $l_{i+1}$ 的曲线的三阶导数为常数 $\frac{l''_{i+1}-l''_i}{\Delta{s}}$ ，四阶及以上的导数全为0，由泰勒展开公式可得：
$\begin{equation} l_{i+1}=l_i+l'_i\Delta{s}+\frac{1}{2}l''_i\Delta{s^2}+\frac{1}{6}\frac{l''_{i+1}-l''_i}{\Delta{s}}\Delta{s^3} \end{equation}$
$\begin{equation} l'_{i+1}=l'_i+l''_i\Delta{s}+\frac{1}{2}\frac{l''_{i+1}-l''_i}{\Delta{s}}\Delta{s^2} \end{equation}$
进一步整理可得：
$\begin{equation} l_i+\Delta{s}l'_i+\frac{1}{3}\Delta{s}^2l''_i-l_{i+1}+\frac{1}{6}\Delta{s}^2l''_{i+1}=0 \end{equation}$
$\begin{equation} l'_i+\frac{1}{2}\Delta{s}l''_i-l'_{i+1}+\frac{1}{2}\Delta{s}l''_{i+1}=0 \end{equation}$
写成矩阵形式就是：
$\begin{equation} \begin{bmatrix} 1 & \Delta{s} & \frac{1}{3}\Delta{s}^2 & -1 & 0 & \frac{1}{6}\Delta{s}^2 \\ 0 & 1 & \frac{1}{2}\Delta{s} & 0 & -1 & \frac{1}{2}\Delta{s} \end{bmatrix} \begin{bmatrix} l_i \\ l'_i \\ l''_i \\ l_{i+1} \\ l'_{i+1} \\ l''_{i+1} \end{bmatrix}=0 \end{equation}$
记上式等式左侧的系数矩阵为 $A_{eq\_sub}$ ，左侧列向量为待优化的值，对于 $i=1,2,\dots{n}$ 可得：
$\begin{equation} \begin{bmatrix} A_{eq\_sub} & \begin{smallmatrix} 0 & 0 & 0 & \dots \\ 0 & 0 & 0 & \dots \end{smallmatrix} \\ \begin{smallmatrix} 0 & 0 & 0 & \\ 0 & 0 & 0 & \end{smallmatrix} & A_{eq\_sub} & \begin{smallmatrix} 0 & 0 & 0 & \dots \\ 0 & 0 & 0 & \dots \end{smallmatrix} \\ & \dots\\ \begin{smallmatrix} 0 & 0 & 0 & \\ 0 & 0 & 0 & \end{smallmatrix} & \dots & A_{eq\_sub} \end{bmatrix} \begin{bmatrix} l_1 \\ l'_1 \\ l''_1 \\ \vdots \\ l_n \\ l'_n \\ l''_n \end{bmatrix}=0 \end{equation}$
上式等式可记为
$\begin{equation} A_{eq}x=b_{eq} \end{equation}$
其中左侧的系数矩阵维度为 $(2 n - 2) * 3 n$ 。

3.2 对汽车的四个角点的约束

如图所示
在这里插入图片描述

汽车的角点：
$\begin{equation} l_{p_1}=l+d_1\sin\theta+\frac{w}{2}\cos\theta \end{equation}$
$\begin{equation} l_{p_2}=l+d_1\sin\theta-\frac{w}{2}\cos\theta \end{equation}$
$\begin{equation} l_{p_3}=l-d_2\sin\theta+\frac{w}{2}\cos\theta \end{equation}$
$\begin{equation} l_{p_4}=l-d_2\sin\theta-\frac{w}{2}\cos\theta \end{equation}$
再对三角函数做近似处理：
$\begin{equation} \sin\theta≈\tan\theta≈l' \end{equation}$
$\begin{equation} \cos\theta≈1 \end{equation}$

角点的连线上的点不超过 $l_{min}$ 和 $l_{max}$ ，一种简单的处理方法是寻找自车当前位置前后一定范围内(比如 $s_i-d_2, s_i+d_1]$ )的 $l_{maxi}$ 的最小值，记为 $ub_i$ ，车辆的四个角点都应小于 $ub_i$ ，同理寻找自车当前位置前后一定范围内的 $l_{mini}$ 的最大值，记为 $lb_i$ ，车辆的四个角点都应大于 $lb_i$ :

$\begin{equation} lb_i≤l_i+d_1l'_i+\frac{w}{2}≤ub_i \end{equation}$
$\begin{equation} lb_i≤l_i+d_1l'_i-\frac{w}{2}≤ub_i \end{equation}$
$\begin{equation} lb_i≤l_i-d_1l'_i+\frac{w}{2}≤ub_i \end{equation}$
$\begin{equation} lb_i≤l_i-d_1l'_i-\frac{w}{2}≤ub_i \end{equation}$
根据式5可以将式19~22写成矩阵形式：
$\begin{equation} \begin{bmatrix} 1 & d_1 & 0 \\ 1 & d_1 & 0 \\ 1 & -d_2 & 0 \\ 1 & -d_2 & 0 \\ -1 & -d_1 & 0 \\ -1 & -d_1 & 0 \\ -1 & d_2 & 0 \\ -1 & d_2 & 0 \\ \end{bmatrix} \begin{bmatrix} l_i \\ l'_i \\ l''_i \end{bmatrix} ≤ \begin{bmatrix} ub_i-\frac{w}{2} \\ ub_i+\frac{w}{2} \\ ub_i-\frac{w}{2} \\ ub_i+\frac{w}{2} \\ -lb_i+\frac{w}{2} \\ -lb_i-\frac{w}{2} \\ -lb_i+\frac{w}{2} \\ -lb_i-\frac{w}{2} \\ \end{bmatrix} \end{equation}$
同理，记上式等式左侧的系数矩阵为 $A_{sub}$ ，左侧列向量为 $b_{sub}$ ，对于对于 $i=1,2,\dots{n}$ 可得
$\begin{equation} \begin{bmatrix} A_{sub} \\ & A_{sub} \\ & & \dots \\ & & & A_{sub} \end{bmatrix} \begin{bmatrix} l_1 \\ l'_1 \\ l''_1 \\ \vdots \\ l_n \\ l'_n \\ l''_n \end{bmatrix} ≤ \begin{bmatrix} b_{sub_1} \\ b_{sub_2} \\ \vdots \\ b_{sub_n} \end{bmatrix} \end{equation}$
记为
$\begin{equation} Ax≤b \end{equation}$

4 代价函数

$cost\_function=w_{ref}\sum_i{l_i}^2+w_{dl}\sum_i{l'_i}^2+w_{ddl}\sum_i{l''_i}^2+w_{dddl}\sum_i{(l''_{i+1}-l''_i)}^2+w_{mid}\sum_i{(l_i-\frac{l_{mini+l_{maxi}}}{2})}^2$
对应的约束是式12和式25，求解出 $l_1,l'_1,l''_1\dots,l_n.l'_n,l''_n$ 与 $s_1,s_2,\dots,s_n$ 结合即得到二次规划下的最优路径，再转化到笛卡尔坐标系下完成路径规划。