版權(quán)聲明：本文為CSDN博主「曼陀羅彼岸花」的原創(chuàng)文章，遵循 CC 4.0 BY-SA 版權(quán)協(xié)議，轉(zhuǎn)載請附上原文出處鏈接及本聲明。

        原文鏈接：

        https://blog.csdn.net/tiandijun/article/details/62226163

        下面是路徑規(guī)劃最常用的A*算法的介紹。

        1、路徑規(guī)劃定義

        路徑規(guī)劃是指的是機(jī)器人的最優(yōu)路徑規(guī)劃問題，即依據(jù)某個或某些優(yōu)化準(zhǔn)則（如工作代價最小、行走路徑最短、行走時間最短等），在工作空間中找到一個從起始狀態(tài)到目標(biāo)狀態(tài)能避開障礙物的最優(yōu)路徑。

        也就是說，應(yīng)注意以下三點：

        ?   明確起始位置及終點

        ?   避開障礙物

        ?   盡可能做到路徑上的優(yōu)化

        機(jī)器人的路徑規(guī)劃應(yīng)用場景極豐富，最常見如游戲中NPC及控制角色的位置移動，百度地圖等導(dǎo)航問題，小到家庭掃地機(jī)器人、無人機(jī)，大到各公司正爭相開拓的無人駕駛汽車等。

        這里介紹一下在游戲以及無人機(jī)航線規(guī)劃上最常見的A*算法。

        2、A*算法詳解

        在計算機(jī)科學(xué)中，A*算法作為Dijkstra算法的擴(kuò)展，因其高效性而被廣泛應(yīng)用于尋路及圖的遍歷，如星際爭霸等游戲中就大量使用。

        在理解算法前，我們需要知道幾個概念：

        搜索區(qū)域（The Search Area）：圖中的搜索區(qū)域被劃分為了簡單的二維數(shù)組，數(shù)組每個元素對應(yīng)一個小方格，當(dāng)然我們也可以將區(qū)域等分成是五角星、矩形等，通常將一個單位的中心點稱之為搜索區(qū)域節(jié)點（Node），而非方格（Squares）。

        開放列表(Open List)：我們將路徑規(guī)劃過程中待檢測的節(jié)點存放于Open List中，而已檢測過的格子則存放于Close List中。

        父節(jié)點（parent）：在路徑規(guī)劃中用于回溯的節(jié)點，開發(fā)時可考慮為雙向鏈表結(jié)構(gòu)中的父節(jié)點指針。

        路徑排序（Path Sorting）：具體往哪個節(jié)點移動由以下公式確定：F(n) = G(n) + H(n)。G代表的是從初始位置A沿著已生成的路徑到指定待檢測格子的移動開銷。H指定待測格子到目標(biāo)節(jié)點B的估計移動開銷。

        啟發(fā)函數(shù)（Heuristics Function）：H為啟發(fā)函數(shù)，也被認(rèn)為是一種試探，由于在找到唯一路徑前，我們不確定在前面會出現(xiàn)什么障礙物，因此用了一種計算H的算法，具體根據(jù)實際場景決定。在我們簡化的模型中，H采用的是傳統(tǒng)的曼哈頓距離（Manhattan Distance），也就是橫縱向走的距離之和。

        如圖中所示，綠色方塊為機(jī)器人起始位置A，紅色方塊為目標(biāo)位置B，藍(lán)色為障礙物。

        現(xiàn)用A*算法尋找出一條自綠色A到紅色B的最短路徑，經(jīng)簡化，每個方格的邊長為10，即垂直水平方向移動開銷為10。節(jié)點對角線為10，因此斜對角移動開銷約等于14。因此具體步驟如下：

        （1）將A點加入到Open List中，圖中所示，上下左右移動一格距離為10，斜對角移動距離為14。環(huán)繞綠色方塊的就是待檢測格子，左下角的值就是G值，右下角為H值，左上角對應(yīng)的就是F值，找到F值最小的節(jié)點作為新的起始位置。

        （2）綠色格子右側(cè)的節(jié)點F為40，選作當(dāng)前處理節(jié)點，并將這個點從Open List刪除，增加到Close List中，對這個節(jié)點周圍的8個格子進(jìn)行判斷，若是不可通過或已經(jīng)在Close List中，則忽略之。否則執(zhí)行以下步驟：

        若當(dāng)前處理格子的相鄰格子已經(jīng)在Open List中，那就計算臨近節(jié)點經(jīng)當(dāng)前處理節(jié)點到起點的距離G是否比原G值小，若小，則把相鄰節(jié)點的父節(jié)點（parent）設(shè)置為當(dāng)前處理節(jié)點。

        若當(dāng)前處理格子的相鄰格子不在Open List中，那么把它加入，并將它的父節(jié)點設(shè)置為該節(jié)點。

        （3）重復(fù)1、2步驟，直到終點B加入到了Open List中，再沿著各節(jié)點的父節(jié)點回溯遍歷，將遍歷得到的節(jié)點坐標(biāo)保存下來，所得的節(jié)點就是最短路徑。

        最終效果如圖所示：

        在Github上找到了一個A-star的c++源碼：https://github.com/booirror/data-structures-and-algorithm-in-c供參考。

        但也發(fā)現(xiàn)，在整個計算過程中，A*算法結(jié)合了啟發(fā)式方法，利用估值函數(shù)F(H)來估計途中當(dāng)前點與終點距離，并由此決定搜索方向，當(dāng)這條路失敗會重新嘗試其他路徑，但不理想的估值函數(shù)會導(dǎo)致整個算法運行很慢，而且這種算法雖說在時間上最優(yōu)，但也存在空間增長是指數(shù)級別的缺點。因此在往高維狀態(tài)空間進(jìn)行運算時，速度會受到影響，基于A*算法迭代加深的IDA*算法則有效解決了空間增長帶來的問題。

        3、自動駕駛對路徑規(guī)劃的需求

        目前業(yè)內(nèi)對自動駕駛的技術(shù)方案觀點較為一致，主要可分為四個部分：

        因此首先要做的就是對外部環(huán)境的實時獲取及車輛的動態(tài)路徑規(guī)劃。 傳統(tǒng)機(jī)器人路徑規(guī)劃大致可分三種：

        ?   靜態(tài)結(jié)構(gòu)化環(huán)境下的路徑規(guī)劃

        ?   動態(tài)已知環(huán)境下的路徑規(guī)劃

        ?   動態(tài)不確定環(huán)境下的路徑規(guī)劃

        將其與自動駕駛對應(yīng)起來，靜態(tài)的規(guī)劃就是根據(jù)地理信息以及交通規(guī)則在已知的全局地圖上進(jìn)行道路循跡，但這個技術(shù)對于目前自動駕駛實現(xiàn)來說并沒有什么實際應(yīng)用價值。

        自動駕駛需要的是對預(yù)先已選擇好的最優(yōu)路徑，甚至在未知的環(huán)境下，基于實時不確定的場景，進(jìn)行動態(tài)調(diào)整的路徑規(guī)劃技術(shù)，而這對地圖的需求、外部信息采集等就還是要依賴上一篇提及的如攝像頭、激光雷達(dá)、傳感器等硬件的支持。

        之前網(wǎng)上有在轉(zhuǎn)載的一篇《從算法上解讀自動駕駛是如何實現(xiàn)的》也有所總結(jié)，提到目前自動駕駛上應(yīng)用較廣的有Dijkstra、Lee、Floyd、雙向搜索算法以及蟻群算法，大家如果感興趣可以自行搜索學(xué)習(xí)，這里不再贅述。

        現(xiàn)有傳統(tǒng)機(jī)器人路徑規(guī)劃技術(shù)已經(jīng)發(fā)展得較為成熟，而將該技術(shù)如何更為符合場景地應(yīng)用到自動駕駛技術(shù)上還有很長的探索階段，但現(xiàn)已存在的包括A*算法在內(nèi)的一系列最優(yōu)路徑算法將會越來越由于圖論、人工智能、機(jī)器人技術(shù)、自動駕駛等多學(xué)科的融合下得到更大的發(fā)展。