同济汽车余卓平教授：无人车运动规划算法综述

原创 2017-10-16 余卓平教授 厚势

厚势按：本文为同济大学汽车学院余卓平教授、李奕姗博士、熊璐教授发表在 2017 年 8 月出版的《同济大学学报（自然科学版）》上关于无人驾驶汽车运动规划算法的综述论文。作为无人驾驶汽车感知、决策与规划、执行的三大组成部分之一，运动规划环节极为重要！

论文回顾了无人驾驶汽车的运动规划问题，其运动受微分约束，且运行环境既包括结构化的道路也包括非结构化的旷野。根据具有阿克曼转向性质的车辆模型所具有的微分平坦性质，可以简化无人驾驶汽车的轨迹生成问题。此外，相比直接轨迹生成法，路径-速度分解法更常用，回旋线、样条曲线、多项式螺旋线是使用较多的路径生成曲线。

其中，具有重要实用意义的两大类无人驾驶汽车运动规划算法分别是：以快速随机扩展树算法（RRT）为代表的基于采样的规划算法和以 A* 搜索算法为代表的基于搜索的规划算法。

运动规划是与机器人、人工智能和控制理论等学科息息相关的一个研究领域，其任务是规划出驱使机器人从当前位置运动到目标位置的一系列指令 [1]。随着智能汽车的兴起，以无人车为研究对象的运动规划问题越来越受到重视。

传统机器人领域的运动规划算法包括概率路图法周 [2] 、可视图法 [3]、细胞分解法 [4] 等基于几何模型的搜索算法，以及基于虚拟势场和导航函数的人工势场方法 [5] 等。

近年来新的运动规划算法不断出现和发展，在无人车运动规划领域具有代表性的是基于随机采样的快速随机扩展树法 [6] 和各种基于搜索的状态格子算法 [7]。这些算法已经被成功用于 2007 年 DARPA 挑战赛各个参赛队伍的无人车辆上。例如，麻省理工学院（MIT）参赛车辆使用基于随机采样的算法获得了比赛第四名 [8]。此外，国内也对运动规划算法进行了大量的研究。例如，北京理工大学的 INBOT 车辆利用搜索算法 [9] 在中国智能车挑战赛中取得佳绩。

本文总结了目前文献中经常出现的各种适用于无人车运动规划的算法，分别从理论、算法实时性等方面分析比较了它们在理论上的优势和缺点，为今后的深人研究提供参考。

1. 问题定义

当给定车辆的几何形状和动力学模型、所处环境障碍物的分布情况、以及一个初始状态和一个目标状态集之后，运动规划的任务就是找出一系列控制输人，驱动车辆从初始状态运动到目标状态，并且在运动过程中避免和障碍物发生碰撞。

图 1 车辆模型和规划算法的关系

为了更好地描述运动规划问题以及规划算法，必须选择正确描述车辆模型。车辆模型与规划算法的关系如图 1 所示。

图 2 车辆模型

目前最常用的是具有阿克曼转向性质的车辆模型。该模型将车辆视为平面刚体，具有 3 个自由度，构形空间 C 为二维特殊欧式群 SE（2），选取后轴中心 R 作为参考点。如图 2 中，车辆（轴距为 b）在大地坐标系下的坐标为（x，y），航向角为 θ，那么车辆位置唯一确定。另外，在转向时，在方向盘转角为 Φ 的情况下，车辆转向路径的半径 r 和曲率 κ 分别如图 2 所示。车辆的状态方程见式（1）。

车辆可以看作是构形空间中的一点，构形空间中与障碍物发生干涉的点的集合为构形空间障碍 C_obstacle，构形空间自由区域为 C_free，连续映射 τ ：[0,1] → C_free 称为构形空间中的一条可行路径。可行路径并不能反映车辆所受到的非完整约束，在构形空间上再加上速度、曲率的维度，则得到状态空间。类似于构形空间，可以定义状态空间的起始构型 X_init 和终止构型 X_goal，障碍区域 X_obstacle和自由区域 X_free，可行轨迹在构形空间中的投影就是可行路径。

