1. 最佳優(yōu)先搜索（Best-First Search）

最佳優(yōu)先搜索（BFS），又稱A算法，是一種啟發(fā)式搜索算法(Heuristic Algorithm)。[不是廣度優(yōu)先搜索算法（ Breadth First Search , BFS )]

BFS算法在廣度優(yōu)先搜索的基礎上，用啟發(fā)估價函數(shù)對將要被遍歷到的點進行估價，然后選擇代價小的進行遍歷，直到找到目標節(jié)點或者遍歷完所有點，算法結束。

要實現(xiàn)最佳優(yōu)先搜索我們必須使用一個優(yōu)先隊列（priority queue）來實現(xiàn)，通常采用一個open優(yōu)先隊列和一個closed集，open優(yōu)先隊列用來儲存還沒有遍歷將要遍歷的節(jié)點，而closed集用來儲存已經被遍歷過的節(jié)點。

1.1 最佳優(yōu)先搜索的過程

最佳優(yōu)先搜索的過程可以被描述為：

1、將根節(jié)點放入優(yōu)先隊列open中。

2、從優(yōu)先隊列中取出優(yōu)先級最高的節(jié)點X。

3、根據(jù)節(jié)點X生成子節(jié)點Y:

X的子節(jié)點Y不在open隊列或者closed中，由估價函數(shù)計算出估價值，放入open隊列中。

X的子節(jié)點Y在open隊列中，且估價值優(yōu)于open隊列中的子節(jié)點Y，將open隊列中的子節(jié)點Y的估價值替換成新的估價值并按優(yōu)先值排序。

X的子節(jié)點Y在closed集中，且估價值優(yōu)于closed集中的子節(jié)點Y，將closed集中的子節(jié)點Y移除，并將子節(jié)點Y加入open優(yōu)先隊列。

4、將節(jié)點X放入closed集中。

5、重復過程2,3,4直到目標節(jié)點找到，或者open為空，程序結束。

BFS不能保證找到一條最短路徑。然而，它比Dijkstra算法快的多，因為它用了一個啟發(fā)式函數(shù)（heuristic function）快速地導向目標結點。

這篇博客對BFS進行了詳細的介紹用的是羅馬尼亞度假問題

https://blog.csdn.net/weixin_46582817/article/details/115217489

2. A-Star算法

1968年發(fā)明的A*算法就是把啟發(fā)式方法（heuristic approaches）如BFS，和常規(guī)方法如Dijsktra算法結合在一起的算法。

A-Star算法是一種靜態(tài)路網中求解最短路徑最有效的直接搜索方法，也是解決許多搜索問題的有效算法。

??和Dijkstra一樣，A*能用于搜索最短路徑。

??和BFS一樣，A*能用啟發(fā)式函數(shù)引導它自己。

左圖為Astar算法效果圖，右圖為Dijkstra算法效果圖

Astar算法與Dijkstra算法的效果差不多，但Astar算法訪問的節(jié)點數(shù)明顯比Dijkstra算法少得多，說明其速度更快，運行時間更短。

2.1 Astar算法所屬分類

A*算法在最短路徑搜索算法中分類為：

??直接搜索算法：直接在實際地圖上進行搜索，不經過任何預處理；

??啟發(fā)式算法：通過啟發(fā)函數(shù)引導算法的搜索方向；

??靜態(tài)圖搜索算法：被搜索的圖的權值不隨時間變化（后被證明同樣可以適用于動態(tài)圖的搜索 ）

2.2 Astar算法基本概念

A*算法啟發(fā)函數(shù)表示為： f(n)=g(n)+h(n)

?f(n) 是從初始狀態(tài)經由狀態(tài)n到目標狀態(tài)的代價估計

?g(n) 是在狀態(tài)空間中從初始狀態(tài)到狀態(tài)n的實際代價

?h(n) 是從狀態(tài)n到目標狀態(tài)的最佳路徑的估計代價

（對于路徑搜索問題，狀態(tài)就是圖中的節(jié)點，代價就是距離）

Astar算法是啟發(fā)式搜索算法，啟發(fā)式搜索是在狀態(tài)空間中對每一個搜索的位置進行評估，得到最好的位置，再從這個位置進行搜索直到目標。

這樣可以省略大量無謂的搜索路徑，提高效率。在啟發(fā)式搜索中，對位置的估價是十分重要的。采用了不同的估價可以有不同的效果。

