无人驾驶的规划与控制（三）——动作规划（轨迹规划和速度规划）
无人驾驶的动作规划旨在将宏观的指令转化为具体的轨迹曲线，其核心目的是通过优化轨迹和速度来实现安全和高效的路径规划。动作规划分为轨迹规划和速度规划两个主要部分：轨迹规划通过优化二维平面上的轨迹，而速度规划则在已知轨迹的基础上，结合动态障碍物的预测信息，调整速度以避免碰撞并提高效率。轨迹规划涉及车辆模型的定义、道路中心线的采样与定义，以及基于多项式螺旋线的轨迹生成方法。速度规划则采用S-T图算法，通过设置网格内的成本值，利用搜索算法找到最优速度路径。这种方法在无人驾驶中具有重要的应用价值。
文章目录
无人驾驶的运动规划方案
路径规划系统
2.1 车辆动态模型
2.2 道路边界条件设定
2.3 潜在路径生成
2.4 基于轨迹节点构建有向图并进行路径搜索
- 3 速度规划
-
- 3.1 S-T图算法
- 3.2 设置Cost
1 无人驾驶的动作规划
动作规划的主要任务是将上游的行为决策（behavior）转换为一条具有时间信息的轨迹曲线（trajectory），并最终传递给底层的反馈控制系统。
一种简单的思路是，将动作规划问题拆分成两个问题：
- 轨迹规划（Trajectory Planning）：二维空间内优化运动轨迹的问题。
- 速度规划（Speed Planning）：确定了轨迹之后，采用什么样的速度沿这条轨迹行驶。
分开优化未必能确保全局最优解，但在工程实践中，分开优化方案通常被视为更实际且有效的解决方案。
2 轨迹规划
2.1 车辆模型
车辆的姿态向量为\overline{x}=(x,y,\theta,\kappa,\upsilon)
| 符号 | 含义 |
|---|---|
| (x,y) | 车辆在二维片面内的位置 |
| \theta | 车辆的朝向 |
| \kappa | 曲率，即朝向\theta的变化率 |
| \upsilon | 车辆的速度，即轨迹任意点的切线速度 |
这些姿态变量遵循以下运动学关系：
\begin{aligned} \dot{x} &=v \cos \theta \\ \dot{y} &=v \sin \theta \\ \dot{\theta} &=v \kappa \end{aligned}
轨迹相对于车辆姿态的运动关系体现为：
\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{cases}
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{cases}
| 符号 | 含义 |
|---|---|
| s | 纵向位移，道路中心线切线方向位移 |
| l | 横向位移 |
给定某条道路Lane(k)，其纵向尺寸l_k保持不变。那么，该道路可以表示为一个随着中心线横向位移的点集{p(s, l_k) : s ∈ ℝ⁺}，这样的坐标系统即为SL坐标系统。
2.3 候选轨迹生成
首先，具体定义车辆的运动轨迹（Trajectory）为一个从区间[0,1]到车辆姿态向量集合C=\{\vec{x}\}的连续映射，即\rho :[0,1] \rightarrow C。这一轨迹可以理解为一系列车辆姿态的有序序列，表示车辆在区间[0,1]内的连续运动状态。
| 符号 | 含义 |
|---|---|
| \rho_{1}(0) | 第一条道路的起点姿态 |
| \rho_{1}(1) | 第一条道路的终点姿态 |
| q_{\text {init}} | 广义上的，初始姿态向量，q_{\text { init }}=\left(x_{I}, y_{I}, \theta_{l}, \kappa_{l}\right) |
| q_{\mathrm{ end}} | 广义上的，目标姿态向量，q_{\mathrm{goal}}=\left(x_{G}, y_{G}, \theta_{G}, k_{G}\right) |
轨迹优化的目的是在所有可能的轨迹中，选择满足边界条件的轨迹曲线，然后寻觅一条最平滑且成本最低的曲线。这些候选轨迹曲线通过类似于路由寻径模块中的散布采样点生成，每条轨迹均可被视为沿着车道中心线纵向位移方向，连接不同轨迹点的平滑曲线。
在轨迹点之间采用多项式螺旋线作为连接路径，以生成候选曲线。多项式螺旋线属于一类通过弧长s的多项式函数来表示曲率的曲线族。通常选择三阶或五阶多项式螺旋线，其曲率κ与轨迹弧长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|^{\left|s_{\text { ual }} / \Delta r\right|}条候选曲线的搜索问题。
所有的轨迹点构成了一个图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的推进，这个投影形成的平行四边形也会持续向右发展。
改写说明
图中3种方案分别为：
| 方案 | 说明 |
|---|---|
| Speed Plan 3 | 加速直接超过a和b |
| Speed Plan 2 | 超过a（Overtake），然后让b先过（Yield） |
| Speed Plan 1 | 让车a和b，一直跟在后面 |
3.2 设置Cost
根据决策信息，可以动态调整障碍物周边的Cost，以实现速度控制的目的。例如，可以通过调整障碍物周围的Cost来实现速度控制。
上游采取a超车行动，在S-T图中将a运动轨迹上方网格的Cost降低；
上游采取b让车行动，在S-T图中将b运动轨迹下方网格的Cost降低；
为确保不发生碰撞，所有动态障碍物的运动轨迹经过的网格Cost必须调高。
因此，设定Cost在速度规划的S-T图算法中扮演着关键角色。在此框架下，采用A*或Dijkstra搜索算法，可以得到最优Cost轨迹。在获得轨迹后，通过计算轨迹上任一点的一阶导数和二阶导数，可以推导出速度和加速度，从而完成速度规划的计算。
参考文章
《第一本无人驾驶技术书》刘少山