鞏 浩,譚向全,李佳欣,吳清文,2

(1.中國科學(xué)院 a.長春光學(xué)精密機(jī)械與物理研究所;b.空間光學(xué)系統(tǒng)在軌制造與集成系統(tǒng)重點(diǎn)實(shí)驗(yàn)室,長春 130033;2.中國科學(xué)院大學(xué)材料與光電研究中心,北京 100049)

0 引言

移動機(jī)器人被廣泛應(yīng)用于工業(yè)生產(chǎn)、運(yùn)輸、服務(wù)業(yè)等領(lǐng)域,已成為當(dāng)下科技發(fā)展的重點(diǎn)研究方向[1]。路徑規(guī)劃就是機(jī)器人根據(jù)自身傳感器對環(huán)境的感知,自行規(guī)劃出一條符合機(jī)器人約束條件的安全無碰撞的運(yùn)行路線[2-3],而路徑長度、安全性、高效性則是衡量規(guī)劃路徑是否最優(yōu)的重要指標(biāo)[4]。

路徑規(guī)劃根據(jù)地圖是否已知分為全局路徑規(guī)劃以及局部路徑規(guī)劃。常見的全局路徑規(guī)劃算法有A*算法、RRT算法、Dijkstra算法、Floyd算法等[5],局部路徑規(guī)劃算法有人工勢場法[6]、動態(tài)窗口算法[7]等。

RRT算法是一種基于采樣的路徑規(guī)劃算法,因其計(jì)算成本低,被廣泛使用于移動機(jī)器人路徑規(guī)劃[8]以及機(jī)械臂軌跡規(guī)劃[9]。RRT算法通過隨機(jī)采樣擴(kuò)展搜索,算法結(jié)構(gòu)簡單、運(yùn)算速度快。然而RRT算法也存在一些不足:盲目搜索,隨機(jī)性強(qiáng),收斂性差,再現(xiàn)性差;路徑上冗余點(diǎn)多,曲折不光滑,一般不是最優(yōu)路徑[10-12]。針對這些問題,一些國內(nèi)外學(xué)者提出了相關(guān)改進(jìn)。KUFFNER等[13]提出RRT-Connect算法,通過采用起始點(diǎn)與目標(biāo)點(diǎn)雙向擴(kuò)展隨機(jī)樹,大大降低了搜索時(shí)間。KANG等[14]提出改進(jìn)的RRT-Connect算法,通過采用基于三角形不等式的重新排列樹方法,改進(jìn)了RRT算法規(guī)劃路徑上冗余點(diǎn)多的缺點(diǎn)。KARAMAN等[15]提出改進(jìn)算法RRT*,通過重選父節(jié)點(diǎn)進(jìn)行更新樹,可以求解次優(yōu)路徑,但其計(jì)算成本高,效率低。劉慧等[16]提出改進(jìn)的雙向RRT*算法,通過結(jié)合動態(tài)末梢節(jié)點(diǎn)導(dǎo)向和勢場導(dǎo)向進(jìn)行偏置采樣,改善了RRT算法隨機(jī)性強(qiáng)的缺點(diǎn)。GAMMELL等[17]提出改進(jìn)算法Informed RRT*,通過橢圓啟發(fā)式搜索的最優(yōu)采樣,改進(jìn)了RRT算法隨機(jī)性強(qiáng)的缺點(diǎn),但在復(fù)雜環(huán)境情況下容易陷入局部極值問題。欒添添等[18]通過劃分采用區(qū)域,采用概率目標(biāo)偏置策略和多級步長擴(kuò)展進(jìn)行路徑規(guī)劃,提升了節(jié)點(diǎn)搜索效率。杜傳勝等[19]提出同根雙向擴(kuò)展的貪心RRT算法,將擴(kuò)展方式改為由起始點(diǎn)與目標(biāo)點(diǎn)連線的中間點(diǎn)同時(shí)向起始點(diǎn)和目標(biāo)點(diǎn)進(jìn)行雙向擴(kuò)展,加快路徑生成,但是搜索過程容易陷入局部極值。