啟發(fā)函數(shù)（Heuristics Function）（估價函數(shù)）：?H為啟發(fā)函數(shù)，也被認為是一種試探。

由于在找到唯一路徑前，不確定在前面會出現(xiàn)什么障礙物，計算H的算法（例如，H可采用傳統(tǒng)的曼哈頓距離）具體根據(jù)實際場景決定

父節(jié)點（parent）：?在路徑規(guī)劃中用于回溯的節(jié)點。

搜索區(qū)域（The Search Area）：?前面圖中的搜索區(qū)域被劃分為了簡單的二維數(shù)組，數(shù)組每個元素對應一個小方格，也可以將區(qū)域等分成是五角星，矩形等，通常將一個單位的中心點稱之為搜索區(qū)域節(jié)點（Node）?！?/p>

開放列表(Open List)： 將路徑規(guī)劃過程中待檢測的節(jié)點存放于Open List中。

關閉列表(Close List)：?將路徑規(guī)劃過程中已經檢查過的節(jié)點放在Close List。

2.3 啟發(fā)函數(shù)單調性的推導

單調性：單調性是Astar算法非常重要的一個性質，它決定了在用Astar算法進行路徑搜索時，一定能找到一條最優(yōu)路徑。

設父節(jié)點的坐標為(x0,y0)，它的任意一個子節(jié)點的坐標為(xi,yi)，所以對兩者之間的h(x)，就一定滿足

在Astar算法的運行過程中，后繼的f(x)值時大于當前f(x)的值，即f(x)在之后對子節(jié)點的搜索擴展是單調遞增的，不會像人工勢場法一樣出現(xiàn)局部極小值，因此使用Astar算法規(guī)劃出的路徑一定是最優(yōu)路徑.

2.4 設計代價函數(shù)時所需注意的點

在使用A*算法的過程中，啟發(fā)代價的值必須盡量接近于真實值（盡量選取能取到的最大值，這樣可以提升搜索效率），以保證規(guī)劃出的路徑盡可能地與實際地圖環(huán)境相符合。

根據(jù)所需的模型選擇不同的啟發(fā)代價的計算方法時，同樣必須保證啟發(fā)代價所得的估計值小于真實值。

2.5 代價函數(shù)的選擇

2.5.1 曼哈頓距離

曼哈頓距離函數(shù)在標準坐標系中，表示起始和目標兩節(jié)點的絕對值總和，其估計值就是從當前點做水平和垂直運動，到達目標點的成本的估計，因此，曼哈頓距離函數(shù)中，兩點的距離如下

K——相鄰兩節(jié)點之間的距離，即單位距離； (x1,y1)——起始節(jié)點的坐標; (x2,y2)——目標節(jié)點的坐標;

2.5.2 歐幾里得距離

歐幾里得距離又稱為歐式距離，它是衡量兩點之間距離遠近的最常用的方法之一。歐幾里得距離函數(shù)的值可以直接看作起始節(jié)點和目標節(jié)點，在歐式空間中的直線距離，其表達式如下所示

K——相鄰兩節(jié)點之間的距離，即單位距離； (xi,yi)——當前節(jié)點的坐標; (xk,yk)——目標節(jié)點的坐標;

歐幾里德距離函數(shù)作為啟發(fā)代價的計算方法時，雖然尋徑時搜索空間增加從而導致搜索效率的降低，但其所得的估計值最小；

而在使用曼哈頓距離函數(shù)時，雖然尋徑需要遍歷的柵格空間少于歐幾里得距離函數(shù)，搜索效率較高，但這種方法得到的估計值與歐幾里得距離函數(shù)相比，距離真實值更遠。

2.6 確定最終路徑

已經確定了目標節(jié)點的坐標為，根據(jù)此目標節(jié)點的位置，和先前設置的方向存儲函數(shù)之中的方向，從目標節(jié)點利用方向反推至起始節(jié)點。具體實現(xiàn)的示意圖如下

2.7 路徑平滑

我們需要對規(guī)劃出的路徑進行平滑處理，將路徑的轉折處轉化為平滑的曲線，使之更加符合無人車的實際運動路徑。

主要有基于B樣條曲線的路徑優(yōu)化方法，基于Dubins圓的路徑優(yōu)化方法，和基于Clothoid曲線的路徑優(yōu)化方法，基于貝塞爾曲線的路徑平滑算法。

