一、规划控制——理论体系概述

概要：本文主要学习规划控制的基本组成，以便更好地了解规划控制各模块的相关理论知识体系。

---

前言

无人车规划控制的任务基本就是对感知到周围环境的基础上结合无人车的目的地和当前环境的位置对车辆做出正确合理的决策和控制。

---

一、规划控制系统划分方法

整个决策控制规划的软件系统可以细化为以下几种方法：

1. 按照解决问题的不同层面或者按照解决问题时间窗口长短进行划分。如Breakdown into long，medium，short term horizons。
2. 按照空间域和时间域来划分。如Breakdown of spatial path vs velocity planning。
3. 基于算法的优劣来进行细化，如：Homotopy search vs optimization

最常用的是2007年DARPA（美国国防预先研究计划局） urban chanllenge常用的划分方法——按照解决问题的不同层面来进行划分。

按照自上而下解决方法分为如下几个模块：

1. Mission/Route Planning：路径规划
2. Behavior Planner/Decision marking：行为决策
3. Motion planning/Local planner：运动规划
4. Motion Control：运动控制
5. Vehicle：车辆

二、规划控制基本组成

1. 路径规划（Misson / Route Planning）

路径规划是指生成一条连接车辆起始位置和终点位置的一个几何路径。
路径规划分为：

6. 全局路径规划（route planning）：根据全局静态地图规划出一条从起始点到终点的一条可行的最优路径。
7. 局部路径规划（Misson planning）

路径规划作为无人车决策规划控制系统最上游的模块，其解决的问题是根据起始点和目的地计算出一条起点到终点最佳道路的一条行驶序列（lane order）。如图所示。

路径规划输入：起始点、目的地、路网（road graph）、cost function（损失函数）
路径规划输出：车道序列（lanes sequence）、道路分割（road segments）严格依赖高精地图的绘制。
原理：首先基于高精地图绘制的路网，在一定的范围内将所有可能经过的道路（lane）进行一些分布的撒点。我们称这些点为lane point，这些点代表了无人车可能经过的lane位置的抽样点，点和点之间是通过有向带权的边进行连接，而这就代表了点与点之间cost即潜在代价，cost包括避免变道，避免左转、右转，避免很窄的道路等等，也就是对其进行惩罚（penalize），而我们把路径规划的问题都可以利用graph search的方法进行实现。
graph search方法：

1. Dijkstra：
   Dijkstra 算法是一个基于「贪心」、「广度优先搜索」、「动态规划」求一个图中一个点到其他所有点的最短路径的算法，时间复杂度 O(n2)。
2. A*
   A*（念做：A Star）算法是一种很常用的路径查找和图形遍历算法。它有较好的性能和准确度。
   A*算法最初发表于1968年，由Stanford研究院的Peter Hart, Nils Nilsson以及Bertram Raphael发表。它可以被认为是Dijkstra算法的扩展。

基于其具体的算法原理及方法，后面文章会详细说到。

2. 行为决策（Behavioral Decision Making）

行为决策在整个无人车的规划控制系统（planning control system ）中扮演了一个副驾驶的角色，作为无人车在行驶过程中产生何种行为和决策的重要大脑。

行为决策汇集了很多重要的车辆信息，其中：

1. 车辆本身信息：当前的状态、位置、速度、朝向以及所处的车道、行径轨迹
2. 无人车一定距离范围之内感知的信息
3. 交通信息

行为决策所解决的问题就是知晓了上述所有信息上，结合人类的驾驶经验进行无人车行驶策略的设计。

行为决策输入：

1. Plan message：本车轨迹。
2. status message：位置、速度、朝向以及所处的车道。
3. forward message：无人车上一个行为决策周期中做出什么决策，如跟车、停车、转弯、超车、变道等。
4. obstacle message：如临近路口目标车辆状态信息、意图和预测轨迹。
5. Traffic recognition message：如一定范围内lane的变化情况、直行道到左转道路遇到的人性道、道路标志牌、红绿灯等。
6. human experience：驾驶人的驾驶风格和驾驶习惯。

行为决策输出：宏观的行为决策指令，供给下游的motion planning具体执行。

行为决策由于汇集了很多重要的不同类别的信息，所以该模块很难用单纯的一个数学模型去解决。
这里总结了行为决策规划的几种常用方法如下：

1. 有限状态机（Finite state Machine）
   有限状态机的运行机制大致是：
   （1）系统所处的状态是明确并且有限的，必定属于状态全集中的某一种；
   （2）系统接受输入，根据判定条件，决定是维持当前状态，还是切换到某一个新的状态；
   （3）在维持或切换的过程中，执行一些预设的操作。
   可以认为有限状态机是一个离散系统，每接受一次输入，进行一次判断和切换。

2. 基于规则的方法（Rule based approach：Minimum Vioaltion Planning）

3. 基于形式化的方法（Formal Methods（STL,LTL,process algebra, u-calulus））
   如：线性时间逻辑(LTL)：Linear-Time Logic，提供了一种非常直观但是在数学上又很精确的表示方法来描述线性时间性质。它是描述系统约束的形式化方法，是模态逻辑的一种，其中的模态算子用于指示时间的顺序，而非精确的时间。

4. 马尔可夫决策过程方法（Markov Decision Process）
   马尔可夫决策过程（Markov Decision Process，简称MDP）是强化学习中的一个重要概念，它是一种数学模型，用于描述决策者在不确定环境中进行决策的过程。

