概述
apollo 代码代码文档，转载从github
apollo文档
侵删
@[toc]# 二次规划ST速度优化
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 minimize21⋅xT⋅H⋅x+fT⋅xs.t.LB≤x≤UBAeqx=beqAx≤b
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+a1i⋅t+a2i⋅t2+a3i⋅t3+a4i⋅t4+a5i⋅t5
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 intlimits_{0}^{d_i} (f_i')^2(s) ds + w_2 cdot intlimits_{0}^{d_i} (f_i'')^2(s) ds + w_3 cdot intlimits_{0}^{d_i} (f_i^{primeprimeprime})^2(s) ds Big) cost1=i=1∑n(w1⋅0∫di(fi′)2(s)ds+w2⋅0∫di(fi′′)2(s)ds+w3⋅0∫di(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_jBig)^2 cost2=i=1∑nj=1∑m(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_jBig)^2 cost3=i=1∑nj=1∑o(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) f′i(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 j−1 的点对，且( j ∈ [ 1 , . . . , m ] jin[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∣∣∣∣∣∣∣∣∣∣∣∣>∣∣1tj−1tj−12tj−13tj−14tj−15∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣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∣∣∣∣∣∣∣∣∣∣∣∣>∣∣1tj−1tj−12tj−13tj−14tj−15∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣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 ∣∣1tktk2tk3tk4tk5−1−t0−t02−t03−t04−t05∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣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 Ax≤b
首先基于道路宽度和周围的障碍物找到点 ( s j , l j ) (s_j, l_j) (sj,lj)的下边界 l l b , j l_{lb,j} llb,j，且 j ∈ [ 0 , m ] jin[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∣∣∣∣∣∣∣∣
最后
以上就是忧心哈密瓜为你收集整理的qp_spline_st_speed_optimizer_cn的全部内容，希望文章能够帮你解决qp_spline_st_speed_optimizer_cn所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
![[原] 快速上手：在R中使用CVXR](/uploads/reation/bcimg8.png)
发表评论 取消回复