無人車路徑規劃演算法實現

A. 有哪些應用於移動機器人路徑規劃的演算法

機器人家上了解到，在二維二值地圖（FREE or OCCUPIED）場景下進行路徑規劃的方法。我看之前有同學在回答的時候配上了這幅圖：

這幅圖上的演算法羅列的還是很全面的，體現了各個演算法的出生順序。但是並不能很好的對他們進行一個本質的分類。剛剛那位同學說的graph-based和sampling-based的分類方法我感覺有點概念重疊不能夠對規劃演算法進行這樣的分類，下面通過自己這一年多的研究和實踐對規劃演算法進行一個簡單的分類：

這幅圖上的演算法羅列的還是很全面的，體現了各個演算法的出生順序。但是並不能很好的對他們進行一個本質的分類。剛剛那位同學說的graph-based和sampling-based的分類方法我感覺有點概念重疊不能夠對規劃演算法進行這樣的分類，下面通過自己這一年多的研究和實踐對規劃演算法進行一個簡單的分類：

兩大類：
1. 完備的（complete)
2. 基於采樣的（sampling-based）又稱為概率完備的

一 完備的規劃演算法

A*演算法

所謂完備就是要達到一個systematic的標准，即：如果在起始點和目標點間有路徑解存在那麼一定可以得到解，如果得不到解那麼一定說明沒有解存在。
這一大類演算法在移動機器人領域通常直接在occupancy grid網格地圖上進行規劃（可以簡單理解成二值地圖的像素矩陣）以深度優先尋路演算法、廣度優先尋路演算法、Dijkstra(迪傑斯特拉)演算法為始祖，以A*演算法(Dijstra演算法上以減少計算量為目的加上了一個啟發式代價)最為常用，近期的Theta*演算法是在A*演算法的基礎上增加了line-of-sight優化使得規劃出來的路徑不完全依賴於單步的柵格形狀（答主以為這個演算法意義不大，不就是規劃了一條路徑再簡單平滑了一下么）。
完備的演算法的優勢在與它對於解的捕獲能力是完全的，但是由此產生的缺點就是演算法復雜度較大。這種缺點在二維小尺度柵格地圖上並不明顯，但是在大尺度，尤其是多維度規劃問題上，比如機械臂、蛇形機器人的規劃問題將帶來巨大的計算代價。這樣也直接促使了第二大類演算法的產生。

二 基於采樣的規劃演算法

RRT-connect演算法
這種演算法一般是不直接在grid地圖進行最小柵格解析度的規劃，它們採用在地圖上隨機撒一定密度的粒子來抽象實際地圖輔助規劃。如PRM演算法及其變種就是在原始地圖上進行撒點，抽取roadmap在這樣一個拓撲地圖上進行規劃；RRT以及其優秀的變種RRT-connect則是在地圖上每步隨機撒一個點，迭代生長樹的方式，連接起止點為目的，最後在連接的圖上進行規劃。這些基於采樣的演算法速度較快，但是生成的路徑代價（可理解為長度）較完備的演算法高，而且會產生「有解求不出」的情況（PRM的逢Narrow space卒的情況）。這樣的演算法一般在高維度的規劃問題中廣泛運用。

三 其他規劃演算法
除了這兩類之外還有間接的規劃演算法：Experience-based（Experience Graph經驗圖演算法）演算法：基於經驗的規劃演算法，這是一種存儲之前規劃路徑，建立知識庫，依賴之進行規劃的方法，題主有興趣可以閱讀相關文獻。這種方法犧牲了一定的空間代價達到了速度與完備兼得的優勢。此外還有基於廣義Voronoi圖的方法進行的Fast-marching規劃，類似dijkstra規劃和勢場的融合，該方法能夠完備地規劃出位於道路中央，遠離障礙物的路徑。答主最近也在研究此類演算法相關的工作。

APF（人工勢場）演算法

至於D* 、勢場法、DWA(動態窗口法)、SR-PRM屬於在動態環境下為躲避動態障礙物、考慮機器人動力學模型設計的規劃演算法。

B. A*演算法介紹

姓名：車文揚 學號：16020199006

【嵌牛導讀】：A*演算法的逐步詳解

【嵌牛鼻子】：啟發式演算法

【嵌牛提問】：A*演算法的原理是什麼？

【嵌牛正文】：

A*演算法

