apollo 代码代码文档,转载从github
apollo文档
侵删
@[toc]# 二次规划ST速度优化
1 定义
从二次规划样条路径中选取一条路径后,Apollo将路线上的所有障碍物和自动驾驶车辆(ADV)展现在一个时间-路径图上(path-time ST),该路径图表示了路径上的站点变化。速度优化的任务是在ST图上找到一条合理的,无障碍的路径。
Apollo使用多个样条来表示速度参数,在ST图上表示为一系列的ST点。Apollo会对二次规划的结果做再次的平衡以获得最佳的速度参数。QP问题的标准类型定义为:
minimize21⋅xT⋅H⋅x+fT⋅xs.t.LB≤x≤UBAeqx=beqAx≤b
2 目标函数
2.1 获取样条段
将路ST速度参数分为 n 段,每段路径用一个多项式来表示。
2.2 定义样条段函数
每个样条段 i 都有沿着参考线的累加距离di。每段的路径默认用5介多项式表示。多项式介数可以通过配置参数进行调整。
s=fi(t)=a0i+a1i⋅t+a2i⋅t2+a3i⋅t3+a4i⋅t4+a5i⋅t5
2.3 定义样条段优化函数
Apollo首先定义cost1以使路径更加平滑:
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定义cost2表示最后的S-T路径和S-T巡航路径(有速度限制且m个点)的差值:
cost2=i=1∑nj=1∑m(fi(tj)−sj)2
同样地,Apollo定义了cost3表示第一个S-T路径和随后的S-T路径(o个点)的差值:
cost3=i=1∑nj=1∑o(fi(tj)−sj)2
最后得出的目标函数为:
cost=cost1+cost2+cost3
3 约束条件
3.1 初始点约束
假设第一个点是(t0, s0),且s0在路径fi(t), f′i(t), 和fi(t)′′上(位置、速率、加速度)。Apollo将这些约束转换为QP约束的等式为:
Aeqx=beq
3.2 单调约束
路线必须是单调的,比如车辆只能往前开。
在路径上采样 m 个点,对每一个 j和j−1 的点对,且(j∈[1,...,m]),如果两个点都处在同一个样条k上,则:
∣∣1tjtj2tj3tj4tj5∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣akbkckdkekfk∣∣∣∣∣∣∣∣∣∣∣∣>∣∣1tj−1tj−12tj−13tj−14tj−15∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣akbkckdkekfk∣∣∣∣∣∣∣∣∣∣∣∣
如两个点分别处在不同的样条k和l上,则:
∣∣1tjtj2tj3tj4tj5∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣akbkckdkekfk∣∣∣∣∣∣∣∣∣∣∣∣>∣∣1tj−1tj−12tj−13tj−14tj−15∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣alblcldlelfl∣∣∣∣∣∣∣∣∣∣∣∣
3.3 平滑节点约束
该约束的目的是使样条的节点更加平滑。假设两个段segk 和segk+1互相连接,且segk的累计值 s 为sk。计算约束的等式为:
fk(tk)=fk+1(t0)
即:
∣∣1tktk2tk3tk4tk5∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ak0ak1ak2ak3ak4ak5∣∣∣∣∣∣∣∣∣∣∣∣=∣∣1t0t02t03t04t05∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ak+1,0ak+1,1ak+1,2ak+1,3ak+1,4ak+1,5∣∣∣∣∣∣∣∣∣∣∣∣
然后,
∣∣1tktk2tk3tk4tk5−1−t0−t02−t03−t04−t05∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣ak0ak1ak2ak3ak4ak5ak+1,0ak+1,1ak+1,2ak+1,3ak+1,4ak+1,5∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣=0
等式中得出的结果为t0 = 0。
同样地,为下述等式计算约束等式:
fk′(tk)=fk+1′(t0)fk′′(tk)=fk+1′′(t0)fk′′′(tk)=fk+1′′′(t0)
3.4 点采样边界约束
在路径上均匀的取样 m 个点,检查这些点上的障碍物边界。将这些约束转换为QP约束不等式,使用不等式:
Ax≤b
首先基于道路宽度和周围的障碍物找到点 (sj,lj)的下边界llb,j,且j∈[0,m]。计算约束的不等式为:
∣∣∣∣∣∣∣∣11...1t0t1...tmt02t12...tm2t03t13...tm3t04t14...tm4t05t15...tm5∣∣∣∣∣∣∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣aibicidieifi∣∣∣∣∣∣∣∣∣∣∣∣≤∣∣∣∣∣∣∣∣llb,0llb,1...llb,m∣∣∣∣∣∣∣∣
同样地,对上边界lub,j,计算约束的不等式为:
∣∣∣∣∣∣∣∣11...1t0t1...tmt02t12...tm2t03t13...tm3t04t14...tm4t05t15...tm5∣∣∣∣∣∣∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣aibicidieifi∣∣∣∣∣∣∣∣∣∣∣∣≤−1⋅∣∣∣∣∣∣∣∣lub,0lub,1...lub,m∣∣∣∣∣∣∣∣
3.5 速度边界优化
Apollo同样需要建立速度限制边界。
在st曲线上取样 m 个点,为每个点j获取速度限制的上边界和下边界,例如vub,j 和 vlb,j,约束定义为:
f′(tj)≥vlb,j
即:
∣∣∣∣∣∣∣∣00...011...1t0t1...tmt02t12...tm2t03t13...tm3t04t14...tm4∣∣∣∣∣∣∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣aibicidieifi∣∣∣∣∣∣∣∣∣∣∣∣≥∣∣∣∣∣∣∣∣vlb,0vlb,1...vlb,m∣∣∣∣∣∣∣∣
且,
f′(tj)≤vub,j
即:
∣∣∣∣∣∣∣∣00...011...1t0t1...tmt02t12...tm2t03t13...tm3t04t14...tm4∣∣∣∣∣∣∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣aibicidieifi∣∣∣∣∣∣∣∣∣∣∣∣≤∣∣∣∣∣∣∣∣vub,0vub,1...vub,m∣∣∣∣∣∣∣∣