本文結(jié)合概率目標(biāo)偏置與人工勢場相結(jié)合的動態(tài)采樣策略、步長函數(shù)、貪心思想、預(yù)留安全距離策略對RRT算法進(jìn)行改進(jìn),提高了節(jié)點(diǎn)搜索的效率。對初步規(guī)劃路徑進(jìn)行剪枝優(yōu)化以及曲線擬合,減少路徑上冗余點(diǎn)數(shù),提高了路徑質(zhì)量,使最終路徑滿足移動機(jī)器人運(yùn)動學(xué)特性,保證了最終路徑的連續(xù)性和平滑性。

1 RRT算法原理

RRT算法是基于采樣原理的一種典型路徑規(guī)劃算法。通過隨機(jī)采樣進(jìn)行節(jié)點(diǎn)擴(kuò)展,生成一個(gè)隨機(jī)樹,當(dāng)尋找到目標(biāo)點(diǎn)時(shí),通過自由樹的回溯思想,生成路徑。節(jié)點(diǎn)擴(kuò)展示意圖如圖1所示。

圖1 RRT算法隨機(jī)樹擴(kuò)展示意圖

(a) 隨機(jī)采樣 (b) 人工勢場

表1 RRT偽代碼

2 改進(jìn)RRT算法

2.1 采樣優(yōu)化

傳統(tǒng)RRT算法采用隨機(jī)采樣的方式,采樣節(jié)點(diǎn)盲目擴(kuò)張,產(chǎn)生大量無效節(jié)點(diǎn),導(dǎo)致算法搜索時(shí)間長、收斂性差。針對此問題,本文采用概率目標(biāo)偏置與人工勢場相結(jié)合的動態(tài)采樣策略,采樣函數(shù)為:

(1)

式中:Xrand1為隨機(jī)采樣的一點(diǎn),P1與P2為0～1之間的隨機(jī)數(shù),α為目標(biāo)權(quán)重因子,β為隨機(jī)權(quán)重因子。首先在地圖上隨機(jī)采樣一點(diǎn)Xrand1,然后通過兩個(gè)隨機(jī)數(shù)P1與P2決定選取哪種采樣方式確定Xrand。α越大,越傾向于選擇目標(biāo)點(diǎn)作為采樣點(diǎn);α越小且β越大,越傾向于選擇隨機(jī)點(diǎn)作為采樣點(diǎn),保留一部分隨機(jī)采樣,幫助脫離局部極值;借鑒人工勢場法,有(1-α)(1-β)概率選擇目標(biāo)點(diǎn)與隨機(jī)點(diǎn)相結(jié)合的點(diǎn)作為采樣點(diǎn)。對于不用場景,合理選擇α與β的大小,得到較好的路徑規(guī)劃效果。

2.2 步長函數(shù)

考慮局部避障能力,且考慮快速到達(dá)目標(biāo)點(diǎn)的要求,引入變步長函數(shù)。變步長函數(shù)包括兩部分:

(1)快速擴(kuò)展步長函數(shù)?？紤]快速到達(dá)目標(biāo)點(diǎn),構(gòu)造以Xnear、Xgoal之間的歐氏距離為自變量的函數(shù),要求接近目標(biāo)點(diǎn)時(shí)的步長為L1,根據(jù)起始點(diǎn)處四周無障礙時(shí)的步長要求Lmax,可以求得a,最后得到快速擴(kuò)展步長函數(shù):

(2)

式中:D為Xnear、Xgoal的歐氏距離。

(2)局部避障因子?？紤]局部障礙物避障問題,構(gòu)造以Xnear為圓心、r為半徑的鄰近圓區(qū)域內(nèi)障礙物檢測。

(3)

