路径规划基础：图搜索算法与路径生成
大家好，我是你们的机器人运动规划讲师。今天我们来聊聊路径规划里最基础、也最核心的一块内容——图搜索算法。说实话，这部分内容我每次讲都觉得特别亲切，因为我在做第一个机器人项目时，就是被这几个算法“折磨”过来的。但搞懂了它们，后面的路就好走了。
一、图搜索的核心思想
路径规划，说白了就是“从A点到B点，怎么走最合适”。我们通常把环境抽象成一张图，图里有节点（位置）和边（可通行路径）。然后算法就在这张图上找一条最优路径。
我个人习惯把图搜索分为两大类：
- 确定性搜索：比如Dijkstra、A*，它们知道整个地图信息，属于“全局规划”。
- 随机采样搜索：比如RRT，它不知道全局地图，边探索边规划，适合高维空间。
你想想看，如果地图很小，用Dijkstra就挺好。但如果地图很大，或者维度很高（比如机械臂的关节空间），那Dijkstra就扛不住了。这时候RRT就派上用场了。
二、Dijkstra算法：最朴素的“地毯式搜索”
Dijkstra算法是图搜索的“老大哥”。它的思路很简单：从起点开始，一层一层往外扩展，直到找到终点。每次扩展都选当前代价最小的节点。
我在项目中遇到过一个问题：用Dijkstra规划一条10公里长的路径，地图网格是0.1米分辨率，结果跑了整整3分钟才出结果。为什么？因为它把整个地图都搜了一遍，很多无关区域也搜了。
核心要点：Dijkstra保证找到最短路径，但效率低。适合小规模地图或对最优性要求极高的场景。
伪代码大概长这样：
function Dijkstra(Graph, start, goal):
dist = {start: 0}
prev = {}
queue = priority_queue([(0, start)])
while queue not empty:
current = queue.pop()
if current == goal:
return reconstruct_path(prev, goal)
for neighbor in Graph.neighbors(current):
new_dist = dist[current] + cost(current, neighbor)
if new_dist < dist.get(neighbor, inf):
dist[neighbor] = new_dist
prev[neighbor] = current
queue.push((new_dist, neighbor))
return None避坑指南：我曾经在项目中直接用Dijkstra处理动态障碍物，结果路径频繁重规划，机器人走走停停。后来我改用D* Lite（动态Dijkstra变种），才解决了这个问题。
三、A*算法：带“方向感”的搜索
A*算法是Dijkstra的升级版。它加了一个启发式函数h(n)，用来估计当前节点到终点的距离。这样搜索就有了“方向感”，不会盲目乱搜。
启发式函数怎么选？我建议：
- 网格地图：用曼哈顿距离（四方向移动）或欧几里得距离（八方向移动）
- 路网地图：用直线距离
- 高维空间：用欧几里得距离，但要注意维度灾难
你想想看，如果启发式函数h(n)永远小于真实距离，A*保证找到最优路径。如果h(n)大于真实距离，A*会更快，但可能不是最优。这个权衡在实际项目中很常见。
function AStar(Graph, start, goal):
open_set = priority_queue([(0, start)])
g_score = {start: 0}
f_score = {start: heuristic(start, goal)}
while open_set not empty:
current = open_set.pop()
if current == goal:
return reconstruct_path(came_from, current)
for neighbor in Graph.neighbors(current):
tentative_g = g_score[current] + cost(current, neighbor)
if tentative_g < g_score.get(neighbor, inf):
came_from[neighbor] = current
g_score[neighbor] = tentative_g
f_score[neighbor] = tentative_g + heuristic(neighbor, goal)
open_set.push((f_score[neighbor], neighbor))
return None注意：A*的启发式函数设计不好，会导致搜索效率下降。我曾经见过一个团队用欧几里得距离做启发式，但地图是四方向网格，结果搜索范围比Dijkstra还大。嗯，这里要注意：启发式函数要和实际移动方式匹配。
四、RRT算法：随机探索的“冒险家”
RRT（Rapidly-exploring Random Tree）算法和前面两个完全不同。它不依赖全局地图，而是通过随机采样来探索空间。每次随机选一个点，然后从最近的已有节点向这个点生长一段距离。
我记得第一次用RRT做机械臂规划时，看到它像藤蔓一样在关节空间里“乱窜”，心里直打鼓。但后来发现，它确实能绕过各种障碍物，找到一条可行路径。
RRT的核心步骤：
- 随机采样一个点q_rand
- 找到树上离q_rand最近的节点q_near
- 从q_near向q_rand方向生长一步，得到q_new
- 检查q_new到q_near的路径是否碰撞
- 如果无碰撞，将q_new加入树中
- 重复直到q_new接近目标
function RRT(start, goal, environment):
tree = [start]
while not reached_goal(tree, goal):
q_rand = random_sample(environment)
q_near = nearest_neighbor(tree, q_rand)
q_new = steer(q_near, q_rand, step_size)
if not collision(q_near, q_new, environment):
tree.append(q_new)
if distance(q_new, goal) < threshold:
return extract_path(tree, start, q_new)
return None个人经验：RRT的步长设置很关键。步长太大，容易跳过狭窄通道；步长太小，树生长太慢。我一般设成地图尺寸的1/20到1/10，然后根据实际效果微调。
五、路径生成与离散化
算法找到的路径通常是一系列节点（离散点）。但机器人不能直接沿着这些点走，因为：
- 节点之间是直线连接，不够平滑
- 节点密度可能不均匀
- 机器人有运动学约束（比如不能原地转弯）
所以我们需要做两件事：
- 路径离散化：把路径上的节点插值成更密集的点序列。比如每0.1米一个点。
- 路径平滑：用样条曲线（如B样条、贝塞尔曲线）对离散点进行拟合，生成连续平滑的轨迹。
我建议用三次B样条做平滑，因为它能保证C2连续（加速度连续），而且计算量不大。代码实现大概是这样：
import numpy as np
from scipy.interpolate import splprep, splev
def smooth_path(path_points, num_points=100):
# path_points: Nx2 或 Nx3 的路径点
path_array = np.array(path_points)
# 参数化
tck, u = splprep([path_array[:,0], path_array[:,1]], s=0)
# 生成平滑点
u_new = np.linspace(0, 1, num_points)
smooth_x, smooth_y = splev(u_new, tck)
return np.column_stack((smooth_x, smooth_y))避坑指南：我曾经直接用A*输出的路径点驱动机器人，结果机器人走到拐角时剧烈抖动，差点撞墙。后来加了平滑处理，问题就解决了。记住：路径规划的输出只是“建议”，真正执行前一定要做平滑和插值。
六、知识体系总览
下面这张图总结了本章的核心内容，我画成了流程图，方便你理解各个算法之间的关系和应用场景。
这张图把本章的知识点串起来了。你可以看到，确定性搜索和随机采样搜索是两条不同的技术路线，各有各的适用场景。而不管用哪种算法，最后都要经过路径离散化和平滑，才能变成机器人能执行的轨迹。
总结一下：Dijkstra适合小地图、追求最优；A*适合中等地图、需要效率；RRT适合高维空间、复杂障碍。实际项目中，我经常把A*和RRT结合起来用——先用A*做全局规划，再用RRT做局部避障。效果还不错。
好了，这一章的内容就到这里。路径规划是机器人运动控制的基石，搞懂了这些算法，后面学轨迹优化、避障策略就会轻松很多。记住我的一句话：没有最好的算法，只有最合适的算法。选算法时，一定要考虑你的地图大小、维度、实时性要求。