自动驾驶决策规划算法-路径决策算法：速度规划

本文为学习的学习笔记。

1 ST图

1.1 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:.5]；
  t_list = 0.5:0.5:8;  

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

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