路徑規劃是指的是機器人的最優路徑規劃問題，即依據某個或某些優化准則（如工作代價最小、行走路徑最短、行走時間最短等），在工作空間中找到一個從起始狀態到目標狀態能避開障礙物的最優路徑。機器人的路徑規劃應用場景極豐富，最常見如游戲中NPC及控制角色的位置移動，網路地圖等導航問題，小到家庭掃地機器人、無人機大到各公司正爭相開拓的無人駕駛汽車等。

目前路徑規劃演算法分為：

A*演算法原理：

在計算機科學中，A*演算法作為Dijkstra演算法的擴展，因其高效性而被廣泛應用於尋路及圖的遍歷，如星際爭霸等游戲中就大量使用。在理解演算法前，我們需要知道幾個概念：

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

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

父節點（parent）：在路徑規劃中用於回溯的節點，開發時可考慮為雙向鏈表結構中的父結點指針。

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

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

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

我們把要搜尋的區域劃分成了正方形的格子。這是尋路的第一步，簡化搜索區域。這個特殊的方法把我們的搜索區域簡化為了2 維數組。數組的每一項代表一個格子，它的狀態就是可走(walkalbe)或不可走(unwalkable) 。現用A*演算法尋找出一條自A到B的最短路徑，每個方格的邊長為10，即垂直水平方向移動開銷為10。因此沿對角移動開銷約等於14。具體步驟如下：

從起點 A 開始，把它加入到一個由方格組成的open list(開放列表) 中，這個open list像是一個購物清單。Open list里的格子是可能會是沿途經過的，也有可能不經過。因此可以將其看成一個待檢查的列表。查看與A相鄰的8個方格 ，把其中可走的 (walkable) 或可到達的(reachable) 方格加入到open list中。並把起點 A 設置為這些方格的父節點 (parent node) 。然後把 A 從open list中移除，加入到close list(封閉列表) 中，close list中的每個方格都是不需要再關注的。

如下圖所示，深綠色的方格為起點A，它的外框是亮藍色，表示該方格被加入到了close list 。與它相鄰的黑色方格是需要被檢查的，他們的外框是亮綠色。每個黑方格都有一個灰色的指針指向他們的父節點A。

下一步，我們需要從open list中選一個與起點A相鄰的方格。但是到底選擇哪個方格好呢？選F值最小的那個。我們看看下圖中的一些方格。在標有字母的方格中G = 10 。這是因為水平方向從起點到那裡只有一個方格的距離。與起點直接相鄰的上方，下方，左方的方格的G 值都是10 ，對角線的方格G 值都是14 。H值通過估算起點到終點( 紅色方格) 的Manhattan 距離得到，僅作橫向和縱向移動，並且忽略沿途的障礙。使用這種方式，起點右邊的方格到終點有3 個方格的距離，因此H = 30 。這個方格上方的方格到終點有4 個方格的距離( 注意只計算橫向和縱向距離) ，因此H = 40 。

比較open list中節點的F值後，發現起點A右側節點的F=40，值最小。選作當前處理節點，並將這個點從Open List刪除，移到Close List中。

對這個節點周圍的8個格子進行判斷，若是不可通過（比如牆，水，或是其他非法地形）或已經在Close List中，則忽略。否則執行以下步驟：

若當前處理節點的相鄰格子已經在Open List中，則檢查這條路徑是否更優，即計算經由當前處理節點到達那個方格是否具有更小的 G值。如果沒有，不做任何操作。相反，如果G值更小，則把那個方格的父節點設為當前處理節點 ( 我們選中的方格 ) ，然後重新計算那個方格的 F 值和 G 值。

若當前處理節點的相鄰格子不在Open List中，那麼把它加入，並將它的父節點設置為該節點。

按照上述規則我們繼續搜索，選擇起點右邊的方格作為當前處理節點。它的外框用藍線打亮，被放入了close list 中。然後我們檢查與它相鄰的方格。它右側的3個方格是牆壁，我們忽略。它左邊的方格是起點，在close list 中，我們也忽略。其他4個相鄰的方格均在open list 中，我們需要檢查經由當前節點到達那裡的路徑是否更好。我們看看上面的方格，它現在的G值為14 ，如果經由當前方格到達那裡，G值將會為20( 其中10為從起點到達當前方格的G值，此外還要加上從當前方格縱向移動到上面方格的G值10) ，因此這不是最優的路徑。看圖就會明白直接從起點沿對角線移動到那個方格比先橫向移動再縱向移動要好。

當把4個已經在open list 中的相鄰方格都檢查後，沒有發現經由當前節點的更好路徑，因此不做任何改變。接下來要選擇下一個待處理的節點。因此再次遍歷open list ，現在open list中只有7 個方格了，我們需要選擇F值最小的那個。這次有兩個方格的F值都是54，選哪個呢？沒什麼關系。從速度上考慮，選擇最後加入open list 的方格更快。因此選擇起點右下方的方格，如下圖所示。

接下來把起點右下角F值為54的方格作為當前處理節點，檢查其相鄰的方格。我們發現它右邊是牆（牆下面的一格也忽略掉，假定牆角不能直接穿越)，忽略之。這樣還剩下 5 個相鄰的方格。當前方格下面的 2 個方格還沒有加入 open list ，所以把它們加入，同時把當前方格設為他們的父親。在剩下的 3 個方格中，有 2 個已經在 close list 中 ( 一個是起點，一個是當前方格上面的方格，外框被加亮的 ) ，我們忽略它們。最後一個方格，也就是當前方格左邊的方格，檢查經由當前方格到達那裡是否具有更小的 G 值。沒有，因此我們准備從 open list 中選擇下一個待處理的方格。

不斷重復這個過程，直到把終點也加入到了open list 中，此時如下圖所示。注意在起點下方2 格處的方格的父親已經與前面不同了。之前它的G值是28並且指向它右上方的方格。現在它的G 值為20 ，並且指向它正上方的方格。這是由於在尋路過程中的某處使用新路徑時G值更小，因此父節點被重新設置，G和F值被重新計算。

那麼我們怎樣得到實際路徑呢？很簡單，如下圖所示，從終點開始，沿著箭頭向父節點移動，直至回到起點，這就是你的路徑。

A*演算法總結：

1. 把起點加入 open list 。

2. 重復如下過程：

a. 遍歷open list ，查找F值最小的節點，把它作為當前要處理的節點，然後移到close list中

b. 對當前方格的 8 個相鄰方格一一進行檢查，如果它是不可抵達的或者它在close list中，忽略它。否則，做如下操作：

□  如果它不在open list中，把它加入open list，並且把當前方格設置為它的父親

□  如果它已經在open list中，檢查這條路徑 ( 即經由當前方格到達它那裡 ) 是否更近。如果更近，把它的父親設置為當前方格，並重新計算它的G和F值。如果你的open list是按F值排序的話，改變後你可能需要重新排序。

c. 遇到下面情況停止搜索：

□  把終點加入到了 open list 中，此時路徑已經找到了，或者

□  查找終點失敗，並且open list 是空的，此時沒有路徑。

3. 從終點開始，每個方格沿著父節點移動直至起點，形成路徑。

C. slam演算法是什麼

SLAM是Simultaneous localization and mapping縮寫，意為「同步定位與建圖」，主要用於解決機器人在未知環境運動時的定位與地圖構建問題。

Simultaneous Localization and Mapping (SLAM)原本是Robotics領域用來做機器人定位的，最早的SLAM演算法其實是沒有用視覺camera的（Robotics領域一般用Laser Range Finder來做SLAM）。

SLAM對實時性要求比較高，而要做到比較精確、穩定、可靠、適合多種場景的方案一般計算量相對較大，目前移動式設備的計算能力還不足夠支撐這么大的計算量，為了達到實時性能，往往需要在精確度和穩定性上做些犧牲。

因此在具體的應用中，往往需要根據移動設備所具有的感測器組合、計算能力、用戶場景等，選擇和深度定製合適的SLAM演算法。比如，無人駕駛汽車和手機端AR類應用的SLAM演算法就非常不同。

SLAM的典型應用領域

機器人定位導航領域：地圖建模。SLAM可以輔助機器人執行路徑規劃、自主探索、導航等任務。國內的科沃斯、塔米以及最新面世的嵐豹掃地機器人都可以通過用SLAM演算法結合激光雷達或者攝像頭的方法，讓掃地機高效繪制室內地圖，智能分析和規劃掃地環境，從而成功讓自己步入了智能導航的陣列。

VR/AR方面：輔助增強視覺效果。SLAM技術能夠構建視覺效果更為真實的地圖，從而針對當前視角渲染虛擬物體的疊加效果，使之更真實沒有違和感。VR/AR代表性產品中微軟Hololens、谷歌ProjectTango以及MagicLeap都應用了SLAM作為視覺增強手段。

無人機領域：地圖建模。SLAM可以快速構建局部3D地圖，並與地理信息系統（GIS）、視覺對象識別技術相結合，可以輔助無人機識別路障並自動避障規劃路徑，曾經刷爆美國朋友圈的Hovercamera無人機，就應用到了SLAM技術。

無人駕駛領域：視覺里程計。SLAM技術可以提供視覺里程計功能，並與GPS等其他定位方式相融合，從而滿足無人駕駛精準定位的需求。例如，應用了基於激光雷達技術Google無人駕駛車以及牛津大學MobileRoboticsGroup11年改裝的無人駕駛汽車野貓(Wildcat)均已成功路測。

以上內容參考：slam路徑規劃演算法 - CSDN

D. 基於激光雷達的SLAM和路徑規劃演算法研究與實現

本文僅供學習使用，並非商業用途，全文是針對哈爾濱工業大學劉文之的論文《移動機器人的路徑規劃與定位技術研究》進行提煉與學習。論文來源中國知網，引用格式如下：
[1]劉文之. 基於激光雷達的SLAM和路徑規劃演算法研究與實現[D].哈爾濱工業大學,2018.

相關坐標系轉換原理已經在前一篇文章寫完了，直接上轉換方程。

這里他的運動模型選擇的是基於里程計的運動模型，還有一種基於速度的運動模型，其實都差不多，整體思想都一樣。里程計是通過計算一定時間內光電編碼器輸出脈沖數來估計機器人運動位移的裝置，主要是使用光電碼盤。根據光電碼盤計算出此時輪子的速度，然後通過已知的輪子半徑來獲得單位時間 每個輪子 的位移增量。

高等數學可知單位時間位移增量就是速度，對速度在一定時間上進行積分就得到這一段時間所走過的路程。

根據上圖，我們可以求出來機器人航向角角速度、圓弧運動半徑和機器人角度變化量，由此可以解的機器人在當前時刻的位姿。

實際上也是有誤差，所以單獨依靠里程計會與實際結果產生較大誤差，所以必須引入其他的外部感測器對外部環境的觀測來修正這些誤差，從而提高定位精度。

首先肯定需要將激光雷達所測得的端點坐標從極坐標、機器人坐標中轉換到世界坐標中。

這張略過，暫時不需要看這個

路徑規劃演算法介紹：

因為該演算法會產生大量的無用臨時途徑，簡單說就是很慢，所以有了其他演算法。

了解兩種代價之後，對於每一個方塊我們採用預估代價與當前路徑代價相加的方法，這樣可以表示每一個路徑點距離終點的距離。在BFS搜索過程的基礎上，優先挑選總代價最低的那個路徑進行搜索，就可以少走不少彎路。（演算法講解 https://www.bilibili.com/video/BV1bv411y79P?from=search&seid=3623681329596549549 ）

在局部路徑規劃演算法之中，我們選用DWA演算法（dynamic window approach），又叫動態窗口法。動態窗口法主要是在速度(v, w)空間中采樣多組速度，並模擬機器人在這些速度下一定時間內的軌跡。在得到多組軌跡後，對這些軌跡進行評價，選取最優的軌跡所對應的速度來驅動機器人運動。
state sampling就是按照之前給出的全局路徑規劃，無論是Dijkstra還是A* 都可以方便的得到state sampling，DWA演算法所需要提前建立的action sampling有兩種：

但是無論是什麼情況，上述所做的工作就是把機器人的位移轉化到世界坐標中來，而不是機器人坐標系。速度采樣結束之後，只需要對小車的軌跡進行評判，就可以得到最優解了。下面介紹速度采樣的辦法。

對速度進行采樣一般有以下三個限制：

當確定了速度范圍之後，就需要根據速度解析度來對小車速度離散化，在每一時刻將小車在不同直線速度角速度組合下所即將要行駛的距離都可視化出來。

其中每一條軌跡都是很多小直線連接起來的。

需要用評價函數來對上述軌跡進行選擇，選擇最適合的軌跡

最後為了讓三個參數在評價函數里所發揮的作用均等，我們使用歸一化處理來計算權重。

演算法流程整體如下：