常德网站建设网站优化_猎头公司排行_10000个免费货源网站_网站快速排名优化哪家好

本文为学习自动驾驶决策规划算法第二章第七节速度规划的学习笔记。

1 ST图

如上图假设有一个障碍物横向穿过自车轨迹，则对应的ST图如下图所示

自车可以规划如下图所示的速度，对于绿色轨迹，在$t_1$、$t_2$时刻，在$s$上自车都未到达障碍物位置，表示的是让行，让障碍物先过；
对于橙色轨迹，在$t_1$、$t_2$时刻，在$s$上自车都已超过障碍物位置，表示的是先行，自己先过；
这样就对应两个凸空间，橙色表示的是加速通过凸空间，绿色表示的是让行凸空间；

1.1 SL与ST之间的相互影响

初始时刻，自车沿直线匀速行驶
在第$i$个规划周期自车感知到有动态障碍物：

假设判断会与障碍物发生碰撞，如果自车轨迹不做任何变化，则无法做速度规划来避免碰撞，除非倒车，如下图所示，

可以在碰撞区域生成一个虚拟障碍物，重新做路径规划：

此时的ST图就变为下图所示：

可以看出当前帧的SL会影响当前帧的ST；
同理第$i$帧的轨迹加$i+1$帧的预测会影响$i+1$帧的轨迹规划，进而影响$i+1$帧的ST：


Apollo在不同的版本中对EMplanner不断地更新，早已取消SL，ST迭代机制，本文中后续内容SL只负责静态障碍物，ST只负责动态障碍物。

2 速度规划详解

2.1 生成ST图

前提是已有路径规划的结果，速度规划以规划的路径作为参考线。
第一步生成路径的index2s；
第二步将动态障碍物投影到以路径规划为坐标轴的frenet坐标系上，得到动态障碍物的$(s_0,l_0,\dot{s_0},\dot{l_0})$；

第三步，动态障碍物投影生成ST图：
假设障碍物在上述frenet坐标系下是匀速运动，即$\dot{s_0}$和$\dot{l_0}$都是常数，那么障碍物到达路径的时间是
$$\begin{array}{c}{t}_a=\frac{l_0}{\dot{l_0}}\end{array}$$
障碍物到达路径时的$s$坐标是：
$$\begin{array}{c}{s}_a=s_0+t_a\cdot \dot{s_0}\end{array}$$

有些场景还是需要结合路径规划的，比如前方有慢车、对向来车等。

2.2 动态规划

速度规划的起点为路径规划的起点，
$$\begin{array}{c}s=0\end{array}$$
$$\begin{array}{c}\dot{s}=|v|\end{array}$$
$$\begin{array}{c}\ddot{s}=|a|\end{array}$$
如下图所示，$s_{end}$表示路径规划的路径长度，$t_{end}$表示速度规划的时间最大值，影响$t_{end}$的因素可能有很多，会导致不同场景下这个值的变化范围比较大，而$t_{end}$很大的时候会导致动态规划的规模很大，实际上可以人为给$t_{end}$指定一个值，比如8s，速度规划不一定要规划到路径终点，动态规划的过程与路径规划中的动态规划类似：

如上图所示，速度规划的离散点间距较小，连接离散点的曲线可以直接用直线，动态规划的代价函数包括障碍物代价、加速度代价、推荐速度代价、jerk代价等等：
$$\begin{array}{c}dp\_cost=障碍物代价+加速度代价+推荐速度代价+jerk代价\end{array}$$

2.2.1 障碍物代价

点到线段的最短距离

$$\begin{array}{c}{d}_1=|{\mathbf{v}}_1|\end{array}$$
$$\begin{array}{c}{d}_2=|{\mathbf{v}}_1+{\mathbf{v}}_2|\end{array}$$
$$\begin{array}{c}{d}_3=h=\frac{|{\mathbf{v}}_1\times {\mathbf{v}}_2|}{|{\mathbf{v}}_2|}\end{array}$$
点到线段的最短距离为$d_1$或$d_2$或$d_3$，可以通过向量的点积来判断是哪一个：

