三维地图中的A星轨迹规划

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); figure(1) mesh(datax,datay,dataz); hold on; figure(1) plot3(x,y,z,'r--*'); axis equal;