无人机三维路径规划这几年关注度一直很高，但真正能把静态路径搜索和动态避障串到一起跑通的资料其实不多。很多同学下载了所谓的“完整代码”，打开一看只有一条从起点到终点的静态航迹，所谓动态避障不过是在固定威胁边上画了个圆。我最早做这个方向的时候也踩过类似的坑，后来把整套逻辑拆开重写，才真正搞清楚启发式算法在三维空间里应该怎么落地。这篇就把我实际调试过程中积累的东西整理一下，重点是动态避障的触发机制、三维栅格地图的建模细节，以及Matlab代码里那些容易让人头秃的坑。

如果你正在做无人机航迹规划相关的课题或项目，手里有一份基于启发式算法的三维路径规划代码但不知道怎么改成真正的动态避障，或者想在仿真环境下复现完整的“全局规划+局部避让”流程，这篇文章应该能帮你省不少时间。

1. 三维路径规划和二维到底差在哪：先从问题定义说起

1.1 三维环境的建模方式：栅格法、概率路线图与采样式

三维路径规划和二维最大的区别，不在于多了一个Z轴坐标，而在于搜索空间的性质发生了根本变化。二维平面里，障碍物可以抽象成多边形，路径是一条不会相交的折线；三维空间里，无人机要考虑的不只是水平面上的绕行，还有高度层的选择——同样一个障碍物，我可以从左边绕、从右边绕，也可以从上面飞过去，关键是看飞行代价哪个更小。

环境建模方式直接影响后续所有算法的工作前提，这里对比一下三种主流做法：

栅格法是目前Matlab仿真里最常见的选择。不是因为它最好，而是因为它最直观：你可以在三维数组上直接做索引，障碍标记为1，空闲标记为0，A*算法的open list和closed list用三维节点坐标管理起来非常自然。但代价也很明显——一个100x100x50的栅格就是50万个节点，如果启发式函数设计得不好，搜索效率会非常难看。

我在实际项目里一般会把地图分辨率从默认的1米改成2米或3米做降采样测试，先把算法跑通，再逐步提高精度。仿真阶段千万别一上来就用高分辨率栅格，否则你一天的时间都会耗在等待搜索完成上。

1.2 代价函数里藏着所有约束：长度、威胁、高度和平滑

路径规划的本质，不是“找到一条路”，而是在满足约束的前提下找到一条代价最低的路。代价函数是启发式算法的灵魂，也是最容易被低估的部分。

一套完整的三维路径代价函数，至少应该包含四个分量：

1. 路径长度代价：这是最基础的，越短越好，用相邻节点的欧氏距离累加即可。
2. 威胁代价：对地图里每个威胁区的中心点，计算路径节点到中心的距离，距离越近代价越高。一般用指数函数或分段函数模拟，让无人机在安全距离之外就提前规避。
3. 高度代价：飞行高度不是随意的，太低容易撞地面障碍，太高油耗和暴露风险都增加。通常设计一个最优高度区间，高或者低都会增加惩罚。
4. 平滑代价：相邻两段路径的夹角变化不能太剧烈，这对应无人机的最小转弯半径约束。

function cost = computeCost(node, parent, threatList, params) % 路径长度代价 segLength = norm(node.pos - parent.pos); lengthCost = segLength * params.w_length; % 高度代价 heightCost = params.w_height * abs(node.pos(3) - params.optimalHeight); % 威胁代价 threatCost = 0; for i = 1:size(threatList, 1) distToThreat = norm(node.pos - threatList(i, 1:3)); if distToThreat < threatList(i, 4) threatCost = threatCost + params.w_threat * (1 / (distToThreat + eps)); end end % 平滑代价（计算夹角变化） vec1 = parent.pos - node.pos; vec2 = node.pos - node.nextPos; % 注意这里需要预估值 cosAngle = vec1' * vec2 / (norm(vec1) * norm(vec2) + eps); smoothCost = params.w_smooth * (1 - cosAngle); cost = lengthCost + heightCost + threatCost + smoothCost; end

上面这段代码里平滑代价的计算方式是简化的，实际用的时候需要知道下一节点的位置，所以一般是在扩展到下一节点之后再回头计算当前段。这提醒我们：代价函数和搜索算法的耦合度很高，设计的时候就要想清楚哪些信息在扩展时是已知的。

1.3 为什么启发式算法是当前的主流选择

三维路径规划可用的算法一大把：人工势场法、动态规划、A*、Dijkstra、遗传算法、粒子群、蚁群……为什么启发式类算法在无人机路径规划里最活跃？

