1. 从“寻路”到“寻优”路径规划算法的核心价值我们每天都在做路径规划。早上出门打开手机地图输入目的地软件会瞬间为你规划出几条路线并告诉你哪条最快、哪条最省油、哪条红绿灯最少。这背后就是路径规划算法在默默工作。但路径规划的价值远不止于此。从仓库里AGV小车的穿梭到无人机在复杂空域的自主飞行从游戏里NPC如何绕过障碍物找到玩家到物流公司如何为上千辆卡车安排最优的配送顺序——这些场景的核心都是路径规划算法。简单来说路径规划就是在一个有障碍物的空间里为移动的智能体可以是机器人、车辆、甚至是一个虚拟点找到一条从起点到终点的可行路径。但“可行”只是最低要求我们真正追求的是“最优”。这个“优”的标准因场景而异可能是距离最短、时间最少、能耗最低、安全性最高或者是综合了多种因素的复杂指标。因此路径规划算法从来不是一套固定的公式而是一个庞大的工具箱里面装满了针对不同问题、不同约束条件的专用工具。作为一名在机器人导航和智能调度领域摸爬滚打了十多年的工程师我处理过从简单的二维栅格地图寻路到考虑动态交通流、车辆动力学约束的三维时空联合规划。我发现很多初学者甚至一些有经验的开发者在面对具体问题时常常会陷入“算法选择困难症”A*听起来很万能RRT好像很高级Dijkstra是不是过时了其实没有最好的算法只有最合适的算法。理解每种算法的“脾气秉性”、能力边界和适用场景比死记硬背公式重要得多。这篇文章我就结合自己的实战经验为你系统性地拆解主流路径规划算法的内核帮你建立起清晰的认知框架让你在面对具体需求时能快速、准确地找到那把对的“钥匙”。2. 环境建模算法施展拳脚的舞台在讨论任何算法之前我们必须先搭建舞台——也就是对真实世界进行数学抽象这个过程叫做环境建模。模型建得好不好直接决定了后续规划的效率和质量。一个粗糙的模型会让再精巧的算法也步履维艰而一个过细的模型又会带来不必要的计算负担。常见的环境建模方法主要有以下几种它们各有优劣适用于不同的场景。2.1 栅格法最直观的“像素化”世界这是最经典、最易于理解和实现的方法。我们把整个环境划分成均匀的网格就像一张像素图片。每个格子Cell只有两种状态空闲可通行或占用障碍物。机器人或车辆被看作一个点或者占据若干个格子的物体。核心原理与实现假设我们有一个10x10米的房间将其划分为0.1米见方的格子就会得到一个100x100的二维数组。数组中的每个元素存储一个值比如0表示空闲1表示障碍物。规划时算法就在这个0和1组成的矩阵里寻找通路。实战心得与避坑指南分辨率选择是门艺术格子太大会丢失环境细节可能导致规划出的路径过于贴近障碍物甚至因为“锯齿状”路径而无法被实际机器人执行机器人有体积不是质点。格子太小地图数据量会呈平方级增长严重拖慢搜索速度。我的经验法则是格子的边长至少要比机器人的半径或半宽大20%-30%以确保安全裕度。“膨胀”障碍物是关键操作由于机器人有物理尺寸我们不能只把障碍物本身标记为占用还需要在其周围进行“膨胀”操作。例如机器人半径为0.3米我们就需要把所有障碍物向外“画”出一个0.3米的圈这个圈内的格子也标记为占用。这样规划时把机器人视为质点找到的路径就是绝对安全的。很多初学者忘记这一步导致仿真完美实物一跑就撞。存储与搜索优化对于大规模栅格地图直接使用二维数组可能内存效率低下。可以使用稀疏数据结构如只记录障碍物位置或者采用四叉树、八叉树用于三维进行分层管理加速空闲区域的查询。2.2 可视图法连接“安全点”的直线网络当环境中的障碍物可以用多边形如矩形、凸多边形精确描述时可视图法非常高效。其核心思想是如果路径是由一系列直线段组成那么只要这些线段不与任何障碍物相交路径就是安全的。因此我们把所有障碍物的顶点多边形角点以及起点、终点作为图的节点然后连接所有彼此“可见”连线不穿越障碍物的节点形成一张图。路径规划就变成了在这张图上寻找最短路径。核心原理与实现算法首先需要计算所有顶点之间的可见性。这是一个计算几何问题可以通过射线投射等方法判断连线是否与任何障碍物边相交。构建好可视性图后就可以运用图搜索算法如Dijkstra来寻找最短路径。优缺点与适用场景优点路径长度是精确的几何最短路径在给定的顶点集合下路径非常平滑由直线段组成非常适合轮式机器人。缺点构建可视性图的计算复杂度较高为O(n³)n为顶点数。当障碍物形状复杂、顶点众多时图会变得非常庞大。此外路径必须经过顶点这可能导致路径不是全局最优可能存在不经过顶点的更短路径。适用场景适用于障碍物数量不多、形状规则多为凸多边形的静态结构化环境比如室内房间、仓库货架间。2.3 拓扑法抓住环境的“骨架”拓扑法不关心环境的精确几何形状而是关注其连通性。它像抽象出一张地铁线路图只关心有哪些“站点”关键位置点如门、走廊交点以及它们之间如何连接而不关心站间轨道的具体弯曲程度。核心原理与实现通常通过“骨架化”或“路线图”方法来实现。例如对栅格地图进行细化提取出环境的中心线骨架骨架的交点和端点成为节点。或者在自由空间中随机采样点并连接彼此邻近且连线不碰撞的点形成一张随机路线图PRM。优缺点与适用场景优点表示非常紧凑特别适合大规模环境。规划速度快因为搜索空间图的规模远小于几何空间。高层任务规划如“去厨房拿杯水”常基于拓扑地图。缺点丢失了精确的几何信息因此基于拓扑图规划出的路径通常只是一个“序列”需要底层控制器结合局部地图进行跟踪。无法直接用于需要精确避障或考虑动力学约束的场景。适用场景大规模室内外导航如商场、校园、多层级的路径规划系统顶层用拓扑图做粗规划底层用几何地图做细规划和避障。2.4 选择与融合没有银弹在实际项目中我很少只使用单一的环境模型。更常见的做法是分层或融合。例如在自动驾驶中顶层路径规划可能使用车道级的拓扑图高精地图决定走哪条车道中层轨迹规划则使用连续的几何空间考虑车辆动力学生成平滑的轨迹底层控制再处理实时障碍物用栅格或点云表示。理解每种模型的本质才能灵活地将它们组合起来应对复杂的现实问题。3. 全局规划算法纵观全局的“战略家”全局规划器掌握环境的完整先验信息一张已知的地图它的任务是找出从起点到终点的最优或次优路径。这类算法是路径规划的基石。3.1 Dijkstra算法稳扎稳打的“老将”Dijkstra算法是图搜索中最经典的算法之一。它解决的是在带权重的图中找到从单一源点到所有其他节点的最短路径问题。算法流程拆解初始化创建一个集合S用于存放已找到最短路径的节点。为所有节点分配一个“最短距离估计值”起点设为0其他节点设为无穷大。所有节点的前驱节点设为空。循环当S未包含所有节点时执行 a. 从不在S中的节点里选出当前“最短距离估计值”最小的节点u。 b. 将节点u加入S。 c. 松弛操作对于u的每一个邻居节点v检查如果通过u到达v的距离即u的距离 边(u,v)的权重小于v当前的距离估计值则更新v的距离估计值为这个更小的值并记录u为v的前驱节点。结束当终点被加入S时算法可以提前终止。通过回溯前驱节点即可得到从起点到终点的最短路径。为什么它有效Dijkstra算法采用了贪心策略每次都选择当前已知的、距离起点最近的未处理节点。可以证明当一个节点被加入S时它到起点的距离就已经是最短距离不会再被更新。这是因为所有边的权重都必须为非负值。实战中的注意事项复杂度使用优先队列最小堆优化后时间复杂度为O((VE)logV)其中V是节点数E是边数。对于栅格地图这样每个节点只有4或8个邻居的图效率尚可。但对于节点非常多的图它会比较慢因为它“一视同仁”地向所有方向探索。权重设计算法的威力很大程度上取决于边的权重设计。权重可以是距离、时间、能耗甚至是风险系数。例如在越野路径规划中平坦草地权重为1泥泞地权重为5陡坡权重为10算法会自动找出综合代价最低的路径。提前终止实际应用中我们通常只关心到特定终点的路径。因此一旦终点被从优先队列中弹出即其最短路径已确定算法就可以立即停止无需计算所有节点的最短路径。3.2 A*算法有“方向感”的智慧搜索A*算法是对Dijkstra算法的革命性改进它通过引入一个启发式函数让搜索变得有方向性从而极大地提高了效率。它是游戏AI和机器人学中应用最广泛的路径规划算法。核心思想A*为每个节点n维护一个代价函数f(n) g(n) h(n)。g(n)从起点到节点n的实际代价与Dijkstra中的距离相同。h(n)从节点n到终点的估计代价这就是启发式函数。算法总是优先扩展f(n)值最小的节点。g(n)保证了路径的最优性不走冤枉路h(n)则引导搜索朝向终点的方向避免了像Dijkstra那样盲目地向四周扩散。启发式函数h(n)的设计艺术 启发式函数是A*的灵魂它必须满足两个条件可采纳性h(n)必须永远不大于从n到终点的实际最小代价。这保证了A*找到的路径是最优的。一致性单调性对于任意节点n和其后继节点n’有h(n) cost(n, n) h(n)。这保证了算法的高效性。常用启发式函数曼哈顿距离适用于只能朝上下左右四个方向移动的栅格四连通。h(n) |x_n - x_goal| |y_n - y_goal|。欧几里得距离适用于可以朝任意方向移动的栅格八连通或连续空间。h(n) sqrt((x_n - x_goal)² (y_n - y_goal)²)。这是最常用的启发函数。对角线距离用于八连通栅格的更精确估计。为什么A*比Dijkstra快得多我们可以看一个极端例子在一个空旷的栅格地图中从左上角寻路到右下角。Dijkstra会像一个不断扩大的圆盘一样均匀探索而A*由于有h(n)的引导其探索区域更像一个指向终点的椭圆访问的节点数少得多。代码实现中的关键技巧# 伪代码结构示意 open_set PriorityQueue() # 优先队列按f值排序 open_set.put(start, f(start)) came_from {} # 记录路径 g_score {node: inf for node in all_nodes} g_score[start] 0 f_score {node: inf for node in all_nodes} f_score[start] h(start) while not open_set.empty(): current open_set.get() if current goal: return reconstruct_path(came_from, current) for neighbor in get_neighbors(current): tentative_g_score g_score[current] cost(current, neighbor) if tentative_g_score g_score[neighbor]: # 找到一条到neighbor的更优路径 came_from[neighbor] current g_score[neighbor] tentative_g_score f_score[neighbor] g_score[neighbor] h(neighbor) if neighbor not in open_set: open_set.put(neighbor, f_score[neighbor])避坑指南与性能调优启发函数的选择至关重要h(n)越接近真实代价A的效率越高。如果h(n) 0A就退化成了Dijkstra如果h(n)恰好等于真实代价A*将沿着最优路径直线前进效率最高。但h(n)绝不能大于真实代价否则会破坏最优性。处理平局情况当多个节点f值相同时优先队列的弹出顺序会影响搜索效率。一个常见的技巧是优先处理h值更小的节点这能进一步引导搜索。内存与速度的权衡A需要维护open_set和closed_set或通过g_score判断。对于超大规模地图内存可能成为瓶颈。此时可以考虑迭代加深A(IDA*) 等变种它们用时间换空间。非网格环境的应用A*同样可以应用于可视性图或拓扑图只需定义好节点、邻居关系和代价函数即可。3.3 D与DLite应对变化的“动态规划师”现实世界是动态的。当机器人沿着A规划好的路径行进时地图上可能突然出现未知障碍物比如一把突然挪到路上的椅子。重新从当前位置调用A进行全局规划固然可以但效率低下因为它丢弃了之前所有的计算成果。D*Dynamic A*及其优化版本D* Lite算法就是为了解决这个问题而生。核心思想——反向搜索与代价传播 与A从起点正向搜索到终点不同D系列算法采用反向搜索。它最初假设环境是已知的从目标点开始向起点搜索计算出每个节点到目标点的最优代价。当机器人在行进中探测到某条边的代价发生变化时例如发现新的障碍物D*不会重新规划整条路径而是局部地、高效地更新受影响的节点代价并重新计算出一条新的最优路径。这个过程就像在水面投下一颗石子涟漪代价更新只传播到必要的范围。DLite的简化理解* D* Lite算法比原始D*更简洁高效是目前动态环境重规划的实际标准。它维护两个估计值g(s)与A*类似表示从起点到s的代价估计。rhs(s)一个基于节点s的邻居的、更易维护的g(s)估计值。如果g(s) rhs(s)则称节点s是“一致的”。当发现某条边(u, v)的代价增加时例如v变成障碍物算法会更新v的rhs值因为从u到v的路径不再可行然后将v及其受影响的前驱节点加入一个优先队列。算法不断从队列中取出最关键的节点进行处理通过局部传播更新使受影响的节点恢复“一致”状态从而快速得到新的路径。适用场景 D* Lite非常适合在部分未知或动态变化的环境中进行的在线重规划例如机器人探索未知区域、自动驾驶车辆在动态交通流中行驶。它的计算开销远小于每次变化都重新运行A*。4. 局部规划与反应式算法应对未知的“战术家”全局规划依赖于一张先验地图。但在真实世界中地图往往不完整并且充满了动态、未预料到的障碍物如行人、其他移动车辆。局部规划器或反应式控制器的作用就是基于机器人实时传感器激光雷达、摄像头、超声波的数据在全局路径的指导下进行实时避障和微调。4.1 动态窗口法兼顾动力学与安全的实时避障动态窗口法是一种非常经典且实用的局部规划方法它直接考虑了机器人的运动学速度、加速度限制和动力学约束在速度空间中进行搜索直接生成安全的控制指令线速度和角速度。算法原理三步走采样速度空间在机器人当前可达的线速度v和角速度ω范围内按照一定分辨率进行离散采样形成一系列(v, ω)对。这个范围由机器人的最大加减速度决定构成了一个“动态窗口”。轨迹模拟与评价对每一个速度对(v, ω)假设机器人以此速度匀速运动一段短时间如0.5-1秒模拟出未来一段轨迹。然后用一个评价函数给这条轨迹打分。评价函数通常包括朝向目标程度轨迹末端方向与目标点方向的偏差。速度倾向于选择更快的速度。安全距离轨迹上离最近障碍物的距离。这是最重要的安全项。与全局路径的贴合度轨迹末端与全局参考路径的距离。选择最优指令选择评价函数得分最高的(v, ω)对发送给机器人的底层电机控制器执行。然后在下一个控制周期通常是几十毫秒重复整个过程。为什么DWA如此有效因为它将规划与控制紧密结合。它不是在几何空间找一条路径而是在控制指令空间中直接寻找最优解。同时由于模拟的轨迹很短且基于当前传感器数据它能很好地处理动态障碍物。评价函数的设计非常灵活可以根据不同任务调整权重。实操中的调参经验模拟时间太短机器人“目光短浅”可能陷入局部最优如面对U型障碍太长计算量大且基于当前数据的预测会不准确。通常设置在0.5-2秒之间与机器人的最大速度匹配。评价函数权重这是调参的核心。在拥挤环境中应大幅提高“安全距离”的权重在开阔地带追求速度时则提高“速度”权重。需要大量实地测试来找到平衡点。振荡问题机器人在狭窄通道或对称障碍前可能左右摇摆。可以通过在评价函数中加入“平滑性”惩罚与前一个周期的速度变化量来缓解。4.2 人工势场法被“力”驱动的直观方法人工势场法的思想非常直观将目标点视为吸引机器人的“引力场”将障碍物视为排斥机器人的“斥力场”。机器人处在一个虚拟的合力场中像一个小球一样沿着合力的方向负梯度方向运动。数学模型引力场U_att(q) 0.5 * k_att * ρ^2(q, q_goal) 其中k_att是引力增益ρ是机器人当前位置q到目标点q_goal的距离。引力F_att -∇U_att -k_att * (q - q_goal)。斥力场U_rep(q) 0.5 * k_rep * (1/ρ(q, q_obs) - 1/ρ0)^2 如果ρ(q, q_obs) ρ0否则为0。其中ρ0是障碍物的影响半径k_rep是斥力增益。斥力F_rep -∇U_rep。合力F_total F_att Σ F_rep。机器人沿着F_total的方向移动。优点与致命缺陷优点概念简单计算量小反应速度快易于实现。缺陷——局部极小值问题这是势场法的阿喀琉斯之踵。当引力和斥力在某一点达到平衡时合力为零机器人会停滞不前。例如在对称的走廊里目标在正前方左右墙的斥力相等引力向前可能导致机器人在走廊入口就停止。或者当目标点在一个障碍物后面时机器人可能永远无法到达。解决方案与变种随机扰动当检测到机器人速度接近零时给它一个随机的小推力帮助它逃出局部极小点。导航函数一种经过特殊设计的势场能保证只有一个全局极小值在目标点但构造复杂。与其他方法结合通常不单独使用势场法作为规划器而是作为局部避障的辅助手段或者与全局规划器结合由全局规划器提供一条粗略路径势场法负责局部跟踪和避障。4.3 向量场直方图与障碍物膨胀VFH及其变种VFH VFH*是另一种高效的反应式方法特别适用于基于激光雷达的机器人。核心思想VFH将机器人周围的障碍物信息来自激光雷达的扫描数据转换到极坐标直方图中。直方图的每个扇区代表一个角度其值代表该方向上的障碍物密度。然后算法在直方图中寻找一个足够宽的、障碍物密度低的“山谷”这个山谷的方向就是机器人应该前进的方向。同时它会考虑当前机器人的运动方向以保持运动的平滑性。与DWA的对比DWA在速度空间搜索直接输出控制指令天然考虑动力学。VFH在角度空间搜索输出的是一个期望的行驶方向需要再配合一个速度控制器。VFH计算通常更快但在处理复杂动力学约束方面不如DWA直接。障碍物膨胀的再强调无论是DWA还是VFH在计算到障碍物的距离时必须使用膨胀后的障碍物。传感器检测到的是障碍物的边缘但规划时需要的是机器人的轮廓与障碍物边缘的距离。这一步的疏忽是导致实物测试碰撞的最常见原因之一。通常的做法是在传感器数据层或代价地图层直接对障碍物进行膨胀处理。5. 采样规划算法解决高维与复杂约束的“探险家”当规划空间维度很高如机械臂有6个以上关节或者路径需要满足复杂的微分约束如车辆不能横向移动时基于图搜索的方法如A*会面临“维度灾难”——搜索空间太大。采样规划算法通过随机采样的方式来探索空间不求最优解但求快速找到一条可行解。5.1 快速探索随机树从起点生长一棵树RRT的基本思想非常直观从起点开始随机地向空间中的点生长一棵树直到树的某个分支触及终点附近。基本RRT算法步骤初始化树T仅包含起点q_start。循环直到满足终止条件如达到最大迭代次数或找到路径 a.随机采样在自由空间中随机生成一个点q_rand。 b.最近邻查找在树T中找到距离q_rand最近的节点q_near。 c.扩展从q_near朝着q_rand的方向以固定步长ε生长一段得到一个新点q_new。检查q_near到q_new的连线是否与障碍物碰撞。 d.添加节点如果无碰撞则将q_new加入树T并将q_near设为q_new的父节点。当q_new进入终点区域时算法成功。通过从终点回溯到起点即可得到路径。为什么RRT有效它的随机采样特性使其能快速探索高维空间。虽然单次扩展是随机的但整体上树会以概率1的方式逐渐充满整个自由空间。RRT的局限性不是最优的找到的路径通常曲折、不光滑。效率不稳定由于随机性规划时间方差较大。对狭窄通道不友好在狭窄通道口随机采样点很难恰好落在通道内导致树难以通过。5.2 RRT*渐进最优的改进RRT*在RRT的基础上增加了两个关键步骤使其能找到渐进最优随着采样点增加路径代价无限接近最优的路径。两大核心改进重新选择父节点在成功添加q_new后RRT*不会简单地将其父节点定为q_near。它会在q_new附近的一个邻域内例如半径为r的球内寻找树T中所有的节点检查如果以这些节点中的某一个作为q_new的父节点是否能得到一条从起点到q_new代价更低的路径。如果是就重新连接重选父节点。重布线在重新选择父节点后RRT*还会考虑q_new是否能成为其邻域内其他节点的更好父节点。即对于邻域内的每个节点q_nearby检查如果让q_nearby以q_new为父节点是否能降低从起点到q_nearby的代价。如果是就断开q_nearby与原父节点的连接重新连接到q_new上。效果这两个操作使得树的结构不断被优化路径代价随着迭代次数增加而不断降低。最终得到的路径比基本RRT平滑、高效得多。5.3 应用于车辆路径规划满足微分约束标准的RRT/RRT在几何空间中工作生成的路径可能无法被车辆执行例如要求车辆瞬间侧向移动。为了解决这个问题产生了运动学RRT等变种。关键修改状态空间节点的状态不再是(x, y)而是(x, y, θ, v, ...)包含了位置、朝向、速度等。距离度量最近邻查找不能再用简单的欧氏距离而需要使用能反映状态差异的度量例如结合位置差和角度差。扩展Steering这是最核心的改动。不能简单地从q_near向q_rand画一条直线。需要调用一个局部规划器或控制系统模拟器。例如给定当前状态q_near位置、朝向、速度和一个控制输入(加速度 前轮转角)通过积分车辆运动学模型如自行车模型一段时间得到一条可行的轨迹片段其末端状态就是q_new。这个q_new是动力学可行的。代价函数路径的代价不再是几何长度可能是时间、能耗或舒适度与加速度、曲率相关。通过这样的改造RRT*就能在满足车辆非完整约束不能侧移和动力学约束的情况下规划出可行的轨迹。这在自动驾驶的复杂场景规划中非常有用。6. 算法选型与工程实践指南面对琳琅满目的算法如何选择我总结了一个简单的决策流程和实战心得。选型决策树环境是否完全已知、静态是- 使用全局规划器。需要最优路径吗是 -A*如果图规模不大。需要处理任意代价函数是 -Dijkstra。环境是规则多边形考虑可视图法。否有未知或动态部分- 需要全局局部结合。规划空间维度高或有复杂运动约束吗如机械臂、车辆是- 优先考虑采样规划器RRT*。否如地面移动机器人栅格导航- 继续。对实时性要求极高且环境动态性强吗是-局部规划器作为主控制器如DWA并可能需要一个轻量级的全局规划器如D* Lite提供粗略指导。否- 可以使用A*进行周期性重规划。工程实践中的关键点分层规划架构是主流几乎没有单一算法能解决所有问题。一个典型的机器人导航栈如ROS中的Navigation Stack就采用了分层架构全局规划层A*生成一条粗略路径局部规划层DWA/TEB负责跟踪全局路径并实时避障底层控制器执行速度指令。感知模块不断更新代价地图D* Lite的思想可以融入全局规划器的重规划模块中。代价地图的艺术规划算法依赖于代价地图。如何将传感器数据激光、视觉、深度融合成一张稳定、准确的代价地图本身就是一个大课题。常见做法是多层代价地图静态层先验地图、障碍层实时传感器、膨胀层。每个层有不同的衰减模型和更新策略。平滑与优化无论是A*在栅格上找出的“锯齿”路径还是RRT生成的“随机”路径通常都不够平滑不适合机器人直接跟踪。后处理步骤必不可少例如简单平滑使用梯度下降法或二次规划在保持避障的前提下最小化路径的曲率和长度。轨迹优化对于有时间要求的可以使用时间弹性带TEB等方法将路径点与时间关联同时优化路径形状和速度剖面使其动力学可行。仿真与实物的鸿沟在仿真中完美的算法在实物上可能失败。原因包括传感器噪声、执行器误差、延迟、地面打滑、机器人模型不精确等。必须进行充分的实物测试并在算法中增加鲁棒性设计如增加安全裕度、加入状态估计滤波器如卡尔曼滤波、设计恢复行为如被困时原地旋转重新建图。路径规划是一个理论与实践紧密结合的领域。理解算法的核心思想是基础但真正的能力体现在能够根据具体机器人平台、传感器特性、任务需求和环境特点将这些算法进行裁剪、组合和调优。没有一劳永逸的解决方案只有不断迭代和适配的过程。希望这篇总结能为你提供一个清晰的路线图当你在实践中遇到具体问题时知道该从哪个工具箱里拿起哪件工具并明白如何使用和打磨它。