appollo-二次规划ST速度优化

二次规划ST速度优化
from https://github.com/ApolloAuto/apollo/blob/master/docs/specs/qp_spline_st_speed_optimizer_cn.md

Tip: 为了更好的展示本文档中的等式，我们建议使用者使用带有插件的Chrome浏览器，或者将Latex等式拷贝到在线编辑公式网站进行浏览。

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

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

minimize12⋅xT⋅H⋅x+fT⋅xs.t.LB≤x≤UBAeqx=beqAx≤bminimize \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 都有沿着参考线的累加距离did_idi。每段的路径默认用5介多项式表示。多项式介数可以通过配置参数进行调整。

s=fi(t)=a0i+a1i⋅t+a2i⋅t2+a3i⋅t3+a4i⋅t4+a5i⋅t5s = 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首先定义cost1cost_1cost1以使路径更加平滑：

cost1=∑i=1n(w1⋅∫0di(fi′)2(s)ds+w2⋅∫0di(fi′′)2(s)ds+w3⋅∫0di(fi′′′)2(s)ds)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=1∑n(w1⋅0∫di(fi′)2(s)ds+w2⋅0∫di(fi′′)2(s)ds+w3⋅0∫di(fi′′′)2(s)ds)
然后，Apollo定义cost2cost_2cost2表示最后的S-T路径和S-T巡航路径（有速度限制且m个点）的差值：

cost2=∑i=1n∑j=1m(fi(tj)−sj)2cost_2 = \sum_{i=1}^{n}\sum_{j=1}^{m}\Big(f_i(t_j)- s_j\Big)^2 cost2=i=1∑nj=1∑m(fi(tj)−sj)2
同样地，Apollo定义了cost3cost_3cost3表示第一个S-T路径和随后的S-T路径（o个点）的差值：

cost3=∑i=1n∑j=1o(fi(tj)−sj)2cost_3 = \sum_{i=1}^{n}\sum_{j=1}^{o}\Big(f_i(t_j)- s_j\Big)^2 cost3=i=1∑nj=1∑o(fi(tj)−sj)2
最后得出的目标函数为：

cost=cost1+cost2+cost3cost = cost_1 + cost_2 + cost_3 cost=cost1+cost2+cost3
3 约束条件
3.1 初始点约束
假设第一个点是(t0t0t0, s0s0s0)，且s0s0s0在路径fi(t)f_i(t)fi(t), f′i(t)f'i(t)f′i(t), 和fi(t)′′f_i(t)''fi(t)′′上（位置、速率、加速度）。Apollo将这些约束转换为QP约束的等式为：

Aeqx=beqA_{eq}x = b_{eq} Aeqx=beq
3.2 单调约束
路线必须是单调的，比如车辆只能往前开。

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

∣1tjtj2tj3tj4tj5∣⋅∣akbkckdkekfk∣>∣1tj−1tj−12tj−13tj−14tj−15∣⋅∣akbkckdkekfk∣\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∣∣∣∣∣∣∣∣∣∣∣∣
如两个点分别处在不同的样条kkk和lll上，则：

∣1tjtj2tj3tj4tj5∣⋅∣akbkckdkekfk∣>∣1tj−1tj−12tj−13tj−14tj−15∣⋅∣alblcldlelfl∣\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 平滑节点约束
该约束的目的是使样条的节点更加平滑。假设两个段segkseg_ksegk 和segk+1seg_{k+1}segk+1互相连接，且segkseg_ksegk的累计值 s 为sks_ksk。计算约束的等式为：

fk(tk)=fk+1(t0)f_k(t_k) = f_{k+1} (t_0) fk(tk)=fk+1(t0)
即：

∣1tktk2tk3tk4tk5∣⋅∣ak0ak1ak2ak3ak4ak5∣=∣1t0t02t03t04t05∣⋅∣ak+1,0ak+1,1ak+1,2ak+1,3ak+1,4ak+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∣∣∣∣∣∣∣∣∣∣∣∣
然后，

∣1tktk2tk3tk4tk5−1−t0−t02−t03−t04−t05∣⋅∣ak0ak1ak2ak3ak4ak5ak+1,0ak+1,1ak+1,2ak+1,3ak+1,4ak+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
等式中得出的结果为t0t_0t0 = 0。

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

