A*算法简介-matlab篇

如果你对A*多少有过了解，但是不知道如何编程，这篇文章可以帮助你；

如果你对A*毫无了解，想熟悉了解下，这篇文章和参考文章可以帮助你；

如果你对A*了如指掌，并且是算法大师，这篇文章帮不到你，请指教一下。

我接下来基本上所有文章都是针对Atsushi Sakai这个作者在GitHub上发布的，添加注释以及自己的一些思考，希望可以帮助到一些新入门的人。希望大家对机器人方面的算法都大致有个了解，然后选择合适的算法，改进下，用到自己的学习中或者工作中去。

A星算法核心公式就是F值的计算：[1]
F = G + H

F - 方块的总移动代价
G - 开始点到当前方块的移动代价
H - 当前方块到结束点的预估移动代价

A*算法的逻辑：[2]

 

function [] = AStarSample()
%AStarSample() A*法による最短?路探索プログラム
%
% Author: Atsushi Sakai
%
% Reference:A*による最短?路探索MATLABプログラム - MY ENIGMA 
%           http://d.hatena.ne.jp/meison_amsl/20140503/1399080847
%
% Copyright (c) 2014, Atsushi Sakai
% All rights reserved.
% License : Modified BSD Software License Agreement

clear all;
close all;
disp('A Star Path Planning start!!');

%参数
p.start=[1,1];  %起始点
p.goal=[10,3];  %目标点
p.XYMAX=11;     %地图的最大高度和宽度

%获取边界数据
obstacle=GetBoundary(p);

%获取障碍物数据与边界数据一起获取
nObstacle=20;%障碍物数量
obstacle=GetObstacle(nObstacle,obstacle,p);

%最短路径生成
path=AStar(obstacle,p);

%创建图
figure(1)
if length(obstacle)>=1
    plot(obstacle(:,1),obstacle(:,2),'om');hold on;
end
plot(p.start(1),p.start(2),'*r');hold on;
plot(p.goal(1),p.goal(2),'*b');hold on;
if length(path)>=1
    plot(path(:,1),path(:,2),'-r');hold on;
end
axis([0-0.5 p.XYMAX+1+0.5 0-0.5 p.XYMAX+1+0.5])
grid on;

end

function path=AStar(obstacle,p)
% 一个通过A *方法找到最短路径的程序
% 返回最短路径的坐标列表

path=[];%路径
%用于在计算过程中存储节点信息[x，y，cost，px（父节点），py（父节点）]存储起始节点
open=[p.start(1) p.start(2) h(p.start,p.goal) p.start(1) p.start(2)];
close=[];%用于存储计算的节点信息

%到相邻节点的运动模型通过更改此参数，可以指定机器人的运动
next=MotionModel();

findFlag=false;%目标发现标志

while ~findFlag
      %如果没有open列表中没有点了，而且还没有找到目标点，则找不到路径。  
      if isempty(open(:,1)) disp('No path to goal!!'); return; end
      %选择成本最低的开放节点，类似于优先队列，把cost最低的点放到最前面
      [Y,I] = sort(open(:,3));
      open=open(I,:);
      
      %目标判断
      if isSamePosi(open(1,1:2),p.goal)  %判断这个点是不是目标点
          disp('Find Goal!!');
          %将目标节点移至“关闭”的开头
          close=[open(1,:);close];open(1,:)=[];
          findFlag=true;
          break;
      end
      
      for in=1:length(next(:,1))
          %计算相邻节点的位置和成本
          m=[open(1,1)+next(in,1) open(1,2)+next(in,2) open(1,3)];%open(1,1:2)作为父节点，计算周边点的位置，权重此时是父节点的权重
          %公式F= G + H;
          %m(3)+next(in,3)相当于G，h(m(1:2),p.goal)相当于H，这里我认为减这一项h(open(1,1:2),p.goal)没有必要，可有可无。
          m(3)=m(3)+next(in,3)+h(m(1:2),p.goal)-h(open(1,1:2),p.goal);%计算成本-h(open(1,1:2),p.goal)
          
          %如果相邻节点是障碍物，则查找下一个节点
          if isObstacle(m,obstacle) continue; end
          
          %在打开和关闭列表中搜索m
          [flag, targetInd]=FindList(m,open,close);

          if flag==1 %如果在open列表上,注意是同一个节点的两个不同路径的估价值
              if m(3)<open(targetInd,3) %如果新得到的cost小于open列表中的cost值
                  open(targetInd,3)=m(3);%更新权重，和父节点
                  open(targetInd,4)=open(1,1);
                  open(targetInd,5)=open(1,2);
              end
          elseif flag==2 %如果在close列表中,注意是同一个节点的两个不同路径的估价值
              if m(3) < close(targetInd,3)%小于之前计算的
                  vpa(m(3),10)
                  vpa(close(targetInd,3),10)
                  %更新父节点
                  close(targetInd,4)=open(1,1);
                  close(targetInd,5)=open(1,2);
                  open=[open; close(targetInd,:)];
                  close(targetInd,:)=[];%转到打开列表
              end
          else %如果两者都不
              %添加到带有父节点索引的打开列表
              open=[open;[m open(1,1) open(1,2)]];
          end
      end

      %相邻节点计算出的打开节点移动到关闭节点
      if findFlag==false
          close=[close; open(1,:)];
          open(1,:)=[];
      end
      
      %路径搜索的步骤动画