核心原因是搜索效率和最优性的平衡。拿A*来说，它的启发式函数h(n)提供了对“从当前节点到终点还需多少代价”的估计，有了这个估计，搜索方向就会倾向终点，而不再像Dijkstra那样毫无方向地往外围扩散。在三维空间这种节点量级下，方向性带来的效率提升是数量级的差距。

而进化类算法（遗传、粒子群）的优势在于，它们把路径当作一个整体来优化，天然适合处理“全局最优”和“多约束耦合”问题。缺点是计算代价高、实时性差，这在动态避障中非常致命。

我最后采用的方案是：静态全局规划用改进的A，动态避障用局部重规划*。A*负责在障碍环境中找一条初始最优路径，动态障碍物出现时，只在局部做一个小范围的绕行搜索，而不是把整条路径推翻重来。这其实就是一种“全局规划+局部修正”的混合架构，在工程上最实用。

2. 动态避障的底层逻辑：不是每次都要重新规划

2.1 动态障碍物的感知模型与碰撞检测

很多人一听“动态避障”就以为要实时跑算法，其实动态避障的第一步是感知和检测，而不是规划。如果连障碍物在哪儿、往哪飞、速度多快都没搞清楚，再NB的规划算法也没意义。

在Matlab仿真里，动态障碍物的感知模型一般有两种做法：

1. 理想感知模型：认为无人机能实时获取所有动态障碍物的精确位置、速度和半径。这种模型简单，适合验证规划算法的正确性。
2. 有限视场模型：模拟机载传感器，无人机只能感知以自身为中心、一定半径范围内（比如50米）的障碍物。这种模型更接近真实。

我建议你做动态避障实验时，一定要用第二种模型。原因很简单：如果无人机能感知全局所有动态障碍物，那“动态避障”就退化成“时序上连续变化的静态规划”，毫无工程价值。真实飞行中，传感器就是有限的。

碰撞检测的数学描述其实不复杂：把无人机看成一个质点或一个半径为r的球体，动态障碍物也是一个球体，两个球心距离小于半径之和就判定碰撞。关键是要检测的是未来时刻的碰撞，而不是当前时刻的碰撞。这个“未来”从哪里来？从障碍物的速度矢量外推。

2.2 从TTC（碰撞时间）看局部绕行的触发时机

TTC，Time to Collision，碰撞时间，是动态避障里最重要的一个指标。它的定义是：按照当前相对速度和位移，距离碰撞还有多少秒。

function ttc = computeTTC(obsPos, obsVel, uavPos, uavVel, safeDist) relPos = obsPos - uavPos; relVel = obsVel - uavVel; a = relVel' * relVel; b = 2 * (relPos' * relVel); c = relPos' * relPos - safeDist^2; discriminant = b^2 - 4 * a * c; if discriminant < 0 || a < eps ttc = inf; % 不可能碰撞 else t1 = (-b - sqrt(discriminant)) / (2 * a); t2 = (-b + sqrt(discriminant)) / (2 * a); if t2 < 0 ttc = inf; % 已经远离 else ttc = max(t1, 0); % 最近碰撞时间 end end end

这个函数的几何意义其实是解一个一元二次方程：相对位移矢量在相对速度方向上的投影，什么时候会和安全圆相交。TTC越小，碰撞风险越大。那我一般设两个阈值：

• TTC < 8秒：进入预警状态，开始调用局部重规划。
• TTC < 3秒：进入紧急规避状态，放弃最优性，直接执行最安全的避让机动。

为什么是8秒和3秒？这取决于无人机的速度、机动能力和传感器的更新频率。速度8m/s的无人机，8秒预警相当于64米的安全距离，足够完成一次局部绕行规划；3秒紧急规避则是留给无法完整规划时的兜底缓冲。这个阈值没有唯一正确答案，但一定要跟无人机的速度量级相匹配，别用固定值。

2.3 全局重规划与局部路径调整的选择策略

这一段是我个人觉得整个动态避障流程里最值得琢磨的。

动态障碍物出现之后，你有两种选择：一是保留全局路径不变，只在受影响的局部路径段做一个绕行；二是在新的障碍信息下重新进行全局搜索。两种做法各有代价：

局部重新规划的优点是快、稳，计算量小，适合障碍物只是临时穿过航线的情况。缺点是如果障碍物长期挡在唯一通道上，局部绕行可能绕到死胡同，最后还要回到全局。

全局重新规划的优点是一劳永逸，在新信息下重新搜索一条完整路径。缺点是计算量大，而且如果环境变化频繁，无人机可能永远在“重新规划”，路径一直定不下来。

我采用的策略是一个基于距离的切换机制：

