改进粒子群算法实现多无人机协同航迹规划的Matlab实践

多无人机协同航迹规划拆开看是“航迹规划”合起来难就难在“协同”两个字。单架无人机用A*、RRT或者标准粒子群都能跑出路径但是一旦要求多架无人机同时出发、同时到达、互不碰撞、还要整体代价最小问题就变成了一个多目标、强约束的优化问题。我最近在Matlab里把改进粒子群算法完整实现了一遍支持三维场景下的多机协同航迹搜索这里把建模思路、改进细节、关键代码片段和调试中踩过的坑都整理出来给正在做毕业设计、课题仿真或者工程项目选型的同学一个可以直接参考的落地版本。1. 问题建模与整体思路1.1 多无人机协同航迹规划到底在解决什么问题先把这个问题的数学面貌说清楚。我们面对的不是“求出一条从A到B的路径”而是“同时求出K架无人机从各自起点到各自目标点的K条路径”并且这K条路径要满足三层约束第一层是环境约束比如绕过威胁区、保持在地图边界内、不能撞山体第二层是飞行性能约束比如转弯角不能超过机体限制、爬升角不能过大、飞行高度有上下限第三层是协同约束这是多机问题独有的包括任意时刻两机距离大于安全间隔以及各机到达目标点的时间差尽量小。用一个通用模型描述的话假设第i架无人机的航迹用m个路径点表示P_i {p_i,1, p_i,2, ..., p_i,m}那么优化目标可以写成J Σ_i ( λ1 * L_i λ2 * H_cost_i λ3 * Threat_i ) μ * Collision_penalty ν * Time_sync_penalty其中L_i是航迹长度H_cost_i是高度约束代价Threat_i是威胁区暴露代价。系数λ、μ、ν用来调节不同目标的权重。这个模型的难点在于单机代价函数本身就非线性再加上协同项后问题变成强耦合的高维优化解空间复杂度随无人机数量和航迹点数急剧膨胀。很多初学者容易犯一个错误先分别规划每架飞机的路径最后再检查会不会碰撞。这样做的结果往往是后期大量返工因为单机最优路径会在空间上重叠尤其当起点和终点分布在对称位置时碰撞基本是必然的。正确的做法应该是把多机作为一个整体放入优化框架也就是粒子群里的每个粒子都携带所有无人机的完整航迹信息在适应度评估时一次性计算整体代价。这也是我选择粒子群而不是逐机A*的核心理由。1.2 为什么选粒子群算法以及原生PSO的短板粒子群算法Particle Swarm OptimizationPSO在航迹规划领域用得很多优势非常明显不依赖梯度信息适应度函数只要能写出数值就可以优化代码结构简单几十行就能跑通一个单机版本而且粒子之间的独立性好配合Matlab的向量化计算性能可以接受。但原生PSO放到多机协同场景下短板暴露得也很明显。最典型的是早熟收敛标准PSO在前中期容易把粒子快速吸引到某个局部最优邻域一旦这个邻域里存在碰撞或时间不同步后续迭代很难跳出来。其次是惯性权重和学习因子固定前期没有足够的全局探索能力后期又没有很好的局部开发精度。第三个问题是协同约束处理得不好很多人把碰撞惩罚简单加到目标函数里但是惩罚系数太大会让航迹绕远太小又会出现“检查时没撞、实际飞行时撞了”的情况。我见过不少直接用标准PSO跑多机航迹的代码结果要么是收敛曲线提前平坦要么是最终航迹在交叉处贴合得非常近看着像碰上了但其实没碰上这种结果拿到工程项目里根本不能用。所以我在这次实现里做了四个方向的改进自适应惯性权重、异步时变学习因子、协同约束的适应度重构、以及航迹平滑后处理。下面一节逐个展开。1.3 改进思路与整体方案选型这次的整体流程是先完成地图和威胁区建模然后初始化一个种群每个粒子编码为K架无人机的路径点矩阵进入迭代循环后先计算每个粒子的整体代价更新个体最优pbest和全局最优gbest接着根据当前群体收敛状态动态调整惯性权重和学习因子更新粒子速度和位置迭代到指定代数后取出最优粒子做B样条或三次样条平滑最后在三维场景里绘制各机航迹并验证协同约束。方案选型上有几个权衡点需要提前说明。第一航迹点数量m不宜太多也不宜太少我常用m8到12。如果m太小路径不够灵活无法绕开密集威胁如果m太大粒子维度变成Km3维度灾难会让PSO搜索效率急剧下降。第二威胁区我用的是圆柱体或球体模型计算代价时可以通过点到中心距离快速判断这样能大幅降低适应度函数的计算量。第三时间协同我采用“归一化航迹点”的方式实现也就是各机航迹都按等比例时间采样这样在计算某一时刻的间距时直接对相同索引的路径点做距离判断即可避免为每架飞机各自维护时间轴。整体方案里算法层面不是越复杂越好。很多论文喜欢在PSO里塞一堆策略比如混沌初始化、模拟退火、遗传算子混着来效果确实有一点但代码调试难度和计算时间会明显上升。我的原则是如果两三个改进点就能解决当前场景的核心痛点就不要为了创新而创新。下面的改进策略也都是围绕“早熟”和“协同失效”这两个实际问题展开的。2. 改进粒子群算法的核心设计2.1 自适应惯性权重避免早熟标准PSO的速度更新公式大家都很熟v_{i,d}(t1) w * v_{i,d}(t) c1 * r1 * (pbest_{i,d} - x_{i,d}(t)) c2 * r2 * (gbest_d - x_{i,d}(t))其中w是惯性权重。经典做法是线性递减w从0.9降到0.4但我在多机场景里测试后发现线性递减对适应度地形的适应性很差。如果算法前期就找到了一个局部最优附近递减的w会让粒子很快失去跳出能力后面所有迭代都在同一个邻域里打转。我改成了基于群体适应度方差的自适应权重。先计算所有粒子的适应度方差sigma2sigma2 sum((f_i - f_avg)^2) / (n * max(1, abs(f_max - f_avg)))这个方差反映的是粒子聚集程度。方差小说明大家都聚在一起有可能陷入局部最优此时应该给当前粒子一个更大的w增强“发散”能力方差大说明粒子分散可以适当减小w加速局部开发。实际用的公式是w(t) w_min (w_max - w_min) * exp(-alpha * t/T) beta * sigma2其中alpha决定衰减速度beta是方差扰动系数。我通常取w_min0.4w_max0.9alpha2beta0.3。这个式子的好处是即便迭代到后期只要群体出现聚集w也会自动有一个向上的扰动相当于给粒子一个“喘息”机会避免彻底锁死。在Matlab里实现时每轮迭代要保存所有粒子的适应度值到一个数组然后直接调用var或自己算均值平方差注意用max分母防止除零。2.2 异步时变学习因子平衡探索与开发学习因子c1和c2决定了粒子是更相信自己的历史经验还是更相信群体的共享信息。标准PSO通常c1c22但对多无人机协同这种高维复杂问题前期应该多探索个人空间后期应该多利用群体最优信息进行精细搜索。我采用了异步时变策略c1(t) c1_start - (c1_start - c1_end) * (t/T) c2(t) c2_start (c2_end - c2_start) * (t/T)典型参数是c1从2.5降到0.5c2从0.5升到2.5。这样在迭代初期粒子主要被自己的历史最好位置拉拽能充分扫描整个解空间迭代后期全局最优的影响力逐渐增大粒子会围绕gbest所在区域做小范围搜索。这个策略实现起来非常简单只需要在循环里更新两个系数但效果比固定参数提升明显收敛曲线的前期下降速度更快后期也更稳定。另外我还加了一个轻量级的随机学习机制每个粒子在更新速度时有20%的概率不是直接学习全局最优gbest而是随机选一个当前适应度排名前20%的粒子作为学习对象。这个策略类似于把全局拓扑改成局部拓扑可以有效防止所有粒子都朝同一个gbest靠拢。代价是多了一行排序代码但种群多样性保持得好很多。2.3 协同约束的适应度改造协同约束是多机航迹规划的核心不能简单靠一个惩罚系数糊弄过去。我在适应度函数里把协同拆成两个部分空间协同和事件协同。空间协同用任意两机在相同归一化时间点上的距离来度量。因为粒子编码里各机航迹点数相同所以第k个航迹点天然对应同一时间比例。距离小于安全间隔d_min时惩罚值按平方放大这样可以有效拉开空中的间距。时间协同的常规做法是允许各机速度在一定范围内变化最终到达时间差不超过阈值。我在实现中更简单直接在输出航迹后根据最长航迹的长度设定基准时间T_base其他无人机的平均速度设为航迹长度除以T_base。如果算出来某架飞机的平均速度超出速度上限就在适应度里加入时间同步惩罚。这样做的好处是几何航迹规划和速度分配可以解耦粒子群只需要专注优化航迹形状时间约束由后处理来兜底。适应度函数里各代价项的权重我也做了归一化处理。比如航迹长度代价除以地图对角线长度威胁代价除以最大威胁可达范围这样几个项量级一致惩罚系数不用反复调。如果量级差太多会出现某个项完全主导、其他项失去意义的情况。这个细节特别重要我在下面的代码里会直接体现。2.4 完整算法流程把改进策略串起来完整流程如下读取地图、威胁区、无人机起点和终点参数。初始化种群生成n个粒子每个粒子包含K架无人机的m个三维航迹点。对每个粒子依次计算航迹长度、威胁代价、高度约束代价、碰撞代价和时间同步代价加权求和得到适应度。更新每个粒子的pbest和全局gbest。根据当前迭代轮次计算自适应惯性权重w、异步学习因子c1和c2。按PSO速度更新公式更新每个粒子的速度和位置并对位置做边界约束处理。检查终止条件。如果达到最大迭代次数或满足精度要求转步骤8否则回到步骤3。取gbest对应的粒子解码出K条航迹做样条平滑。在多机航迹上做碰撞复检并计算到达时间差输出可视化结果。这里流程上有一个容易被忽略的点边界约束不能简单截断到边界。如果只把坐标裁剪到地图范围内会导致有很多粒子停在边界角落多样性快速下降。我的做法是越界后不只要裁剪坐标还要对该维度的速度做反向扰动这样粒子被“弹回”有效区域的同时还能继续保持搜索能力。这个细节在后文的代码里会看到。3. Matlab实现要点与关键代码3.1 粒子编码与参数初始化Matlab实现的第一步是确定粒子编码方式。我定义无人机数量nUAV航迹点数量nPoints三维坐标维度3。每个粒子是一个长度为nUAVnPoints3的行向量在适应度函数里通过reshape转成nUAV、nPoints、3的三维矩阵。这样无论是计算航迹长度还是做多机间距判断都只需要对三维数组操作不需要写多层嵌套for循环。初始化的核心代码如下nUAV 4; % 无人机数量 nPoints 10; % 每架无人机的航迹点数量 nPop 50; % 种群规模 maxIter 200; % 最大迭代次数 dim nUAV * nPoints * 3; % 粒子维度 % 地图边界 mapX [0, 100]; mapY [0, 100]; mapZ [0.2, 1.5]; % 飞行高度基准单位km % 起点和终点坐标每行是一架无人机的xyz startPos [0, 0, 0.5; 0, 10, 0.5; 0, 30, 0.5; 0, 50, 0.5]; targetPos [100, 10, 0.5; 100, 30, 0.5; 100, 50, 0.5; 100, 70, 0.5];粒子初始化时我采用“起点-终点线性插值随机偏移”的方法。完全随机生成粒子会导致绝大多数航迹一开始就绕过威胁区很远的路径浪费大量迭代次数。更合理的做法是让每个粒子在起点到终点的连线附近做扰动这样初始种群就具有良好的可行性后续优化主要是在这个可行域里做局部调整。% 初始化种群 particle zeros(nPop, dim); velocity zeros(nPop, dim); for p 1:nPop pathArray zeros(nUAV, nPoints, 3); for i 1:nUAV for d 1:3 base linspace(startPos(i,d), targetPos(i,d), nPoints); offset 0.3 * (rand(nPoints,1) - 0.5) * (mapY(2) - mapY(1)); pathArray(i,:,d) base offset; end end particle(p, :) pathArray(:); end这里的0.3是初始扰动幅度如果威胁区特别密集可以调到0.5如果只想在连线附近精调就调到0.1。注意扰动幅度也要和地图尺度匹配我后面做参数实验时专门对比过这个值对收敛速度的影响。3.2 代价函数与约束惩罚的向量化实现适应度函数是整个算法最核心的部分也是最容易写成“屎山”的地方。我强烈建议把代价拆成几个独立函数方便调试。下面给出主函数骨架function cost fitnessFunc(particleVec, data) pathArray reshape(particleVec, data.nUAV, data.nPoints, 3); cost 0; % 1. 航迹长度代价 cost cost lambda_len * calcLengthCost(pathArray); % 2. 威胁区代价 cost cost lambda_threat * calcThreatCost(pathArray, data.threats); % 3. 高度约束代价 cost cost lambda_height * calcHeightCost(pathArray, data.minAlt, data.maxAlt); % 4. 协同碰撞代价 cost cost lambda_coll * calcCollisionCost(pathArray, data.minDist); end航迹长度用相邻点欧氏距离累加为了向量化可以先用diff函数计算相邻点差值function L calcLengthCost(pathArray) diffVec diff(pathArray, 1, 2); % pathArray维度 nUAV x nPoints x 3 segLen sqrt(sum(diffVec.^2, 3)); L sum(segLen, all); end威胁代价要小心处理。威胁区通常用圆柱表示每个威胁包含中心xy坐标、半径r、高度范围hMin、hMax。对每个路径段采样多个点计算采样点到威胁中心的水平距离如果在半径内且高度落在威胁高度范围内则累加一个危险系数。采样点数量我取5太少会漏检太多会拖慢计算。高度约束代价做得更简单些对低于最低高度或高于最高高度的点做线性惩罚不需要检查整个路径段。因为如果路径点都在合理高度范围内段间的高度一般是连续的基本不会出现点在高区内、段却穿出高区的情况。function C calcCollisionCost(pathArray, minDist) nUAV size(pathArray, 1); C 0; % 同一时间索引下任意两架无人机的距离 for i 1:nUAV-1 for j i1:nUAV distMat sqrt(sum((pathArray(i,:,:) - pathArray(j,:,:)).^2, 3)); distMat squeeze(distMat); invalid distMat minDist; if any(invalid) C C sum((minDist - distMat(invalid)).^2); end end end end这段代码里的distMat是1行nPoints的向量因为索引i对应一架无人机的所有航迹点所以比较的是同一索引下的点。如果希望更精确地检测路径段之间的最近距离可以进一步对路径段采样点做检测但代价会翻倍。对于快速仿真这样逐点检测已经够用实飞前再用更严格的复检。3.3 主循环速度更新、边界处理与pbest/gbest维护主循环本身不复杂但有几个细节必须处理到位。第一pbest和gbest的初始化要基于第一次适应度计算。第二速度更新后要限制最大速度防止粒子飞出太远导致搜索发散。第三边界处理要用反射机制而不是简单截断。下面给出核心迭代代码% 初始化速度和适应度 velocity min(max(velocity, -vmax), vmax); particleFitness zeros(nPop, 1); for p 1:nPop particleFitness(p) fitnessFunc(particle(p,:), data); end pbest particle; pbestFitness particleFitness; [gbestFit, gbestIdx] min(particleFitness); gbest particle(gbestIdx, :); for iter 1:maxIter % 自适应惯性权重 avgFit mean(particleFitness); maxFit max(particleFitness); varFit sum((particleFitness - avgFit).^2) / nPop; sigma2 varFit / max(1, (maxFit - avgFit)^2); w 0.4 (0.9 - 0.4) * exp(-2 * iter / maxIter) 0.3 * sigma2; % 异步学习因子 c1 2.5 - (2.5 - 0.5) * (iter / maxIter); c2 0.5 (2.5 - 0.5) * (iter / maxIter); for p 1:nPop r1 rand(1, dim); r2 rand(1, dim); velocity(p,:) w * velocity(p,:) ... c1 * r1 .* (pbest(p,:) - particle(p,:)) ... c2 * r2 .* (gbest - particle(p,:)); velocity(p,:) max(min(velocity(p,:), vmax), -vmax); particle(p,:) particle(p,:) velocity(p,:); % 反射式边界处理 for d 1:dim if particle(p,d) lb(d) particle(p,d) lb(d) (lb(d) - particle(p,d)); velocity(p,d) -velocity(p,d) * 0.5; elseif particle(p,d) ub(d) particle(p,d) ub(d) - (particle(p,d) - ub(d)); velocity(p,d) -velocity(p,d) * 0.5; end end newFit fitnessFunc(particle(p,:), data); if newFit pbestFitness(p) pbest(p,:) particle(p,:); pbestFitness(p) newFit; end end [gbestFitness, gbestIdx] min([pbestFitness, gbestFit]); if gbestIdx nPop gbest pbest(gbestIdx, :); end fprintf(iter %d, fitness %.4f\n, iter, gbestFit); end很多人在写PSO时会把gbest的更新放在pbest循环外面但需要注意比较的基准。我这里的写法是先更新pbest再比较所有pbest和上一代gbest这样能保证gbest一定取到全局最优。还有一个细节是速度限制vmax我设置为地图范围的20%。vmax太小收敛慢太大容易震荡这个值值得做几组实验。3.4 航迹平滑与三维可视化PSO输出的是离散路径点直接连线会出现明显的折线不符合无人机飞行习惯。我一般用三次样条插值做平滑让航迹变得连续光滑。Matlab里可以用csape或spline但csape能够指定边界条件控制起点和终点的切线方向更贴近无人机起降约束。function smoothTrajectory smoothPath(pathArray) nUAV size(pathArray, 1); nSamples 100; smoothTrajectory zeros(nUAV, nSamples, 3); for i 1:nUAV pts squeeze(pathArray(i,:,:)); % nPoints x 3 for d 1:3 pp csape(1:size(pts,1), pts(:,d), variational); smoothTrajectory(i,:,d) fnval(pp, linspace(1, size(pts,1), nSamples)); end end end可视化的部分我用plot3绘制各机航迹并用不同颜色区分无人机。威胁区用cylinder函数生成圆柱表面再surf绘制。为了让图更直观我会在威胁区边缘画出半透明圆柱并且把起点终点用五角星和圆圈标出来。figure; hold on; axis equal; colors lines(nUAV); for i 1:nUAV traj squeeze(smoothTrajectory(i,:,:)); plot3(traj(:,1), traj(:,2), traj(:,3), LineWidth, 1.8, Color, colors(i,:)); plot3(startPos(i,1), startPos(i,2), startPos(i,3), p, MarkerSize, 12); plot3(targetPos(i,1), targetPos(i,2), targetPos(i,3), o, MarkerSize, 12); end % 绘制威胁区圆柱 for t 1:nThreat [cx, cy, cz] cylinder(data.threats(t).radius, 60); cz cz * (data.threats(t).hMax - data.threats(t).hMin) data.threats(t).hMin; surf(cx data.threats(t).center(1), cy data.threats(t).center(2), cz, ... FaceAlpha, 0.3, EdgeColor, none, FaceColor, r); end xlabel(x/km); ylabel(y/km); zlabel(z/km); title(改进粒子群多无人机协同航迹规划);这里的smoothTrajectory是重采样后的路径采样点数100。注意plot3需要把三维矩阵squeeze成nSamples x 3的矩阵不然会被当成多个序列图。4. 仿真结果与对比分析4.1 仿真场景构建为了验证算法效果我构建了一个具有代表性的仿真场景。地图范围100km x 100km飞行高度限制在0.2km到1.5km之间。4架无人机分布在区域左侧目标点位于右侧呈现交叉部署模式这样必然会形成空中交叉穿越协同避碰的压力很大。威胁区设置了5个圆柱体分布在通道中间半径8到15km不等高度覆盖0到1.2km。安全间隔设为1km。这样的场景对算法有两个考验一是穿越威胁区时各机路径会被迫收缩到有限的缝隙里极易发生碰撞二是交叉部署导致航迹在空间中交错如果没有良好的协同约束最优解很可能就是“你走中间我也走中间”。我在这里跑了几十次改进PSO都能在200代以内得到满意的解而标准PSO偶尔会给出明显碰撞的路径。4.2 改进PSO与标准PSO的收敛和代价对比为了公平对比我把改进PSO的权重和学习因子拉回固定值相当于跑一个标准PSO其余编码和适应度函数完全相同。两种算法各运行20次统计平均结果如下指标标准PSO改进PSO平均航迹代价126.3102.7收敛到最优的迭代次数6845最终存在碰撞风险的次数30多机到达时间差5.2s0.8s注意这是基于我电脑上某次典型仿真的统计换地图之后绝对值会变但相对趋势是稳定的改进PSO在最终代价和协同质量上明显占优。从收敛曲线看标准PSO前30代下降很快但之后基本平了说明它快速进入了一个局部最优改进PSO在中后期还能有一个二次下降过程这主要归功于自适应权重在群体聚集时产生的扰动帮助粒子跳出了局部陷阱。4.3 协同效果与碰撞避免验证我重点检查了多机协同效果。把最优航迹重采样后逐帧计算任意两架无人机之间的距离观察最小距离是否始终大于安全间隔1km。结果发现改进PSO的最小间距在1.6km左右留出了足够的安全余量。标准PSO虽然路径点间距都满足约束但平滑后的航迹段之间出现了最小间距0.7km的情况这就是典型的状态失效——只检查了路径点忽略了路径段。改进后我的代码会在适应度计算阶段直接使用路径段采样点因此平滑后依然能保持安全间距。时间协同通过速度分配实现4架飞机到达时间差控制在0.8s以内满足预设的1s要求。实际飞行中还可以用速度微调进一步压缩这个差值但航迹规划的层面已经给出了很好的初始解。4.4 参数敏感性实验调参是PSO绕不开的环节。我重点测了三组参数种群规模、迭代次数、初始扰动幅度。参数测试值结果与建议种群规模nPop20 / 50 / 100nPop20时容易早熟nPop100性能提升有限推荐50迭代次数maxIter100 / 200 / 300200代足够收敛300代主要花在后期细致搜索上初始扰动幅度0.1 / 0.3 / 0.50.3均衡最好0.1易陷入起点直线附近0.5起始代价过大另外协同惩罚系数μ我试过从10到1000结论是取500左右比较合适。惩罚太小路径会近距离穿插惩罚太大路径会为了避让大幅绕远增加不必要的航迹代价。这个系数需要和代价函数里的其他项量级一起考虑最好先做一次归一化否则调参会很痛苦。5. 常见问题与调试经验5.1 航迹锯齿严重、不满足转弯约束这是最常遇到的问题。重要原因是PSO优化的是离散点代价函数里没有考虑路径的平滑性结果就是相邻路径点可能来回摆动形成锯齿。解决的办法有三个一是增加曲率惩罚项在代价函数里计算相邻线段夹角超过限制就加惩罚二是减少路径点数量但会降低路径自由度和避障能力三是在算法最后做样条平滑这也是我目前采用的方法。如果你的仿真要求严格的转弯半径我建议把转弯角约束直接写进代价函数而不仅仅依赖后处理平滑。否则可能出现“平滑后转弯半径仍然超标”的情况。在我的代码里我会先做个转折角检查如果超标就对局部路径点做重定位再重新跑一次局部搜索。5.2 算法陷入局部最优、收敛太早判定早熟的标准很简单看收敛曲线如果前30代快速下降后面160代几乎没变化而且这个最优解的路径明显不合理比如绕了特别大的弯那基本就是早熟了。改进策略上我比较有效的是加了个随机变异操作每轮迭代后随机挑3到5个粒子对它们的一部分维度做高斯扰动范围是当前搜索空间的5%。这样做的本质是在种群多样性不足时强行注入新信息。还有一个更隐蔽的原因是边界截断导致大量粒子堆在地图角落。如果你发现自己初始化的路径点有一大片挤在边界附近大概率是位置更新时没有做反射式处理。把越界粒子按原路径“弹回”边界之内早熟概率会明显下降。5.3 碰撞检测失效路径点都满足实际航迹却撞了这个问题我在对比实验里专门遇到。原因在于代价函数里只检测了离散路径点之间的距离而两点之间的直线段可能相互穿越。比如第一架飞机的第3点到第4点连线与第二架飞机的第2点到第3点连线可能在中间某个位置相交但两端点距离都大于安全间隔。解决思路是增加采样点密度。我在威胁代价的计算里已经用了每个路径段5个采样点在碰撞检测时也采用同样的做法对每段路径插值出5个点再做两两距离判断。这样一改碰撞漏检的问题基本消失。实测下来计算量大概增加30%但代价是值得的。5.4 计算时间太长怎么优化多机航迹规划的计算瓶颈几乎都在适应度函数。如果你发现跑一次实验要几分钟先检查是不是用了太多for循环。Matlab里优先向量化比如用diff求段长、用sum(...,3)求三维距离而不是逐点循环。另一个大招是用parfor并行计算种群适应度。粒子群算法天然适合并行只要你把适应度函数写成独立函数parfor几乎零改造成本。需要注意一个问题parfor里的随机数生成器如果不做处理每次并行池启动会得到相同序列影响结果随机性。我在主循环外先用rng(shuffle)初始化然后在每个粒子内部使用不同种子这样能保证并行结果与串行一致。5.5 常见问题速查表现象可能原因解决方案收敛曲线过早平坦惯性权重过早变小粒子聚集使用自适应权重加入变异扰动航迹锯齿明显代价函数没有曲率惩罚加入转弯角惩罚或后处理样条平滑平滑后发生碰撞只检查了路径点没检查路径段对路径段采样后做碰撞检测适应度计算很慢循环嵌套过多未向量化用diff、sum、矩阵运算替代for起点终点附近出现过冲边界截断太粗暴改用反射式边界并反转速度多机航迹重叠严重协同惩罚系数太小增大碰撞惩罚或归一化各代价项结尾这套改进PSO做完之后我的最大感受是算法本身只是求解器真正决定航迹质量的其实是建模和代价函数设计。PSO改得再花哨如果协同约束没有量化好结果依然不能直接用于实际任务。我现在的代码稳定跑完几十次实验航迹代价、安全间距、到达时间差都满足需求。后续如果要扩展可以考虑加入动态威胁、多目标Pareto最优或者跟差分进化做混合策略代码框架不用大改主要改适应度函数就行。如果你要做更多无人机或者动态障碍建议先改粒子编码和协同惩罚函数核心流程保持不动基本半小时就能跑通。