机器人路径规划与轨迹优化：Dijkstra、A*、RRT*、Minimum Snap串讲

上个月帮一个飞控团队做路径规划方案选型，对方一开始只提了两个要求：让无人机在仿真环境里绕开障碍物飞到目标点，轨迹别抖。等我把A*、RRT*、Minimum Snap全部串起来跑完一轮，他们才意识到“规划”这件事其实被拆成了两个完全不同性质的问题——先找到一条路，再把这条路变成机器人能真正执行的运动曲线。很多人把这两个阶段混为一谈，导致代码写了一堆，机器人还是走不动。

这篇文章就把我常用的四个算法：Dijkstra、A*、RRT*、Minimum Snap按“从路径搜索到轨迹优化”这条主线完整过一遍。每个算法我都会讲清楚它解决什么问题、核心思路是什么、代码怎么写、实战里有哪些坑。无论你是刚接触机器人导航，还是已经跑通过某个算法但想系统理解几个方法之间的承接关系，这篇文章应该都能帮到你。

1. 为什么要把A*和Minimum Snap放在一起聊？

1.1 运动规划的全流程：从路径搜索到轨迹优化

先说一个最容易被新手绕晕的概念：路径规划和轨迹规划不是一回事。

路径规划解决的是“走哪条路”，输出的是一系列离散的点，比如(0,0) → (1,0) → (1,1) → (2,1)。这条路径只需要满足一个核心约束——不碰到障碍物。没人关心机器人经过这些点的时候速度是多少、加速度会不会超限。

轨迹规划解决的是“怎么走”，输出的是关于时间的函数，比如位置p(t)、速度v(t)、加速度a(t)。这条轨迹要满足的是动力学约束，比如最大速度、最大加速度，甚至还要考虑电机力矩上限。

所以完整的机器人运动规划流程通常是两段式：

1. 用搜索类算法（Dijkstra、A*）或采样类算法（RRT*）在环境中找出一条无碰撞的离散路径。
2. 用优化类算法（Minimum Snap、Minimum Jerk）把这条离散路径变成光滑、动态可行的轨迹。

A和Minimum Snap正好站在这个流程的两端，中间还缺一个能在复杂连续空间里快速找到路径的算法，那就是RRT。而Dijkstra作为A*的前身，能帮你理解启发式搜索到底优化了什么。把这四个算法串起来，你其实就掌握了一条完整的技术链路。

1.2 四个算法的定位对比

我用下面的表格总结一下这四个算法在我实际项目里的定位差异：

注意一个细节：Dijkstra和A的输出是路径点，RRT的输出也是路径点，但Minimum Snap的输入恰恰就是路径点。这个衔接关系非常关键，它决定了你在实际工程里必须把它们组合起来用，而不是只选其中一个。

2. Dijkstra：图搜索的“地基”与效率瓶颈

2.1 原理与实现

Dijkstra算法的本质是带权最短路径搜索。它维护一个优先队列，每次从队列中取出当前距离起点最近的未访问节点，然后松弛它周围所有邻居的距离。这个过程可以理解为“以起点为圆心，像水波一样向外扩散”，直到波前沿触达目标点。

我在实际项目中用Python写过一版最精简的Dijkstra，核心逻辑大致如下：

import heapq def dijkstra(grid, start, goal): # grid: 0表示可通过，1表示障碍 rows, cols = len(grid), len(grid[0]) dist = [[float('inf')] * cols for _ in range(rows)] dist[start[0]][start[1]] = 0 pq = [(0, start)] parent = {} while pq: d, (r, c) = heapq.heappop(pq) if d > dist[r][c]: continue if (r, c) == goal: break for dr, dc in [(1,0),(-1,0),(0,1),(0,-1)]: nr, nc = r + dr, c + dc if 0 <= nr < rows and 0 <= nc < cols and grid[nr][nc] == 0: nd = d + 1 if nd < dist[nr][nc]: dist[nr][nc] = nd parent[(nr, nc)] = (r, c) heapq.heappush(pq, (nd, (nr, nc))) # 回溯路径 path = [] node = goal while node in parent: path.append(node) node = parent[node] path.reverse() return path