1. 检测到动态障碍物挡住当前路径段，先截取无人机前方一定距离（比如当前点到下一个航路点之间）作为局部搜索窗口。
2. 在这个窗口内用A*快速搜索一条绕行路径，如果搜索成功且绕行代价不超过全局路径的某个阈值（比如1.3倍），就接受局部绕行。
3. 如果局部搜索失败（比如障碍物堵死了窗口内所有通道），则触发全局重新规划。

这里有个小经验：全局重规划的冷却时间一定要设。否则动态障碍物A刚被避开、障碍物B出现在另一个方向，无人机反复触发全局规划，路径会一直跳，实际飞行中非常危险。我在Matlab里一般设一个5秒的冷却时钟，冷却时间内即使发现更优的全局路径也不切换。

3. Matlab代码框架拆解：核心模块与关键实现

3.1 地图初始化与威胁区域生成

先讲地图初始化。这一部分看似简单，却决定了后续所有算法的运行表现。我见过太多人直接把别人的地形数据load进来就开始跑A*，结果搜索半天找不到路径，其实是地图边界和障碍分布的问题。

我的代码里，地图初始化分三步：

第一步，生成三维栅格矩阵。用zeros初始化一个nx×ny×nz的数组，nz表示高度层。地形起伏可以通过一个二维的高度矩阵来表示，每个栅格的第三维索引如果在高度矩阵之下，就标记为障碍。

function map = initMap(mapSize, heightMap, resolution) map = zeros(ceil(mapSize / resolution)); for ix = 1:size(map, 1) for iy = 1:size(map, 2) h = heightMap(ix, iy); for iz = 1:ceil(h / resolution) map(ix, iy, iz) = 1; % 地形障碍 end end end end

第二步，加入静态威胁区。科研场景里最常见的威胁是大楼、禁飞区、雷达探测范围，统一抽象成球体或圆柱体。圆柱体适合楼房建筑，球体适合雷达站或者信号塔的探测半径。

第三步，设置动态障碍物的初始参数。每一个动态障碍物我单独用一个结构体数组记录：位置、速度、半径、运动轨迹（在仿真循环里更新）。

注意：Matlab的循环效率是出了名的慢。地图初始化可以用循环，但到了A*搜索这种核心逻辑，能向量化就向量化，否则100x100x50的栅格会跑到你怀疑人生。

3.2 改进A*算法的三维扩展与启发函数设计

A*从二维扩展到三维，核心改动只有两点：邻域节点的定义和启发函数的设计。

二维A*的邻域一般是8方向（上下左右加上4个对角）；三维的呢？是先26方向（3x3x3的全部邻居）。每一个邻居节点对应一次坐标偏移，用三层嵌套循环生成就行。注意对角线移动的代价要乘以√2或√3，很多初学者在这里丢掉精度，导致路径出现明显的锯齿形。

启发函数我推荐用对角距离的推广——切比雪夫距离。因为无人机在三维栅格里可以沿任意方向移动，曼哈顿距离会过多估计代价（导致次优），而欧氏距离在允许对角线移动的模型里又低估代价（导致搜索膨胀）。切比雪夫距离是dimension-wise的max，恰好匹配“可以走对角线”的移动模型：

h = max(abs(x - goalX), abs(y - goalY), abs(z - goalZ));

不过在实际飞行中，无人机有个重要约束是爬升率限制。水平方向可以快速移动，但竖直方向爬升或下降的速度是有限的。这意味着相邻两点的高度差不能太大，否则会产生动力学不可行路径。我的做法是在启发函数里加一个高度惩罚项：

h = max(abs(x - goalX), abs(y - goalY), abs(z - goalZ)) ... + penaltyFactor * abs(z - goalZ);

这样搜索会倾向于选择高度变化平缓的路径，避免那种忽上忽下的折线轨迹。

3.3 动态避障模块：检测-预测-绕行三步走

动态避障模块是整个代码的“核心中的核心”，我的实现遵循三步走策略。

第一步，检测。在无人机当前位置沿着当前路径段往前扫描，每个路径段取均匀分布的采样点，对每个采样点检查是否有动态障碍物在附近（距离小于安全半径）。这一步的目的不是精确碰撞，而是快速判断路径是否已经被“污染”。

第二步，预测。对检测到靠近的动态障碍物，用它的速度矢量外推未来位置，结合无人机的当前速度，调用前面写的computeTTC函数，求出碰撞时间。

第三步，绕行。如果TTC小于阈值，就进入局部重规划。局部重规划的做法是：以当前无人机位置为起点、以原路径前方某个未受影响的路点为终点，在一个“以原路径段为中心的管状邻域”内重新用A*搜索。管状邻域的半径大约取3~4个栅格，这样既能避开障碍物，又不会偏离原路径太远。

