【无人机三维路径规划】基于遗传粒子群算法求解单无人机三维路径规划问题含危险障碍附matlab代码

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, ’散点图’ ]);