引言想象你站在一个陌生的城市里手机导航告诉你“从当前位置到火车站预计步行15分钟”。你并不需要知道每一条街道的名字只需要跟着屏幕上的蓝色线条走就行了。这条蓝色线条就是路径规划的结果。路径规划是机器人学、自动驾驶、游戏AI等领域最基础也最关键的问题给定起点和终点以及环境中的障碍物找到一条可行且尽可能优的路径。从扫地机器人绕开茶几到自动驾驶汽车选择最佳车道再到游戏里的角色自动寻路背后都离不开路径规划算法。路径规划算法可以粗略分为两大类基于搜索的算法如Dijkstra、A和基于采样的算法如RRT、RRT。前者适合结构化环境有明确的地图网格后者适合高维空间或复杂环境如机械臂的关节空间。如果把路径规划比作“在迷宫中找出口”那么基于搜索的算法就是“拿着地图逐格排查”而基于采样的算法则是“闭着眼睛随机走边走边标记”。两种方法各有优劣适用于不同的场景。前置知识在学习路径规划算法之前你需要了解以下基础概念图Graph由节点Node和边Edge组成的数据结构用于表示环境中的位置和可通行关系。代价Cost从一点移动到另一点的开销可以是距离、时间、能耗等。启发式Heuristic对“从当前点到目标点还有多远”的估计。估计越准搜索越快。障碍物Obstacle环境中不可通行的区域。状态空间State Space所有可能位置的集合。对于二维平面状态空间就是整个地图对于机械臂状态空间是所有可能的关节角度组合。第一章基于搜索的路径规划——Dijkstra与A*1.1 Dijkstra算法一步步“摊开”探索Dijkstra算法是图论中最经典的最短路径算法之一。它的核心思想是从起点开始逐步探索所有可达节点每次选择当前距离起点最近的未探索节点进行扩展直到到达终点。算法步骤将起点的距离设为0其他所有节点的距离设为无穷大。将所有节点放入一个优先队列按距离排序。重复以下步骤直到队列为空或到达终点从队列中取出距离最小的节点u。对于u的每个邻居v如果通过u到达v的距离比当前记录的v的距离更短则更新v的距离并将v加入队列。当终点被取出时算法结束此时记录的距离就是最短距离。Dijkstra的特点保证找到最短路径当所有边权非负时。缺点没有方向感——它会向所有方向均匀扩展即使目标就在右边它也会先探索左边。在大型地图上效率较低。如果把Dijkstra比作“在黑暗中摸索”它就像一个盲人用手杖一寸一寸地探测周围虽然最终能找到出口但会走很多冤枉路。1.2 A*算法给Dijkstra装上“指南针”A*算法在Dijkstra的基础上引入了启发式函数用“方向感”来引导搜索。它的核心公式是f(n)g(n)h(n)f(n)g(n)h(n)其中g(n)从起点到当前节点n的实际代价和Dijkstra一样。h(n)从当前节点n到目标节点的估计代价启发式函数。f(n)从起点经过n到目标点的总估计代价。A算法每次选择f(n)最小的节点进行扩展。如果h(n)设计得好不超过真实距离A一定能找到最短路径而且比Dijkstra快得多。常见的启发式函数环境类型启发式函数说明四方向网格上下左右曼哈顿距离|x₁-x₂| |y₁-y₂|八方向网格含对角线切比雪夫距离max(|x₁-x₂|, |y₁-y₂|)任意方向欧几里得距离√((x₁-x₂)² (y₁-y₂)²)A*的特点比Dijkstra快得多因为有启发式函数引导搜索方向。保证最优只要h(n)是可采纳的即不超过真实距离。缺点随着地图变大计算时间和路径代价可能呈指数级增长。如果把Dijkstra比作“盲人摸象”A*就是“手里拿着指南针的探险家”——它知道目标在哪个方向会优先探索那个方向少走很多弯路。1.3 Dijkstra vs A*一张表看懂对比维度DijkstraA*核心思想按距离均匀扩展按“距离估计”引导扩展是否需要启发式否是是否保证最优是非负权是h可采纳时搜索效率较低无方向较高有方向适用场景需要所有节点距离单目标最短路径第二章基于采样的路径规划——RRT与RRT*当环境维度很高如机械臂有6个关节或地图非常复杂时基于网格的搜索算法会面临“维度灾难”——状态空间太大无法全部遍历。基于采样的算法通过随机采样来探索空间不需要遍历整个地图。2.1 RRT快速扩展随机树随机生长的“触手”RRTRapidly-exploring Random Tree的核心思想是从起点开始像树的根系一样向空间中随机生长直到触碰到终点。算法步骤初始化树只包含起点。重复以下步骤直到到达终点或达到最大迭代次数随机采样在状态空间中随机生成一个点q_rand。找最近节点在树中找到距离q_rand最近的节点q_near。扩展从q_near向q_rand方向走一步步长固定生成新节点q_new。碰撞检测如果q_near到q_new的路径没有碰到障碍物则将q_new加入树中。当q_new到达终点附近时算法结束从终点回溯到起点得到路径。RRT的特点概率完备只要有足够时间一定能找到路径。速度快不需要遍历整个空间适合高维环境。缺点找到的路径通常不是最优的——路径往往呈“锯齿状”弯弯曲曲。如果把RRT比作“在黑暗中盲目生长的藤蔓”它从起点向各个方向随机延伸虽然最终能碰到目标但路径往往不是最短的。2.2 RRT*让RRT“越走越聪明”RRT是对RRT的改进目标是在找到路径的同时不断优化它。RRT与RRT的核心区别在于两个关键步骤步骤一重新选择父节点当新节点q_new生成后RRT*不会简单地把它挂在最近的节点上而是在q_new附近一定范围内寻找所有“近邻”节点计算“从起点经过每个近邻到q_new”的代价选择代价最小的那个作为q_new的父节点。步骤二重布线RewiringRRT*还会检查q_new附近的已有节点如果把某个近邻节点的父节点改成q_new是否能减小该节点到起点的代价如果可以就进行更改。这两个步骤让RRT*能够持续优化已生成的树随着迭代次数的增加路径越来越短。RRT*的特点渐进最优随着迭代次数增加路径不断优化最终逼近最优解。适应性强适用于各种复杂环境和高维空间。缺点计算开销大需要较多迭代次数才能得到满意路径。如果说RRT是“闭着眼睛乱长”RRT*就是“边生长边修剪”——它不仅让树向各个方向延伸还会不断修剪树枝让整棵树越来越紧凑、路径越来越短。2.3 RRT vs RRT*一张表看懂对比维度RRTRRT*核心思想随机扩展直到触达目标随机扩展 持续优化是否重新选父节点否是是否重布线否是路径质量较差锯齿状较好不断优化是否渐进最优否是计算开销较小较大第三章路径规划的实际应用3.1 自动驾驶中的路径规划自动驾驶的路径规划通常分为两个层次全局路径规划基于地图如高精地图从起点到目的地规划一条宏观路线通常用A*算法。局部路径规划在行驶过程中根据实时感知到的障碍物在全局路径的基础上进行避障调整常用RRT*或优化方法。3.2 游戏AI中的寻路在《星际争霸》《Dota》等游戏中当玩家给单位下达移动指令时游戏引擎会使用A算法在网格地图上计算路径。由于游戏地图通常是静态的A的效率优势非常明显。3.3 机器人运动规划对于机械臂等高自由度机器人状态空间是关节角度空间6维甚至更高无法用网格表示。RRT/RRT*是这类问题的常用解法。总结路径规划算法的选择取决于环境的复杂度和对最优性的要求场景推荐算法原因二维网格地图需要最短路径A*有方向感比Dijkstra快需要所有节点到起点的距离Dijkstra无启发式全图遍历高维空间如机械臂RRT/RRT*不需要遍历整个空间需要高质量路径且时间充裕RRT*渐进最优路径不断优化