式中:Sobs為鄰近圓區(qū)域內(nèi)障礙物的面積,Sr為鄰近圓區(qū)域總面積。

綜合以上,設(shè)計(jì)步長函數(shù)為:

L=(1-ρ)LD

(4)

步長函數(shù)的添加會增加計(jì)算時(shí)間,合理設(shè)計(jì)局部無障礙區(qū)域的最大步長、快速擴(kuò)展步長函數(shù),可以在滿足局部避障能力的前提下快速到達(dá)目標(biāo)點(diǎn),相對減少搜索時(shí)間。

2.3 貪心思想

隨機(jī)采樣思想實(shí)際應(yīng)用時(shí),新節(jié)點(diǎn)一般不能采樣到目標(biāo)點(diǎn)。針對這一問題,傳統(tǒng)RRT算法設(shè)定新節(jié)點(diǎn)Xnew到達(dá)以Xgoal為圓心、單步長為半徑的圓區(qū)域內(nèi),且Xnew、Xgoal連線無碰撞障礙物,此時(shí)認(rèn)為新節(jié)點(diǎn)可直達(dá)目標(biāo)點(diǎn),則判斷為完成路徑搜索,返回路徑。傳統(tǒng)RRT算法在實(shí)際應(yīng)用時(shí),因?yàn)椴介L限制,存在新生長節(jié)點(diǎn)在目標(biāo)點(diǎn)四周振蕩現(xiàn)象[19],難以收斂。針對此問題,本文引入基于貪心思想的目標(biāo)點(diǎn)檢測,考慮到極度貪心思想可能會導(dǎo)致路徑錯(cuò)過更優(yōu)解[19],擬根據(jù)不同場景選取不同尺寸的目標(biāo)點(diǎn)區(qū)域檢測。即當(dāng)有新節(jié)點(diǎn)被加入樹中時(shí),若新節(jié)點(diǎn)在目標(biāo)點(diǎn)檢測區(qū)域內(nèi),則對Xnew、Xgoal進(jìn)行連線障礙物碰撞檢測,若無碰撞,則直連Xnew、Xgoal,返回路徑;若有碰撞,則繼續(xù)采樣搜索,重復(fù)進(jìn)行。引入貪心思想的節(jié)點(diǎn)擴(kuò)展示意圖如圖3所示。

圖3 貪心思想的節(jié)點(diǎn)擴(kuò)展示意圖

圖3中,新節(jié)點(diǎn)Xnew進(jìn)入目標(biāo)檢測范圍內(nèi),利用貪心思想,直連Xgoal,通過障礙物碰撞檢測函數(shù)計(jì)算得Xnew、Xgoal連線之間無障礙物,直接將Xgoal加入隨機(jī)樹中,提前結(jié)束路徑搜索。這種貪心思想可以很好地解決目標(biāo)點(diǎn)附近新節(jié)點(diǎn)的振蕩問題,減少迭代次數(shù)。針對不同場景,合理設(shè)置目標(biāo)點(diǎn)檢測區(qū)域半徑的取值,能夠有效提高算法的收斂速度。

2.4 安全距離

考慮傳統(tǒng)RRT算法的隨機(jī)采樣性,存在新節(jié)點(diǎn)距離障礙物過近的現(xiàn)象,可能導(dǎo)致生成路徑緊貼障礙物,并且考慮到移動機(jī)器人有外形尺寸限制,此時(shí)路徑不符合移動機(jī)器人實(shí)際工作需求,故在路徑規(guī)劃過程考慮行車安全距離,安全距離如圖4所示。

圖4 預(yù)留安全距離示意圖

當(dāng)樹節(jié)點(diǎn)生長過程中,與局部避障因子原理類似,在Xnew加入樹節(jié)點(diǎn)之前加一步驟:檢測以新生成節(jié)點(diǎn)Xnew為圓心、安全距離d為半徑的圓區(qū)域障礙物檢測,若不存在障礙物,則將Xnew加入樹中,繼續(xù)操作;若存在障礙物,則丟棄Xnew,重復(fù)采樣。