因此，运动规划问题可用三元组（X_free， X_init，X_goal）来描述，规划算法就是要找出状态方程（1）的一系列输人，其对应的轨迹是状态空间中的可行轨迹，起始于初始状态，最终到达目标状态集中。

对于像无人车这样具有连续状态空间的运动规划问题，计算复杂度都是非常高的。为了提高算法效率和实用性，必须降低算法对完备性的要求。基于随机采样和基于网格搜索的算法是目前常用的规划算法，它们分别具有概率完备性和解析度完备性，从而降低算法的计算复杂度。

2. 轨迹生成

2.1 轨迹生成与非线性控制

轨迹生成是各类无人车运动规划算法的一个基本程序，其目的是构造状态空间中的一条轨迹（不考虑障碍物）连接任意给定的两个状态。这一问题的解决方法一般有幂零法 [10]、数值计算法 [11]、Chained Form [12] 以及微分平坦法 [13]，其中：

幂零法可以求得一系列分段常数的控制输人，其对应轨迹可连接任意给定的初始状态和目标状态，但是运动规划采用的无人车模型是不满足幂零的条件的；
数值计算方法往往用来求解最优轨迹，但是求解效率较低；
对于可转化为 Chained Form 的系统，可以使用三角函数或多项式函数精确求解控制输人，但是多数情况下其轨迹过于复杂；
微分平坦是某些非线性系统具有的一种良好性质，对于这类系统可以通过构建系统平坦输出来解决轨迹生成问题，是目前常用的方法。

运动规划所采用的车辆模型具有微分平坦性质，其平坦输出为后轴中心 R 的坐标。因此，轨迹生成就是要构造 R 的一条足够光滑且满足各种有界性的轨迹来连接两个给定状态。

2.2 轨迹生成方法

常见的轨迹生成方法有两大类，直接构造法和路径-速度分解法。

直接构造法是指直接构造车辆后轴中心 R 坐标关于时间的函数。文献 [14] 针对非结构化环境使用 5 次多项式来构造满足初始和终止位置、速度和加速度的轨迹，而且 5 次多项式使得加速度变化率最小，有利于车辆的平稳行驶。但是这一方法需要多次调整轨迹的时间区间，来保证整条轨迹上速度、加速度、曲率和曲率变化率的有界性。文献 [15] 在 [14] 的基础上针对结构化环境中的轨迹生成方法作出了改进，计算道路中线的 Frenet 标架下轨迹的坐标，可以适应各种道路环境下的轨迹生成问题。

路径一速度分解法 [16] 最初是为了解决有移动障碍物的环境下运动规划的问题而提出的，先构造一条避开静态障碍物的路径，再在路径上规划速度，避开移动障碍物。对于轨迹生成而言无需考虑障碍物，只需要考虑无人车的运动学和动力学约束。直接轨迹生成法要一次性满足速度、加速度、曲率和曲率导数的有界性，难度较大；而路径-速度分解法先生成一条曲率连续有界的路径，然后在路径上生成连续有界的速度，同时保证有界的加速度和有界的曲率导数，降低了轨迹生成的难度。

（a）路径生成

Dubins 曲线 [17] 和 Reeds and Shepp（RS）曲线 [18] 是连接构形空间中任意两点的最短路径，分别对应无倒车和有倒车的情况。它们都是由最大曲率圆弧和直线组成的，在圆弧和直线连接处存在曲率不连续，实际车辆按照这样曲线行驶时必须在曲率不连续处停车调整方向轮才能继续行驶。

回旋线 [19] 的曲率与曲线长度成正比关系，可以用作直线到圆弧之间的过渡曲线，从而改造Dubins 曲线和 RS 曲线，实现曲率连续性，比较有代表性的是 CC-Steer [20]，适用于低速下的运动规划。