简而言之：行为决策是根据当前的交通场景和环境感知的信息的理解，来确定自身当前驾驶状态，在交通规则的约束和驾驶经验的指导下规划出合理的驾驶行为。

3. 运动规划（Motion Planning /Local Planning）

动作规划是在一个较小的时空区域内解决无人车从A点到B点的问题。也就是在遵循道路交通规则的前提下，将自动驾驶车辆从当前位置导航到目的地的一种方法。

相对于行为决策来说，动作规划要更加具体，具体解决无人车短暂时间内从A点到B点的路径规划包括：

1. 选择哪个路径以及具体的路径点。
2. 到达每一个路径点无人车的速度、朝向、加速度以及相关运动变量（加速度曲率、曲率的高位倒数等）。

动作规划输入：

1. 环境感知信息
2. 位置信息
3. 地图信息

动作规划输出：

1. 带有时间信息的轨迹

动作规划需要保证以下两点：

1. 后续时间内，生成A点到B点的时空轨迹需要保持一定的一致性。
2. 生成A点到B点的轨迹点需要保证每一个点的速度、朝向、加速度等等都是在下游反馈控制可以实际操作的物理范围之内。

简而言之，motion planning就是根据环境的感知、所处的位置具体将行为决策宏观指令解释成带有时间信息的轨迹曲线，来给最底层的反馈控制（motion control） 进行实际的对车的操作。

motion planning 一般可以拆分成两块：

1. 轨迹规划（trajectory planning）
   解决了二维平面上根据行为决策和地图信息，在定义好的cost函数下面进行优化的轨迹问题。
2. 速度规划（speed planning）
   在选定了轨迹之后，解决用什么速度来行驶的问题。

动作规划的常用方法如下：

1. Searching based Methods
2. Stochastic Sampling Methods
   Stochastic Sampling Methods也称随机采样方法，构象搜索的目的是找到目标的所有首选构象。这些构象与势能表面的局部最小值有关。更深的局部最小值可能对应于目标可观察的、部分稳定的状态。随机采样方法可用于探索势能表面，从而定位局部最小值。
3. MPC

简而言之，运动规划模块就是解决在一定约束范围下的一个优化问题。
回顾：

整体规划过程如下：
路径规划——行为决策——动作规划，如下图所示。

1. 路径规划：根据全局的静态地图、起点和终点的位置、损失函数来规划出一条最佳的道路行驶的序列。
2. 行为决策：通过接受路径规划的结果、感知预测的信息，综合决定无人车如何进行行驶，如跟车、换道、避障、遇到交通灯、行人等待等。
3. 运动规划：根据接受行为决策的输出结果来计算出我们具体需要经过哪一些路径点以及经过这些点的无人车的朝向、速度、加速度等信息。

4. 运动控制（Motion Control）

控制车辆尽可能遵循运动规划输出的时空轨迹（trajectory），该模块是直接和无人车底层控制接口CANBus对接的模块，其核心任务是消化上层规划模块输出的轨迹点。
通过结合自车车辆属性以及外界的物理因素的动力学计算转换成对车辆的线性控制（drive by wire）的控制（油门、刹车、方向盘）尽可能控制车辆去执行车辆沿轨迹的行驶。

运动控制输入：

1. 运动轨迹
2. 状态信息

运动控制输出：

1. 方向盘（Steering）控制
2. 油门（Throttle）控制
3. 刹车（Brake）控制

控制模块主要包含：

1. 车辆自身的控制
2. 外界物理环境交互的建模

常见控制方法如下：

1. PID
   PID算法是工业应用中最广泛算法之一，在闭环系统的控制中，可自动对控制系统进行准确且迅速的校正。PID算法已经有100多年历史，在四轴飞行器，平衡小车、汽车定速巡航、温度控制器等场景均有应用。
2. Stanley
   Stanley方法是一种基于横向控制跟踪误差为前轴中心到最近路径点的距离的非线性反馈函数，并能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可以直观的获得控制车辆方向盘转角的控制变量。
3. Pure pursuit
   Pure Pursuit方法，顾名思义，纯轨迹跟踪。它是一种基于几何追踪的路径跟随算法，由Craig Coulter于1992年提出。因控制方法较为简单且直接，无须过多考虑车辆的运动学与动力学模型，且调节参数少，可适用于自动驾驶车辆或移动机器人中。
4. LQR
   LQR (linear quadratic regulator)即线性二次型调节器，是现代控制理论中发展最早也最为成熟的一种状态空间设计法。LQR可得到状态线性反馈的最优控制规律，易于构成闭环最优控制。
5. MPC
   MPC全称可指Model Predictive Control模型预测控制（又称RHC, Receding Horizon ）。
   原理：在每一个采用时刻，根据获得的当前测量信息，在线求解一个有限时间开环优化问题，并将得到的控制序列的第一个元素作用于被控对象。在下一个采样时刻，重复上述过程：用新的测量值作为此时预测系统未来动态的初始条件，刷新优化问题并重新求解。
   特点：MPC最大的特点在于，相对于LQR控制而言，MPC可以考虑空间状态变量的各种约束，而LQR，PID等控制只能够考虑输入输出变量的各种约束。

---

总结

本文主要是对于自动驾驶规划控制学习的概述、主要介绍了规划控制系统的划分方法、规划控制中的基本组成模块，以及各个模块的主要原理、解决问题以及常见解决方法，这篇文章希望可以对想要了解、学习自动驾驶规划控制方向的同学们有一定的帮助。
喜欢的朋友们动动小手点个关注，我会定期分享我的一些知识总结和心得体会，感谢大家!