【无人驾驶规划】BOSS无人车规划算法

1、boss运动规划结构

boss无人车运动规划结构如图所示

其中任务规划的任务就是全局的路由寻径；
行为规划的任务是结合全局路径信息和当前交通、障碍物等情况，给运动规划模块输入一系列的局部目标，比如说沿着一条车道运动到某个位置或者运动到一个特定的位姿或者停车位，同时也可以给运动规划模块输入特定的最大速度以及加减速指令等等；
运动规划接收行为规划的目标，生成安全运动轨迹。
运动规划分为两种模式，一种是on-road模式就是正常上路模式，另一种是unstructured driving比如停车场。
运动规划的流程：
1、根据给定的运动目标，规划一条路径；
2、生成一系列的轨迹去跟踪这条路径，这里涉及到轨迹生成；
3、从生成的轨迹中选取最优轨迹。
注：路径生成算法、轨迹评价函数等在不同场景下是不同的。

2、轨迹生成

boss采用模型预测的轨迹生成算法生成轨迹，从当前状态到目标状态。
模型预测轨迹生成算法论文：Howard, T., Kelly, A.: Optimal rough terrain trajectory generation for wheeled mobile robots.International Journal of Robotics Research 26(2), 141–166 (2007)
模型预测算法可以表示为：

其中x表示车辆状态，包括位置，朝向，曲率，速度；$u(p,x)$表示参数化的控制输入。

2.1、状态约束

车辆导航考虑三个约束，位置(x,y)和朝向($\theta$)，约束方程为：

其中$X_C$为目标状态，$X_F(p,x)$表示根据模型和控制策略预测出来的车辆状态，$X_I$为当前状态，$C(x,p)$表示目标状态和当前轨迹之差，也就是目标函数。理想状态下 C (