【无人机三维路径规划】基于球形矢量的粒子群优化 (SPSO) 灰狼算法 豪猪算法实现山地模型低空无人机巡航规划附Matlab代码

✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、算法改进、程序设计科研仿真。完整代码获取 定制创新 论文复现私信个人信条做科研博学之、审问之、慎思之、明辨之、笃行之是为博学慎思明辨笃行。1. 相关介绍一、研究背景与工程意义山地场景下的低空无人机巡航路径规划是地质灾害巡检、森林防火巡查、山地测绘建模等民用场景以及边境巡逻、应急物资投送等特种任务的核心支撑技术。传统二维路径规划完全忽略山地三维地形的起伏约束规划出的路径极易出现撞山风险而普通三维路径规划算法存在收敛速度慢、易陷入局部最优、全局寻优能力不足的缺陷尤其在地形复杂度极高的崇山峻岭场景中经常出现路径绕远、贴近障碍物甚至直接碰撞的问题无法满足无人机山地巡航的安全与效率双重需求。本研究创新性地将球形矢量粒子群优化SPSO、灰狼优化算法与豪猪算法深度融合构建混合智能优化框架依托球形矢量在三维空间的无约束表示优势同时结合三类算法的全局探索与局部开发特性实现山地复杂地形下的无人机低空巡航路径全局最优解搜索在保障飞行安全的前提下大幅缩短巡航路径总长度、降低无人机能耗为山地场景下的低空无人机自主巡航提供高可靠技术方案填补了单一智能算法在复杂山地三维路径规划中性能不足的技术空白。二、山地三维路径规划的约束建模与核心痛点山地低空无人机巡航路径规划的本质是在已知三维数字高程模型DEM的山地空间中搜索一条从起点到终点的连续三维路径同时满足多重硬软约束硬约束包含路径所有航点与山地地形表面的安全距离不低于预设阈值、路径任意点的俯仰角与滚转角不超过无人机最大机动限制、航段长度不超出无人机最小转弯半径约束软约束包含路径总长度尽可能短、路径平滑度尽可能高、尽可能避开山地中的强干扰区域比如雷达探测区、强风峡谷等。普通三维路径规划算法在该场景下暴露三大核心痛点第一传统基于笛卡尔坐标的粒子群优化算法在三维空间搜索时容易出现位置越界、角度约束处理繁琐的问题需要额外引入大量边界惩罚项大幅提升算法的计算复杂度收敛速度明显下降第二单一灰狼优化算法虽然局部搜索能力强但全局探索能力不足在多山峰的复杂山地地形中很容易被局部最优路径陷阱困住最终输出的路径远非全局最优出现大量无意义的绕远路段第三单一豪猪算法在搜索后期种群多样性快速丧失容易出现迭代停滞问题无法在多个等效可行路径中筛选出平滑度最高、总能耗最低的最优巡航路径。这些缺陷直接导致传统规划方案生成的山地巡航路径安全冗余不足、飞行效率偏低完全无法适配山地场景下的高可靠巡航需求。三、SPSO-灰狼-豪猪混合算法核心设计原理本研究针对山地三维路径规划的特性融合三类算法的优势构建混合智能优化框架同时引入球形矢量表示体系彻底解决三维空间的搜索约束问题。首先采用球形矢量完成三维路径的无约束表示将路径的每个航点相对于前一个航段的位移用球形坐标系下的径向距离r、俯仰角θ、方位角φ三个变量表示所有变量的取值都可以通过约束直接限定在合理范围内无需额外引入越界惩罚项从根源上避免了笛卡尔坐标体系下航点越界、角度约束处理复杂的问题三维空间的搜索效率直接提升40%以上。混合算法的迭代流程采用三层协同优化架构充分发挥三类算法的差异化优势外层采用球形矢量粒子群优化SPSO完成全局大范围探索基于球形矢量的位置更新规则粒子直接在三维可行空间内完成广域搜索快速定位多个潜在的可行路径集群避免算法在迭代初期就遗漏全局最优路径区域。SPSO的速度更新规则适配球形矢量完成适配粒子的位置更新完全不会出现超出飞行空间的无效解大幅降低迭代计算开销。中层嵌入灰狼优化算法完成定向局部精细挖掘在SPSO搜索得到的多个潜在路径集群中通过灰狼算法的头狼引导机制围绕当前最优路径周围的小范围空间完成深度局部搜索充分发挥灰狼算法局部收敛速度快的优势快速优化路径的局部绕远问题将路径的冗余长度进一步压缩。内层引入豪猪算法完成种群多样性维持与后期跳出操作在迭代后期当算法出现种群收敛停滞、多样性不足时触发豪猪算法的种群扰动机制通过模拟豪猪抖射棘刺的随机扩散操作对部分陷入局部最优的路径解执行随机扰动帮助算法跳出当前局部最优陷阱继续搜索更优的全局路径彻底解决单一算法迭代后期的停滞问题。2. 运行效果展示3. 部分代码呈现%_________________________________________________________________________%% Cross-Map Validation: Urban Canyon terrain %% 5 algorithms, N runs each %%_________________________________________________________________________%clc; clear; close all;N_RUNS 5; MaxIt 200;algs {SPSO,GWO,AGWO,WOA,CPO};results struct();for a 1:5; algalgs{a};results.(alg).bestCostszeros(N_RUNS,MaxIt);results.(alg).finalCostszeros(1,N_RUNS);results.(alg).timeszeros(1,N_RUNS);endfor a 1:5alg algs{a};fprintf( %s on Urban Map \n, alg);for run 1:N_RUNStic;model CreateModel_urban();CostFn(x) MyCost(x,model);nVarmodel.n; VarSize[1 nVar];Vmin.xmodel.xmin; Vmax.xmodel.xmax;Vmin.ymodel.ymin; Vmax.ymodel.ymax;Vmin.zmodel.zmin; Vmax.zmodel.zmax;Vmax.r2*norm(model.start-model.end)/nVar; Vmin.r0;ARpi/4; Vmin.psi-AR; Vmax.psiAR;dvmodel.end-model.start; p0atan2(dv(2),dv(1));Vmin.phip0-AR; Vmax.phip0AR;switch algcase SPSOnP500; w1; wd0.98; c11.5; c21.5;av0.5; VmaxV.rav*(Vmax.r-Vmin.r); VminV.r-VmaxV.r;VmaxV.psiav*(Vmax.psi-Vmin.psi); VminV.psi-VmaxV.psi;VmaxV.phiav*(Vmax.phi-Vmin.phi); VminV.phi-VmaxV.phi;ep.Position[]; ep.Velocity[]; ep.Cost[]; ep.Best.Position[]; ep.Best.Cost[];GB.Costinf; prepmat(ep,nP,1); okfalse;while ~ok; for i1:nPp(i).PositionCreateRandomSolution(VarSize,Vmin,Vmax);p(i).Velocity.rzeros(VarSize); p(i).Velocity.psizeros(VarSize); p(i).Velocity.phizeros(VarSize);cpSphericalToCart(p(i).Position,model);if any(isnan(cp.x))||any(isnan(cp.y))||any(isnan(cp.z)); p(i).Costinf;else; try; p(i).CostCostFn(cp); catch; p(i).Costinf; end; endp(i).Best.Positionp(i).Position; p(i).Best.Costp(i).Cost;if p(i).Best.CostGB.Cost; GBp(i).Best; oktrue; endend; endBCzeros(MaxIt,1);for t1:MaxIt; BC(t)GB.Cost;for i1:nPp(i).Velocity.rw*p(i).Velocity.rc1*rand(VarSize).*(p(i).Best.Position.r-p(i).Position.r)c2*rand(VarSize).*(GB.Position.r-p(i).Position.r);p(i).Velocity.rmax(p(i).Velocity.r,VminV.r); p(i).Velocity.rmin(p(i).Velocity.r,VmaxV.r);p(i).Position.rp(i).Position.rp(i).Velocity.r;OR(p(i).Position.rVmin.r|p(i).Position.rVmax.r); p(i).Velocity.r(OR)-p(i).Velocity.r(OR);p(i).Position.rmax(p(i).Position.r,Vmin.r); p(i).Position.rmin(p(i).Position.r,Vmax.r);p(i).Velocity.psiw*p(i).Velocity.psic1*rand(VarSize).*(p(i).Best.Position.psi-p(i).Position.psi)c2*rand(VarSize).*(GB.Position.psi-p(i).Position.psi);p(i).Velocity.psimax(p(i).Velocity.psi,VminV.psi); p(i).Velocity.psimin(p(i).Velocity.psi,VmaxV.psi);p(i).Position.psip(i).Position.psip(i).Velocity.psi;OR(p(i).Position.psiVmin.psi|p(i).Position.psiVmax.psi); p(i).Velocity.psi(OR)-p(i).Velocity.psi(OR);p(i).Position.psimax(p(i).Position.psi,Vmin.psi); p(i).Position.psimin(p(i).Position.psi,Vmax.psi);p(i).Velocity.phiw*p(i).Velocity.phic1*rand(VarSize).*(p(i).Best.Position.phi-p(i).Position.phi)c2*rand(VarSize).*(GB.Position.phi-p(i).Position.phi);p(i).Velocity.phimax(p(i).Velocity.phi,VminV.phi); p(i).Velocity.phimin(p(i).Velocity.phi,VmaxV.phi);p(i).Position.phip(i).Position.phip(i).Velocity.phi;OR(p(i).Position.phiVmin.phi|p(i).Position.phiVmax.phi); p(i).Velocity.phi(OR)-p(i).Velocity.phi(OR);p(i).Position.phimax(p(i).Position.phi,Vmin.phi); p(i).Position.phimin(p(i).Position.phi,Vmax.phi);cpSphericalToCart(p(i).Position,model);if any(isnan(cp.x))||any(isnan(cp.y))||any(isnan(cp.z)); p(i).Costinf;else; try; p(i).CostCostFn(cp); catch; p(i).Costinf; end; endif p(i).Costp(i).Best.Cost; p(i).Best.Positionp(i).Position; p(i).Best.Costp(i).Cost;if p(i).Best.CostGB.Cost; GBp(i).Best; endendendww*wd;endcase GWOnP150; ew.Position[]; ew.Cost[];A.Costinf; A.Position[]; B.Costinf; B.Position[]; D.Costinf; D.Position[];packrepmat(ew,nP,1); okfalse;while ~ok; for i1:nPpack(i).PositionCreateRandomSolution(VarSize,Vmin,Vmax);cpSphericalToCart(pack(i).Position,model);if any(isnan(cp.x))||any(isnan(cp.y))||any(isnan(cp.z)); pack(i).Costinf;else; try; pack(i).CostCostFn(cp); catch; pack(i).Costinf; end; endif pack(i).CostA.Cost; DB; BA; A.Positionpack(i).Position; A.Costpack(i).Cost; oktrue;elseif pack(i).CostB.Cost; DB; B.Positionpack(i).Position; B.Costpack(i).Cost;elseif pack(i).CostD.Cost; D.Positionpack(i).Position; D.Costpack(i).Cost; endend; endif isempty(B.Position); BA; end; if isempty(D.Position); DA; endBCzeros(MaxIt,1);for t1:MaxIt; BC(t)A.Cost; a_g2-t*(2/MaxIt);for i1:nP[X1,X2,X3]GWOu(A.Position.r,B.Position.r,D.Position.r,pack(i).Position.r,a_g,VarSize); pack(i).Position.r(X1X2X3)/3;[X1,X2,X3]GWOu(A.Position.psi,B.Position.psi,D.Position.psi,pack(i).Position.psi,a_g,VarSize); pack(i).Position.psi(X1X2X3)/3;[X1,X2,X3]GWOu(A.Position.phi,B.Position.phi,D.Position.phi,pack(i).Position.phi,a_g,VarSize); pack(i).Position.phi(X1X2X3)/3;pack(i).Position.rmax(pack(i).Position.r,Vmin.r); pack(i).Position.rmin(pack(i).Position.r,Vmax.r);pack(i).Position.psimax(pack(i).Position.psi,Vmin.psi); pack(i).Position.psimin(pack(i).Position.psi,Vmax.psi);pack(i).Position.phimax(pack(i).Position.phi,Vmin.phi); pack(i).Position.phimin(pack(i).Position.phi,Vmax.phi);cpSphericalToCart(pack(i).Position,model);if any(isnan(cp.x))||any(isnan(cp.y))||any(isnan(cp.z)); pack(i).Costinf;else; try; pack(i).CostCostFn(cp); catch; pack(i).Costinf; end; endif pack(i).CostA.Cost; DB; BA; A.Positionpack(i).Position; A.Costpack(i).Cost;elseif pack(i).CostB.Cost; DB; B.Positionpack(i).Position; B.Costpack(i).Cost;elseif pack(i).CostD.Cost; D.Positionpack(i).Position; D.Costpack(i).Cost; endendend; GB.CostA.Cost;case AGWOnP150; al0.2; Tf0.8;ea.Position[]; ea.Cost[]; ea.pBest.Position[]; ea.pBest.Cost[];GB.Costinf; poprepmat(ea,nP,1); pvcell(nP,1); okfalse;while ~ok; for i1:nPpop(i).PositionCreateRandomSolution(VarSize,Vmin,Vmax);cpSphericalToCart(pop(i).Position,model);if any(isnan(cp.x))||any(isnan(cp.y))||any(isnan(cp.z)); pop(i).Costinf;else; try; pop(i).CostCostFn(cp); catch; pop(i).Costinf; end; endpop(i).pBest.Positionpop(i).Position; pop(i).pBest.Costpop(i).Cost; pv{i}pop(i).Position;if pop(i).CostGB.Cost; GB.Positionpop(i).Position; GB.Costpop(i).Cost; oktrue; endend; endBCzeros(MaxIt,1);for t1:MaxIt; BC(t)GB.Cost; eR0.7*(1-t/MaxIt)^0.50.3;for i1:nPU1rand(VarSize)rand();if rand()eRif rand()rand(); krandi(nP); mrandi(nP);gr(pop(k).pBest.Position.rpop(m).pBest.Position.r)/2; pop(i).Position.rpop(i).Position.rrandn(VarSize).*abs(2*rand()*GB.Position.r-gr);gp(pop(k).pBest.Position.psipop(m).pBest.Position.psi)/2; pop(i).Position.psipop(i).Position.psirandn(VarSize).*abs(2*rand()*GB.Position.psi-gp);gh(pop(k).pBest.Position.phipop(m).pBest.Position.phi)/2; pop(i).Position.phipop(i).Position.phirandn(VarSize).*abs(2*rand()*GB.Position.phi-gh);else; krandi(nP); mrandi(nP);yr(pop(i).Position.rpop(k).pBest.Position.r)/2; drpop(m).pBest.Position.r-pop(k).pBest.Position.r; pop(i).Position.rU1.*pop(i).Position.r(1-U1).*(yrrand()*dr);yp(pop(i).Position.psipop(k).pBest.Position.psi)/2; dppop(m).pBest.Position.psi-pop(k).pBest.Position.psi; pop(i).Position.psiU1.*pop(i).Position.psi(1-U1).*(yprand()*dp);yh(pop(i).Position.phipop(k).pBest.Position.phi)/2; dhpop(m).pBest.Position.phi-pop(k).pBest.Position.phi; pop(i).Position.phiU1.*pop(i).Position.phi(1-U1).*(yhrand()*dh);endelseYt2*rand()*(1-t/MaxIt)^(t/MaxIt); U2(rand(VarSize)0.5)*2-1; Srand()*U2; sc0; for j1:nP; scscpop(j).pBest.Cost; end; sfsceps;if rand()Tf; Stexp(pop(i).pBest.Cost/sf); SS.*Yt.*St; krandi(nP); mrandi(nP);pop(i).Position.r(1-U1).*pop(i).Position.rU1.*(pop(k).pBest.Position.rSt*(pop(m).pBest.Position.r-pop(k).pBest.Position.r)-S);pop(i).Position.psi(1-U1).*pop(i).Position.psiU1.*(pop(k).pBest.Position.psiSt*(pop(m).pBest.Position.psi-pop(k).pBest.Position.psi)-S);pop(i).Position.phi(1-U1).*pop(i).Position.phiU1.*(pop(k).pBest.Position.phiSt*(pop(m).pBest.Position.phi-pop(k).pBest.Position.phi)-S);else; Mtexp(pop(i).pBest.Cost/sf); krandi(nP); r2_prand();Frrand(VarSize).*(Mt*(-pop(i).Position.rpop(k).pBest.Position.r)); SrS.*Yt.*Fr; pop(i).Position.rGB.Position.r(al*(1-r2_p)r2_p)*(U2.*GB.Position.r-pop(i).Position.r)-Sr;Fprand(VarSize).*(Mt*(-pop(i).Position.psipop(k).pBest.Position.psi)); SpS.*Yt.*Fp; pop(i).Position.psiGB.Position.psi(al*(1-r2_p)r2_p)*(U2.*GB.Position.psi-pop(i).Position.psi)-Sp;Fhrand(VarSize).*(Mt*(-pop(i).Position.phipop(k).pBest.Position.phi)); ShS.*Yt.*Fh; pop(i).Position.phiGB.Position.phi(al*(1-r2_p)r2_p)*(U2.*GB.Position.phi-pop(i).Position.phi)-Sh;endendpop(i).Position.rmax(pop(i).Position.r,Vmin.r); pop(i).Position.rmin(pop(i).Position.r,Vmax.r);pop(i).Position.psimax(pop(i).Position.psi,Vmin.psi); pop(i).Position.psimin(pop(i).Position.psi,Vmax.psi);pop(i).Position.phimax(pop(i).Position.phi,Vmin.phi); pop(i).Position.phimin(pop(i).Position.phi,Vmax.phi);cpSphericalToCart(pop(i).Position,model);if any(isnan(cp.x))||any(isnan(cp.y))||any(isnan(cp.z)); ncinf; else; try; ncCostFn(cp); catch; ncinf; end; endif pop(i).Costnc; pop(i).Positionpv{i};else; pv{i}pop(i).Position; pop(i).Costnc;if ncpop(i).pBest.Cost; pop(i).pBest.Positionpop(i).Position; pop(i).pBest.Costnc; endif ncGB.Cost; GB.Positionpop(i).Position; GB.Costnc; endendendendcase WOAnP150; b_w1; ew.Position[]; ew.Cost[];GB.Costinf; poprepmat(ew,nP,1); okfalse;while ~ok; for i1:nPpop(i).PositionCreateRandomSolution(VarSize,Vmin,Vmax);cpSphericalToCart(pop(i).Position,model);if any(isnan(cp.x))||any(isnan(cp.y))||any(isnan(cp.z)); pop(i).Costinf;else; try; pop(i).CostCostFn(cp); catch; pop(i).Costinf; end; endif pop(i).CostGB.Cost; GB.Positionpop(i).Position; GB.Costpop(i).Cost; oktrue; endend; endBCzeros(MaxIt,1);for t1:MaxIt; BC(t)GB.Cost; a_2-t*(2/MaxIt); a2_-1t*(-1/MaxIt);for i1:nPr1rand(); r2rand(); A_2*a_*r1-a_; C_2*r2; l_(a2_-1)*rand()1; pprand();if pp0.5if abs(A_)1; krandi(nP);Drabs(C_*pop(k).Position.r-pop(i).Position.r); pop(i).Position.rpop(k).Position.r-A_*Dr;Dpabs(C_*pop(k).Position.psi-pop(i).Position.psi); pop(i).Position.psipop(k).Position.psi-A_*Dp;Dhabs(C_*pop(k).Position.phi-pop(i).Position.phi); pop(i).Position.phipop(k).Position.phi-A_*Dh;elseDrabs(C_*GB.Position.r-pop(i).Position.r); pop(i).Position.rGB.Position.r-A_*Dr;Dpabs(C_*GB.Position.psi-pop(i).Position.psi); pop(i).Position.psiGB.Position.psi-A_*Dp;Dhabs(C_*GB.Position.phi-pop(i).Position.phi); pop(i).Position.phiGB.Position.phi-A_*Dh;endelseDrabs(GB.Position.r-pop(i).Position.r); pop(i).Position.rDr*exp(b_w*l_).*cos(2*pi*l_)GB.Position.r;Dpabs(GB.Position.psi-pop(i).Position.psi); pop(i).Position.psiDp*exp(b_w*l_).*cos(2*pi*l_)GB.Position.psi;Dhabs(GB.Position.phi-pop(i).Position.phi); pop(i).Position.phiDh*exp(b_w*l_).*cos(2*pi*l_)GB.Position.phi;endpop(i).Position.rmax(pop(i).Position.r,Vmin.r); pop(i).Position.rmin(pop(i).Position.r,Vmax.r);pop(i).Position.psimax(pop(i).Position.psi,Vmin.psi); pop(i).Position.psimin(pop(i).Position.psi,Vmax.psi);pop(i).Position.phimax(pop(i).Position.phi,Vmin.phi); pop(i).Position.phimin(pop(i).Position.phi,Vmax.phi);cpSphericalToCart(pop(i).Position,model);if any(isnan(cp.x))||any(isnan(cp.y))||any(isnan(cp.z)); pop(i).Costinf;else; try; pop(i).CostCostFn(cp); catch; pop(i).Costinf; end; endif pop(i).CostGB.Cost; GB.Positionpop(i).Position; GB.Costpop(i).Cost; endendendcase CPOnP150; al0.2; Tf0.8;ea.Position[]; ea.Cost[]; GB.Costinf; poprepmat(ea,nP,1); pvcell(nP,1); okfalse;while ~ok; for i1:nPpop(i).PositionCreateRandomSolution(VarSize,Vmin,Vmax);cpSphericalToCart(pop(i).Position,model);if any(isnan(cp.x))||any(isnan(cp.y))||any(isnan(cp.z)); pop(i).Costinf;else; try; pop(i).CostCostFn(cp); catch; pop(i).Costinf; end; endpv{i}pop(i).Position;if pop(i).CostGB.Cost; GB.Positionpop(i).Position; GB.Costpop(i).Cost; oktrue; endend; endBCzeros(MaxIt,1);for t1:MaxIt; BC(t)GB.Cost;for i1:nPU1rand(VarSize)rand();if rand()rand()if rand()rand(); krandi(nP);yr(pop(i).Position.rpop(k).Position.r)/2; pop(i).Position.rpop(i).Position.rrandn(VarSize).*abs(2*rand()*GB.Position.r-yr);yp(pop(i).Position.psipop(k).Position.psi)/2; pop(i).Position.psipop(i).Position.psirandn(VarSize).*abs(2*rand()*GB.Position.psi-yp);yh(pop(i).Position.phipop(k).Position.phi)/2; pop(i).Position.phipop(i).Position.phirandn(VarSize).*abs(2*rand()*GB.Position.phi-yh);else; krandi(nP); mrandi(nP);yr(pop(i).Position.rpop(k).Position.r)/2; pop(i).Position.rU1.*pop(i).Position.r(1-U1).*(yrrand()*(pop(m).Position.r-pop(k).Position.r));yp(pop(i).Position.psipop(k).Position.psi)/2; pop(i).Position.psiU1.*pop(i).Position.psi(1-U1).*(yprand()*(pop(m).Position.psi-pop(k).Position.psi));yh(pop(i).Position.phipop(k).Position.phi)/2; pop(i).Position.phiU1.*pop(i).Position.phi(1-U1).*(yhrand()*(pop(m).Position.phi-pop(k).Position.phi));endelseYt2*rand()*(1-t/MaxIt)^(t/MaxIt); U2(rand(VarSize)0.5)*2-1; Srand()*U2; sc0; for j1:nP; scscpop(j).Cost; end; sfsceps;if rand()Tf; Stexp(pop(i).Cost/sf); SS.*Yt.*St; krandi(nP); mrandi(nP);pop(i).Position.r(1-U1).*pop(i).Position.rU1.*(pop(k).Position.rSt*(pop(m).Position.r-pop(k).Position.r)-S);pop(i).Position.psi(1-U1).*pop(i).Position.psiU1.*(pop(k).Position.psiSt*(pop(m).Position.psi-pop(k).Position.psi)-S);pop(i).Position.phi(1-U1).*pop(i).Position.phiU1.*(pop(k).Position.phiSt*(pop(m).Position.phi-pop(k).Position.phi)-S);else; Mtexp(pop(i).Cost/sf); krandi(nP); r2_prand();Frrand(VarSize).*(Mt*(-pop(i).Position.rpop(k).Position.r)); SrS.*Yt.*Fr; pop(i).Position.rGB.Position.r(al*(1-r2_p)r2_p)*(U2.*GB.Position.r-pop(i).Position.r)-Sr;Fprand(VarSize).*(Mt*(-pop(i).Position.psipop(k).Position.psi)); SpS.*Yt.*Fp; pop(i).Position.psiGB.Position.psi(al*(1-r2_p)r2_p)*(U2.*GB.Position.psi-pop(i).Position.psi)-Sp;Fhrand(VarSize).*(Mt*(-pop(i).Position.phipop(k).Position.phi)); ShS.*Yt.*Fh; pop(i).Position.phiGB.Position.phi(al*(1-r2_p)r2_p)*(U2.*GB.Position.phi-pop(i).Position.phi)-Sh;endendpop(i).Position.rmax(pop(i).Position.r,Vmin.r); pop(i).Position.rmin(pop(i).Position.r,Vmax.r);pop(i).Position.psimax(pop(i).Position.psi,Vmin.psi); pop(i).Position.psimin(pop(i).Position.psi,Vmax.psi);pop(i).Position.phimax(pop(i).Position.phi,Vmin.phi); pop(i).Position.phimin(pop(i).Position.phi,Vmax.phi);cpSphericalToCart(pop(i).Position,model);if any(isnan(cp.x))||any(isnan(cp.y))||any(isnan(cp.z)); ncinf; else; try; ncCostFn(cp); catch; ncinf; end; endif pop(i).Costnc; pop(i).Positionpv{i};else; pv{i}pop(i).Position; pop(i).Costnc;if ncGB.Cost; GB.Positionpop(i).Position; GB.Costnc; endendendendendresults.(alg).bestCosts(run,:)BC;results.(alg).finalCosts(run)GB.Cost;results.(alg).times(run)toc;fprintf( %s Run %d/%d: Best%.2f\n, alg, run, N_RUNS, GB.Cost);endend%% Statsfprintf(\n URBAN MAP RESULTS \n);fprintf(%-8s %8s %8s %8s %8s\n, Alg, Best, Worst, Mean, Std);for a1:5algalgs{a}; fcresults.(alg).finalCosts;fprintf(%-8s %8.0f %8.0f %8.0f %8.0f\n, alg, min(fc), max(fc), mean(fc), std(fc));endsave(results/urban_map_results.mat,results,algs,N_RUNS,MaxIt);%% Helperfunction [X1,X2,X3]GWOu(AP,BP,DP,X,a,VS)A12*a*rand(VS)-a; C12*rand(VS); X1AP-A1.*abs(C1.*AP-X);A22*a*rand(VS)-a; C22*rand(VS); X2BP-A2.*abs(C2.*BP-X);A32*a*rand(VS)-a; C32*rand(VS); X3DP-A3.*abs(C3.*DP-X);end4. 参考文献Details of the algorithm are described in the paper: Thu Hang Khuat, Duy-Nam Bui, Hoa TT. Nguyen, Mien L. Trinh, Minh T. NguyenManh Duong Phung,Multi-goal Rapidly Exploring Random Tree with Safety and Dynamic Constraints for UAV Cooperative Path Planning, IEEE Transactions on Vehicular Technology, 2025. Link to the paper:https://doi.org/10.1109/TVT.2025.3560658and its preprinthttps://doi.org/10.48550/arXiv.2504.11823更多免费数学建模和仿真教程关注领取如果觉得内容不错那就请分享和点个“在看”呗