在本电子书中，您将了解运动规划的工作原理，以及如何将其应用于各种自主系统。您还将了解 MATLAB® 和 Navigation Toolbox™ 中提供的运动规划算法，以及如何为您的应用选择最佳算法。
简介
曾几何时，汽车离不开驾驶员，机器也离不开实体控制器。但现在不一样了：如今，高度智能的自动驾驶汽车已经能够自己变道、礼让行人甚至侧方停车。
自动驾驶汽车、机器人操作臂、无人地面交通工具 (UGV) 和无人机等系统要实现自主，离不开三大支柱，运动规划就是其中之一。另外两大支柱则是感知和控制。
自主系统工作流。
与人类非常相似，自主系统也是通过扫描环境来探索新环境，以了解自己所在的位置和周围环境。一旦得出环境地图，运动规划算法就会规划一条通往指定目的地的无障碍路径。算法会决定沿着路径要采取的下一步，控制器根据这一决定向作动器发送命令，使得系统移动。
运动规划的常见应用包括：
仓库中的自主移动机器人导航。
最后一公里无人机配送。
使用机器人操作臂进行分拣。
什么是运动规划？
运动规划是一种计算问题，旨在寻找将机器人或车辆从初始状态移动至目标状态的动作序列。“运动规划”和“路径规划”这两个词经常混用，但两者有一个关键区别。运动规划在车辆位置随时间变化时生成车辆的运动，而路径规划只生成车辆的路径。通过运动规划，车辆可以在遵循现有路径的同时改变运动，如以下两个自动驾驶汽车场景所示：
场景 1：红灯亮起时，汽车减速后停止；绿灯亮起时，汽车继续行驶，这是运动变化，不是规划路径变化。
场景 1：运动规划。
场景 2：汽车在高速公路上变道——路径和运动都发生了变化。
场景 2：路径规划。
状态空间和其他运动规划关键概念
在实际应用中，运动规划的实现有赖于多个功能部件。其中包括使用同步定位与地图构建 (SLAM) 算法生成的环境地图，以及机器人或车辆的状态（位置和方向）。机器人的状态间变换定义了其运动。可以应用于机器人的变换集合称为状态空间或配置空间 (Cspace)。配置空间可以包括自由空间（其中的机器人状态被视为有效）和障碍空间（其中的机器人状态被视为无效）。
例如，在自动驾驶汽车中，汽车的位置及其驶向或方向共同代表了它的状态。对于自动驾驶汽车的自动泊车，停车场的地图标识了自由空间和障碍空间，状态空间表示使用运动模型定义的所有可能的前进和后退机动的集合。
路径代价、最优性和完备性
路径代价
当机器人或车辆在寻找路径时，它所采取的每一步都与代价相关联。穿越自由空间的代价通常设为零，穿越包含障碍物的空间的代价设为无穷大。
最优性
如果路径规划算法总能找到最优路径，则称其为最优算法。为了使路径最优，其转换代价（边缘代价）之和在从初始位置到目标位置的所有可能路径中必须是最低的。
完备性
在有限的时间内，当路径存在时，路径规划算法能找出路径，当路径不存在时，算法能报告路径不存在，则称该算法为完备的。
最优且完备的路径规划算法所提供的路径不一定是最短的，但代价会是最小的。在某些特定的情况下（例如，让室内机器人沿着走廊移动），可以将机器人沿走廊中心移动的代价定义为低于靠近墙壁移动的代价。在这种情况下，最优路径是让机器人沿着走廊中心移动，减少与墙壁碰撞的机会。
运动规划的常见类型
运动规划有许多不同类型的方法。最常见的方法如下：
- 基于搜索的规划和基于采样的规划方法，取决于搜索树或图的创建方式
- 全局和局部路径规划方法，取决于规划是在整个地图中还是在某一子集中完成
接下来我们将逐一探讨每种方法。
基于搜索的规划
基于搜索的规划创建一个可搜索的图，将每个车辆状态或配置标识为一个节点。该图从起始节点扩展到目标节点，使用基于代价和启发式的方法来寻找最短路径。基于搜索的规划通常在离散化地图上执行，其中地图被细分为栅格单元，状态数是有限的或可数无限的（可以为每个状态分配一个唯一的整数）。
离散状态空间通常用二维栅格地图表示，其中各个网格的中心是要搜索的状态。一种常见的地图表示方法是占据栅格地图。
A* 算法是一种常用的基于搜索的方法，用于在离散栅格地图中寻找路径。
基于搜索的规划。
应用于栅格地图的 A* 算法。
当车辆或机器人可被视为一个点且规划阶段不涉及运动模型或运动学方程时，栅格地图上基于搜索的规划通常适用。如果路径规划算法为机器人提供了要遵循的路点，则可以使用控制算法来添加运动学约束。
基于采样的规划
在基于采样的规划中，搜索树或路线图是通过在状态空间中随机添加节点来创建的。使用连续运动模型，可以找到可能的无碰撞路径。基于采样的规划通常使用启发式方法来探索搜索空间并偏转搜索方向。创建后，树或路线图使用碰撞检查或搜索方法来寻找到达目标的最短路径。
RRT 算法是一种常用的基于采样的方法，用于在连续状态空间中寻找路径。
基于搜索的规划。
基于采样的运动规划适用于高维搜索空间，例如寻找一组有效的配置，使机械臂能够拾取物体。基于采样的规划广泛适用于多种实际应用，虽然不能提供完备解，但仍广受欢迎。如果搜索树的密度使样本足够接近，则当解存在时，找到解的概率会收敛到 1。这使得一些基于采样的规划器（例如 RRT 和 RRT*）在概率上是完备的。
全局和局部路径规划
全局路径规划又称基于地图的规划，它根据有关环境的先验知识寻找最优路径。全局规划算法规划初始路径，以避开环境中已知的静态障碍。例如，一个自主移动机器人可以规划一条全局路径，在有墙壁等静态障碍物的走廊上，将一本书从一个办公室送到另一个办公室。
局部路径规划又称动态重规划，它重新计算路径，以避开未知的动态障碍。局部规划算法跟踪全局规划并创建局部轨迹，同时避开新引入的障碍。例如，一辆自动驾驶汽车可能会规划局部轨迹，变道以避开其他车辆，然后重新汇入全局路径以抵达目的地。
自动驾驶汽车变道机动的局部路径规划。
使用 MATLAB 进行运动规划的四步工作流
Navigation Toolbox 提供了用于实现各种规划算法的类，包括常见的基于搜索的规划器（例如 A*）和基于采样的规划器（例如 RRT 和 RRT*）。该工具箱还提供路径指标，来评估所规划路径的避障间隙和平滑度。
路径指标：平滑度。
路径指标：间隙。
此外，Navigation Toolbox 提供了一个接口，可让您在系统化的四步工作流中实现基于采样的运动规划算法：
- 表示状态空间。
- 定义状态校验器。
- 对新状态进行采样并检查有效性。
- 将一组有效状态表示为路径。
表示状态空间
自定义状态空间类 nav.StateSpace 允许您定义一个状态空间，在其中包含任何应用的可能状态或配置。例如，stateSpaceDubins 和 stateSpaceReedsShepp 通过连接状态空间中的任意两个状态来支持自动泊车规划，以便状态空间模拟汽车类机器人或带有阿克曼转向的机器人的运动。
Navigation Toolbox 提供以下现成的状态空间。
| 状态空间 | 配置 | 环境 | 应用 |
|---|---|---|---|
stateSpaceSE2 |
(x, y, θ) | 二维 | 完整机器人 |
stateSpaceSE3 |
(x, y, z, qw, qx, qy, qz) | 三维 | 机器人操作臂 |
stateSpaceDubins |
(x, y, θ) | 二维 | 具有最小转弯半径的非完整车辆 |
stateSpaceReedsShepp |
(x, y, θ) | 二维 | 具有最小转弯半径的非完整车辆 |
状态空间 SE2。
状态空间 Dubins。
定义状态校验器
状态校验器基于状态空间，并与通过 SLAM 算法获得的地图相对应。它检查单个状态的有效性或两个采样状态之间的运动的有效性。例如，碰撞检查器是一种状态校验器，可指示机器人状态或配置与障碍物发生碰撞的情况。
Navigation Toolbox 提供以下状态校验器，用于校验二维和三维占据地图中的状态和离散化运动。
| 状态校验器 | 类型 | 应用 |
|---|---|---|
validatorOccupancyMap |
occupancyMap、binaryOccupancyMap |
二维占据地图 |
validatorVehicleCostmap |
vehicleCostmap |
二维占据地图 |
validatorOccupancyMap3D |
occupancyMap3D |
三维占据地图 |
这些状态校验器派生自工具箱中提供的自定义状态校验器 nav.StateValidator，可用于确定单个状态的有效性或任意两个状态之间的运动的有效性。
对新状态进行采样并检查有效性
基于采样的规划算法在定义的状态空间中随机对状态采样，并使用状态校验器创建从起点到目标的无障碍路径。RRT 和 PRM 等算法使用不同的采样方案对状态进行采样，并创建搜索树或路线图。
对于通过 SLAM 算法获得的地图，为对地图内的状态进行采样，会应用与地图外侧界限相对应的状态空间边界。
表示采样状态的集合
您可以使用 Navigation Toolbox 中的 plan 函数将规划算法的输出整理成树状数据结构。您可以使用 navPath 类存储给定状态空间中的状态集合，并对它们进行插值以获得路径。
选择运动规划算法
Navigation Toolbox 中提供了以下运动规划算法。
| 算法 | 类型和适用范围 | 优点 |
|---|---|---|
| 基于栅格的 A* | 全局规划 |
|
| Hybrid A* | 全局规划 |
|
| 概率路线图 (PRM) | 全局规划 |
|
| 快速扩展随机树 (RRT) | 全局规划 |
|
| RRT* | 全局规划 |
|
| 轨迹最优 Frenet | 局部轨迹生成器 |
|
A* 算法是一种离散路径规划器，可在栅格地图上创建连接起始节点和目标节点的加权图。A* 适用于离散化栅格地图，使用 x-y 线性连接扩展搜索树。算法基于代价函数逐一探索节点，该函数估计从一个节点移动到另一个节点的代价。
工作原理
为了找到代价最小的路径，A* 使代价函数 \(f(n)\) 最小化：
\(f(n) = g(n) + h(n)\)
其中
- n 是路径上的下一个节点
- \(g(n)\) 是从起始节点到 n 的路径的代价
- \(h(n)\) 是一个启发式函数，用于估计从 n 到目标的最低代价路径的代价
A* 算法。
MATLAB 中的 A* 算法
Navigation Toolbox 中的 plannerAStarGrid 提供了最常用的启发式函数，例如欧几里得和曼哈顿。对于 h(n) 和 g(n)，您可以使用预定义的代价函数或自定义的代价函数。您可以指定 binaryOccupancyMap 或 occupancyMap 对象作为规划器的输入。
应用
A* 算法直接适用于全驱动轮式机器人。A* 在基于计算机的应用中也很常见，例如电子游戏中的寻路。
Hybrid A* 是 A* 的扩展。与 A* 一样，Hybrid A* 适用于离散化搜索空间，但它将每个栅格单元与车辆的一个连续 3D 状态 (x, y, θ) 相关联。它使用由运动基元组成的连续状态空间生成平滑的可行驶路径。
工作原理
Hybrid A* 使用高效的引导式启发，使搜索树向目标方向展开。它还使用路径的解析展开，从而提高准确性和减少规划时间。
Hybrid A* 根据已有的精确目标姿态，为车辆生成平滑的可行驶路径。
起始节点，S = (1.5, 2.5, pi/2)
目标节点 1，G1 = (5.5, 5.5, 0)
目标节点 2，G2 = (5.5, 5.5, -pi/2)
MATLAB 中的 Hybrid A*
Navigation Toolbox 提供了 plannerHybridAStar，它使用 validatorOccupancyMap 或 validatorVechicleCostmap 等状态校验器，在占据地图中进行碰撞检查，并包含针对您的应用进行调整的代价和参数。
应用
与传统 A* 不同，Hybrid A* 适用于具有非完整约束的车辆，如自动驾驶汽车。它保证了运动学可行性，并考虑了车辆的方向和速度等微分约束。
Hybrid A* 用于自动泊车。
基于搜索的算法不适合具有高自由度或地图尺寸非常大的应用。存储来自大型栅格地图的数据需要很高的计算成本。PRM 是一种基于采样的规划算法，在这种情况下很有用。
工作原理
PRM 是地图中不同可能路径的网络图。该图由给定区域内有限数量的随机点或节点生成。在对每个节点进行随机采样后，PRM 算法通过连接固定半径内的所有节点来创建多个节点簇。
一旦构建好路线图，就可以在地图上查询从给定起始位置到给定目标位置的路径。由于 PRM 允许在同一路线图中针对不同的起点和目标位置进行多次查询，因此如果地图是静态的（不随时间变化），则可以节省计算时间。PRM 使用图搜索算法（如 A* 规划器）在其创建的路线图中搜索路径。
PRM 规划器。
MATLAB 中的 PRM
Robotics System Toolbox™提供了 mobileRobotPRM，它以 binaryOccupancyMap 作为输入，通过连接随机采样的节点在地图的自由空间中创建路线图。您可以使用 findpath 函数来查询从起点到目标位置的路径。由于 PRM 在寻找无障碍路径时不考虑车辆的尺寸，因此您可以使用 inflate 函数根据车辆的半径对地图进行放大。
应用
MATLAB 中的 PRM 实现适用于完整移动机器人应用，例如没有未知障碍的静态仓库环境，您可以在其中改变起始和目标位置，而不需要机器人重新学习整个地图。
PRM 规划器。
RRT 是一种适用于非完整约束的基于采样的算法。RRT 算法能有效地搜索非凸高维空间。它使用定义的状态空间中的随机样本，以增量方法创建搜索树。搜索树最终遍及整个搜索空间，并将起始状态连接到目标状态。
工作原理
RRT 规划器按照以下步骤生长搜索树，以起始状态 Xstart 为根：
- 规划器在状态空间中采样一个随机状态 Xrand。
- 规划器基于状态空间中的距离定义，找到一个已经在搜索树中并且最接近 Xrand 的状态 Xnear。
- 规划器从 Xnear 向 Xrand 扩展，直至达到状态 Xnew。
- 新状态 Xnew 被添加到搜索树中。
重复这个过程，直至树达到 Xgoal。每次采样一个新节点 Xnew 时，都会对它与其他节点的连接进行碰撞检查。要实现从 Xstart 到 Xgoal 的可行驶路径，可以使用运动基元或运动模型，如 Reeds-Shepp。
MATLAB 中的 RRT
Navigation Toolbox 提供了 plannerRRT，它采用第 3 部分所述的基于采样的可自定义规划接口。
双向 RRT (biRRT) 是 RRT 的一种变体，它创建两个树，从起始状态和目标状态同时开始。biRRT 对机器人操作臂很有用，因为它可以提高高维空间中的搜索速度。Robotics System Toolbox 中，manipulatorRRT 就提供了这一算法。
应用
RRT 特别适用于以下类型的路径规划问题：包含障碍物；涉及高自由度机器人（如机器人操作臂）的微分约束；移动机器人路径规划。注意，RRT 规划可能产生包含急转弯的路径。您可以使用路径平滑算法来补偿这些不规则性。
PRM 规划器。
RRT 算法给出了有效路径，但不一定是最短路径。RRT* 是 RRT 算法的优化版本。虽然在现实中不可行，但在理论上，RRT* 算法可以在节点数接近无穷时提供到达目标的最短可能路径。
工作原理
RRT* 的基本原理与 RRT 相同，但它有两个关键的补充，使其能够产生显著不同的结果：
- RRT* 包含每个节点的代价，该代价由相对于其父节点的距离定义。它总是在 Xnew 附近的固定半径内寻找代价最低的节点。
- RRT* 检查节点成本是否降低，并对搜索树重新布线，以获得更短、更平滑的路径。
RRT* 规划器。
MATLAB 中的 RRT*
Navigation Toolbox 中的 plannerRRTStar 适合解决几何规划问题。
应用
RRT* 给出了一个渐近最优解，因此特别适合机器人操作臂等高维问题。它在包含许多障碍物的密集环境中也很有用。虽然 RRT* 寻找具有最少节点的最短路径，但它不适用于非完整车辆。相比之下，RRT 可用于非完整车辆，并且能够处理微分约束。
使用 RRT* 和 MPC 跟踪控制器实现侧方停车。
轨迹最优 Frenet 是一个局部规划器，它根据全局参考路径来规划轨迹。轨迹是一组状态，状态中的变量是时间的函数。在需要考虑速度的情况下，轨迹规划很有用。
工作原理
作为局部规划器，轨迹最优 Frenet 需要一个全局参考路径，形式为一组 \([𝑥𝑦𝜃]\) 路点。对于沿弯曲的连续参考路径（如高速公路）的规划，它使用 Frenet 坐标，该坐标由行程长度和距参考路径的横向距离组成。
轨迹最优 Frenet 从初始状态开始进行备选轨迹采样，相对参考路径偏离一定的横向距离。它使用 Frenet 参考系和两种状态：
- 笛卡尔状态：\([𝑥\,𝑦\, 𝜃\, 𝜅\, 𝑥̇ \,𝑥̈] \)
- Frenet 状态：\([𝑠\,𝑠̇ \,𝑠̈\, 𝑙 \,𝑙̇ \,𝑙̈] 𝑠=𝑙𝑜𝑛𝑔𝑖𝑡𝑢𝑑𝑒𝑎𝑙𝑜𝑛𝑔𝑅𝑒𝑓𝑃𝑎𝑡ℎ, 𝑙=𝑙𝑎𝑡𝑒𝑟𝑎𝑙𝐷𝑒𝑣𝑖𝑎𝑡𝑖𝑜𝑛𝑤𝑟𝑡𝑅𝑒𝑓𝑃𝑎𝑡ℎ\)
初始状态通过五阶多项式连接到采样的终点状态，该多项式试图最小化抖动并保证状态的连续性。对采样轨迹进行运动学可行性、碰撞和代价评估。
高速公路行驶轨迹规划。
MATLAB 中的轨迹最优 Frenet
Navigation Toolbox 中的 trajectoryOptimalFrenet 沿参考路径寻找最优轨迹，其中参考路点由 Hybrid A* 或 RRT 之类的全局规划器生成。trajectoryOptimalFrenet 生成多个备选路径，并根据最终状态与参考路径的偏差、路径平滑度、时间和距离来评估这些路径的代价。它使用 validatorOccupancyMap 等状态校验器来检查状态的有效性。
应用
轨迹最优 Frenet 可作为全局规划器与车辆或机器人控制器之间的局部规划器。它适用于高速公路驾驶应用，如变道机动和自适应巡航控制（包括速度保持）。它还可以用于移动机器人和其他阿克曼型车辆的动态重规划。
