商德勇 ,汪俊杰 ,樊 虎 ,索雙富

(1. 中國(guó)礦業(yè)大學(xué)(北京) 機(jī)電與信息工程學(xué)院,北京 100083;2. 中國(guó)礦業(yè)大學(xué)(北京) 智慧礦山與機(jī)器人研究院,北京 100083;3. 煤礦智能化與機(jī)器人創(chuàng)新應(yīng)用應(yīng)急管理部重點(diǎn)實(shí)驗(yàn)室,北京 100083;4.清華大學(xué) 機(jī)械工程系,北京 100081)

0 引言

機(jī)器人廣泛應(yīng)用于汽車、航空、物流、智能制造等各種行業(yè)。機(jī)械臂執(zhí)行任務(wù)時(shí),其快速合理的路徑規(guī)劃是高效完成任務(wù)的關(guān)鍵,其路徑規(guī)劃問(wèn)題是國(guó)內(nèi)外學(xué)者研究的重點(diǎn)方向之一[1]。機(jī)械臂的路徑規(guī)劃目的是給定一個(gè)目標(biāo)構(gòu)型,在滿足機(jī)器人的運(yùn)動(dòng)學(xué)約束的前提下,求取一條從起始構(gòu)型狀態(tài)開(kāi)始連接到目標(biāo)構(gòu)型并避開(kāi)障礙物的可行路徑,通常還要考慮實(shí)時(shí)性要求[2-4]。

對(duì)于移動(dòng)機(jī)器人,常用的路徑規(guī)劃算法有基于圖搜索方法的Dijkstra、A*[5-7],或是人工勢(shì)場(chǎng)法[8-9]。串聯(lián)機(jī)械臂機(jī)器人的空間狀態(tài)由各個(gè)關(guān)節(jié)角確定,構(gòu)型空間是六維空間。在這種高維空間中,A*等算法會(huì)出現(xiàn)“指數(shù)爆炸”的情況,計(jì)算量劇增[10],且需要對(duì)構(gòu)型空間顯式精確建模。研究顯示基于采樣的算法更加適合于高維構(gòu)型空間[11]。其思想是在機(jī)器人構(gòu)型空間內(nèi)不斷地隨機(jī)采樣,利用無(wú)障礙空間中的采樣點(diǎn)構(gòu)成連接起始點(diǎn)和目標(biāo)點(diǎn)的路徑。

LAVALLE[12]提出了快速搜索隨機(jī)樹(shù)算法(Rapidly-exploring Random Tree,RRT),但基本RRT算法由于采用均勻隨機(jī)采樣,其收斂速度慢、路徑非最優(yōu)、多次規(guī)劃路徑不一致,許多學(xué)者對(duì)其進(jìn)行了改進(jìn)。KUFFNER等[13]提出改進(jìn)的RRT-connect算法加快了收斂速度。KARAMAN等[14]提出RRT*算法得到起點(diǎn)到終點(diǎn)的漸近最優(yōu)路徑。LSLAM等[15]針對(duì)RRT*收斂慢的缺點(diǎn),提出智能RRT*算法(RRT*-smart),快速降低整體路徑長(zhǎng)度。NOREEN等[16]詳細(xì)比較了RRT、RRT*和RRT*-smart,指出較長(zhǎng)的計(jì)算時(shí)間在高維空間下會(huì)有更明顯表現(xiàn)。馬冀桐等[17]采用降維映射的方式將封閉形障礙在機(jī)械臂構(gòu)型空間預(yù)先離線表示。GAMMELL等[18]改變了RRT*在空間中盲目采樣的形式,被稱為informed-RRT*,但是這種方法在起點(diǎn)、終點(diǎn)距離較大時(shí)不具有優(yōu)越性。張振等[19]將采樣點(diǎn)限制在以目標(biāo)點(diǎn)為圓心的圓形區(qū)域內(nèi),且半徑隨節(jié)點(diǎn)碰撞情況動(dòng)態(tài)變化,減少搜索盲目性。張立彬等[20]以柵格地圖形式優(yōu)化數(shù)據(jù)存儲(chǔ)結(jié)構(gòu),查找相鄰節(jié)點(diǎn),提升了計(jì)算速度。