紅色為未平滑路徑，綠色方塊為最終路徑，黃色為貝塞爾曲線擬合得到，藍色為spcrv函數(shù)平滑得到。

2.8 Astar算法的優(yōu)缺點

優(yōu)點：

相對于需要將所有節(jié)點展開搜尋的算法，A*算法最大的優(yōu)點就是引入了啟發(fā)信息作為向目標點移動的決策輔助，所以不再需要遍歷整個地圖，降低了計算復雜度，減少了時間損耗少。

缺點：

基于柵格法來分割地圖，精度越高，柵格分的就越小，就會使工作量幾何式的增長。

估價函數(shù)選取的不合理也有可能導致無法規(guī)劃出最優(yōu)路徑。

距離估計與實際值越接近，估價函數(shù)取得就越好。估價函數(shù)f(n)在g(n)一定的情況下，會或多或少的受距離估計值h(n)的制約，節(jié)點距目標點近，h值小，f值相對就小，能保證最短路的搜索向終點的方向進行，明顯優(yōu)于Dijkstra算法的毫無方向的向四周搜索。

2.9 Astar算法流程

如下圖所示，綠色是起點A，紅色是終點B，一系列藍色是障礙物，從A移動到B,繞過障礙。

1、用方格（三角形、五角形）劃分空間，簡化搜索區(qū)域?？臻g被劃分為二維數(shù)組，數(shù)組中每個元素代表空間中的一個方格，可被標記為可行或不可行。未來的路徑就是一系列可行方塊的集合。Nodes的概念涵蓋了一系列可行方塊（或其它方塊）

2、將起點A放到Openlist中，Openlist存放著一系列需要檢查的節(jié)點（方塊）

3、給Openlist中每個節(jié)點賦值F=G+H （起點為0，橫向和縱向的移動代價為 10 ，對角線的移動代價為 14）

4、檢查Openlist，選取F值最小的節(jié)點（此處為A點）。

5、將A點從Openlist中刪除，放入Closelist中（Closelist中存放不可尋點，即已被訪問過的點），把A點臨近節(jié)點放入Openlist中，并將A點設為臨近節(jié)點的父節(jié)點。

