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

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)=[r_x(s),r_y(s),r_{\theta }$