控制工程入门：路径规划——A*、Dijkstra、RRT、RRT* 和 PRM

Contents — find the section you need

将机器人从起点移动到目标点很少意味着绘制一条直线。一个可用的路径规划必须考虑墙壁、通道宽度、机器人占地面积、转弯限制、定位不确定性、过时的地图、移动的人员以及停止距离。路径规划决定了机器人的行进方向；轨迹生成和控制则决定了如何在动态条件下沿着该路径行进。一个安全的架构能够将这些职责区分开来，同时共享它们的限制。

本入门指南将网格 Dijkstra 算法和 A 算法与连续空间 RRT、RRT 和 PRM 算法进行了比较。它解释了障碍物膨胀、启发式算法、采样、复杂度、SLAM/Nav2 集成、实现检查以及独立的安全行为。有关相邻层级的内容，请参阅Visual SLAM Primer、ROS 2 Primer和Sensor Fusion Primer。

实践结论

Dijkstra 算法保证在非负权重图上找到最短路径，但会向与目标无关的方向扩展。A* 算法在保持最优性的同时，使用可采纳的启发式方法来引导扩展。
RRT 算法倾向于在高维连续空间中快速找到可行路径。随着样本数量的增加，RRT* 算法会逐渐逼近最优路径，但会增加邻域和重连的成本。PRM 算法在足够静态的空间中分摊路线图构建的成本。
对于具有半径、定位误差和停止距离的机器人而言，通过未膨胀地图的最短路径是碰撞路径。
返回的路径并不能证明安全性。地图的新鲜度、局部感知、跟踪误差、动态障碍物、重新规划延迟和紧急停止等因素需要单独处理。

选择算法前定义自由空间

设状态空间为 $\mathcal X$ ，障碍物空间为 $\mathcal X_{obs}$ ，自由空间为 $\mathcal X_{free}=\mathcal X\setminus\mathcal X_{obs}$ 。路径 $\sigma:[0,1]\to\mathcal X_{free}$ 连接 $x_s$ 和 $x_g$ ，并经过 $\sigma(0)=x_s,\sigma(1)=x_g$ 。二维点机器人使用 $x=(x,y)$ ；车辆在此基础上增加航向 $\theta$ 、速度和转向；机械臂则包含所有关节角度。简化状态可以降低搜索成本，但可能会产生下游车辆无法实现的曲线。

障碍物膨胀通过扩展障碍物将有限机器人转化为点搜索。此处 $r_{loc}$ 应为已定义的置信度下的不确定性项，例如根据特定置信水平选择的半径，而不是未指定的平均误差。概念上的最小值为：

r_{inflate}=r_{robot}+r_{loc}+r_{safe}

其中 $r_{robot}$ 为机器人半径，定位不确定性由前面的项表示， $r_{safe}$ 为跟踪/停止裕度。实际上，安全裕度会随地图分辨率、传感器盲区、来车速度和制动能力而变化。裕度太小会导致碰撞；裕度太大则会导致无法通行。

图示：Duskcoil，概念图而非实测图。单元格大小和膨胀必须根据实际机器人占地面积、不确定性和操作范围推导得出。

网格搜索：Dijkstra 和 A*

对于具有非负边成本 $c(u,v)\ge0$ 的图 $G=(V,E)$ ，Dijkstra 算法会反复确定具有最低已知起始成本的未确定节点。 $g(n)$ 并放松邻居。一旦确定，其 $g$ 值即为最短。对于二叉堆，一个典型的复杂度为 $O((|V|+|E|)\log|V|)$ 。它保证了图的最短距离，但在没有目标信息的情况下，往往会过度扩展。

A* 算法按以下方式对节点进行排序：

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

其中 $h(n)$ 是剩余成本的下界。一个可接受的启发式函数永远不会高估真实的剩余成本；因此，A* 算法仍然是最优的。曼哈顿距离适用于 4 连通网格，而欧氏距离或切比雪夫距离可能适用于 8 连通移动。一个一致的启发式函数还满足 $h(n)\le c(n,n')+h(n')$ 并减少重新扩展。

加权 A* 算法通过 $w>1$ 缩放启发式函数，以牺牲最优性为代价更快地找到可行路径。这可能是一种合理的操作权衡。如果路径是明确的，则边的成本不仅可以编码长度，还可以编码膨胀障碍风险、净空、转弯、能量或地形。输出是最小定义成本，而不一定是最小几何长度。

