二次规划ST速度优化

二次规划ST速度优化

原地址：https://daobook.github.io/apollo/docs/specs/qp_spline_st_speed_optimizer_cn.html

本文是Apollo早期速度优化的原理。

1 定义

从二次规划样条路径中选取一条路径后，Apollo将路线上的所有障碍物和自动驾驶车辆（ADV）展现在一个时间-路径图上（path-time ST），该路径图表示了路径上的站点变化。速度优化的任务是在ST图上找到一条合理的，无障碍的路径。

Apollo使用多个样条来表示速度参数，在ST图上表示为一系列的ST点。Apollo会对二次规划的结果做再次的平衡以获得最佳的速度参数。QP问题的标准类型定义为：

$$\begin{gathered} minimize\frac{1}{2}\cdot x^T\cdot H\cdot x+f^T\cdot x \\ s.t.LB\le x\le UB \\ {A}_{eq}x=b_{eq} \\ Ax\le b \end{gathered}$$

2 目标函数

2.1 获取样条段

将路ST速度参数分为 n 段，每段路径用一个多项式来表示。

2.2 定义样条段函数

每个样条段 i 都有沿着参考线的累加距离$d_i$。每段的路径默认用5介多项式表示。多项式介数可以通过配置参数进行调整。

$$s=f_i(t)=a_{0i}+a_{1i}\cdot t+a_{2i}\cdot t^2+a_{3i}\cdot t^3+a_{4i}\cdot t^4+a_{5i}\cdot t^5$$

2.3 定义样条段优化函数

Apollo首先定义$cos{t}_1$以使路径更加平滑：

$$cos{t}_1=\sum _{i=1}^n(w_1\cdot \underset{0}{\overset{d_i}{\int }}(f_i^{\prime }{)}^2(s)ds+w_2\cdot \underset{0}{\overset{d_i}{\int }}(f_i^{\prime \prime }{)}^2(s)ds+w_3\cdot \underset{0}{\overset{d_i}{\int }}(f_i^{\prime \prime \prime }{)}^2(s)ds)$$

然后，Apollo定义$cos{t}_2$表示最后的S-T路径和S-T巡航路径（有速度限制且m个点）的差值：

$$cos{t}_2=\sum _{i=1}^n\sum _{j=1}^m(f_i(t_j)-s_j{)}^2$$

同样地，Apollo定义了$cos{t}_3$表示第一个S-T路径和随后的S-T路径（o个点）的差值：

$$cos{t}_3=\sum _{i=1}^n\sum _{j=1}^o(f_i(t_j)-s_j{)}^2$$

最后得出的目标函数为：

$$cost=cos{t}_1+cos{t}_2+cos{t}_3$$

3 约束条件

3.1 初始点约束

假设第一个点是($t0$, $s0$)，且$s0$在路径$f_i(t)$, $f^{\prime }i(t)$, 和$f_i(t{)}^{\prime \prime }$上（位置、速率、加速度）。Apollo将这些约束转换为QP约束的等式为：

$$A_{eq}x=b_{eq}$$

3.2 单调约束

路线必须是单调的，比如车辆只能往前开。

在路径上采样 m 个点，对每一个 $j$和$j-1$ 的点对，且($j\in [1,...,m]$)，如果两个点都处在同一个样条$k$上，则：

∣ 1 t j t j 2 t j 3 t j 4 t j 5 ∣ ⋅ ∣ a k b k c k d k e k f k ∣ > ∣ 1 t j − 1 t j − 1 2 t j − 1 3 t j − 1 4 t j − 1 5 ∣ ⋅ ∣ a k b k c k d k e k f k ∣ \begin{vmatrix} 1 & t_j & t_j^2 & t_j^3 & t_j^4&t_j^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_k \\ b_k \\ c_k \\ d_k \\ e_k \\ f_k \end{vmatrix} > \begin{vmatrix} 1 & t_{j-1} & t_{j-1}^2 & t_{j-1}^3 & t_{j-1}^4&t_{j-1}^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k} \\ b_{k} \\ c_{k} \\ d_{k} \\ e_{k} \\ f_{k} \end{vmatrix} ​1​tj​​tj2​​tj3​​tj4​​tj5​​ ​⋅ ​ak​bk​ck​dk​ek​fk​​ ​> ​1​tj−1​​tj−12​​tj−13​​tj−14​​tj−15​​ ​⋅ ​ak​bk​ck​dk​ek​fk​​ ​

如两个点分别处在不同的样条$k$和$l$上，则：

∣ 1 t j t j 2 t j 3 t j 4 t j 5 ∣ ⋅ ∣ a k b k c k d k e k f k ∣ > ∣ 1 t j − 1 t j − 1 2 t j − 1 3 t j − 1 4 t j − 1 5 ∣ ⋅ ∣ a l b l c l d l e l f l ∣ \begin{vmatrix} 1 & t_j & t_j^2 & t_j^3 & t_j^4&t_j^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_k \\ b_k \\ c_k \\ d_k \\ e_k \\ f_k \end{vmatrix} > \begin{vmatrix} 1 & t_{j-1} & t_{j-1}^2 & t_{j-1}^3 & t_{j-1}^4&t_{j-1}^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{l} \\ b_{l} \\ c_{l} \\ d_{l} \\ e_{l} \\ f_{l} \end{vmatrix} ​1​tj​​tj2​​tj3​​tj4​​tj5​​ ​⋅ ​ak​bk​ck​dk​ek​fk​​ ​> ​1​tj−1​​tj−12​​tj−13​​tj−14​​tj−15​​ ​⋅ ​al​bl​cl​dl​el​fl​​ ​