function [newPath, success] = localRepath(map, currentPos, targetPos, localRadius, dynamicObs) % 截取局部区域 minRange = floor(currentPos - localRadius); maxRange = ceil(currentPos + localRadius); subMap = map(minRange(1):maxRange(1), ... minRange(2):maxRange(2), ... minRange(3):maxRange(3)); % 将动态障碍物投影到局部地图，标记为障碍 for i = 1:length(dynamicObs) obsPosGrid = worldToGrid(dynamicObs(i).pos); if all(obsPosGrid >= minRange & obsPosGrid <= maxRange) localIdx = obsPosGrid - minRange + 1; subMap = markSphereOccupied(subMap, localIdx, dynamicObs(i).radius); end end % 在局部地图上用A*搜索 [newPath, success] = astar3D(subMap, ... worldToGrid(currentPos) - minRange + 1, ... worldToGrid(targetPos) - minRange + 1); % 坐标转换回全局 if success newPath = newPath + minRange - 1; newPath = gridToWorld(newPath); end end

这个局部重规划有一个很关键的优势：搜索空间小，速度快。一个20×20×10的局部栅格只需要搜索4000个节点，通常在几毫秒内就能出结果，足以满足实时性要求。

3.4 路径平滑与可视化

A*搜出来的路径是折线状的，无人机直接沿折线飞会频繁加减速，不仅费电，姿态还不稳。路径平滑是一个不可省略的后处理步骤。

我常用的平滑方法是三次B样条。B样条的好处是：生成的曲线穿过控制点附近，但不会剧烈摆动，而且局部修改只影响一段曲线，不会波及全局。在Matlab里可以用样条工具箱，也可以自己写基函数：

function smoothPath = bsplineSmooth(path, k) % path: n×3 的原始路径点 % k: 阶数，一般取3~4 n = size(path, 1); t = linspace(0, 1, n); % 原始参数 tt = linspace(0, 1, (n-1)*10); % 加密参数 smoothPath = zeros(length(tt), 3); for dim = 1:3 smoothPath(:, dim) = spline(t, path(:, dim), tt); end end

可视化方面，我一般用plot3绘制三维路径，用scatter3绘制障碍物，动态障碍物则在循环里用set函数更新位置。这样能直观看到无人机绕行动作是否正确。

4. 跑通代码才算开始：调参路线图与仿真里的坑

4.1 栅格分辨率和搜索代价的平衡

栅格分辨率是最影响整体体验的参数。设得太粗，路径会穿过实际障碍物边缘，看起来就像“贴着墙走”；设得太细，A*的搜索时间呈立方级增长，动态避障的实时性直线下降。

我在不同分辨率下跑了同一组实验，对比结果很直观：

我的建议是：仿真验证阶段用1~2米分辨率，跑大场景批量实验用5米分辨率。真实飞行的路径规划通常不直接依赖栅格——因为真实环境里障碍物坐标误差就远大于分辨率，高分辨率除了好看没有实际意义。

4.2 动态障碍物速度更新频率与重规划频率的匹配

这个坑我踩得很深刻。

最开始做动态避障仿真时，我把动态障碍物位置更新和A重规划放在同一个循环里，每一帧都更新障碍物、每一帧都重算路径。结果路径一直在抖——前一帧绕左边走，下一帧障碍物动了一点，A又觉得绕右边更优，路径左右横跳。这在无人机飞行中是完全不可接受的。

后来我查资料和反复试验，总结出一个经验：感知的更新频率和规划的更新频率应该是解耦的。感知频率要快，保证不漏检；规划频率要慢，保证路径稳定。

在Matlab里我这样实现：

• 主循环每0.1秒更新一次动态障碍物位置（10Hz感知频率）。
• 但只有每0.5秒才检查一次是否需要重新规划（2Hz规划频率）。
• 两次规划之间，无人机沿着上一条已经规划好的路径飞行。

这样做之后，路径抖动问题立刻消失了。道理也很简单：重规划周期太短，路径还没来得及飞完就又被推翻，必然会震荡。给规划一个“惯性期”，让当前路径先执行一部分，再决定是否调整，系统稳定性会好很多。

4.3 常见“路径穿墙”和“路径振荡”的排查思路

“穿墙”问题，指的是规划出来的路径明明避开了障碍物网格，但可视化时发现路径穿过了障碍物表面。这个问题最常见的原因有两个：

原因一：插值精度不够。A*搜索的路径点是栅格中心点，用plot3直线连接时，直线段可能穿过相邻的障碍物栅格。解决方法是把路径点加密，比如在相邻两个节点之间取10个插值点，逐段检查插值点是否落在障碍物栅格里。