多项式螺旋线 [21] 的曲率是曲线长度的多项式函数，回旋线是一种特殊的多项式螺旋线。根据微分几何理论，给定函数献则，κ(s)整个曲线的形状就完全确定了。但是为了求解曲线形状，在给定边界条件后，必须使用数值手段求解多项式中的待定系数，求解效率较低。

样条曲线具有封闭的表达式，容易保证曲率连续性。B 样条曲线 [22] 可以实现曲率连续性，三次 Bézier 曲线 [23] 可以保证曲率的连续性和有界性。η^3 曲线 [24] 是一种七次样条曲线，它有着很好的性质：曲率连续性和曲率导数的连续性，这对于高速行驶车辆是很有意义的。

（b）速度生成

文献 [25] 通过对路径指定线加速度来生成速度。线加速度可以是某一常数，或是由比例一微分控制器来生成。

文献 [26] 研究了在给定时间内通过一段给定路径的速度生成问题，其解决方案是将时间域划分为若干区间，使用速度关于时间的三次样条函数来插值，但这一方法容易产生加速度变化率较大的问题。

文献[27]直接用速度关于路径长度的二次多项式来生成速度，这种方法较为简单。

3. 基于采样的算法

基于随机采样的运动规划算法的基本思路是：通过对状态空间均匀随机采样来构建一个连通图，当初始、目标状态都在图中或是都可以连接到图中时，则问题得以解决。基于随机采样的算法不需要对状态空间自由区域显式建模，轨迹的可行性由碰撞检测来验证。

典型的基于随机采样的算法有概率路图法 [28]（Probabilistic Road Map，PRM）和快速随机扩展树法 [6]（Rapidly Random Tree，RRT）。

3.1 基本算法 PRM/RRT

3.1.1 PRM 的构建

预处理阶段：对状态空间内的安全区域均匀随机采样 n 个点，每个采样点分别与一定距离内的邻近采样点连接，并丢弃掉与障碍物发生碰撞的轨迹，最终得到一个连通图。
查询阶段：对于给定的一对初始和目标状态，分别将其连接到已经构建的图中，再使用搜索算法寻找满足要求的轨迹。

容易看出，一旦构建一个 PRM 之后，可以用于解决不同初始、目标状态的运动规划问题，但是这个特性对于无人车运动规划而言是不必要的。另外，PRM 要求对状态之间作精确连接，这对于存在复杂微分约束的运动规划问题是十分困难的。

3.1.2 RRT 的构建

树的初始化：初始化树的结点集和边集，结点集只包含初始状态，边集为空。
树的生长：对状态空间随机采样，当采样点落在状态空间安全区域时，选择当前树中离采样点最近的结点，将其向采样点扩展（或连接）。若生成的轨迹不与障碍物发生碰撞，则将该轨迹加人树的边集，该轨迹的终点加人到树的结点集。
重复上述步骤，直至扩展到目标状态集中。

相比 PRM 的无向图而言，RRT 构建的是初始状态作为根结点、目标状态作为叶结点的树结构，对于不同的初始和目标状态，需要构建不同的树。另外，RRT 不要求状态之间的精确连接，更适合解决像无人车运动规划这样的运动动力学问题。

3.2 提高算法效率和求解质量的措施

基于随机采样的规划算法具有概率完备性。以 RRT 为例，随着采样点数目的增加，树最终会稠密地充满整个状态空间自由区域，找到可行解的概率是收敛到 1 的。

然而，除了概率完备性，人们更希望算法有更高的效率，且能求得更高质量的解。为此，标准 RRT 在以下几方面作了改进。

（a）均匀采样

标准 RRT 算法对状态空间均匀随机采样，当前树中结点获得扩展的概率与其区域面积成正比，所以树会向着状态空间的空旷区域生长，均匀充满状态空间的自由区域。盲目的扩展伴随着大量的碰撞检查，严重影响程序效率。

