算法解析：自动驾驶实时路径规划
文章来源：Vehicle
发布时间：2021-10-11
本节主要介绍在自动道路驾驶领域现有研究中使用的规划技术。给定一条由路线规划（导航）提供的路线，在道路上行驶的运动规划（以下简称规划）主要是在考虑车辆运动模型、车辆应遵循的航路点和交通环境的约束条件下，包括静态障碍物和动态障碍物，寻找车辆行驶的最佳路径。
规划可以分为增量方法，即通过重复使用以前搜索的信息来寻找状态转换的最佳顺序（从一开始就没有完全指定），以及试图为车辆找到最佳单状态转换的本地方法。全局或局部路径也与车辆执行的决策或操纵有很强的相关性，因此也将讨论操纵规划。如下图所示，路径搜索在从路线规划器中选择路线后启动，并作为搜索最佳操纵的输入（即使车辆具有最正确和安全行为的操纵）。然而，最终路径可能会根据最佳操纵而改变，如这两个模块之间的反馈回路所示。一旦路径最终确定，就生成了最终的轨迹规划。
a 通过增量采样或离散几何结构（即增量搜索）找到最佳的动作序列。
（3） 根据给定的约束条件，找到最佳轨迹，以完成几何曲线的优化。
例如，当车辆在道路上时，它遵循从路线导航规划获取的路径点序列，然后构造车辆的几何路径（图3a）。这些航路点必须是无障碍物的，因为车辆需要与其他车辆相互作用，以便在道路上协同移动。根据已导出的几何路径和与其他车辆的相互作用，自动车辆必须决定其下一个“高水平”动作（图3b）；即是否应超越领先车辆及时到达下一个航路点？正如所暗示的，这些高层决策取决于路径，因为车辆需要参考航路点来决定其最佳行动。如果航路点和正确的操纵已经完成，那么轨迹规划描述了寻找连接所确定航路点的最佳方式的程序（图3c）。
在搜索最佳路径时，增量搜索是指搜索配置或状态没有事先完全指定的技术。这样的算法还会重复使用以前搜索的信息来提高搜索速度。主要使用两种技术：
（1）快速探索随机树Rapidly exploring random trees（RRTs）
（2）格子规划器Lattice Planners（LP）
Rapidly exploring random trees快速探索随机树 (RRTs)
RRTs构造了一个树型数据结构，通过在每次迭代中添加新的配置（顶点），这些配置（顶点）从配置空间随机采样，直到达到目标配置为止。RRT也可以推广到状态空间中，在状态空间中，树是从采样状态而不是配置中生长出来的（LaValle，1998）。
表2给出了构造RRT的基本算法（LaValle，1998）。
在表2中，G是树拓扑图；C是配置空间；xrandom是从配置空间中随机抽样的配置；xNEAR是从距离度量中最接近xrandom的顶点；u是一个选定的输入，使xrandom和xNEAR之间的距离最小，以确保新配置包含在Xfree中；xnew是新配置，如果在固定的时间间隔Dt内应用u，则获得新配置，并且xnew是根据状态或配置转换公式计算的。需要一个碰撞检测算法来确保xnew是无障碍的。在步骤7中，新配置作为顶点添加到树中，最后，在步骤8中，在随机采样配置和xNEAR之间创建一条边。这些步骤如图4所示。
RRT在概率上完全保证运动学可行性，可以很容易地实时实现，并处理一般动力学模型。RRT规划师的这些优势正是它们被用于许多自动驾驶案例的原因。RRT的主要优点是对自由空间的快速探索；然而，它们的主要缺点在于它们创建的不规则路径，以及树扩展对最近邻度量的强烈依赖。RRTs的其他局限性还包括对每个扩展节点进行碰撞检查的必要性，这可能会导致计算复杂性。此外，最优性保证常常被忽视，而倾向于快速探索自由空间。
Macek等人（2006）使用RRT算法的简单版本和B样条曲线来生成平滑路径。随机参数包括加速度、加速度持续时间、采样时间和方位角。碰撞检查是通过假设车辆和障碍物的圆形表示来执行的。该算法在800m*800m模拟环境中进行了测试，该模拟环境中有静止和移动的障碍物，类似于真实的交通环境，并且使用了配备有激光雷达、单目摄像机、惯性测量单元（IMU）和GPS传感器的自动车辆。虽然可以有效地找到路径，但车辆仅限于较低的最大速度和加速度，而且在一次实验中，由于障碍物跟踪错误，车辆不得不减速以彻底搜索最佳路径。
Kuwata等人。（2009）使用闭环控制器来摇摆配置的采样并模拟朝向它们的动态轨迹，而不是直接从配置空间采样（见图5）。而经典的方法，如RRT算法所示，在Kuwata等人采用的方法中，从配置空间随机抽样（上面的第3步）。从闭环控制器的输入空间中选择样本（即闭环RRT，称为CL-RRT）。此外，加入到树上的每一条边都是一条动态可行的轨迹，通过正向仿真计算出每个顶点的可穿越性代价，评估可能轨迹的可行性。
该算法结合了有效的技术，如只在最佳航路点序列中选择状态和配置，以及车辆状态的重新传播，以降低构造状态空间的计算复杂度。搜索过程中存在大量的启发式算法，这可能会增加执行时间或收敛时间。在DARPA城市挑战赛实施期间，发生了两起车祸事件（Fletcher等人，2008年）。Aoude等人提出了一种改进CL-RRT的方法。（2010b）其中结合了使用博弈论方面的每个可能轨迹的威胁评估。所开发的算法（称为RRT-Reach）搜索整棵树中的威胁，假设每个策略都从障碍物中获得了完美的知识，并且仅用两辆车进行了仿真测试。在障碍处理方面，CL-RRT的另一个改进是由Aoude等人实施的。其中，高斯过程的最优性估计需要进一步的研究，但需要在实际时间范围内进行偏差估计。
Karaman和Frazzoli（2011）开发了RRT*算法，以保证所提出路径的最优性。在RRT*中，所有可行连接都是根据其去代价来评估的，并且只在最小代价路径上添加顶点到树中。由RRT*找到的解决方案更有可能是最佳方案。Jeon等人（2013）采用RRT*算法，并通过半车动态模型计算出路径，使用该动态模型可在U形转弯处产生动态可行的轨迹，并在无障碍的环路环境中实现。虽然使用动态模型会导致更多的计算工作用于树边缘的可行性检查，但与RRT*的组合会导致在给定控制输入间隔内（即由路线规划师提供）最优的解决方案，但不一定是全局最优的。
Garrote等人（2014）也使用RRT*，但使用两个额外的例程对其进行修改（一个用于碰撞检查，另一个用于展开）。在碰撞检查方面，对于每一个探测到的节点，都要测量碰撞检查的时间，以限定规划所需的时间。惩罚例程负责检查从每个新节点生长树是否可行或不可行。利用这两个额外的特征增强RRT*算法的思想是根据动态障碍物的运动来扩展树。在Garrote等的工作（2014），ego车辆模拟为矩形，障碍物为圆形。该算法仅在静态障碍物和动态障碍物的模拟环境中进行了测试，没有对仿真性能进行统计测试。
Reyes Castro等人（2013）给出了更先进的技术。车辆和路网的动力学系统被形成Kripke结构（Kripke，1963），并使用非确定性有限自动机来捕捉交通规则。这两种技术的组合用于增加RRT*（称为最小违规RRT*），但道路网络上没有障碍物。
状态格构造了一个离散的搜索空间，使得相关的状态连续，以确定的方式获取目标状态，并满足车辆的微分约束。由于启用了预计算（Howard，2009），这也降低了计算成本（Howard，2009），除非晶格弯曲以遵循车道或道路的形状（Madas等人，2013年）。它们通常非常适合非完整和高度受限的环境（Likhachev和Ferguson，2009），例如道路环境。
Lattice Planner的解析完成。这意味着控制空间可以根据每一个分辨率的变化自动调整，并且可以对空间进行一致的探索。Lattice Planner还保证了最佳性和平滑性，因为它们不会引入与反向指针相关的不连续性（Pivtoraiko等人，2009年）。生成的计划基本上接近车辆的实际运动，但由于无法快速考虑备选目标状态，因此无法有效地执行规避操作（Werling等人，2011年）。此外，航向角的离散化可能是有问题的，并且可能导致定向样本之间的振荡（如图6所示），穷举采样可能导致不必要的计算复杂性。
Likhachev和Ferguson（2009）在车辆和目标附近使用高分辨率的多分辨率状态晶格，而在其他地方使用低分辨率。每种状态都包括位置（x，y），方向（h）和速度，与常规晶格相比，搜索速度增加了3倍。不同的分辨率使车辆能够构建长期计划，而不会通过穷举采样增加规划的计算复杂性。该多分辨率网格采用确定性算法（称为Anytime Dynamic）进行搜索，该算法适用于规划空间的有限性和不完全感知性以及有限的计算时间。在DARPA城市挑战赛期间，使用一辆真实世界的自动车辆进行了实验，以15 mph的最大速度行驶，但发现主要缺点是曲率不连续。
为了解决曲率问题，Rufli和Siegwart（2010）通过在4D配置空间（包括2D位置、航向和转向角）上定义一个2D输入集U（航向和转向角被离散化），构造了一个输入和状态格。网格跨越24个方向，这些节点通过三次多项式曲线连接起来，并且具有自我变形的能力，以适应给定参考路径的弯曲和狭窄道路结构。
全局固定状态格需要平坦的地形和固定的机动性，因此限制了车辆的移动，因为与格点集轨迹的微小差别可以显著增加搜索时间和计算量。
这一缺陷促使了时空状态格的发展，如Ziegler和Stiller（2009）在图7中所示，其中状态空间和时间在与非时间晶格类似地离散之前结合在单个流形中。在他们的工作中，五次多项式被用于连接顶点，并且显示了一个用于晶格匹配道路形状的变形范例。时间的增加增加了维数（二维位置、二维速度、二维加速度和时间由每个状态描述）。该算法能实时有效地对状态空间进行采样，并能更灵活地分配边缘权值。缺点是时间和速度集合上的固定值，以及五次多项式与车辆运动方程之间的不平衡。
在McNaughton等人（2011）将曲率添加到每个状态（初始状态包含2D位置、航向和曲率），并且路径（车辆和采样端点之间）用三次多项式缓和曲线连接。然后将时间和速度范围分配给每个顶点，以实现时空搜索；但是需要恒定的加速度，使得车辆很难遵循这样的加速度曲线。时间和速度范围的分配减少了搜索空间，但引入了次优性，并且需要在评估每个轨迹后计算精确值。为了找到最佳路径，动态规划算法在格子中穷尽搜索，同时考虑障碍物的存在、行程时间和期望行为。
为了改进McNaughton等人的方法，Xu等人。（2012）使用四次曲率多项式来提供规划周期之间的连续曲率变化率；不仅从采样端点建立连接，而且从当前车辆姿态进行连接。另一个区别是，速度曲线是反向生成的，在评估替代路径时，也考虑了舒适性、效率和能耗。
Gu等人也使用了时空晶格。（2013）在一个2级规划方法中，首先生成一个最佳无碰撞参考路径，然后对状态空间进行采样，以便根据参考路径找到最佳路径。构建参考路径是为了处理详尽的采样；从而导致更集中的搜索和更人性化的驾驶风格。
RRT和Lattice Planner的比较如表3所示：
综上所述，RRT和Lattice Planner都使用数据结构（分别是树和格）对状态空间进行采样，试图以一种快速、安全的方式对其进行探索。在这两种情况下都可以快速探索，并向规划模块提供一系列可能的路径，供车辆遵循。然而，据称规划范围相对较大，并且，对于道路行驶的动态特性，当障碍物或障碍物突然出现时，需要重新规划例行程序来补充这些增量搜索方法。最后，为了提高安全性，应该使用额外的碰撞预测模块，而不是算法的内置碰撞检查功能。
实时搜索整个图并不总是有效的；因此，一些方法使用有限的视界，无论是在时间还是空间上。
在局部搜索级别，用于道路自动驾驶的最流行的技术可能是搜索空间包含某个几何曲线（例如回旋曲线或样条曲线）以及该曲线的几个横向位移。然后通过一个代价函数对每个候选路径进行评估，考虑到距离和时间成本、加速和碰撞检查。
在复杂的动态环境中，横向移动的轨迹可能无法很好地执行（Gu和Dolan，2012）。因为他不是朝着目标搜索整个状态空间，这样会在无限时间范围内需要大量的计算能力（用于构建状态空间）。Benenson等人提出的部分运动规划（Partial Motion Planning-PMP）可以使用。PMP使用短时间范围与RRT相结合，并利用不可避免碰撞状态（ICS）的概念（即无法避免碰撞的车辆状态定义）进行安全检查。不可避免的碰撞状态保证不会发生碰撞，但需要对车辆周围环境有充分的了解（Althoff等人，2011年），而PMP方法通常接近最佳状态保证（Kushleyev和khachev，2009年）。如下图所示，如果找到不可避免碰撞状态（ICS），则搜索替代路径，并且在每个时刻以RRT方式展开步骤节点。
总之，横向移动的轨迹和PMP方法都旨在找到适合车辆行驶的最佳下一个动作（就路径作为一个动作而言）。迂回轨迹在动作或状态空间中采样最终条件，而PMP方法使用RRT在有限的时间范围内进行规划。对于车道和道路边界以及其他交通参与者的运动而言，对状态空间进行采样的技术可能会导致平滑和安全的路径。这与PMP和对动作空间进行采样的技术相矛盾，这可能导致适应大量障碍物处理的路径不可行。
1. Real-time motion planning methods for autonomous on-road driving: State-of-the-art and future research directions -Christos Katrakazas a , Mohammed Quddus a,⇑ , Wen-Hua Chen b,1 , Lipika Deka a
2. Partial Motion Planning Framework for Reactive Planning Within Dynamic Environments
3. Multi-layer occupancy grid mapping for autonomous vehicles navigation
4. Robust Calculation of Ego-Vehicle Corridors
5. Multi-layer occupancy grid mapping for autonomous vehicles navigation
获取更多评论