连续和高维空间：RRT、RRT*、PRM

精细网格在六关节机械臂构型空间或车辆姿态空间中会变得非常复杂。RRT 从自由空间中采样 $x_{rand}$ ，找到最近的树节点 $x_{near}$ ，向其方向行驶一段有限的距离，进行碰撞检测，然后添加 $x_{new}$ 。它是概率完备的：在足够的样本量下，当存在可行路径时，找到可行路径的概率接近于 1。但它不能保证找到最短的初始路径。

RRT* 在附近的顶点中选择成本最低的父节点，并在成本更低的情况下通过新顶点重新连接邻居。它是渐近最优的，而不是有限时间最优的；邻居搜索、碰撞检测和重新连接都需要计算。在运行截止日期前衡量质量，而不是承诺“最优”。

PRM 对自由配置进行采样，并将附近无碰撞的配置对连接成可重用的路线图。它在静态工厂或重复机械臂查询环境中很有吸引力，因为预处理成本可以摊销。动态障碍物会使边失效。狭窄通道难以进行均匀采样，因此可能需要障碍物边界采样、路径偏置采样或任务信息采样。

图：Duskcoil，简化版。采样、碰撞检测和连接性并非性能测量或最终生产路线。

方法	空间/代表性成本	结果属性	良好拟合	主要失效模式
Dijkstra	图， $O((V+E)\log V)$	最短非负成本路径	无启发式，完整成本场	远离目标扩展
A*	图；最坏情况可比较	具有可接受 $h$ 的最短路径	单次网格查询	高估 $h$ ，成本较差
RRT	连续；依赖于样本	概率完备	快速可行的高维路径	狭窄通道，粗略碰撞检查
RRT*	连续；重连开销	渐近最优	在剩余时间内改进	截止时间/运行时间
PRM	预处理加查询	具有采样条件的概率完备	静态重复查询	动态空间中的过时边

SLAM、Nav2 和局部规划

SLAM 提供地图和位姿估计，但规划器需要时间戳对齐的变换以及清晰的占用/成本含义。闭环或重新定位可能会改变地图帧的位姿；继续沿旧路线行驶可能不安全。将不确定性、定位重置和地图更新事件输入到重新规划规则中；参见可视化 SLAM 入门。

在类似 Nav2 的 ROS 2 架构中，全局成本地图和规划器选择大范围路线，而局部成本地图和控制器处理附近的障碍物和速度。全局 A* 算法可以选择走廊；局部层必须让行、停止或绕行行人。纯粹的局部层可能会陷入死胡同。明确定义规划器、控制器、恢复机制、地图更新频率、截止时间和优先级。 ROS 2 Primer中描述的ROS 2传输机制并不能保证实时性和安全性。

部署前，请测量有效载荷、传感器视场角、最大速度/减速度、定位误差和地图分辨率等参数。测试在狭窄通道中的实际通行能力。检查规划路径与连续运动和运动学参数之间的碰撞情况：网格路径可以在单元格之间以差速驱动、车辆或机械臂无法实现的方式转弯。

对于动态障碍物，请测量检测的及时性、相对速度、制动距离和重新规划时间；切勿因规划器延迟而继续移动。将陌生人、坑洞、透明障碍物和传感器故障视为安全情况，而非自动释放单元格。当没有可用路径、局部路径不安全、协方差过大、地图过时或跟踪误差超出其范围时，应减速、停止或交出控制权。紧急停止功能必须独立于规划器的输出运行。

实现清单和安全性

定义状态空间、覆盖范围、帧、地图分辨率以及未知单元格的含义。
使膨胀过程包含定位误差、速度和停止距离；测试真实的狭窄通道。
验证启发式方法的可采纳性，或记录故意放宽的保证。
记录采样规划器的碰撞检测结果、随机种子、截止时间和无解行为。
注入重新定位、地图变化、传感器丢失、动态障碍物和通信延迟。
验证无路径、地图过时和跟踪偏差事件的安全停止和可诊断的日志路径。

参考资料

检查你的理解

车辆能否沿着任意无障碍路线行驶？

车辆的行驶半径和转弯限制至关重要。车辆到达某一点的路径未必可行。

Try it in the browser

Lab Change the conditions in a browser experimentNav2 有路径但不会移动：诊断工作流程

控制工程入门：路径规划——A、Dijkstra、RRT、RRT 和 PRM