A* 算法是一种离散路径规划器，可在栅格地图上创建连接起始节点和目标节点的加权图。A* 适用于离散化栅格地图，使用 x-y 线性连接扩展搜索树。算法基于代价函数逐一探索节点，该函数估计从一个节点移动到另一个节点的代价。

工作原理

为了找到代价最小的路径，A* 使代价函数 \(f(n)\) 最小化：

\(f(n) = g(n) + h(n)\)

其中

• n 是路径上的下一个节点
• \(g(n)\) 是从起始节点到 n 的路径的代价
• \(h(n)\) 是一个启发式函数，用于估计从 n 到目标的最低代价路径的代价

MATLAB 中的 A* 算法

Navigation Toolbox 中的 plannerAStarGrid 提供了最常用的启发式函数，例如欧几里得和曼哈顿。对于 h(n) 和 g(n)，您可以使用预定义的代价函数或自定义的代价函数。您可以指定 binaryOccupancyMap 或 occupancyMap 对象作为规划器的输入。

查看 A* 示例

应用

A* 算法直接适用于全驱动轮式机器人。A* 在基于计算机的应用中也很常见，例如电子游戏中的寻路。

Hybrid A* 是 A* 的扩展。与 A* 一样，Hybrid A* 适用于离散化搜索空间，但它将每个栅格单元与车辆的一个连续 3D 状态 (x, y, θ) 相关联。它使用由运动基元组成的连续状态空间生成平滑的可行驶路径。

工作原理

Hybrid A* 使用高效的引导式启发，使搜索树向目标方向展开。它还使用路径的解析展开，从而提高准确性和减少规划时间。

Hybrid A* 根据已有的精确目标姿态，为车辆生成平滑的可行驶路径。

MATLAB 中的 Hybrid A*

Navigation Toolbox 提供了 plannerHybridAStar，它使用 validatorOccupancyMap 或 validatorVechicleCostmap 等状态校验器，在占据地图中进行碰撞检查，并包含针对您的应用进行调整的代价和参数。

查看 Hybrid A* 示例

应用

与传统 A* 不同，Hybrid A* 适用于具有非完整约束的车辆，如自动驾驶汽车。它保证了运动学可行性，并考虑了车辆的方向和速度等微分约束。

基于搜索的算法不适合具有高自由度或地图尺寸非常大的应用。存储来自大型栅格地图的数据需要很高的计算成本。PRM 是一种基于采样的规划算法，在这种情况下很有用。

工作原理

PRM 是地图中不同可能路径的网络图。该图由给定区域内有限数量的随机点或节点生成。在对每个节点进行随机采样后，PRM 算法通过连接固定半径内的所有节点来创建多个节点簇。

一旦构建好路线图，就可以在地图上查询从给定起始位置到给定目标位置的路径。由于 PRM 允许在同一路线图中针对不同的起点和目标位置进行多次查询，因此如果地图是静态的（不随时间变化），则可以节省计算时间。PRM 使用图搜索算法（如 A* 规划器）在其创建的路线图中搜索路径。

MATLAB 中的 PRM

Robotics System Toolbox™提供了 mobileRobotPRM，它以 binaryOccupancyMap 作为输入，通过连接随机采样的节点在地图的自由空间中创建路线图。您可以使用 findpath 函数来查询从起点到目标位置的路径。由于 PRM 在寻找无障碍路径时不考虑车辆的尺寸，因此您可以使用 inflate 函数根据车辆的半径对地图进行放大。

查看 PRM 示例

应用

MATLAB 中的 PRM 实现适用于完整移动机器人应用，例如没有未知障碍的静态仓库环境，您可以在其中改变起始和目标位置，而不需要机器人重新学习整个地图。

RRT 是一种适用于非完整约束的基于采样的算法。RRT 算法能有效地搜索非凸高维空间。它使用定义的状态空间中的随机样本，以增量方法创建搜索树。搜索树最终遍及整个搜索空间，并将起始状态连接到目标状态。

工作原理

RRT 规划器按照以下步骤生长搜索树，以起始状态 X_start 为根：

1. 规划器在状态空间中采样一个随机状态 X_rand。
2. 规划器基于状态空间中的距离定义，找到一个已经在搜索树中并且最接近 X_rand 的状态 X_near。
3. 规划器从 X_near 向 X_rand 扩展，直至达到状态 X_new。
4. 新状态 X_new 被添加到搜索树中。

