一種全局與局部融合的動態路徑規劃方法

張潔，張林

（商洛學院數學與計算機應用學院，陜西商洛 726000）

機器人移動過程中自主導航和避障應用研究的一個關鍵問題是如何提高移動機器人自主學習的能力，使其能夠在未知環境下通過自主學習從起點移動到達指定目標點而不發生碰撞。

文獻[1]針對多臺移動式起重機纜索并聯機器人的定位、避障規劃與控制問題，采用三維網格圖方法構建環境圖模型，并將相對定位與絕對定位方法相結合，提出了一種基于網格的人工勢場協同定位方法，對CPRMCS系統進行了全局路徑規劃。文獻[2]針對非完整約束輪式機器人的非線性跟蹤與避障控制問題，引入了擴展狀態觀測器估計輪式移動機器人的未知擾動和速度信息，并為復雜環境下的目標跟蹤和避障，設計了一種非線性控制器，使機器人在障礙物檢測區域內有效避障。文獻[3]針對移動機器人路徑規劃優化的問題，給出一個基于布谷鳥搜索算法設計的一種新的元啟發式方法，該方法在機器人與目標和障礙物之間建立了滿足機器人避障條件的函數關系，可用于移動機器人在未知或部分已知環境下的路徑規劃。此外，常見的針對機器人路徑規劃的方法還包括基于生物啟發式[4]的遺傳算法、蟻群算法及細菌勢場方法，基于模型的動態窗口法和人工勢場法，基于節點的Dijkstra算法、A*算法及D*算法，基于采樣的快速搜索隨機樹方法和Voronoi圖方法[5]。全局路徑規劃解決了機器人要去哪里的問題。定位完成后，機器人將自己的位置[6]設為起點，目標點設為終點。從起點到終點的全局路徑規劃過程中有三個基本要求：一是準確到達目標點；二是航行過程中不能與障礙物發生碰撞[7]；三是選擇最快到達目的地的路徑。傳統的Dijkstra算法雖然是最短路徑算法，但屬于貪心算法，需要遍歷每個連接節點。節點越多，耗時越長。

本文將Dijkstra算法和動態窗口方法相結合，提出一種新的機器人混合路徑規劃方法。

1 算法原理

Dijkstra算法使用了貪心算法模式求解最短路徑。該算法解決關于有向圖中單個源點到其他節點的最短路徑問題[8]，其主要思路是每次迭代時選擇的下一個節點是標記點之外距離源點最近的節點。在機器人導航領域，這個算法將一個節點固定為起始點，從而尋找到目標點的最佳路徑。

解決機器人導航過程中如何到達目的地的問題用全局路徑規劃，最優路徑可以由機器人根據全局地圖計算出，但很多不確定因素在實際移動過程中存在。如全局地圖與局部實際情況不符，某個地方多出行人或障礙物等情況[9]。局部路徑規劃非常重要，否則如果只按照全局地圖執行導航任務，如同盲人過馬路一樣危險。局部路徑規劃主要是對局部未知障礙物進行避開障礙，避障后返回已計算的全局路徑的值。

本算法的核心思想是將基于局部路徑規劃的動態窗口法和最短路徑算法的全局規劃算法結合起來使用，算法首先根據全局規劃方法規劃一個最短行駛路徑，在機器人移動過程中使用動態窗口法不斷進行局部規劃，并根據局部規劃和全局規劃后的最優解進行實際的路徑規劃。

1.1 算法1（全局規劃算法）

輸入：起始點pt

輸出：最短路徑dj

假設算法路徑網中每個節點都有標號（dt,Pt），dt是從出發點s到點t的最短路徑長度；Pt表示從s到t的最短路徑中t點的前一個點。求解從出發點s到點t的最短的路徑算法的基本過程為：

第一步，初始化。出發點設置為Ps=null ds=0；所有其他點di=∞，pi未定義標記起始點s，記k=s，其他所有點設為未標記。

第二步，檢驗從所有已標記的點k到其他的直接連接的未標記的點j的距離，并設置：

其中，表示 w（k，j）從 k到 j的路徑長度。

第三步，選取下一個節點。從所有未標記的點中選取路徑最小的點i，點i被選為最短路徑中的一點，并設為已標記的點。

第四步，找到點i的前一節點。從已經標記的點集合中找到直接連接到點i的節點，并標記。

標記點i。若所有的節點已標記，則算法結束。否則，記k=i，轉到第二步繼續。

1.2 算法2（局部規劃算法）

