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

        原文鏈接：

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

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

        1、路徑規劃定義

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

        也就是說(shuō)，應注意以下三點(diǎn)：

        ?   明確起始位置及終點(diǎn)

        ?   避開(kāi)障礙物

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

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

        這里介紹一下在游戲以及無(wú)人機航線(xiàn)規劃上最常見(jiàn)的A*算法。

        2、A*算法詳解

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

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

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

        開(kāi)放列表(Open List)：我們將路徑規劃過(guò)程中待檢測的節點(diǎn)存放于Open List中，而已檢測過(guò)的格子則存放于Close List中。

        父節點(diǎn)（parent）：在路徑規劃中用于回溯的節點(diǎn)，開(kāi)發(fā)時(shí)可考慮為雙向鏈表結構中的父節點(diǎn)指針。

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

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

        如圖中所示，綠色方塊為機器人起始位置A，紅色方塊為目標位置B，藍色為障礙物。

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

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

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

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

        若當前處理格子的相鄰格子不在Open List中，那么把它加入，并將它的父節點(diǎn)設置為該節點(diǎn)。

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

        最終效果如圖所示：

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

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

        3、自動(dòng)駕駛對路徑規劃的需求

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

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

        ?   靜態(tài)結構化環(huán)境下的路徑規劃

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

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

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

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

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

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