0. 简介

对于自动驾驶以及机器人而言，除了SLAM以外，另一个比较重要的部分就是轨迹规划了。而最近作者看到了几篇比较好的文章，分别为《A Review of Motion Planning Techniques for Automated Vehicle》、《A review of motion planning algorithms for intelligent robots》、《A review of motion planning for highway autonomous driving》这里就结合各个文章阐述的要点给各位对轨迹规划进行科普，以便了解未来几年需要解决的差距和挑战。

1. 机器人领域传统算法

机器人规划算法根据其原理和发明的年代可以分为两类：传统算法和基于ML的算法。传统算法由四组组成，包括图搜索算法（如A*）、基于采样的算法如快速探索随机树（RRT）、插值曲线算法（如线和圆）、基于反应的算法（如DWA）。基于ML的规划算法包括经典的ML算法，如支持向量机（SVM），最优值RL，如深度Q-学习网络（DQN）和策略梯度RL（如演员批评算法）。下图总结了规划算法的类别。

2. 机器人领域机器&强化学习算法

另一部分就是基于ML的算法的发展，例如经典的ML，如SVM，在早期阶段被用来实现简单的运动规划，但其性能很差，因为SVM的一步预测是短视的。它需要精心准备的矢量作为输入，而这些矢量不能完全代表基于图像的数据集的特征。在卷积神经网络（CNN）发明之后，从图像中提取高级特征的工作有了很大的改进（Lecun等人，1998）。CNN被广泛用于许多与图像相关的任务，包括运动规划，但它不能处理复杂的时间序列运动规划问题。这些更适合马尔科夫链（Chan等人，2012）和长短期记忆（LSTM）（Inoue等人，2019）。然后，神经网络与LSTM或基于马尔科夫链的算法（例如，Q学习（Smart & Kaelbling，2002））相结合，实现时间序列运动规划。然而，其效率是有限的（例如，在网络收敛方面表现不佳）。当谷歌DeepMind引入自然DQN（Mnih等人，2013年，2015年）时，取得了突破性进展，其中回复缓冲区是重复使用旧数据以提高效率。然而，由于噪声影响了对状态动作值（Q值）的估计，在鲁棒性方面的表现是有限的。因此发明了双DQN（Hasselt等人，2016；Sui等人，2018）和决斗DQN（Wang等人，2015）来应对噪声造成的问题。双重DQN利用另一个网络来评估DQN中Q值的估计，以减少噪声，而在决斗DQN中利用优势值（A值）来获得更好的Q值，噪声大多被减少。Q学习、DQN、双DQN和决斗DQN都是基于最优值（Q值和A值）来选择最佳的时间顺序行动。

最优值算法后来被政策梯度法（Sutton等人，1999）所取代，其中梯度法（Zhang，2019）被直接利用来升级政策，用于生成最优行动。政策梯度法在网络收敛方面比较稳定，但在网络收敛速度方面缺乏效率。Actor-critic算法（（Cormen等人，2009；Konda & Tsitsiklis，2001））通过actor-critic架构提高了收敛速度。然而，收敛速度的提高是以牺牲收敛的稳定性为代价的，因此，演员批判算法的网络在早期阶段的训练中很难收敛。于是发明了异步优势演员批评（A3C）（Gilhyun，2018；Mnih等人，2016）、优势演员批评（A2C）足彩代理1（Babaeizadeh等人，2016）、信任区域策略优化（TRPO）（Schulman等人，2017a）和近似策略优化（PPO）（Schulman等人，2017b）算法来应对这一缺陷。A3C和A2C利用多线程技术（Mnih等人，2016）来加快收敛速度，而TRPO和PPO通过在TRPO中引入信任区域约束，以及在PPO中引入 “代用 “和自适应惩罚来改进行为批评算法的策略，以提高收敛速度和稳定性。然而，数据在训练后会被丢弃，因此必须收集新的数据来训练网络，直到网络收敛。

包括确定性策略梯度（DPG）（Silver等人，2014）和深度DPG（（Lillicrap等人，2019；Munos等人，2016））在内的非策略梯度算法被发明出来，通过重放缓冲区重用数据。DDPG融合了行为批判性架构和确定性策略，以提高收敛速度。综上所述，经典ML、最优值RL和策略梯度RL是机器人运动规划中典型的ML算法，这些基于ML的运动规划算法的发展如图5所示。

