规划决策篇：3.2 SL/ST图速度规划

直到世界尽头tk
3,687 阅读2分钟

Apollo算法的版本更迭

1. Apollo1.5

EMPlanner引入SL、ST解耦规划，引入凸空间，先决策后规划。

2. Apollo2.5

加入LatticePlanner。

3. Apollo3.5

取消了SL\ST迭代机制，SL只管静态障碍物，ST只管动态障碍物，加入场景模块(scenario模块，不同场景使用不同的算法)。

4. Apollo5.0

取消了SL的dp动态规划模块。

5. Apollo6.0

变动不大。

为什么取消SL/ST迭代机制

车辆的纵向变速能力远强于横向变速能力，SL/ST迭代机制中，ST会影响到SL规划，如果动态障碍的速度剧烈变化，那么ST图也会剧烈变化，则SL也会随之剧烈变化，导致几何线型不稳定。

特定场景需要使用ST去调整SL

1.前方同向行驶的车辆速度很小，自车的速度很大；

2.前方有反向行驶的车辆迎面而来，即对向而来。

这种情况下需要由感知模块对障碍物进行跟踪，生成虚拟障碍物，交付给SL规划模块去重新规划线型。

速度规划方案整体流程

SLSL图只管静态障碍物，STST图只管动态障碍物，少数场景下STST图会影响SLSL图。

简化考虑动态障碍物为质点模型(所以STST图是斜直线)，运动模型为SLSL空间下的匀速或者匀加速运动。

1.由动态障碍物生成STST图

2.基于ST图进行动态规划生成凸空间

3.基于凸空间进行二次规划

4.轨迹后处理

具体方案

生成ST图

本周期，以本周期规划的路径为参考线(未和上周期轨迹进行拼接的轨迹)，将动态障碍物质点进行投影，并且计算出其纵向速度s0˙\dot{s_0}和横向速度l0˙\dot{l_0},由于我们假设障碍物在SLSL空间下进行匀速运动，所以s0˙\dot{s_0}和l0˙\dot{l_0}不变。

同时我们要讨论障碍物点是在行车范围内，还是在行车范围外：

  • 障碍物在行车范围外：∣l0∣>∣K∣|l_0|>|K|

image.png

考虑障碍物点到达轨迹线的时刻为tt，同时考虑一个安全距离KK，则：

t=−l0/l0˙t= -l_0/\dot{l_0}

障碍物点刚刚切入安全边界的时刻为tint_{in}，刚刚切出安全边界的时刻toutt_{out}：

tin=min(t+K/l0˙,t−K/l0˙)tout=max(t+K/l0˙,t−K/l0˙)t_{in}=min(t+K/\dot{l_0},t-K/\dot{l_0})\\ t_{out}=max(t+K/\dot{l_0},t-K/\dot{l_0})\\

若t<0t<0，则说明障碍物是逐渐远离规划轨迹的，对该障碍物不给予考虑。

  • 障碍物在在行车范围内:∣l0∣<=∣K∣|l_0|<=|K|
tin=0tout=(K−∣l0∣)/∣l0˙∣t_{in}=0\\ t_{out}=(K-|l_0|)/|\dot{l_0}|\\

然后我们可以计算障碍物在刚刚切入行车范围边界时、刚刚切出行车范围边界时的ST图上的点，将两点进行连接，即可得到该动态障碍物的ST图

sin=s0+tin∗s0˙sout=s0+tout∗s0˙s_{in}=s_0 + t_{in} * \dot{s_0}\\ s_{out}=s_0 + t_{out} * \dot{s_0}

image.png

  • 注意

我们需要依据动态障碍物的具体运动情况来决定是否生成虚拟障碍物，如果当前感知到的动态障碍物：

  1. 横向速度(ll方向速度l˙\dot{l})很小且横向距离很大(ll很大)，则不考虑该动态障碍物；
  2. 横向速度很小且横向距离很小，则该动态障碍物可能堵住车辆前进轨迹，感知模块需要生成虚拟障碍物。大概逻辑：感知模块需要跟踪该障碍物，判断相邻帧之间看到的障碍物是否为同一个障碍物，速度规划模块拿到虚拟障碍物后做决策(怎么绕)，然后下一帧路径规划拿到虚拟障碍物标签后做路径规划，然后速度模块进行速度规划，绕过去；
  3. 横向速度较大则将其投影到STST图。

动态规划

STST速度规划中的动态规划整体逻辑与SLSL路径规划的整体逻辑一致，细节稍有出入，首先离散点散布的更加稠密，相邻层之间的路段权重计算稍有不同。