3.3 平滑节点约束

该约束的目的是使样条的节点更加平滑。假设两个段$se{g}_k$ 和$se{g}_{k+1}$互相连接，且$se{g}_k$的累计值 s 为$s_k$。计算约束的等式为：

$$f_k(t_k)=f_{k+1}(t_0)$$

即：
∣ 1 t k t k 2 t k 3 t k 4 t k 5 ∣ ⋅ ∣ a k 0 a k 1 a k 2 a k 3 a k 4 a k 5 ∣ = ∣ 1 t 0 t 0 2 t 0 3 t 0 4 t 0 5 ∣ ⋅ ∣ a k + 1 , 0 a k + 1 , 1 a k + 1 , 2 a k + 1 , 3 a k + 1 , 4 a k + 1 , 5 ∣ \begin{vmatrix} 1 & t_k & t_k^2 & t_k^3 & t_k^4&t_k^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4} \\ a_{k5} \end{vmatrix} = \begin{vmatrix} 1 & t_{0} & t_{0}^2 & t_{0}^3 & t_{0}^4&t_{0}^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k+1,0} \\ a_{k+1,1} \\ a_{k+1,2} \\ a_{k+1,3} \\ a_{k+1,4} \\ a_{k+1,5} \end{vmatrix} ​1​tk​​tk2​​tk3​​tk4​​tk5​​ ​⋅ ​ak0​ak1​ak2​ak3​ak4​ak5​​ ​= ​1​t0​​t02​​t03​​t04​​t05​​ ​⋅ ​ak+1,0​ak+1,1​ak+1,2​ak+1,3​ak+1,4​ak+1,5​​ ​

然后，
∣ 1 t k t k 2 t k 3 t k 4 t k 5 − 1 − t 0 − t 0 2 − t 0 3 − t 0 4 − t 0 5 ∣ ⋅ ∣ a k 0 a k 1 a k 2 a k 3 a k 4 a k 5 a k + 1 , 0 a k + 1 , 1 a k + 1 , 2 a k + 1 , 3 a k + 1 , 4 a k + 1 , 5 ∣ = 0 \begin{vmatrix} 1 & t_k & t_k^2 & t_k^3 & t_k^4&t_k^5 & -1 & -t_{0} & -t_{0}^2 & -t_{0}^3 & -t_{0}^4&-t_{0}^5\\ \end{vmatrix} \cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4} \\ a_{k5} \\ a_{k+1,0} \\ a_{k+1,1} \\ a_{k+1,2} \\ a_{k+1,3} \\ a_{k+1,4} \\ a_{k+1,5} \end{vmatrix} = 0 ​1​tk​​tk2​​tk3​​tk4​​tk5​​−1​−t0​​−t02​​−t03​​−t04​​−t05​​ ​⋅ ​ak0​ak1​ak2​ak3​ak4​ak5​ak+1,0​ak+1,1​ak+1,2​ak+1,3​ak+1,4​ak+1,5​​ ​=0

等式中得出的结果为$t_0$ = 0。

同样地，为下述等式计算约束等式：

$$\begin{gathered} f_k^{\prime }(t_k)=f_{k+1}^{\prime }(t_0) \\ {f}_k^{\prime \prime }(t_k)=f_{k+1}^{\prime \prime }(t_0) \\ {f}_k^{\prime \prime \prime }(t_k)=f_{k+1}^{\prime \prime \prime }(t_0) \end{gathered}$$

3.4 点采样边界约束

在路径上均匀的取样 m 个点，检查这些点上的障碍物边界。将这些约束转换为QP约束不等式，使用不等式：

$$Ax\le b$$

首先基于道路宽度和周围的障碍物找到点 $(s_j,l_j)$的下边界$l_{lb,j}$，且$j\in [0,m]$。计算约束的不等式为：

∣ 1 t 0 t 0 2 t 0 3 t 0 4 t 0 5 1 t 1 t 1 2 t 1 3 t 1 4 t 1 5 . . . . . . . . . . . . . . . . . . 1 t m t m 2 t m 3 t m 4 t m 5 ∣ ⋅ ∣ a i b i c i d i e i f i ∣ ≤ ∣ l l b , 0 l l b , 1 . . . l l b , m ∣ \begin{vmatrix} 1 & t_0 & t_0^2 & t_0^3 & t_0^4&t_0^5 \\ 1 & t_1 & t_1^2 & t_1^3 & t_1^4&t_1^5 \\ ...&...&...&...&...&... \\ 1 & t_m & t_m^2 & t_m^3 & t_m^4&t_m^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_i \\ b_i \\ c_i \\ d_i \\ e_i \\ f_i \end{vmatrix} \leq \begin{vmatrix} l_{lb,0}\\ l_{lb,1}\\ ...\\ l_{lb,m}\\ \end{vmatrix} ​11...1​t0​t1​...tm​​t02​t12​...tm2​​t03​t13​...tm3​​t04​t14​...tm4​​t05​t15​...tm5​​ ​⋅ ​ai​bi​ci​di​ei​fi​​ ​≤ ​llb,0​llb,1​...llb,m​​ ​

