上个月帮一个飞控团队做路径规划方案选型对方一开始只提了两个要求让无人机在仿真环境里绕开障碍物飞到目标点轨迹别抖。等我把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)。这条轨迹要满足的是动力学约束比如最大速度、最大加速度甚至还要考虑电机力矩上限。所以完整的机器人运动规划流程通常是两段式用搜索类算法Dijkstra、A*或采样类算法RRT*在环境中找出一条无碰撞的离散路径。用优化类算法Minimum Snap、Minimum Jerk把这条离散路径变成光滑、动态可行的轨迹。A和Minimum Snap正好站在这个流程的两端中间还缺一个能在复杂连续空间里快速找到路径的算法那就是RRT。而Dijkstra作为A*的前身能帮你理解启发式搜索到底优化了什么。把这四个算法串起来你其实就掌握了一条完整的技术链路。1.2 四个算法的定位对比我用下面的表格总结一下这四个算法在我实际项目里的定位差异算法所属类别输入输出适用场景Dijkstra图搜索栅格地图/拓扑图离散路径点带权图最短路径保证全局最优A*启发式图搜索栅格地图/拓扑图离散路径点静态环境快速寻路Dijkstra的加速版RRT*随机采样连续空间/高维构型空间离散路径点高维空间、复杂约束条件下的可行路径Minimum Snap轨迹优化路径点序列时间分配光滑时间多项式轨迹无人机/机械臂的平滑、动态可行轨迹注意一个细节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) 0A*退化成Dijkstra。如果h(n)永远小于等于真实代价可采纳性A*保证找到最短路径。如果h(n)更接近真实代价搜索效率越高但计算量可能增加。如果h(n)经常高估真实代价A*会更快但可能丢掉最优路径。在栅格地图中常用的启发函数有三种启发函数公式适用场景曼哈顿距离h dx欧氏距离h sqrt(dx^2 dy^2)八邻域移动任意角度对角线距离h max(dx, dy)或考虑对角代价八邻域移动且对角代价合理我在做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.80.9倍作为安全余量并根据传感器误差动态调整。第三路径平滑。A*输出的路径是折线机器人直行、转弯交替出现直接执行会一顿一顿的。常见做法是“视线优化”从起点开始依次检查路径点若能直连后一个点且不碰障碍物就跳过中间点。这样可以把折线拉直大幅减少转向次数。4. RRT*靠随机采样对抗高维空间4.1 RRT基本框架栅格搜索算法在高维空间会遇到两个致命问题栅格数量随维度指数增长内存爆炸而且机器人关节空间里很多约束比如机械臂的关节角度限制很难表达成栅格。这时候就要换思路——不再遍历整个空间而是用随机采样去碰概率。RRTRapidly-exploring Random Tree的思路非常直白在状态空间中随机采样一个点x_rand。在已有树上找到离x_rand最近的节点x_near。从x_near朝x_rand方向延伸固定步长得到新节点x_new。如果x_new到x_near这条边不碰障碍物就把x_new加入树中。重复直到新节点接近目标。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_iter2000, step_len0.5, search_radius0.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.51米比较合理。搜索半径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 Σ_{i4} 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) / (ij-7) * (t_f^(ij-7) - t_0^(ij-7))当i 4或j 4时对应项为零因为次数低于4的项求四阶导后为0。整个问题变成一个带等式约束的二次规划QPminimize: c^T Q c subject to: A_eq c b_eq等式约束用来强制轨迹在关键路径点上的位置、速度、加速度满足要求。比如在起点和终点位置固定为给定值速度、加速度通常设为0在中间路径点只约束位置让速度、加速度连续即可。5.3 一个实际求解的简化过程实际工程里我们会把整段轨迹按照路径点分成多个时间段每个时间段用一个多项式表示比如5段轨迹对应5个多项式通常为5阶或7阶。分段多项式的好处是可以在中间路径点自由设置“该点的速度/加速度连续”约束且每段次数不用太高避免数值不稳定。下面是我常用的一个Python求解框架依赖NumPy和CVXPY或quadprogimport numpy as np def compute_flat_snap_trajectory(waypoints, time_alloc, order7): waypoints: [(x0,y0,z0), (x1,y1,z1), ..., (xn,yn,zn)] time_alloc: [t0, t1, ..., tn]第一个为0最后一个为总时间 segments len(waypoints) - 1 # 每个多项式用 order1 个系数表示 # 构建 Q 矩阵分段对角 Q np.zeros((segments * (order 1), segments * (order 1))) for idx in range(segments): T time_alloc[idx1] - 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) / (ij-7) * T**(ij-7)) Q[idx*(order1):(idx1)*(order1), idx*(order1):(idx1)*(order1)] 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.21.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米下面是大致结果算法搜索/优化耗时路径长度轨迹平滑度可复现性Dijkstra750ms68.3m差折线完全可复现A*120ms65.1m差折线完全可复现RRT*2000轮2.6s70.5m差折线弱随机种子影响A*路径 Minimum Snap15ms优化阶段65.1m不变好完全可复现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教你最后的动态平滑。这条线走完一遍你对机器人运动规划的理解就不是“会调用哪个库”而是“知道在哪个环节该解决什么问题以及为什么要这么解决”。