确定规划起点

已知上周期的规划轨迹点，当前时刻为tt，以t+dtt+dt时间的点位(即路径规划的起点)作为速度规划的起点。

规划时间长短

STST图中的纵轴SS的最大值依据SLSL规划中的最大值SendS_{end}来确定，那么横轴的最大时间TendT_{end}如何确定，我们需要考虑未来多长的时间呢？这个时间受到很多因素的影响，首先我们不可能把这个时间定的过长，这样程序计算时间很长，时间也不能定的太短。但是与路径规划不一样的是，我们没有必要一定要把SendS_{end}或者TendT_{end}内全部规划完。所以我们暂定Tend=8sT_{end}=8s。

布设离散点

由于机动车在纵向（沿参考线的前进方向）上的机动变速能力很强，远强于横向机动能力，所以我们纵向离散点的布设间距可以采用不均匀采样方案，离规划起点较近的路段间距小，较远的路段间距大一些。时间维度每0.5s采样一个点。

路段权重计算

由于采样密度增加了，相邻层之间点与点的连线（即路段）不再使用五次多项式而是使用直线代替。

路段权重由障碍物代价、加速度代价、jerkjerk代价、推荐速度代价组成。

  • 障碍物代价

对于一条具体的边（路段），我们在线段上均匀采样NN个点，分别计算每个点到障碍物STST图的代价，点到STST图的代价是点到线段最短距离disdis的函数，某路段上的采样点到某障碍物STST图的距离计算示意图：

image.png

对于路段上某个具体的点pp，计算其到STST线段的最短距离，需要先判断点在线段的内侧还是外侧，通过向量的点乘即可判断，点到STST线段的最小距离disdis计算如图所示：

image.png 图中两个向量叉乘的几何意义为：代表这两个向量围成的平行四边形的面积。

我们定义某路段的点ii(共采样55个点)到第jj个STST线段的障碍物代价costobi,jcost^{i,j}_{ob}为：

image.png 其中(disdis为点到线段的最短距离)

假设共有KK个障碍物的STST图，则某路段的障碍物代价权重LinkCostobLinkCost_{ob}

LinkCostob=wob∗∑k=1K∑i=15costobi,kLinkCost_{ob}=w_{ob}*\sum_{k=1}^K\sum_{i=1}^5cost^{i,k}_ {ob}\\
  • 加速度代价和jerkjerk代价

设规划起点的的SLSL坐标为(s0,l0s_0,l_0),有s0=0,l0=0s_0=0,l_0=0，笛卡尔坐标系下的速度大小为v0v_0，加速度大小为a0a_0,则有s0˙=v0,s0¨=a0\dot{s_0}=v_0,\ddot{s_0}=a_0，由起点计算到第一层各点的路段的速度、加速度、jerkjerk(速度三阶导数)：

image.png

直接简单差分即可计算得到速度规划起点到第一层各点路段的速度、加速度和jerk值，加速度的平方乘相应的权重即可得到加速度代价，jerkjerk值的平方乘相应的权重即可得到jerkjerk代价。

但是这里还有一个细节问题需要考虑，由于一条路段的速度、加速度、jerkjerk值的计算不仅与前序层状态有关，还与当前层状态值有关，当我们计算第二层各点到第三层各点的加速度、jerkjerk时，会发现前序层（第二层）各点的状态是不唯一的，以s12s_{12}为例，由于其前序层有多个点，所以其自身的状态也有多个值，我们应该记录从规划起点到s12s_{12}的最优路径在第一层中所经过的点xx，以这个点xx和s12s_{12}为前后状态计算得到路段状态stasta，将该状态stasta作为s12s_{12}向后计算的起始状态。

LinkCostaccel=waccel∗s¨2LinkCostjerk=wjerk∗jerk2LinkCost_{accel}=w_{accel}*\ddot{s}^2\\ LinkCost_{jerk}=w_{jerk}*{jerk}^2\\
  • 推荐速度代价

推荐速度vrv_r由导航给出，推荐速度代价为：

LinkCostexp=wexp∗(s˙−vr)2LinkCost_{exp}=w_{exp}*(\dot{s}-v_r)^2\\

计算出所有路段的权重后，进行动态规划搜路即可获得ST规划结果粗解。动态规划代码逻辑可参见：juejin.cn/post/714656…

生成凸空间

此步骤和路径规划的凸空间生成类似(依据动态规划的粗解生成凸空间)：juejin.cn/post/714656…

