杜 軒，歐資臻

（三峽大學(xué)機(jī)械與動力學(xué)院 三峽大學(xué)水電機(jī)械設(shè)備設(shè)計與維護(hù)湖北重點實驗室，湖北 443002）

0 引言

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

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

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

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

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

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

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

圖1 八鄰域搜索方式

圖2 冗余拐點

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

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

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

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

式中：

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

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

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

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

傳統(tǒng)D*Lite的重規(guī)劃便是通過上述式(3)來選取最小的k(s)值，從而選取所需要進(jìn)行維護(hù)后續(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ī)劃的局部路徑必然會帶來很大的風(fēng)險。

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

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

2 改進(jìn)算法的原理

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

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

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

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

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

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

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

圖4 局部避障策略

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

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

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

式中：

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

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

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

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

式中：

η表示斥力增益。

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

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

式中：

ζ：引力增益。

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

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

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

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

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

重規(guī)劃從E5開始向B2進(jìn)行路徑規(guī)劃，E5往外擴(kuò)張節(jié)點E4和D5，兩者傳統(tǒng)的k(s)值和rhs(s)值相等，如圖5(c)所示，但根據(jù)改進(jìn)后的式(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)化

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

圖6 局部極值處理機(jī)制

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

圖7 虛擬目標(biāo)點的設(shè)置示意圖

3 實驗對比與仿真分析

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

本文采用柵格法來創(chuàng)建路徑規(guī)劃的環(huán)境地圖，將環(huán)境劃分為60×40個大小均勻的柵格（假定每個單位長度為1cm），黑色表示占據(jù)柵格，白色表示自由柵格，假設(shè)起始點為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，行走的方向不再局限八領(lǐng)域，即偏轉(zhuǎn)角不再局限于的整數(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ī)劃相關(guān)數(shù)據(jù)數(shù)據(jù)對比

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

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

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

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

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

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

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

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

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

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

4 結(jié)語

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