本文提出一種改進(jìn)的動(dòng)態(tài)區(qū)域采樣(RRT*-Dynamic Region,RRT*-DR)路徑規(guī)劃算法,通過(guò)限制一個(gè)采樣區(qū)域,減少不必要的路徑擴(kuò)展。采樣區(qū)域隨路徑的優(yōu)化而動(dòng)態(tài)變化,快速獲得一條最優(yōu)避障路徑。同時(shí)提出一種近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制,減少了碰撞檢測(cè)失敗次數(shù),降低了計(jì)算量,提高了路徑規(guī)劃效率。

1 基于改進(jìn)RRT*算法(RRT*-DR)的運(yùn)動(dòng)規(guī)劃

針對(duì)RRT*收斂速度慢的問(wèn)題,提出一種改進(jìn)的路徑規(guī)劃算法RRT*-DR。核心思想是將規(guī)劃過(guò)程分為擴(kuò)展和優(yōu)化兩個(gè)步驟,首先利用半目標(biāo)導(dǎo)向的RRT快速搜索到一條初始路徑,之后利用動(dòng)態(tài)區(qū)域采樣的RRT*對(duì)該初始路徑進(jìn)行迭代優(yōu)化,使其快速收斂到最優(yōu)路徑。

1.1 基本RRT與RRT*算法原理

RRT算法是針對(duì)高維空間路徑規(guī)劃問(wèn)題最常用的算法之一,其基本原理是在構(gòu)型空間隨機(jī)采樣一個(gè)節(jié)點(diǎn),在已有節(jié)點(diǎn)樹(shù)中找到距該隨機(jī)點(diǎn)最近的節(jié)點(diǎn),朝隨機(jī)點(diǎn)擴(kuò)展一個(gè)步長(zhǎng)得到一個(gè)擴(kuò)展點(diǎn),若擴(kuò)展途中沒(méi)有障礙,則添加該擴(kuò)展點(diǎn)和擴(kuò)展路徑到節(jié)點(diǎn)樹(shù)上[21]。通過(guò)不斷重復(fù)上述步驟,即可擴(kuò)展出一棵節(jié)點(diǎn)樹(shù),探索整個(gè)空間。RRT算法的缺點(diǎn)是無(wú)法得到最優(yōu)路徑,路徑代價(jià)較大,這一問(wèn)題在RRT*上得到了解決。

RRT*算法在經(jīng)典RRT的基礎(chǔ)上,增加了重選父節(jié)點(diǎn)和重布線過(guò)程。經(jīng)由每一步擴(kuò)展之后的重選父節(jié)點(diǎn)和重布線的處理,節(jié)點(diǎn)趨向于連接鄰域內(nèi)路徑代價(jià)最小的點(diǎn),隨著迭代的進(jìn)行,所有節(jié)點(diǎn)都趨近于以最小路徑代價(jià)連接到起點(diǎn),節(jié)點(diǎn)樹(shù)趨于平滑。起點(diǎn)到無(wú)障礙空間任一點(diǎn)都是在保證避障的前提下,通過(guò)相對(duì)最短的接近直線的路徑來(lái)連接。同理,經(jīng)過(guò)足夠迭代,也能保證起始點(diǎn)到目標(biāo)點(diǎn)的路徑漸進(jìn)最優(yōu)性。

1.2 半目標(biāo)導(dǎo)向的快速擴(kuò)展過(guò)程

傳統(tǒng)算法的隨機(jī)樹(shù)采樣過(guò)程采用均勻采樣,盡可能地探索到整個(gè)空間,具備很強(qiáng)的探索性,但忽略了目的性,即朝向目標(biāo)運(yùn)動(dòng)的最終目標(biāo)。若過(guò)于強(qiáng)調(diào)探索性,對(duì)整個(gè)空間的探索會(huì)浪費(fèi)大量時(shí)間和資源;若過(guò)于強(qiáng)調(diào)目的性,會(huì)因?yàn)檎系K物的攔阻而受困于局部極小值。因此,需要平衡擴(kuò)展的探索性和目的性。對(duì)此,采用半目標(biāo)導(dǎo)向的擴(kuò)展方式,如圖1所示,設(shè)置一個(gè)目標(biāo)導(dǎo)向閾值p,在每次采樣之前取一個(gè)隨機(jī)數(shù)rgoal,并與p比較。若rgoalp,將目標(biāo)點(diǎn)作為采樣點(diǎn),引導(dǎo)隨機(jī)樹(shù)直接向目標(biāo)點(diǎn)擴(kuò)展。

