改進D* Lite和人工勢場法的移動機器人路徑規(guī)劃研究

杜 軒，歐資臻

（三峽大學機械與動力學院 三峽大學水電機械設備設計與維護湖北重點實驗室，湖北 443002）

0 引言

移動機器人的應用已經(jīng)越來越多的滲透到多個領域，而路徑規(guī)劃問題是移動機器人在應用中一直需要解決的首要問題和難點問題。移動機器人的路徑規(guī)劃就是在給定的環(huán)境中從起始點到目標點，規(guī)劃出了一條在時間、距離和空間上無碰撞的最滿意路徑，其核心問題在于路徑規(guī)劃算法。

路徑規(guī)劃的首要工作是對環(huán)境進行建模，柵格地圖以其較強的實用性在路徑規(guī)劃領域得到了廣泛的使用，主流算法有gmapping[1]、hector[2]和cartographer[3]等，由此也衍生出了一系列以基于柵格地圖為環(huán)境的路徑規(guī)劃算法。

針對不確定的環(huán)境，D*Lite[4]是很好的選擇。然而，一方面由于基于柵格地圖的八鄰域搜索方式限制，所規(guī)劃出的路徑會出現(xiàn)很多不必要的冗余節(jié)點；另一方面，在障礙物出現(xiàn)后，D*Lite重規(guī)劃所規(guī)劃的路徑離障礙物較近，在機器人實際運行中會很容易發(fā)生碰撞。

針對上述問題，本文首先對D*Lite所規(guī)劃的路徑節(jié)點，構造了一個節(jié)點篩選機制，去除冗余節(jié)點，使路徑變得更加平滑；重規(guī)劃環(huán)節(jié)則結合人工勢場，使其與障礙物之間始終保持一個安全的距離；對于人工勢場法可能出現(xiàn)的局部極值問題，本文采取設置虛擬目標點的方法，來擺脫困境。

1 路徑規(guī)劃算法的研究現(xiàn)狀

依據(jù)不同的使用背景環(huán)境，可將路徑規(guī)劃算法分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃主要運用于解決已知環(huán)境中的移動機器人的路徑規(guī)劃問題，例如A*[5]、快速擴展隨機樹RRT[6]和D*Lite等。D*Lite算法是被提出用以實現(xiàn)變起點、定目標點的路徑規(guī)劃，是一種啟發(fā)式的增量搜索算法，借鑒了LPA*算法的思想，使得機器人能夠在有未知障礙物的環(huán)境下可以快速進行動態(tài)重規(guī)劃；局部路徑規(guī)劃算法主要用于解決在局部環(huán)境，遇上未知障礙物后的移動機器人路徑規(guī)劃問題，例如動態(tài)窗口法（DWA）[7]、遺傳算法[8]和人工勢場法[9]等等，DWA主要是通過速度空間和狀態(tài)空間來預測運動軌跡，再通過評價函數(shù)來選取最優(yōu)路徑，但由于局部環(huán)境不容易建模和控制，所以可能會存在數(shù)據(jù)誤差導致碰撞的風險，而人工勢場法則是通過對障礙物建立斥力場來始終保持一個安全距離，相比之下人工勢場法更加安全。

基于柵格地圖進行路徑搜索的D*Lite，搜索采取八鄰域的搜索方式，如圖1所示，導致了偏轉的角度限制于的的整數(shù)倍，這不可避免的導致所規(guī)劃的路徑會存在如圖2所示的冗余拐點，虛線直線為理想最優(yōu)路徑，而黑色路徑則是實際上算法得到的路徑。

圖1 八鄰域搜索方式

圖2 冗余拐點

對于上述問題，有Theta*[10]、Lazy Theta*[11]等方法，可在靜態(tài)已知環(huán)境下，對任意角度進行規(guī)劃，但不適用于動態(tài)環(huán)境；動態(tài)環(huán)境下，F(xiàn)ile D*[12]方法采取路徑插值的方式可以很好的平滑路徑，但消耗資源較多；黃魯?shù)热颂岢隽私Y合懶惰視線算法的方法，很好地解決了八鄰域擴展方式的搜索[13]，但算法計算時間較長。

路徑規(guī)劃算法的衡量指標除了路徑的長短外，還需要考慮其安全性，即與障礙物之間的距離。尤其是存在未知障礙物時，所觸發(fā)的重規(guī)劃環(huán)節(jié)。

按照傳統(tǒng)D*Lite，當機器人在按照預規(guī)劃路徑行走到中途時，發(fā)現(xiàn)了存在未知的障礙物導致所規(guī)劃的路徑不能繼續(xù)正常前進，則以機器人當前所在位置為新的起始點。

