1.什么是Motion Planning
Motion Planning是在遵循道路交通规则的前提下，将自动驾驶车辆从当前位置导航到目的地的一种方法。
在实际开放道理场景下，自动驾驶要处理的场景非常繁杂：空旷的道路场景、与行人、障碍物共用道理的场景、空旷的十字路口、繁忙的十字路口、违反交通规则的行人/车辆、正常行驶的车辆/行人等等。场景虽然复杂，但都可以拆解为一系列简单行为(behavior)的组合:
将这些简单的行为(behavior)组合起来，就可以完成复杂的驾驶行为。
2、Motion Planning的约束条件(constraints)
Motion Planning是一个复杂的问题，它的执行过程需要满足很多约束条件：
2.1 车辆运动学约束
车辆运动受到运动学约束，比如它不能实现瞬时侧向移动，前驱的车辆必须依赖前轮的转向才能实现变道、转向等操作，在弯道上不能速度过快等等。通常我们采用单车模型(Bicycle Model)对车辆运动进行建模。
2.2 静态障碍物(Static Obstacle)约束
静态障碍物(Static Obstacle)是道路上静止的车辆、路面中间的石墩子等车辆不可行驶的区域。Motion Planning需要避开这些静态障碍物，避免与它们发生碰撞。解决碰撞的思路大概有两种：
1）将静态障碍物(Static Obstacle)在网格占位图中表示出来，然后检测规划路线是否与静态障碍物区域相交。
2）将车辆的轮廓扩大，比如扩展成一个圆形，然后检测障碍物是否与Circle发生碰撞。
2.3 动态障碍物约束
Motion Planning要实时处理行人、车辆等各种运动的障碍物，避免与障碍物发生碰撞事故。
2.4 道路交通规则约束
车辆在道路上行驶必须要遵守车道线约束规则(比如左转专用道只能左转、实线不能变道、路口必须遵守红绿灯的指示)和各种标志标牌的指示。
3、Motion Planning的优化目标
了解Motion Planning的约束条件之后，需要构造目标优化函数，然后最小化目标函数，从而获得在当前环境下的最优运动轨迹。目标函数的种类有很多，下面枚举一些常用的目标函数。
通过组合设计自己的目标优化函数，从而获得较好的Planning效果。
4、分级运动规划器(Hierarchical Motion Planning)
Motion Planning是一个异常复杂的问题，所以通常我们把它切分为一系列的子问题(Sub Problem)。比如Mission Planner、Behavior Planner、Local Planner、Vehicle Control等。
4.1 Mission Planner
Mission Planner关注High-Level的地图级别的规划；通过Graph Based的图搜索算法实现自动驾驶路径的规划。
4.2 Behavior Planner
Behavior Planner主要关注交通规则、其它道路交通参与者(自行车、行人、社会车辆)等等，决定在在当前场景下应该采取何种操作(如停车让行、加速通过、避让行人等等)。
Behavior Planner的实现方式比较常见的有几种：有限状态机(Finite State Machines)、规则匹配系统(Rule Based System)、强化学习系统(Reinforcement Learning)。
有限状态机中的State是各个行为决策，根据对外界环境的感知和交通规则的约束在各个状态之间转换。比如在路口红绿灯的场景，当路口交通灯为红色不可通行时，车辆会首先切换到Decelerate to Stop状态，然后在路口停止线完全停下来，进入Stop状态，并持续在Stop状态等待，直至交通灯变为绿色允许车辆通行，车辆进入Track Speed状态，继续前行。
Rule-Based System是通过一系列的分级的规则匹配来决定下一步的决策行为。比如交通灯绿色->通行；交通灯红色->停车等待。
基于强化学习的Behavior Planner系统如下：
4.3 Local Planner
Local Planner关注如何生成舒适的、碰撞避免的行驶路径和舒适的运动速度，所以Local Planner又可以拆分为两个子问题：Path Planner和Velocity Profile Generation。Path Planner又分为Sampling-Based Planner、Variational Planner和Lattice Planner。
最经典的Sampling-Based Planner算法是Rapidly Exploring Random Tree，RRT算法。
Variational Planner根据Cost Function进行优化调整，从而避开障碍物，生成安全的轨迹。
Lattice Planner将空间搜索限制在对车辆可行的Action Space。
Velocity Profile Generation要考虑到限速、速度的平滑性等。
Vehicle Control将Planner的规划结果转化为车辆的运动行为。
待阅读材料
- P. Polack, F. Altche, B. Dandrea-Novel, and A. D. L. Fortelle, “The kinematic bicycle model: A consistent model for planning feasible trajectories for autonomous vehicles” 2017 IEEE Intelligent Vehicles Symposium (IV), 2017. Gives an overview of the kinematic bicycle model.
- S. Karaman and E. Frazzoli, “Sampling-based optimal motion planning for non-holonomic dynamical systems,” 2013 IEEE International Conference on Robotics and Automation, 2013. Introduces the RRT* algorithm as an example of sampling-based planning.
- N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “CHOMP: Gradient optimization techniques for efficient motion planning,” 2009 IEEE International Conference on Robotics and Automation, 2009. Introduces the CHOMP algorithm as an example of applying calculus of variations to planning.
- M. Pivtoraiko, R. A. Knepper, and A. Kelly, “Differentially constrained mobile robot motion planning in state lattices,” Journal of Field Robotics, vol. 26, no. 3, pp. 308-333, 2009. Introduces the state lattice planning method.
参考链接
1、Course自动驾驶课程：Motion Planning for Self-Driving Cars
