做3d动画视频接私活的网站,南通网站建设电话,网站群的建设,信息类网站建设基于粒子群算法对UAV进行三维路径规划--matlab版本 设置粒子群算法的相关参数#xff0c;迭代次数为100#xff0c;粒子数为50#xff0c;惯性权重为1.2#xff0c;学习因子c1c21.5#xff0c;路径节点为4个#xff08;可自行增加或减少#xff09;#xff0c;其中每个…基于粒子群算法对UAV进行三维路径规划--matlab版本 设置粒子群算法的相关参数迭代次数为100粒子数为50惯性权重为1.2学习因子c1c21.5路径节点为4个可自行增加或减少其中每个节点为三维坐标xyz为了使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 i1: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 i1: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 iter1:iter_num for i1: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 i1: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学习因子c1c21.5路径节点为4个可自行增加或减少其中每个节点为三维坐标xyz为了使UAV轨迹光滑利用三次样条拟合散点对路径节点进行处理。 最后需要判断路径是否与障碍相交若相交则设置一个比较大的惩罚项反之保存当前适应度和路径节点本代码的目标函数包括路径的总长度和路径节点距离地面的高度均为越小越好 下图为优化的路径结果图。