圖1 半目標(biāo)導(dǎo)向

該擴(kuò)展方式可以兼顧探索性和目的性。一方面,將目標(biāo)點(diǎn)作為隨機(jī)采樣點(diǎn)時(shí)可以使節(jié)點(diǎn)樹(shù)快速接近目標(biāo);另一方面,隨機(jī)得到采樣點(diǎn)可以向自由無(wú)障礙空間擴(kuò)展,繞過(guò)障礙,避免陷入局部極小。當(dāng)環(huán)境中障礙物比較多時(shí),可以適當(dāng)增大p以提高隨機(jī)探索概率;當(dāng)障礙物比較少時(shí),可以適當(dāng)減小p以增大目標(biāo)導(dǎo)向的概率。

半目標(biāo)導(dǎo)向采樣函數(shù)的偽代碼如表1所示。

表1 半目標(biāo)導(dǎo)向采樣

1.3 動(dòng)態(tài)采樣區(qū)域優(yōu)化過(guò)程

找到初始路徑后,進(jìn)入優(yōu)化過(guò)程。傳統(tǒng)RRT*的優(yōu)化過(guò)程針對(duì)整個(gè)空間進(jìn)行擴(kuò)展優(yōu)化,導(dǎo)致其在高維構(gòu)型空間收斂緩慢[22]。

本文對(duì)RRT*算法進(jìn)行改進(jìn),引入動(dòng)態(tài)區(qū)域優(yōu)化方法,用于優(yōu)化初始路徑。其思想是在初始路徑節(jié)點(diǎn)構(gòu)成的初始路徑周圍進(jìn)行采樣擴(kuò)展,且隨著RRT*的優(yōu)化,初始路徑逐漸改變,采樣范圍也隨之改變,即始終在當(dāng)前最優(yōu)路徑周圍進(jìn)行采樣擴(kuò)展和優(yōu)化,如圖2所示。

圖2 動(dòng)態(tài)區(qū)域優(yōu)化原理圖

將擴(kuò)展資源全部用于對(duì)改善路徑有幫助的區(qū)域,且在有限區(qū)域內(nèi)可以快速增大節(jié)點(diǎn)密度,密化節(jié)點(diǎn)樹(shù),進(jìn)而快速得到漸進(jìn)最優(yōu)的路徑。動(dòng)態(tài)區(qū)域優(yōu)化過(guò)程偽代碼如表2所示。

表2 動(dòng)態(tài)區(qū)域采樣

在路徑節(jié)點(diǎn)周圍一定范圍內(nèi)采樣,只密化有意義區(qū)域的隨機(jī)樹(shù),而放棄遠(yuǎn)離起點(diǎn)終點(diǎn)的區(qū)域。每次得到新的樹(shù)節(jié)點(diǎn)后,進(jìn)行的重選父節(jié)點(diǎn)和重布線過(guò)程可以優(yōu)化隨機(jī)樹(shù),得到長(zhǎng)度更短的新路徑。如此循環(huán)迭代得到一系列新的路徑節(jié)點(diǎn),可以在新路徑節(jié)點(diǎn)周圍采樣,經(jīng)過(guò)持續(xù)迭代,可以快速收斂到最優(yōu)路徑。在優(yōu)化路徑過(guò)程中,重選父節(jié)點(diǎn)和重布線過(guò)程的選取范圍為一個(gè)圓,其半徑為rneighbor,該值影響算法執(zhí)行時(shí)間。隨著迭代的進(jìn)行,樹(shù)節(jié)點(diǎn)密度增大,若rneighbor始終保持在較大數(shù)值,在每次重選父節(jié)點(diǎn)、重布線過(guò)程中會(huì)涉及大量近鄰節(jié)點(diǎn)的計(jì)算,將消耗大量計(jì)算時(shí)間。因此,隨著迭代次數(shù)的增加,應(yīng)逐漸減小rneighbor,并最終收斂到一個(gè)定值,如式(1)所示:

(1)

其中n為節(jié)點(diǎn)樹(shù)上的節(jié)點(diǎn)個(gè)數(shù)。由式(1)可知,當(dāng)節(jié)點(diǎn)樹(shù)開(kāi)始擴(kuò)展時(shí),rneighbor大小為其初始值。隨著節(jié)點(diǎn)樹(shù)擴(kuò)大密化,節(jié)點(diǎn)增多,rneighbor逐漸減小,最終收斂到初始值的0.2倍。

1.4 近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制

RRT及其擴(kuò)展算法都屬于單線查詢型算法[23],因其基于采樣的特性,可以不對(duì)障礙物進(jìn)行顯示建模,故提高了環(huán)境適應(yīng)性。對(duì)于環(huán)境障礙物的探索,通過(guò)碰撞檢測(cè)環(huán)節(jié)完成,而碰撞檢測(cè)環(huán)節(jié)是整個(gè)擴(kuò)展尋路過(guò)程中最耗時(shí)的環(huán)節(jié),在障礙物集中的區(qū)域尤其嚴(yán)重,且因?yàn)榕鲎矙z測(cè)失敗而舍棄的節(jié)點(diǎn)也占到了總迭代節(jié)點(diǎn)數(shù)量的一半以上[24]。傳統(tǒng)RRT*在擴(kuò)展得到新節(jié)點(diǎn)之后進(jìn)行碰撞檢測(cè),碰撞檢測(cè)失敗的節(jié)點(diǎn)被直接舍棄,沒(méi)有得到利用,不能為后續(xù)擴(kuò)展提供指導(dǎo)。本文提出一種近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制,即將每一次碰撞檢測(cè)失敗的節(jié)點(diǎn)單獨(dú)放置,進(jìn)行記錄,后續(xù)將樹(shù)節(jié)點(diǎn)向隨機(jī)點(diǎn)擴(kuò)展時(shí),先判斷該隨機(jī)點(diǎn)是否鄰近障礙物,若隨機(jī)點(diǎn)臨近障礙物且待擴(kuò)展樹(shù)節(jié)點(diǎn)距離隨機(jī)點(diǎn)較近時(shí),縮短擴(kuò)展步長(zhǎng),使擴(kuò)展得到的新節(jié)點(diǎn)靠近障礙而不碰撞,如圖3所示。既避免了碰撞檢測(cè)失敗舍棄節(jié)點(diǎn)而浪費(fèi)擴(kuò)展時(shí)間,又使得隨機(jī)樹(shù)更加靠近障礙,進(jìn)而使得規(guī)劃路徑更優(yōu)。

圖3 近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制

RRT*的碰撞檢測(cè)過(guò)程將算法擴(kuò)展步驟得到的連接樹(shù)節(jié)點(diǎn)和隨機(jī)采樣點(diǎn)的路徑連線平均分為若干段,判斷每一均分點(diǎn)是否使障礙相撞。若其中至少有一個(gè)均分點(diǎn)與障礙相撞,則檢測(cè)失敗。將碰撞檢測(cè)失敗的路徑均分點(diǎn)命名為障礙標(biāo)志點(diǎn)pcollision。當(dāng)采樣得到隨機(jī)點(diǎn)后,進(jìn)行擴(kuò)展步驟,搜索到隨機(jī)樹(shù)中相距最近樹(shù)節(jié)點(diǎn),即待擴(kuò)展點(diǎn)。隨后判斷待擴(kuò)展點(diǎn)周圍半徑r的圓形范圍內(nèi)是否存在障礙標(biāo)志點(diǎn)。若不存在,則表示待擴(kuò)展點(diǎn)遠(yuǎn)離障礙,可按照原步長(zhǎng)stepsize擴(kuò)展;若存在,則表示待擴(kuò)展點(diǎn)靠近障礙,有碰撞障礙的可能,得到待擴(kuò)展點(diǎn)到最近障礙標(biāo)志點(diǎn)間的距離l。