3. 自动驾驶领域传统算法

智能交通系统的应用显著地帮助驾驶员减少一些与驾驶相关的繁琐任务。具体而言，归功于巡航控制（CC）、自适应巡航控制（ACC）和最近的协作ACC（CACC）的发展，高速公路驾驶已经变得更为安全，可以使用其中预先定义好的与前车的间距来控制纵向执行器、油门和制动踏板。来提高车辆整体的安全性、舒适度、交通时间和能量消耗。这类系统被称为高级驾驶辅助系统（ADAS），下图为自动驾驶车辆的通用框架，其中对于自动驾驶而言，感知、决策和控制最为重要。而我们主要要讲的规划就是在决策层当中

与机器人类似，这部分工作也同样适用于机器人导航的关键方面，因为它提供了全局和局部的轨迹规划以描述机器人的行为。它考虑了机器人从起始位置到终止位置的动力学和运动学模型。车辆和机器人之间执行运动规划的主要区别在于，前者解决了必须遵守交通规则的道路网络，而后者必须处理没有太多规则需要遵守的开放环境，它仅需要到达最终的目的地。

对于自动驾驶而言，其只需要以下几个功能，前两个部分不属于运动规划，因此不在本文的讨论范围。 下面主要讨论的是后三点

• 路线规划(route planning): 从起点到终点的长距离规划。
• 预测 (pediction): 通过储存的当前和历史动态信息来预测周围物体的动向。比如：道路信息，车道线的变化，路段交通规则，以及周围车辆的行为。
• 决策 (decision making):
• 生成 (generation):
• 形变 (deformation):

在过去几十年中，移动机器人中的路径规划已经成为一个研究课题。大多数作者将该问题划分为全局规划和局部规划。

大量的导航技术来自移动机器人，只是自动驾驶会根据规则加以修改。这些规划技术根据它们在自动驾驶中的实现被分类为四组：图搜索、采样、插值和数值优化（见表格I）。下面描述了在自动驾驶运动规划中实现的最相关的路径规划算法。下面我们来简述每个模型

2.1 基于图搜索的规划器

Dijkstra算法：它是一种在图中寻找单源最短路径的图搜索算法。配置空间被近似为离散的网格单元空间、lattice等。

A-star算法(A*)：它是一种图搜索算法，由于实现了启发式函数，因此能够进行快速的节点搜索（它是Dijkstra图搜索算法的扩展）。它最重要的设计为代价函数的确定。移动机器人中的一些应用已经被作为改进的基础，例如动态A_(D_)、Field D_ 、Theta_、Anytime repairing （ARA_）和Anytime D_ （AD*）等

State Lattice算法：该算法使用带有网格状态（通常是超维状态）的规划区域的离散表示。该网格被称为state lattice，在它上面应用运动规划搜索。这个算法中的路径搜索是基于包含所有可行特征的一组lattices或者primitives的局部查询，允许车辆从一个初始状态行驶到一些其它的状态。代价函数决定了预先计算的lattices之间的最佳路径。通过不同的实现（例如A_或者D_）应用节点搜索算法。

2.2 基于采样的规划器

快速探索随机树（RRT）：它属于基于采样的算法，其适合于在线的路径规划。它通过在导航区域执行随机搜索从而允许在半结构化空间中进行快速规划，还能够考虑非完整约束（例如车辆的最大转弯半径和动量）。然而，生成的路径不是最优的，其路径有突变且不是曲率连续的。RRT*这种新的实现收敛到一个最优的解。

2.3 插值曲线规划器

计算机辅助几何设计（CAGD）等技术通常被用作给定道路点集的路径平滑解决方案。这些允许运动规划器通过考虑可行性、舒适度、车辆动力学和其它参数来拟合给定的道路描述，以便绘制轨迹。

直线和圆圈：不同段的道路网络能够通过使用直线和圆圈对已知路径点进行插值来表示。

螺旋曲线：这种类型的曲线是根据Fresnel积分定义的。使用螺旋曲线能够定义曲率连续变化的轨迹，因为它们的曲率等价于其弧长，这使得直线段和曲线段之间平滑过渡，反之亦然。螺旋线已经被应用于公路和铁路的设计，还适用于类似车辆的机器人。