%       animation(open,close,p,obstacle);

end

%获取最短路径坐标列表
path=GetPath(close,p.start);

end

function result=h(a,b)
%启发式功能
%这里，二维空间中a和b之间的范数距离
result=norm(a-b);

end

function obstacle=GetObstacle(nob,obstacle,p)
%创建一些由随机数指定的障碍，
%一个函数，返回除起始点或目标点之外的任何东西

%用随机数创建障碍
ob=round(rand([nob,2])*p.XYMAX);

%忽略是否在开始和目标处设置障碍
removeInd=[];%用于存储要删除的障碍物索引的列表
for io=1:length(ob(:,1))
    if(isSamePosi(ob(io,:),p.start) || isSamePosi(ob(io,:),p.goal))
        removeInd=[removeInd;io];
    end   
end
ob(removeInd,:)=[];%清单

obstacle=[obstacle;ob];

end

function result=isSamePosi(a,b)
%确定2x1向量是否相同的函数
result=false;
if a(1)==b(1) && a(2)==b(2)
    result=true;
end
end

function boundary=GetBoundary(p)
% 采集区域边界数据
boundary=[];
for i1=0:(p.XYMAX+1)
    boundary=[boundary;[0 i1]];
end
for i2=0:(p.XYMAX+1)
    boundary=[boundary;[i2 0]];
end
for i3=0:(p.XYMAX+1)
    boundary=[boundary;[p.XYMAX+1 i3]];
end
for i4=0:(p.XYMAX+1)
    boundary=[boundary;[i4 p.XYMAX+1]];
end
boundary=[boundary;[11 11]];
boundary=[boundary;[9 1]];
boundary=[boundary;[10 2]];
boundary=[boundary;[11 3]];
boundary=[boundary;[10 1]];
boundary=[boundary;[11 2]];
boundary=[boundary;[11 1]];

end

function animation(open,close,p,obstacle)
% 顺序显示搜索状态的功能

figure(1)
if length(obstacle)>=1
    plot(obstacle(:,1),obstacle(:,2),'om');hold on;
end
plot(p.start(1),p.start(2),'*r');hold on;
plot(p.goal(1),p.goal(2),'*b');hold on;
plot(open(:,1),open(:,2),'xr');hold on;
plot(close(:,1),close(:,2),'xk');hold on;

axis([0-0.5 p.XYMAX+1+0.5 0-0.5 p.XYMAX+1+0.5])
grid on;
pause;

end

function flag=isObstacle(m,obstacle)

for io=1:length(obstacle(:,1))
    if isSamePosi(obstacle(io,:),m(1:2))
        flag=true;return;
    end
end
flag=false;%不是障碍
end

function next=MotionModel()
%到相邻节点的运动模型通过更改此参数，可以指定机器人的运动
% [x y cost]
next=[1 1 1
      1 0 1
      0 1 1
      -1 0 1
      0 -1 1
      -1 -1 1
      -1 1 1
      1 -1 1];
end

function path=GetPath(close,start)
%获取从开始到目标的坐标列表的功能
ind=1;%目标在关闭列表的顶部
path=[];
while 1
    %在列表中注册坐标
    path=[path; close(ind,1:2)];
    
    %确定您是否已到达起点
    if isSamePosi(close(ind,1:2),start)   
        break;
    end
    
    %在关闭列表中查找父节点
    for io=1:length(close(:,1))
        if isSamePosi(close(io,1:2),close(ind,4:5))
            ind=io;
            break;
        end
    end
end

end

function [flag, targetInd]=FindList(m,open,close)
    targetInd=0;
    %是否在open数组中
    if ~isempty(open)
        for io=1:length(open(:,1))
            if isSamePosi(open(io,:),m(1:2))
                flag=1;
                targetInd=io;
                return;
            end
        end
    end
    %是否在close数组中
    if ~isempty(close)
        for ic=1:length(close(:,1))
            if isSamePosi(close(ic,:),m(1:2))
                flag=2;
                targetInd=ic;
                return;
            end
        end
    end
    %都没有
    flag=3;return;
end

这是matlab版本的，他后续的还有个python版本，等我注释都写好后，再发上来。我还准备了一个公众号，到时候把二维码贴上来。方便大家接收更新。

大家有不明白的可以留言，重点区域再更新下。

参考文献

[1] https://www.cnblogs.com/leoin2012/p/3899822.html

[2] Youtube视频