评论

收藏

[C++] 【三维路径规划】基于matlab A_star算法三维路径规划【含Matlab源码 003期】

编程语言 编程语言 发布于:2021-07-05 18:14 | 阅读数:447 | 评论:0

  
一、简介
  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;

三、运行结果
DSC0000.png


四、备注
  版本:2014a

  
关注下面的标签,发现更多相似文章