優(yōu)化改進RRT和人工勢場法的路徑規(guī)劃算法

辛 鵬,王艷輝,+,劉曉立,馬希青,徐 東

(1.河北工程大學(xué) 機械與裝備工程學(xué)院河北省智能工業(yè)裝備技術(shù)重點實驗室,河北 邯鄲 056038;2.河北工程大學(xué) 河北省高品質(zhì)冷鐓鋼技術(shù)創(chuàng)新中心,河北 邯鄲 056038)

0 引言

移動機器人可以完成自主移動,自行從當(dāng)前位姿到目標(biāo)位姿進行工作,在工業(yè)生產(chǎn)和日常生活中具有普遍的應(yīng)用。路徑規(guī)劃是移動機器人開發(fā)的核心部分,即針對當(dāng)前環(huán)境規(guī)劃出從初始位姿到最終位姿的無碰撞路徑[1-3]。目前,常用的路徑規(guī)劃算法為：A*算法[4]、動態(tài)窗口法[5]、蟻群算法[6]、遺傳算法[7]等。

快速搜索隨機樹(Rapidly Random Tree,RRT)是非常經(jīng)典的算法,其搜索的隨機性同時適用于二維和三維空間。但是搜索過程中隨機樹向四周均勻擴散,因此隨機性過大,特別是在狹窄通道尋找路徑時,會消耗過多時間,搜索效率較低。目前針對RRT算法國內(nèi)外已經(jīng)有了大量的研究和探索。文獻[8]為了提升雙向RRT*算法的效率,提出將終點偏向的思想以及樣條插值法融入到雙向RRT*算法中,使搜索時更具有偏向性,搜索效率更高。并對規(guī)劃好的路徑剔除冗余節(jié)點,減少路徑長度。文獻[9]為了降低雙向搜索隨機樹采樣的隨機性,設(shè)置了引力場引導(dǎo)算法向目標(biāo)點搜索。為提升搜索效率,在起點和目標(biāo)點之間加入擴展節(jié)點,由擴展節(jié)點分別向起點和目標(biāo)點擴展,即將原來的兩棵隨機樹擴展為4個。文獻[10]為提高收斂速度并規(guī)劃出更優(yōu)的起始路徑,增大了雙向RRT*算法ChooseParent(Vnear,Vnew)和Rewire(Vnear,Vnew)中Vnew的搜索范圍,并驗證了算法滿足概率完備性和漸進最優(yōu)性。文獻[11]針對RRT算法規(guī)劃的路徑冗余節(jié)點和路徑拐點較多的問題,采取以概率P向目標(biāo)點擴展隨機樹,并在擴展時增加角度約束,提高搜索效率。對生成的軌跡運用三次B樣條曲線進行重新規(guī)劃,相較于傳統(tǒng)RRT有較大的提升。文獻[12]為了滿足智能車在復(fù)雜環(huán)境中的快速、平穩(wěn)的行駛,提出同心圓RRT算法,該算法使用同心圓生成隨機點,在選擇鄰近點時充分考慮到車輛的運動學(xué)情況和鄰近點與目標(biāo)點的距離,算法在實際環(huán)境中有很好的有效性和實用性。文獻[13]針對RRT算法搜索盲目、容易陷入極小環(huán)境的缺點,在RRT算法中加入目標(biāo)導(dǎo)向,并針對未知環(huán)境,無障礙環(huán)境,傳感器能掃到邊界和傳感器不能掃到邊界等幾種情況提出不同的搜索策略,針對新環(huán)境高效地規(guī)劃出路徑。文獻[14]為了使智能車在障礙物較多且分布不規(guī)則的環(huán)境中平穩(wěn)移動,提出連續(xù)曲率RRT算法。在RRT算法擴展時考慮到周圍環(huán)境和車輛自身影響,加入目標(biāo)偏向和適當(dāng)?shù)亩攘亢瘮?shù)及環(huán)境、車身的約束,極大地提升了算法規(guī)劃效率。

