无人机三维路径规划这事儿听起来挺玄乎，但用粒子群算法整起来还挺有意思。咱们直接上干货，用Matlab搞个能自动规划路径还能避障的脚本

基于粒子群算法对UAV进行三维路径规划--matlab版本 设置粒子群算法的相关参数，迭代次数为100，粒子数为50，惯性权重为1.2，学习因子c1=c2=1.5，路径节点为4个（可自行增加或减少），其中每个节点为三维坐标（x，y，z），为了使UAV轨迹光滑，利用三次样条拟合散点对路径节点进行处理。 最后，需要判断路径是否与障碍相交，若相交，则设置一个比较大的惩罚项，反之保存当前适应度和路径节点，本代码的目标函数包括路径的总长度和路径节点距离地面的高度，均为越小越好 下图为优化的路径结果图。

先整初始化参数，这里头门道不少。粒子群迭代次数设100次其实有点保守，不过新手先别浪。直接上代码：

%% 参数设置 iter_num = 100; % 老司机可以调大 particle_num = 50; % 粒子数别省内存 w = 1.2; % 惯性权重给高点 c1 = 1.5; c2 = 1.5;% 学习因子要对称 node_num = 4; % 路径节点数，新手建议4个先跑通

路径节点用三维坐标表示，这里得注意z轴是高度。初始化粒子位置的时候要随机撒点，但别让无人机钻地：

% 初始化粒子位置（x,y,z三维坐标） particle_pos = zeros(node_num, 3, particle_num); for i=1:particle_num particle_pos(:,:,i) = [randi([0,100],node_num,1),... % x坐标 randi([0,100],node_num,1),... % y坐标 randi([20,50],node_num,1)]; % z不低于20米 end

适应度函数是关键，既要算路径长度又要看飞行高度。用三次样条插值把离散点连成丝滑曲线：

function fitness = calc_fitness(path) % 三次样条插值 t = 1:size(path,1); xx = spline(t, path(:,1)); yy = spline(t, path(:,2)); zz = spline(t, path(:,3)); % 采样100个点计算长度 tt = linspace(1,size(path,1),100); dx = diff(ppval(xx,tt)); dy = diff(ppval(yy,tt)); dz = diff(ppval(zz,tt)); total_length = sum(sqrt(dx.^2 + dy.^2 + dz.^2)); % 高度惩罚项 height_penalty = sum(max(0, 30 - path(:,3))); % 低于30米要惩罚 fitness = total_length + height_penalty*100; % 障碍物检测（后面补上） if check_collision(path) fitness = fitness + 1e6; % 直接加个百万级惩罚 end end

障碍物检测这块儿简单粗暴点，用球体障碍做个演示：

function collision = check_collision(path) % 设置障碍物位置和半径 obstacles = [40,50,30,15; 70,20,25,10]; % [x,y,z,radius] for i=1:size(obstacles,1) obs = obstacles(i,:); % 计算路径点到障碍物中心的距离 dist = sqrt(sum((path - obs(1:3)).^2, 2)); if any(dist < obs(4)) collision = true; return end end collision = false; end

主循环里粒子更新公式得注意矩阵运算，别用for循环把自己绕进去：

%% 主迭代循环 global_best = Inf; for iter=1:iter_num for i=1:particle_num % 速度更新（注意维度对齐） particle_vel(:,:,i) = w*particle_vel(:,:,i) + ... c1*rand*(particle_best(:,:,i)-particle_pos(:,:,i)) + ... c2*rand*(global_best_pos - particle_pos(:,:,i)); % 位置更新 particle_pos(:,:,i) = particle_pos(:,:,i) + particle_vel(:,:,i); % 边界约束（别飞出地图） particle_pos(:,:,i) = min(max(particle_pos(:,:,i),0),100); end end

跑完迭代后记得把最优路径画出来，用plot3搞个三维可视化：

%% 结果可视化 figure('Color',[0.9 0.9 0.9]) plot3(optimal_path(:,1),optimal_path(:,2),optimal_path(:,3),'r-o','LineWidth',2) hold on % 画障碍物 [x,y,z] = sphere; for i=1:size(obstacles,1) surf(obstacles(i,4)*x + obstacles(i,1),... obstacles(i,4)*y + obstacles(i,2),... obstacles(i,4)*z + obstacles(i,3),... 'FaceAlpha',0.3) end grid on xlabel('X轴'); ylabel('Y轴'); zlabel('高度') title('无人机三维避障路径')

实际跑起来可能会发现，有时候路径会突然抽风绕远路——这通常是惩罚项没设置好导致的。建议调试时先关掉障碍物检测，单独调路径长度和高度权重的比例。另外，节点数增加到6-8个会让路径更灵活，但计算量也会指数级增长，建议根据无人机性能量力而行。

基于粒子群算法对UAV进行三维路径规划--matlab版本 设置粒子群算法的相关参数，迭代次数为100，粒子数为50，惯性权重为1.2，学习因子c1=c2=1.5，路径节点为4个（可自行增加或减少），其中每个节点为三维坐标（x，y，z），为了使UAV轨迹光滑，利用三次样条拟合散点对路径节点进行处理。 最后，需要判断路径是否与障碍相交，若相交，则设置一个比较大的惩罚项，反之保存当前适应度和路径节点，本代码的目标函数包括路径的总长度和路径节点距离地面的高度，均为越小越好 下图为优化的路径结果图。