版權(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ù)某個(gè)或某些優(yōu)化準(zhǔn)則（如工作代價(jià)最小、行走路徑最短、行走時(shí)間最短等），在工作空間中找到一個(gè)從起始狀態(tài)到目標(biāo)狀態(tài)能避開障礙物的最優(yōu)路徑。
也就是說，應(yīng)注意以下三點(diǎn)：
? 明確起始位置及終點(diǎn)
? 避開障礙物
? 盡可能做到路徑上的優(yōu)化
機(jī)器人的路徑規(guī)劃應(yīng)用場景極豐富，最常見如游戲中NPC及控制角色的位置移動(dòng)，百度地圖等導(dǎo)航問題，小到家庭掃地機(jī)器人、無人機(jī)，大到各公司正爭相開拓的無人駕駛汽車等。
這里介紹一下在游戲以及無人機(jī)航線規(guī)劃上最常見的A*算法。
2、A*算法詳解
在計(jì)算機(jī)科學(xué)中，A*算法作為Dijkstra算法的擴(kuò)展，因其高效性而被廣泛應(yīng)用于尋路及圖的遍歷，如星際爭霸等游戲中就大量使用。
在理解算法前，我們需要知道幾個(gè)概念：
搜索區(qū)域（The Search Area）：圖中的搜索區(qū)域被劃分為了簡單的二維數(shù)組，數(shù)組每個(gè)元素對應(yīng)一個(gè)小方格，當(dāng)然我們也可以將區(qū)域等分成是五角星、矩形等，通常將一個(gè)單位的中心點(diǎn)稱之為搜索區(qū)域節(jié)點(diǎn)（Node），而非方格（Squares）。
開放列表(Open List)：我們將路徑規(guī)劃過程中待檢測的節(jié)點(diǎn)存放于Open List中，而已檢測過的格子則存放于Close List中。
父節(jié)點(diǎn)（parent）：在路徑規(guī)劃中用于回溯的節(jié)點(diǎn)，開發(fā)時(shí)可考慮為雙向鏈表結(jié)構(gòu)中的父節(jié)點(diǎn)指針。
路徑排序（Path Sorting）：具體往哪個(gè)節(jié)點(diǎn)移動(dòng)由以下公式確定：F(n) = G(n) + H(n)。G代表的是從初始位置A沿著已生成的路徑到指定待檢測格子的移動(dòng)開銷。H指定待測格子到目標(biāo)節(jié)點(diǎn)B的估計(jì)移動(dòng)開銷。
啟發(fā)函數(shù)（Heuristics Function）：H為啟發(fā)函數(shù)，也被認(rèn)為是一種試探，由于在找到唯一路徑前，我們不確定在前面會(huì)出現(xiàn)什么障礙物，因此用了一種計(jì)算H的算法，具體根據(jù)實(shí)際場景決定。在我們簡化的模型中，H采用的是傳統(tǒng)的曼哈頓距離（Manhattan Distance），也就是橫縱向走的距離之和。
如圖中所示，綠色方塊為機(jī)器人起始位置A，紅色方塊為目標(biāo)位置B，藍(lán)色為障礙物。
現(xiàn)用A*算法尋找出一條自綠色A到紅色B的最短路徑，經(jīng)簡化，每個(gè)方格的邊長為10，即垂直水平方向移動(dòng)開銷為10。節(jié)點(diǎn)對角線為10，因此斜對角移動(dòng)開銷約等于14。因此具體步驟如下：
（1）將A點(diǎn)加入到Open List中，圖中所示，上下左右移動(dòng)一格距離為10，斜對角移動(dòng)距離為14。環(huán)繞綠色方塊的就是待檢測格子，左下角的值就是G值，右下角為H值，左上角對應(yīng)的就是F值，找到F值最小的節(jié)點(diǎn)作為新的起始位置。
（2）綠色格子右側(cè)的節(jié)點(diǎn)F為40，選作當(dāng)前處理節(jié)點(diǎn)，并將這個(gè)點(diǎn)從Open List刪除，增加到Close List中，對這個(gè)節(jié)點(diǎn)周圍的8個(gè)格子進(jìn)行判斷，若是不可通過或已經(jīng)在Close List中，則忽略之。否則執(zhí)行以下步驟：
若當(dāng)前處理格子的相鄰格子已經(jīng)在Open List中，那就計(jì)算臨近節(jié)點(diǎn)經(jīng)當(dāng)前處理節(jié)點(diǎn)到起點(diǎn)的距離G是否比原G值小，若小，則把相鄰節(jié)點(diǎn)的父節(jié)點(diǎn)（parent）設(shè)置為當(dāng)前處理節(jié)點(diǎn)。
若當(dāng)前處理格子的相鄰格子不在Open List中，那么把它加入，并將它的父節(jié)點(diǎn)設(shè)置為該節(jié)點(diǎn)。
（3）重復(fù)1、2步驟，直到終點(diǎn)B加入到了Open List中，再沿著各節(jié)點(diǎn)的父節(jié)點(diǎn)回溯遍歷，將遍歷得到的節(jié)點(diǎn)坐標(biāo)保存下來，所得的節(jié)點(diǎn)就是最短路徑。
最終效果如圖所示：
在Github上找到了一個(gè)A-star的c++源碼：https://github.com/booirror/data-structures-and-algorithm-in-c供參考。
但也發(fā)現(xiàn)，在整個(gè)計(jì)算過程中，A*算法結(jié)合了啟發(fā)式方法，利用估值函數(shù)F(H)來估計(jì)途中當(dāng)前點(diǎn)與終點(diǎn)距離，并由此決定搜索方向，當(dāng)這條路失敗會(huì)重新嘗試其他路徑，但不理想的估值函數(shù)會(huì)導(dǎo)致整個(gè)算法運(yùn)行很慢，而且這種算法雖說在時(shí)間上最優(yōu)，但也存在空間增長是指數(shù)級別的缺點(diǎn)。因此在往高維狀態(tài)空間進(jìn)行運(yùn)算時(shí)，速度會(huì)受到影響，基于A*算法迭代加深的IDA*算法則有效解決了空間增長帶來的問題。
3、自動(dòng)駕駛對路徑規(guī)劃的需求
目前業(yè)內(nèi)對自動(dòng)駕駛的技術(shù)方案觀點(diǎn)較為一致，主要可分為四個(gè)部分：
因此首先要做的就是對外部環(huán)境的實(shí)時(shí)獲取及車輛的動(dòng)態(tài)路徑規(guī)劃。 傳統(tǒng)機(jī)器人路徑規(guī)劃大致可分三種：
? 靜態(tài)結(jié)構(gòu)化環(huán)境下的路徑規(guī)劃
? 動(dòng)態(tài)已知環(huán)境下的路徑規(guī)劃
? 動(dòng)態(tài)不確定環(huán)境下的路徑規(guī)劃
將其與自動(dòng)駕駛對應(yīng)起來，靜態(tài)的規(guī)劃就是根據(jù)地理信息以及交通規(guī)則在已知的全局地圖上進(jìn)行道路循跡，但這個(gè)技術(shù)對于目前自動(dòng)駕駛實(shí)現(xiàn)來說并沒有什么實(shí)際應(yīng)用價(jià)值。
自動(dòng)駕駛需要的是對預(yù)先已選擇好的最優(yōu)路徑，甚至在未知的環(huán)境下，基于實(shí)時(shí)不確定的場景，進(jìn)行動(dòng)態(tài)調(diào)整的路徑規(guī)劃技術(shù)，而這對地圖的需求、外部信息采集等就還是要依賴上一篇提及的如攝像頭、激光雷達(dá)、傳感器等硬件的支持。
之前網(wǎng)上有在轉(zhuǎn)載的一篇《從算法上解讀自動(dòng)駕駛是如何實(shí)現(xiàn)的》也有所總結(jié)，提到目前自動(dòng)駕駛上應(yīng)用較廣的有Dijkstra、Lee、Floyd、雙向搜索算法以及蟻群算法，大家如果感興趣可以自行搜索學(xué)習(xí)，這里不再贅述。
現(xiàn)有傳統(tǒng)機(jī)器人路徑規(guī)劃技術(shù)已經(jīng)發(fā)展得較為成熟，而將該技術(shù)如何更為符合場景地應(yīng)用到自動(dòng)駕駛技術(shù)上還有很長的探索階段，但現(xiàn)已存在的包括A*算法在內(nèi)的一系列最優(yōu)路徑算法將會(huì)越來越由于圖論、人工智能、機(jī)器人技術(shù)、自動(dòng)駕駛等多學(xué)科的融合下得到更大的發(fā)展。