以上文獻對RRT算法在節(jié)點擴展和路徑平滑性上做了大量優(yōu)化。本文將改進RRT算法與改進動態(tài)窗口法融合。在全局中,融入人工勢場法調(diào)整擴展方向,提取路徑中關(guān)鍵節(jié)點,并進行重新規(guī)劃,增加搜索效率,減少路徑長度。在局部中,通過優(yōu)化改進RRT算法引導(dǎo)動態(tài)窗口法,防止陷入局部最優(yōu)。

1 改進RRT算法

RRT算法廣泛用于二維和三維場景的路徑規(guī)劃中。以起始點為種子隨機向四周進行生長,直到目標(biāo)點也在隨機樹上或者距離隨機樹足夠近,此時可以在隨機樹上找到從初始位置到目標(biāo)位置的有效路徑。RRT算法隨機性較高,在擴展時,隨機向四周擴散,搜索效率較低,且規(guī)劃的路徑中拐點和冗余路段較多,不利于機器人移動。因此,本文針對RRT提出以下方面的改進。

1.1 加入人工勢場的思想

傳統(tǒng)RRT算法在搜索路徑中擴展方向隨機,搜索時間較長,效率較低。因此,在RRT算法增加擴展點時加入人工勢場分量,使得擴展時既有隨機性,又考慮到障礙物以及目標(biāo)點對當(dāng)前節(jié)點擴展的影響。使隨機樹擴展時具有目的性,提高搜索效率。

人工勢場的引力函數(shù)為：

(1)

式中：ε為引力系數(shù),ρ(P,Pgoal)為當(dāng)前點與目標(biāo)點的間距。

斥力函數(shù)為：

(2)

式中：μ為斥力系數(shù);ρ(P,Pobs)為當(dāng)前點與障礙物點的間距;ρ0為障礙物影響范圍,超過這個范圍,障礙物不對當(dāng)前節(jié)點產(chǎn)生斥力影響。

合勢場為：

U=Uatt+Urep。

(3)

傳統(tǒng)的人工勢場法計算當(dāng)前位置周圍節(jié)點合勢場,搜索范圍過小,不利于對周圍節(jié)點的判定。因此,采用擴展鄰域思想,將搜索范圍改為7×7搜索鄰域[15],如圖1所示。

圖1 7×7搜索鄰域人工勢場

將人工勢場思想加入到RRT算法中,在擴展時加入改進人工勢場偏向。此時,擴展節(jié)點由人工勢場法進行引導(dǎo),在文獻[16]的基礎(chǔ)上加入障礙物的斥力勢場,如圖2所示。圖中：P_rand為生成的隨機節(jié)點,P_near為距離隨機節(jié)點最近的點,P_test為向隨機點擴展一個步長,P_test為改進人工勢場法擴展出的節(jié)點,P_real為實際擴展節(jié)點。

圖2 擴展時加入人工勢場思想

將改進RRT算法與傳統(tǒng)RRT算法分別在無障礙物以及有障礙環(huán)境下進行比對,驗證改進的有效性。無障礙物環(huán)境下兩種算法對比如圖3所示。

a 無障礙物傳統(tǒng)RRT算法 b 無障礙物改進RRT算法 圖3 無障礙下改進RRT算法與傳統(tǒng)RRT算法對比

當(dāng)前環(huán)境中存在障礙物時,如果擴展點在障礙物上,則放棄該點,重新進行搜索。當(dāng)有障礙物時,兩種算法的對比如圖4所示。

a 有障礙物傳統(tǒng)RRT算法 b 有障礙物改進RRT算法圖4 有障礙下改進RRT算法與傳統(tǒng)RRT算法對比

在有障礙物和無障礙物環(huán)境下對兩種算法進行比對,通過路徑長度以及隨機樹擴展方向可以看出,改進RRT算法規(guī)劃的路徑更短且擴展時更具有偏向性。

1.2 路徑優(yōu)化