文献 [29] 提出 RRT-Connect 算法，同时构建两棵分别起始于初始状态和目标状态的树，当两棵树生长到一起时则找到可行解。Goal-biasing [30] 是指在随机采样序列中以一定比例插人目标状态，引导树向目标状态扩展，加快求解速度，提高求解质量。

文献 [31] 提出启发式 RRT 算法（Heuristic RRT，hRRT），使用启发式函数增加扩展代价低的结点被采样的概率，这个函数要求计算树中每个结占的代价，但是在复杂环境中，代价函数的定义往往是很困难的。

文献 [32] 提出 f-biased 采样方法，先将状态空间离散化为网格，再使用 Dijkstra 算法计算每个网格上的代价，这个网格所在区域的点的代价值都等于该值。利用这样的代价函数可以构建更好的启发式函数来引导树的扩展。

文献 [33] 中结合驾驶员视觉注意力模型进行偏向性采样中，利用视觉特征信息引导运动规划，使规划出的轨迹更符合人类到驶行为。

（b）距离度量

「距离」用来度量构形空间（状态空间）中两个构形（状态）之间路径（轨迹）的代价，这个代价可以理解为路径的长度、消耗的能量或是花费的时间。

不同于欧氏空间，车辆的构形空间或状态空间中距离没有闭形表达式，其计算难度等价于轨迹生成的难度；如果考虑障碍物的话，距离的计算难度是等价于整个运动规划的难度的。因此，运动规划中距离的定义是采用类似欧氏距离的表达式。文献 [34] 给出构形空间 SE(3) 中加权距离表达式：

类似这些距离的表达式并不能真实反映构形或状态之间的远近。然而，标准 RRT 根据采样点选择树中临近点进行扩展时，要使用这样的距离表达式来比较远近，难免会带来误差，影响 RRT 探索整个状态空间的能力。RG-RRT [35]（Reachability Guided RRT）可以消除不准确的距离对 RRT 探索能力的影响。RG-RRT 计算树中结点的能达集，当采样点到结点的距离大于采样点到该结点能达集的距离时，该节点才有可能被选中进行扩展。

（c）障碍物

碰撞检查是基于采样的算法的效率瓶颈之一，通常的做法是对路径等距离离散化，再对每个点处的构形作碰撞检查。

文献 [36] 提出 RC-RRT（Resolution Complete RRT）来降低靠近障碍物的结点获得扩展的概率，具体做法是对输人空间离散化，对于某个结点，其某个输人只能使用一次；若某个输人对应的轨迹与障碍物碰撞，则对该节点加上一个惩罚值，该惩罚值越高，该节点获得扩展的概率越小。

文献 [37] 结合 RG-RRT 和 RC-RRT 的优点，提出了 EG-RRT（Environment Guided RRT）使算法更适合处理有狭窄通道的障碍物环境。

文献 [38] 与文献 [39] 分别提出 DD-RRT（Dynamic Domain RRT）与 AD-RRT（Adaptive Dynamic Domainrrt），限制采样区域在当前树所在的局部空间，以防止靠近障碍物的结点反复扩展失败，提高算法效率。

文献 [40] 中提出了 T-RRT 算法，它作用于代价地图上，在进行搜索和扩展时，引人了过渡测试（Transition Test），利用 Metropolis 准则来判断新结点的代价是否能够满足扩展的要求，同时控制无效扩展，从而提高算法效率并提高路径的质量。

（d）实时性

anytime RRT [41] 是一种实时性较高的算法，它先快速构建一个 RRT，获得一个可行解并记录其代价。之后算法会继续采样，但仅将有利于降低可行解代价的结点插人树中，从而逐渐获得较优的可行解。

Replanning [42] 是另一种实时规划算，它将整个规划任务分解为若干等时间的子任务序列，在执行当前任务的同时规划下一个任务。

3.3 最优性

尽管各种改进措施可以大幅度提高求解质量，但是仍难以保证获得最优解。近年来最优化规划算法发展较快，出现了许多能求渐进最优解的算法。

