路径规划A*算法解决三维路径规划问题

Posted 2021-06-27 MatlabQQ1575304183

A*算法

A算法是一种典型的启发式搜索算法，建立在Dijkstra算法的基础之上，广泛应用于游戏地图、现实世界中，用来寻找两点之间的最短路径。A算法最主要的是维护了一个启发式估价函数，如式(1)所示。

f(n)=g(n)+h(n)(1)

其中，f(n)是算法在搜索到每个节点时，其对应的启发函数。它由两部分组成，第一部分g(n)是起始节点到当前节点实际的通行代价，第二部分h(n)是当前节点到终点的通行代价的估计值。算法每次在扩展时，都选取f(n)值最小的那个节点作为最优路径上的下一个节点。

在实际应用中，若以最短路程为优化目标，h(n)常取作当前点到终点的欧几里得距离(Euclidean Distance)或曼哈顿距离(Manhattan Distance)等。若令h(n)=0，表示没有利用任何当前节点与终点的信息，A*算法就退化为非启发的Dijkstra算法，算法搜索空间随之变大，搜索时间变长。

A*算法步骤如下，算法维护两个集合：P表与Q表。P表存放那些已经搜索到、但还没加入最优路径树上的节点；Q表维护那些已加入最优路径树上的节点。

（1）P表、Q表置空，将起点S加入P表，其g值置0，父节点为空，路网中其他节点g值置为无穷大。

（2）若P表为空，则算法失败。否则选取P表中f值最小的那个节点，记为BT，将其加入Q表中。判断BT是否为终点T，若是，转到步骤（3）；否则根据路网拓扑属性和交通规则找到BT的每个邻接节点NT，进行下列步骤:

①计算NT的启发值

f(NT)=g(NT)+h(NT)(2)

g(NT)=g(BT)+cost(BT, NT)(3)

其中,cost(BT, NT)是BT到NT的通行代价。

②如果NT在P表中，且通过式(3)计算的g值比NT原先的g值小，则将NT的g值更新为式(3)结果，并将NT的父节点设为BT。

③如果NT在Q表中，且通过式(3)计算的g值比NT原先的g值小，则将NT的g值更新为式(3)结果，将NT的父节点设为BT，并将NT移出到P表中。

④若NT既不在P表，也不在Q表中，则将NT的父节点设为BT，并将NT移到P表中。

⑤转到步骤（2）继续执行。

（3）从终点T回溯，依次找到父节点，并加入优化路径中，直到起点S，即可得出优化路径。

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;

​