多项式曲线：这些曲线通常被实现用于满足插值点所需要的约束，即在拟合位置、角度和曲率约束等方面是有用的。

贝塞尔曲线：这些是参数化的曲线，它们依赖于控制点来定义其形状。贝塞尔曲线的核心为Bernstein多项式。这些曲线已经被广泛应用于CAGD应用、技术制图、航空和汽车设计。

样条曲线：样条曲线是在子区间上划分的分段多项式参数化曲线，它能够定义为多项式曲线、b样条曲线（也能够用贝塞尔曲线表示）或者螺旋曲线。每个子段之间的连接处被称为节点，它们通常在样条曲线的连接处具有高度平滑的约束。

2.4 数值优化

函数优化：该技术寻找函数的实数根（最小化变量输出）。它已经被实现用于改进移动机器人中障碍物和狭窄通道的势场法（PFM）

4. 自动驾驶领域智能算法

在运动规划通常被划分为高级规划和低级规划：

• 高级预测：需要做到的是通过对环境的分析，运动风险的评估，做出决策，并生成一些列的候选行为。类似于人的大脑，为行为做出指示。
• 低级反应：从高层规划中变形生成的运动。类似于小脑，不需要几乎不需要思考的产生运动，且会有应急反应，令真实的轨迹与路径有一定区别。

我们这一节将会带着上一节的内容进行更细的划分

4.1 空间配置分析(也就是我们怎么样表示地图)

空间配置分析是选择演化空间的一个去构成。它是一种集合算法，主要用于运动生成或指定时的变形。这些方法是基于几何方面的；它们指的是用粗略的分解来限制计算时间的预测方法，或者用更细的分布来更准确的反应方法。主要的困难是找到正确的空间配置参数以获得运动和环境的良好表现[41]。如果离散化太粗，碰撞风险将被很好地解释，并且不可能在两个连续的分解之间尊重运动学约束；然而，如果离散化太细，算法的实时性会很差。我们将空间分解区分为三个主要的子系列，如下图所示：采样点、连接单元和Lattice

基于采样的分解:最流行的随机方法是概率道路图（PRM）[41]。它在构建阶段使用在进化空间中挑选的随机采样。这些采样点与它们的邻居相连，形成一个无障碍的路线图，然后在第二个查询阶段由寻路算法解决，例如Dijkstra（见III-B2）[42]。在[33]中，作者首先根据参考路径，例如小车道中心线，对配置空间进行采样，然后根据目标函数选择最佳采样点集，最后为路径分配一个速度曲线以尊重安全和舒适标准.

基于连接单元分解:这些方法首先利用几何学将空间分解成单元，然后构建一个占用网格和/或单元连接图，应用实例见下图。在占用网格的方法中，在小车周围生成一个网格。障碍物的检测信息被叠加在网格上。在连接图的方法中，节点代表单元，而边则是单元之间的相邻关系。该图可以被解释为沿着细胞的边缘的路径或在连接的细胞内寻找的路径。主要的方法有 visibility decomposition、Voronoi decomposition、driving corridor、Vector Field Histograms (VFH)、exact decomposition、Dynamic Window (DW)这些主要的算法

Lattice 表示法：在运动规划中，Lattice 是一种规则的空间结构，是网格的概括[22]。可以定义运动基元，将Lattice的一个状态准确地连接到另一个状态。所有由网格产生的可行状态演化被表示为机动的可达性图。Lattice表征同时汇编了道路边界和运动学约束，并且可以快速重新规划，这对高速公路规划很有用。

4.2 寻路算法

寻路算法系列是运筹学中图形理论的一个分支，用于解决图形表示下的组合概率问题。该图可以是加权的，也可以是带有采样点、单元或操纵节点的定向图。基本原理是在图中寻找路径以优化成本函数。比如Dijkstra、A_、Anytime Weighted A_ (AWA_)、hybrid-state A_、D_、RRT、RRT_这些。详细的内容都已经在上面讲过了。

与基于抽样的分解类似，概率图搜索也不太适合于高速公路的结构化环境。此外，高速公路通常是一个已知的环境。从这个意义上说，在自主车辆的高速公路运动规划中，确定性的寻路方式更受青睐。

4.3 吸引力和排斥力

