首先看看運(yùn)行效果，分別有三種模式，代碼運(yùn)行前需要通過鼠標(biāo)點(diǎn)擊設(shè)置起點(diǎn)和終點(diǎn)。

第一種模式直接輸出最短路徑

第二種模式輸出最短路徑的生成過程

第三種模式輸出最短路徑的生成過程和詳細(xì)探索的過程

代碼獲取

gitee鏈接：https://gitee.com/chenshao777/A_Star_Matlab (麻煩點(diǎn)個(gè)Star哦！感謝?。?/span>

B站上我也發(fā)了視頻，不過之前的版本沒有添加鼠標(biāo)點(diǎn)擊功能【晨少原版 | 已開源】A*路徑規(guī)劃算法Matlab仿真

代碼是2022年除夕前一天完成的

博主肝代碼不易，還請各位點(diǎn)贊、關(guān)注支持一下，哈哈！

一、A* 算法原理

二、A* 算法實(shí)現(xiàn)步驟

一、A* 算法原理

A* 算法是專門用來求解地圖中最短路徑的算法，同樣的算法有很多，但實(shí)際中最常用的就是A*算法。

舉個(gè)例子來說，A*算法通常要將地圖網(wǎng)格化，如下圖所示：

假設(shè)有一只烏龜在追小白兔，烏龜此時(shí)的位置是（2,2），小白兔的位置是（6,6），假設(shè)小白兔靜止不動(dòng)。

根據(jù)A *算法的原理，烏龜只能向左、向右、向上、向下走，那么（1,2）、（2,1）、（3,2）、（2,3）是烏龜下一輪可以到達(dá)的點(diǎn)，這些點(diǎn)叫做 待探索的點(diǎn)

步驟一

尋找下一步可以到達(dá)的節(jié)點(diǎn)，將這些待探索的點(diǎn)加入待探索數(shù)組 frontier 中，也叫邊界數(shù)組。

計(jì)算出新加入點(diǎn)的代價(jià)，代價(jià) = 當(dāng)前代價(jià) + 預(yù)估代價(jià) ， 公式表達(dá)為 F= G + H

----------------------------------------

所謂 當(dāng)前代價(jià) G 就是從起點(diǎn)到達(dá)當(dāng)前點(diǎn)所經(jīng)過的路程，例如（1,2）、（2,1）、（3,2）、（2,3）這四個(gè)點(diǎn)的當(dāng)前代價(jià)都等于（2,2）點(diǎn)到達(dá)其所需的路程，即為 1 。

----------------------------------------

所謂 預(yù)估代價(jià) H 就是從當(dāng)前點(diǎn)到終點(diǎn)的曼哈頓距離（橫縱坐標(biāo)差值之和，| x1 - x2| + |y1 - y2|）

所以（1,2）、（2,1）、（3,2）、（2,3）四個(gè)待探索點(diǎn)的預(yù)估代價(jià)分別為9、9、7、7 。

當(dāng)前代價(jià) / 預(yù)估代價(jià)結(jié)果如下圖所示：

步驟二

將待探索點(diǎn)按照代價(jià)的大小升序排序，則排序后的待探索數(shù)組中待探索點(diǎn)的順序?yàn)?/p>

（3,2）、（2,3）、（1,2）、（2,1）

接著取出第一個(gè)，即代價(jià)最小的點(diǎn)作為小烏龜?shù)拇溯喣繕?biāo)點(diǎn)，即為下一輪的起始點(diǎn)，并把該點(diǎn)從待探索數(shù)組 frontier 中刪去 ，加入 已探索數(shù)組 already_frontier 中，則會(huì)得到下面的情況：

此時(shí)小烏龜在（3,2）位置，且為了更形象的表達(dá)，圖中將待探索點(diǎn)標(biāo)成了藍(lán)色，已探索點(diǎn)標(biāo)成了綠色（已探索點(diǎn)目前有（2,2）和（3,2），待探索點(diǎn)有（1,2）、（2,1）、（2,3）)

步驟三

記錄下當(dāng)前點(diǎn)到起點(diǎn)的路徑，可以這么記 [(2,2) , (3,2)]，在matlab中可以表現(xiàn)為一個(gè)2行2列的矩陣

2? ?2

3? ?2