6、給Openlist中每個節(jié)點賦值，選取F值最小的節(jié)點設為當前節(jié)點Node，(若當前節(jié)點Node為終點，則尋路結束，若Openlist中沒有可尋節(jié)點，則尋路失?。?/p>

7、檢查當前節(jié)點Node周圍臨近節(jié)點。忽略Closelist中的節(jié)點和不可行節(jié)點（障礙），如果臨近節(jié)點在Openlist中，則對比一下是否從當前節(jié)點到臨近節(jié)點的G值比原G值（直接到臨近節(jié)點的實際代價值）小，若是，把當前節(jié)點作為父節(jié)點。否，不做改動。如果臨近節(jié)點不在Openlist中，將其加入到Openlist中，將當前節(jié)點設為它的父節(jié)點。

8、若上步驟中新節(jié)點未造成任何改動，將新節(jié)點作為當前節(jié)點，重復6,7）中的步驟，直到找到目標節(jié)點。

尋找到目標節(jié)點

9、從目標節(jié)點回溯可以找到初始點，從而確定路徑

上述描述路徑的圖片有些錯誤，具體步驟如下圖所示。

3. 其他Astar算法

3.1 Astar——三維地圖規(guī)劃

3.1.1 3D-Astar原理

三維柵格地圖，顧名思義不是簡單的二維平面，它必須得有三維方向，也就是高度方向上的拓展。柵格地圖在XY水平面上的柵格的投影顏色不盡相同，柵格黃色程度越高，表明此處的高度越高。

為了使啟發(fā)代價的值應該盡量接近于真實值，三維柵格地圖中仍然選用歐幾里得距離作為估價函數(shù)計算啟發(fā)代價的計算方法。

但本次實驗所處的環(huán)境為三維避障空間，因此相較于二維空間的路徑規(guī)劃，我們將公式做三維化推廣，具體形式如下：

h(n)——當前節(jié)點到目標節(jié)點的啟發(fā)代價值；

g(n-1)——當前節(jié)點到它的父節(jié)點之間的路徑代價；

D(n-1, n)——當前節(jié)點與它的子節(jié)點之間的代價值。

g(n-1)與二維規(guī)劃中的距離代價計算方法一致，但D(n-1, n)在計算時用父節(jié)點與子節(jié)點之間的距離值除以三維避障環(huán)境中設定的柵格可通行程度，h(n)也是用當前節(jié)點到目標節(jié)點的啟發(fā)代價值除以地圖環(huán)境中預先設定的柵格可通行程度。

3.1.2 基于MATLAB實現(xiàn)3D-Astar

這里是代碼的GitHub地址：

https://github.com/ybmasmiling/Astar_3D

3.2 距離與能量復合Astar算法

經典Astar算法路徑規(guī)劃是取兩節(jié)點間曼哈頓距離作為距離估計為最優(yōu)原則搜索路徑，從而得到最短路徑。

搜索路徑的過程中并沒有考慮實際道路坡度和道路滾動阻力系數(shù)對行駛車輛最短路徑搜索的影響，也沒有考慮在道路上行駛的能量損耗問題，即經典Astar算法搜索的最短路徑并非符合實際車輛行駛的最優(yōu)路徑。

3.2.1 最短路徑啟發(fā)函數(shù)

最短路徑啟發(fā)函數(shù):

?

最終得到的最短路徑啟發(fā)函數(shù)如下式：

3.2.2 最短道路損耗功啟發(fā)函數(shù)

道路損耗功啟發(fā)函數(shù):

?

最終得到的最短道路損耗功啟發(fā)函數(shù)如下式：

3.2.3 綜合啟發(fā)函數(shù)

綜合啟發(fā)函數(shù)：

?

綜合式啟發(fā)函數(shù)統(tǒng)一最短路徑啟發(fā)函數(shù)和最小道路額外功函數(shù)，不同的權重大小決定最短路徑啟發(fā)函數(shù)和最小道路額外功函數(shù)在綜合式啟發(fā)函數(shù)中所占不同的比重。

3.3 雙向Astar算法

傳統(tǒng)A算法在大環(huán)境中搜索，存在著搜索效率低的問題。

傳統(tǒng)A算法是從起點開始向終點搜索，直到尋到可行路徑停止搜索，在搜索路徑的前期階段遠g(n)小于h(n),搜索時橫向搜索范圍窄，縱向搜索范圍寬，搜索速度快，在搜索的后期階段g(n)遠大于h(n),搜索時縱向搜索范圍窄，橫向搜索范圍寬，搜索速度慢；**而改進后的雙向A搜索算法分別從起點和終點開始搜索，當搜索到相同的當前節(jié)點時，搜索結束。

**相比于傳統(tǒng)A算法，雙向A*搜索算法都處于g(n)遠小于h(n)階段，一定程度上提高了算法的搜索效率，縮短搜索時間。

4. MATLAB實現(xiàn)Astar算法

4.1 defColorMap.m

用于初始化地圖參數(shù)

?

?

function [field,cmap] = defColorMap(rows, cols)
cmap = [1 1 1; ... ? ? ? % 1-白色-空地
 ? ?0 0 0; ... ? ? ? ? ? % 2-黑色-靜態(tài)障礙
 ? ?1 0 0; ... ? ? ? ? ? % 3-紅色-動態(tài)障礙
 ? ?1 1 0;... ? ? ? ? ? ?% 4-黃色-起始點 
 ? ?1 0 1;... ? ? ? ? ? ?% 5-品紅-目標點
 ? ?0 1 0; ... ? ? ? ? ? % 6-綠色-到目標點的規(guī)劃路徑 ? 
 ? ?0 1 1]; ? ? ? ? ? ? ?% 7-青色-動態(tài)規(guī)劃的路徑


% 構建顏色MAP圖
colormap(cmap);


% 定義柵格地圖全域，并初始化空白區(qū)域
field = ones(rows, cols);


% 障礙物區(qū)域
obsRate = 0.3;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;

?

?

4.2 getChildNode.m

用于獲取子節(jié)點信息

?

?

function childNodes = getChildNode(field,closeList, parentNode)
% 選取父節(jié)點周邊8個節(jié)點作為備選子節(jié)點，線性化坐標
% 排除超過邊界之外的、位于障礙區(qū)的、位于closeList中的


[rows, cols] = size(field);
[row_parentNode, col_parentNode] = ind2sub([rows, cols], parentNode);
childNodes = [];
closeList = closeList(:,1);


% 第1個子節(jié)點(右節(jié)點)
childNode = [row_parentNode, col_parentNode+1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
 ? ? ? ?childNode(2) < 1 || childNode(2) > cols)
 ? ?if field(childNode(1), childNode(2)) ~= 2
 ? ? ? ?childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
 ? ? ? ?if ~ismember(childNode_LineIdx, closeList)
 ? ? ? ? ? ?childNodes(end+1) = childNode_LineIdx;
 ? ? ? ?end
 ? ?end
end


% 第2個子節(jié)點（右上節(jié)點）
childNode = [row_parentNode-1, col_parentNode+1];
child_brother_node_sub1 = [row_parentNode-1, col_parentNode];
child_brother_node_sub2 = [row_parentNode, col_parentNode+1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
 ? ? ? ?childNode(2) < 1 || childNode(2) > cols)
 ? ?if field(childNode(1), childNode(2)) ~= 2 
 ? ? ? ?if ?~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
 ? ? ? ? ? ?childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
 ? ? ? ? ? ?if ~ismember(childNode_LineIdx, closeList)
 ? ? ? ? ? ? ? ?childNodes(end+1) = childNode_LineIdx;
 ? ? ? ? ? ?end
 ? ? ? ?end ? 
 ? ?end
end




% 第3個子節(jié)點（上節(jié)點）
childNode = [row_parentNode-1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
 ? ? ? ?childNode(2) < 1 || childNode(2) > cols)
 ? ?if field(childNode(1), childNode(2)) ~= 2
 ? ? ? ?childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
 ? ? ? ?if ~ismember(childNode_LineIdx, closeList)
 ? ? ? ? ? ?childNodes(end+1) = childNode_LineIdx;
 ? ? ? ?end
 ? ?end
end




% 第4個子節(jié)點（左上）
childNode = [row_parentNode-1, col_parentNode-1];
child_brother_node_sub1 = [row_parentNode-1, col_parentNode];
child_brother_node_sub2 = [row_parentNode, col_parentNode-1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
 ? ? ? ?childNode(2) < 1 || childNode(2) > cols)
 ? ?if field(childNode(1), childNode(2)) ~= 2 
 ? ? ? ?if ?~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
 ? ? ? ? ? ?childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
 ? ? ? ? ? ?if ~ismember(childNode_LineIdx, closeList)
 ? ? ? ? ? ? ? ?childNodes(end+1) = childNode_LineIdx;
 ? ? ? ? ? ?end
 ? ? ? ?end ?
 ? ?end
end




% 第5個子節(jié)點(左節(jié)點)
childNode = [row_parentNode, col_parentNode-1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
 ? ? ? ?childNode(2) < 1 || childNode(2) > cols)
 ? ?if field(childNode(1), childNode(2)) ~= 2
 ? ? ? ?childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
 ? ? ? ?if ~ismember(childNode_LineIdx, closeList)
 ? ? ? ? ? ?childNodes(end+1) = childNode_LineIdx;
 ? ? ? ?end
 ? ?end
end




% 第6個子節(jié)點（左下）
childNode = [row_parentNode+1, col_parentNode-1];
 ? ?child_brother_node_sub1 = [row_parentNode, col_parentNode-1];
 ? ?child_brother_node_sub2 = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
 ? ? ? ?childNode(2) < 1 || childNode(2) > cols)
 ? ?if field(childNode(1), childNode(2)) ~= 2 
 ? ? ? ?if ?~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
 ? ? ? ? ? ?childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
 ? ? ? ? ? ?if ~ismember(childNode_LineIdx, closeList)
 ? ? ? ? ? ? ? ?childNodes(end+1) = childNode_LineIdx;
 ? ? ? ? ? ?end
 ? ? ? ?end 
 ? ?end
