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

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

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

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

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

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

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

2、從優(yōu)先隊(duì)列中取出優(yōu)先級(jí)最高的節(jié)點(diǎn)X。

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

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

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

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

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

5、重復(fù)過程2,3,4直到目標(biāo)節(jié)點(diǎn)找到，或者open為空，程序結(jié)束。

BFS不能保證找到一條最短路徑。然而，它比Dijkstra算法快的多，因?yàn)樗昧艘粋€(gè)啟發(fā)式函數(shù)（heuristic function）快速地導(dǎo)向目標(biāo)結(jié)點(diǎn)。

這篇博客對(duì)BFS進(jìn)行了詳細(xì)的介紹用的是羅馬尼亞度假問題

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

2. A-Star算法

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

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

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

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

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

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

2.1 Astar算法所屬分類

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

??直接搜索算法：直接在實(shí)際地圖上進(jìn)行搜索，不經(jīng)過任何預(yù)處理；

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

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

2.2 Astar算法基本概念

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

?f(n) 是從初始狀態(tài)經(jīng)由狀態(tài)n到目標(biāo)狀態(tài)的代價(jià)估計(jì)

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

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

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

Astar算法是啟發(fā)式搜索算法，啟發(fā)式搜索是在狀態(tài)空間中對(duì)每一個(gè)搜索的位置進(jìn)行評(píng)估，得到最好的位置，再?gòu)倪@個(gè)位置進(jìn)行搜索直到目標(biāo)。

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

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

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

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

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

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

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

2.3 啟發(fā)函數(shù)單調(diào)性的推導(dǎo)

單調(diào)性：?jiǎn)握{(diào)性是Astar算法非常重要的一個(gè)性質(zhì)，它決定了在用Astar算法進(jìn)行路徑搜索時(shí)，一定能找到一條最優(yōu)路徑。

設(shè)父節(jié)點(diǎn)的坐標(biāo)為(x0,y0)，它的任意一個(gè)子節(jié)點(diǎn)的坐標(biāo)為(xi,yi)，所以對(duì)兩者之間的h(x)，就一定滿足

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

2.4 設(shè)計(jì)代價(jià)函數(shù)時(shí)所需注意的點(diǎn)

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

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

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

2.5.1 曼哈頓距離

曼哈頓距離函數(shù)在標(biāo)準(zhǔn)坐標(biāo)系中，表示起始和目標(biāo)兩節(jié)點(diǎn)的絕對(duì)值總和，其估計(jì)值就是從當(dāng)前點(diǎn)做水平和垂直運(yùn)動(dòng)，到達(dá)目標(biāo)點(diǎn)的成本的估計(jì)，因此，曼哈頓距離函數(shù)中，兩點(diǎn)的距離如下

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

2.5.2 歐幾里得距離

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

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

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

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

2.6 確定最終路徑

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

2.7 路徑平滑

我們需要對(duì)規(guī)劃出的路徑進(jìn)行平滑處理，將路徑的轉(zhuǎn)折處轉(zhuǎn)化為平滑的曲線，使之更加符合無(wú)人車的實(shí)際運(yùn)動(dòng)路徑。

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

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

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

優(yōu)點(diǎn)：

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

缺點(diǎn)：

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

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

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

2.9 Astar算法流程

如下圖所示，綠色是起點(diǎn)A，紅色是終點(diǎn)B，一系列藍(lán)色是障礙物，從A移動(dòng)到B,繞過障礙。

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

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

3、給Openlist中每個(gè)節(jié)點(diǎn)賦值F=G+H （起點(diǎn)為0，橫向和縱向的移動(dòng)代價(jià)為 10 ，對(duì)角線的移動(dòng)代價(jià)為 14）

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

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

6、給Openlist中每個(gè)節(jié)點(diǎn)賦值，選取F值最小的節(jié)點(diǎn)設(shè)為當(dāng)前節(jié)點(diǎn)Node，(若當(dāng)前節(jié)點(diǎn)Node為終點(diǎn)，則尋路結(jié)束，若Openlist中沒有可尋節(jié)點(diǎn)，則尋路失敗）

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

8、若上步驟中新節(jié)點(diǎn)未造成任何改動(dòng)，將新節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn)，重復(fù)6,7）中的步驟，直到找到目標(biāo)節(jié)點(diǎn)。

尋找到目標(biāo)節(jié)點(diǎn)

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

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

3. 其他Astar算法

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

3.1.1 3D-Astar原理

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

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

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

h(n)——當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的啟發(fā)代價(jià)值；

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

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

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

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

這里是代碼的GitHub地址：

https://github.com/ybmasmiling/Astar_3D

3.2 距離與能量復(fù)合Astar算法

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

搜索路徑的過程中并沒有考慮實(shí)際道路坡度和道路滾動(dòng)阻力系數(shù)對(duì)行駛車輛最短路徑搜索的影響，也沒有考慮在道路上行駛的能量損耗問題，即經(jīng)典Astar算法搜索的最短路徑并非符合實(shí)際車輛行駛的最優(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ù)，不同的權(quán)重大小決定最短路徑啟發(fā)函數(shù)和最小道路額外功函數(shù)在綜合式啟發(fā)函數(shù)中所占不同的比重。

3.3 雙向Astar算法

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

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

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

4. MATLAB實(shí)現(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-紅色-動(dòng)態(tài)障礙
 ? ?1 1 0;... ? ? ? ? ? ?% 4-黃色-起始點(diǎn) 
 ? ?1 0 1;... ? ? ? ? ? ?% 5-品紅-目標(biāo)點(diǎn)
 ? ?0 1 0; ... ? ? ? ? ? % 6-綠色-到目標(biāo)點(diǎn)的規(guī)劃路徑 ? 
 ? ?0 1 1]; ? ? ? ? ? ? ?% 7-青色-動(dòng)態(tài)規(guī)劃的路徑


% 構(gòu)建顏色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é)點(diǎn)信息