計(jì)算障礙標(biāo)志點(diǎn)到待擴(kuò)展點(diǎn)與隨機(jī)采樣點(diǎn)形成的直線的距離d,以此判斷在擴(kuò)展方向上是否存在障礙標(biāo)志點(diǎn),如式(2)所示:

(2)

當(dāng)待擴(kuò)展點(diǎn)到最近障礙標(biāo)志點(diǎn)的距離小于原始擴(kuò)展步長(zhǎng)時(shí),若上述距離d小于設(shè)定閾值d0,則表示待擴(kuò)展點(diǎn)的擴(kuò)展方向上有障礙存在。在l小于步長(zhǎng)且d

(3)

近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制偽代碼如表3所示。

表3 近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制

為驗(yàn)證近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制的有效性,在地圖1中進(jìn)行仿真實(shí)驗(yàn)。設(shè)置相同的輸入?yún)?shù)與迭代次數(shù),分別使用完整的RRT*-DR算法和未采用變步長(zhǎng)機(jī)制的RRT*-DR,比較與障礙物碰撞的次數(shù)以及規(guī)劃消耗時(shí)間。圖4a和圖4b分別是由未加變步長(zhǎng)機(jī)制和采用變步長(zhǎng)機(jī)制的RRT*-DR擴(kuò)展過(guò)程得到的pcollision分布圖。障礙標(biāo)志點(diǎn)pcollision用圓圈表示,顯然若圓圈越密集,則表示碰撞次數(shù)越多。由圖4可見(jiàn),采用變步長(zhǎng)機(jī)制的擴(kuò)展過(guò)程,藍(lán)色圓圈更少,即其與障礙碰撞的次數(shù)更少。通過(guò)5組對(duì)比仿真得到結(jié)果如表4所示,未采用和采用該機(jī)制的算法平均碰撞次數(shù)分別為1416.2次和585.4次,可見(jiàn)采用該機(jī)制后碰撞次數(shù)減少58.7%,且規(guī)劃時(shí)間相對(duì)減少24.3%。故采用近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制,通過(guò)減小靠近障礙標(biāo)志點(diǎn)的節(jié)點(diǎn)的擴(kuò)展步長(zhǎng),有效降低了與障礙物的碰撞次數(shù),進(jìn)而減少重新取點(diǎn)擴(kuò)展的重復(fù)操作,有效縮短了規(guī)劃時(shí)間。

表4 近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制比較

圖4 有無(wú)近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制的規(guī)劃仿真

1.5 connect思想

為進(jìn)一步提高搜索效率,引入RRT-connect算法的思想,使每次迭代產(chǎn)生的隨機(jī)點(diǎn)xrand輪流作為起點(diǎn)節(jié)點(diǎn)樹(shù)和終點(diǎn)節(jié)點(diǎn)樹(shù)的引導(dǎo)點(diǎn),并分別對(duì)起點(diǎn)節(jié)點(diǎn)樹(shù)和終點(diǎn)節(jié)點(diǎn)樹(shù)進(jìn)行擴(kuò)展,即同時(shí)從起始點(diǎn)和目標(biāo)點(diǎn)生長(zhǎng)出兩棵節(jié)點(diǎn)樹(shù),并利用半目標(biāo)導(dǎo)向機(jī)制的引導(dǎo)作用,著重朝另一節(jié)點(diǎn)樹(shù)的方向擴(kuò)展出新節(jié)點(diǎn),加快對(duì)狀態(tài)空間的探索和初始路徑的產(chǎn)生。原理如圖5所示。

圖5 connect思想

connect思想執(zhí)行步驟為:

(1)執(zhí)行半目標(biāo)導(dǎo)向機(jī)制,得到一個(gè)隨機(jī)點(diǎn)xrand,從起點(diǎn)節(jié)點(diǎn)樹(shù)搜索歐式距離最近的樹(shù)節(jié)點(diǎn),并以其為基礎(chǔ)擴(kuò)展出一個(gè)新節(jié)點(diǎn)xnew_s,添加到起點(diǎn)節(jié)點(diǎn)樹(shù)上;

(2)同步驟(1),產(chǎn)生一個(gè)隨機(jī)點(diǎn)xrand,從終點(diǎn)節(jié)點(diǎn)樹(shù)搜索歐式距離最近的樹(shù)節(jié)點(diǎn),并以其為基礎(chǔ)擴(kuò)展出一個(gè)新節(jié)點(diǎn)xnew_e,添加到終點(diǎn)節(jié)點(diǎn)樹(shù)上;