这里我用的是四邻域移动，每一步代价为1。如果你用八邻域，对角线移动的代价应该是√2而不是1，否则路径会失真，而且后面A*的启发函数也要跟着调整。这是非常容易被忽略的细节。

2.2 为什么A*能取代它

Dijkstra最大的问题不是正确性，而是效率。它不会区分“这个方向看起来接近目标”和“这个方向明显背道而驰”，所有方向都一视同仁地扩展。在一个1000×1000的栅格地图里，如果目标点就在起点右前方500米处，Dijkstra依然会像涟漪一样扩散整个圆形区域，然后再找到目标。

我做过一个简单实验：在500×500的随机障碍地图上，Dijkstra平均要扩展约18万个节点才能找到目标，而A*只需要扩展约3万个节点。差距在6倍左右。地图越大、目标越远，这个倍数越夸张。

Dijkstra唯一不可替代的场景是：你需要保证在所有节点权重情况下的最短路径，且启发函数很难设计。比如路网中每条边的代价不是欧氏距离，而是跟地形、交通状况相关的动态值，这时候A*的启发函数可能失效，Dijkstra依然能给出正确结果。所以它至今仍是很多路由算法、网络协议的基础。

3. A*的核心：启发函数决定了搜索效率的天花板

3.1 代价模型与启发函数

A*在Dijkstra的基础上加了一个启发函数h(n)，用来估计从当前节点n到目标点的代价。整体评估函数是：

f(n) = g(n) + h(n)

其中g(n)是从起点到节点n的实际代价，h(n)是从n到目标的估计代价。A*每次从优先队列中取f(n)最小的节点进行扩展，所以它优先朝“看起来能更快到达目标”的方向搜索。

启发函数的选择直接决定A*的表现：

• 如果h(n) = 0，A*退化成Dijkstra。
• 如果h(n)永远小于等于真实代价（可采纳性），A*保证找到最短路径。
• 如果h(n)更接近真实代价，搜索效率越高，但计算量可能增加。
• 如果h(n)经常高估真实代价，A*会更快，但可能丢掉最优路径。

在栅格地图中，常用的启发函数有三种：

我在做AGV导航时，最初用了曼哈顿距离配合四邻域，路径是曲折的，AGV转弯频繁。后来改成八邻域+对角线距离，路径明显顺直，搜索速度还快了不少。

3.2 网格地图中的工程问题

A*的代码跟Dijkstra非常像，区别只在加入启发函数：

def astar(grid, start, goal): rows, cols = len(grid), len(grid[0]) start, goal = tuple(start), tuple(goal) def heuristic(a, b): # 对角线距离 dx, dy = abs(a[0] - b[0]), abs(a[1] - b[1]) return max(dx, dy) + (1.414 - 1) * min(dx, dy) g_score = {start: 0} f_score = {start: heuristic(start, goal)} pq = [(f_score[start], start)] parent = {} while pq: _, current = heapq.heappop(pq) if current == goal: break # 八邻域 for dr, dc in [(1,0),(-1,0),(0,1),(0,-1),(1,1),(1,-1),(-1,1),(-1,-1)]: nr, nc = current[0] + dr, current[1] + dc if not (0 <= nr < rows and 0 <= nc < cols): continue if grid[nr][nc] != 0: continue step_cost = 1.0 if dr == 0 or dc == 0 else 1.414 tentative_g = g_score[current] + step_cost if tentative_g < g_score.get((nr, nc), float('inf')): parent[(nr, nc)] = current g_score[(nr, nc)] = tentative_g f = tentative_g + heuristic((nr, nc), goal) heapq.heappush(pq, (f, (nr, nc))) # 回溯 path = [] node = goal while node in parent: path.append(node) node = parent[node] path.reverse() return path