重复这个过程，直至树达到 X_goal。每次采样一个新节点 X_new 时，都会对它与其他节点的连接进行碰撞检查。要实现从 X_start 到 X_goal 的可行驶路径，可以使用运动基元或运动模型，如 Reeds-Shepp。

MATLAB 中的 RRT

Navigation Toolbox 提供了 plannerRRT，它采用第 3 部分所述的基于采样的可自定义规划接口。

双向 RRT (biRRT) 是 RRT 的一种变体，它创建两个树，从起始状态和目标状态同时开始。biRRT 对机器人操作臂很有用，因为它可以提高高维空间中的搜索速度。Robotics System Toolbox 中，manipulatorRRT 就提供了这一算法。

查看 RRT 示例

应用

RRT 特别适用于以下类型的路径规划问题：包含障碍物；涉及高自由度机器人（如机器人操作臂）的微分约束；移动机器人路径规划。注意，RRT 规划可能产生包含急转弯的路径。您可以使用路径平滑算法来补偿这些不规则性。

RRT 算法给出了有效路径，但不一定是最短路径。RRT* 是 RRT 算法的优化版本。虽然在现实中不可行，但在理论上，RRT* 算法可以在节点数接近无穷时提供到达目标的最短可能路径。

工作原理

RRT* 的基本原理与 RRT 相同，但它有两个关键的补充，使其能够产生显著不同的结果：

• RRT* 包含每个节点的代价，该代价由相对于其父节点的距离定义。它总是在 X_new 附近的固定半径内寻找代价最低的节点。
• RRT* 检查节点成本是否降低，并对搜索树重新布线，以获得更短、更平滑的路径。

MATLAB 中的 RRT*

Navigation Toolbox 中的 plannerRRTStar 适合解决几何规划问题。 

查看 RRT* 示例

应用

RRT* 给出了一个渐近最优解，因此特别适合机器人操作臂等高维问题。它在包含许多障碍物的密集环境中也很有用。虽然 RRT* 寻找具有最少节点的最短路径，但它不适用于非完整车辆。相比之下，RRT 可用于非完整车辆，并且能够处理微分约束。

轨迹最优 Frenet 是一个局部规划器，它根据全局参考路径来规划轨迹。轨迹是一组状态，状态中的变量是时间的函数。在需要考虑速度的情况下，轨迹规划很有用。

工作原理

作为局部规划器，轨迹最优 Frenet 需要一个全局参考路径，形式为一组 \([𝑥𝑦𝜃]\) 路点。对于沿弯曲的连续参考路径（如高速公路）的规划，它使用 Frenet 坐标，该坐标由行程长度和距参考路径的横向距离组成。

轨迹最优 Frenet 从初始状态开始进行备选轨迹采样，相对参考路径偏离一定的横向距离。它使用 Frenet 参考系和两种状态：

• 笛卡尔状态：\([𝑥\,𝑦\, 𝜃\, 𝜅\, 𝑥̇ \,𝑥̈] \)
• Frenet 状态：\([𝑠\,𝑠̇ \,𝑠̈\, 𝑙 \,𝑙̇ \,𝑙̈] 𝑠=𝑙𝑜𝑛𝑔𝑖𝑡𝑢𝑑𝑒𝑎𝑙𝑜𝑛𝑔𝑅𝑒𝑓𝑃𝑎𝑡ℎ, 𝑙=𝑙𝑎𝑡𝑒𝑟𝑎𝑙𝐷𝑒𝑣𝑖𝑎𝑡𝑖𝑜𝑛𝑤𝑟𝑡𝑅𝑒𝑓𝑃𝑎𝑡ℎ\)

初始状态通过五阶多项式连接到采样的终点状态，该多项式试图最小化抖动并保证状态的连续性。对采样轨迹进行运动学可行性、碰撞和代价评估。

MATLAB 中的轨迹最优 Frenet

Navigation Toolbox 中的 trajectoryOptimalFrenet 沿参考路径寻找最优轨迹，其中参考路点由 Hybrid A* 或 RRT 之类的全局规划器生成。trajectoryOptimalFrenet 生成多个备选路径，并根据最终状态与参考路径的偏差、路径平滑度、时间和距离来评估这些路径的代价。它使用 validatorOccupancyMap 等状态校验器来检查状态的有效性。

应用

轨迹最优 Frenet 可作为全局规划器与车辆或机器人控制器之间的局部规划器。它适用于高速公路驾驶应用，如变道机动和自适应巡航控制（包括速度保持）。它还可以用于移动机器人和其他阿克曼型车辆的动态重规划。