同样地，对上边界$l_{ub,j}$，计算约束的不等式为：

∣ 1 t 0 t 0 2 t 0 3 t 0 4 t 0 5 1 t 1 t 1 2 t 1 3 t 1 4 t 1 5 . . . . . . . . . . . . . . . . . . 1 t m t m 2 t m 3 t m 4 t m 5 ∣ ⋅ ∣ a i b i c i d i e i f i ∣ ≤ − 1 ⋅ ∣ l u b , 0 l u b , 1 . . . l u b , m ∣ \begin{vmatrix} 1 & t_0 & t_0^2 & t_0^3 & t_0^4&t_0^5 \\ 1 & t_1 & t_1^2 & t_1^3 & t_1^4&t_1^5 \\ ...&...&...&...&...&... \\ 1 & t_m & t_m^2 & t_m^3 & t_m^4&t_m^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_i \\ b_i \\ c_i \\ d_i \\ e_i \\ f_i \end{vmatrix} \leq -1 \cdot \begin{vmatrix} l_{ub,0}\\ l_{ub,1}\\ ...\\ l_{ub,m}\\ \end{vmatrix} ​11...1​t0​t1​...tm​​t02​t12​...tm2​​t03​t13​...tm3​​t04​t14​...tm4​​t05​t15​...tm5​​ ​⋅ ​ai​bi​ci​di​ei​fi​​ ​≤−1⋅ ​lub,0​lub,1​...lub,m​​ ​

3.5 速度边界优化

Apollo同样需要建立速度限制边界。

在st曲线上取样 m 个点，为每个点$j$获取速度限制的上边界和下边界，例如$vub,j$ 和 $vlb,j$，约束定义为：

$$f^{\prime }(t_j)\ge v_{lb,j}$$
即：
∣ 0 1 t 0 t 0 2 t 0 3 t 0 4 0 1 t 1 t 1 2 t 1 3 t 1 4 . . . . . . . . . . . . . . . . . . 0 1 t m t m 2 t m 3 t m 4 ∣ ⋅ ∣ a i b i c i d i e i f i ∣ ≥ ∣ v l b , 0 v l b , 1 . . . v l b , m ∣ \begin{vmatrix} 0& 1 & t_0 & t_0^2 & t_0^3 & t_0^4 \\ 0 & 1 & t_1 & t_1^2 & t_1^3 & t_1^4 \\ ...&...&...&...&...&... \\ 0& 1 & t_m & t_m^2 & t_m^3 & t_m^4 \\ \end{vmatrix} \cdot \begin{vmatrix} a_i \\ b_i \\ c_i \\ d_i \\ e_i \\ f_i \end{vmatrix} \geq \begin{vmatrix} v_{lb,0}\\ v_{lb,1}\\ ...\\ v_{lb,m}\\ \end{vmatrix} ​00...0​11...1​t0​t1​...tm​​t02​t12​...tm2​​t03​t13​...tm3​​t04​t14​...tm4​​ ​⋅ ​ai​bi​ci​di​ei​fi​​ ​≥ ​vlb,0​vlb,1​...vlb,m​​ ​
且，
$$f^{\prime }(t_j)\le v_{ub,j}$$
即：
∣ 0 1 t 0 t 0 2 t 0 3 t 0 4 0 1 t 1 t 1 2 t 1 3 t 1 4 . . . . . . . . . . . . . . . . . . 0 1 t m t m 2 t m 3 t m 4 ∣ ⋅ ∣ a i b i c i d i e i f i ∣ ≤ ∣ v u b , 0 v u b , 1 . . . v u b , m ∣ \begin{vmatrix} 0& 1 & t_0 & t_0^2 & t_0^3 & t_0^4 \\ 0 & 1 & t_1 & t_1^2 & t_1^3 & t_1^4 \\ ...&...&...&...&...&... \\ 0 &1 & t_m & t_m^2 & t_m^3 & t_m^4 \\ \end{vmatrix} \cdot \begin{vmatrix} a_i \\ b_i \\ c_i \\ d_i \\ e_i \\ f_i \end{vmatrix} \leq \begin{vmatrix} v_{ub,0}\\ v_{ub,1}\\ ...\\ v_{ub,m}\\ \end{vmatrix} ​00...0​11...1​t0​t1​...tm​​t02​t12​...tm2​​t03​t13​...tm3​​t04​t14​...tm4​​ ​⋅ ​ai​bi​ci​di​ei​fi​​ ​≤ ​vub,0​vub,1​...vub,m​​ ​