改進RRT算法規(guī)劃的路徑中依舊存在冗余路段,此時,對改進后的路徑進行二次調(diào)節(jié)[17],提取關(guān)鍵節(jié)點,去除冗余節(jié)點,規(guī)劃出新的路徑。具體實現(xiàn)為：

(1)將所有節(jié)點按順序放入集合{q1,q2,q3,…,qn}。

(2)從起始節(jié)點q1逐一連接集合中的節(jié)點,直至與qt+1節(jié)點之間的連線通過障礙物,qt為集合中的關(guān)鍵點。此時,從qt開始,依次與剩下的節(jié)點連接,直到找到所有的關(guān)鍵點。

(3)從起始點依次連接關(guān)鍵點和目標(biāo)點,規(guī)劃出新的路徑,具體優(yōu)化實現(xiàn)如圖5所示。

a 改進RRT算法 b 提取關(guān)鍵轉(zhuǎn)折點 c 優(yōu)化后路徑圖5 優(yōu)化步驟

a 傳統(tǒng)動態(tài)窗口法 b 改進動態(tài)窗口法路徑圖6 改進動態(tài)窗口法與傳統(tǒng)動態(tài)窗口法對比

由表1可知,優(yōu)化后,路徑長度減少了2.1%,路徑中拐點減少了75%。改進后的算法規(guī)劃的路徑更加平滑,拐點更少,路徑長度更短,適合全局規(guī)劃,但是不適合機器人障礙物較多情況下移動且不能實現(xiàn)動態(tài)避障。此時應(yīng)將優(yōu)化改進算法加入改進動態(tài)窗口法,在局部優(yōu)化機器人移動,使機器人移動實現(xiàn)全局最優(yōu)。

表1 改進RRT算法與優(yōu)化后路徑對比

2 改進動態(tài)窗口法

動態(tài)窗口法是模擬出速度空間中所有速度下的路徑,在一定時間內(nèi)對機器人的朝向、機器人移動速度以及障礙物的距離等進行評分并按照一定比例進行匯總,最終得到總分最高的路徑即最佳路徑。傳統(tǒng)動態(tài)窗口法評價函數(shù)固定,因此并不能對當(dāng)前位置距離目標(biāo)點等情況進行很好的判斷,不利于機器人快速向目標(biāo)點靠近。

2.1 機器人運動學(xué)模型

假定機器人在極短時間內(nèi)的移動為直線,運動中角速度為ω,線速度為v,則機器人在Δt內(nèi)的運動可由公式推導(dǎo)出：

x=x+vxΔtcosθt-vyΔtsinθt;y=y+vxΔtsinθt+vyΔtcosθt;

θt=θt+ωΔt。

(4)

通過式(4)以及采集到的速度信息,可以預(yù)估出在下一時間段內(nèi)機器人的軌跡。

2.2 速度采樣

速度空間中存在大量的角速度與線速度。此時,考慮到機器人本身以及當(dāng)前環(huán)境,應(yīng)對速度進行約束,速度范圍為：

Vm={(v,ω)|v∈[vmin,vmax],ω∈[ωmin,ωmax]}。

(5)

電機力矩等因素會限制機器人的移動速度,在現(xiàn)實環(huán)境下,機器人的移動速度為：

(6)

為了使機器人在碰到障礙物之前能夠及時停下,對機器人設(shè)置速度上限：

例如下面的句子中，①句是定語從句，因為that在從句中充當(dāng)了句子的主語，有實際的意義，不能去掉，否則句子的結(jié)構(gòu)不完整，這個句子中that與中心詞news構(gòu)成修飾與被修飾的關(guān)系；②句是同位語從句，因為該句子有完整的主語（The news）、謂語（is）等，所以該句子中的that從句可以去掉，不影響句子的完整性，這里的從句是主句的具體內(nèi)容。

(7)

式中dist(v,ω)為在當(dāng)前路徑下機器人與障礙物的最短間距。

2.3 改進評價函數(shù)