文献 [43~45] 根据随机几何图理论对标准 PRM 和做出改进，得到了具有渐近最优性质的 PRM、RRG 和 RRT 算法。在状态空间中随机采样个点，并将距离小于的点连起来，就构成了随机几何图（Random Geometric Graph，RGG）。渐近最优性要求 r(n)满足：

按照这一理念对标准 PRM 和 RRT 改造即得到 PRM* 和 RRG 算法。在 RRG 基础上引人「重新连接」步骤，即检查新插人结点作为其临近点的父结点是否会使其临近点的代价降低，若降低，则去掉临近点原来的父子关系，将当前插人点作为其父结点，这就是 RRT 算法。大量的结点连接和局部调整使得 PRM 和 RRT* 的效率十分低下。

文献 [46] 中提出了 LBT-RRT 算法，将 RRG 和 RRT 菁算法结合起来，引人一个估计参数 1 + ε，当 ε = 0 时，LBT-RRT 算法就是 RRG 算法，而当 ε = ∞ 时，则就是 RRT* 算法。

算法构造 RRG 所用的图和 RRT* 兴所用的树，第一张图保证最终路径的最低代价，第二个树则保证路径代价在 1 + ε 倍的最低代价中，通过 1 + ε 提高算法的效率并保证路径的渐进最优性。文献 [47] 在 PRM 的基础上做出改进，大大减少了结点连接的数量，并且可以获得弱渐近最优性，在实际应用中可以获得比 PRM* 和 RRT* 更高的效率。

尽管 RRT* 长及其 Anytime-RRT* 已被用于解决带有微分约束的运动规划问题，但是效率很低。文献 [48] 提出了不需要求解两点边值问题的渐进最优算法 SST* （Stable Sparse RRT*），通过对输人空间采样来扩展树，并且引人「修剪」树的操作来维持稀疏的数据结构，提高了算法效率。

RRT* 长与标准 RRT 一样具有盲目探索整个状态空间的特点，其计算效率还有很大的提升空间。文献 [49] 提出了 lnformed RRT* 算法，来避免 RRT* 对整个状态空间的不必要的探索，其思路是先用 RRT* 找到一个解，然后把采样区域限制到一个以初始、目标状态为焦点并包含当前解的椭球内，继续优化当前解。文献 [50] 提出了 BIT*（Batch Informed Trees）算法，其思路是先构建一个满足渐进最优条件的 RGG，然后利用启发式搜索构建一个从初始状态到目标状态的树，之后反复构造更稠密的 RGG，并在已有的树结构基础上作优化。经试验验证，BIT 可以获得比 RRT* 、FMT* 和 lnformed RRT* 更高的效率。

另外还有一类算法将 RRT* 和其他的算法融合，可以大大提高算法的效率，例如文献 [51] 提出 TG-RRT* 算法，在生成随机点后，利用起始点、终止点、随机点，组成三角形的内心或形心来代替随机点。通过三角几何学缩小搜索范围，从而大大减小算法所用的搜索时间和生成的结点数；文献 [52] 中将 RRT* 和 T-RRT 结合，在 RRT* 中引人过渡测试，使 RRT* 具备渐近最优的性质，同时提高算法效率。

4. 基于搜索的算法

解决运动规划问题的另一大类算法是启发性搜索算法，其基本思想是：将状态空间通过确定的方式离散成一个图，然后利用各种启发式搜索算法搜索可行解甚至是最优解·这类算法具有解析完备性，甚至是解析最优性。

尽管对于高维状态空间，搜索算法需要处理大量网格，效率低下。但是，对于无人车运动规划问题而言，这类算法已经有了比较成熟的应用。

4.1 状态格子

状态格子 [53] 是一种对状态空间离散化的手段。状态格子由结点（表示状态）和从该结点出发到达相邻结点的运动基元组成，一个状态结点可以通过其运动基元变换到另一个状态结点。这样，状态格子就将原来连续的状态空间转化为一个搜索图，运动规划问题就变成了在图中搜索出一系列将初始状态变换到目标状态的运动基元。