这段代码我在多个项目里复用，遇到需要调整的地方基本是以下三个：

第一，open表去重。上面代码里没有处理重复入队的问题，因为tentative_g < g_score[...]这个判断已经保证只有更优的节点才会被重新计算，旧的堆元素即使还在堆里，取出来也会因为g_score不匹配被跳过。但如果你的地图很大，堆里的过期元素会占内存。工程上可以给每个节点维护一个version标记，入队时更新版本号，出队时校验版本号，及时丢弃过期元素。

第二，障碍物膨胀。直接把机器人当质点处理是新手最容易犯的错误。真实机器人体积有限，必须把障碍物向外膨胀机器人的半径。膨胀完之后的栅格地图才送给A*搜索。这个膨胀半径如果设太小，机器人过弯时会蹭墙；设太大，狭窄通道会被完全堵死，路径直接不存在。我在实际项目中通常取机器人半径的0.8～0.9倍作为安全余量，并根据传感器误差动态调整。

第三，路径平滑。A*输出的路径是折线，机器人直行、转弯交替出现，直接执行会一顿一顿的。常见做法是“视线优化”：从起点开始，依次检查路径点，若能直连后一个点且不碰障碍物，就跳过中间点。这样可以把折线拉直，大幅减少转向次数。

4. RRT*：靠随机采样对抗高维空间

4.1 RRT基本框架

栅格搜索算法在高维空间会遇到两个致命问题：栅格数量随维度指数增长，内存爆炸；而且机器人关节空间里很多约束（比如机械臂的关节角度限制）很难表达成栅格。这时候就要换思路——不再遍历整个空间，而是用随机采样去碰概率。

RRT（Rapidly-exploring Random Tree）的思路非常直白：

1. 在状态空间中随机采样一个点x_rand。
2. 在已有树上找到离x_rand最近的节点x_near。
3. 从x_near朝x_rand方向延伸固定步长，得到新节点x_new。
4. 如果x_new到x_near这条边不碰障碍物，就把x_new加入树中。
5. 重复直到新节点接近目标。

RRT的数学特性是：只要采样次数足够多，树就会指数级覆盖整个空间。这个“指数级”其实是它最大的优势——高维空间里其他方法都退化时，随机采样还能保持合理的搜索效率。

4.2 RRT*的rewire机制

RRT只能找到可行路径，但这条路径通常很差——绕路、抖动、不够短。RRT*在RRT的基础上增加了一个关键的**重连（rewire）**步骤：

当新节点x_new加入树中后，检查x_new周围半径r以内的所有节点。如果从x_new连接到某个邻居节点，得到的路径代价比它原来的父节点路径更小，就重新设置父节点。

这个操作让RRT*具备了渐进最优性——随着迭代次数增加，路径代价会不断降低，最终收敛到最优解。

核心代码框架如下：

def rrt_star(start, goal, area, obstacle_check, max_iter=2000, step_len=0.5, search_radius=0.8): nodes = [start] parent = {start: None} cost = {start: 0} for _ in range(max_iter): x_rand = sample_free_space(area) x_near = nearest(nodes, x_rand) x_new = steer(x_near, x_rand, step_len) if obstacle_check(x_near, x_new): continue # 找近邻节点 near_nodes = near(nodes, x_new, search_radius) # 选择代价最小的父节点 best_parent = x_near best_cost = cost[x_near] + distance(x_near, x_new) for candidate in near_nodes: if not obstacle_check(candidate, x_new): continue new_cost = cost[candidate] + distance(candidate, x_new) if new_cost < best_cost: best_cost = new_cost best_parent = candidate nodes.append(x_new) parent[x_new] = best_parent cost[x_new] = best_cost # Rewire for node in near_nodes: if node == best_parent: continue new_cost = cost[x_new] + distance(x_new, node) if new_cost < cost[node] and obstacle_check(x_new, node): parent[node] = x_new cost[node] = new_cost # 从目标回溯路径 return construct_path(parent, start, goal, nodes)