吸引力和排斥力的方法是一种生物仿生的方法。演化空间的符号是符号化为所需运动（如合法速度）的吸引力。障碍物的排斥力（如道路边界、车道标记、障碍物）。标线、障碍物）。因此，其主要优点是对场景表现的动态演变做出反应 对场景表现的动态演变作出反应。然后 小我私家车辆的运动是由结果的力向量引导的，因此没有明确的空间力矢量引导，因此不需要明确的空间分解。常见的方法有使用Artificial Potential Field (APF) 、Velocity Vector Field (VVF).以及elastic band algorithm。

4.4 参数化曲线和半参数化曲线

参数化和半参数化曲线是高速公路上路径规划算法的主要几何方法，至少有两个原因：

（1）高速公路的道路是由一系列简单和预定义的曲线（直线、圆和棍子[105]）构成的；
（2）预定义的曲线集很容易实现，作为候选解集进行测试。

此外，由于一些基于曲线的算法直接考虑到了车辆的运动学约束，它们被广泛用于补充其他方法。几何考虑通常与动态约束分开。这些解耦的方法可能会导致车辆在发生阻塞情况或重新规划时需要很长的反应时间。否则，曲线规划器也很适合预测性的方法，因为它们定义了从起点到目标点或单元或机动的运动。这一特性也适用于重新规划阶段。我们对曲线规划器的两种利用方式进行了区分。首先，无点曲线子家族被用来建立运动学上可行的轨迹，作为一组候选解决方案（机动）。第二，基于点的子家族使用曲线来适应一组选定的航点（采样点或单元）。第一个程序是一个集合算法，需要一个决策者来返回最方便的机动，而第二个程序需要在拟合路径或轨迹之前进行空间分解，因此是一个求解算法。两者都被考虑用于生成函数。图8（a），左边的是无点曲线子家族指的是格子的原理，称为触角算法。在样条曲线中，我们区分了B́ezier曲线；见图8（b）。它们使用控制点而不是内插点作为拐点。因此，除了起始点和结束点之外，不方便的地方是不经过定义的控制点。它们的优点是实施简单，因此与前面讨论的方法相比，计算成本低。

4.5 数值优化

运动规划的优化问题被定义为基于逻辑和启发式方法的解决算法。它们是决策和生成函数的一部分。优化通常表示为在一组约束条件下的状态变量序列中成本函数的最小化，并且是竞争性组合操作研究的一部分，以避免组合爆炸。在运动规划应用中，我们区分了两个感兴趣的领域。第一个领域侧重于寻找解决复杂问题的有效算法，并通过启发式方法改善搜索时间（一些启发式方法的细节见III-B2）。第二种是对问题的数学研究，以推导出特定的属性，从而在一个限制性空间中找到预测性的解决方案。由于第一个领域通常被用来减少算法的计算时间，下面只进一步讨论第二个领域。

基本的解决方案是Linear Programming（LP）。在LP表述中，该算法在线性等价或不等价的情况下解决线性成本函数。Simplex算法是最流行的算法之一，在[119]中，提出了一种基于空间的轨迹规划与顺序LP（SLP）。于非线性问题（NLP），非线性优化被用于非线性回归问题的特殊情况，例如路径优化的Levenberg-Marquardt算法，或用于非线性积分，用边界值问题（BVP）求解器解释。对于多目标问题，使用二次编程优化（QP）涉及到对原始问题的凸近似解的迭代搜索，如用顺序二次编程（SQP）对距离偏移、速度二次误差、加速度、抽搐和偏航率函数进行优化。或用混合整数二次规划（MIQP）将变道和超车的问题形式化。对于具体的预测性应用，在模型预测控制（MPC）下的解决方法非常流行。MPC算法的主要优势在于其重新规划的能力，但对于非凸和高复杂度的问题来说，它们仍然太差。动态编程（DP）通过将复杂的计算问题分解成更简单的子问题，甚至是相互依存的问题，从而获得了其功效。数值优化在运动规划中被广泛使用，要么是为了减少图的探索的解算时间，要么是为了利用问题的数学特性。这些算法可以通过通用的数值解析工具来解决。文献中主要遇到的框架是CVX和CVXGEN软件、Gurobi和YALMIP求解器、Matlab优化工具箱、NPSOL包和ACADO工具包

4.6 智能算法