fk′(tk)=fk+1′(t0)fk′′(tk)=fk+1′′(t0)fk′′′(tk)=fk+1′′′(t0)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约束不等式，使用不等式：

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

∣1t0t02t03t04t051t1t12t13t14t15..................1tmtm2tm3tm4tm5∣⋅∣aibicidieifi∣≤∣llb,0llb,1...llb,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∣∣∣∣∣∣∣∣
同样地，对上边界lub,jl_{ub,j}lub,j，计算约束的不等式为：

∣1t0t02t03t04t051t1t12t13t14t15..................1tmtm2tm3tm4tm5∣⋅∣aibicidieifi∣≤−1⋅∣lub,0lub,1...lub,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 个点，为每个点jjj获取速度限制的上边界和下边界，例如vub,jv{ub,j}vub,j 和 vlb,jv{lb,j}vlb,j，约束定义为：

f′(tj)≥vlb,jf'(t_j) \geq v_{lb,j} f′(tj)≥vlb,j
即：

∣01t0t02t03t0401t1t12t13t14..................01tmtm2tm3tm4∣⋅∣aibicidieifi∣≥∣vlb,0vlb,1...vlb,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′(tj)≤vub,jf'(t_j) \leq v_{ub,j} f′(tj)≤vub,j
即：

∣01t0t02t03t0401t1t12t13t14..................01tmtm2tm3tm4∣⋅∣aibicidieifi∣≤∣vub,0vub,1...vub,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∣∣∣∣∣∣∣∣

appollo-二次规划ST速度优化相关推荐

YUV视频格式到RGB32格式转换的速度优化上篇（转）
YUV视频格式到RGB32格式转换的速度优化上篇 HouSisong@GMail.com 2007.10.30 tag: YUV,YCbCr,YUV ...
页面加载速度优化的12个建议
1.合并Js文件和CSS 将JS代码和CSS样式分别合并到一个共享的文件,这样不仅能简化代码,而且在执行JS文件的时候,如果JS文件比较多,就需要进行多次"Get"请求,延长加载速 ...
近邻取样插值和其速度优化
本文的主要内容转载自博客 <图像缩放算法 >或<图形图像处理-之-高质量的快速的图像缩放上篇近邻取样插值和其速度优化>,将其中的代码改写在自己的机器上进行实际测试,从而更加 ...
netty系列之:一个价值上亿的网站速度优化方案
文章目录简介本文的目标支持多个图片服务 http2处理器处理页面和图像价值上亿的速度优化方案总结简介其实软件界最赚钱的不是写代码的,写代码的只能叫马龙,高级点的叫做程序员,都是苦力活. ...
速度优化_网站打开速度慢？如何优化？
网站打开速度直接影响用户的第一道体验,打开速度偏慢,用户可能就没耐心继续等下去了,造成大量用户流失,今天我们谈谈如何优化网站打开速度: 网站打开速度慢?如何优化? 服务器配置服务器要安全.稳定性良好 ...
分享网页加载速度优化的一些技巧？
日期:2013-2-17 来源:GBin1.com 不管你是不是相信,在最近的几年里,互联网网页的大小已经显著增大了.由HTTP Archive研究得出的结果表明,目前平均一个页面的大小是1.25M ...
LaTeX的编译速度优化方案
1 运行字体如果是卡在 eu1lmr.fd 的话,是因为 XeLaTeX 在定位字体. 用管理员运行 fc-cache -fv 一般能够解决. 其它方法: https://www.zhihu.com ...
Ansible执行过程分析、异步模式和速度优化
Ansible系列(七):执行过程分析.异步模式和速度优化分类: Linux服务篇 undefined 我写了更完善的Ansible专栏文章:一步到位玩儿透Ansible Ansible系列文章:h ...
网站加载速度优化_您肯定要优化网站的加载速度。这是如何做。
网站加载速度优化 Do you remember this iconic scene from a very famous Star Wars Parody? 您还记得著名的<星球大战> ...

appollo-二次规划ST速度优化

appollo-二次规划ST速度优化相关推荐

最新文章

热门文章