這裡，我們更新主程式如下：

%   editor:  Robert.Cao
%   2018.9.1
clc
clear all
close all
disp('A Star Path Planing 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

grid on;
 

其實，這裡與前面對比，主要增加了path=AStar(obstacle,p)這調語句，這個function 函式AStar就是完成A*演算法的程式，我首先給出它的程式，然後來分析它：

function path=AStar(obstacle,p)


path=[];

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
      if isempty(open(:,1)) disp('No path to goal!!');
       return;
      end
       [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)];
          %m(3)=m(3)+next(in,3)+h(m(1:2),p.goal)-h(open(1,1:2),p.goal);
          
          m(3)=next(in,3)+h(m(1:2),p.goal);
          if isObstacle(m,obstacle) continue; end
          
         
          [flag, targetInd]=FindList(m,open,close)
       
          if flag==1 
              disp("cao")
              continue;
          elseif flag==2 
              continue;
          else 
              
              open=[open;[m open(1,1) open(1,2)]]
          end
         end
        if findFlag==false
          close=[close; open(1,:)]
          open(1,:)=[];
      end
    
end

path=GetPath(close,p.start);
            
 

這裡，通過next=MotionModel()去更新狀態，整個狀態完成findFlag在while完成， 

if isempty(open(:,1)) disp('No path to goal!!');
       return;
      end

這個就是判斷有沒有可能到達目標，因為障礙物是隨機產生的，所有是有這個可能的，

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)];
          %m(3)=m(3)+next(in,3)+h(m(1:2),p.goal)-h(open(1,1:2),p.goal);
          
          m(3)=next(in,3)+h(m(1:2),p.goal);
          if isObstacle(m,obstacle) continue; end
          
         
          [flag, targetInd]=FindList(m,open,close)
       
          if flag==1 
              disp("cao")
              continue;
          elseif flag==2 
              continue;
          else 
              
              open=[open;[m open(1,1) open(1,2)]]
          end
         end
        if findFlag==false
          close=[close; open(1,:)]
          open(1,:)=[];
      end

這個是迴圈找到最優的軌跡放到close裡面儲存下來。

最後，看看matlab模擬效果：

這個整個程式我會上傳上去，地址如下：