【无人机路径规划】基于A*算法求解无人机三维路径规划问题matlab源码

1模型介绍

2 部分代码

clc ; clear all ; [ datax , datay , dataz ]= loadmap ( ’map.xyz’ ); startpoint =[ datax ( 1 , 30 ), datay ( 30 , 1 ), dataz ( 30 , 30 ) +0.1 ]; %起点 endpoint =[ datax ( 1 , 390 ), datay ( 390 , 1 ), dataz ( 390 , 390 ) +0.1 ]; %终点 q = endpoint - startpoint ; q = q / norm ( q ); direction = q ; position = startpoint ; limit =[ pi / 6 , pi / 6 ]; dataz = dataz .* 2.5 ; % r_posi=[300,320]; % R=3; % dataz=radar_pos(r_posi,R,datax,datay,dataz); % r_posi1=[100,120];R1=3; r_posi2 =[ 200 , 150 ]; R2 = 4 ; r_posi3 =[ 300 , 320 ]; R3 = 5 ; r_posi4 =[ 150 , 320 ]; R4 = 3.5 ; r_posi5 =[ 200 , 80 ]; R5 = 4.2 ; % dataz=radar_pos(r_posi1,R1,datax,datay,dataz); dataz = radar_pos ( r_posi2 , R2 , datax , datay , dataz ); dataz = radar_pos ( r_posi3 , R3 , datax , datay , dataz ); dataz = radar_pos ( r_posi4 , R5 , datax , datay , dataz ); dataz = radar_pos ( r_posi5 , R5 , datax , datay , dataz ); lest = sqrt ( sum (( position - endpoint ) .^ 2 )); h = 0.5 ; k = 0 ; while lest > h         k = k +1         route ( k ,:)= position ;       [ open_list , dir_list ]= openlist ( position , direction , h , limit );     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     fate = zeros ( 25 , 3 );         for i = 1 : 25                 xipt = open_list ( i , 1 );         yipt = open_list ( i , 2 );       [ zopt , xind , yind ]= mappoint ( xipt , yipt , datax , datay , dataz );         if ( zopt > open_list ( i , 3 ))             fate ( i ,:)=[ 1 , 1 , 1 ];             open_list ( i ,:)=[ 0 , 0 , 0 ];         end             end     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%         G = k * h ;           H = H_func ( open_list , endpoint );       [ mini , ind ]= min ( H );         lest = sqrt ( sum (( position - endpoint ) .^ 2 ));           F = G + mini ;         position = open_list ( ind ,:);         direction = dir_list ( ind ,:);       end x = route (:, 1 ); y = route (:, 2 ); z = route (:, 3 ) +21 ; figure ( 1 ) mesh ( datax , datay , dataz +20 ); hold on ; % view(0,10) plot3 ( x , y , z , ’r--*’ ); axis equal ;

3 仿真结果

4 参考文献

[1]张帅等. "基于改进A*算法的无人机航迹规划." 飞行力学 3(2016):39-43.

5 代码下载

微信扫一扫赞赏作者 赞赏

长按二维码向我转账

受苹果公司新规定影响，微信 iOS 版的赞赏功能被关闭，可通过二维码转账支持公众号。