A* 演算法的MATLAB代碼詳解（包教包會）
本文中代碼參考A*演算法的Matlab實作，并對其做出了進一步修改，對A* 演算法的matlab代碼進行詳細注釋，修改繪圖顯示結果，幫助剛接觸的小白同學能更好的學習，
這里只介紹A* 演算法的代碼部分，對于A* 演算法的理論基礎后面有時間會單獨寫一篇博客來介紹，
目錄
- 主程式：
- 1.PlotGrid
- 2. MotionModel
- 3.Manhattan_cost
- 4.isopen
- 5.isObstacle
- 6.GetPath
- 7.GetObstacle
- 8.GetBoundary
- 9.FindList
- 10.FillPlot
- 運行結果圖：
主程式：
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實作
轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/227160.html
標籤：AI