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