首先重新計算可穿越柵格的新啟發(fā)值h，即：

式中：

s`表示當前節(jié)點s的前繼節(jié)點。

c(s，s`)表示當前節(jié)點s到s`的歐氏距離。

從目標點開始向起始點重新維護rhs(s)值和g(s)，rhs(s)計算公式如下：

同時計算k(s)值，即：

傳統(tǒng)D*Lite的重規(guī)劃便是通過上述式(3)來選取最小的k(s)值，從而選取所需要進行維護后續(xù)節(jié)點的rhs(s)值和g(s)值。但由此存在的一個問題是，突變?yōu)檎系K物柵格的自由柵格本是全局規(guī)劃在其區(qū)域中選出的最優(yōu)點，自由柵格突變?yōu)檎系K物柵格后，其附近所在區(qū)域依舊是較優(yōu)區(qū)域，所以不可避免的在重規(guī)劃環(huán)節(jié)中，會優(yōu)先選取與未知障礙物很近的柵格，由此所規(guī)劃的局部路徑必然會帶來很大的風險。

在機器人運動中，采用人工勢場法進行避障時，當遇到特定位置及形狀的未知障礙物時，機器人很可能會在障礙物的斥力場和目標點的引力場共同作用下，達到一個力平衡，這將會導致機器人陷入局部極值的困境，無法進行下一步的規(guī)劃。

本文通過對傳統(tǒng)D*Lite算法及人工勢場法進行分析研究，由于存在大量路徑冗余節(jié)點、局部重規(guī)劃的路徑容易與障礙物發(fā)生碰撞和人工勢場陷入局部極值的問題，影響了規(guī)劃路徑的長度、安全性和有效性。因此本文通過節(jié)點優(yōu)化機制、融入人工勢場法的局部重規(guī)劃環(huán)節(jié)和設置虛擬目標點的方法，使其規(guī)劃的路徑在實際運行中更加安全、高效。

2 改進算法的原理

2.1 全局路徑節(jié)點優(yōu)化

針對D*Lite算法所規(guī)劃的路徑存在較多冗余節(jié)點等缺點，提出了一種節(jié)點篩選機制，保留路徑必經(jīng)的關鍵節(jié)點，使經(jīng)過節(jié)點篩選后的路徑長度更短、拐點更少，路徑更優(yōu)。節(jié)點篩選機制的流程圖具體步驟如下：

圖3 節(jié)點篩選機制流程圖

首先獲取D*Lite算法初步所規(guī)劃的所有全局路徑節(jié)點，用U{Pi，1≤i≤n}表示，其中P1代表起始點，Pn代表目標點，P1和Pn都加入至節(jié)點集合L。

從起始點P1做直線依次連接P2、P3…Pi等，判斷P1Pi是否經(jīng)過障礙物，若直線經(jīng)過障礙物，則將Pi-1加入至集合L中，并判定P2，…，Pi-2為冗余節(jié)點，以此類推，Pi，2繼續(xù)往前出發(fā)，直至目標點，將所有關鍵節(jié)點全部添加至集合L，最終輸出集合L所代表的最優(yōu)路徑。

2.2 局部路徑避障優(yōu)化

針對傳統(tǒng)D*Lite重規(guī)劃路徑與未知障礙物之間的距離過近的問題，提出一種引入人工勢場法的局部避障策略，如圖4所示。

圖4 局部避障策略

當機器人按全局路徑行進時，發(fā)現(xiàn)存在未知障礙物，首先判障礙物是否會影響前進的軌跡，即機器人行走到障礙物的作用范圍(Q*)內(nèi)，則開始進行結合人工勢場法的重規(guī)劃。

初始化障礙物危險范圍(0.3Q*)內(nèi)的g(s)值和rhs(s)值為無窮大，這樣在規(guī)劃中則不需要考慮地圖上危險柵格的啟發(fā)值等。判斷并計算機器人當前所處的位置與障礙物的距離D(s，sd)。

D(s，sd)計算公式為：

式中：

sd表示未知障礙物所處的柵格。

Q*表示為以障礙物為中心的斥力場作用范圍。

當機器人與障礙物間的距離處于一個安全距離，即0.3Q*＜D(s，sd)＜Q*，且障礙物對預規(guī)劃的路徑存在阻礙，則開始進行局部規(guī)劃。首先從預規(guī)劃的全局路徑中各個關鍵節(jié)點所劃分的分段路徑中選取障礙物所在區(qū)間得分段路徑，從而將區(qū)間末端節(jié)點設為局部規(guī)劃的目標點s`goal，將機器人當前所在的位置設為局部規(guī)劃的起始點s`start。

以未知障礙物為斥力中心建立斥力勢場，對機器人產(chǎn)生排斥勢場Urep和排斥力Frep(s)：

式中：

η表示斥力增益。

D(s)表示點s與障礙物之間最近的距離。

以局部目標點為引力中心建立引力勢場，對機器人產(chǎn)生引力勢場Uatt和引力Fatt(s)：

式中：

ζ：引力增益。

將上述斥力和引力相結合則得到該點的總勢能U(q)：

先將上述的該點斥力結合至傳統(tǒng)D*Lite的k(s)值當中，將式(3)變?yōu)椋?/p>

再將合力F(s)融入rhs(s)的計算公式中，將式(2)變?yōu)椋?/p>

最終，根據(jù)所維護的rhs(s)值和g(s)輸出所規(guī)劃的局部路徑。假設B1和E5分別為全局路徑中的某一段起始點和目標點，根據(jù)傳統(tǒng)D*Lite全局規(guī)劃算法，所規(guī)劃的預規(guī)劃路徑為B1-B2-C3-D4-E5，如圖5(a)所示。當機器人行走到B2時，發(fā)現(xiàn)原本的自由柵格C3變?yōu)檎系K物柵格，則計算新的啟發(fā)值h(s)，如圖5(b)所示。傳統(tǒng)D*Lite算法便依據(jù)新的啟發(fā)值h(s)，開始進行重規(guī)劃，將B2E5分別設置為局部規(guī)劃的初始點和目標點，最終得到B2-C2-D3-D4-E5，可以看出是路徑圍繞在未知障礙物C3周圍且很近，如圖5(c)所示。

在傳統(tǒng)D*Lite重規(guī)劃的基礎上，引入人工勢場法，便立即將當前所在位置B2設置為局部規(guī)劃新起點并以此為中心建立引力勢場，E5設置為局部規(guī)劃目標點，以C3及其附近的障礙物為中心建立斥力勢場。

重規(guī)劃從E5開始向B2進行路徑規(guī)劃，E5往外擴張節(jié)點E4和D5，兩者傳統(tǒng)的k(s)值和rhs(s)值相等，如圖5(c)所示，但根據(jù)改進后的式(11)，D5的值將會高于E4，如圖5(d)所示，最終選取E4作為下一節(jié)點，舍棄D5，以此類推，得到E3-E2-D1-C1后續(xù)節(jié)點，并最終確定B2-C1-D1-E2-E3-E4-E5路徑。

圖5 重規(guī)劃路徑對比

如此，融入了人工勢場法的局部路徑規(guī)劃，最終所規(guī)劃出的路徑，將會與障礙物始終保持一個安全的距離。

2.3 避免局部極值的優(yōu)化

為了擺脫人工勢場陷入局部極值的困境，本文提出了一種設置虛擬目標點的方法來應對局部極值的處理機制，如圖6所示。

圖6 局部極值處理機制

當機器人陷入斥力引力相平衡的困境時，如圖7所示，F(xiàn)att與Frep大小相等，方向相反，此時合力F=0，無法進行下一步規(guī)劃，則需要設置一個虛擬目標點，如圖7中的六角星所示，其方向為斥力繞引力順時針旋轉90°，與機器人的距離為，Sdis為機器人與障礙物的最近距離，將s代入式(8)中，產(chǎn)生新的引力F`att，此時新的合力F`=F`att，方向為斥力繞引力順時針旋轉90°，將機器人牽引出當前極值困境，選取機器人的斥力Frep與Fatt的夾角a=160°為一個閾值，當夾角a≥160°時，可能會有局部極值的風險，則始終在機器人的斥力繞引力順時針旋轉90°方向上，選取距離為的位置設置虛擬目標點，直到夾角a＜160°。