end




% 第7個子節(jié)點（下）
childNode = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
 ? ? ? ?childNode(2) < 1 || childNode(2) > cols)
 ? ?if field(childNode(1), childNode(2)) ~= 2
 ? ? ? ?childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
 ? ? ? ?if ~ismember(childNode_LineIdx, closeList)
 ? ? ? ? ? ?childNodes(end+1) = childNode_LineIdx;
 ? ? ? ?end
 ? ?end
end




% 第8個子節(jié)點（右下）
childNode = [row_parentNode+1, col_parentNode+1];
 ? ?child_brother_node_sub1 = [row_parentNode, col_parentNode+1];
 ? ?child_brother_node_sub2 = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
 ? ? ? ?childNode(2) < 1 || childNode(2) > cols)
 ? ?if field(childNode(1), childNode(2)) ~= 2 ?
 ? ? ? ?if ?~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
 ? ? ? ? ? ?childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2)); 
 ? ? ? ? ? ?if ~ismember(childNode_LineIdx, closeList)
 ? ? ? ? ? ? ? ?childNodes(end+1) = childNode_LineIdx;
 ? ? ? ? ? ?end
 ? ? ? ?end
 ? ?end
end

?

?

4.3 Astar.m

主程序

?

?

% 基于柵格地圖的機器人路徑規(guī)劃算法
% A*算法
clc
clear
close all