?

?

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


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


% 第1個(gè)子節(jié)點(diǎn)(右節(jié)點(diǎn))
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個(gè)子節(jié)點(diǎn)（右上節(jié)點(diǎn)）
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個(gè)子節(jié)點(diǎn)（上節(jié)點(diǎn)）
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個(gè)子節(jié)點(diǎn)（左上）
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個(gè)子節(jié)點(diǎn)(左節(jié)點(diǎn))
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個(gè)子節(jié)點(diǎn)（左下）
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個(gè)子節(jié)點(diǎn)（下）
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個(gè)子節(jié)點(diǎn)（右下）
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

主程序

?

?

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


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


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


%% 預(yù)處理


% 初始化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結(jié)構(gòu)體
 ? ?openList(i).node = childNodes(i);
 ? ?openList(i).g = norm([row_startPos,col_startPos] - [row,col]); ?% 實(shí)際代價(jià)采用歐式距離
 ? ?openList(i).h = abs(row_goalPos - row) + abs(col_goalPos - col); ?% 估計(jì)代價(jià)采用曼哈頓距離
 ? ?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開始搜索移動(dòng)代價(jià)最小的節(jié)點(diǎn)
[~, idx_min] = min([openList.f]); ?
parentNode = openList(idx_min).node;


% 進(jìn)入循環(huán)
while true ?
 ? ?
 ? ?% 找出父節(jié)點(diǎn)的8個(gè)子節(jié)點(diǎn)，障礙物節(jié)點(diǎn)用inf，
 ? ?childNodes = getChildNode(field, closeList,parentNode);
 ? ?
 ? ?% 判斷這些子節(jié)點(diǎn)是否在openList中，若在，則比較更新；沒在則追加到openList中
 ? ?for i = 1:length(childNodes)
 ? ? ? ?
 ? ? ? ?% 需要判斷的子節(jié)點(diǎn)
 ? ? ? ?childNode = childNodes(i);
 ? ? ? ?[in_flag,idx] = ismember(childNode, [openList.node]);
 ? ? ? ? ? 
 ? ? ? ?% 計(jì)算代價(jià)函數(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); % 采用曼哈頓距離進(jìn)行代價(jià)估計(jì)
 ? ? ? ?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移出移動(dòng)代價(jià)最小的節(jié)點(diǎn)到closeList
 ? ?closeList(end+1,: ) = [openList(idx_min).node, openList(idx_min).f];
 ? ?openList(idx_min)= [];
 
 ? ?% 重新搜索：從openList搜索移動(dòng)代價(jià)最小的節(jié)點(diǎn)
 ? ?[~, idx_min] = min([openList.f]);
 ? ?parentNode = openList(idx_min).node;
 ? ?
 ? ?% 判斷是否搜索到終點(diǎn)
 ? ?if parentNode == goalPos
 ? ? ? ?closeList(end+1,: ) = [openList(idx_min).node, openList(idx_min).f];
 ? ? ? ?break
 ? ?end
end


%% 畫路徑
% 找出目標(biāo)最優(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 算法效果

運(yùn)行總時(shí)間

柵格地圖大?。?0x20）

柵格地圖大?。?0x30）

柵格地圖大?。?0x40）

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

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

5. python實(shí)現(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實(shí)現(xiàn)Astar算法

可以參考這篇文章：

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

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

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

編輯：黃飛

?