无人驾驶的规划与控制（三）——动作规划（轨迹规划和速度规划）

1 无人驾驶的动作规划

动作规划的目的：是将上游行为决策（behavior）的宏观指令解释成一条带有时间信息的轨迹曲线（trajectory），最后给底层的反馈控制。

一种简单的思路是，将动作规划问题拆分成两个问题：

1. 轨迹规划（Trajectory Planning）：二维平面上的优化轨迹问题。
2. 速度规划（Speed Planning）：选定轨迹后，用什么样的速度行驶这条轨迹。

分开优化不一定能保证总体最优解，但在实际工程实践中，分开优化是更实际有效的解决方案。

2 轨迹规划

2.1 车辆模型

车辆的姿态向量为$\overline{x}=(x,y,\theta ,\kappa ,\upsilon )$

符号	含义
$(x,y)$	车辆在二维片面内的位置
$\theta$	车辆的朝向
$\kappa$	曲率，即朝向$\theta$的变化率
$\upsilon$	车辆的速度，即轨迹任意点的切线速度

这些姿态变量满足以下关系：
$$\begin{array}{rl}\dot{x}& =v\cos \theta \\ \dot{y}& =v\sin \theta \\ \dot{\theta }& =v\kappa \end{array}$$
轨迹相对于车辆姿态的系统关系为：
$$\begin{array}{l}\mathrm{d}x/\mathrm{d}s=\cos (\theta (s))\\ \mathrm{d}y/\mathrm{d}s=\sin (\theta (s))\\ \mathrm{d}\theta /\mathrm{d}s=\kappa (s)\end{array}$$

2.2 道路定义

轨迹规划算法非常依赖于地图中对于道路中心线（Center Line）的定义，用一个采样函数去定义道路，采样函数为$r(s)=\left[r_x(s),r_y(s),r_{\theta }(s),r_{\kappa }(s)\right]$，车辆姿态点和道路坐标系以及道路采样函数的关系满足：
$$\begin{array}{l}{x}_r(s,l)=r_x(s)+l\cos \left(r_{\theta }(s)+\pi /2\right)\\ y_r(s,l)=r_y(s)+l\sin \left(r_{\theta }(s)+\pi /2\right)\\ {\theta }_r(s,l)=r_{\theta }(s)\\ {\kappa }_r(s,l)={\left(r_{\kappa }(s{)}^{-1}-l\right)}^{-1}\end{array}$$

符号	含义
$s$	纵向位移，道路中心线切线方向位移
$l$	横向位移

假设对于某条道路Lane(k)，纵向宽度$l_k$不变。那么该道路可以标示为一个随着中心线横向位移的点集$\left\{p\left(s,l_k\right):s\in {\mathbf{R}}^{+}\right\}$，这样的一个坐标系统为SL坐标系统。

2.3 候选轨迹生成

首先，定义车辆的轨迹（Trajectory）为一个从[0,1]区间到车辆姿态向量集合$C=\{x\}$的连续映射：$\rho :[0,1]\to C$，或者理解为一系列车辆姿态的序列。

符号	含义
${\rho }_1(0)$	第一条道路的起点姿态
${\rho }_1(1)$	第一条道路的终点姿态
$q_{\text{init}}$	广义上的，初始姿态向量，$q_{\text{ init }}=\left(x_I,y_I,{\theta }_l,{\kappa }_l\right)$
$q_{\mathrm{e}\mathrm{n}\mathrm{d}}$	广义上的，目标姿态向量，$q_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}=\left(x_G,y_G,{\theta }_G,k_G\right)$

轨迹优化的目标，就是在所有可能轨迹中，筛选出满足边界条件的轨迹曲线，再寻找一条最平滑，Cost最低的曲线。候选轨迹曲线用类似路由寻径模块中的“撒点”采样生成，这样，一条轨迹就可以看成是沿着Lane的中心线纵向位移方向，连接不同Trajectory Point的平滑曲线。