%% 柵格界面、場景定義
% 行數(shù)和列數(shù)
rows = 20;
cols = 20;
[field,cmap] = defColorMap(rows, cols);


% 起點、終點、障礙物區(qū)域
startPos = 2;
goalPos = rows*cols-2;
field(startPos) = 4;
field(goalPos) = 5;


%% 預處理


% 初始化closeList
parentNode = startPos;
closeList = [startPos,0];


% 初始化openList
openList = struct;
childNodes = getChildNode(field,closeList,parentNode);
for i = 1:length(childNodes)
 ? ?[row_startPos,col_startPos] = ind2sub([rows, cols],startPos);
 ? ?[row_goalPos,col_goalPos] = ind2sub([rows, cols],goalPos); ? 
 ? ?[row,col] = ind2sub([rows, cols],childNodes(i));


 ? ?% 存入openList結構體
 ? ?openList(i).node = childNodes(i);
 ? ?openList(i).g = norm([row_startPos,col_startPos] - [row,col]); ?% 實際代價采用歐式距離
 ? ?openList(i).h = abs(row_goalPos - row) + abs(col_goalPos - col); ?% 估計代價采用曼哈頓距離
 ? ?openList(i).f = openList(i).g + openList(i).h;
end


% 初始化path
for i = 1:rows*cols
 ? ?path{i,1} = i; ?% 線性索引值
end
for i = 1:length(openList)
 ? ?node = openList(i).node;
 ? ?path{node,2} = [startPos,node];
end 


%% 開始搜索
% 從openList開始搜索移動代價最小的節(jié)點
[~, idx_min] = min([openList.f]); ?
parentNode = openList(idx_min).node;


% 進入循環(huán)
while true ?
 ? ?
 ? ?% 找出父節(jié)點的8個子節(jié)點，障礙物節(jié)點用inf，
 ? ?childNodes = getChildNode(field, closeList,parentNode);
 ? ?
 ? ?% 判斷這些子節(jié)點是否在openList中，若在，則比較更新；沒在則追加到openList中
 ? ?for i = 1:length(childNodes)
 ? ? ? ?
 ? ? ? ?% 需要判斷的子節(jié)點
 ? ? ? ?childNode = childNodes(i);
 ? ? ? ?[in_flag,idx] = ismember(childNode, [openList.node]);
 ? ? ? ? ? 
 ? ? ? ?% 計算代價函數(shù)
 ? ? ? ?[row_parentNode,col_parentNode] = ind2sub([rows, cols], parentNode);
 ? ? ? ?[row_childNode,col_childNode] = ind2sub([rows, cols], childNode);
 ? ? ? ?[row_goalPos,col_goalPos] = ind2sub([rows, cols],goalPos);
 ? ? ? ?g = openList(idx_min).g + norm( [row_parentNode,col_parentNode] -...
 ? ? ? ? ? ?[row_childNode,col_childNode]);
 ? ? ? ?h = abs(row_goalPos - row_childNode) + abs(col_goalPos - col_childNode); % 采用曼哈頓距離進行代價估計
 ? ? ? ?f = g + h;
 ? ? ? ?
 ? ? ? ?if in_flag ? % 若在，比較更新g和f ? ? ? ?
 ? ? ? ? ? ?if f < openList(idx).f
 ? ? ? ? ? ? ? ?openList(idx).g = g;
 ? ? ? ? ? ? ? ?openList(idx).h = h;
 ? ? ? ? ? ? ? ?openList(idx).f = f;
 ? ? ? ? ? ? ? ?path{childNode,2} = [path{parentNode,2}, childNode];
 ? ? ? ? ? ?end
 ? ? ? ?else ? ? ? ? % 若不在，追加到openList
 ? ? ? ? ? ?openList(end+1).node = childNode;
 ? ? ? ? ? ?openList(end).g = g;
 ? ? ? ? ? ?openList(end).h = h;
 ? ? ? ? ? ?openList(end).f = f;
 ? ? ? ? ? ?path{childNode,2} = [path{parentNode,2}, childNode];
 ? ? ? ?end
 ? ?end
 ? ? ? 
 ? ?% 從openList移出移動代價最小的節(jié)點到closeList
 ? ?closeList(end+1,: ) = [openList(idx_min).node, openList(idx_min).f];
 ? ?openList(idx_min)= [];
 
 ? ?% 重新搜索：從openList搜索移動代價最小的節(jié)點
 ? ?[~, idx_min] = min([openList.f]);
 ? ?parentNode = openList(idx_min).node;
 ? ?
 ? ?% 判斷是否搜索到終點
 ? ?if parentNode == goalPos
 ? ? ? ?closeList(end+1,: ) = [openList(idx_min).node, openList(idx_min).f];
 ? ? ? ?break
 ? ?end
