路径规划：让机器人从A到B的“智慧选择”

引言

想象你站在一个陌生的城市里，手机导航告诉你"从当前位置到火车站，预计步行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*：一张表看懂

对比维度	Dijkstra	A*
核心思想	按距离均匀扩展	按"距离+估计"引导扩展
是否需要启发式	否	是
是否保证最优	是（非负权）	是（h可采纳时）
搜索效率	较低（无方向）	较高（有方向）
适用场景	需要所有节点距离	单目标最短路径

第二章：基于采样的路径规划------RRT与RRT*

当环境维度很高（如机械臂有6个关节）或地图非常复杂时，基于网格的搜索算法会面临"维度灾难"------状态空间太大，无法全部遍历。基于采样的算法通过随机采样来探索空间，不需要遍历整个地图。

2.1 RRT（快速扩展随机树）：随机生长的"触手"

RRT（Rapidly-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的父节点。

步骤二：重布线（Rewiring）

RRT*还会检查q_new附近的已有节点：如果把某个近邻节点的父节点改成q_new，是否能减小该节点到起点的代价？如果可以，就进行更改。

这两个步骤让RRT*能够持续优化已生成的树，随着迭代次数的增加，路径越来越短。

RRT*的特点：

渐进最优：随着迭代次数增加，路径不断优化，最终逼近最优解。
适应性强：适用于各种复杂环境和高维空间。
缺点：计算开销大，需要较多迭代次数才能得到满意路径。

如果说RRT是"闭着眼睛乱长"，RRT*就是 "边生长边修剪" ------它不仅让树向各个方向延伸，还会不断修剪树枝，让整棵树越来越紧凑、路径越来越短。

2.3 RRT vs RRT*：一张表看懂

对比维度	RRT	RRT*
核心思想	随机扩展直到触达目标	随机扩展 + 持续优化
是否重新选父节点	否	是
是否重布线	否	是
路径质量	较差（锯齿状）	较好（不断优化）
是否渐进最优	否	是
计算开销	较小	较大

第三章：路径规划的实际应用

3.1 自动驾驶中的路径规划

自动驾驶的路径规划通常分为两个层次：

全局路径规划：基于地图（如高精地图），从起点到目的地规划一条宏观路线，通常用A*算法。
局部路径规划：在行驶过程中，根据实时感知到的障碍物，在全局路径的基础上进行避障调整，常用RRT*或优化方法。

3.2 游戏AI中的寻路

在《星际争霸》《Dota》等游戏中，当玩家给单位下达移动指令时，游戏引擎会使用A算法在网格地图上计算路径。由于游戏地图通常是静态的，A的效率优势非常明显。

3.3 机器人运动规划

对于机械臂等高自由度机器人，状态空间是关节角度空间（6维甚至更高），无法用网格表示。RRT/RRT*是这类问题的常用解法。

总结

路径规划算法的选择，取决于环境的复杂度 和对最优性的要求：

场景	推荐算法	原因
二维网格地图，需要最短路径	A*	有方向感，比Dijkstra快
需要所有节点到起点的距离	Dijkstra	无启发式，全图遍历
高维空间（如机械臂）	RRT/RRT*	不需要遍历整个空间
需要高质量路径且时间充裕	RRT*	渐进最优，路径不断优化