【航迹规划】Astar算法三维航迹规划【Matlab 309期】

一、简介

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，即可得出优化路径。

二、源代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A* ALGORITHM Demo
% Interactive A* search demo
% 04-26-2005
%   Copyright 2009-2010 The MathWorks, Inc.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%DEFINE THE 2-D MAP ARRAY
% 
% n=input('Please insert the obstacle numbers');
% 
% for i=1:n
% 
%     ab=input('Please insert the obstacle numbers')
%     obstacles(i,:)
clear all
close all
clc
tic
%加载地形数据
data1=load('newMoun300.dat');
l = 100;%地形数据长,即y轴数据长度
w = 150;%地形数据宽，即x轴数据长度
data=reshape(data1,l,w); 
mapstep = 5;%网格大小
nl = l/mapstep;
nw = w/mapstep;%nw最大值30
OPEN_COUNT=0;
CLOSED_COUNT=0;
MAP=2*(ones(nw,nl));
for z=1:1:3
% data(nl,nw);
z
p0.x = 20;
p0.y = 60;
p0.z = 1754;%起点坐标
xval=p0.x/mapstep;
yval=p0.y/mapstep;
MAP(p0.x/mapstep,p0.y/mapstep)=1;
% xval=p0.x;
% yval=p0.y;
% MAP(p0.x,p0.y)=1;
xStart=xval;%Starting Position起始点1
yStart=yval;%Starting Position
p1.x = 120;

p1.y = 60;
p1.z = 1342;%终点坐标

xTarget=floor(p1.x/mapstep);
yTarget=floor(p1.y/mapstep);
% zTarget=floor(p1.z);
% 
MAP(p1.x/mapstep,p1.y/mapstep)=0;
% xTarget=floor(p1.x);
% yTarget=floor(p1.y);
% % zTarget=floor(p1.z);
% 
% MAP(p1.x,p1.y)=0;

% maxpoint = (p1.x-p0.x)/mapstep;%最大点个数clc

   
   switch z
       case 1
            MAP=dataprocess(MAP);
% %             dataprocess(nw,nl,mapstep);%处理地形数据 dataprocess;%处理地形数据 
% i=1
%  for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=-1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%              continue;
       case 2
%             dataprocess1(nw,nl,mapstep);%处理障碍物1地形数据
MAP=dataprocess1(MAP);%处理地形数据 
% i=2
% for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=-1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%                 for i1 = 9:1:11
%                 for j1 = 13:1:15
%                    MAP(i1,j1)=-1;
% 
%                 end
%                 end
%                 continue;
              
  case 3
%            dataprocess2(nw,nl,mapstep);%处理障碍物2地形数据 
MAP=dataprocess2(MAP);%处理地形数据 
% i=3
%  for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=-1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%                 for i1 = 19:1:21
%                 for j1 = 11:1:13
%                    MAP(i1,j1)=-1;
% 
%                 end
%                 end
%               continue;
      
   end

%   for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=-1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%             

%  for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=-1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%                 for i1 = 9:1:11
%                 for j1 = 13:1:15
%                    MAP(i1,j1)=-1;
% 
%                 end
%                 end
              
%  for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=-1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end
%                 for i1 = 19:1:21
%                 for j1 = 11:1:13
%                    MAP(i1,j1)=-1;
% 
%                 end
%               end





OPEN=[];
%CLOSED LIST STRUCTURE
%--------------
%X val | Y val |
%--------------
% CLOSED=zeros(MAX_VAL,2);
CLOSED=[];

%Put all obstacles on the Closed list
k=1;%Dummy counter
for i=1:nw
    for j=1:nl
        if(MAP(i,j) == -1)
            CLOSED(k,1)=i;
            CLOSED(k,2)=j;
            k=k+1;
        end
    end
    end%检测所有的障碍物，放入closed表
CLOSED_COUNT=size(CLOSED,1);
%set the starting node as the first node设置初始节点为第一节点
xNode=xval;
yNode=yval;
OPEN_COUNT=1;
path_cost=0;%h
goal_distance=distance(xNode,yNode,xTarget,yTarget);
OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distance);%将起始点插入open表中，并置open表的第个元素为0，放入close表中。*从open表撤离放入close表时，open表中对应第一个元素置0
OPEN(OPEN_COUNT,1)=0;
CLOSED_COUNT=CLOSED_COUNT+1;
CLOSED(CLOSED_COUNT,1)=xNode;
CLOSED(CLOSED_COUNT,2)=yNode;
NoPath=1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% START ALGORITHM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while((xNode ~= xTarget || yNode ~= yTarget) && NoPath == 1)
%  plot(xNode+.5,yNode+.5,'go');
 exp_array=expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,nw,nl);
 exp_count=size(exp_array,1);
 %UPDATE LIST OPEN WITH THE SUCCESSOR NODES
 %OPEN LIST FORMAT
 %--------------------------------------------------------------------------
 %IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
 %--------------------------------------------------------------------------
 %EXPANDED ARRAY FORMAT
 %--------------------------------
 %|X val |Y val ||h(n) |g(n)|f(n)|
 %--------------------------------
 for i=1:exp_count
    flag=0;
    for j=1:OPEN_COUNT
        if(exp_array(i,1) == OPEN(j,2) && exp_array(i,2) == OPEN(j,3) )
            OPEN(j,8)=min(OPEN(j,8),exp_array(i,5));
            if OPEN(j,8)== exp_array(i,5)%当前节点的扩展节点与open表中坐标相同的元素比较，如果扩展节点的f小，则更新open表中相应元素的父节点，和代价值
                %UPDATE PARENTS,gn,hn
                OPEN(j,4)=xNode;
                OPEN(j,5)=yNode;
                OPEN(j,6)=exp_array(i,3);
                OPEN(j,7)=exp_array(i,4);
            end;%End of minimum fn check
            flag=1;
        end;%End of node check
%         if flag == 1
%             break;
    end;%End of j for

三、运行结果

在这里插入图片描述

四、备注