速度空間中不同的速度預(yù)估出不同的軌跡,使用評價函數(shù)對預(yù)估的軌跡進行評分。傳統(tǒng)的評價函數(shù)為：

G(v,ω)=σ(αhead(v,ω)+βdist(v,ω)+γvel(v,ω))。

(8)

式中：head(v,ω)為在當(dāng)前速度下移動時移動機器人的朝向與實際期望的偏離,vel(v,ω)表示機器人當(dāng)前移動的線速度和角速度,σ為平滑系數(shù),α、β、γ為系數(shù)。

傳統(tǒng)評價函數(shù)中,α、β和γ都是固定值,機器人在移動中不能很好地兼顧到與障礙物的距離和與目標(biāo)點的距離。為了使機器人在運行中動態(tài)窗口法能夠快速規(guī)劃出有效路徑,將固定比例改為動態(tài)比例。當(dāng)機器人與目標(biāo)節(jié)點和障礙物節(jié)點都間隔較大時,增加head(v,ω)比例系數(shù),使得機器人移動時更偏向目標(biāo)點。當(dāng)距離障礙物和目標(biāo)點較近時,增加vel(v,ω)比例,以便快速繞開障礙物到達目標(biāo)點,改進后評價函數(shù)為：

(9)

式中：r表示障礙物的影響領(lǐng)域半徑,h表示機器人當(dāng)前節(jié)點與目標(biāo)節(jié)點的間隔,R表示起始點與目標(biāo)點的距離。為避免在周圍無障礙物時,dist(v,ω)比例過大,將dist(v,ω)設(shè)置在一定范圍內(nèi),即dist(v,ω)∈(0,2r)。

由表2可知,對比傳統(tǒng)RRT算法,改進動態(tài)窗口法規(guī)劃的路徑長度減少了0.36%,由于地圖較小,路徑縮短的并不明顯。規(guī)劃時間減少了11.28%。改進后明顯提高了規(guī)劃效率,縮短了路徑規(guī)劃時間。

表2 傳統(tǒng)動態(tài)窗口法與改進動態(tài)窗口法時間對比

為了驗證評價函數(shù)中不同系數(shù)對實驗結(jié)果的影響,加入消融實驗。實驗結(jié)果如表3所示。

表3 改進評價函數(shù)消融實驗

通過表3可以看出,通過調(diào)整vel(v,ω)、head(v,ω)比例,能夠有效減少路徑長度,提高搜索效率,但規(guī)劃的路徑距離障礙物更近,因此需要適當(dāng)提高dist(v,ω)比例,使路徑與障礙物保持一定距離,進而使得機器人在移動時能夠更加安全地到達目標(biāo)點。

3 融合算法

傳統(tǒng)動態(tài)窗口法在規(guī)劃路徑中容易陷入局部最優(yōu),需要全局的路徑規(guī)劃算法引導(dǎo)。改進后的RRT算法雖然路徑平滑,但不能實現(xiàn)動態(tài)避障,不適合機器人移動。因此,使用優(yōu)化改進RRT算法進行全局規(guī)劃,將全局規(guī)劃的路徑按照優(yōu)化后的關(guān)鍵節(jié)點進行分段,在相鄰節(jié)點間使用改進動態(tài)窗口法規(guī)劃路徑,如圖7所示,全局的融合算法以此類推。

圖7 融合算法示意圖

圖8 融合算法實現(xiàn)流程圖

4 仿真驗證與實物驗證

4.1 仿真驗證

為檢驗改進RRT算法的高效性以及融合算法的有效性,在MATLAB 2017中使用矩陣建立柵格圖,黑色方格表示障礙物,白色方格表示無障礙物。起始節(jié)點(4,3),目標(biāo)點(29,28)。使用兩組實驗檢測和對比。

(1)實驗一 優(yōu)化改進 RRT算法與傳統(tǒng)RRT、傳統(tǒng)A*、改進RRT算法、動態(tài)窗口法以及融合算法的路徑規(guī)劃效果如圖9所示。5種算法規(guī)劃時間、路徑長度以及拐點數(shù)對比如表3所示。