3 路徑優(yōu)化

初步規(guī)劃的路徑中含有大量冗余點(diǎn)且拐點(diǎn)多,無法滿足移動機(jī)器人的實(shí)際運(yùn)動需求。因此本文通過對初始路徑進(jìn)行剪枝優(yōu)化去除冗余點(diǎn),然后通過對剪枝優(yōu)化后的路徑進(jìn)行曲線優(yōu)化去除拐點(diǎn)。

3.1 剪枝優(yōu)化

傳統(tǒng)RRT算法搜索出來的路徑是由許多線段連接而成,考慮兩點(diǎn)之間線段最短這一思想,對初步規(guī)劃的路徑進(jìn)行剪枝優(yōu)化,去除冗余點(diǎn)。具體做法是利用自由樹的回溯思想對路徑節(jié)點(diǎn)進(jìn)行重選父節(jié)點(diǎn),過程中考慮剪枝后路徑的安全距離。剪枝優(yōu)化示意圖如圖5所示。

圖5 剪枝優(yōu)化示意圖

圖5中,首先重選Xgoal的父節(jié)點(diǎn)。連接Xinit與Xgoal,如果連線未碰撞障礙物,則直接將Xinit視為Xgoal的父節(jié)點(diǎn),去除Xinit與Xgoal之間的點(diǎn),連接Xinit、Xgoal形成路徑;如果連線碰撞障礙物,檢測Xinit后的一點(diǎn)X1。連接X1與Xgoal,重復(fù)上述檢測,檢測到X5、Xgoal之間無碰撞,但連線距離障礙物過近,丟棄X5,直至檢測到X6,滿足X6、Xgoal之間無碰撞且連線距離障礙物有足夠安全距離,將X6視為Xgoal的父節(jié)點(diǎn)。接著重選X6的父節(jié)點(diǎn),重復(fù)上述操作。直至重選到某一點(diǎn)的父節(jié)點(diǎn)為Xinit,得到剪枝優(yōu)化后的路徑。

3.2 曲線優(yōu)化

剪枝優(yōu)化后的路徑是由許多線段連接而成,在節(jié)點(diǎn)處存在航向角突變的問題,不符合移動機(jī)器人的運(yùn)動學(xué)特性,無法滿足實(shí)際應(yīng)用需求。因此本文對剪枝優(yōu)化后的路徑進(jìn)行平滑處理,使最終生成的軌跡滿足移動機(jī)器人的運(yùn)動學(xué)特性。

為了使優(yōu)化后路徑曲率連續(xù),又考慮到實(shí)際應(yīng)用中不可能為了曲率連續(xù)完全放棄直線行駛,故采用三次B樣條曲線對路徑轉(zhuǎn)折點(diǎn)處進(jìn)行擬合處理。

三次B樣條曲線的表達(dá)式為:

(5)

式中:Pk(k=0,1,2,3)為控制點(diǎn),Fk,3(t)為三次B樣條基函數(shù),推導(dǎo)公式為式(6)。

(6)

應(yīng)用式(6),四點(diǎn)構(gòu)造一段三次B樣條曲線,公式具體化為:

(7)

最終,三次B樣條的擬合曲線為:

P(t)=P0·F0,3(t)+P1·F1,3(t)+
P2·F2,3(t)+P3·F3,3(t)

(8)

4 仿真分析

為了驗(yàn)證本文改進(jìn)算法的有效性,將RRT算法、RRT-Connect算法、文獻(xiàn)[20]算法、本文改進(jìn)RRT算法(曲線優(yōu)化前)分別在無障礙場景、簡單場景、復(fù)雜場景、狹窄通道場景下進(jìn)行仿真分析,對仿真結(jié)果進(jìn)行對比分析。仿真實(shí)驗(yàn)運(yùn)行環(huán)境:系統(tǒng)windows 10,處理器Intel(R) Core(TM) i7-7700HQ CPU @2.80 GHz,內(nèi)存雙通道8.00 GB。