规则状态格子可以对状态空间进行离散化，不依赖于具体的环境结构，构造方便，每个格子都可以由一个格子通过平移变换得到，运动基元不必在线计算。但是在结构化环境中，车辆在大地坐标系上的航向角取值范围大，按照固定的离散方式无法反映环境的特点。

文献 [54] 针对道路上车辆的运动规划问题构造了符合道路形状的状态格子，格子在工作空间上按道路中线分布，方向角与道路中线的切线方向一致，曲率中心与道路中心曲率中心一致。这样规划出的路径更加符合道路的形状，缺点是格点的运动基元需要根据道路形状在线计算。文献 [54] 将时间考虑为状态格子的维度，方便处理移动障碍物。文献 [25] 把速度、时间和加速度加入到状态格子的维度，使车辆具备了在高速公路上实现保持车距、变换车道等基本功能。

4.2 搜索算法

构建起状态格子后往往要使用图搜索算法来搜索最优轨迹。

Dijkstra [55] 算法是经典的最短路径搜索算法。然而对于给定的一组初始，目标状态，其广度优先的性质将会导致太多无关节点的搜索，效率很低。A* [56] 是一种启发式最优搜索算法，其框架与 Dijkstra 基本一致，不同点在于引入了对当前结点到目标结点最低代价的估计函数。

A* 算法维护两个集合：OPEN 和 CLOSED，OPEN 是存储待扩展节点的优先队列，根据通过当前结点的路径总代价来排序，CLOSED 存储已经扩展过的节点。算法每次从 OPEN 中取出最优先的节点，进行扩展，并将该节点插入到 CLOSED 中，该节点所扩展到的节点如果不在 CLOSED 中，就插入到 OPEN 中，这样循环下去，直到扩展到目标结点。

可见 A* 算法通过启发式函数引导靠近目标结点的节点获优先扩展的机会，但在格子数量很多时，A* 处理的节点数目过多。

Weighted A* [57] 通过增加启发式函数的权重进一步引导搜索方向向这些目标节点进行，搜索速度很快，但是容量陷入局部极小值，无法保证全局最优解。ARA* [58] 是在 Weighted A* 基础上发展出的具有随时性质的搜索算法，它通过多次调用加权 A* 算法且每次调用就缩小启发式函数的权重，这样算法可以快速求出可行解，通过引入集合 INCONS 使得每次循环可以继续使用上一次循环的信息，对路径做出优化，逐渐逼近最优解。

由于一个启发式函数很难同时兼顾效率和最优性，那么考虑到启发式函数对搜索结果和结点数目影响，Sandip Aine 等提出了MHA* 算法 [59]，引入多个启发式式函数，保证其中有一个启发式函数在单独使用时可以找到最优解，从而通过协调不同启发式函数生成的路径代价，可以兼顾算法的效率和最优性。DMHA* [60] 在 MHA* 的基础上在线实时生成合适的启发式函数，从而避免局部最小值问题。A*-Connect [61] 是将 RRT-Connect 的思想运用到 MHA*上，分别从起点和终点进行搜搜，直到两个树重合为止。

A* 及其变种 Weighted A *，ARA* 可以有效解决确定性环境中的搜索问题，但对于环境复杂多变的情况，环境中的代价分布会发生变化，简单地重复使用A* 虽然可以重新搜索出最优路径，但是算效率较低。LPA* [62] 可以处理状态格子的运动基元的代价是时变的情况，当环境发生变化时可以通过对较少数数节点的重新搜索规划出新的最优路径；D* [63] 适合处理复杂多变的环境，能够根据所获得环境信息快速重新规划从当前状态到目标状态的最优轨迹，适合机器人在行进过程中探测出新的障碍物时重新选择路径；D* -Lite [64] 是对 LPA* 的发展，可以获得与 D* 同样的结果，但是效率比 D* 高。