如上图所示
左侧情况对应
$$\begin{array}{c}\{\begin{array}{l}{\mathbf{v}}_1\cdot {\mathbf{v}}_2＞0\\ {\mathbf{v}}_2\cdot {\mathbf{v}}_3＞0\end{array}\end{array}$$
中间情况对应
$$\begin{array}{c}\{\begin{array}{l}{\mathbf{v}}_1\cdot {\mathbf{v}}_2＜0\\ {\mathbf{v}}_2\cdot {\mathbf{v}}_3＜0\end{array}\end{array}$$
右侧情况对应
$$\begin{array}{c}\{\begin{array}{l}{\mathbf{v}}_1\cdot {\mathbf{v}}_2＜0\\ {\mathbf{v}}_2\cdot {\mathbf{v}}_3＞0\end{array}\end{array}$$
所以，如果${\mathbf{v}}_1\cdot {\mathbf{v}}_2$与${\mathbf{v}}_2\cdot {\mathbf{v}}_3$同号，则有：
$$\begin{array}{c}min\_d=min(|{\mathbf{v}}_1|,|{\mathbf{v}}_3|)\end{array}$$
如果${\mathbf{v}}_1\cdot {\mathbf{v}}_2$与${\mathbf{v}}_2\cdot {\mathbf{v}}_3$异号，则有：
$$\begin{array}{c}min\_d=\frac{|{\mathbf{v}}_1\times {\mathbf{v}}_2|}{|{\mathbf{v}}_2|}\end{array}$$
那么可以根据点到障碍物的距离，定义代价函数，比如
$$\begin{array}{c}cost\_obs=\{\begin{array}{ll}\infty & |min\_d|＜2\\ 衰减函数& 2＜|min\_d|＜3\\ 0& 3＜|min\_d|\end{array}\end{array}$$

2.2.2 加速度、jerk以及参考速度代价

如上图所示，已知起点的运动数据（式3、4、5），可得：
$$\begin{array}{c}\dot{s_1}=\frac{s_1=0}{t_1-0}\end{array}$$
$$\begin{array}{c}\ddot{s_1}=\frac{\dot{s_1}-|\mathbf{v}|}{t_1-0}\end{array}$$
$$\begin{array}{c}{s_1}^{\prime \prime \prime }=\frac{\ddot{s_1}-|\mathbf{a}|}{t_1-0}\end{array}$$
那么加速度代价就是：
$$\begin{array}{c}cost\_acc=w_{acc}\cdot {\ddot{s}}^2\end{array}$$
那么jerk代价就是：
$$\begin{array}{c}cost\_jerk=w_{jerk}\cdot s^{\prime \prime \prime 2}\end{array}$$
参考速度代价就是：

$$\begin{array}{c}cost\_ref\_speed=w_{ref}\cdot (\dot{s}-v_r{)}^2\end{array}$$
确定好代价函数后，利用动态规划计算到达第一行和最后一列离散点的最小代价和对应的路径（如下图中红色虚线区域内），就是动态规划的结果，也即是速度规划的粗解（如下图绿色折线或橙色折线）:

2.3 二次规划

如图黑色线段表示障碍物，绿色折线表示动态规划的粗解，那么紫色区域就是二次规划的凸空间：

与路径规划相似，速度规划的凸空间也有$s_{lb}$、$s_{ub}$、$\dot{s_{lb}}$、$\dot{s_{ub}}$，对于速度的上下界，在有障碍物的区域（如上图橙色虚线的区域），以障碍物的速度为上下界，另外路径本身有曲率，车辆也要满足一些运动约束：
$$\begin{array}{c}{a}_{ymax}=0.2g\end{array}$$
所以有
$$\begin{array}{c}{v}_2\le R\cdot a_{ymax}=\frac{a_{ymax}}{kappa}\end{array}$$
$$\begin{array}{c}0\le \dot{s}\le {(\frac{a_{ymax}}{kappa})}^{\frac{1}{2}}\end{array}$$
动力学约束：
$$\begin{array}{c}-6\le \ddot{s}\le 4\end{array}$$
不允许倒车约束：
$$\begin{array}{c}{s}_{i+1}\ge s_i\end{array}$$
连续性约束：
$$\begin{array}{c}{s}_{i+1}=s_i+\dot{s_i}dt+\frac{1}{2}\ddot{s_i}d{t}^2+\frac{1}{6}(\frac{{\ddot{s}}_{i+1}-\ddot{s_i}}{dt})d{t}^3\end{array}$$
$$\begin{array}{c}{\dot{s}}_{i+1}=\dot{s_i}+\ddot{s_i}dt+\frac{1}{2}\frac{{\ddot{s}}_{i+1}-{\ddot{s}}_i}{dt}d{t}^3\end{array}$$
另外二次规划的代价函数为：
$$\begin{array}{c}cost\_function=\sum w_1({\dot{s}}_i^2-v_{ref}{)}^2+w_2{\ddot{s}}_i^2+w_3{s_i^{\prime \prime \prime }}^2\end{array}$$

3 代码实现

代码实现
动态规划采样
s的范围从0开始到路径规划的path总长度，可以采用非均匀采样，s越小的越密，越大越稀疏；
t采用均匀采样，每0.5s采样一个点，最大8s；

s_list = [0:0.5:4.5,...5.5:1:14.5,...16:1.5:29.5,...32:2.5:54.5]；t_list = 0.5:0.5:8;  

代价计算
对于加速度代价，如果加速度超过动力学约束时，给较大的代价：

% 计算加速度代价，这里注意，加速度不能超过车辆动力学上下限
if cur_s_dot2 < 4 && cur_s_dot2 > -6cost_accel = w_cost_accel * cur_s_dot2^2;
else% 超过车辆动力学限制，代价会增大很多倍cost_accel = 100000 * w_cost_accel * cur_s_dot2^2;
end