4.1 參數(shù)設(shè)置

仿真4種場景地圖尺寸均為800×800,本文算法的其余參數(shù)如表2所示。每個(gè)算法在地圖上進(jìn)行10組實(shí)驗(yàn),每組實(shí)驗(yàn)包括10次路徑規(guī)劃,規(guī)劃結(jié)果以圖表形式輸出。

表2 參數(shù)設(shè)置

4.2 不同算法仿真分析

本小節(jié)對RRT算法、RRT-Connect算法、文獻(xiàn)[20]算法、本文改進(jìn)RRT算法(剪枝優(yōu)化前)的仿真結(jié)果進(jìn)行對比,驗(yàn)證改進(jìn)算法的有效性。仿真結(jié)果如圖6～圖9所示。

(a) RRT算法 (b) RRT-Connect算法

從圖6～圖9中可以看出,4種算法均可以完成從起始點(diǎn)到目標(biāo)點(diǎn)的全局路徑規(guī)劃,通過對比仿真結(jié)果可知:本文算法改進(jìn)后,冗余節(jié)點(diǎn)大量減少;隨機(jī)樹擴(kuò)展的避障能力得到增強(qiáng);通過狹窄通道能力增強(qiáng)。對4種地圖分別測試10組,每組測試10次,測試結(jié)果進(jìn)行取平均值處理后如表3～表6所示。

表3 不同場景下路徑搜索時(shí)間對比表

表4 不同場景下迭代次數(shù)對比表

表5 不同場景下路徑長度對比表

表6 不同場景下路徑上節(jié)點(diǎn)個(gè)數(shù)對比表

文獻(xiàn)[20]算法在狹窄通道地圖中迭代20 000次內(nèi)規(guī)劃路徑成功率約70%。

對100*4次實(shí)驗(yàn)的路徑搜索時(shí)間分析可知,本文算法可以很好地減少路徑搜索時(shí)間,提升路徑規(guī)劃效率。4種場景下,本文算法的路徑規(guī)劃時(shí)間相對于RRT算法分別減少了98.06%、92.29%、84.74%、69.64%,相對于RRT-Connect算法分別減少了85.23%、42.90%、25.07%、59.38%,相對于文獻(xiàn)[20]算法分別減少了4.17%、70.08%、92.08%、96.21%。

文獻(xiàn)[20]算法在狹窄通道地圖中迭代20 000次內(nèi)規(guī)劃路徑成功率約70%。

對100*4次實(shí)驗(yàn)的迭代次數(shù)分析可知,本文算法可以很好地減少迭代次數(shù),提升路徑規(guī)劃效率。4種場景下,本文算法的迭代次數(shù)相對于RRT算法分別減少了92.59%、72.93%、78.74%、41.83%,相對于RRT-Connect算法分別減少了63.79%、3.92%、35.87%、1.31%,相對于文獻(xiàn)[20]算法分別減少了0.00%、85.96%、90.13%、91.42%。

文獻(xiàn)[20]算法在狹窄通道地圖中迭代20 000次內(nèi)規(guī)劃路徑成功率約70%。

對100*4次實(shí)驗(yàn)生成的路徑長度分析可知,本文算法可以很好地縮短路徑長度。4種場景下,本文算法規(guī)劃的路徑長度相對于RRT算法分別減少了22.23%、16.96%、16.41%、7.02%,相對于RRT-Connect算法分別減少了20.26%、15.04%、15.07%、5.47%,相對于文獻(xiàn)[20]算法分別增加了0.00%、0.18%、3.63%、8.50%。

文獻(xiàn)[20]算法在狹窄通道地圖中迭代20 000次內(nèi)規(guī)劃路徑成功率約70%。