另外，A* 还有其他一些变种。M* [65] 是专门进行多个机器人运动规划的搜索算法；R* [66] 是由 A* 结合随机采样发展而来的搜索算法，它避免了对启发式函数的过度依赖，同时通过随机采样来避开局部极小值，获得全局最优解；Field-D* [67] 和 Theta* [68] 都是对传统工作空间网格法的发展。

5. 算法实现

上述基于随机采样的算法和基于搜索的算法在很多无人车上都得到应用例如2007年DARPA挑战赛第一名车辆「Boss」[69] 中，运动规划模块采用三次螺旋线作为轨迹曲线，利用 A* 算法作为规划算法，将障碍物、车道、曲线曲率、曲率导数以及车辆速度、加速度等因素纳入轨迹代价中，在状态空间中不断搜索得到最终路径。

再如在 2016 年中国智能车未来挑战赛中取得佳绩的西安交通大学的「夸父一号」[70] 中，利用 Bézier 曲线作为轨迹曲线，根据城市环境常见工况，例如换道，转弯， U 型转弯等，预先生成模板路径曲线库，在规划之前先在曲线库中根据边界条件选择模板曲线，再根据实际环境利用 RRT 算法对曲线进行修改，从而大大提高了算法效率。

除了可以通过自己编写代码实现算法外，各个研究机构分别推出开放源代码的程序库，供应世界各地学者使用交流。其中，以基于采样的算法程序库 OMPL [71] 和基于搜索的算法程序库 SBPL [72] 最具代表性。

开源运动规划库（Open Motion Planning Library，OMPL）是用 C++ 编写的程序库，包含两个部分：

核心算法库 OMPL，包含各种基于采样的运动规划算法的实现；
界面OMPL。应用程序，是核心算法库的可视化前端，使用Python + PyQt实现。

OMPL 并不提供碰撞检查程序库，可以结合 FCL [73] 等开源程序库来使用。

SBPL（Search-Based Planning Library）是专门针对基于搜索的运动规划算法而开发的程序库，几乎涵盖了第 4.2 节所列举的所有搜索算法，使用 C++ 实现。

6. 发展趋势

随着无人车技术的发展，运动规划算法与其他方法结合，从而优化规划过程。

（a）与车辆动力学结合

将动力学参数评价指标和最优规划等结合，利用直接构造法进行规划是近年采用较多的方法，在这个过程中可以充分考虑车辆动力学因素，规划出的轨迹更加合理。

Benz S500 Intelligent Drive 采用直接构造法 [74]，将车辆在车道中的相对位置、加速度、横摆角速度以及曲率等作为优化指标，利用序列二次规划方法进行轨迹计算。

沈峘等 [75] 基于单轨模型，结合车辆无滑移条件，利用魔术公式来描述轮胎与路面的动力学模型，构造车辆动力学约束公式，同时将车辆动力性、操纵性和稳定性评价指标作为优化指标，将整个运动规划问题转化为非线性规划问题，从而获得可行轨迹。

国防科技大学 [76] 将模型预测控制融入轨迹规划中，预先选择边界条件范围离线计算可行路径集，在行为决策系统做出全局路径后，将阿克曼转角模型作为预测模型，在需要规划轨迹的全局路径段从可行路径集中选择部分路径并结合速度计算生成备选轨迹集，之后将动力学因素和障碍物等作为评价指标，利用最优规划从备选轨迹集中选出最终轨迹。该方法由于使用离线计算的可行路径集，因此大大提高算法效率。

运动规划也在自主泊车领域得到应用，自主泊车的工况相对单一，并且车辆行驶速度较低，因此采用的方法更加灵活。陈荣华等 [77] 利用基于互补约束的数学规划方法（MPCC）和Ｒ函数法将泊车过程中的避障条件约束转化为避障模型，同时结合车辆动力学模型建立行车模型，对泊车过程进行联立动态规划，提高了规划算法的普适性和鲁棒性。李红等 [78] 考虑下层路径跟踪的限制，在障碍物约束的基础上，加入车辆转向角度和角速度约束，对泊车轨迹进行优化，从而提高规划结果的实用性。