这里，采用多项式螺旋线连接轨迹点，生成候选曲线。多项式螺旋线，代表了一类曲率用弧长（对应这里的s）多项式函数来表示的曲线簇。一般使用三阶或者五阶多项式螺旋线，曲率$\kappa$和轨迹弧长$s$的关系为：
$$三阶：k(s)=k_0+k_{1}s+k_2{s}^2+k_3{s}^3$$

$$五阶：k(s)=k_0+k_{1}s+k_2{s}^2+k_3{s}^3+k_4{s}^4+k_5{s}^5$$

三阶和五阶的区别为：三阶多项式会导致曲率的二阶导数$\mathrm{d}{\kappa }^2/\mathrm{d}{s}^2$（对应方向盘转速）不连续，而五阶可以同事保持曲率的一阶导数和二阶导数的连续性。

最后，最优利用梯度下降方法搜索，得到三阶（五阶）螺旋线连接的轨迹。

2.4 根据轨迹点构建有向图并搜索

在车辆模型，道路模型，轨迹点模型，和多项式螺旋曲线的设定下，轨迹规划问题可以简化为：$\left|l_{\text{ total }}/\Delta l\right|\times \left|s_{\text{ total }}/\Delta s\right|$个轨迹点连接成的${\left|l_{\text{ total }}/\Delta l\right|}^{|s_{\text{ ual }}/\Delta r|}$条候选曲线的搜索问题。

所有的轨迹点构成了一个图$G=(V,E)$。

符号	含义
$l_{total}$	道路总宽度
$\Delta l$	宽度的最小采样长度
$s_{total}$	道路总长度
$\Delta s$	长度的最小采样长度
$V$	轨迹点，$v\in V,v=(x,y,s,l)$
$E$	三阶（五阶）多项式螺旋曲线

3 速度规划

3.1 S-T图算法

速度规划的任务，是在动作规划的轨迹上，考虑下游执行限制和行为决策结果，在每个轨迹点上，加入速度和加速度信息。

速度规划主要考虑对动态障碍物的规避，这里引入S-T（纵向位移 - 时间）的概念，并把无人车的速度规划问题，抽象到在S-T图上搜索的问题进行求解。

任何一个S-T图都基于一条已经给定的轨迹曲线，根据预测模块对动态障碍物的轨迹预测，每个动态障碍物都会在这条给定的轨迹上有所投影，从而产生一定S-T区域的覆盖，这里举一个S-T图速度优化算法的例子。

图中，无人车正在思考轨迹规划要选取的换道轨迹，目标车道有前车b，后车a，则a和b在轨迹上的投影就是平行于s轴的线段，随着t的增加，这个投影平行四边形也会不断向右延伸。

模仿轨迹规划的地图划分，把地图分成小网格（Lattice Grid），对每个网格赋予Cost，最终速度规划问题就可以归纳为：从起点$(s=0,t=0)$到终点$(s=S_{end},t>0)$在网格上寻找最小路径Cost的搜索问题。

图中3种方案分别为：

方案	说明
Speed Plan 3	加速直接超过a和b
Speed Plan 2	超过a（Overtake），然后让b先过（Yield）
Speed Plan 1	让车a和b，一直跟在后面

3.2 设置Cost

根据决策信息，可以灵活调整障碍物周边的Cost，来达到速度调整的目的。例如，

上游决定对a超车，就在S-T图上将a运动轨迹上方网格的Cost调小；
上游决定对b让车，就在S-T图上将b运动轨迹下方网格的Cost调小；
为了避免碰撞，所有动态障碍物轨迹经过的网格Cost都需要调大。

因此，设置Cost是速度规划中S-T图算法的关键，在这个基础上，利用A*或者Dijkstra搜索算法，就能得到最小Cost轨迹。在得到了轨迹之后，通过计算任意轨迹点处的一阶导数和二阶倒数，就能计算出速度和加速度，从而完成速度规划的计算。

参考文章

《第一本无人驾驶技术书》刘少山

在Typora中输入公式

Typora开启行内公式

常用数学符号的 LaTeX 表示方法

一份不太简短的LATEX2ε介绍【精华】