動態窗口法（DWA方法）是基于機器人運動學和動力學[10]的局部路徑規劃算法，核心思想是將路徑規劃問題轉化成速度矢量空間上的約束優化問題。本算法中輸入參數包括：當前狀態、模型參數、目標點、評價函數的參數、障礙物位置和障礙物半徑。輸出參數為：控制量和軌跡集合。由于機器人運動可以分解為直線速度ν和旋轉速度 ω，所以 Vr(ν,ω)集合便是機器人的速度矢量空間，在此空間中實現對機器人運動控制的命令搜索。速度矢量空間Vr由速度矢量集合Vs、速度動態窗口Va和許可速度矢量Vd的交集構成。

式(2)中的Vd是許可速度，表示機器人的加速度能力不導致碰撞的速度，可表示為：

式(3)中，distant(ν,ω)表示機器人障礙物的最小距離，aν，aω為機器人平移的加速度和旋轉加速度。

為了從速度矢量空間中得到機器人在k+1時刻的最優值，需要提前設定DWA的標準目標函數：

heading(ν,ω)函數可以對機器人 k+1 時刻朝向目標點的程度作出評估，當機器人與目標點的夾角為0°時，此函數取最大值：

distan(ν,ω)函數可以對機器人 k+1 時刻自身位置和目標點的距離作出評估，函數值越大說明與目標點距離越大，發生碰撞的幾率越小；函數值越小說明離障礙物越近，發生碰撞幾率越大。distant(ν,ω)定義如式(6)所示：

νel(ν,ω)函數是機器人速度的大小，其定義如式(7)所示：

其中ν是機器人平均速度，νmax是在νr機器人的最大速度。

1.3 算法3（混合規劃算法）

本算法的輸入參數分別為算法1和算法2的輸出參數。輸出參數是優化過的運動方向和時速。算法首先會每隔一個時間間隔檢測局部規劃算法中計算出的局部移動速度和方向是否和全局的一致，如果一致算法會休眠一個時間周期后重新獲取新的輸入參數。如果不一致，就會計算全局路徑中可達的抽樣節點到當前位置的距離dn及抽樣節點到目標節點的距離dj。然后計算dn與dj相加后的計算值，選擇計算值最小的一條路徑繼續前進，一個時間周期后重新再檢測輸入參數直到到達目的地為止。具體流程如圖1所示。

圖1 算法3的計算流程

2 仿真實驗與結果分析

2.1 實驗環境

仿真平臺使用matlab進行驗證，機器人狀態包括坐標（x,y）、航向角、速度、角速度。機器人初始位置設為（0,0），目標點位置為（10,10），共設置 10個障礙物，坐標點分別為（0,2），（2,4），（2,5），（4,2），（5,4），（5,6），（5,9），（8,8），（8,9），（7,9），用于沖突判定的障礙物半徑R=0.5，模擬區域范圍為[-1 11，-1 11]。評價函數參數[11]分別由航向得分比重、距離得分比重、速度得分比重、向前模擬軌跡的時間衡量，模擬實驗結果包括走過的軌跡及導航時間。

DWA輸入參數有返回控制量及軌跡，機器人移動到下一個時刻的狀態量根據當前速度和角速度推導[12]。由坐標上兩點之間的距離確定機器人是否到達目標點，記錄機器人走過的所有位置坐標，作為機器人移動的歷史數據。

將本文提出的融合方法與DWA方法進行實驗比對。

2.2 實驗結果分析

實驗結果表明，在保證其他約束條件一致的情況下，兩種算法都能成功實現移動機器人由起始點至目標點的導航，且圖2機器人起始狀態、圖3中間狀態和圖4終止狀態圖表明，兩種算法的移動軌跡基本一致，但根據表1和表2的導航用時數據，兩種算法的導航用時有所不同。在保持障礙物數量及位置不變及障礙物半徑相同的條件下，8次實驗中融合算法的導航用時比只使用DWA算法的導航用時平均減少了5.467 s，減少比例約為12.3%。

圖2 機器人初始狀態

圖3 機器人中間狀態

圖4 機器人終止狀態

表1 機器人8次導航用時

表2 機器人8次導航平均用時

3 結論

針對機器人在移動過程中的路徑規劃的問題，本文給出了一種融合Dijkstra方法和動態窗口法的混合路徑規劃方法。基于動態窗口算法，構造了一種考慮全局最優路徑的評價函數，在保證路徑規劃的全局最優性的同時，可避免動態窗口法陷入局部最優的問題。相比較傳統Dijkstra算法，本文提出的融合路徑規劃的方法可實現機器人在實時避開障礙的同時更加節省時間。但因為實驗中障礙物分布不復雜，地圖較為簡單，以及較為理想的條件設置的障礙物半徑都相同，所以本文給出的方法在實際應用中還可進一步的擴展優化。