基于A*算法的路徑規(guī)劃實踐
1. 機器人路徑優(yōu)化問題描述
1.1 機器人路徑優(yōu)化問題綜述
? ?路徑最優(yōu)規(guī)劃是移動機器人系統(tǒng)中最重要的組成部分之一,分為點到點的路徑規(guī)劃和全覆蓋路徑規(guī)劃。點到點的路徑規(guī)劃是按照走路線最短、行走時間最短等一定的評價標準進行，在其工作空間中找到一條從起始點到目標點的能避開障礙物的最優(yōu)路徑。根據(jù)是否給定全局的環(huán)境可主要劃分為全局路徑規(guī)劃和局部路徑規(guī)劃。本文主要研究給定環(huán)境下進行的路徑的規(guī)劃，在進行全局路徑規(guī)劃過程中,要找到一條從起點到終點的最優(yōu)路徑,首先進行整體環(huán)境建模,在此基礎上通過搜索算法進行最短路徑規(guī)劃。
? ?在給定環(huán)境的情況下需要對全局環(huán)境進行建模，環(huán)境建模方法通常有柵格法、機器視覺法和拓撲圖法等,在進行環(huán)境建模時通常采用其中一種或多種方法結(jié)合的建模方法。其中柵格法是最常見的建模方法,通過給定的環(huán)境進行柵格量化機器人和障礙物位置可以用柵格的坐標來描述,編程易實現(xiàn)。
1.2 環(huán)境建模方法
? ?環(huán)境建模方法通常有機器視覺法、拓撲圖和法柵格法等。
? ?機器視覺法是機器人將采集到的環(huán)境地圖信息發(fā)送給計算機終端，計算機對圖像進行分析處理與識別，將結(jié)果反饋給機器人,其優(yōu)點處理精度高,處理內(nèi)容豐富,可進行復雜的非線性處理,?有靈活的變通能力, 一般來說只要改變軟件就可以改變處理內(nèi)容。其缺點是處理速度還是一個問題,?特別是進行復雜的處理更是如此。
? ?柵格法是將機器人和工作環(huán)境量化在相鄰單元的小格中，并建立坐標。機器人初始位置置于起點處,以機器人為中心形成上、下、左、右、左上、左下、右上、右下的八連通柵格。連接機器人柵格頂點到障礙物外可視頂點形成可視線,這樣從起點到目標點就有多條可搜索路徑。
1.3 機器人路徑優(yōu)化解決方法
? ?現(xiàn)在對于一直工作環(huán)境的路徑規(guī)劃常見的算法有遺傳算法、粒子群算法、蟻群算法、人工勢場法、模糊規(guī)則法、神經(jīng)網(wǎng)絡、模擬退火算法、快速擴展隨機樹、A-Star算法等。這些算法各有特點，有優(yōu)點也有缺點。這些方法大部分都需要在一個確定的空間內(nèi)對障礙物進行建模，計算復雜度與機器人自由度呈指數(shù)關系，并通過對狀態(tài)空間中的采樣點進行碰撞檢測，來路徑規(guī)劃問題。本文主要采用對A-Star算法改進進行對路徑進行優(yōu)化，并會與其他的一種方法進行比較。
2. A*算法介紹
2.1 算法綜述
? ?A-Star算法是一種基于啟發(fā)式搜索的一種最好優(yōu)先的算法，同時又加上了一些約束條件。在狀態(tài)空間中,對每一個要搜索的位置進行評估,得到最好的位置。再以這個位置進行搜索,直到到達目標位置。這樣可以減少搜索范圍,降低問題復雜度,從而提高了效率。在啟發(fā)式搜索中,對位置的估價,需要用到估價函數(shù),這是十分重要的。啟發(fā)式搜索在搜索的過程中選取“最佳節(jié)點”后舍棄其他的兄弟節(jié)點，父親節(jié)點，而一直得搜索下去。這種搜索的結(jié)果很明顯，由于舍棄了其他的節(jié)點，可能也把 最好的節(jié)點都舍棄了。但A-Star在搜索時，便沒有舍棄節(jié)點（除非該節(jié)點是死節(jié)點），在每一步的估價中 都把當前的節(jié)點和 以前的節(jié)點的估價值比較得到一個“最佳的節(jié)點”。這樣可以有效的防止“最佳節(jié)點”的丟失。
? ?總的來說A-Star算法能夠高效快速的解決路徑規(guī)劃問題，在尋找最優(yōu)路徑時間上都較其他算法快速。
2.2 A-Star算法步驟
? ?A-Star用到一種估計函數(shù)，公式表示為：f(n)=g(n)+h(n),其中,f(n)是從初始狀態(tài)經(jīng)由狀態(tài)n到目標狀態(tài)的代價估計，g(n)是在狀態(tài)空間中從初始狀態(tài)到狀態(tài)n的實際代價，h(n)是從狀態(tài)n到目標狀態(tài)的最佳路徑的估計代價。（對于路徑搜索問題，狀態(tài)就是圖中的節(jié)點，代價就是距離）。
其運算步驟如下：
- 把起點加入open列表中
- 遍歷open列表中，找到F值最小的節(jié)點，把它作為當前處理的節(jié)點，并把該節(jié)點加入close列表中。
- 對該節(jié)點的8個相鄰格子進行判斷，如果格子是不可抵達的或者在close列表中，則忽略它，否則如下操作：
a. 如果相鄰格子不在open列表中，把它加入，并將parent設置為該節(jié)點和計算f,g,h值
b. 如果相鄰格子已在open列表中，并且新的G值比舊的G值小，則把相鄰格子的parent設置為該節(jié)點，并且重新計算f值。 - 重復2,3步，直到終點加入了open列表中，表示找到路徑；或者open列表空了，表示沒有路徑。
如果開啟列表已經(jīng)空了，說明路徑不存在。 - 最后從目標格開始，沿著每一格的父節(jié)點移動直到回到起始格，這就是路徑。
3.A-Star算法求解機器人路徑優(yōu)化
3.1 環(huán)境建模
? ?本文采用柵格法建模，從文件中讀取bmp格式圖片先將其灰度化，然后將其轉(zhuǎn)化成一個nn的環(huán)境區(qū)域，即將圖片劃分成nn個像素塊。并且顯示的圖像上有x，y坐標軸的顯示，可以看到圖像的像素大小。圖片中障礙物轉(zhuǎn)化為灰色像素塊，并用0表示；無障礙物的轉(zhuǎn)化為白色像素塊，并用1表示。
3.2 基本思路
? ?在全局路徑規(guī)劃中，機器人從起點開始到節(jié)點再從節(jié)點到目標點的代價值用遍歷的柵格總和來表示，也就是機器人每覆蓋一個柵格，成本代價就是從起點到節(jié)點的覆蓋柵格數(shù)的累加，估計代價就是從當前節(jié)點到目標點的柵格數(shù)累加。機器人在覆蓋柵格的時候首先要判斷目標柵格是否是自由柵格，然后判斷這個自由柵格是否是關聯(lián)性最大的柵格，與相關柵格比較如果關聯(lián)值最大即作為覆蓋柵格。如果關聯(lián)屬性值大小一樣，在機器人的八連通方向上按照順時針柵格。
3.3 路徑規(guī)劃流程
3.4 A-Star算法的改進方向
? ?根據(jù)全局環(huán)境這個條件，可設計雙向搜索的方式，即起始點和目標點同時搜索，當從兩點搜索到同一坐標時，連接兩邊搜索過的最短路徑，即為要求的路徑。這樣可節(jié)約搜索時間，提高路徑規(guī)劃效率，但是否為最短路徑，還需要仿真實驗來驗證。
4. 總結(jié)
? ?仿真結(jié)果證明本文提出的A-Star算法大幅縮小了排查節(jié)點的范圍，提高了計算效率和尋路過程中的方向性，使得找點更趨向于終點，且利用柵格化的環(huán)境建模方法能很好地處理復雜的平面地形，便于算法進行處理。且該設計可改變柵格地圖起點和終點的設置，都能實現(xiàn)路徑規(guī)劃。。仿真實驗結(jié)果表明該方法具有可行性和有效性，該設計使路徑規(guī)劃能夠獲得較優(yōu)路線。文章來源：http://www.zghlxwxcb.cn/news/detail-688696.html
5. 代碼
5.1 文件主函數(shù)
%%下載地圖并初始化參數(shù)
figure(1)
mapOriginal=imbinarize(imread('Maps/Map (9).bmp')); %從bmp文件讀取地圖
resolutionX=100;
resolutionY=100;
mapResized=imresize(mapOriginal,[resolutionX resolutionY]);
map=mapResized;
%連接矩陣-定義機器人可允許的動作
conn=[1 1 1;
1 2 1;
1 1 1];
display_process=true; %顯示節(jié)點的處理
% 增加一個單位像素的邊界——考慮到機器人的大小
for i=1:size(mapResized,1)
for j=1:size(mapResized,2)
if mapResized(i,j)==0
if i-1>=1, map(i-1,j)=0; end
if j-1>=1, map(i,j-1)=0; end
if i+1<=size(map,1), map(i+1,j)=0; end
if j+1<=size(map,2), map(i,j+1)=0; end
if i-1>=1 && j-1>=1, map(i-1,j-1)=0; end
if i-1>=1 && j+1<=size(map,2), map(i-1,j+1)=0; end
if i+1<=size(map,1) && j-1>=1, map(i+1,j-1)=0; end
if i+1<=size(map,1) && j+1<=size(map,2), map(i+1,j+1)=0; end
end
end
end
image((map==0).*0 + (map==1).*255 + (mapResized-map).*150);
colormap(gray(256))
%選出初始點和目標點
disp('select source in the image');
[x,y] = ginput(1);
source=[double(int8(y)) double(int8(x))]; % source position in Y, X format
disp('select goal in the image');
[x,y] = ginput(1);
goal = [double(int8(y)) double(int8(x))]; % goal position in Y, X format
if length(find(conn==2))~=1, error('no robot specified in connection matrix'); end
%% Compute path
%節(jié)點的結(jié)構(gòu)被認為是位置、位置、歷史成本、啟發(fā)式成本、總成本、封閉列表中的父索引
Q=[source 0 heuristic(source,goal) 0+heuristic(source,goal) -1]; %A*算法的處理隊列，開放列表
closed=ones(size(map)); % 黑色已標記存到閉列表
closedList=[]; % the closed list taken as a list
pathFound=false;
tic;
counter=0;
size(Q)
while size(Q,1)>0
[A, I]=min(Q,[],1);
n=Q(I(5),:); % 過程最小成本元素
Q=[Q(1:I(5)-1,:);Q(I(5)+1:end,:)]; % 刪除正在處理中的元素
if n(1)==goal(1) && n(2)==goal(2) %目標測試
pathFound=true;break;
end
[rx,ry,rv]=find(conn==2); %連接矩陣中的機器人位置
[mx,my,mv]=find(conn==1); %可能移動的數(shù)組
for mxi=1:size(mx,1) %遍歷所有的移動
newPos=[n(1)+mx(mxi)-rx n(2)+my(mxi)-ry]; %可能的新節(jié)點
if checkPath(n(1:2),newPos,map) %如果從n到newPos的路徑是無碰撞的
if closed(newPos(1),newPos(2))~=0 %還沒有關閉
historicCost=n(3)+historic(n(1:2),newPos);
heuristicCost=heuristic(newPos,goal);
totalCost=historicCost+heuristicCost;
add=true; %還沒有在更好的代價下排好隊
if length(find((Q(:,1)==newPos(1)) .* (Q(:,2)==newPos(2))))>=1
I=find((Q(:,1)==newPos(1)) .* (Q(:,2)==newPos(2)));
if Q(I,5)<totalCost, add=false;
else Q=[Q(1:I-1,:);Q(I+1:end,:);];add=true;
end
end
if add
Q=[Q;newPos historicCost heuristicCost totalCost size(closedList,1)+1]; %在隊列中添加新節(jié)點
end
end
end
end
closed(n(1),n(2))=0;closedList=[closedList;n]; %更新閉列表
i0 = counter;
i1 = 40;
counter=counter+1;
if display_process == true && (rem(i0,i1) == 0)
temp_img = (map==0).*0 + ((closed==0).*(map==1)).*125 + ((closed==1).*(map==1)).*255 + (mapResized - map).*100 ;
%畫出目標和起始點
temp_img(goal(1), goal(2) ) = 160;
temp_img(source(1), source(2) ) = 160;
image(temp_img);
M(counter)=getframe;
end
size(Q)
end
if ~pathFound
error('no path found')
end
%% Plot complete path繪制完整路線
figure(2)
path=[n(1:2)]; %從源信息檢索路徑
prev=n(6);
while prev>0
path=[closedList(prev,1:2);path];
prev=closedList(prev,6);
end
path=[(path(:,1)*size(mapOriginal,1))/resolutionX (path(:,2)*size(mapOriginal,2))/resolutionY];
pathLength=0;
for i=1:length(path)-1, pathLength=pathLength+historic(path(i,:),path(i+1,:)); end
fprintf('processing time=%d \nPath Length=%d \n\n', toc,pathLength);
imshow(mapOriginal);
line(path(:,2),path(:,1));
5.2 checkPath.m
function feasible=checkPath(n,newPos,map)
feasible=true;
dir=atan2(newPos(1)-n(1),newPos(2)-n(2));
for r=0:0.5:sqrt(sum((n-newPos).^2))
posCheck=n+r.*[sin(dir) cos(dir)];
if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) && ...
feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))
feasible=false;break;
end
if ~feasiblePoint(newPos,map), feasible=false; end
end
5.3 feasiblePoint.m
function feasible=feasiblePoint(point,map)
feasible=true;
% check if collission-free spot and inside maps
if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(1),point(2))==1)
feasible=false;
end
5.4 heuristic.m
function h=heuristic(X,goal)
h = sqrt(sum((X-goal).^2));
5.5 historic.m
function h=historic(a,b)
h = sqrt(sum((a-b).^2));
6. 效果圖
文章來源地址http://www.zghlxwxcb.cn/news/detail-688696.html
到了這里，關于基于A*算法的路徑規(guī)劃實踐(MATLAB語言)的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網(wǎng)！