圖7 虛擬目標點的設置示意圖

3 實驗對比與仿真分析

為驗證算法的準確性，以Linux Ubuntu16.04為平臺，python和C++語言為編程環(huán)境進行仿真。實驗的硬件平臺包括：Intel Core i7處理器、激光雷達和IMU等。在相同硬件平臺上，將本文算法與傳統(tǒng)D*Lite算法等進行比較，針對全局路徑的節(jié)點篩選機制，選取了D*Lite和改進D*Lite這兩種算法進行比較。對于重歸劃，選取了本文算法與傳統(tǒng)D*Lite算法進行比較。

本文采用柵格法來創(chuàng)建路徑規(guī)劃的環(huán)境地圖，將環(huán)境劃分為60×40個大小均勻的柵格（假定每個單位長度為1cm），黑色表示占據(jù)柵格，白色表示自由柵格，假設起始點為S(5，5)，用圓形表示，終點為G(55，35)，用五角星表示。

3.1 全局路徑節(jié)點比較實驗

傳統(tǒng)D*Lite所規(guī)劃的路徑如圖8(a)所示，節(jié)點優(yōu)化后的路徑如圖8(b)所示，對比可知，優(yōu)化后的路徑相比較傳統(tǒng)算法D*Lite，行走的方向不再局限八領域，即偏轉角不再局限于的整數(shù)倍，所規(guī)劃的路徑也更加平滑。