但是除了ss值的上下边界、还需要计算s˙\dot{s}的上下边界：由于我们已经知道了路径规划的结果ss序列和kappakappa序列，但是速度动态规划的ss序列和路径规划的ss序列不是完全吻合的，需要通过插值得到速度动态规划的点位斜率值，设允许的最大向心加速度值为ara_r，然后得到速度规划某点位最小最大速度s˙imin\dot{s}^{min}_{i}和s˙imax\dot{s}^{max}_{i}

s˙imin=0s˙imax=(ar/(∣kappai∣+0.0001))0.5\dot{s}^{min}_{i}=0\\ \dot{s}^{max}_{i}=(a_r/(|kappa_{i}|+0.0001))^{0.5}

二次规划

通过生成凸空间我们可以得到STST图中的凸空间：

convexS={(t1,s1min,s1max),(t2,s2min,s2max),...,(tn,snmin,snmax)}convexS=\{(t_1,s^{min}_{1},s^{max}_{1}),(t_2,s^{min}_{2},s^{max}_{2}),...,(t_n,s^{min}_{n},s^{max}_{n})\}

决策变量x\pmb{x}

x=[s1,s˙1,s¨1,s2,s˙2,,s¨2,...,sn,s˙n,s¨n]\pmb{x}=[s_1,\dot{s}_1,\ddot{s}_{1},s_2,\dot{s}_2,,\ddot{s}_{2},...,s_n,\dot{s}_n,\ddot{s}_{n}]

约束

  • 速度约束

若加速超过障碍物，则速度要大于动态障碍物的速度，减速反之。

  • 加速度约束

设允许的最大向心加速度为amaxra^{r}_{max}，某轨迹点的速度为viv_i，半径为rir_i，斜率为kik_i，依据向心加速度公式可得约束：

vi2/ri≤ amaxr→s˙i2/ri≤ amaxr→s˙i≤ (amaxr/ki)0.5v^{2}_{i}/r_{i} \leq\ a^{r}_{max} \rightarrow \dot{s}^{2}_{i}/r_{i} \leq\ a^{r}_{max} \rightarrow \dot{s}_{i} \leq\ (a^{r}_{max}/k_{i})^{0.5}\\
  • 动力学约束
−6≤ s¨i≤ 4 -6 \leq\ \ddot{s}_{i} \leq\ 4\\
  • 连续性约束

参考二次规划求路径的文章中分段加加速度法(juejin.cn/post/714944…

si+1=si+s˙i⋅dt+12⋅s¨i⋅dt2+16⋅(s¨i+1−s¨idt)⋅dt3s˙i+1=s˙i+s¨i⋅dt+12⋅(s¨i+1−s¨idt)⋅dt2s_{i+1} = s_i + \dot{s}_{i}·dt+\frac{1}{2}·\ddot{s}_{i}·dt^{2}+\frac{1}{6}·(\frac{\ddot{s}_{i+1}-\ddot{s}_{i}}{dt})·dt^{3}\\ \dot{s}_{i+1} = \dot{s}_{i} +\ddot{s}_{i}·dt+\frac{1}{2}·(\frac{\ddot{s}_{i+1}-\ddot{s}_{i}}{dt})·dt^{2}
  • 不允许倒车约束
si≤ si+1s_{i} \leq\ s_{i+1}

目标函数

目标函数考虑推荐速度代价、ST曲线平滑代价

cost=∑i=1n[wexp⋅(s˙i−vref)2+w2⋅(s¨i2)+w3⋅(s¨i2)]cost=\sum_{i=1}^n [w_{exp}·(\dot{s}_{i}-v_{ref})^2+ w_2·(\ddot{s}^2_{i})+ w_3·(\ddot{s}^2_{i})]

第三项应该是sis_i对时间的3阶导, makrdown好像打不出三个点。

考虑上述目标函数和约束可进行STST规划的求解。

后处理

ST规划结果增密

由于控制模块的执行频率是规划模块执行频率的十倍，如果规划结果的点位数量不够，会导致规划的效果不好，但是过多的待规划点位导致计算时间增长，考虑到个人电脑的算力问题，所以我们的方案是减少规划点位数目，待规划出结果后对轨迹进行增密，使用简单的质点运动学模型进行轨迹插值增密即可。

融合SL轨迹结果和ST速度结果

STST规划的点位比SLSL规划的点位要更加密集，所以需要以STST规划结果的ss序列对SLSL规划结果进行插值后融合。

拼接轨迹

与上周期轨迹进行拼接。

代码实现

写完再更。

参考来源

space.bilibili.com/287989852

avatar
文章
阅读
粉丝