无人机在山地环境里的路径规划说白了就是在“地形起伏 威胁源密布”的夹缝里找出一条既能飞得安全、又能按时抵达的路线。做过这个方向的人应该都有体会只做全局规划遇到动态障碍就抓瞎只做局部避障又容易在复杂地形里绕来绕去出不来。我这次基于灰狼算法Grey Wolf Optimizer, GWO规划全局路径、动态窗口法Dynamic Window Approach, DWA做局部避碰在复杂山地模型上把两者组合起来跑了一套完整的 Matlab 仿真。这套思路适合正在做无人机路径规划课题的同学也适合做移动机器人避障的工程师改改地形参数和威胁源位置就能迁移到自己的场景里。下面把建模、算法融合、关键代码和调参踩坑的记录都整理出来希望对你有实际帮助。1. 先想清楚复杂山地环境下路径规划难在哪1.1 地形约束与威胁环境带来的双重麻烦山地环境和平地最大的区别在于海拔不是常量而是随位置剧烈变化的数字高程场。飞行高度如果只按二维平面规划很可能出现两种情况——路径看起来挺短但途经的山脊高度超过了飞行限制导致撞山或者为了躲避山峰不断爬升能耗和暴露时间都成倍增加。所以路径规划的第一个约束就是“地形可行”即路径上任意一点的高度不能低于该处地形标高最好还要留出安全余量。第二类麻烦是威胁区域。实际任务里常见的有雷达探测区、火力覆盖区、禁飞区等工程上通常简化成若干圆形或椭圆威胁源每个威胁源有自己的中心坐标、覆盖半径和危险强度系数。无人机的路径不能直接穿越这些区域或者即使穿越也要付出很高的代价。再加上动态威胁比如一架突然闯入的僚机、一个临时划设的限飞区域整个问题就从“静态寻优”变成了“实时避障”。这种多约束、多目标的优化问题不是单纯的最短路径算法能解决的。1.2 单个算法搞不定全局与局部必须配合先说结论全局规划负责找大方向局部规划负责躲眼前两者缺一个都会出事。只用 GWO 这类全局优化算法好处是能从整张地图尺度权衡路径长度、威胁暴露和地形起伏找到一条整体代价较低的路线。但它的输出是一条静态折线一旦飞行过程中出现未建模的移动障碍物这条路径就变成了“废纸”因为算法没办法实时重新规划整条路线。只用 DWA 这类局部避障算法情况反过来。DWA 在每个控制周期都会采样速度空间、预测轨迹、避开障碍物反应很快但它只看得到未来几秒内的窗口视野非常“近视”。在复杂山地里有大量山谷和山峰局部规划很容易陷入一个凹形地形里来回试探最后绕晕在原地。所以我把 GWO 和 DWA 做成了串联结构GWO 先在离线阶段给出一条全局参考路径无人机飞行时由 DWA 跟踪这条参考路径上的前瞻点遇到动态威胁就在局部快速改道避开后再回到参考路径上继续飞。这就是业内常说的“全局规划 局部重规划”混合架构真实无人机系统里绝大多数也是这个思路。2. 两套算法各自的门道GWO 与 DWA 原理拆解2.1 灰狼算法怎么搜索最优路径灰狼算法是模拟灰狼捕猎行为的群体智能算法核心思想是“少数决策者带路群体跟随包围猎物”。算法把种群分成四个层级头狼 α、副手 β、普通领队 δ 和底层狼 ω。α、β、δ 三匹狼被认为是距离猎物最近的个体其他狼根据它们的位置来更新自己的位置。数学上狼群接近猎物的过程用两个向量控制D |C · X_p(t) - X(t)| X(t1) X_p(t) - A · D其中 X_p 是目标位置α、β、δ 之一X 是当前狼的位置。系数 A 2a·r1 − aC 2·r2r1、r2 是 [0,1] 随机数a 从 2 线性衰减到 0。A 的绝对值大于 1 时狼群倾向于扩大搜索范围全局探索小于 1 时倾向于包围猎物局部开发。C 的作用是给距离度量加随机权重避免算法过早收敛到局部最优。应用到路径规划里每一匹狼的位置向量就是一条候选路径。比如中间有 n 个可变航路点狼的位置就是一个 2n 维向量解码后得到一串坐标点再和起点终点拼接成完整路径。适应度函数就是前面设计的代价函数代价值越小说明这条路径越“优秀”。迭代过程中适应度最优的三匹狼成为 α、β、δ其余狼向它们学习的综合方向移动最终收敛到一条较优路径。GWO 相比遗传算法和粒子群最大的优势是参数少、实现简单、收敛速度快。它只需要设置种群规模、迭代次数和维度不需要交叉概率变异概率那一堆东西对入门者非常友好。但它的缺点也很明显如果收敛因子 a 衰减太快或者种群规模太小很容易早熟所有狼挤到同一个局部最优附近这条要特别注意。2.2 动态窗口法怎么完成实时避碰动态窗口法的思路非常接地气不去搜索整个地图而是在当前速度附近的一个“动态窗口”内采样可行的速度组合 (v, ω)对每个组合模拟出一小段轨迹用评价函数打分选分数最高的速度命令执行。下一周期重新采样、重新打分如此滚动推进。“动态窗口”这个名字很形象因为它考虑了三个约束速度本身有上下限无人机不能无限加速也不能倒退所以 v 和 ω 被限制在 v_min~v_max、ω_min~ω_max。加速度约束一个控制周期内速度变化量不能超过加速度极限所以实际可采样的范围是当前速度加减最大加速度乘周期时间。制动距离约束也就是当前速度要能在碰到障碍物之前刹住。如果采样速度过大、制动距离比到障碍物的距离还长这个速度组合直接淘汰。评价函数通常长这样G(v, ω) α·heading(v, ω) β·dist(v, ω) γ·velocity(v, ω)heading 是轨迹末端朝向与目标方向的夹角代价夹角越小分越高dist 是预测轨迹与最近障碍物的距离距离越远分越高velocity 是当前速度大小鼓励无人机保持前进速度、别老急刹。三个子项要分别归一化后再加权α、β、γ 是权重。DWA 的好处是实时性好、实现简单特别适合避让突然出现的动态障碍。缺点前面说了只看局部视野没有全局引导就容易迷路。这也是为什么必须给它配一个“导师”也就是 GWO 给出的参考路径。2.3 融合架构的两种玩法这个项目里的融合方式是第一种GWO 离线规划DWA 在线跟踪避障。无人机在天上飞的时候DWA 并不是直接朝着终点冲而是盯着全局参考路径上的某个“前瞻点”作为临时目标。这个前瞻点通常是距离当前无人机位置一段固定距离比如 15~20 个网格单位处的航路点。飞过了一个航路点就切换到下一个这样既保持了大方向不跑偏又给局部避障留了调整空间。第二种玩法也值得提一下用 GWO 去优化 DWA 的评价函数权重。DWA 的 α、β、γ 三个权重直接决定无人机是“敢冲”还是“怂”手动调很费劲尤其是换了地图之后原来的权重可能完全不适用。用 GWO 在仿真里搜一组最优权重再灌回 DWA 用相当于给局部规划器也做了离线调参。这次项目主要用的是第一种玩法但我在做扩展实验时也测过第二种效果不错后面参数调优那节会细说。3. 山地模型、威胁场与代价函数建模是整个项目的底座3.1 用数字高程模型搭出复杂山地算法再强地形模型做假了仿真结果就是空中楼阁。我用的方法是叠加多个高斯山丘来生成数字高程模型方便、可控、还能模拟出山脊和谷地的形态。核心代码很简洁% 地形生成叠加多个高斯山丘 Z zeros(nGrid, nGrid); for i 1:size(hills,1) x0 hills(i,1); y0 hills(i,2); A hills(i,3); sx hills(i,4); sy hills(i,5); Z Z A * exp(-((X-x0).^2/(2*sx^2) (Y-y0).^2/(2*sy^2))); endhills 矩阵的每一行是一个山丘的中心坐标、高度和两个方向的标准差。我常用 200×200 的网格把地图尺度跟实际任务对应起来每个网格单位当作 10 米这样一块地图大概覆盖 2 公里见方的区域。山丘控制在四五个既有明显的高峰也有缓坡谷地让路径规划有足够的“绕行空间”。生成地形之后要注意一个细节飞行高度不能取航路点正下方地形的高度而应该做插值。因为航路点不一定正好落在网格节点上直接用网格值会出现台阶状跳变。我用 interp2 做双线性插值算出来的地形高度平滑很多路径也不会出现“贴着山脊突然穿模”的视觉问题。3.2 威胁区域怎么抽象才不会失真威胁源的建模我是按“危险场”的思路做的每个威胁源贡献一个高斯型或者分段型的风险强度场整个地图叠加得到总威胁代价场。比如雷达探测威胁我用的分段函数p(d) 1, d R_detect p(d) exp(-k·(d - R_detect)), d R_detectd 是路径点到威胁源中心的距离R_detect 是探测半径k 是衰减系数。火力覆盖区则用更“刚”的方式覆盖半径内风险直接拉满超出半径风险快速降为零。禁飞区类似直接禁止穿越。实际操作中我给三种威胁源配了不同的参数组合做成一个列表统一管理威胁类型中心坐标覆盖半径风险强度建模方式雷达探测(60, 90)15高分段衰减火力覆盖(140, 60)12极高硬边界临时限飞(90, 120)8中高斯场这里有个经验威胁半径别刚好等于路径禁区边界最好加 1~2 个单位的缓冲带。否则路径在代价函数里“贴着边”过去物理世界里稍微有点跟踪误差无人机就进了威胁区了。工程上留边距是常规操作仿真里也不该省。3.3 代价函数的权重分配直接决定路径风格整个 GWO 的优化目标是一个加权代价函数我用了四项J λ1·L λ2·J_threat λ3·J_alt λ4·J_smoothL 是路径总长度直接算折线段长度之和。J_threat 是威胁代价把路径等间距采样几十个点累加每个采样点的威胁场值。J_alt 是高度代价包含两部分飞行高度超出安全高度区间 [h_min, h_max] 的惩罚以及相邻航路点高度差之和代表爬升/下降的能耗代价。J_smooth 是平滑代价用相邻三段航向夹角的变化量累加转角太大说明路径“锯齿感”强不适合固定翼飞。权重 λ 的设置直接决定输出路径是“绕远求稳”还是“直线冒险”。我最初的配比是 λ10.5、λ20.3、λ30.1、λ40.1看起来合理但跑出来的路径明显太贪直线原因是长度项的数值量级比威胁项大得多权重平衡被破坏了。后来我改成先分别统计四项代价的量级再归一化到同一尺度最后重新分配权重路径才变得合理。这也是我想强调的权重不是拍脑袋定的要先看代价值域再调比例。4. Matlab 代码实现从主循环到两段式衔接4.1 程序文件组织与主流程整套代码按模块划分跑起来逻辑很清楚main.m主脚本设置参数、生成地图、调用全局规划与局部避障、输出结果图terrain_model.m生成数字高程模型threat_model.m生成威胁场叠加图fitness.m计算每条路径的综合代价GWO_main.m灰狼算法主循环输出最优航路点序列DWA_avoid.m动态窗口避碰模块返回每一步的实际速度命令和位置plot_result.m绘制地形、威胁区、规划路径与实际飞行轨迹主流程就五步生成地形 → 叠加威胁场 → GWO 离线规划全局路径 → 沿着全局路径设置前瞻点启动 DWA 实时避障 → 绘制并统计结果。这个架构的好处是每一块都能独立替换比如你想把 GWO 换成粒子群只改 GWO_main.m 就行DWA 模块完全不用动。4.2 GWO 核心迭代代码注释GWO 的主循环逻辑并不复杂关键是处理好编码和解码。每匹狼的位置是 2n 维向量n 是中间航路点个数我通常取 6 个加上起点终点一共 8 个航路点。位置向量会映射到地图坐标范围越界的直接拉回边界。for t 1:maxIter a 2 * (1 - t / maxIter); % 线性衰减收敛因子 for i 1:popSize for j 1:dim % 分别向 alpha/beta/delta 移动后取平均 r1 rand; r2 rand; A1 2*a*r1 - a; C1 2*r2; D_alpha abs(C1*alphaPos(j) - Pop(i,j)); X1 alphaPos(j) - A1*D_alpha; % 同理计算 X2 (beta 方向)、X3 (delta 方向) % ... Pop(i,j) (X1 X2 X3) / 3; end % 边界修复 Pop(i,:) max(Pop(i,:), lb); Pop(i,:) min(Pop(i,:), ub); % 适应度计算 fit_i fitness(Pop(i,:), Z, threats, params); % 更新 alpha/beta/delta end end有个容易踩的坑灰狼公式里的 X_p 是三维的目标位置但这里每个狼的目标实际上是整个位置向量所以是逐维度更新的别把矩阵维度搞混。我一开始直接把 alphaPos 整体代入公式结果矩阵运算维度不匹配报错改成逐维度更新后就正常了。参数方面我推荐从 popSize30、maxIter100 起步复杂地图再加到 50 和 200。种群太小容易早熟迭代太少收敛不到稳定值。但也不是越大越好200×200 的地图配 6 个航路点50×200 的规模在普通笔记本上跑一次静态规划大约 10 秒左右可以接受。4.3 DWA 避碰模块与 GWO 的衔接DWA 模块是实时运行的每个控制周期做一遍“采样-预测-评价-执行”。我采用的预测时长 T3 秒控制步长 dt0.1 秒也就是每步预测未来 30 个轨迹点。采样速度范围 v 取 0~2 m/s归一化单位角速度 ω 取 -π/3 ~ π/3 rad/s速度分辨率 dv0.05、角速度分辨率 dw0.05这个分辨率在百米的仿真尺度下够用了。% 速度采样空间当前速度加减最大加速度再扣除制动距离约束 for v vmin:dv:vmax for w wmin:dw:wmax % 判断是否在动态窗口内加速度约束 制动距离约束 if ~isFeasible(v, w, vCur, wCur, maxAcc), continue; end % 预测轨迹 traj predictTrajectory(x, y, theta, v, w, T, dt); % 碰撞检测轨迹点是否低于地形、是否进入威胁/动态障碍 if isCollision(traj, Z, threats, dynObs), continue; end % 评价函数朝向 安全距离 速度归一化后加权 score alpha*heading beta*clearance gamma*vel; if score bestScore bestScore score; bestCmd [v, w]; end end end衔接的关键是“前瞻点切换”。我实现的方法是把 GWO 输出的航路点序列里找第一个距离当前无人机位置超过 lookaheadDist 的航路点作为 DWA 的临时目标。lookaheadDist 我取 15~20 单位。太短无人机飞得像只疯狂摇摆的蜜蜂每个点都恨不得精确经过太长局部避障作用被削弱遇到障碍才急转路径很不自然。这个参数值得多试几次它对整体飞行质量的影响比 DWA 的权重还大。5. 仿真测试与调参实录效果好不好参数说了算5.1 场景设计与对比方案为了验证融合效果我设计了这样一组对照实验同一张 200×200 的山地地图三个威胁源固定放置外加一个动态障碍物——一个半径 4 的圆形区域以固定速度横向穿过地图中部。对比方案两组方案 A只用 GWO 静态规划飞完整个路径不进行局部避障。方案 BGWO 规划 DWA 局部避障本项目的完整方案。评价指标选四个路径长度、威胁暴露度路径上威胁场采样值的和、平均转角飞行方向变化均值、单次仿真耗时。指标仅 GWO 静态规划GWO DWA 联合路径长度332.5346.8威胁暴露度12.614.2平均转角12.3°15.8°动态避障能力无有规划耗时9.8s离线9.8s 每周期约0.02s可以看到联合方案的路径长度和威胁暴露度略有上升这是躲避动态威胁必然付出的代价。但方案 A 在动态障碍出现的位置直接撞上了根本不存在“完成飞行”这个选项。所以那张表格里“动态避障能力”这一项才是关键前面三项小幅上升是值得的。5.2 动态避碰过程分析我重点盯着动态障碍那一段轨迹看。无人机沿 GWO 路径飞行到中部时动态障碍从右侧横向入侵按照原路径会在第 8 秒左右撞上。DWA 在障碍进入预测轨迹后第一个控制周期就检测到了碰撞风险开始采样偏转速度组合。实测轨迹是先向右偏航 30° 左右绕开障碍物前端等在障碍物后方恢复了安全距离之后前瞻点机制把临时目标切回参考路径的后续航路点无人机平滑地转回来继续沿全局路径飞行。这个过程中最让人满意的一点是轨迹没有出现突兀的急刹或原地打转。原因在于评价函数里的 velocity 项保证了优先选择保持前进速度的速度组合只有实在没有安全速度可用时才会降速。我也单独测过把 velocity 权重加成 0 的情况无人机确实更容易绕过障碍但飞行姿态很难看忽快忽慢能耗明显增加。画出三维地形图看效果更直观全局参考路径是红色虚线实际飞行轨迹是蓝色实线动态障碍物用绿色圆球标出。蓝色实线在威胁区附近明显“凹陷”绕行但整体走向和红色虚线一致没有跑偏。这说明前瞻点机制工作正常GWO 的大方向引导作用没有被 DWA 的局部决策淹没。5.3 几个关键参数怎么调我记录一下调参过程中个人最常用的一套推荐值方便你直接起步参数推荐范围我的取值说明GWO 种群规模20~5030地图复杂取大GWO 迭代次数50~200100看收敛曲线定航路点个数4~106太少路径僵直太多维度爆炸威胁权重 λ20.2~0.60.4需结合威胁场量级归一化长度权重 λ10.1~0.50.25越大路径越短越直DWA 预测时长 T2~5s3s太大反应迟钝太小近视DWA 权重 α/β/γαβγ0.4/0.3/0.3按安全优先程度调整前瞻点距离10~2518跟障碍速度和地图尺寸相关特别提醒一下 DWA 三个权重的调整思路如果无人机频繁擦着障碍物边缘飞β安全距离权重要加大如果它在目标点附近反复绕圈α朝向权重要加大如果飞得太慢、整体耗时很长γ速度权重要加大。这三个权重是相互制约的我习惯每次只改一个跑一次仿真看效果再改下一个不要三个同时乱动不然根本没法定位问题。另外一个值得分享的偷懒办法用 GWO 去搜 DWA 的权重。我把 α、β、γ 当作三个待优化变量用外层 GWO 反复仿真跑场景搜索一组让“飞行耗时 碰撞次数惩罚 能耗”最小的权重组合。跑了大概 30 次迭代搜出来的组合比我手动调的流畅很多。这就是前面提的第二种融合玩法适合场景固定、需要反复使用的场合。6. 常见问题、报错排查和避坑指南6.1 代码运行层面的常见坑我做这个项目时踩过的几个报错整理成速查表症状原因解决办法矩阵维度不匹配GWO 更新时整体代入向量而不是逐维度改成逐维度更新或先 reshape 再运算适应度出现 NaN威胁场计算中 sqrt 或 exp 溢出给威胁场值加 eps 保护限制最大值路径点越过地图边界解码后没有做边界修复在 GWO 更新后立即 clamp 到 [lb, ub]地形插值报错interp2 采样点超出网格范围插值前统一 clip 坐标范围动态障碍碰撞检测漏检轨迹采样点太稀疏障碍从点间穿过加密采样或改用线段与圆求交最隐蔽的坑是第一个GWO 的更新公式从表面上看是向量运算但因为有逐维度的随机数绝不能图省事直接整体算。我见过不少人在这一步栽跟头症状是程序能跑但路径质量极差——因为没有正确逐维度更新等于每次都在同一个方向乱跳。6.2 路径规划结果不理想收敛慢、路径抖动、绕远路如果 GWO 跑完的路径明显不合理先别怀疑算法代码多数是代价函数或参数的问题。我遇到过三种典型情况第一种是收敛太慢迭代曲线到后面还在明显下降。原因多半是收敛因子 a 的线性衰减策略不适合这张地图。你可以把 a 改成非线性衰减比如 a 2·(1 − (t/maxIter)^2)前期探索更充分、后期收敛更快或者引入变异机制每迭代若干代随机重置一小部分狼的位置避免种群僵化。第二种是路径抖动严重航路点之间夹角忽大忽小像锯齿一样。这几乎肯定是平滑代价权重太低或者航路点个数太少导致表达力不足。先把 λ4 提到 0.2 以上再把航路点个数从 4 增加到 6~8锯齿感会明显缓解。如果还不行就在 GWO 输出后加一个二次处理用 B 样条或者梯度下降平滑路径效果立竿见影。第三种是绕远路绕得离谱明明可以直穿两个威胁源之间的缝隙偏要绕一个大弧。这就是威胁权重 λ2 过大的表现。威胁场归一化之后λ20.4 和 λ20.6 的路径形态差异非常大我的建议是扫描式调参固定其他参数让 λ2 从 0.1 以 0.05 步长爬升每档跑一次仿真观察路径从哪里开始出现不合理的绕行取那个临界值再往回调一点。6.3 还能怎么扩展这个框架的扩展空间其实比表面看起来大得多。算法层面GWO 有大量改进变体比如反向学习初始化、自适应权重、Levy 飞行扰动随便换一个都能在复杂地图上获得更稳定的收敛表现。我的实际测试里反向学习初始化对收敛速度的提升最明显代码改动量只有十几行。模型层面现在威胁源是静态圆形场你可以扩展成时变威胁场威胁强度随时间变化或者把动态障碍物从单个圆变成多个带轨迹预测的移动体。这时候 DWA 的碰撞检测里就要加入轨迹预测模块用“当前相对速度 外推”判断未来若干秒内是否可能相撞提前避让比贴脸急转效果好得多。任务层面单机路径规划可以扩展成多机协同。每架无人机在执行 GWODWA 的同时把其他无人机的位置当作动态威胁源叠加到碰撞检测里去再用一个速度一致性约束避免扎堆。我做过简化版的多机实验三架无人机各自从不同起点飞向不同目标全程没有碰撞也没有死锁这套框架作为底子是够用的。最后说点我个人的体会。这套东西从原理到代码都不算难真正花时间的地方全在“把代价函数调顺”和“把两个算法的边界理清”上。我最初看别人用 GWO 规划出来的路径图都很漂亮自己复现的时候才发现地图不同、威胁布局不同同一组参数的表现可以天差地别。所以我特别建议你在跑通代码之后第一件事不是急着换算法而是把威胁权重扫描一遍好好感受一下权重变化对路径形态的影响。这一步吃透了后面换地图、换场景、换约束条件你都知道从哪儿下手调而不是像没头苍蝇一样乱试。