二次规划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问题的标准类型定义为:

m i n i m i z e 1 2 ⋅ x T ⋅ H ⋅ x + f T ⋅ x s . t . L B ≤ x ≤ U B A e q x = b e q A x ≤ b minimize \frac{1}{2} \cdot x^T \cdot H \cdot x + f^T \cdot x \\ s.t. LB \leq x \leq UB \\ A_{eq}x = b_{eq} \\ Ax \leq b minimize21xTHx+fTxs.t.LBxUBAeqx=beqAxb

2 目标函数

2.1 获取样条段

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

2.2 定义样条段函数

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

s = f i ( t ) = a 0 i + a 1 i ⋅ t + a 2 i ⋅ t 2 + a 3 i ⋅ t 3 + a 4 i ⋅ t 4 + a 5 i ⋅ t 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 s=fi(t)=a0i+a1it+a2it2+a3it3+a4it4+a5it5

2.3 定义样条段优化函数

Apollo首先定义 c o s t 1 cost_1 cost1以使路径更加平滑:

c o s t 1 = ∑ i = 1 n ( w 1 ⋅ ∫ 0 d i ( f i ′ ) 2 ( s ) d s + w 2 ⋅ ∫ 0 d i ( f i ′ ′ ) 2 ( s ) d s + w 3 ⋅ ∫ 0 d i ( f i ′ ′ ′ ) 2 ( s ) d s ) cost_1 = \sum_{i=1}^{n} \Big( w_1 \cdot \int\limits_{0}^{d_i} (f_i')^2(s) ds + w_2 \cdot \int\limits_{0}^{d_i} (f_i'')^2(s) ds + w_3 \cdot \int\limits_{0}^{d_i} (f_i^{\prime\prime\prime})^2(s) ds \Big) cost1=i=1n(w10di(fi)2(s)ds+w20di(fi′′)2(s)ds+w30di(fi′′′)2(s)ds)

然后,Apollo定义 c o s t 2 cost_2 cost2表示最后的S-T路径和S-T巡航路径(有速度限制且m个点)的差值:

c o s t 2 = ∑ i = 1 n ∑ j = 1 m ( f i ( t j ) − s j ) 2 cost_2 = \sum_{i=1}^{n}\sum_{j=1}^{m}\Big(f_i(t_j)- s_j\Big)^2 cost2=i=1nj=1m(fi(tj)sj)2

同样地,Apollo定义了 c o s t 3 cost_3 cost3表示第一个S-T路径和随后的S-T路径(o个点)的差值:

c o s t 3 = ∑ i = 1 n ∑ j = 1 o ( f i ( t j ) − s j ) 2 cost_3 = \sum_{i=1}^{n}\sum_{j=1}^{o}\Big(f_i(t_j)- s_j\Big)^2 cost3=i=1nj=1o(fi(tj)sj)2

最后得出的目标函数为:

c o s t = c o s t 1 + c o s t 2 + c o s t 3 cost = cost_1 + cost_2 + cost_3 cost=cost1+cost2+cost3

3 约束条件

3.1 初始点约束

假设第一个点是( t 0 t0 t0, s 0 s0 s0),且 s 0 s0 s0在路径 f i ( t ) f_i(t) fi(t), f ′ i ( t ) f'i(t) fi(t), 和 f i ( t ) ′ ′ f_i(t)'' fi(t)′′上(位置、速率、加速度)。Apollo将这些约束转换为QP约束的等式为:

A e q x = b e q A_{eq}x = b_{eq} Aeqx=beq

3.2 单调约束

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

在路径上采样 m 个点,对每一个 j j j j − 1 j-1 j1 的点对,且( j ∈ [ 1 , . . . , m ] j\in[1,...,m] j[1,...,m]),如果两个点都处在同一个样条 k k 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} 1tjtj2tj3tj4tj5 akbkckdkekfk > 1tj1tj12tj13tj14tj15 akbkckdkekfk

如两个点分别处在不同的样条 k k k l l 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} 1tjtj2tj3tj4tj5 akbkckdkekfk > 1tj1tj12tj13tj14tj15 alblcldlelfl

3.3 平滑节点约束

该约束的目的是使样条的节点更加平滑。假设两个段 s e g k seg_k segk s e g k + 1 seg_{k+1} segk+1互相连接,且 s e g k seg_k segk的累计值 s s k s_k sk。计算约束的等式为:

f k ( t k ) = f k + 1 ( t 0 ) f_k(t_k) = f_{k+1} (t_0) fk(tk)=fk+1(t0)

即:
∣ 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} 1tktk2tk3tk4tk5 ak0ak1ak2ak3ak4ak5 = 1t0t02t03t04t05 ak+1,0ak+1,1ak+1,2ak+1,3ak+1,4ak+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 1tktk2tk3tk4tk51t0t02t03t04t05 ak0ak1ak2ak3ak4ak5ak+1,0ak+1,1ak+1,2ak+1,3ak+1,4ak+1,5 =0

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

同样地,为下述等式计算约束等式:

f k ′ ( t k ) = f k + 1 ′ ( t 0 ) f k ′ ′ ( t k ) = f k + 1 ′ ′ ( t 0 ) f k ′ ′ ′ ( t k ) = f k + 1 ′ ′ ′ ( t 0 ) f'_k(t_k) = f'_{k+1} (t_0) \\ f''_k(t_k) = f''_{k+1} (t_0) \\ f'''_k(t_k) = f'''_{k+1} (t_0) fk(tk)=fk+1(t0)fk′′(tk)=fk+1′′(t0)fk′′′(tk)=fk+1′′′(t0)

3.4 点采样边界约束

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

A x ≤ b Ax \leq b Axb

首先基于道路宽度和周围的障碍物找到点 ( s j , l j ) (s_j, l_j) (sj,lj)的下边界 l l b , j l_{lb,j} llb,j,且 j ∈ [ 0 , m ] j\in[0, m] j[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...1t0t1...tmt02t12...tm2t03t13...tm3t04t14...tm4t05t15...tm5 aibicidieifi llb,0llb,1...llb,m

同样地,对上边界 l u b , j l_{ub,j} lub,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...1t0t1...tmt02t12...tm2t03t13...tm3t04t14...tm4t05t15...tm5 aibicidieifi 1 lub,0lub,1...lub,m

3.5 速度边界优化

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

在st曲线上取样 m 个点,为每个点 j j j获取速度限制的上边界和下边界,例如 v u b , j v{ub,j} vub,j v l b , j v{lb,j} vlb,j,约束定义为:

f ′ ( t j ) ≥ v l b , j f'(t_j) \geq v_{lb,j} f(tj)vlb,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...011...1t0t1...tmt02t12...tm2t03t13...tm3t04t14...tm4 aibicidieifi vlb,0vlb,1...vlb,m
且,
f ′ ( t j ) ≤ v u b , j f'(t_j) \leq v_{ub,j} f(tj)vub,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...011...1t0t1...tmt02t12...tm2t03t13...tm3t04t14...tm4 aibicidieifi vub,0vub,1...vub,m

你可能感兴趣的:(Apollo,apollo,planning,决策规划,自动驾驶,算法)