无人机三维路径规划这几年关注度一直很高但真正能把静态路径搜索和动态避障串到一起跑通的资料其实不多。很多同学下载了所谓的“完整代码”打开一看只有一条从起点到终点的静态航迹所谓动态避障不过是在固定威胁边上画了个圆。我最早做这个方向的时候也踩过类似的坑后来把整套逻辑拆开重写才真正搞清楚启发式算法在三维空间里应该怎么落地。这篇就把我实际调试过程中积累的东西整理一下重点是动态避障的触发机制、三维栅格地图的建模细节以及Matlab代码里那些容易让人头秃的坑。如果你正在做无人机航迹规划相关的课题或项目手里有一份基于启发式算法的三维路径规划代码但不知道怎么改成真正的动态避障或者想在仿真环境下复现完整的“全局规划局部避让”流程这篇文章应该能帮你省不少时间。1. 三维路径规划和二维到底差在哪先从问题定义说起1.1 三维环境的建模方式栅格法、概率路线图与采样式三维路径规划和二维最大的区别不在于多了一个Z轴坐标而在于搜索空间的性质发生了根本变化。二维平面里障碍物可以抽象成多边形路径是一条不会相交的折线三维空间里无人机要考虑的不只是水平面上的绕行还有高度层的选择——同样一个障碍物我可以从左边绕、从右边绕也可以从上面飞过去关键是看飞行代价哪个更小。环境建模方式直接影响后续所有算法的工作前提这里对比一下三种主流做法建模方式基本思路适合场景主要问题栅格法将空间离散化为等尺寸立方体标记占用/空闲城市低空、固定障碍分布明确的场景分辨率越高内存开销越大搜索节点爆炸概率路线图PRM随机采样生成路点连接近邻形成路网高维空间、障碍不规则的复杂地形采样点质量依赖随机分布可能漏掉窄通道采样搜索法RRT系列直接在连续空间采样扩展不做全局离散动态环境、高维约束路径质量粗糙需要后端平滑栅格法是目前Matlab仿真里最常见的选择。不是因为它最好而是因为它最直观你可以在三维数组上直接做索引障碍标记为1空闲标记为0A*算法的open list和closed list用三维节点坐标管理起来非常自然。但代价也很明显——一个100x100x50的栅格就是50万个节点如果启发式函数设计得不好搜索效率会非常难看。我在实际项目里一般会把地图分辨率从默认的1米改成2米或3米做降采样测试先把算法跑通再逐步提高精度。仿真阶段千万别一上来就用高分辨率栅格否则你一天的时间都会耗在等待搜索完成上。1.2 代价函数里藏着所有约束长度、威胁、高度和平滑路径规划的本质不是“找到一条路”而是在满足约束的前提下找到一条代价最低的路。代价函数是启发式算法的灵魂也是最容易被低估的部分。一套完整的三维路径代价函数至少应该包含四个分量路径长度代价这是最基础的越短越好用相邻节点的欧氏距离累加即可。威胁代价对地图里每个威胁区的中心点计算路径节点到中心的距离距离越近代价越高。一般用指数函数或分段函数模拟让无人机在安全距离之外就提前规避。高度代价飞行高度不是随意的太低容易撞地面障碍太高油耗和暴露风险都增加。通常设计一个最优高度区间高或者低都会增加惩罚。平滑代价相邻两段路径的夹角变化不能太剧烈这对应无人机的最小转弯半径约束。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仿真里动态障碍物的感知模型一般有两种做法理想感知模型认为无人机能实时获取所有动态障碍物的精确位置、速度和半径。这种模型简单适合验证规划算法的正确性。有限视场模型模拟机载传感器无人机只能感知以自身为中心、一定半径范围内比如50米的障碍物。这种模型更接近真实。我建议你做动态避障实验时一定要用第二种模型。原因很简单如果无人机能感知全局所有动态障碍物那“动态避障”就退化成“时序上连续变化的静态规划”毫无工程价值。真实飞行中传感器就是有限的。碰撞检测的数学描述其实不复杂把无人机看成一个质点或一个半径为r的球体动态障碍物也是一个球体两个球心距离小于半径之和就判定碰撞。关键是要检测的是未来时刻的碰撞而不是当前时刻的碰撞。这个“未来”从哪里来从障碍物的速度矢量外推。2.2 从TTC碰撞时间看局部绕行的触发时机TTCTime 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 全局重规划与局部路径调整的选择策略这一段是我个人觉得整个动态避障流程里最值得琢磨的。动态障碍物出现之后你有两种选择一是保留全局路径不变只在受影响的局部路径段做一个绕行二是在新的障碍信息下重新进行全局搜索。两种做法各有代价局部重新规划的优点是快、稳计算量小适合障碍物只是临时穿过航线的情况。缺点是如果障碍物长期挡在唯一通道上局部绕行可能绕到死胡同最后还要回到全局。全局重新规划的优点是一劳永逸在新信息下重新搜索一条完整路径。缺点是计算量大而且如果环境变化频繁无人机可能永远在“重新规划”路径一直定不下来。我采用的策略是一个基于距离的切换机制检测到动态障碍物挡住当前路径段先截取无人机前方一定距离比如当前点到下一个航路点之间作为局部搜索窗口。在这个窗口内用A*快速搜索一条绕行路径如果搜索成功且绕行代价不超过全局路径的某个阈值比如1.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*的搜索时间呈立方级增长动态避障的实时性直线下降。我在不同分辨率下跑了同一组实验对比结果很直观栅格尺寸地图尺寸节点数静态A*耗时路径长度路径质量1m100×100×50500,000约35秒最短平滑但计算爆炸2m50×50×2562,500约6秒略长质量可接受5m20×20×104,000约0.3秒明显变长折线明显需平滑我的建议是仿真验证阶段用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代码最大的不真实之处在于地图是一块纯平的矩形空间无人机可以任意高度飞行。但真实作业环境一定有地形起伏、禁飞区域还有无人机本身的动力学限制。至少要考虑三个限制飞行高度下限必须高于地形高度加上安全离地高度。在栅格地图里这一点已经在初始化时处理了——地形以下的栅格全部标记为障碍。最大爬升/下降率相邻路径点的高度差不能超过最大爬升率乘以两点之间的飞行时间。这个约束要在代价函数里加惩罚项或者在搜索时限制邻居节点的Z轴偏移量。最大转弯角受限于无人机的机动能力路径相邻两段的夹角不能超过某个值否则飞控跟不上。我在代码里加动力学约束的方式很简单在A*的扩展邻居节点时用父节点到当前节点的方向作为基准把与基准方向夹角超过最大转弯角的邻居节点直接剪枝。这样搜索出来的路径天然满足转弯约束。5.2 与其它启发式算法的对比与替换思路A不是唯一的启发式选择。实际项目中我做过A、RRT*、遗传算法、粒子群四种算法的对比实验结论可以给大家参考算法搜索速度路径质量动态避障适配度适用场景A*快有引导较好中等需要局部重规划栅格地图清晰的环境RRT*中等较好渐近最优高天然支持增量规划连续空间、复杂约束遗传算法慢好全局优化低不适合实时离线全局优化粒子群中等好容易陷入局部最优低离线全局优化如果你的项目动态障碍物特别多我建议你把A替换成RRT。RRT的增量重规划能力比A更自然——它不需要维护一个固定的栅格地图直接在连续空间采样动态障碍物出现时只要删除涉及该区域的边重新生长即可。但代价是实现复杂度高不少你需要仔细管理树的结构Matlab代码量会翻倍。5.3 部署到真实飞控前的最后一公里坐标转换与实时代价最后谈一下从Matlab仿真到真实系统的距离。首要问题是坐标转换。栅格地图里的节点位置是“地图坐标”真实飞行器用的是经纬度和高度WGS84或UTM坐标。你需要建立一个地图坐标系到地理坐标系的仿射变换——平移、旋转、缩放。这一步写起来不难但最容易出错建议单独写一个坐标转换模块并做好单测。第二个问题是计算效率。Matlab的实时性远不够真实飞控上一般用C重新实现规划算法。好消息是A*的核心逻辑是语言无关的你在Matlab里调试好的启发函数、代价权重、避障触发阈值可以直接翻译成C代码。我一般会先写好一份不依赖Matlab工具箱的原生Matlab代码只用基础语法再把这份代码作为C移植的“伪代码蓝本”。第三个问题是传感器噪声。仿真里假设感知模型能精确测量障碍物位置和速度真实传感器一定有误差。所以部署到真实飞控之前建议先加一层高斯噪声到障碍物位置和速度里跑一遍仿真看路径是否还能稳定收敛。这一步能提前暴露很多“仿真能跑、真机必炸”的问题。6. 常见问题速查表为了方便排查我把动态避障启发式搜索里最常见的几个问题和对应策略汇总成一张表现象可能原因检查/解决方向路径穿墙栅格分辨率太低提高分辨率或对路径做插值碰撞检测路径穿墙无人机安全半径没加碰撞检测时增大障碍物半径路径振荡重规划频率太高重规划周期延长到0.3~0.5秒路径振荡启发函数不可采纳改成欧氏距离验证再调整权重找不到路径地图中连通性差降低障碍物密度或增加路径搜索允许绕行范围找不到路径动态障碍物标记范围过大减小动态障碍物的安全距离膨胀系数路径过曲折平滑代价权重太低增大w_smooth或改用B样条平滑避障反应太慢TTC阈值设太小增大预警阈值提前进入重规划7. 最后再分享一个小技巧动态避障的重规划触发我后来改成了一种“半拍错开”的策略感知频率是20Hz重规划频率固定是2Hz但重规划时刻永远在感知时刻之后的20毫秒。也就是说无人机每次重规划用的都是“上一次感知的数据”而不是“最新感知的数据”。这样做的效果是感知数据噪声不会被直接放大到规划结果里路径稳定性反而更好。如果你现在正被路径振荡困扰不妨试试这个办法。有时候不是算法不行而是系统的采样节拍设计得不太对。无人机路径规划是个综合工程从建图、搜索、避障到平滑每一步的耦合关系都要理清楚。希望这篇整理能帮你少走点弯路也欢迎在调试中遇到更具体的问题来交流。