4.3 实用中的参数调整

RRT*最折磨人的三个参数是采样策略、步长和搜索半径。

采样策略上，我常用的改进是目标偏置采样：以概率5%～10%直接采样目标点，其余时候随机采样。5%对于二维平面够用，三维复杂环境可以提高到10%。太高会让树过早收敛到局部区域，太低会浪费迭代次数。

步长决定了树的扩展细粒度。步长太大会导致狭窄通道难以穿越，小缝隙直接被“跨过去”；步长太小会让树扩展缓慢。我的经验是步长选为环境特征尺寸的1%~5%，或者是从起点到目标的直线距离的2%左右。比如50米距离的巡检任务，步长取0.5～1米比较合理。

搜索半径r在RRT*中控制rewire的邻居范围。理论上应该是r = η * (log(n)/n)^(1/d)，其中d是空间维度。但这个公式计算量不小，实际工程里我通常根据节点密度动态调整：节点越多，半径越小，避免每次插入新节点都要遍历几百个邻居。也可以用KD树做近邻搜索，把耗时从O(N)降到O(logN)，在大规模场景中效果非常明显。

另外要提醒一个坑：RRT的结果没有可复现性。同样的地图和参数，两次运行可能得到不同路径。在需要确定性输出的场景（比如离线生成参考轨迹用于评审），你要固定随机种子，或者最后对RRT的结果再做一次平滑处理。

5. Minimum Snap轨迹优化：让无人机真的飞得起来

5.1 为什么路径不能直接飞

先看一个实际问题：A或RRT输出的路径点，能不能直接作为无人机飞行指令？答案是不能。

无人机飞行控制系统接收的一般是位置参考曲线，要求这条曲线至少保证位置连续、速度连续。如果用A*输出的折线作为位置指令，在折点处速度方向会瞬间改变，意味着加速度无限大，无人机根本不可能跟踪。就算你强行降低速度过弯，也会导致飞行时间增加、姿态抖动。

Minimum Snap的思想是：给定一系列路径点，生成一段多项式轨迹，使其四阶导数（加加速度的导数，英文常叫jerk rate或snap）平方积分最小。为什么要最小化snap？因为加加速度（jerk）直接影响电机力矩变化的剧烈程度，snap越小，飞行越平稳、电池效率越高、机械磨损越小。

5.2 最小加加速度的数学表达

对于每个维度（x、y、z），轨迹用一个高阶多项式表示：

p(t) = c0 + c1*t + c2*t^2 + ... + cN*t^N

写成向量形式：p(t) = c^T * T(t)，其中T(t) = [1, t, t^2, ..., t^N]^T。

Minimum Snap优化目标函数为：

J = ∫ (d^4p/dt^4)^2 dt

对多项式求四阶导：

d^4p/dt^4 = Σ_{i>=4} i*(i-1)*(i-2)*(i-3)*ci*t^(i-4)

这是一个关于系数c的二次型：J = c^T * Q * c。

矩阵Q里的每个元素都是对时间多项式积分的结果：

Q[i][j] = i*(i-1)*(i-2)*(i-3) * j*(j-1)*(j-2)*(j-3) / (i+j-7) * (t_f^(i+j-7) - t_0^(i+j-7))

当i < 4或j < 4时，对应项为零，因为次数低于4的项求四阶导后为0。

整个问题变成一个带等式约束的二次规划（QP）：

minimize: c^T Q c subject to: A_eq c = b_eq

等式约束用来强制轨迹在关键路径点上的位置、速度、加速度满足要求。比如在起点和终点，位置固定为给定值，速度、加速度通常设为0；在中间路径点，只约束位置，让速度、加速度连续即可。

5.3 一个实际求解的简化过程