(3)判斷xnew_e的周邊鄰域內(nèi)是否存在起點(diǎn)節(jié)點(diǎn)樹(shù)的節(jié)點(diǎn),若存在,則將xnew_e與其連接,得到初始路徑;否則重復(fù)步驟(1)～步驟(3)。

connect思想偽代碼如表5所示。

表5 connect思想

綜上,RRT*-DR算法流程圖如圖6所示。

圖6 RRT*-DR算法流程圖

2 仿真驗(yàn)證

本章在MATLAB環(huán)境下仿真驗(yàn)證RRT*-DR算法的有效性與優(yōu)越性,與RRT*、RRT*-connect算法比較。仿真設(shè)備平臺(tái)的配置為主頻2.30 GHz的Intel Core i5-6200U CPU,內(nèi)存16 GB,搭載Windows 10和Ubuntu 18.04雙操作系統(tǒng)。

在MATLAB中分別導(dǎo)入如圖7所示3種500 mm×500 mm地圖環(huán)境,分別對(duì)各算法的越障能力、尋優(yōu)能力、復(fù)雜環(huán)境尋路能力進(jìn)行驗(yàn)證。其中白色部分為自由空間,黑色部分為障礙空間。

圖7 仿真地圖

2.1 地圖1

地圖環(huán)境1中以(90,250)為起始點(diǎn),以(400,250)為目標(biāo)點(diǎn),環(huán)境障礙在地圖環(huán)境中呈對(duì)稱結(jié)構(gòu),并半包圍起始點(diǎn),考察算法的越障能力。各算法的基本參數(shù)設(shè)置為相同,步長(zhǎng)為20 mm,目標(biāo)導(dǎo)向閾值p為0.5,迭代次數(shù)為800次。3種算法仿真結(jié)果如圖8所示,分別進(jìn)行20次重復(fù)測(cè)試,將測(cè)試指標(biāo)整理數(shù)據(jù)如圖9所示。

圖8 地圖1中的算法規(guī)劃效果對(duì)比

圖9 地圖1中的仿真結(jié)果對(duì)比

由圖9a可知,本文所提RRT*-DR算法在迭代次數(shù)較少時(shí)便可得到相對(duì)于RRT*與RRT*-connect算法路徑代價(jià)更小的路徑,平均比RRT*路徑短,且隨著迭代次數(shù)的增多,可以快速收斂到最佳路徑。圖9b中比較了兩種算法在得到一定路徑代價(jià)的路徑所需要的規(guī)劃時(shí)間。在該障礙情況較為簡(jiǎn)單的地圖中,路徑優(yōu)化到接近最優(yōu)路徑時(shí),三者所需時(shí)間較為接近,整體上看RRT*-DR得到較優(yōu)路徑所需時(shí)間更少,收斂速度相比RRT*更快。3種算法仿真結(jié)果數(shù)據(jù)對(duì)比如表6所示。

表6 地圖1中的仿真數(shù)據(jù)對(duì)比

2.2 地圖2

地圖環(huán)境2在設(shè)置有直接分割起點(diǎn)終點(diǎn)障礙的基礎(chǔ)上,增加一個(gè)障礙缺口,提供一個(gè)同倫類路徑的選擇。若連接起點(diǎn)終點(diǎn)的一條路徑,在不分離和跨過(guò)障礙的基礎(chǔ)上,能夠連續(xù)變換為另一條路徑,則這兩條路徑同倫[25]。增加不同同倫類路徑通道,可以測(cè)試算法是否具有良好的優(yōu)化能力,收斂得到一條代價(jià)最小的路徑。分別以(5,5)為起始點(diǎn),(300,140)為目標(biāo)點(diǎn)進(jìn)行路徑規(guī)劃,仿真結(jié)果及數(shù)據(jù)如圖10和圖11所示。

圖10 地圖2中的算法規(guī)劃效果對(duì)比

圖11 地圖2中的仿真結(jié)果對(duì)比

