第四章 路径规划基础：图搜索与采样算法
各位同学好，我是你们的老朋友。今天咱们来聊聊路径规划——说白了，就是让机器人知道“怎么走”的问题。
我记得刚入行那会儿，带我的老师傅跟我说过一句话：“飞控搞得好不好，一半看路径规划。”当时我不太理解，后来踩了不少坑才明白——路径规划确实是整个系统的骨架。骨架歪了，后面调参再漂亮也白搭。
这一章，咱们重点啃四个算法：Dijkstra、A*、RRT 和 RRT*。外加一个路径平滑的技巧。嗯，内容不少，但我会尽量讲得接地气些。
4.1 图搜索算法：Dijkstra 与 A*
先说说图搜索。你想想看，如果环境是已知的、离散的，比如一张栅格地图，那图搜索就是最直接的办法。
4.1.1 Dijkstra：最朴素的“地毯式搜索”
Dijkstra 算法的核心思想其实很简单：从起点开始，一层一层往外扩，每次都选当前代价最小的节点继续扩展。直到扩展到终点为止。
我在项目中遇到过一个问题：用 Dijkstra 做无人机航路规划，地图稍微大一点（比如 1000×1000 的栅格），计算时间就飙到好几秒。为什么？因为它没有方向感，四面八方都搜，很多无用功。
Dijkstra 的核心特点：
- 保证找到最短路径（前提是边权非负）
- 时间复杂度 O(V²) 或 O(E log V)（用优先队列）
- 没有启发信息，搜索范围大
// 伪代码：Dijkstra 核心逻辑
function Dijkstra(graph, start, goal):
openSet = {start}
gScore[start] = 0
while openSet is not empty:
current = node in openSet with smallest gScore
if current == goal:
return reconstructPath(cameFrom, current)
openSet.remove(current)
for each neighbor of current:
tentative_g = gScore[current] + dist(current, neighbor)
if tentative_g < gScore[neighbor]:
gScore[neighbor] = tentative_g
cameFrom[neighbor] = current
if neighbor not in openSet:
openSet.add(neighbor)
return failure我的小建议：如果地图不大（比如 200×200 以内），Dijkstra 完全够用。但要是地图大了，或者对实时性有要求，就得请出 A* 了。
4.1.2 A*：带“指南针”的 Dijkstra
A* 和 Dijkstra 的区别，说白了就一句话：Dijkstra 只知道“我已经走了多远”，A* 还知道“离终点大概还有多远”。
这个“大概还有多远”就是启发函数 h(n)。常用的有曼哈顿距离、欧几里得距离。我个人的习惯是，在栅格地图上用曼哈顿距离，在连续空间里用欧几里得距离。
// 伪代码：A* 核心逻辑
function AStar(graph, start, goal):
openSet = {start}
gScore[start] = 0
fScore[start] = h(start, goal) // 启发式估计
while openSet is not empty:
current = node in openSet with smallest fScore
if current == goal:
return reconstructPath(cameFrom, current)
openSet.remove(current)
for each neighbor of current:
tentative_g = gScore[current] + dist(current, neighbor)
if tentative_g < gScore[neighbor]:
gScore[neighbor] = tentative_g
fScore[neighbor] = tentative_g + h(neighbor, goal)
cameFrom[neighbor] = current
if neighbor not in openSet:
openSet.add(neighbor)
return failure避坑指南：我曾经因为启发函数选错了，导致 A* 搜出来的路径明显不是最优。后来才发现，启发函数必须满足“可容许性”——也就是估计距离不能超过实际距离。否则 A* 就退化成 Dijkstra 了，甚至更差。
4.2 采样算法：RRT 与 RRT*
图搜索虽然好，但有个硬伤——它需要把环境离散化。如果环境是连续的高维空间（比如机械臂的关节空间），栅格化会带来“维度灾难”。这时候，采样算法就派上用场了。
4.2.1 RRT：快速探索随机树
RRT 的思路很暴力：随机采样，然后往采样点方向生长。说白了就是“瞎蒙，但蒙得有策略”。
我记得第一次在仿真里跑 RRT，看到那棵树像藤蔓一样在障碍物之间蜿蜒生长，还挺有成就感的。但后来发现，RRT 有个毛病——它找到的路径往往很“绕”，不是最优的。
RRT 的核心步骤：
- 在自由空间随机采样一个点 q_rand
- 在树上找到离 q_rand 最近的节点 q_near
- 从 q_near 向 q_rand 方向生长一步，得到 q_new
- 如果 q_new 到 q_near 的路径无障碍，就把 q_new 加入树中
- 重复直到树到达目标区域
// 伪代码：RRT 核心逻辑
function RRT(start, goal, environment):
tree = {start}
for i = 1 to maxIterations:
q_rand = randomSample()
q_near = nearestNeighbor(tree, q_rand)
q_new = steer(q_near, q_rand, stepSize)
if collisionFree(q_near, q_new, environment):
tree.add(q_new)
if distance(q_new, goal) < threshold:
return extractPath(tree, q_new)
return failure4.2.2 RRT*：让路径更优
RRT* 是 RRT 的升级版。它多做了两件事：重新选择父节点（rewire）和重新布线。说白了，就是每加入一个新节点，都看看能不能“抄近道”。
我在做无人机避障时对比过 RRT 和 RRT*。同样的场景，RRT 跑了 500 次迭代找到一条路径，RRT* 跑了 800 次迭代，但路径长度短了 30%。嗯，这个 trade-off 值得考虑。
我的经验：如果对实时性要求极高（比如每秒要规划一次），RRT 更合适。如果对路径质量有要求（比如节省燃料），那就用 RRT*。当然，现在也有 RRT*-Connect、Informed RRT* 这些变种，大家感兴趣可以自己研究。
4.3 路径平滑与优化
算法规划出来的路径，往往是折线——一段一段的，拐角尖锐。这样的路径，飞控根本没法直接跟踪。为什么？因为无人机有惯性，急转弯会掉高度，甚至失控。
路径平滑，说白了就是把折线变成曲线。常用的方法有：
- B 样条曲线：控制点少，局部可调，适合在线使用
- 贝塞尔曲线：简单直观，但全局控制，改一个点整条线都变
- 多项式插值：适合做轨迹规划，可以同时约束位置、速度、加速度
平滑的核心原则：路径要连续，一阶导数（速度）要连续，二阶导数（加速度）最好也连续。这样飞控跟踪起来才顺滑。
// 示例：用三次样条做路径平滑
// 输入：路径点列表 waypoints = [(x0,y0), (x1,y1), ..., (xn,yn)]
// 输出：平滑后的轨迹参数
function cubicSplineSmooth(waypoints):
n = len(waypoints) - 1
// 构建三对角矩阵求解样条系数
// 这里省略具体数学推导，实际工程中常用现成库
// 比如 scipy.interpolate.CubicSpline
return splineCoefficients避坑指南：我曾经在平滑时没考虑障碍物，结果平滑后的路径穿过了障碍物。后来我加了一步“碰撞检测 + 局部重规划”，才算解决问题。记住：平滑不能以牺牲安全性为代价。
4.4 小结
这一章咱们聊了路径规划的四大金刚：Dijkstra 适合小地图、A* 适合有启发信息的场景、RRT 适合高维空间、RRT* 适合追求路径质量的场景。最后还讲了路径平滑——让规划出来的路径真正能用。
说实话，这些算法每个都值得单独开一章讲。但作为入门，大家先掌握核心思想，后面实战中再慢慢深入。下一章咱们会把这些算法放到真机上去跑，到时候你们就知道“纸上得来终觉浅”是什么意思了。
公众号:蓝海资料掘金营，微信deep3321