原因二：碰撞检测使用的半径过小。无人机本身有翼展、有旋翼半径，如果只把无人机当成单个质点检测，路径跟障碍物表面的间距可能小于安全距离。解决办法是在判断碰撞时，把无人机的安全半径加到障碍物半径上。

“路径振荡”则是动态避障场景特有的。除了前面说的重规划频率过高，还有一个常见原因是启发函数不可采纳（inadmissible）。如果启发函数经常高估实际代价，A*就会在次优路径之间来回试探，不停切换路径。排查办法是：把启发函数改成简单的欧氏距离（一定低估），看振荡是否消失。如果是启发函数的问题，再逐步调整权重。

5. 从Demo到实战：如何把这份代码改成自己的任务

5.1 添加地形约束与飞行器动力学限制

很多Demo代码最大的不真实之处在于：地图是一块纯平的矩形空间，无人机可以任意高度飞行。但真实作业环境一定有地形起伏、禁飞区域，还有无人机本身的动力学限制。

至少要考虑三个限制：

1. 飞行高度下限：必须高于地形高度加上安全离地高度。在栅格地图里，这一点已经在初始化时处理了——地形以下的栅格全部标记为障碍。
2. 最大爬升/下降率：相邻路径点的高度差不能超过最大爬升率乘以两点之间的飞行时间。这个约束要在代价函数里加惩罚项，或者在搜索时限制邻居节点的Z轴偏移量。
3. 最大转弯角：受限于无人机的机动能力，路径相邻两段的夹角不能超过某个值，否则飞控跟不上。

我在代码里加动力学约束的方式很简单：在A*的扩展邻居节点时，用父节点到当前节点的方向作为基准，把与基准方向夹角超过最大转弯角的邻居节点直接剪枝。这样搜索出来的路径天然满足转弯约束。

5.2 与其它启发式算法的对比与替换思路

A不是唯一的启发式选择。实际项目中，我做过A、RRT*、遗传算法、粒子群四种算法的对比实验，结论可以给大家参考：

如果你的项目动态障碍物特别多，我建议你把A替换成RRT。RRT的增量重规划能力比A更自然——它不需要维护一个固定的栅格地图，直接在连续空间采样，动态障碍物出现时只要删除涉及该区域的边，重新生长即可。但代价是实现复杂度高不少，你需要仔细管理树的结构，Matlab代码量会翻倍。

5.3 部署到真实飞控前的最后一公里：坐标转换与实时代价

最后谈一下从Matlab仿真到真实系统的距离。

首要问题是坐标转换。栅格地图里的节点位置是“地图坐标”，真实飞行器用的是经纬度和高度（WGS84或UTM坐标）。你需要建立一个地图坐标系到地理坐标系的仿射变换——平移、旋转、缩放。这一步写起来不难，但最容易出错，建议单独写一个坐标转换模块并做好单测。

第二个问题是计算效率。Matlab的实时性远不够，真实飞控上一般用C++重新实现规划算法。好消息是，A*的核心逻辑是语言无关的，你在Matlab里调试好的启发函数、代价权重、避障触发阈值，可以直接翻译成C++代码。我一般会先写好一份不依赖Matlab工具箱的原生Matlab代码（只用基础语法），再把这份代码作为C++移植的“伪代码蓝本”。

第三个问题是传感器噪声。仿真里假设感知模型能精确测量障碍物位置和速度，真实传感器一定有误差。所以部署到真实飞控之前，建议先加一层高斯噪声到障碍物位置和速度里跑一遍仿真，看路径是否还能稳定收敛。这一步能提前暴露很多“仿真能跑、真机必炸”的问题。

6. 常见问题速查表

为了方便排查，我把动态避障+启发式搜索里最常见的几个问题和对应策略汇总成一张表：

7. 最后再分享一个小技巧

动态避障的重规划触发，我后来改成了一种“半拍错开”的策略：感知频率是20Hz，重规划频率固定是2Hz，但重规划时刻永远在感知时刻之后的20毫秒。也就是说，无人机每次重规划用的都是“上一次感知的数据”，而不是“最新感知的数据”。这样做的效果是：感知数据噪声不会被直接放大到规划结果里，路径稳定性反而更好。

如果你现在正被路径振荡困扰，不妨试试这个办法。有时候不是算法不行，而是系统的采样节拍设计得不太对。无人机路径规划是个综合工程，从建图、搜索、避障到平滑，每一步的耦合关系都要理清楚。希望这篇整理能帮你少走点弯路，也欢迎在调试中遇到更具体的问题来交流。