基于人工智能的算法主要用于决策层面，其优势在于它们能够回答一般性问题并吸收新的修改而不影响算法的结构。它们主要用作预测规划的求解算法，但也用作符号集算法，很少用于反应变形。

这里根据 “认知/理性” (cognitive/rational) 和 “规则/学习” (rules/learning) 的程度将 AI 算法分为四类，如上图所示，类别分别是：逻辑方法(logic approaches)、启发式算法(heuristic algorithms)、近似推理(approximate reasoning) 和类人方法(human-like methods)。

4.6.1 基于逻辑的方法

此方法使用具有推理引擎的知识库来自动化推理，从而做出决策（类似专家系统）。它们的快速架构允许它们用于预测性或反应性规划。这些系统的主要优点是它们的直观设置可以模拟人类逻辑和理性推理。但知识库需要对具有大量因果规则和调整参数的大量环境变量进行离散化。

• 基于规则的推理 (rule-based reasoning): 这是最著名的推理引擎。在 [1] 和 [2] 中分别使用该方法完成了高速公路上基于规则的意图预测，和变道操作。主要优点是因果关系清晰，符号方便，实现简单。 主要缺点是循环推理和规则的详尽枚举，导致无限循环并影响计算时间。
  决策树 (decision trees): 为了显示规则机制，决策树被提升为压缩图形表示和决策支持工具。通过枚举所有可能的导航车道来描述决策树。决策树的好处是很容易解释，坏处是计算变得非常复杂，具有不确定或近似值。除了需要遵守规则之外，还要检测和预测其他车辆的非法和危险行为。
• 有限状态机 (Finite State Machine, FSM): 为了避免详尽的规则声明，有限状态机（FSM）给出了系统行为的抽象模型，表示由动作/条件链接的系统状态。FSM 不是绝对确定的，它允许更复杂的状态关系，比如利用并行的多个状态图来处理并发状态，这些状态非常适合在决策过程中执行同时动作（例如让步和合并）。其缺点是它们仅基于确定性知识，不能推广到未知情况。
  
• 贝叶斯网络 (Bayesian networks): 在不确定的情况下，采用使用马尔可夫模型的贝叶斯网络。它们是基于概率转换的因果关系的统计表示。他们提出了一个基于知识的参数识别步骤，以确定输出序列最可能的状态序列。匹配最预期障碍物的行为意图以返回最佳自我行为（停止 | 巡航 | 加速 | 减速和右转 | 左转 | 直行）。部分可观察马尔可夫决策过程 (Partially Observable Markov DecisionProcess, POMDP) 相关论文 [3] [4]

4.6.2 启发式算法

启发式算法基于经验，它们旨在找到一个近似解，因此有着角度的时间复杂度。在运动规划中，它们通常返回一组动作，但也能够返回路径或轨迹。其坏处是它们提供了一个局部解决方案，它不能保证全局最优性和准确性。

• 代理 (Agent): 智能代理是一种以理性和社会行为为模型的自主实体，它适应对环境的观察。 它的行为可以基于条件-动作规则（仅取决于当前感知）、世界模型（它如何影响环境）、要实现的目标或对目标的效用。启发式代理的能力允许包含多策略决策。它们特别适合不确定的环境。 主要缺点是难以确保收敛到解决方案。相关论文 [5]
• 支持向量机 (Support Vector Machines, SVM): SVM 是依赖于信息搜索算法的代理意图的统计学习分类器。使用贝塞尔曲线拟合方法提供控制点的论文 [6]。 基于相对速度和位置的个性化车道变更决策 SVM [7]
• 进化方法 (Evolutionary methods): 即使用遗传算法。相关论文：两种遗传算法改进的模糊控制模块[8] 基于速度，角度，刹车和时间的遗传算法控制[9]
  AI启发式算法是解决经典方法缺点的一个很好的替代方案。 它们的属性对于高速公路驾驶是可以接受的，在这种情况下，不需要最佳解决方案，近似解决方案就足够了。没有为自动驾驶汽车应用程序构建的 AI 启发式框架，但是，可以使用主要框架来解决这个系列，例如 用于代理的 JADE、DEAP、Jenetics 或用于进化算法的 Matlab 全局优化工具箱。

4.6.3 人工智能近似推理

