手把手教用matlab做無人駕駛(三)-路徑規劃A*演算法
阿新 • • 發佈:2019-01-01
這裡,我們更新主程式如下:
% 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模擬效果:
這個整個程式我會上傳上去,地址如下: