移動機器人路徑規劃算法仿真及實現

郭昭烽，謝 玲，劉紅英，邱池健

(1.光大環境科技(中國)有限公司，江蘇 南京 211102；2.南京理工大學紫金學院，江蘇 南京 210046)

louisguo1983@163.com;12011833@qq.com;328392801@qq.com;865457907@qq.com

1 引言(Introduction)

路徑規劃技術是機器人領域研究的核心之一。路徑規劃是指按照一定的評價標準(如選擇最短的規劃路徑、最短的規劃時間或者最小的工作代價等)，在存有障礙物的環境地圖中，從起點到目標點之間找到可行地避免碰撞的路徑。路徑規劃大致可分為全局路徑規劃和局部路徑規劃兩大類，又有靜態、動態兩個分支。全局靜態規劃算法從經典的BFS、DFS發展到后來的Dijkstra算法和A*算法，當外部環境不斷發生變化時，又由A*算法演變到D*算法。除去經典的路徑規劃算法之外，如今人工智能相關算法也為機器人路徑規劃開拓了另一個方向，如蟻群算法、模擬退火算法、神經網絡、粒子群算法和遺傳算法等。

在機器人運動的過程中起到決定性作用的是路徑規劃技術。如何提高機器人路徑規劃的技術，從實質上看，是如何創造出效率性更高的算法，或是針對機器人實際應用場景，將已有的算法進行有針對性的優化，極大程度地促進機器人技術的發展，這對整個機器人的發展領域具有極其重要的意義。

2 路徑規劃原理(Principles of path planning)

機器人路徑規劃的過程中，主要分三步進行。

首先是根據已知的地圖信息來建模，障礙點、可行點、加權值等相關的地圖數據參數被存儲成可被算法識別的數據流。

其次，選擇合適的算法進行研究，根據機器人所需要執行的任務進行實際優化，這一點可以使機器人具有應對單一任務的專一性，根據實際情況消除不必要的計算過程，提高機器人的路徑規劃效率。

最后，根據改進的路徑規劃算法為機器人尋找一條由機器人當前位置到目標點位置的無碰撞最優路徑，機器人根據路徑信息及配置的物理參數進行尋路。機器人路徑規劃原理如圖1所示。

圖1 路徑規劃原理Fig.1 Principles of path planning

3 路徑規劃環境建模(Path planning environment modeling)

研究機器人路徑規劃，離不開對機器人移動環境的數據建模，環境建模是算法執行的關鍵，算法要依據環境信息給出結果。將機器人移動的實際三維環境空間進行規劃劃分，提取環境的信息特征，抽象計算機可以存儲和操作的數字信息。算法能根據建模的環境信息進行運算和反饋。對于局部路徑規劃而言，環境建模需對環境出現的新變量進行及時處理，能夠高效地、準確地將環境信息進行更新。這樣機器人就可以利用建立起來的地圖信息進行路徑搜索。常見的環境建模方法有柵格模型、幾何模型、拓撲模型、三位模型等。

本文采用柵格模型作為仿真實驗的建模方法。柵格法環境建模的具體步驟為：首先將規劃環境柵格化，處理成統一的單元模型數據，根據障礙物在環境中的分布信息，將對應的單元柵格處理成障礙柵格，其余為自由柵格。其次鏈接各個柵格處理成圖，然后在圖中搜尋一條可行路徑，將路徑的坐標存儲反饋給機器人，坐標即是單元柵格的單元格編號。最后機器人根據所得編號對應的實際坐標進行移動完成路徑搜索。其優點在于需要構成的二維陣列易于實現，且容易被計算機存儲、操作和顯示，同時易于修改和擴大地圖規模，便于實驗數據的獲得。

4 路徑規劃算法研究及仿真(Research and simulation of path planning algorithm)

首先設置一個尺寸為15×15柵格的障礙地圖作為每個算法的統一輸入源，其次設置不同尺寸的地圖，對各個算法進行路徑搜索時間的統計。本文對Dijkstra、A*、D*的核心算法進行研究，在Windows 10系統中使用Pycharm開發平臺，用Python語言對各個算法進行編程實現，以及設計GUI使算法中的各個步驟進行可視化處理，以此進行仿真實驗。

4.1 環境建模

使用柵格法建立一張完全有向圖，每一個柵格可以連接上下左右四個點。每個柵格的基礎屬性為笛卡爾坐標系的坐標(，)和一個判斷是否為自由柵格的屬性，以及一個保存可探索的柵格坐標的數組，其他屬性根據不同算法重新構建。柵格模型建立的地圖如圖2所示。

圖2 算法仿真的地圖Fig.2 Algorithm simulation map

白色代表障礙柵格，用數字表示的代表自由柵格。黑色柵格代表權值為1，數字3柵格代表權值為2，數字1柵格代表權值為3，數字2柵格代表權值為4。各個柵格的位置如下：

式(1)中，、代表該柵格在笛卡爾坐標系下的坐標位置，、代表柵格的像素寬度和高度，將在繪制GUI的時候使用。

4.2 Dijkstra算法

Dijkstra算法是從一個頂點到其余各頂點的最短路徑算法，解決的是有權圖中最短路徑問題。其主要特點是從起始點開始，采用貪心算法的策略，每次遍歷到始點距離最近且未訪問過的頂點的鄰接節點，直到擴展到終點為止。本文采用的是對BFS算法的改寫，BFS算法中使用的地圖，默認權值都為1，即從一個單元柵格到另外的節點單元柵格默認距離都為1。單元柵格的距離默認為1，這種情況表示在平地上運動，而現實的環境情況下，平地只是環境因素的一部分，還有山坡、低洼或者沼澤地等其他不同的環境條件，而在這些有坡度的環境因素下，機器人通過這些地方就要改變其速度，或者以時間為代價通過。也就是說，BFS得出的路徑在有權圖中并不是最優路徑。因此我們通過給每個自由柵格增加一個權的屬性，表示當前柵格到其他單元柵格的距離值，在有權圖中采用Dijkstra算法求出最短路徑。

本文使用一個優先隊列存儲，將這個要探索的單元格添加到隊列中，而單元柵格到單元柵格的距離為公式(2)所得：

作為隊列的優先級編號，距離越短，優先級越高，即優先隊列會將當前單元柵格到附近最短距離的單元柵格出隊，判斷其當前的狀態和后續節點的狀態。

在實現Dijkstra算法時，我們引用Python.queue創建一個PriorityQueue，該隊列是一個優先搜索隊列，不同于普通隊列，該隊列的出入隊是根據隊列中對象的優先級而定的。該優先隊列有兩個參數，即index和object，index為優先隊列對象object的優先值，優先隊列會根據對象的優先值進行排序，每次得到的隊頭的對象都是優先值最高的，對應的index為最低數值。優先隊列的存儲和讀取函數為push()和get()，將對象入隊：PriorityQueue.push([index,object])；獲取隊頭對象：PriorityQueue.get()。算法首先根據傳入的地圖信息初始化distance，將不是終點的節點對向的權值設置為無窮大。算法執行到最后將返回兩個字典對象parent和distance，機器人將通過遍歷parent字典尋找最短路徑。而distance記錄點到點的距離，可以通過訪問該字典求得最短路徑的長度。

分別設置起始點(0,0)、終點(9,9)和起始點(0,0)、終點(14,14)進行仿真，該算法仿真結果如圖3所示。

圖3 Dijkstra算法路徑搜索Fig.3 Dijkstra algorithm path search

根據仿真結果可以看出，第一組參數得出的最短路徑為13，第二組參數得出的最短路徑為26。通過仿真的路徑搜索可視化過程可以看出，Dijkstra算法與BFS算法類似，都是層層逼近終點，但是由于有優先值的加入，使得相對較遠的子節點成為優先選擇的對象，而不是靠近當前的節點作為下一步搜索的節點。

4.3 A＊算法

A*算法引入了啟發式搜索這個概念，也就是增加了一個啟發函數，該函數能夠計算得出一個值，該值代表當前柵格到目的柵格移動的優先級，數值越高優先級越低。可以通過下面的函數來表示節點的優先級：

其中，()是節點的最終優先級，當算法在選取下輪遍歷的節點時，選取當前節點列表中優先級最高的節點作為下次循環的起點；()是節點到目標點的距離；()為節點到目標點的預估代價。()啟發式函數如下：

式(4)是曼哈頓距離的計算，式(5)是歐拉距離的計算，對于不同的地圖模型，將選取不同的啟發式函數。

首先建立兩個列表OPENSET和CLOSESET，列表存儲的是節點對象，OPENSET將存放可以被選擇移動的節點，CLOSESET存放被選擇過或不可選擇的節點。根據Point類中的屬性進行判斷，即Point.obs和Point.close中有一個為True，則這個節點將會被保存在CLOSESET中。算法從起始點開始逐層搜索，將從OPENSET中選取代價最小的節點開始探索，根據當前節點中臨近節點列表中的節點的代價，選取最小代價的點，添加到OPENSET中，作為下一次探索的起始點。在退出當前節點時，會將當前節點的now_Point.close設置為True并添加到CLOSESET中。

分別設置起始點(0,0)、終點(9,9)和起始點(0,0)、終點(14,14)，以及分別采用曼哈頓距離和歐拉距離作為啟發式，仿真結果如圖4所示。

圖4 A*算法路徑搜索Fig.4 A* algorithm path search

如圖4所示，兩個啟發式得到的路徑結果都相同，圖4(a)、圖4(c)的距離為22，圖4(b)、圖4(d)的距離為26。但是我們可以很明顯地看出，曼哈頓距離在柵格模型中搜索的范圍相對于采用歐拉距離作為啟發式的搜索范圍更少，也就是說，A*算法在柵格模型中采用曼哈頓距離作為啟發式方程，將會縮小時間復雜度，提高算法的效率。

4.4 D＊算法

D*算法及其延伸算法廣泛應用于移動機器人和自主車輛導航中。D*算法的核心與Dijkstra算法和A*算法類似，需要維護一張OPENSET表。該列表記錄每一個單元柵格的五個狀態：NEW、OPEN、CLOSED、RAISE、LOWER。NEW代表該節點從未加入OPENSET中。OPEN代表該節點當前處于OPENSET表中。CLOSED代表該節點已被OPENSET列表移除。而RAISE和LOWER則記錄移動成本的代價，RAISE代表當前移動成本比未改變地圖信息時的成本高；LOWER代表當前移動成本比未改變地圖信息時的成本低。有了這五個狀態，算法可以在改變地圖信息后，對路徑進行重新預估和修正。

當障礙點的增加數量過多的時候，還會引發另一個問題：死鎖的出現，即障礙點的出現，導致自由柵格不能通過臨近節點找到一條新的路線。而解決這個問題的方法，則是保存當前路徑數據，然后使算法重新規劃路徑。

首先建立一張維護列表OPENLIST記錄節點的狀態，D*算法是反向搜索，所以將終點先添加到列表中。每一個單元節點有三個狀態：NEW、OPEN和CLOSED，NEW代表當前節點未被加入OPENLIST中；OPEN代表當前節點在OPENLIST中；CLOSED代表當前節點不在OPENLIST中。此時的CLOSED不再是被移除的意思，即該節點曾被列入列表中。算法的搜索是擴張過程，即每次從維護列表中選取一個節點，對其節點的移動代價進行預估，將移動代價最小的節點保存到OPENLIST中，并修改其狀態屬性，同時將當前節點和選取節點的變化狀態傳播到臨近節點，更新其對應的各個臨近節點的權值。D*算法從目標點開始搜索，即每個節點都有一個反向指針，該指針由下一步移動的節點指向當前節點，最終的路徑只要按照反向指針指向的節點搜索，就能得到路徑。

當在指定路徑上有障礙物產生時，將檢測到障礙物的附近節點放入OPENLIST中，此次將這些節點的預估值進行重新計算，只保留預估值最小的節點，并修改其狀態，同時將節點的狀態變化傳播到臨近節點。如果遇到堵塞，則按照Dijkstra算法從節點列表中選取最優的點進行回溯，并更新列表中的節點信息。

設置起點為(0,0)、終點為(14,14)，設置兩組不同后續生成的障礙點分別為：第一組：(0,5)，(3,5)，(13,10)，(14,10)；第二組：(5,10)，(5,0)，(5,2)，(12,12)。新生成的障礙點將渲染成數字2柵格，與原有障礙白色柵格形成對比，仿真結果如圖5所示。

圖5 D*算法路徑搜索Fig.5 D* algorithm path search

通過仿真結果可以看出，機器人在地圖信息改變之后，在排除鎖死情況發生的狀況下，大致會沿著原來的路徑前進，當遇到障礙物時，會選擇繞開障礙物，走臨近的可移動節點，直至終點。圖5(b)和圖5(d)中的數字1柵格表示上一次路徑的選擇，也就是說障礙生成之后，數字1柵格不是當前路徑規劃中的最優點。這就是D*算法不同于靜態環境下的路徑搜索算法的優點，即能在動態環境中實時修改前進路線，保證自身的安全。

4.5 算法仿真數據分析

首先建立尺寸不同的地圖,因為可視化算法過程中有大量的圖形輸出，會影響算法的時間復雜度，所以對于算法運行時間的數據搜集，采取封閉式測試，只保留算法的核心計算代碼，僅保留路徑輸出作為終點，采用python.time.process_time()函數，分別在算法起始點和終點設立start_time、end_time，最后將end_time與start_time相減得出的結果作為算法運行的時間，取各個算法仿真20 次所得的time，去掉最大值、最小值，取平均數作為最后的數據來源。在每輪實驗中引入一組隨機的障礙點添加到地圖中，確保封閉測試得到數據的隨機性和準確性。測試20 輪得到數據如表1、表2和表3所示。

表1 路徑規劃算法的運行時間Tab.1 Running time of the path planning algorithm

表2 A＊算法和D＊算法歐拉公式啟發式運行時間Tab.2 Heuristic running time of A* algorithm and D*algorithm Euler formula

表3 A＊算法和D＊算法曼哈頓公式啟發式運行時間Tab.3 Heuristic running time of A* algorithm and D*algorithm Manhattan formula

A*算法在將歐拉距離和曼哈頓距離分別作為不同的啟發式的時候，采用曼哈頓距離的時間消耗少于歐拉距離，因此在現實環境中，機器人使用A*算法路徑規劃時，要根據現實環境來選擇采用這兩者中的哪一個作為啟發式函數。在A*算法仿真實驗中，我們提出了兩個不同的啟發式。由于采用柵格模型建模，柵格模型類似于城市網格，可以類比，在設計公交路線系統、搜索城市中點對點的最小路徑時，便可以采用啟發式為曼哈頓距離的A*算法。而本文所實現的A*算法，其中OPENLIST和CLOSELIST都是采用List的方式存儲數據，Python中的列表是基于數組的類型，所以可以改用二叉堆這種數據結構，使其節點的添加和刪除的操作速度更快，降低耗時。

D*算法彌補了A*算法不適合在動態環境中規劃路徑的缺陷。D*算法在未出現新的障礙源時，路徑規劃算法基本與A*算法一致。在出現障礙源時，D*算法會重新對存儲的地圖信息進行刪除和插入操作，需要更多的計算，所以消耗的時間較多。

5 移動機器人路徑規劃實現(Implementation of mobile robot path planning)

路徑規劃實現如圖6所示。

圖6 路徑規劃實現Fig.6 Path planning realization

5.1 開發環境

使用的機器人小車采用JetbotNano作為開發主板，如圖6(a)所示，CPU為ARM的A57，操作系統為Unbuntu 18.10，開發平臺為JupyterLab，開發語言為Python 3.0。

通過WiFi模板進行PC和機器人之間的數據傳輸，以及搭載四個直流電機控制車輪的移動。通過對GPIO口的編程，控制直流電機的運動方向和前進速度。

5.2 具體實現

通過分析仿真實驗數據報告可以看到，由于我們在靜態環境下實現該小車的路徑規劃功能，因此選取了有代表性的A*算法來為機器人規劃行動路徑。

首先需要導入地圖數據，將二維空間地圖抽象成算法可以處理的數據；再根據算法得到的結果，編程控制小車前進。地圖的構建方式也是采用柵格模型，將實際地圖抽象成二維平面，將每個單元柵格的信息采用一組二維數組來保存。此次實驗地圖如圖6(b)所示，數字1單元格為機器人起點位置，數字2單元格為終點，黑色單元格為地圖上的障礙。

在得到A*算法得出的路徑列表之后，通過調用集成的開發庫，使用主板上的i2c總線對四個直流驅動電機進行控制。調用Jetbot.Robot庫創建一個機器人控制對象。Robot庫的forward()、backward()、left()、right()、stop()分別控制機器人前進、倒退、左轉、右轉、停止。根據地圖的參數以及實際地面情況，設置對應的前進速度和時間。

設地圖軸坐標對應為正東，反方向為正西；軸坐標對應為正南，反方向為正北。機器人起始點車頭對應方向為正南。根據路徑列表，機器人首先調整車頭方向，調整的方向為即將前進的下一個單元格的方向。設置一個變量保存當前機器人車頭正對的方向信息，隨后機器人前進一個單元格。方向參數設正南為(0,1)，正西為(-1,0)，正北為(0，-1)，正東為(1,0)，標記數值按照正南方順時針方向設置為1、2、3、4，并保存在一個判斷列表中。車頭方向調整偽代碼如下:

通過對路徑列表的循環遍歷，初始化小車車頭方向，然后前行，直至到達終點。實驗結果如圖6(c)所示。數字3單元格代表機器人前進的路線。實驗結果是機器人成功實現掉頭、轉彎、前行等功能，按照A*算法所得的路徑列表移動，準確在終點停止。

本次實物演示的地圖為一張根據機器人車身定制的2 m×2 m的塑料布材質的地圖，內部大小為20 cm×20 cm的正方形柵格。地圖在空地上展開，在保持平整的情況下，完成路徑規劃的各項參數為：前進電機速度：0.45(驅動電路參數，需要根據具體環境進行測試和調節)；左轉電機參數：0.585；右轉電機參數：5.585；延時：1 s。利用Jetbot小車自身所帶的攝像頭，使小車在每次前進、掉頭時，攝像頭根據比對地圖的數據，保證機器人小車能夠時刻居中，路線不偏移。

6 結論(Conclusion)

基于機器人路徑搜索算法的研究，本文對Dijkstra算法、A*算法、D*算法進行研究，仿真實現了各個算法在路徑搜索上的演示，通過對比各算法的時間復雜度和搜索出的最優路徑以及對應的數據，確定在移動機器人實體上采用A*算法。而通過實驗可知，在柵格網絡中，A*算法采用曼哈頓距離啟發式，仿真可以得到更加優異的結果。同時也提出了如何優化A*算法的思路，即使用二叉樹堆代替數組來降低算法的時間復雜度。最后在移動機器人上實現路徑規劃算法的演示，證明了該算法的可行性。