实际工程里我们会把整段轨迹按照路径点分成多个时间段，每个时间段用一个多项式表示，比如5段轨迹对应5个多项式（通常为5阶或7阶）。

分段多项式的好处是：可以在中间路径点自由设置“该点的速度/加速度连续”约束，且每段次数不用太高，避免数值不稳定。

下面是我常用的一个Python求解框架，依赖NumPy和CVXPY或quadprog：

import numpy as np def compute_flat_snap_trajectory(waypoints, time_alloc, order=7): """ waypoints: [(x0,y0,z0), (x1,y1,z1), ..., (xn,yn,zn)] time_alloc: [t0, t1, ..., tn]，第一个为0，最后一个为总时间 """ segments = len(waypoints) - 1 # 每个多项式用 order+1 个系数表示 # 构建 Q 矩阵（分段对角） Q = np.zeros((segments * (order + 1), segments * (order + 1))) for idx in range(segments): T = time_alloc[idx+1] - time_alloc[idx] # 为当前段构造堆叠的导数系数 local_Q = np.zeros((order + 1, order + 1)) for i in range(4, order + 1): for j in range(4, order + 1): local_Q[i][j] = (i*(i-1)*(i-2)*(i-3) * j*(j-1)*(j-2)*(j-3) / (i+j-7) * T**(i+j-7)) Q[idx*(order+1):(idx+1)*(order+1), idx*(order+1):(idx+1)*(order+1)] = local_Q # 构建等式约束 A_eq c = b_eq # ... 这里省略详细组装代码，约束包括： # 1. 每段起点位置、速度、加速度 # 2. 每段终点位置、速度、加速度 # 3. 相邻段之间的连续性约束 # 用求解器求解 QP # import cvxpy as cp # c_var = cp.Variable(segments * (order + 1)) # prob = cp.Problem(cp.Minimize(cp.quad_form(c_var, Q)), # [A_eq @ c_var == b_eq]) # prob.solve() # return c_var.value # 这就是所有分段多项式的系数

这段代码我故意省略了约束矩阵细节，因为完整代码太长，但在实际项目中你真正要花时间的恰恰是这些约束。我建议大家先写一个单段8阶多项式的版本，验证约束和Q矩阵无误后，再扩展到多段。

5.4 时间分配：Minimum Snap最容易翻车的地方

轨迹优化里最容易被低估的参数是每个路径点之间的时间分配。同一个路径点序列，时间给得太短，轨迹为了赶上时间会产生很大的速度、加速度，甚至超过无人机物理极限；时间给得太长，轨迹虽然平滑但效率低下。

我常用的时间分配方法是“梯形速度近似”：假设相邻路径点之间按最大速度加速到巡航速度，再减速到0，估算一个基础时间，再乘以1.2～1.5的系数作为保守估计。

举个简单的例子：路径点间距2米，最大飞行速度4m/s，加速度上限2m/s²。那么加速到4m/s需要2秒，行驶距离4米，两个点间距只有2米，所以根本达不到最大速度，实际是加速到中点再减速，总时间约2倍加速时间，即2.8秒左右。取系数1.3，时间分配约3.6秒。

在三维立体空间中，还要注意各维度轨迹是独立优化的，但时间轴是共享的。如果某个维度分配的时间过短，该维度的速度或加速度会极大，此时就需要重新调整时间段分割，保证所有维度都在物理限制以内。

6. 四大算法串起来的实践建议

6.1 选型建议

我整理了一下日常项目里做选型的思路，不一定严谨，但很实用：