RRT*與RRT*-connect在地圖2環(huán)境中迭代次數(shù)較少時(shí)無(wú)法規(guī)劃成功,迭代次數(shù)在500次以上才有較高的規(guī)劃成功率,且經(jīng)過(guò)迭代路徑代價(jià)依舊較高,如圖11a所示。而RRT*-DR以較少迭代次數(shù)就可收斂到最優(yōu)路徑長(zhǎng)度。由圖11b可看出,RRT*-DR將路徑優(yōu)化到一定長(zhǎng)度所需規(guī)劃時(shí)間小于RRT*。3種算法仿真結(jié)果數(shù)據(jù)對(duì)比如表7所示。

表7 地圖2中的仿真數(shù)據(jù)對(duì)比

2.3 地圖3

地圖環(huán)境3設(shè)置有大量雜亂矩形障礙,模擬障礙物較多的復(fù)雜環(huán)境,考察各算法對(duì)多障礙環(huán)境的適應(yīng)性。分別以(5,5)為起始點(diǎn),(450,450)為目標(biāo)點(diǎn)進(jìn)行路徑規(guī)劃,得到的結(jié)果及仿真數(shù)據(jù)如圖12和圖13所示。

圖12 地圖3中的算法規(guī)劃效果對(duì)比

圖13 地圖3中的仿真結(jié)果對(duì)比

由圖13可知,在地圖3環(huán)境中3種算法的優(yōu)化收斂速度比較接近,但RRT*-DR算法所需要的規(guī)劃時(shí)間較少,可有效減少碰撞次數(shù),提高路徑規(guī)劃效率。3種算法仿真結(jié)果數(shù)據(jù)對(duì)比如表8所示。

表8 地圖3中的仿真數(shù)據(jù)對(duì)比

通過(guò)平面地圖仿真實(shí)驗(yàn)可驗(yàn)證RRT*-DR算法具有時(shí)間短、路徑優(yōu)的優(yōu)勢(shì)。相對(duì)于二維地圖,三維地圖環(huán)境中坐標(biāo)點(diǎn)的描述多一個(gè)維度,本文所提出的RRT*-DR算法的核心是半目標(biāo)導(dǎo)向機(jī)制、動(dòng)態(tài)區(qū)域采樣機(jī)制和近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制,均通過(guò)采樣點(diǎn)的坐標(biāo)統(tǒng)計(jì)和相關(guān)距離計(jì)算實(shí)現(xiàn),在坐標(biāo)增加一維時(shí),其效果不受影響,故該算法可適用于三維地圖環(huán)境。

3 實(shí)驗(yàn)驗(yàn)證

在電腦主板研發(fā)階段為驗(yàn)證設(shè)計(jì)合理性,需要對(duì)樣板上各電子元器件的電學(xué)性能進(jìn)行調(diào)試和測(cè)試,傳統(tǒng)方式通常由人工手持示波器探針進(jìn)行焊點(diǎn)檢測(cè)。由于主板待測(cè)焊點(diǎn)尺寸小、密度大,準(zhǔn)確地按壓待測(cè)點(diǎn)有一定難度,且操作人員長(zhǎng)時(shí)間工作容易產(chǎn)生視覺(jué)和肌肉疲勞,用機(jī)械臂夾持探針進(jìn)行觸點(diǎn)點(diǎn)測(cè)是一種低成本高效率的解決方法。

主板檢測(cè)機(jī)器人是通過(guò)機(jī)械臂末端夾持探針來(lái)實(shí)現(xiàn)的,機(jī)器人檢測(cè)實(shí)驗(yàn)平臺(tái)如圖14所示。主板夾具將主板豎直裝夾在機(jī)架上,中央平臺(tái)上放置示波器以及鍵盤(pán)等主板外設(shè)。布置兩臺(tái)機(jī)械臂于主板兩側(cè),可同時(shí)實(shí)現(xiàn)兩組測(cè)點(diǎn)的檢測(cè)需求。

圖14 主板檢測(cè)實(shí)驗(yàn)臺(tái)

主板檢測(cè)環(huán)境存在著主板、夾具、機(jī)架等障礙物,屬于多障礙物復(fù)雜環(huán)境,為檢測(cè)帶來(lái)較大困難,快速高效合理的路徑規(guī)劃是實(shí)現(xiàn)檢測(cè)的關(guān)鍵。

