本文为学习自动驾驶决策规划算法第二章第四节(中) 路径二次规划算法》的学习笔记。
1 二次型
二次型的形式为
1 2 x T H x + f T x \begin{equation} \frac{1}{2}\boldsymbol{x}^TH\boldsymbol{x}+f^T\boldsymbol{x} \end{equation} 21xTHx+fTx
约束
A e q x = b e q \begin{equation} A_{eq}\boldsymbol{x}=b_{eq} \end{equation} Aeqx=beq
或
l b ≤ A 1 x ≤ u b \begin{equation} lb≤A_1\boldsymbol{x}≤ub \end{equation} lb≤A1x≤ub
可以转化为 A x ≤ b A\boldsymbol{x}≤b Ax≤b的形式：
A e q x = b e q ⟹ b e q ≤ A e q x ≤ b e q ⟹ { A e q x ≤ b e q − A e q x ≤ − b e q ⟹ [ A e q − A e q ] x ≤ [ b e q − b e q ] \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} Aeqx=beq⟹beq≤Aeqx≤beq⟹{Aeqx≤beq−Aeqx≤−beq⟹[Aeq−Aeq]x≤[beq−beq]
同理有
l b ≤ A 1 x ≤ u b ⟹ [ A 1 − A 1 ] x ≤ [ u b − l b ] \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} lb≤A1x≤ub⟹[A1−A1]x≤[ub−lb]
2 轻决策与重决策
上一章节动态规划实际上属于决策算法，比如绕障碍时从左边绕还是右边绕，为二次规划开辟了凸空间。
决策算法分重决策和轻决策，重决策是基于人给定的规则，轻决策基于代价函数。
重决策：
优点：
计算量小；
在感知不强的情况下仍然能做决策；
缺点：
场景太多，无法完全覆盖；
人给出的决策所开辟的凸空间未必满足约束，二次规划搜不到解：
轻决策根据设计的代价函数，在离散空间上搜索最优路径，开辟凸空间；
优点：
无认为规则，可以处理复杂场景；
缺点：
复杂场景计算量很大；
依赖预测（速度规划时会涉及）；
要求周围环境全知（感知、定位要求高）；
代价函数设计/最优解未必符合人的驾驶习惯；
3 二次规划算法
如图，动态规划的粗解决策了绕行的方向，图中 l m i n 1 , l m i n 2 , l m i n 3 ... l_{min1},l_{min2},l_{min3}\dots lmin1,lmin2,lmin3...与 l m a x 1 , l m a x 2 , l m a x 3 ... l_{max1},l_{max2},l_{max3}\dots lmax1,lmax2,lmax3...构成凸空间，在该凸空间中使用二次规划求解路径：
已知 s i s_i si和 [ l m i n i , l m a x i ] [l_{mini},l_{maxi}] [lmini,lmaxi]求 l = f ( s ) l=f(s) l=f(s)满足：
f ( s ) f(s) f(s)二阶导数连续；
l m i n i ≤ f ( s i ) ≤ l m a x i l_{mini}≤f(s_i)≤l_{maxi} lmini≤f(si)≤lmaxi；
f ( s ) f(s) f(s)尽可能平滑（各阶导数的平方尽可能小）；
f ( s ) f(s) f(s)尽可能在凸空间的中间；（Apollo）
3.1 分段加加速度优化法
假设连接 l i l_i li和 l i + 1 l_{i+1} li+1的曲线的三阶导数为常数 l i + 1 ′ ′ − l i ′ ′ Δ s \frac{l''_{i+1}-l''i}{\Delta{s}} Δsli+1′′−li′′，四阶及以上的导数全为0，由泰勒展开公式可得：
l i + 1 = l i + l i ′ Δ s + 1 2 l i ′ ′ Δ s 2 + 1 6 l i + 1 ′ ′ − l i ′ ′ Δ s Δ s 3 \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} li+1=li+li′Δs+21li′′Δs2+61Δsli+1′′−li′′Δs3
l i + 1 ′ = l i ′ + l i ′ ′ Δ s + 1 2 l i + 1 ′ ′ − l i ′ ′ Δ s Δ s 2 \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} li+1′=li′+li′′Δs+21Δsli+1′′−li′′Δs2
进一步整理可得：
l i + Δ s l i ′ + 1 3 Δ s 2 l i ′ ′ − l i + 1 + 1 6 Δ s 2 l i + 1 ′ ′ = 0 \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} li+Δsli′+31Δs2li′′−li+1+61Δs2li+1′′=0
l i ′ + 1 2 Δ s l i ′ ′ − l i + 1 ′ + 1 2 Δ s l i + 1 ′ ′ = 0 \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} li′+21Δsli′′−li+1′+21Δsli+1′′=0
写成矩阵形式就是：
[ 1 Δ s 1 3 Δ s 2 − 1 0 1 6 Δ s 2 0 1 1 2 Δ s 0 − 1 1 2 Δ s ] [ l i l i ′ l i ′ ′ l i + 1 l i + 1 ′ l i + 1 ′ ′ ] = 0 \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} [10Δs131Δs221Δs−100−161Δs221Δs] lili′li′′li+1li+1′li+1′′ =0
记上式等式左侧的系数矩阵为 A e q _ s u b A_{eq\sub} Aeq_sub，左侧列向量为待优化的值，对于 i = 1 , 2 , ... n i=1,2,\dots{n} i=1,2,...n可得：
[ A e q _ s u b 0 0 0 ... 0 0 0 ... 0 0 0 0 0 0 A e q _ s u b 0 0 0 ... 0 0 0 ... ... 0 0 0 0 0 0 ... A e q _ s u b ] [ l 1 l 1 ′ l 1 ′ ′ ⋮ l n l n ′ l n ′ ′ ] = 0 \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} Aeq_sub000000000000000000......Aeq_sub......000000......Aeq_sub l1l1′l1′′⋮lnln′ln′′ =0
上式等式可记为
A e q x = b e q \begin{equation} A_{eq}x=b_{eq} \end{equation} Aeqx=beq
其中左侧的系数矩阵维度为 ( 2 n − 2 ) ∗ 3 n (2n-2)*3n (2n−2)∗3n。
3.2 对汽车的四个角点的约束
如图所示
汽车的角点：
l p 1 = l + d 1 sin θ + w 2 cos θ \begin{equation} l_{p_1}=l+d_1\sin\theta+\frac{w}{2}\cos\theta \end{equation} lp1=l+d1sinθ+2wcosθ
l p 2 = l + d 1 sin θ − w 2 cos θ \begin{equation} l_{p_2}=l+d_1\sin\theta-\frac{w}{2}\cos\theta \end{equation} lp2=l+d1sinθ−2wcosθ
l p 3 = l − d 2 sin θ + w 2 cos θ \begin{equation} l_{p_3}=l-d_2\sin\theta+\frac{w}{2}\cos\theta \end{equation} lp3=l−d2sinθ+2wcosθ
l p 4 = l − d 2 sin θ − w 2 cos θ \begin{equation} l_{p_4}=l-d_2\sin\theta-\frac{w}{2}\cos\theta \end{equation} lp4=l−d2sinθ−2wcosθ
再对三角函数做近似处理：
sin θ ≈ tan θ ≈ l ′ \begin{equation} \sin\theta≈\tan\theta≈l' \end{equation} sinθ≈tanθ≈l′
cos θ ≈ 1 \begin{equation} \cos\theta≈1 \end{equation} cosθ≈1
角点的连线上的点不超过 l m i n l_{min} lmin和 l m a x l_{max} lmax，一种简单的处理方法是寻找自车当前位置前后一定范围内(比如 [ s i − d 2 , s i + d 1 ] [s_i-d_2, s_i+d_1] [si−d2,si+d1])的 l m a x i l_{maxi} lmaxi的最小值，记为 u b i ub_i ubi，车辆的四个角点都应小于 u b i ub_i ubi，同理寻找自车当前位置前后一定范围内的 l m i n i l_{mini} lmini的最大值，记为 l b i lb_i lbi，车辆的四个角点都应大于 l b i lb_i lbi:
l b i ≤ l i + d 1 l i ′ + w 2 ≤ u b i \begin{equation} lb_i≤l_i+d_1l'_i+\frac{w}{2}≤ub_i \end{equation} lbi≤li+d1li′+2w≤ubi
l b i ≤ l i + d 1 l i ′ − w 2 ≤ u b i \begin{equation} lb_i≤l_i+d_1l'_i-\frac{w}{2}≤ub_i \end{equation} lbi≤li+d1li′−2w≤ubi
l b i ≤ l i − d 1 l i ′ + w 2 ≤ u b i \begin{equation} lb_i≤l_i-d_1l'_i+\frac{w}{2}≤ub_i \end{equation} lbi≤li−d1li′+2w≤ubi
l b i ≤ l i − d 1 l i ′ − w 2 ≤ u b i \begin{equation} lb_i≤l_i-d_1l'_i-\frac{w}{2}≤ub_i \end{equation} lbi≤li−d1li′−2w≤ubi
根据式5可以将式19~22写成矩阵形式：
[ 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 ] [ l i l i ′ l i ′ ′ ] ≤ [ u b i − w 2 u b i + w 2 u b i − w 2 u b i + w 2 − l b i + w 2 − l b i − w 2 − l b i + w 2 − l b i − w 2 ] \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} 1111−1−1−1−1d1d1−d2−d2−d1−d1d2d200000000 lili′li′′ ≤ ubi−2wubi+2wubi−2wubi+2w−lbi+2w−lbi−2w−lbi+2w−lbi−2w
同理，记上式等式左侧的系数矩阵为 A s u b A_{sub} Asub，左侧列向量为 b s u b b_{sub} bsub，对于对于 i = 1 , 2 , ... n i=1,2,\dots{n} i=1,2,...n可得
[ A s u b A s u b ... A s u b ] [ l 1 l 1 ′ l 1 ′ ′ ⋮ l n l n ′ l n ′ ′ ] ≤ [ b s u b 1 b s u b 2 ⋮ b s u b 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} AsubAsub...Asub l1l1′l1′′⋮lnln′ln′′ ≤ bsub1bsub2⋮bsubn
记为
A x ≤ b \begin{equation} Ax≤b \end{equation} Ax≤b
4 代价函数
c o s t _ f u n c t i o n = w r e f ∑ i l i 2 + w d l ∑ i l i ′ 2 + w d d l ∑ i l i ′ ′ 2 + w d d d l ∑ i ( l i + 1 ′ ′ − l i ′ ′ ) 2 + w m i d ∑ i ( l i − l m i n i + l m a x i 2 ) 2 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 cost_function=wrefi∑li2+wdli∑li′2+wddli∑li′′2+wdddli∑(li+1′′−li′′)2+wmidi∑(li−2lmini+lmaxi)2
对应的约束是式12和式25，求解出 l 1 , l 1 ′ , l 1 ′ ′ ... , l n . l n ′ , l n ′ ′ l_1,l'_1,l''_1\dots,l_n.l'_n,l''_n l1,l1′,l1′′...,ln.ln′,ln′′与 s 1 , s 2 , ... , s n s_1,s_2,\dots,s_n s1,s2,...,sn结合即得到二次规划下的最优路径，再转化到笛卡尔坐标系下完成路径规划。