• 环境是否已知且静态？是，优先用A*，它速度快、可复现、容易调试。如果地图大且路径需要全局最优，考虑分层策略（先粗粒度、后细粒度）。
• 维度高、状态空间连续？比如六轴机械臂关节空间规划，A的栅格要离散成上千万个节点，内存扛不住，这时候RRT是更合理的起点。
• 目标不只是“无碰撞”而要“飞得稳”？不管是A还是RRT，最后都必须接Minimum Snap一类的轨迹优化器。这是很多demo项目跟量产项目之间最大的差距。demo停在路径可视化就结束，量产项目必须在仿真里跑完整的跟踪闭环。
• 环境中存在动态障碍物？上述四个算法全部不适用，需要引入速度障碍法（VO/RVO）或模型预测控制（MPC）做局部重规划。

6.2 实测数据：四种算法的性能对比

自己在仿真环境里跑过一组数据，环境是80米×80米、二维、含随机障碍物的地图，起点和终点距离约60米，下面是大致结果：

Dijkstra路径比A长，因为四邻域移动导致路径只能走45度折线；A换成八邻域后更接近直线；RRT*因为随机探索，路径明显更绕，但也稳拿了“能在复杂障碍环境里找到路径”这一项。真正让我觉得惊艳的是Minimum Snap，耗时只有十几毫秒，却把一条折线变成了连续平滑的飞行轨迹。

6.3 工程中的注意事项

最后的经验分享，每一条都是真金白银换来的。

第一，轨迹优化之前，先做路径压缩。A或RRT给出的路径点往往很密集，直接把这些点全部作为Minimum Snap的航路点，会导致多项式段数过多、计算量暴涨，而且轨迹被约束得过紧，平滑空间有限。我通常用道格拉斯-普克算法或者“视线优化”把路径点从几百个压缩到十几个关键点，再喂给Minimum Snap。这一步能让优化速度和轨迹质量同时提升。

第二，注意数值稳定性。Minimum Snap采用的是时间多项式，时间t的十几阶次方在数值上波动极大。如果总时间是10秒，t^7就是10^7量级，跟低次项系数差了好几个数量级，矩阵求逆很容易出问题。解决方法是做时间归一化，把每段时间归一化到[0,1]，求解完再把结果映射回真实时间。或者使用贝塞尔曲线、B样条作为基底，它们在数值稳定性上优于普通幂多项式。

第三，RRT*的路径要平滑但别过度平滑。如果你把RRT输出的路径点之间距离拉得很近，Minimum Snap会做非常剧烈的“纠正”来满足路径点位置约束，结果轨迹虽然穿过了所有路径点，但姿态变化很突兀。反过来，路径点距离太远，轨迹会大幅偏离预期走廊。我的做法是：RRT生成的路径先做降采样，然后给每个路径点设置一个允许误差半径，再使用走廊约束的轨迹优化，比如利用贝塞尔曲线凸包性质构造安全走廊，确保轨迹不碰障碍物。

第四，仿真能跑通不等于实机能跑。我在真实无人机上踩过一次坑：轨迹在仿真里完美跟踪，但真机一飞，终点处出现明显的超调震荡。排查好久才发现问题出在轨迹终点处对加速度约束不够——多项式只约束了加速度值，没有约束jerk（加加速度），导致结尾处的jerk极大，电机响应不过来。Minimum Snap的“最小snap”本身就是为了压制这个，但如果你没有显式检查各维度的最大jerk是否在电机能力范围内，最好再加一条硬约束或后处理限幅。

第五，时间分配比轨迹优化本身更值得调参。每次新场景都要重新观察速度轮廓，一旦发现在某段区间内速度先冲到上限再急刹，多半是这段的时间分配太短。调整时间分配后重新求解QP，通常能解决80%的“轨迹看着平滑但实际飞不动”问题。

这四个算法本身都不复杂，单独看源码都能看懂。真正有价值的是把它们串起来的那条线：Dijkstra教你图搜索的基本原则，A教你用启发式信息把搜索效率提高一个量级，RRT教你在高维空间里用采样换可行性，Minimum Snap教你最后的动态平滑。这条线走完一遍，你对机器人运动规划的理解就不是“会调用哪个库”，而是“知道在哪个环节该解决什么问题，以及为什么要这么解决”。