（b）与状态参数估计结合

状态参数估计可以更加准确获得车辆参数，因此可以将状态估计器加入规划模块中，通过在线估计车辆状态并将其反馈给规划器，提高轨迹质量。

不同地面类型会引起车辆滑移特性的变化，进而影响车辆状态，文献 [79] 针对这种现象设计滑移估计器，结合 CC-RRT* 算法，不断结合估计参数重新规划轨迹，通过闭环规划提高轨迹安全性。

（c）与机器学习相结合

机器学习作为人工智能的重大分支，其与运动规划的结合可以改善规划结果。

例如，文献 [80] 针对 RRT 算法，训练了一个具有恒定积分时间的非线性参数模型，该模型取代了距离度量模块用在计算节点间代价，从原理上讲，该模型建立在 POSQ 扩展函数——用于解决两点边值（Boundary Value Problem，BVP）问题的转向模型函数的基础上，模型的训练输入为扩展函数对应的距离度量结果，积分时间的大小决定了距离度量的计算复杂度，同时该文献对比了其他训练模型，如神经网络模型，支持向量机模型等，发现这种模型在度量精确度和计算时间上都有很大的优势。

文献 [81] 则利用局部加权学习的方法取代距离度量，同时根据估计结果计算树中结点对应的代价较小的区域，除此之外，每个搜索过的点都被标记了「安全」和「危险」两个标签，利用贝叶斯分类器估计整个状态空间中的安全区域，从而每次都在在安全区域中采样，随着采样的不断进行，估计的安全区域就更加准确，这两个措施使搜索更加趋向于代价低的安全区域，加快搜索速度。

文献 [82] 则将机器学习融入速度生成中，预先模拟多个试验场景，生成每个国内也有这些方面的研究。例如国防科技大学在 A* 算法中加入学习模型 [83]，将整个搜索过程分为利用 A* 算法搜索路径离散点作为子目标点以及利用离线训练好的模型生成最终路径两个阶段，其中训练集合的输入为初始状态和车辆模型的控制输入，输出为对应的终止状态和轨迹对应的代价，训练方法为最小二乘迭代法。由此以看出，训练得到的模型会综合考虑车辆动力学特性和路径安全性，经过验证，这种方法在计算效率和普适性方面都有极大的提高。

7. 总结

（a）采用具有阿克曼转向性质的车辆模型，可以利用其微分平坦性质简化轨迹生成问题。

（b）直接轨迹生成法相对路径－速度分解法而言还不够成熟，使用多项式螺旋线生成路径对速度生成和碰撞检查都有利，应用比较成熟。

（c）缺少准确的距离度量是基于采样的运动规划算法的主要缺陷，相关改进算法的效率有待验证；各种最优化算法虽然不断提高效率，但离实际应用还有距离；Anytime-RRT/ RRT* 是实用性比较高的算法。

（d）基于搜索的运动规划算法在非结构化环境和结构化环境中无人车运动规划问题上都有成功的应用，是一类具有很大实用意义的方法。

参考文献

作者简介

余卓平教授，973 计划首席科学家，同济大学校长助理，同济大学汽车学院前院长。现任国家燃料电池汽车及动力系统工程技术研究中心主任、国家863节能与新能源汽车重大项目总体专家组成员、中国汽车工程学会副理事长、上海汽车工程学会副理事长，长期从事汽车工程专业的教学、科研工作。

熊璐博士，德国斯图加特大学博士后，同济大学汽车学院教授，主要研究方向为汽车系统动力学与控制、新能源汽车整车集成技术。主持或参加国家自然科学基金、国家 973计划项目、863 计划项目、上海市自然科学基金等二十余项政府及企业课题。

整理编辑：厚势分析师拉里佩

转载请注明来自厚势和厚势公号：iHoushi

-END-