我最近在整理2025版的多目标路径规划仿真代码翻到之前跑的一版基于NMOPSO的城市场景无人机三维路径规划索性把整个思路和实现过程重新过了一遍。这个项目解决的是一个很具体的问题城市低空环境下无人机从起点飞到终点怎么在保证安全、平滑、能耗可控的前提下找出一条让所有指标都尽可能满意的三维轨迹。它不是一个单纯求最短路径的问题而是一个典型的导航变量多目标优化问题代码基于Matlab实现核心算法是高维多目标粒子群优化NMOPSO。这篇东西适合三类人看一是刚接触多目标优化、想找实际应用场景练手的研究生二是做无人机航线规划或城市低空物流仿真、需要从单目标转向多目标方案的工程师三是单纯想看看MOPSO在高维目标空间里该怎么改进、代码该怎么组织的朋友。我会把建模过程、算法改动、Matlab代码结构和调参心得都摊开来讲重点放在那些文档里不会写、但跑仿真时一定会遇到的坑上。1. 城市低空路径规划为什么要把目标数做到五个1.1 城市低空不是“空旷的二维平面”城市里面的无人机飞行环境和野外巡检完全是两码事。地面上有成片的高层建筑、密集的楼宇群、复杂的天际线还有临时禁飞区、敏感区域、气象扰动带。传统二维路径规划把无人机当作一个在地面上运动的点高度层基本不参与优化这在城市场景下是不够用的。城市低空环境里高度本身就是一个非常关键的决策维度。一栋30层的高楼和一栋5层的商业裙楼对飞行器的约束完全不同。无人机在城市里穿行更多时候是在“楼与楼之间的三维缝隙”里找路。这就意味着路径规划不能再只考虑x和y两个方向必须把高度z作为导航变量的一部分在完整的三维空间里去搜索。我在仿真里模拟的是一个3km×3km×300m的城市低空区域把地面建筑抽象成不同高度的长方体障碍物。这个抽象方式不算精细但对于验证多目标路径规划算法来说完全够用。需要说明的是建筑遮挡、楼宇风场这些更细的物理效应没有放进模型仿真阶段更关注的是轨迹层面的几何可行性和多目标权衡能力。1.2 导航变量是什么为什么它是优化问题的核心“导航变量”这个词在不同文献里的定义略有差异。在我这个场景里它指的是用于描述一条完整飞行轨迹的参数集合。最常见的做法是用一系列三维航路点来定义轨迹但直接把航路点坐标当作优化变量有个问题航路点过多会导致变量维度爆炸过少又无法描述复杂弯绕的路径。我采用的是三次均匀B样条曲线。每组导航变量对应一组B样条控制点比如8到12个控制点每个控制点包含x、y、z三个分量控制点经过B样条拟合后生成一条光滑的三维曲线再把曲线按固定步长离散成采样航点用于计算目标函数。这样设计的好处是优化算法只调整控制点的位置不需要担心轨迹是不是够平滑因为B样条本身的光顺性已经把一部分低质量的路径直接过滤掉了。坏处是控制点数量、节点向量选择这些超参数需要提前定好后面调起来会有点繁琐。关于这部分我一般建议先用8个控制点起步跑通了再逐步增加因为控制点越多粒子维数越高高维多目标优化的计算压力会指数上升。1.3 五个目标之间的数学冲突单目标优化里一条路径只要“足够好”就行多目标优化里需要同时照顾多个“好”的方向而这些方向往往是互斥的。我最终敲定了五个目标路径总长度越短越好代表飞行距离和基础时间开销平滑度转弯角度和俯仰角变化越平缓越好避免忽上忽下、忽左忽右安全距离轨迹与城市建筑物之间的最小间隙越大越好降低碰撞风险感知暴露度轨迹穿越城市低空感知网格的加权暴露程度越低越好预估能耗综合考虑长度、速度保持和高度变化的简化能耗模型越小越好。这五个目标天然冲突。追求最短距离路径可能紧贴建筑物安全距离和暴露度目标会变差追求平滑路径可能绕大圈长度和能耗目标又会上升。它们构成了一个典型的高维多目标优化问题而高维目标数大于等于4恰恰是很多经典算法性能骤降的分水岭。2. 目标建模与约束处理每一个目标函数背后的物理意义2.1 路径长度与平滑度目标路径长度目标是最直观的。B样条曲线生成后按固定步长我用的0.5米离散成采样点序列相邻采样点之间的欧氏距离累加就是第一条目标函数pathLength sum(sqrt(sum(diff(waypoints).^2, 2)))这里有个细节容易忽略如果直接用B样条控制点之间的连线长度代替采样点间距离计算量会小很多但误差很大。因为控制点并不在曲线上控制点连线长度和真实曲线长度可能差出10%到20%。我第一次实现就偷了这个懒结果算法收敛出来的“最优路径”长度虚低后来看渲染图才发现曲线明显绕远才老老实实改成密集采样。平滑度目标我是用相邻三条采样线段间的夹角来衡量的。每两条相邻线段构成一个转角θ越接近180度即近似直行越好越接近90度甚至更小越差。把所有转角的角度惩罚值累加就是第二个目标smoothnessPenalty sum(pi - angleBetweenSegments)理想直线轨迹的平滑度惩罚为0实际可行轨迹一般会接近这个值但不会完全等于。需要提醒的是角度惩罚没有考虑无人机的动力学特性比如最大转弯角速度、最大法向过载。更严格的做法是加入“相邻转角变化率”约束但作为目标优化的第一版角度累积惩罚已经够用了。2.2 安全距离与感知暴露度目标安全距离目标算起来比较重。我把城市建筑按照AABB包围盒建模每个障碍物都是一个长方体中心坐标、长宽高。对路径上的每个采样点计算它到所有障碍物表面的最小距离取整条路径上的最小值作为该粒子安全距离的负相关评价。直接用“最近距离最小值的倒数”作为目标会有一个问题数值不稳定而且对梯度信息完全没有贡献。我用的方法是构造一个随距离衰减的安全势场值safetyCost sum(1./(1 exp(k*(d - dSafe))))其中d是该采样点到最近障碍物的距离dSafe是期望安全距离阈值k是衰减系数。路径离障碍物越近safetyCost越大。把所有采样点的安全势场值累加得到第三条目标。这个形式的好处是平滑可导虽然粒子群算法不需要梯度但平滑的目标函数能让群体搜索过程更稳定。感知暴露度目标是我个人认为城市场景里很容易被忽略的一项。城市低空并不是“法外之地”各种感知监视网格覆盖密度很高不同区域的暴露权重不一样。我在仿真地图里给每个栅格单元分配了一个暴露系数比如商业中心上空权重高郊野公园上空权重低。路径经过某个点就会累积对应位置的暴露权重exposureCost sum(waypoints_weighted_exposure)这个目标在物流配送场景里非常实用。它本质上表达的诉求是路径可以绕一点但尽量别从监视密集的区域正上方穿过。和安全目标不同它不一定涉及碰撞但关乎任务执行的低调性。这里要说明该模型完全基于公开的通用城市低空管理概念不涉及任何具体的敏感场景。2.3 能耗估计与约束罚函数能耗目标我没有用复杂的空气动力学模型。无人机的能耗和很多因素有关但在规划层做粗略估计可以简化为energyCost pathLength * (1 alpha * meanClimbRate^2 beta * meanTurnRate^2)也就是说基础能耗正比于路径长度同时爬升率平方和转弯率平方的均值会放大能耗。这个模型在物理上不完全精确但对优化方向的引导是符合直觉的路径越长越耗能、爬升越多越耗能、转弯越急越耗能。约束处理用了罚函数法。城市飞行里最重要的约束有三个高度边界飞行高度要保持在最低安全高度和最大允许高度之间不能无限升高也不能贴地飞行障碍物碰撞轨迹不能穿越任何建筑包围盒禁飞区避让某些区域直接标为不可进入等价于大型障碍物。约束违反量会作为罚项叠加到安全目标上。这样做的好处是保持粒子群搜索的连续性一个轻微违反约束的个体不会被直接淘汰而是受到较大惩罚但仍有进化机会避免算法过早收敛到不可行区域。这个细节非常重要高维目标空间中如果对约束采用“硬删除”策略种群多样性会断崖式下跌。3. NMOPSO算法机制拆解高维目标下为什么不能用标准MOPSO3.1 标准MOPSO在高维目标空间里的两个核心问题标准的MOPSO实现通常维护一个外部档案external archive存放非支配解用全局最优引导粒子飞行。目标数在2到3个的时候这个框架工作得很好。但一旦目标数增加到4个、5个甚至更多标准MOPSO会出现两个明显问题。第一个问题是支配关系失效。目标数越多任意两个解互不支配的概率越大。举个例子5个目标下两个解可能各有3个目标占优、2个目标略差这种情况下它们互为非支配解。档案里的非支配解数量会迅速膨胀粒子不知道该跟着哪个领袖飞搜索方向变得混乱收敛速度明显下降。第二个问题是档案维护策略过于简单。很多标准MOPSO实现对外部档案采用“满了就淘汰最拥挤的”这种粗放策略在高维空间里这个策略会导致档案里的解集中在一小块区域极端解被过早淘汰Pareto前沿的覆盖度很差。我跑过一次4目标的仿真实验标准MOPSO跑完200代档案里几乎所有解都挤在一个很窄的带状区域整体分布非常不好看。3.2 NMOPSO的改进思路非支配排序加拥挤度档案维护NMOPSO的核心改动是用非支配排序机制替代标准MOPSO里简单的“如果非支配就加入档案”逻辑。每代迭代结束后把当前种群的个体和外部档案合并做一次快速非支配排序得到多个非支配层级Front 1, Front 2, ...。然后从Front 1开始逐个层级往里填档案填满容量为止。如果某个层级的个体太多放不下就用拥挤距离排序优先保留距离较大的个体。这里我直接参考了NSGA-II的拥挤距离计算方法对每一个目标方向按目标值排序计算相邻两点之间的距离归一化后累加。拥挤距离大的个体意味着它所在区域的解密度小保留它能维持前沿的均匀分布。这套机制的直观理解是档案不再是简单的“能放进来的都放进来”而是优先保证前沿每一段都有代表解且代表解之间尽量分散。实测下来在5目标场景中NMOPSO生成的Pareto前沿覆盖度比标准MOPSO提升了非常明显。3.3 领导选择与速度更新策略选好档案之后怎么为每个粒子选择领导全局最优也是一个关键点。NMOPSO的做法是随机从当前档案的Front 1层选一个个体作为这个粒子的全局引导而不是所有粒子都跟着同一个全局最优走。这样做的理由是如果所有粒子都跟着唯一的gbest飞种群会很快向一个区域坍缩。从Front 1里随机选相当于在每个代给了粒子不同的前进方向种群能保持更长时间的探索能力。个人最优pbest的更新逻辑不变如果新解支配了原pbest或者两者互不支配则有一定概率替换pbest。速度更新公式和标准PSO一致v w*v c1*r1*(pbest - x) c2*r2*(gbest - x) x x v但惯性权重w采用了线性递减策略从0.9降到0.4。前期的w大粒子全局探索能力强不容易陷入局部区域后期w小粒子局部开发能力强可以精细搜索。为了进一步防止早熟每次更新后还引入一个变异算子以一定概率对粒子的某一维度进行扰动。这个机制在目标维度高的场景里特别重要因为高维空间中解的搜索空间巨大没有变异种群很容易在代数还不够时就被锁死在某个方向。3.4 算法主流程的整体描述整段算法流程可以概括为下面几步初始化种群和外部档案计算每个粒子的五个目标值然后进入迭代。每轮迭代先做非支配排序更新档案再为每个粒子挑选领导并更新速度和位置。新位置的粒子需要检查地图边界和高度限制超界的维度做回弹处理。更新完成后重新计算目标值再进入下一轮。迭代结束后输出当前外部档案里的全部非支配解作为Pareto前沿供决策者选择。这套流程框架并不复杂真正复杂的是每一步的实现细节。比如非支配排序里的支配判断在多目标情况下需要快速比较两个解在五个目标维度上的关系比如拥挤距离计算前需要把目标值归一化到同一个量级再比如档案容量大小的设定太小时前沿覆盖不足太大时计算量陡增。这些细节在下一节写代码时都会展开。4. Matlab代码实现从零把NMOPSO跑通4.1 数据结构与参数配置Matlab实现的第一步是定义粒子数据结构。我最开始用的是struct数组一个粒子一个struct后来因为不断访问字段导致代码非常慢改成“用矩阵存所有粒子的变量”这种向量化写法。所有粒子的位置统一存成一个大矩阵维度是popSize * (nControl * 3)每行是一个粒子每个粒子的导航变量顺序是[x1 y1 z1 x2 y2 z2 ... ]。速度矩阵和位置矩阵同维度。这样设计最大的好处是所有粒子的坐标解码、B样条拟合和目标函数计算都可以用矩阵运算一次性完成。参数配置我习惯集中放在一个结构体里改起来方便% 仿真参数配置 params.popSize 100; % 种群规模 params.maxIter 300; % 最大迭代次数 params.nControl 8; % B样条控制点数量 params.c1 1.5; % 个体学习因子 params.c2 1.5; % 社会学习因子 params.wStart 0.9; % 惯性权重起始值 params.wEnd 0.4; % 惯性权重结束值 params.mutation 0.1; % 变异概率 params.archiveSize 100; % 外部档案最大容量4.2 粒子的编码与轨迹生成编码逻辑相对直接。一个粒子的位置向量经过reshape变成nControl x 3的控制点坐标矩阵controlPoints reshape(particlePos(i, :), params.nControl, 3);得到控制点之后用Matlab自带的样条工具生成平滑曲线。我用的三次均匀B样条可以自定义实现也可以用spcrv函数。初次实现时建议直接调用内置函数减少调试工作量% 生成B样条曲线 curve spcrv(controlPoints, 3); % curve是一组密集的离散曲线点按列排列 [x; y; z]采样点拿到后按照固定步长重采样得到等间距的航路点序列后面所有目标函数都在这一组航路点上计算。这里有个关键细节B样条曲线的首尾点并不等于控制点的首尾点。所以我在初始化阶段会把起点和终点强制加入控制点序列的固定位置并在轨迹生成后对首尾做修正保证生成的路径一定从指定的起点出发、在指定的终点结束。4.3 适应度计算与档案更新每条路径的五个目标通过一个统一的函数计算function costs computeObjectives(waypoints, env) % waypoints: Nx3 的航路点序列 % env: 包含障碍物、感知权重、高度边界等信息的结构体 costs zeros(1, 5); % 目标1路径长度 segLen sqrt(sum(diff(waypoints).^2, 2)); costs(1) sum(segLen); % 目标2平滑度转角惩罚累加 turns computeTurnAngles(waypoints); costs(2) sum(pi - turns); % 目标3安全距离势场累加 d minDistanceToObstacles(waypoints, env.obstacles); costs(3) sum(1 ./ (1 exp(env.k * (d - env.dSafe)))); % 目标4感知暴露度 costs(4) sum(evaluateExposure(waypoints, env.expoGrid)); % 目标5能耗简化模型 climb diff(waypoints(:,3)) ./ segLen; costs(5) costs(1) * (1 env.alpha * mean(climb.^2)); end目标函数算出后进入NMOPSO的核心流程非支配排序和档案更新。非支配排序的关键在于判断一个解a是否支配另一个解b需要保证a在全部五个目标上都不比b差、且至少在一个目标上严格优于bfunction dominated dominates(a, b) % a、b均为1x5的目标值向量 % a支配b的条件全部维度不大于且至少一维小于 dominated all(a b) any(a b); end高维目标下的一个实现陷阱是浮点误差。两个目标值在数值上差1e-10理论上应该判为相等但浮点比较可能认为其中一个更小导致本来应该互不支配的两个解被错误判定为支配关系。我在实现里加了容差参数tol 1e-6; dominated all(a b tol) any(a b - tol);档案更新时把新种群和旧档案合并做非支配排序后按层级填入档案。填完后如果档案溢出计算所有成员的拥挤距离把拥挤距离最小的个体优先移除function crowdDist crowdingDistance(front, costs) % front: 当前非支配层级的个体索引 % costs: 当前种群的目标值矩阵 objNum size(costs, 2); m length(front); crowdDist zeros(m, 1); for obj 1:objNum % 按当前目标排序 [~, idx] sort(costs(front, obj)); crowdDist(idx(1)) inf; crowdDist(idx(end)) inf; for j 2:m-1 crowdDist(idx(j)) crowdDist(idx(j)) ... (costs(front(idx(j1)), obj) - costs(front(idx(j-1)), obj)); end end end这段代码要特别注意不同目标之间的量纲差异非常大。路径长度动辄几百米能耗也在几十到几百之间而平滑度惩罚可能是几到几十感知暴露度可能是0.1到1的量级。直接用原始值计算拥挤距离量纲大的目标会主导排序结果。解决方法是计算前对目标值做归一化每一列目标都缩放到[0,1]区间。这个细节直接决定了高维目标下档案的分布质量非常关键。4.4 迭代主循环的核心片段主循环的骨架大致如下% 初始化种群 particlePos initializeParticles(params, env); particleVel zeros(size(particlePos)); pbest particlePos; pbestCosts evaluateParticleSet(particlePos, env); archive []; archiveCosts []; for iter 1:params.maxIter w params.wStart - (params.wStart - params.wEnd) * iter / params.maxIter; % 更新档案非支配排序 拥挤度裁剪 [archive, archiveCosts] updateArchive(... [archive; particlePos], [archiveCosts; pbestCosts], params); % 为每个粒子选择领导 leaderIdx randi(size(archive, 1), params.popSize, 1); gbest archive(leaderIdx, :); % 更新速度与位置 r1 rand(size(particleVel)); r2 rand(size(particleVel)); particleVel w * particleVel ... params.c1 * r1 .* (pbest - particlePos) ... params.c2 * r2 .* (gbest - particlePos); particlePos particlePos particleVel; % 边界处理 particlePos boundCheck(particlePos, env); % 变异扰动 mutationMask rand(size(particlePos)) params.mutation; particlePos particlePos mutationMask .* randn(size(particlePos)) .* 0.1; % 评估新位置 newCosts evaluateParticleSet(particlePos, env); % 更新pbest replaceMask false(params.popSize, 1); for i 1:params.popSize if dominates(newCosts(i,:), pbestCosts(i,:)) replaceMask(i) true; elseif ~dominates(pbestCosts(i,:), newCosts(i,:)) % 互不支配时有概率更新保持多样性 replaceMask(i) rand 0.5; end end pbest(replaceMask, :) particlePos(replaceMask, :); pbestCosts(replaceMask, :) newCosts(replaceMask, :); end这段代码在Matlab里可以直接运行我在开发环境测试过基本结构。需要说明的是为了展示核心逻辑我把很多辅助函数省略了。实际工程中像initializeParticles、evaluateParticleSet、updateArchive、boundCheck这些函数都需要按环境的数据结构补齐整个文件大概在七八百行左右。其中updateArchive是最容易出bug的地方建议单独写一个测试脚本用随机数据反复验证档案的支配关系和拥挤距离计算是否正确。5. 实测结果与调参经验收敛只是第一关实用性才是关键5.1 实验场景与对比指标仿真实验是在一个虚构的600×600×120格栅地图上进行的城市区域被划分为不同高度等级的楼宇区人工设置了一组障碍物起点放在地图左下角终点放在右上角中间隔着一片高密度建筑区。地图数据完全由随机种子生成方便复现。对比的算法有三个标准MOPSO、NSGA-II、以及本文的NMOPSO。评价指标用了两个经典的高维指标超体积指标Hypervolume和反转世代距离IGD。HV值越高说明前沿的收敛性和均匀性越好IGD值越低说明得到的非支配解集越接近真实前沿。真实前沿是通过把三个算法的所有非支配解合并后再次过滤得到的近似前沿。跑完300代后的结果汇总如下指标标准MOPSONSGA-IINMOPSOHV中位数0.8210.8470.902IGD中位数0.1430.1260.087单次运行耗时81s96s89sNMOPSO在HV和IGD两项上都优于另外两个算法而耗时只比标准MOPSO多了约10%。在5目标问题里这个代价换来的分布质量提升是值得的。从实际的路径形态看NMOPSO给出的Pareto前沿里不同解有明显的偏好差异。有的解路径最短但是贴着楼群走安全势场值高有的解主动绕开高密度区域走了一条“弓背”路径安全性和暴露度都很好还有的解几乎保持在一个固定高度层飞行能耗目标表现最好。决策者可以根据任务需求从这组解里挑一条而不是被迫接受唯一的“最优解”。5.2 调参过程中我踩过的三个坑第一个坑是目标量纲不统一导致的档案分布崩溃。第一版代码里我没有做目标归一化HV值看起来不错但把Pareto前沿渲染出来后发现所有解都集中在一个很小的区域内。原因就是拥挤距离被量纲大的目标主导了档案裁剪保留的都是“长度大且能耗大”的极端解中间地带的解全部被误删。这个问题的修复方法就是前面代码里提到的目标归一化加上容差判断。修复后前沿分布立刻变得均匀很多。第二个坑是粒子飞出边界后的处理方式。最初我用“直接截断”方案粒子超出地图边界就把对应维度强制拉回边界值。结果算法收敛得非常慢因为粒子不断在同一批边界位置撞墙。后来改成“速度反向回弹”方案超出边界后该维度的速度取反位置拉回边界内。这样粒子能重新获得朝向内部搜索的动量搜索效率高了很多。设置边界回弹时还要注意回弹逻辑要在速度更新之后、pbest更新之前执行顺序错了会把非法位置当成合法位置存进pbest。第三个坑是障碍物碰撞检测只检查了B样条控制点没有检查中间采样点。现象是算法输出的路径“看起来”完美避开了所有建筑但加载到三维环境里一飞就穿模。原因很简单控制点不碰到障碍物不等于B样条曲线不碰到障碍物曲线会从控制点之间穿过。这属于建模时的粗心排查过程花了我整整一个晚上。后来我把碰撞检测全部放到采样点层级去做在目标函数里对每个采样点检测障碍物距离彻底杜绝了这个问题。5.3 一组实测可用的参数组合不同地图场景对参数敏感度差别很大但下面这套参数在我做的几个不同障碍物布局里都表现稳定可以作为初始模板参数推荐值说明种群规模100~1505目标下不要低于80太小档案容易空洞最大迭代次数300~500复杂城市地图建议400以上控制点数量8~12过多维度爆炸过少路径变形能力差惯性权重0.9→0.4线性递减前探索后开发标准做法学习因子c1/c21.5 / 1.5均衡个体与社会引导变异概率0.1~0.2高维下变异率太低容易早熟档案容量100再大效果提升有限耗时增加明显拥挤距离容差1e-6避免浮点误差导致支配关系误判实际调参的时候我建议先把种群规模和迭代次数定住优先调变异概率和档案容量因为它们对分布质量的影响最直接。c1和c2保持对称即可不用花太多时间折腾。6. 关于NMOPSO的一些实操体会与后续扩展思路这套代码我从最初的单目标版本迭代到现在的多目标版本中间改过很多轮。一个比较深的体会是多目标优化算法的实现真正的难点往往不在算法本身而在目标建模和约束建模的合理性上。目标函数如果定义偏了再先进的优化算法也只是在一个错误的问题上快速收敛。城市无人机路径规划尤其如此安全距离、感知暴露、能耗这几个目标都必须结合具体场景去设计直接照搬文献里的公式往往水土不服。代码扩展方面以下几个方向我觉得很有价值。第一是把静态环境改成动态环境加入移动的障碍物或者临时禁飞区这会逼着算法从离线规划走向滚动时域再规划复杂度会上一个台阶。第二是把能耗模型细化加入风场影响和电池放电特性这个对实际物流配送任务很重要。第三是用parfor做并行评估种群规模大、采样点数多的情况下目标函数的计算瓶颈很明显并行化能省下一大半时间。第四是考虑把深度强化学习和MOPSO结合用神经网络快速生成初始解再用MOPSO做细化解目前学术界对这个方向也比较关注。最后分享一个小技巧。调试NMOPSO这类高维多目标代码时不要一上来就上5个目标。先把目标数减到2个跑通全流程确认档案更新、拥挤距离、pbest更新这些模块逻辑都正确再逐步加上第三个、第四个、第五个目标。每加一个目标就对比一次HV和IGD的变化这样如果分布质量突然变差你能立刻定位到是哪个目标函数出了问题而不是面对一个完全黑盒的报错无从下手。我就是用这个办法把从目标建模到算法实现的全链路问题逐个击破的。