简介：资源为基于粒子群算法（PSO）的无人机三维路径规划MATLAB实现，面向计算机、电子信息工程、数学等专业的课程设计、期末大作业及毕业设计。代码采用参数化编程，结构清晰、注释齐全，涉及三维环境地图构建、路径适应度函数设计、粒子群主优化流程与轨迹可视化等模块，可帮助读者快速掌握智能优化算法在无人机航迹规划中的实际应用。压缩包共5个文件，包含defMap.m、PSO.m、calFitness.m、plotFigure.m四个MATLAB脚本及一个效果示意图，整体大小仅85KB，轻量易用；其中地图构建、适应度计算、算法主循环和结果绘图分文件组织，便于逐模块理解与二次开发。资源支持MATLAB 2014a、2019a、2021a等常见版本，配有示例数据可直接运行，当前已有87人学习下载。整体而言，这份资源既可作为算法学习入门的参考实现，也可直接改用于无人机避障、三维路径搜索等研究场景，适合需要快速获取可运行代码的用户。
1. 粒子群无人机三维路径规划：跑通代码只是第一步
下载过粒子群无人机三维路径规划MATLAB代码的人大多有同感：双击main就能出图，可一旦换地形、加禁飞区，算法立刻不收敛。粒子群优化算法（PSO）是典型的无梯度群体智能搜索，对代价函数的形式几乎不做限制，地形碰撞、威胁区、转弯角这些没法写成光滑可导表达式的约束都能直接放进目标函数，这是它被大量用在无人机三维路径规划上的根本原因。
这篇从三维空间建模讲到粒子编码，再落到MATLAB实现、威胁区罚函数和路径平滑。适合做课题需要快速出图的同学，也适合做无人机巡检、低空物流仿真预研的工程师。读完你会有能力判断：适应度曲线早熟或震荡时，先改哪个参数，而不是凭感觉乱调权重。
2. 三维路径规划的问题建模与粒子编码方式
2.1 为什么路径要表示成一串离散路径点
PSO的搜索对象是一个定长实数向量，所以三维路径必须先被编码成定长向量。工程上最常见的做法是：把起点到终点的连线在x方向等分，得到K个路径点，起点终点固定不动，中间K-2个点作为自由变量，每次迭代只调整它们的y、z坐标，x坐标不参与搜索。这样种群矩阵、速度矩阵、个体最优矩阵的维度全部固定为N×(K-2)×2，向量化计算和内存管理都很简单。
如果改成自由三维点编码，粒子维度变成(K-2)×3，搜索空间直接扩大50%，而且在起点终点距离较长、地形起伏大的场景里容易生成回头路。回头路本身不是不可接受，但它会让路径长度代价失去单调性，收敛明显变慢，所以默认不推荐。下面是三种常见编码结构的对比，选型时对照自己的地形数据规模判断。
| 编码方式 | 决策维度 | 优点 | 主要缺点 |
|---|---|---|---|
| 固定x，编码[y z] | (K-2)×2 | 维度低，路径单调前进，实现简单 | 对x方向陡峭地形不敏感 |
| 自由三维点[x y z] | (K-2)×3 | 表达能力最强 | 容易回退，搜索空间大 |
| 高度场拟合z=f(x,y) | 网格点数量 | 天然表达无碰撞区域 | 维度随分辨率爆炸，不实用 |
2.2 粒子的解码逻辑与维度计算
假设K=7，表示把路径切成6段，中间有5个自由点，每个自由点编码两个数（y和z），粒子总维度D=(K-2)×2=10。解码时需要把一维向量还原成K×3的路径点矩阵，这一步决定了后面所有代价计算是否正确，也是最容易写错的地方。
function wp = decode(particle, start, goal, K) % particle: 1×(K-2)*2 的行向量，排列顺序为 [y1 z1 y2 z2 ...] xs = linspace(start(1), goal(1), K); % x方向均匀切分 yz = reshape(particle, [], 2); % 每行还原一个中间点的[y,z] wp = zeros(K, 3); wp(1,:) = start; % 固定起点 wp(end,:) = goal; % 固定终点 wp(2:end-1,:) = [xs(2:end-1)', yz]; end这里有一个关键点：MATLAB的reshape按列填充，particle里[y1 z1 y2 z2 ...]这种交错排列会被reshape成两列，第一列全是y，第二列全是z，正好对应中间点矩阵的y、z列。如果你在别的语言里照搬这段逻辑，要先确认数组的存储顺序是行优先还是列优先，否则解出来的路径会完全错位。
2.3 代价函数：长度、地形安全、威胁区三个部分
路径规划的本质是把多目标需求压成一个标量适应度。三维路径规划的代价函数一般由三部分合成：路径总长度、地形净空惩罚、威胁区惩罚。长度项保证航迹经济，地形项保证不撞山，威胁项保证绕开雷达或禁飞圆柱。
function cost = pathCost(wp, xq, yq, Z, threat, p) % wp: K×3 路径点矩阵 % xq, yq, Z: 地形网格坐标和高度 % p: 权重与控制参数结构体 segLen = sqrt(sum(diff(wp,1,1).^2, 2)); % 逐段三维距离 lenCost = sum(segLen); hGrid = interp2(xq, yq, Z, wp(:,1), wp(:,2), 'linear'); dz = hGrid + p.clearance - wp(:,3); % 低于净空越大越危险 terrainCost = sum(max(dz, 0).^2); % 只惩罚穿透部分，平方放大 threatCost = 0; for j = 1:size(wp,1) d = norm(wp(j,:) - threat.center); if d < threat.radius threatCost = threatCost + (threat.radius - d)^2; end end cost = p.wLen * lenCost + p.wTer * terrainCost + p.wThr * threatCost; end地形项用interp2做双线性插值，而不是直接找网格最近点，否则路径点恰好在网格线之间时会突然出现较大的高度突变，适应度面会变得不连续。威胁项采用超出半径距离的平方作为惩罚，好处是粒子离威胁越近代价上升越陡，粒子群能顺着代价梯度方向绕开，如果改成cost=inf这种硬拒绝，所有非法粒子代价完全相同，群体就失去了收敛方向的指引。
3. 用Matlab实现PSO无人机三维路径规划的核心代码
3.1 生成测试地形与威胁区
先从一张可复现的测试地图开始。用MATLAB内置的peaks函数叠加正弦扰动，可以生成一座带有多个起伏山峰的500m×500m地形，高度范围大约在180m到260m之间，贴近真实低空飞行的海拔尺度。
clear; clc; rng(42); [xq, yq] = meshgrid(0:10:500, 0:10:500); Z = 180 + 60 * peaks(51) + 5 * sin(xq/50) .* cos(yq/40); threat = struct('center', [250 250 0], 'radius', 60); start = [30, 30, 190]; goal = [470, 460, 205];rng(42)固定随机种子，保证每次运行的地形和初始粒子群完全一致，这在你调参对比实验结果时是必须的。威胁区定义成一个圆柱体，center的z分量实际不影响二维投影计算，保留0只是为了和路径点的三维坐标格式统一。起点和终点的高度略高于地形最低点，避免初始状态就产生大额惩罚。
3.2 粒子群初始化与参数设置
初始化要做三件事：随机生成种群位置、随机生成初始速度、把个体最优初始化为当前位置。位置需要在每维的合法区间内，速度初始值一般取区间宽度的10%到20%，太大会让前几代粒子直接飞出地图。
N = 50; maxIter = 200; c1 = 1.5; c2 = 1.5; wStart = 0.9; wEnd = 0.4; K = 7; D = (K - 2) * 2; lo = repmat([10 181], 1, K-2); % 交替的y下界、z下界 hi = repmat([490 259], 1, K-2); pos = rand(N, D) .* (hi - lo) + lo; vel = randn(N, D) .* (hi - lo) * 0.1; vmax = (hi - lo) * 0.15; pbest = pos; pbestCost = inf(N, 1);z的下界181m对应地形最低点加5m净空，上界259m对应地形最高点附近。这里的关键是lo和hi必须按维度交替构造，因为粒子向量的排列顺序是[y1 z1 y2 z2 ...]，如果简单写成repmat([10 490], 1, D/2)，y和z的边界就反了，后面全部白跑。
3.3 迭代主循环：速度更新、限幅与边界处理
PSO的核心更新公式是v = w*v + c1*r1*(pbest-pos) + c2*r2*(gbest-pos)，三部分分别代表惯性、个体认知和社会认知。惯性权重w从0.9线性降到0.4，前期待探索、后期重收敛，这是粒子群算法原理里最经典的调法。
fitHist = zeros(maxIter, 1); for iter = 1:maxIter w = wStart - (wStart - wEnd) * iter / maxIter; costAll = zeros(N, 1); for i = 1:N wp = decode(pos(i,:), start, goal, K); costAll(i) = pathCost(wp, xq, yq, Z, threat, p); if costAll(i) < pbestCost(i) pbest(i,:) = pos(i,:); pbestCost(i) = costAll(i); end end [bestCost, idx] = min(pbestCost); gbest = pbest(idx,:); r1 = rand(N, D); r2 = rand(N, D); vel = w .* vel + c1 .* r1 .* (pbest - pos) + c2 .* r2 .* (gbest - pos); vel = max(min(vel, vmax), -vmax); pos = pos + vel; pos = max(min(pos, hi), lo); fitHist(iter) = bestCost; end注意更新顺序：所有粒子的适应度计算和pbest更新必须放在同一轮循环内完成，然后再从pbest集合里取全局最优。如果把min(pbestCost)放进单个粒子的循环里，后面的粒子会用到本轮尚未更新的pbest信息，迭代逻辑就乱了。速度限幅用的是向量化max(min(...))，比逐粒子判断快很多。位置 clamp 到合法区间后，越界的维度直接贴边，等价于硬边界约束。
3.4 结果输出与三维可视化
迭代结束后，把最优粒子解码成路径点并画在地形曲面上。surf配合alpha(0.7)让地形半透明，路径用plot3叠加，这样论文插图和数据报告截图可以直接用。
figure; surf(xq, yq, Z, 'EdgeColor', 'none'); hold on; alpha(0.7); colormap(parula); colorbar; bestWp = decode(gbest, start, goal, K); plot3(bestWp(:,1), bestWp(:,2), bestWp(:,3), 'r-o', 'LineWidth', 2); plot3(start(1), start(2), start(3), 'go', 'MarkerSize', 8); plot3(goal(1), goal(2), goal(3), 'mo', 'MarkerSize', 8); grid on; xlabel('x/m'); ylabel('y/m'); zlabel('z/m'); view(45, 30);如果路径穿过了山头，问题不在画图，而在代价函数权重。下一步就去检查威胁区罚函数和地形惩罚的系数量级，这正是第4章要处理的。
4. 约束处理与参数调优：威胁区罚函数与收敛诊断
4.1 罚函数写法：软惩罚优于硬拒绝
很多初学者把威胁区写成立即丢弃粒子的硬约束，这在粒子群算法原理上是错的。硬拒绝会让大量粒子适应度相同，群体失去比较依据，最后只能靠随机碰撞侥幸绕开威胁。正确做法是把威胁区超界量平方加权加入代价，让代价面保持连续。
function pen = threatPenalty(wp, threats, coeff) pen = 0; for j = 1:size(wp, 1) for t = 1:numel(threats) d = norm(wp(j,:) - threats(t).center); if d < threats(t).radius pen = pen + coeff * (threats(t).radius - d)^2; end end end endcoeff的取值取决于地图尺度。500m地图上路径长度代价在700到900之间，威胁惩罚若取coeff=1，超界10m只产生100的惩罚，路径稍微绕远一点就比穿过威胁区更划算，算法会倾向于穿过去。我一般从coeff=5开始调，看到路径穿插威胁区就翻倍，直到路径稳定绕行为止。
4.2 PSO参数基准值与调节方向
下面是三维路径规划场景下经过多次实验验证的参数基准表，多数情况从这里起步能跑出可用结果，再根据适应度曲线微调。
| 参数 | 基准值 | 调节方向 | 对应症状 |
|---|---|---|---|
| 种群数N | 40～60 | 早熟时加大 | 曲线早期骤停 |
| 最大迭代maxIter | 150～300 | 曲线仍下降时加大 | 迭代末段没收敛 |
| 惯性权重w | 0.9→0.4线性递减 | 探索不足则加大初值 | 过早集中到局部 |
| 个体认知c1 | 1.5 | 陷入局部时加大到2.0 | 群体多样性丢失 |
| 社会认知c2 | 1.5 | 收敛过慢时加大到2.0 | 全局最优牵引不足 |
| 速度上限vmax | 坐标区间宽度的15% | 震荡剧烈时减小到10% | 末期来回摆动 |
| 路径点数K | 6～9 | 地形复杂时加到10以上 | 路径穿山或过扁 |
N和maxIter对结果质量的影响最直接，但计算量线性上涨。vmax是新手最容易忽略的参数，它决定粒子单步最大位移，500m地图上vmax取75m意味着粒子一步能跨过整个威胁区，精细搜索根本谈不上，一般取30m以内。
4.3 从适应度曲线诊断早熟与震荡
运行完代码后，直接画semilogy(fitHist)，纵轴取对数能让小幅度变化也看得清楚。三种典型形态对应三种处理方式。
第一种是前20代骤降后变平线，这是早熟，群体过早集中到某个局部最优。优先把N加到80，再把c1调到1.8增强个体探索，同时检查惩罚系数是否过大——如果wTer取到几十，合法区域的代价远小于非法区域，粒子一旦合法就不敢动了，这属于代价函数把搜索空间压成了悬崖。
第二种是全程剧烈锯齿，每代gBest上下跳。这是w过大或vmax过大造成的，把wStart降到0.8、vmax缩小到10%即可。
第三种是缓慢平滑下降但迭代结束还在降，说明没有完全收敛，直接加大maxIter到300，通常不需要动其他参数。
提示：调参时一次只改一个参数，用rng固定随机种子后对比适应度曲线。同时改多个参数即便结果变好，你也不知道是哪一步起了作用。
5. 路径平滑与可行性验证：从折线到可飞行航迹
5.1 样条平滑代码
PSO输出的是折线，无人机无法如此机动。常见做法是用三次样条对x、y、z三个分量分别插值，把K个路径点扩充成200个连续航迹点。
tq = linspace(1, K, 200); sp = spline(1:K, bestWp'); % 对三个维度分别插值 smoothWp = ppval(sp, tq)';平滑后必须重新检查地形净空，因为样条在路径点之间可能插出低于地形的高度。检查方法是把平滑轨迹再送入第2章的interp2地形插值，逐点对比z值。
5.2 可行性验证的三个检查点
第一检查净空：min(smoothWp(:,3) - hSmooth) >= clearance不满足就整体抬升轨迹或增加K。第二检查威胁区：逐点计算到威胁圆心的二维距离。第三检查转弯角，逐点用前后向量的夹角判断机动可行性。
for i = 2:size(smoothWp,1)-1 v1 = smoothWp(i,:) - smoothWp(i-1,:); v2 = smoothWp(i+1,:) - smoothWp(i,:); cosA = sum(v1.*v2) / (norm(v1)*norm(v2)); turnAng(i) = acosd(max(min(cosA,1),-1)); end if max(turnAng) > 60, warning('转弯过急，需增加路径点数量'); end5.3 多轮统计与算法边界确认
只跑一次不能说明算法有效。固定随机种子连跑10次，记录最优代价的均值与标准差，标准差超过均值10%说明参数偏激进，加大N或maxIter。与A*、RRT等算法对比时记住边界：PSO给出的是全局意义上平滑的次优路径，没有完备性保证，窄走廊场景容易失效；RRT快但路径毛糙。固定好随机种子，把10次统计结果连同适应度曲线和三维轨迹图一起存档，这就是一条完整的粒子群无人机三维路径规划验证流程，可以直接支撑技术报告或课题结题。
本文还有配套的精品资源，点击获取
