本文为学习自动驾驶决策规划算法第二章第四节(中) 路径二次规划算法》的学习笔记。
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结合即得到二次规划下的最优路径，再转化到笛卡尔坐标系下完成路径规划。