end


%% 畫路徑
% 找出目標最優(yōu)路徑
path_target = path{goalPos,2};
for i = 1:rows*cols
 ? ? if ~isempty(path{i,2}) 
 ? ? ? ?field((path{i,1})) = 7;
 ? ? end
end
field(startPos) = 4;
field(goalPos) = 5;
field(path_target(2:end-1)) = 6;


% 畫柵格圖
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;
hold on;
[y0,x0] = ind2sub([rows,cols], path_target);
y = y0 + 0.5;
x = x0 + 0.5;
plot(x,y,'-','Color','r','LineWidth',2.5);
hold on;
points = [x',y'];
M = 10;
[x1,y1] = bezir_n(points, M);
plot(x1,y1,'-','Color','y','LineWidth',2.5);
hold on;
values = spcrv([[x(1) x x(end)];[y(1) y y(end)]],3);
plot(values(1,:),values(2,:),?'b','LineWidth',2.5);

?

?

4.4 算法效果

運行總時間

柵格地圖大?。?0x20）

柵格地圖大?。?0x30）

柵格地圖大?。?0x40）

可以看到Astar算法對于柵格地圖越大的情況，搜索時間越長，其速度相比Dijkstra有優(yōu)勢（尤其是在地圖比較大的時候，優(yōu)勢更明顯）。

但其總耗時較長，不適用于實時的路徑規(guī)劃，不適用于局部路徑規(guī)劃，適用于全局路徑規(guī)劃。

5. python實現(xiàn)Astar算法

可以參考這篇文章

https://www.jianshu.com/p/5704e67f40aa

這篇文章介紹了Astar以及后續(xù)的變種算法

python 版本的偽代碼（來源：https://brilliant.org/wiki/a-star-search/）如下：

?

?

make an openlist containing only the starting node
make an empty closed list
 ? while (the destination node has not been reached):
 ? ? ? consider the node with the lowest f score in the open list
 ? ? ? if (this node is our destination node) :
 ? ? ? ? ? we are finished 
 ? ? ? if not:
 ? ? ? ? ? put the current node in the closed list and look at all of its neighbors
 ? ? ? ? ? for (each neighbor of the current node):
 ? ? ? ? ? ? ? if (neighbor has lower g value than current and is in the closed list) :
 ? ? ? ? ? ? ? ? ? replace the neighbor with the new, lower, g value 
 ? ? ? ? ? ? ? ? ? current node is now the neighbor's parent ? ? ? ? ? ?
 ? ? ? ? ? ? ? else if (current g value is lower and this neighbor is in the open list ) :
 ? ? ? ? ? ? ? ? ? replace the neighbor with the new, lower, g value 
 ? ? ? ? ? ? ? ? ? change the neighbor's parent to our current node


 ? ? ? ? ? ? ? else if this neighbor is not in both lists:
 ? ? ? ? ? ? ? ? ? add it to the open list and set its g

?

?

6. Java實現(xiàn)Astar算法

可以參考這篇文章：

https://www.jianshu.com/p/950233af52df

7. 實踐案例—基于ROS實現(xiàn)Astar與DWA算法

本項目以Astar算法作為全局路徑規(guī)劃算法，DWA作為局部路徑規(guī)劃算法，實現(xiàn)效果如下。（具體原理與算法代碼解釋與說明會在之后的文章附上）

編輯：黃飛

?