如表4所示,相較于傳統(tǒng)RRT,優(yōu)化改進RRT規(guī)劃的路徑長度減少了16.51%,搜索時間縮短66.95%,路徑中拐點數(shù)量減少92%;相較于傳統(tǒng)A*算法,優(yōu)化改進RRT算法路徑規(guī)劃時間更短,路徑長度更短且拐點數(shù)更少,路徑規(guī)劃效果更好。傳統(tǒng)動態(tài)窗口法未能找到路徑,融合算法在全局由優(yōu)化改進RRT算法進行引導(dǎo),能夠找到全局最優(yōu)路徑,融合算法比優(yōu)化改進RRT算法路徑更短。

表4 5種算法對比

(2)實驗二 在優(yōu)化改進RRT算法在全局規(guī)劃的軌跡中加入動態(tài)障礙物如圖10中紅色方格所示,以檢驗在規(guī)劃好的路徑中加入障礙物,移動機器人能否避開。

圖10 在優(yōu)化改進RRT算法路徑中加入障礙物

在全局軌跡中添加動態(tài)障礙物,機器人項目標(biāo)點移動以及避障狀態(tài)如圖11所示。機器人在移動中的實時線速度、角速度和位姿如圖12所示。

a 移動機器人避開第一個障礙物 b 移動機器人避開第二個障礙物 c 機器人整體運行軌跡圖11 機器人繞過障礙物

a 機器人運行線速度圖 b 機器人運行角速度圖 c 機器人姿態(tài)變化圖12 機器人位移過程中速度和姿態(tài)變化

通過機器人的運行軌跡以及在運行中角速度、線速度以及姿態(tài)的變化可得出機器人從初始位置平穩(wěn)到達目標(biāo)位置,能夠?qū)崿F(xiàn)避障。

4.2 實物驗證

為了驗證算法在實際環(huán)境中有效性,使用的Hawkbot智能機器人操作系統(tǒng)(Robot Operating System,ROS)三輪小車如圖13所示,實驗環(huán)境如圖14所示。

圖13 Hawkbot移動機器人

圖14 室內(nèi)實際環(huán)境

由于室內(nèi)環(huán)境場景較小,采用Gmapping算法構(gòu)建室內(nèi)環(huán)境,實際建圖效果如圖15所示。

圖15 SLAM建圖

為了更加清晰地看到機器人運行軌跡,使用程序Showpath記錄機器人移動路線,并使用藍色線條顯示。

通過圖16可看出,改進融合算法相較于傳統(tǒng)RRT融合傳統(tǒng)動態(tài)窗口法(Dynamic Window Approach,DWA)效果更好,路徑更加平滑,在繞過障礙物后規(guī)劃的路徑更加偏向目標(biāo)點。

a 傳統(tǒng)RRT算法和傳統(tǒng)DWA b 優(yōu)化改進RRT和改進DWA圖16 傳統(tǒng)融合算法和改進融合算法對比

5 結(jié)束語

在無障礙物條件下,改進RRT算法搜索方向更偏向目標(biāo)點,搜索效率更高。在有障礙物環(huán)境下,優(yōu)化改進RRT算法在路徑長度、路徑規(guī)劃時間和路徑拐點等方面都要優(yōu)于傳統(tǒng)RRT算法和傳統(tǒng)A*算法。將全局的優(yōu)化改進RRT算法與局部的優(yōu)化改進動態(tài)窗口法結(jié)合成為融合算法。通過Hawkbot移動機器人驗證,融合算法路徑更加平滑,規(guī)劃路徑更短,且能夠?qū)崿F(xiàn)避開障礙物。本文算法雖然對RRT算法進行改進,但擴展時仍有一定隨機性,因此更適合在障礙物較少的環(huán)境中規(guī)劃有效路徑。未來將研究多機器人規(guī)劃算法,推進多機器人在實際生活中的協(xié)同工作。