A* 算法的MATLAB代碼詳解（包教包會）node
本文中代碼參考A*算法的Matlab實現，並對其作出了進一步修改，對A* 算法的matlab代碼進行詳細註釋，修改繪圖顯示結果，幫助剛接觸的小白同窗能更好的學習。
這裏只介紹A* 算法的代碼部分，對於A* 算法的理論基礎後面有時間會單獨寫一篇博客來介紹。
web
目錄
主程序：
function main()
clear;
clc;
tic%計時開始
open1=[];
disp('A Star Path Planing start!!')
obstacle=[];
map.XYMAX=20; %%表明咱們要畫一個地圖的長和寬
map.start=[1,20];%起始點 注意必須在地圖範圍內
map.goal=[20,5]; %目標點 注意必須在地圖範圍內
obstacle=GetBoundary(map);%獲得邊界數據
%obstacle=[obstacle;1,6;1,7;18,1;4,19;5,19;5,18;6,18;18,12;19,12;19,13;19,11;11,6;7,1;8,1;8,2;2,12;2,13;2,14;2,15;3,13;11,5;12,5;12,4;12,3;13,3;14,3;17,8;18,8;18,7;18,6;17,6;16,6;4,13;5,13;11,20;11,19;12,19;9,14;10,14;10,13;11,12;10,12;15,18;16,18;16,17;18,16;17,15;14,10;14,9;14,11;14,12;15,10;13,10;3,2;3,3;6,7;6,8;5,8;7,7;7,6;];
nObstacle=50;%在地圖中隨機加入XX個障礙物
obstacle=GetObstacle(nObstacle,obstacle,map);%障礙物和邊界座標
FillPlot(obstacle,'k',1);%畫出障礙點
hold on;%保持圖形不關閉
path=AStar(obstacle,map);%A*算法
if length(path)>=1
plot(path(:,1),path(:,2),'-c','LineWidth',5);hold on;
end
toc%計時結束
function path=AStar(obstacle,map)%算法的主程序
%用於存儲路徑
goal=[map.goal,1,1,1,1];
path=[];
open=[];%建立OpenList
close=[];%建立CloseList
findFlag=false;%用於判斷while循環是否結束
%================1.將起始點放在Openlist中======================
%open變量每一行節點座標，代價值F=G+H,代價值G,父節點座標]
open =[map.start(1),map.start(2),0+Manhattan_cost(map.start,map.goal),0,map.start(1),map.start(2)];
next=MotionModel();%更新狀態--下一步的八個點
openlist=[];
%=======================2.重複如下過程==============================
while ~findFlag
%--------------------首先判斷是否達到目標點，或無路徑-----
if isempty(open(:,1))
disp('No path to goal!!');
return;
end
%判斷目標點是否出如今open列表中
[isopenFlag,Id]=isopen(map.goal,open);
if isopenFlag
disp('Find Goal!!');
close = [open(Id,:);close];
findFlag=true;
break;
end
%按照Openlist中的第三列（代價函數F）進行排序，查找F值最小的節點
[~,I] = sort(open(:,3)); %對OpenList中第三列排序
open=open(I,:);%open中第一行節點是F值最小的
%將F值最小的節點(即open中第一行節點)，放到close第一行(close是不斷積壓的)，做爲當前節點
close = [open(1,:);close];
current = open(1,:);
open1=[open;open1];
open(1,:)=[];%由於已經從open中移除了，因此第一行須要爲空
%對當前節點周圍的8個相鄰節點進行處理
for in=1:length(next(:,1))
%得到相鄰節點的座標,代價值F先等於0,代價值G先等於0 ,後面兩個值是其父節點的座標值，暫定爲零(由於暫時還沒法判斷其父節點座標是多少)
m=[current(1,1)+next(in,1),current(1,2)+next(in,2),0,0,0,0];
m(4)=current(1,4)+next(in,3); % m(4)相鄰節點G值
m(3)=m(4)+Manhattan_cost(m(1:2),map.goal);%m(3)相鄰節點F值
openlist=[openlist;m(:,1:2)];
%>>若是它不可達，忽略它，處理下一個相鄰節點
if isObstacle(m,obstacle)
continue;
end
[flag,targetInd]=FindList(m,open,close);
%>>若是它在Closelist中，忽略此相鄰節點
if flag==1
continue;
%>>若是它不在Openlist中，加入Openlist,並把當前節點設置爲它的父節點
elseif flag==2
m(5:6)=[current(1,1),current(1,2)];%將當前節點做爲其父節點
open = [open;m];%將此相鄰節點加放openlist中
%>>剩下的狀況就是它在Openlist中，檢查由當前節點到相鄰節點是否更好，若是更好則將當前節點設置爲其父節點，並更新F,G值；不然不操做
else
%由當前節點到達相鄰節點更好(targetInd是此相鄰節點在open中的行號 此行的第3列是代價函數F值)
if m(3) < open(targetInd,3)
%更好，則將此相鄰節點的父節點設置爲當前節點，不然不做處理
m(5:6)=[current(1,1),current(1,2)];%將當前節點做爲其父節點
open(targetInd,:) = m;%將此相鄰節點在Openlist中的數據更新
end
end
%下面的end是判斷八個相鄰節點的for循環的end
end
end
PlotGrid(map);%繪製地圖
FillPlot(openlist,'r',0.3);%畫出搜索過的節點，透明度爲0.3，顏色爲紅色
end
closed=close(:,1:6);%屢次一舉的行爲，可是沒有又不行
path=GetPath(close,map.start,map.goal);
end
調用到的其餘函數以下：算法
1.PlotGrid
function PlotGrid(map)%繪製網格,傳入參數爲地圖大小、起點和目標點等信息 for i = 1:map.XYMAX+3%在地圖周圍有一圈圍牆，因此是地圖大小+3 line([-0.5,map.XYMAX+1.5],[i-1.5,i-1.5]);%劃線，重點是line函數的參數用法 end for j = 1:map.XYMAX+3 line([j-1.5,j-1.5],[-0.5,map.XYMAX+1.5]); end plot(map.start(1),map.start(2),'og','MarkerSize',13,'LineWidth',3);%畫出起點圈圈 hold on; plot(map.goal(1),map.goal(2),'ob','MarkerSize',13,'LineWidth',3);%畫出目標點圈圈 axis([-1.5,map.XYMAX+2.5,-1.5,map.XYMAX+2.5]);%設置座標軸 axis equal; end
2. MotionModel
function next = MotionModel()
%調用這個函數目的是找到當前點周圍八個點的座標和移動到該點的代價值
next = [-1,1,14;...%表明當前位置左上角的柵格，移動到改柵格的代價值是14（對角線距離）
0,1,10;...
1,1,14;...
-1,0,10;...
1,0,10;...
-1,-1,14;...
0,-1,10;...
1,-1,14];
end
3.Manhattan_cost
function cost =Manhattan_cost(m,goal) %計算啓發函數代價值 ，這裏採用曼哈頓算法 cost=10*abs(m(1)-goal(1))+10*abs(m(2)-goal(2)); end
4.isopen
function [isopenFlag,Id] = isopen(goal,open)
%判斷目標點否在open列表中，在open中，isopenFlag = 1,不在open中，isopenFlag = 0 .並反回索引號
isopenFlag = 0;
Id = 0;%初始化
if isempty(open)%若是open列表爲空，則不在open列表中
isopenFlag = 0;
else %open列表不爲空時
for i = 1:length(open(:,1))%列表中有多少個座標就循環多少次
if isequal(goal(1:2),open(i,1:2))%在Openlist中
isopenFlag = 1;
Id = i;
return;
end
end
end
end
5.isObstacle
function flag=isObstacle(m,obstacle)
%判斷節點m是否爲障礙點，若是是就返爲1，不是就返回0
for i=1:length(obstacle(:,1))%對全部的障礙物挨個判斷
if isequal(obstacle(i,:),m(1:2))
flag=true;
return;
end
end
flag=false;
end
6.GetPath
function path=GetPath(close,start,goal)%傳入參數爲close列表、起點和目標點
ind=1;
ik=-1;
path=[];
while 1%這部分將close中的行數據進行清洗，目的是在全部的close座標中找出一條最短路徑
path=[path; close(ind,1:2)];%把close裏面的路徑座標提取出來
if isequal(close(ind,1:2),start) %判斷跟起點是否相同
break;
end
for io=1:length(close(:,1))%這個循環的目的是找出相同的座標點
if isequal(close(io,1:2),close(ind,5:6))
ik=ik+1;
ind=io;
break;
end
end
end
next1=MotionModel();%當前點的八鄰域
lujing=[close(1,5:6)];%取目標點前一個依次往回搜
balinyu=[];
jiaoji=[];%這些變量都是起到存放值的做用而已
current=[close(1,5:6),close(1,3:4),close(1,5:6)];
ha=length(close(:,1));
while 1
for in=1:length(next1(:,1)) %此處和以前的正序搜索同樣的
m=[current(1,1)+next1(in,1) , current(1,2)+next1(in,2) , 0 , 0 , 0 ,0];
m(4)=current(1,4)+next1(in,3); % m(4) 相鄰節點G值
m(3)=m(4)+Manhattan_cost(m(1:2),start);% m(3) 相鄰節點F值
balinyu=[balinyu;m];%找出當前點的八鄰域座標，下一步是找在close裏的
end
jiao=ismember(close(:,1:2),balinyu(:,1:2),'rows');%找出八鄰域與close列表的的交集
for ppp=1:length(close(:,1))
if jiao(ppp,1)==1
jiaoji=[close(ppp,:);jiaoji];%堆疊
end
end
jiaoji=sortrows(jiaoji,4); %對OpenList中第三列排序
lujing=[jiaoji(1,1:2);lujing];%第一行節點是F值最小的
current =jiaoji(1,1:6);
balinyu=[];
jiaoji=[];
if ha==1%搜索次數沒了
break;
else
ha=ha-1;
end
end
lujing=unique(lujing,'rows');%刪除重複座標
lujing=[lujing;goal];%加上目標點纔是完整的
for pp=1:length(lujing(:,1))
x = lujing(pp,1);
y = lujing(pp,2);
A(pp,1)=x;
A(pp,2)=y;%這部分是將路徑座標拿出來另外存放
end
plot( A(:,1), A(:,2),'b','linewidth',4)%繪製路線
B=length(close(:,1))-1;
node=B*8;%close中每有一個元素，就說明在open裏面有8個元素
disp(['搜索的節點數爲：',num2str(node)]);
k=0;%用於存放路徑長度的變量
for i=1:length(lujing(:,1))-1
b=10*sqrt(((lujing(i+1,1)-lujing(i,1))^2)+((lujing(i+1,2)-lujing(i,2))^2));%簡單的兩點間距離公式
k=k+b;%路徑長度值的逐個累加
end
disp(['路徑長度爲',num2str(k)]);
axis tight;
end
7.GetObstacle
function obstacle=GetObstacle(nob,obstacle,map)
%生成障礙點的座標
ob=round(rand([nob,2])*map.XYMAX);%隨機生產障礙點
removeInd=[];
for io=1:length(ob(:,1))%遍歷ob數組，檢查哪些座標與start和goal重合，並將其索引存在removeInd中
if(isequal(ob(io,:),map.start) || isequal(ob(io,:),map.goal))
removeInd=[removeInd;io];
end
end
ob(removeInd,:)=[];%刪除重複的節點
obstacle=[obstacle;ob];%將ob障礙點加入到obstacle中
end
8.GetBoundary
function boundary=GetBoundary(map)
%得到地圖的邊界的座標
boundary=[];
for i1=0:(map.XYMAX+1)
boundary=[boundary;[0 i1]];
end
for i2=0:(map.XYMAX+1)
boundary=[boundary;[i2 0]];
end
for i3=0:(map.XYMAX+1)
boundary=[boundary;[map.XYMAX+1 i3]];
end
for i4=0:(map.XYMAX+1)
boundary=[boundary;[i4 map.XYMAX+1]];
end
end
9.FindList
function [flag,targetInd]=FindList(m,open,close)
%{
函數功能：
若是相鄰節點(m存儲其信息) 已經在Closelist中，則flag = 1 targetInd = 其所在close的行數，用來定位
若是相鄰節點(m存儲其信息) 不在Openlist 中，則flag = 2 targetInd = []
若是相鄰節點(m存儲其信息) 已經在Openlist 中，則flag = 3 targetInd = 其所在open的行數，用來定位
%}
%若是openlist爲空，則必定不在openlist中
if isempty(open)
flag=2;
targetInd=[];
else %open不爲空時，須要檢查是否在openlist中
%遍歷openlist，檢查是否在openlist中
for io=1:length(open(:,1))
if isequal(m(1:2),open(io,1:2))%在Openlist中
flag=3;
targetInd=io;
return;
else %不在Openlist中
flag=2;
targetInd=[];
end
end
end
%若是能到這一步，說明： 必定不在Openlist中 那麼須要判斷是否在closelist中
%遍歷Closelist（注意closelist不可能爲空）
for ic = 1:length(close(:,1))
if isequal(m(1:2),close(ic,1:2))%在Closelist中
flag=1;
targetInd=ic;
return;%在Closelist中直接return
end
end
end
10.FillPlot
function FillPlot(coord,color,tp)%傳入參數爲填充點座標、填充顏色和透明度
for i = 1:length(coord(:,1))%求出第一列的長度，有多長就循環填充多少次
x = coord(i,1);%將第i個障礙物的座標賦給x、y而後對他們填充
y = coord(i,2);
X = [x-0.5,x+0.5,x+0.5,x-0.5];%填充的大小
Y = [y-0.5,y-0.5,y+0.5,y+0.5];
fill(X,Y,color,'EdgeColor','none','FaceAlpha',tp);%填充障礙物座標所在柵格
set(gca,'FontSize',40,'Fontname', 'Times New Roman');%設置字體以及字號
set(gca,'XTick',0:5:20);
set(gca,'YTick',0:5:20);%設置座標軸刻度
hold on;
end
axis tight;
end
運行結果圖：
參考資料：
路徑規劃A算法matlab代碼註釋
A算法的Matlab實現
數組