對100*4次實(shí)驗(yàn)生成路徑上的節(jié)點(diǎn)個(gè)數(shù)分析可知,本文算法可以很好地優(yōu)化路徑節(jié)點(diǎn)個(gè)數(shù)。路徑上節(jié)點(diǎn)個(gè)數(shù)可以反應(yīng)采樣函數(shù)的好壞。4種場景下,本文算法規(guī)劃路徑上的節(jié)點(diǎn)個(gè)數(shù)度相對于RRT算法分別減少了22.22%、17.54%、16.36%、6.25%,相對于RRT-Connect算法分別減少了20.76%、14.55%、16.36%、4.76%,相對于文獻(xiàn)[20]算法分別增加了0.00%、4.08%、2.13%、-5.26%。

綜上所述,針對不同地圖環(huán)境,相比于RRT算法、RRT-Connect算法、文獻(xiàn)[20]算法,本文改進(jìn)算法有效提高了收斂速度、搜索效率以及穩(wěn)定性,減短了路徑長度。

4.3 路徑優(yōu)化仿真分析

路徑優(yōu)化包括剪枝優(yōu)化和三次B樣條曲線擬合。為了驗(yàn)證路徑優(yōu)化的效果,選擇復(fù)雜障礙地圖進(jìn)行仿真實(shí)驗(yàn),實(shí)驗(yàn)效果如圖10所示。

(a) 剪枝優(yōu)化前 (b) 剪枝優(yōu)化后

圖10a為初步規(guī)劃路徑,路徑曲折不光滑且冗余點(diǎn)過多;圖10b為剪枝優(yōu)化后的路徑,路徑上冗余點(diǎn)大大減少,曲折性有所改善;圖10c為三次B樣條曲線擬合后的路徑,路徑光滑性得到優(yōu)化。由實(shí)驗(yàn)結(jié)果可知,本文算法有效提高了路徑平滑性、路徑質(zhì)量,符合移動機(jī)器人的運(yùn)動學(xué)特性,提高了行車的安全性。

5 結(jié)論

為了解決傳統(tǒng)RRT算法進(jìn)行移動機(jī)器人路徑規(guī)劃時(shí)存在的問題,提出了一種改進(jìn)RRT算法,通過理論分析以及仿真分析,得到以下結(jié)論:

(1)引入概率目標(biāo)偏置與人工勢場結(jié)合的采樣策略,改善了節(jié)點(diǎn)生成過程,仿真結(jié)果表明,改進(jìn)算法優(yōu)化了節(jié)點(diǎn)擴(kuò)展,降低了路徑搜索的隨機(jī)性,提高了路徑搜索效率,解決了目標(biāo)點(diǎn)四周生長節(jié)點(diǎn)時(shí)的振蕩問題。目標(biāo)權(quán)重因子、隨機(jī)權(quán)重因子的引入使改進(jìn)算法可以適應(yīng)不同地圖環(huán)境。

(2)引入動態(tài)變步長擴(kuò)展策略,節(jié)點(diǎn)步長擴(kuò)展過程中,在距離目標(biāo)點(diǎn)遠(yuǎn)時(shí)進(jìn)行快速擴(kuò)展,在局部障礙密集區(qū)域進(jìn)行權(quán)重減小步長,做到安全高效地?cái)U(kuò)展節(jié)點(diǎn)。

(3)引入基于安全距離的碰撞檢測策略,使生成的節(jié)點(diǎn)與障礙物之間有一定安全距離,保證生成路徑的質(zhì)量以及實(shí)際移動過程中的安全性。

(4)采用考慮安全距離的剪枝優(yōu)化以及拐點(diǎn)三次B樣條曲線擬合對初步生成路徑進(jìn)行處理,解決了路徑曲折不光滑的問題,使最終路徑符合移動機(jī)器人運(yùn)動學(xué)特性。