圖8 兩種算法全局路徑對比

表1為上述兩種算法間的節(jié)點個數(shù)和路徑長度的統(tǒng)計，可知經(jīng)過節(jié)點優(yōu)化后的D*Lite算法與傳統(tǒng)D*Lite算法相比，節(jié)點優(yōu)化后的算法節(jié)點只有原來的44%，路徑長度也為原來的83%，都大幅度下降。

表1 路徑規(guī)劃相關數(shù)據(jù)數(shù)據(jù)對比

3.2 局部避障重規(guī)劃比較實驗

當機器人路徑行進到中途時遇見了未知的障礙物，所進行的局部規(guī)劃如圖9所示。

圖9 三種算法局部規(guī)劃對比

圖9(a)為傳統(tǒng)D*Lite所進行的重規(guī)劃，由圖可看出與未知障礙物的距離很近，若未知障礙物存在某些未掃描出的部件，則會很容易發(fā)生碰撞事故。圖9(b)則為局部重規(guī)劃優(yōu)化后的傳統(tǒng)D*Lite，可看出路徑與未知障礙物保持了一定的安全距離，但路徑冗余節(jié)點過多。圖9(c)則是本文算法，即同時對路徑節(jié)點進行篩選和局部進行優(yōu)化后的D*Lite算法，相比較于另外兩種算法，明顯可看出路徑長度更短，安全可靠性更高。

3.3 實際環(huán)境實驗

為了更好的驗證本算法在真實場景應用中的有效性，本文選擇搭載了激光雷達和IMU的移動機器人為實驗對象，底盤直徑為60，演示圖中使用直徑為3像素的圓形代替，矩形代表已知障礙物，圓形代表行進過程中的未知障礙物，在教學實驗室中布置1200cm×800cm的真實場景。

將本文算法與傳統(tǒng)D*Lite算法進行比較，局部規(guī)劃的起始點坐標為(27，11)，目標點坐標為(53，21)，未知障礙物A圓心坐標為(37，22)，半徑為2，障礙物A與障礙物B之間的距離為69cm，與障礙物C的距離為118cm。當小車按照預規(guī)劃的路徑前進時，遇上了障礙物A，傳統(tǒng)D*Lite所進行的重規(guī)劃路徑，如圖10所示，從障礙物A和障礙物B之間穿過，直徑為60cm的機器人穿越69cm的通道，行走時極其容易發(fā)生碰撞危險行為。而改進后的優(yōu)化算法所進行的移動機器人路徑規(guī)劃，如圖11所示，則從障礙物A和障礙物C之間的通道穿過去，其距離為118cm，機器人能很安全的避開障礙物。對比圖10和圖11的重規(guī)劃路徑，可明顯發(fā)現(xiàn)優(yōu)化后的重規(guī)劃路徑在局部避障上的安全性能大大高于傳統(tǒng)的D*Lite。

圖10 傳統(tǒng)D＊ Lite重規(guī)劃

但機器人避開障礙物A后繼續(xù)行走，到達位置1時，障礙物A和障礙物D對移動機器人的斥力合力，與移動機器人受到的目標點引力達到了平衡，導致移動機器人陷入了局部極值的困境，為此需要設置虛擬目標點，移動機器人在新的虛擬引力作用下，走出局部極值的困境，如圖11所示，并最終安全到達目的地。

圖11 優(yōu)化后的重規(guī)劃

4 結語

本文提出的優(yōu)化算法，旨在解決移動機器人使用傳統(tǒng)D*Lite算法進行路徑規(guī)劃時所存在的問題，分別是預規(guī)劃路徑存在的冗余節(jié)點導致路徑較長；重規(guī)劃路徑與障礙物過近存在碰撞風險。通過節(jié)點篩選機制去除預規(guī)劃路徑存在的冗余節(jié)點，再引入人工勢場法來保證在重規(guī)劃環(huán)節(jié)中與未知障礙物始終保持一個安全距離，同時采取設置虛擬目標點的方法解決了機器實際移動中可能會遇到的局部極值問題，最終得到一條更安全、高效且平滑的路徑。