張潔，張林

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

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

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

本文將Dijkstra算法和動態(tài)窗口方法相結(jié)合，提出一種新的機器人混合路徑規(guī)劃方法。

1 算法原理

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

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

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

1.1 算法1（全局規(guī)劃算法）

輸入：起始點pt

輸出：最短路徑dj

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

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

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

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

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

第四步，找到點i的前一節(jié)點。從已經(jīng)標記的點集合中找到直接連接到點i的節(jié)點，并標記。

標記點i。若所有的節(jié)點已標記，則算法結(jié)束。否則，記k=i，轉(zhuǎn)到第二步繼續(xù)。

1.2 算法2（局部規(guī)劃算法）

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

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

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

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

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

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

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

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

1.3 算法3（混合規(guī)劃算法）

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

圖1 算法3的計算流程

2 仿真實驗與結(jié)果分析

2.1 實驗環(huán)境

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

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

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

2.2 實驗結(jié)果分析

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

圖2 機器人初始狀態(tài)

圖3 機器人中間狀態(tài)

圖4 機器人終止狀態(tài)

表1 機器人8次導航用時

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

3 結(jié)論

針對機器人在移動過程中的路徑規(guī)劃的問題，本文給出了一種融合Dijkstra方法和動態(tài)窗口法的混合路徑規(guī)劃方法?；趧討B(tài)窗口算法，構(gòu)造了一種考慮全局最優(yōu)路徑的評價函數(shù)，在保證路徑規(guī)劃的全局最優(yōu)性的同時，可避免動態(tài)窗口法陷入局部最優(yōu)的問題。相比較傳統(tǒng)Dijkstra算法，本文提出的融合路徑規(guī)劃的方法可實現(xiàn)機器人在實時避開障礙的同時更加節(jié)省時間。但因為實驗中障礙物分布不復雜，地圖較為簡單，以及較為理想的條件設(shè)置的障礙物半徑都相同，所以本文給出的方法在實際應用中還可進一步的擴展優(yōu)化。