【无人机三维路径规划】基于遗传粒子群算法求解单无人机三维路径规划问题附

1 简介

​

2 部分代码

function printall3D(model,dataa,name)
Coorall=model.Coordinate(model.BestResult.roadall,:);
Coordinate=model.Coordinate;
Gridxyz=model.Gridxy;
Map=model.Map;
Gridwidth=model.Gridwidth;
figure
hold on
for i=1:Gridxyz(1)
for j=1:Gridxyz(2)
for z=1:Gridxyz(3)
if Map(i,j,z)==2
pp=[i-1,j-1,z-1];pa=[i,j,z];
V = [pp;pa(1) pp(2:3);pp(1) pa(2) pp(3);pp(1:2) pa(3);
pa;pp(1) pa(2:3);pa(1) pp(2) pa(3);pa(1:2) pp(3)];
F = [1 2 7 4;1 3 6 4;1 2 8 3;
5 8 3 6;5 7 2 8;5 6 4 7];
patch('Faces',F,'Vertices',V,'FaceColor','r',...
'LineWidth',1.5,'EdgeColor','w');
elseif Map(i,j,z)==1
pp=[i-1,j-1,z-1];pa=[i,j,z];
V = [pp;pa(1) pp(2:3);pp(1) pa(2) pp(3);pp(1:2) pa(3);
pa;pp(1) pa(2:3);pa(1) pp(2) pa(3);pa(1:2) pp(3)];
F = [1 2 7 4;1 3 6 4;1 2 8 3;
5 8 3 6;5 7 2 8;5 6 4 7];
patch('Faces',F,'Vertices',V,'FaceColor','b',...
'LineWidth',1.5,'EdgeColor','w');
end
end
end
end
plot3(Coorall(:,1)-0.5,Coorall(:,2)-0.5,Coorall(:,3)-0.5,'linestyle','-','color','r','linewidth',5);
stcoor=Coordinate(model.StartingP,:);
tercoor=Coordinate(model.TerminalP,:);
pp=stcoor-1;pa=stcoor;
V = [pp;pa(1) pp(2:3);pp(1) pa(2) pp(3);pp(1:2) pa(3);
pa;pp(1) pa(2:3);pa(1) pp(2) pa(3);pa(1:2) pp(3)];
F = [1 2 7 4;1 3 6 4;1 2 8 3;
5 8 3 6;5 7 2 8;5 6 4 7];
l1(1)=patch('Faces',F,'Vertices',V,'FaceColor','k',...
'LineWidth',1.5,'EdgeColor','k');
pp=tercoor-1;pa=tercoor;
V = [pp;pa(1) pp(2:3);pp(1) pa(2) pp(3);pp(1:2) pa(3);
pa;pp(1) pa(2:3);pa(1) pp(2) pa(3);pa(1:2) pp(3)];
F = [1 2 7 4;1 3 6 4;1 2 8 3;
5 8 3 6;5 7 2 8;5 6 4 7];
l1(2)=patch('Faces',F,'Vertices',V,'FaceColor','r',...
'LineWidth',1.5,'EdgeColor','k');
legend(l1,{'起点','终点'})
xlabel( 'X/米')
ylabel( 'Y/米')
zlabel( 'Z/米')
title([name,'栅格图']);
StartingP=dataa.Coorallreal(model.StartingP,:);
TerminalP=dataa.Coorallreal(model.TerminalP,:);
roadalla=dataa.Coorallreal(model.BestResult.roadall,:);
figure
hold on
C = gradient(dataa.ht);
mesh(dataa.xalln,dataa.yalln,dataa.ht,C)
l1(1)=plot3(StartingP(1),StartingP(2),StartingP(3),'linestyle','none','color','k','marker','s','markeredgecolor','k','markerfacecolor','k','markersize',10);
l1(2)=plot3(TerminalP(1),TerminalP(2),TerminalP(3),'linestyle','none','color','r','marker','s','markeredgecolor','r','markerfacecolor','r','markersize',10);
plot3(roadalla(:,1),roadalla(:,2),roadalla(:,3),'linestyle','-','color','r','linewidth',5);
[x ,y, z]=sphere();
for i=1:size(dataa.weapon,1)
l1(3)=surf(dataa.weapon(i,4)*x+dataa.weapon(i,1),dataa.weapon(i,4)*y+dataa.weapon(i,2),...
dataa.weapon(i,4)*z+dataa.weapon(i,3));%绘制半径为2的球
end
xlabel( 'X/米')
ylabel( 'Y/米')
zlabel( 'Z/米')
legend(l1,{'起点','终点','武器威胁区'})
xlim([dataa.Xrange(1),dataa.Xrange(2)]);
ylim([dataa.Yrange(1),dataa.Yrange(2)]);
title([name,'散点图']);
%% 画图
lastall=(roadalla(2:end,:)+roadalla(1:end-1,:))./2;
roadallaall=zeros(size(roadalla,1)*2-1,3);
roadallaall(1:2:end,:)=roadalla;
roadallaall(2:2:end,:)=lastall;
xroadindex=roadallaall(:,1)';
yroadindex=roadallaall(:,2)';
zroadindex=roadallaall(:,3)';
bize=roadallaall(1,:);
for i=2:2:size(roadallaall,1)-3
[xx,yy,zz]=bezier3(xroadindex(i:i+2),yroadindex(i:i+2),zroadindex(i:i+2),10,3);
bize=[bize;[xx',yy',zz']];
end
bize=[bize;roadallaall(end,:)];
figure
hold on
C = gradient(dataa.ht);
mesh(dataa.xalln,dataa.yalln,dataa.ht,C)
l1(1)=plot3(StartingP(1),StartingP(2),StartingP(3),'linestyle','none','color','k','marker','s','markeredgecolor','k','markerfacecolor','k','markersize',10);
l1(2)=plot3(TerminalP(1),TerminalP(2),TerminalP(3),'linestyle','none','color','r','marker','s','markeredgecolor','r','markerfacecolor','r','markersize',10);
plot3(bize(:,1),bize(:,2),bize(:,3),'linestyle','-','color','r','linewidth',5);
[x ,y, z]=sphere();
for i=1:size(dataa.weapon,1)
l1(3)=surf(dataa.weapon(i,4)*x+dataa.weapon(i,1),dataa.weapon(i,4)*y+dataa.weapon(i,2),...
dataa.weapon(i,4)*z+dataa.weapon(i,3));%绘制半径为2的球
end
xlabel( 'X/米')
ylabel( 'Y/米')
zlabel( 'Z/米')
legend(l1,{'起点','终点','武器威胁区'})
xlim([dataa.Xrange(1),dataa.Xrange(2)]);
ylim([dataa.Yrange(1),dataa.Yrange(2)]);
title([name,'散点图']);

3 运行结果

4 参考文献

[1]江冰, 郭彭. 基于粒子群算法的三维无人机路径规划方法及规划系统:, CN112230678A[P]. 2021.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。