接著判斷當(dāng)前節(jié)點(diǎn)是否是終點(diǎn)，如果不是終點(diǎn)則繼續(xù)步驟一，繼續(xù)尋找下一步可以探索的點(diǎn)

為了便于大家的理解，再跟著步驟一、二、三進(jìn)行下一輪

- 如上圖所示，找出小烏龜下一輪可以去到的點(diǎn)，判斷周圍的點(diǎn)是否在已探索數(shù)組 already_frontier 中，

- 如果在則忽略，接著判斷這些點(diǎn)是否已經(jīng)在待探索數(shù)組 frontier 中，如果在則比較該點(diǎn)的 當(dāng)前代價(jià)G 與 如果經(jīng)過小烏龜當(dāng)前位置而到達(dá)該點(diǎn)現(xiàn)在位置所需的 當(dāng)前代價(jià)G2 進(jìn)行比較，

- 如果G > G2則將該點(diǎn)的上一個(gè)節(jié)點(diǎn)（可以理解為父節(jié)點(diǎn)）改成小烏龜當(dāng)前所在節(jié)點(diǎn)，更新其當(dāng)前代價(jià)為G2。

最后計(jì)算出他們的代價(jià)，接著對待探索數(shù)組升序排序后，選出第一個(gè)代價(jià)最小的點(diǎn)作為下一輪的起始點(diǎn)。

由于我是以 左下右上 的順序?qū)ふ掖剿鼽c(diǎn)的，所以前兩次選擇到的代價(jià)最小是（3,2）和（4,2），如果你按照 上右下左的順序，則你選擇到的代價(jià)最小會(huì)是（2,3）和（2,4），這個(gè)并不會(huì)影響到最終的最短路徑長度，只是路徑不同罷了

根據(jù)上面三個(gè)步驟一直循環(huán)，就可以得到一條最短的路徑，下圖綠色點(diǎn)表示的即為最短路徑

當(dāng)然也可以是先往上走，再往右走，根據(jù)每個(gè)人自己選擇的待探索點(diǎn)的順序來確定

二、A* 算法實(shí)現(xiàn)步驟

1、將起點(diǎn)加入待探索數(shù)組 frontier ，代價(jià)賦為 0

2、找出 frontier 中代價(jià) F 最小的點(diǎn)，移出 frontier ，添加進(jìn)已探索數(shù)組 already_frontier

3、判斷該點(diǎn)是否是終點(diǎn)，如果不是則繼續(xù)

4、將該節(jié)點(diǎn)周圍的四個(gè)點(diǎn)找出，判斷這四個(gè)點(diǎn)是否在 already_frontier數(shù)組中，如果在則忽略（當(dāng)然這四個(gè)點(diǎn)也不能超出地圖范圍或者位于障礙物中）。

5、判斷這些點(diǎn)是否已經(jīng)在待探索數(shù)組 frontier 中，如果在，則將該點(diǎn)的 當(dāng)前代價(jià)G 與 經(jīng)過當(dāng)前節(jié)點(diǎn)到達(dá)該點(diǎn)的 當(dāng)前代價(jià)G2 進(jìn)行，如果G > G2則將該點(diǎn)的上一個(gè)節(jié)點(diǎn)（可以理解為父節(jié)點(diǎn)）改成當(dāng)前節(jié)點(diǎn)，更新其當(dāng)前代價(jià)為G2，并更新其 代價(jià)F。（這一步非常重要，有利于找出更加短的路徑）

6、將剩下符合要求的點(diǎn)添加進(jìn) frontier ，并計(jì)算 代價(jià) F，對 frontier 數(shù)組按照 代價(jià) F 排序

7、同時(shí)記錄下路徑，即每個(gè)節(jié)點(diǎn)都要記錄其上一個(gè)節(jié)點(diǎn)，方便最后進(jìn)行路徑的回溯找出最短路徑

8、循環(huán) 2 - 7 步驟，直到到達(dá)終點(diǎn)終止循環(huán)

補(bǔ)充：對于第四步來說，可以將可運(yùn)動(dòng)方向改為八個(gè)方向，即上、下、左、右、左上、左下、右上、右下，需要注意的是每次運(yùn)動(dòng)的步長就不只是 1 了，還有根號 2 ，我寫的A*算法仿真（文章開頭的那個(gè)演示）就是按照八個(gè)方向來寫的，這樣得出的路徑會(huì)更貼合實(shí)際。