為檢驗(yàn)本文提出的RRT*-DR算法的有效性,使用法奧FR5協(xié)作機(jī)器人在主板檢測(cè)環(huán)境中進(jìn)行實(shí)驗(yàn)驗(yàn)證。首先在機(jī)械臂ROS系統(tǒng)進(jìn)行可視化仿真,模擬主板檢測(cè)的實(shí)際工況。借助ROSmoveit!的C++編程接口編寫(xiě)運(yùn)動(dòng)仿真程序,導(dǎo)入機(jī)器人的URDF模型,將RRT*-DR路徑規(guī)劃算法添加到運(yùn)動(dòng)規(guī)劃庫(kù)OMPL中,使用moveit!完成調(diào)用和規(guī)劃。由rqt_graph得到各ROS節(jié)點(diǎn)框架關(guān)系圖,如圖15所示。運(yùn)動(dòng)仿真步驟為:機(jī)器人從初始蜷縮狀態(tài)出發(fā),點(diǎn)測(cè)主板正面測(cè)點(diǎn)。仿真環(huán)境中的運(yùn)行結(jié)果如圖16所示。機(jī)械臂從初始位姿以較短路徑到達(dá)目標(biāo)位姿,成功避開(kāi)障礙。

圖16 ROS機(jī)械臂運(yùn)動(dòng)仿真

在試驗(yàn)臺(tái)法奧FR5機(jī)械臂控制系統(tǒng)中導(dǎo)入RRT*-DR算法進(jìn)行實(shí)驗(yàn)驗(yàn)證。如圖17所示,主板被固定在機(jī)架上,機(jī)械臂末端安裝探針,從初始位置出發(fā),執(zhí)行路徑規(guī)劃得到的路徑,最終避開(kāi)障礙平穩(wěn)準(zhǔn)確到達(dá)待測(cè)焊點(diǎn)目標(biāo)位置。進(jìn)行20次路徑規(guī)劃算法測(cè)試,平均規(guī)劃時(shí)間為2.416 s,且全部成功。驗(yàn)證了本文算法的實(shí)用性和有效性。

圖17 機(jī)械臂運(yùn)行實(shí)驗(yàn)

4 結(jié)束語(yǔ)

本文針對(duì)機(jī)械臂運(yùn)動(dòng)路徑規(guī)劃問(wèn)題,提出一種基于RRT*的改進(jìn)算法RRT*-DR。在傳統(tǒng)RRT*的基礎(chǔ)上,將整個(gè)規(guī)劃過(guò)程分為快速擴(kuò)展過(guò)程和優(yōu)化過(guò)程,首先利用半目標(biāo)導(dǎo)向的RRT快速擴(kuò)展找到一條初始路徑,隨后利用動(dòng)態(tài)區(qū)域采樣對(duì)初始路徑進(jìn)行優(yōu)化,快速收斂到漸進(jìn)最優(yōu)路徑,避免了盲目探索與優(yōu)化。此外,引入了近障礙節(jié)點(diǎn)變步長(zhǎng)機(jī)制,進(jìn)一步減少碰撞次數(shù),節(jié)省了碰撞檢測(cè)失敗的時(shí)間和隨機(jī)樹(shù)節(jié)點(diǎn)消耗。在MATLAB中進(jìn)行了3種不同地圖環(huán)境下的仿真,通過(guò)比較多種算法的路徑代價(jià)、迭代次數(shù)、時(shí)間、碰撞次數(shù)等評(píng)價(jià)指標(biāo),驗(yàn)證了本文所提算法的有效性和優(yōu)越性。該算法用于電腦主板檢測(cè)機(jī)器人路徑規(guī)劃,實(shí)現(xiàn)了機(jī)械臂安全高效避障,驗(yàn)證了該算法的有效性。在此研究基礎(chǔ)上,未來(lái)可對(duì)進(jìn)一步進(jìn)行沖擊最優(yōu)優(yōu)化,以減小機(jī)器人關(guān)節(jié)電機(jī)的沖擊,提高機(jī)器人運(yùn)動(dòng)的平滑性。