AI 近似推理模仿人类推理。 如前所述，我们将逻辑方法与专家系统区分开来，区别在于知识库是非布尔型的； 以及对新知识进行分类以适应未来情况的学习方法。 第一个展示了通过直观的演示和解释展示智能行为的优势，而第二个与能够理解、思考和学习的系统有关。

• 模糊逻辑 (Fuzzy logigic): 模糊逻辑依赖于多值变量。该系统在数学上对模糊性进行建模，并通过对不同属性级解决方案的归纳逻辑编程接近认知推理。它们返回一个决策，通常表示为一个机动任务，因此是预测性或反应性求解算法。它们的主要缺点是缺乏可追溯性和缺乏系统的设计方法。[10] 中的作者提出了一个模糊推理决策系统，根据对周围障碍物之间间隙的近距离、中等或远距离评估，检查自我车辆是否有足够的时间改变车道；文献 [11] 和 [12]分别使用关于横向和角度误差的模糊规则来提出自动超车和参考地图的横向跟随。

• 人工神经网络(Artificial Neural Networks, ANN): 是一种流行的近似学习解决方案。它们用于生成和决策功能，作为一种集合和求解算法的非预测性和反应性方法。其主要缺点是缺乏对解决方案的因果解释，而它们的主要优点是它们能够通过对多维数据进行训练来学习。因此，另一个缺点是需要大量数据，这使得它们的计算成本很高。[13] 中的作者使用卷积神经网络 (CNN) 模拟车道变化的驾驶员轨迹，[14] 将学习阶段与人类驾驶员控制输入进行比较。[15] 中使用强化学习来确定场景表示中每个代理的策略，或用于[16]中的多目标超车操作和[17]中的自动车道变换操作。一些新的有前途的研究正在使用深度学习方法[18]，或通过将感知和规划模块集成到端到端学习中[19]。在贝叶斯概率不确定性的情况下，神经网络也被扩展为信念网络[20]。然而，这些方法的挑战之一是确保它们以正确的方式学习环境和驾驶行为的多样性。

AI 近似推理算法在不久的将来似乎很有希望。 它们依赖于扩展到认知属性的逻辑和统计基础。 他们还提供适应性推理，以适应他们的环境。 目前，这些算法的主要障碍是在实际驾驶场景中缺乏反馈。由于近似推理算法的实时实现很复杂，因此倾向于使用特定框架，例如 用于模糊逻辑方法的 Matlab Fuzzy Logic Toolbox 或用于机器学习的 TensorFlow。

4.6.4 人工智能类人方法

AI 类人方法非常适合高速公路场景中的决策，由于该环境的基本规则，驾驶员的行为更容易预测。 它们也很容易理解并与司机分享。 此外，此类算法的应用通常不像为驾驶员建模那样复杂，但在涉及复杂场景时仍然适用。 凭借其简单的架构和异构的实现，人们注意到在人工智能类人方法的自动驾驶文献中没有强调任何主要框架。

• 风险估计器 (Risk estimators): 对自主车辆附近时空的碰撞风险评估，并将风险提示反馈给驾驶员。为了适应驾驶员的风险认知 补偿风险 (compensation risk) 会评估驾驶员的状态，例如压力、困倦或疾病。从而提供更加驾驶建议。
• 分类模型 (taxonomic models): 他们确定了顺序驾驶任务的关系，以及行为和能力要求。这些方法需要逻辑组织，并代表运筹学任务调度。然后必须将它们解释为时空动作约束或由本地规划器解释。其缺点之一是它们难以明确描述，因为它们要么专注于驾驶任务，例如变道或合并，要么几乎没有穷尽。[11] , [21] 定义执行超车操作的操作顺序； [22] 中的作者提出了一种驾驶策略的功能架构，作为针对特定交通情况的一组离散的行为策略。
• 博弈论(game theory): 高速公路规划的想法是将移动的车辆视为观察彼此行为并因此采取适当策略做出反应的参与者。 这种方法的主要优点是它可以快速获得一个训练有素的驾驶员模型，从一个简单的基础模型开始。 主要缺点是它假设所有玩家都遵守规则，因此在现实生活中可能会导致不安全的反应。在 [15] 中，模型在针对其他行为进行训练时制定了更复杂的策略。 作者使用 POMDP 对玩家的知识进行建模，并在训练阶段使用 Jaakola 强化学习。

5. 参考链接

https://zhuanlan.zhihu.com/p/468424055