基于改進(jìn)RRT算法的機(jī)械臂避障路徑規(guī)劃

陳大光,劉興德,賈永濤,錢 程

(1.吉林化工學(xué)院 信息與控制工程學(xué)院,吉林 吉林 132022;2.吉林化工學(xué)院 機(jī)電工程學(xué)院,吉林 吉林 132022;3.中國石油吉林石化公司 化肥廠,吉林 吉林 132021)

隨著“智能制造”的不斷推進(jìn),機(jī)器人技術(shù)得以飛速發(fā)展,給人類生活帶來便利[1]。而機(jī)械臂作為機(jī)器人領(lǐng)域的重要分支,已被廣泛應(yīng)用于工業(yè)生產(chǎn)的各個(gè)領(lǐng)域,特別是在重工業(yè)制造、航空航天、軍事、醫(yī)療等方面[2]。由于機(jī)械臂工作的空間越來越復(fù)雜化,機(jī)械臂的路徑規(guī)劃就顯得格外重要,成為現(xiàn)在許多學(xué)者研究的熱點(diǎn)及其難點(diǎn)[3]。機(jī)械臂的路徑規(guī)劃就是自主地從起始點(diǎn)搜尋到目標(biāo)點(diǎn)動(dòng)作,躲避工作空間內(nèi)的障礙物,找到可行路徑與最優(yōu)路徑[4]。當(dāng)前機(jī)械臂路徑規(guī)劃算法主要有RRT(快速隨機(jī)樹算法)、人工勢能法[5]、蟻群算法[6]、遺傳算法[7]等成熟算法。由于RRT算法具有搜索效率高,并且能解決復(fù)雜環(huán)境下的問題等優(yōu)點(diǎn)被廣泛應(yīng)用。

國內(nèi)外學(xué)者針對(duì)RRT算法的不足進(jìn)行改進(jìn),Cao等[8]在隨機(jī)樹節(jié)點(diǎn)搜索時(shí)加入了目標(biāo)導(dǎo)向的思想,使隨機(jī)樹具有方向性,并利用遺傳算法優(yōu)化生成的路徑。王素琴等[9]提出在RRT算法生成路徑點(diǎn)的基礎(chǔ)上采用貪心算法消除一些冗余節(jié)點(diǎn),但優(yōu)化后的路徑效果并不顯著。阮曉鋼等[10]提出了基于子目標(biāo)搜索策略,進(jìn)行RRT目標(biāo)導(dǎo)向路徑規(guī)劃算法,減少冗余點(diǎn)擴(kuò)展,提高了路徑規(guī)劃的效率。劉恩海等[11]提出改進(jìn)隨機(jī)數(shù)生長方式,加入引力函數(shù),使隨機(jī)數(shù)趨向于目標(biāo)點(diǎn)方向擴(kuò)展,使算法具有方向性。Jeong等[12]在RRT*基礎(chǔ)上提出通過增大選擇父節(jié)點(diǎn)、修剪隨機(jī)樹的運(yùn)行范圍,最終使生成路徑減少,但是增加了運(yùn)行時(shí)長。劉成菊等[13]在標(biāo)準(zhǔn)RRT算法的基礎(chǔ)上融合人工勢場法,利用動(dòng)態(tài)步長提升了隨機(jī)樹擴(kuò)展效率,但并沒有考慮到融合的算法容易陷入局部極小值的問題。

綜合已有的研究成果與存在的問題,本文提出一種基于改進(jìn)的RRT算法來進(jìn)行機(jī)械臂的路徑規(guī)劃研究,在RRT的基礎(chǔ)上加入目標(biāo)偏置策略、節(jié)點(diǎn)拒絕策略和可變步長策略,增強(qiáng)了算法的收斂速度和機(jī)械臂避障的效果,再對(duì)處理后的隨機(jī)樹進(jìn)行平滑性優(yōu)化,最后通過Matlab軟件仿真驗(yàn)證了改進(jìn)RRT算法的優(yōu)越性。

1 RRT算法基本原理

RRT算法由Lavalle教授最先提出的,該算法是通過隨機(jī)采樣進(jìn)行擴(kuò)展節(jié)點(diǎn),最終生成路徑,可以用于解決高低維度的避障問題,所以被廣泛用于機(jī)械臂的路徑規(guī)劃。RRT算法的核心思想是:通過起始點(diǎn)快速延伸成一棵隨機(jī)樹,在無碰撞的條件下搜索求得從起始點(diǎn)到目標(biāo)點(diǎn)最優(yōu)且平滑的一條避障路徑。標(biāo)準(zhǔn)RRT算法采樣擴(kuò)展過程如圖1所示。

圖1 RRT隨機(jī)樹采樣擴(kuò)展過程

在工作地圖空間中,以起始點(diǎn)作為隨機(jī)樹根節(jié)點(diǎn),根據(jù)采樣函數(shù)產(chǎn)生隨機(jī)采樣點(diǎn),然后從所構(gòu)建的隨機(jī)樹中搜尋與采樣點(diǎn)距離最近的節(jié)點(diǎn),連接節(jié)點(diǎn)與節(jié)點(diǎn),這個(gè)連接線的方向就是隨機(jī)樹生長的方向,設(shè)定一個(gè)固定的步長作為隨機(jī)樹的生長步長,并且擴(kuò)展出一個(gè)新的節(jié)點(diǎn),判斷節(jié)點(diǎn)到節(jié)點(diǎn)之間是否穿過障礙物,如果穿過,則放棄該新節(jié)點(diǎn),反之,則將節(jié)點(diǎn)加入樹中。循環(huán)執(zhí)行以上生長過程,最后把生成的新節(jié)點(diǎn)和節(jié)點(diǎn)兩者之間的長度與設(shè)定的步長進(jìn)行比較,如果小于設(shè)定的步長則將兩者相連,這個(gè)路徑就是RRT算法生成的最終路徑。

雖然傳統(tǒng)的RRT算法適用于高低維度的機(jī)械臂路徑規(guī)劃,但也存在一些不足,本文針對(duì)其不足對(duì)RRT算法提出了改進(jìn)方案。

2 改進(jìn)的RRT算法

2.1 采樣點(diǎn)目標(biāo)偏置選擇策略

由于標(biāo)準(zhǔn)RRT算法在擴(kuò)展時(shí)具有盲目性,導(dǎo)致隨機(jī)樹會(huì)產(chǎn)生一些沒有作用的節(jié)點(diǎn),使隨機(jī)搜索擴(kuò)展速度變慢,增加了運(yùn)行時(shí)長,所以本文提出了一種具有導(dǎo)向性的采樣點(diǎn)目標(biāo)偏置策略,對(duì)盲目性問題做了改進(jìn)。該策略的核心思想是:將傳統(tǒng)RRT算法的隨機(jī)采樣函數(shù)加以改進(jìn),引入目標(biāo)偏置函數(shù),使隨機(jī)樹的生長方向朝著目標(biāo)點(diǎn)進(jìn)行傾向,而不是在整個(gè)工作空間內(nèi)進(jìn)行隨機(jī)采樣,從而降低了節(jié)點(diǎn)數(shù)量,進(jìn)一步縮短了路徑規(guī)劃的時(shí)間,采樣擴(kuò)展點(diǎn)偏置策略示意圖如圖2所示。

圖2 擴(kuò)展點(diǎn)偏置策略示意圖

采樣擴(kuò)展點(diǎn)偏置策略原理表達(dá)式如式(1)所示。

(1)

在隨機(jī)采樣過程中,P為隨機(jī)采樣概率值;α為目標(biāo)偏置概率閾值。當(dāng)P<α?xí)r,即隨機(jī)采樣點(diǎn)設(shè)定為目標(biāo)點(diǎn),使隨機(jī)樹的生長方向朝著目標(biāo)點(diǎn)的傾向;反之,以1-P的概率產(chǎn)生連續(xù)隨機(jī)采樣點(diǎn),利用此種策略可以縮減大量的運(yùn)行時(shí)間,提高工作效率。

2.2 可變步長策略與節(jié)點(diǎn)拒絕策略

標(biāo)準(zhǔn)RRT算法還有另外一個(gè)缺陷,就是該算法的擴(kuò)展步長(step distance)是固定不變的,為了提升算法的收斂性,本文提出了可變步長的策略,其核心思想為隨機(jī)樹在初始時(shí)以固定步長進(jìn)行擴(kuò)展生長,進(jìn)入障礙物影響范圍時(shí),變換步長長度。可變步長策略變換公式如式(2)所示。

(2)

式(2)中:grow_step為改進(jìn)后的步長;k為步長系數(shù);|Frep|為斥力標(biāo)量;s為當(dāng)前位置與障礙物的距離;p0為障礙物的影響范圍。當(dāng)s

RRT算法隨機(jī)樹生長過程中會(huì)產(chǎn)生一些無用的節(jié)點(diǎn),減少這些節(jié)點(diǎn)的同時(shí)可以提高算法運(yùn)行效率。加入節(jié)點(diǎn)拒絕策略可以剔除這些無用的節(jié)點(diǎn),節(jié)省工作時(shí)長。節(jié)點(diǎn)拒絕策略的原理表達(dá)式見式(3)。

cost=‖x-q_start‖+‖q_goal-x‖>ε。

(3)

在式(3)中,x為等待候選節(jié)點(diǎn)位置;q_start為起始點(diǎn)位置;q_goal為目標(biāo)點(diǎn)位置;ε為當(dāng)前最優(yōu)路徑代價(jià);cost為成本代價(jià)。當(dāng)從q_start節(jié)點(diǎn)到x節(jié)點(diǎn)及x節(jié)點(diǎn)到q_goal節(jié)點(diǎn)總的距離如果大于cost則保留此節(jié)點(diǎn),反之,剔除此節(jié)點(diǎn)。

2.3 路徑優(yōu)化策略

因傳統(tǒng)RRT算法具有隨機(jī)采樣的特性,生成的路徑一般是曲折、不平滑的折線段,會(huì)有許多非必要的亢余節(jié)點(diǎn),這使后續(xù)機(jī)械臂運(yùn)動(dòng)路徑的動(dòng)作不連貫,對(duì)機(jī)械臂會(huì)產(chǎn)生一定損耗。因此,通過使用貪心策略刪除一些亢余節(jié)點(diǎn),得到連續(xù)的路徑,間接減少了內(nèi)存的消耗。

貪心策略的具體步驟為對(duì)中間任意節(jié)點(diǎn)與目標(biāo)點(diǎn)之間的路徑檢測是否發(fā)生碰撞,如果發(fā)生了碰撞,這兩個(gè)點(diǎn)之間的所有節(jié)點(diǎn)都不能省略,需要對(duì)設(shè)定的任意節(jié)點(diǎn)追溯到上一級(jí)的節(jié)點(diǎn),再進(jìn)行循環(huán)檢測,最后得到簡化后的路徑;反之,如果沒有發(fā)生碰撞,這兩個(gè)點(diǎn)之間的所有節(jié)點(diǎn)都可以省略。

經(jīng)過貪心策略處理后的路徑,再使用三次B樣條曲線對(duì)路徑轉(zhuǎn)折點(diǎn)進(jìn)行平滑性處理[14]。3階B樣條曲線的方程見式(4)。

(4)

(5)

式(4)中,Pi為控制曲線的特征點(diǎn);Fi,3(x)是3次B樣條曲線的基函數(shù);x∈[0,1]。將式(5)代入式(4)中就可以得到式(6),即得到三次B樣條曲線方程。

(6)

將路徑點(diǎn)帶入到曲線方程(6)中,可得到兩者之間的關(guān)系式見式(7)。

(7)

式(7)中,?i、?i+1、?i+2、?i+3為路徑控制點(diǎn)。

2.4 改進(jìn)后RRT算法描述

基于改進(jìn)后的RRT路徑規(guī)劃算法主要步驟流程圖如圖3所示。

圖3 改進(jìn)RRT步驟流程圖

由圖3可以總結(jié)出改進(jìn)后的RRT算法的詳細(xì)步驟如下所示:

(1)首先對(duì)改進(jìn)的RRT算法隨機(jī)樹初始化變量;

(2)調(diào)用采樣點(diǎn)偏置策略,在工作地圖空間內(nèi)生成一個(gè)具有目標(biāo)導(dǎo)向性的隨機(jī)采樣點(diǎn);

(3)采用可變步長調(diào)整策略,在初始時(shí)以固定步長進(jìn)行擴(kuò)展生長,進(jìn)入障礙物影響范圍時(shí),改變步長長度;

(4)檢測新節(jié)點(diǎn)是否發(fā)生碰撞,如果發(fā)生碰撞,通過節(jié)點(diǎn)拒絕策略后返回步驟(2)繼續(xù)循環(huán),反之,生成新節(jié)點(diǎn)q_new;

(5)對(duì)q_new節(jié)點(diǎn)進(jìn)行判斷,是否在目標(biāo)點(diǎn)約束范圍內(nèi),若在范圍內(nèi)進(jìn)行步驟(6),不再返回步驟(2)繼續(xù)循環(huán),直到擴(kuò)展完成;

(6)調(diào)用貪心算法策略,通過刪除路徑上一些冗余節(jié)點(diǎn),然后再使用三次B樣條曲線對(duì)路徑轉(zhuǎn)折點(diǎn)進(jìn)行平滑處理,得到最終路徑。

3 仿真及實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證改進(jìn)RRT算法,仿真實(shí)驗(yàn)是通過在Matlab R2022b軟件進(jìn)行,將改進(jìn)的RRT算法與標(biāo)準(zhǔn)RRT算法、RRT-Connect算法在兩種不同環(huán)境地圖(少量障礙物、大量障礙物)中進(jìn)行驗(yàn)證比較,地圖大小為550 mm×550 mm,起點(diǎn)為(50 mm,50 mm),終點(diǎn)為(500 mm,480 mm),步長設(shè)定為20 mm,分別在兩種環(huán)境地圖下進(jìn)行100次仿真實(shí)驗(yàn),并對(duì)其取平均值,得到最后結(jié)果,驗(yàn)證改進(jìn)后的RRT算法的優(yōu)越性。由圖4、5可以看到三種算法在不同環(huán)境地圖下進(jìn)行仿真驗(yàn)證的結(jié)果,黑色圖形為障礙物,圖4、5中(a)、(b)中實(shí)線為生成的最終路徑,(c)中虛線為優(yōu)化前的路徑,實(shí)線是優(yōu)化后的最終路線。

(a) 標(biāo)準(zhǔn)RRT

在少量障礙物的地圖環(huán)境下,對(duì)RRT、RRT-Connect、改進(jìn)的RRT的算法進(jìn)行仿真驗(yàn)證,得到三種算法最后的路徑圖如圖4所示。

由圖4可以看出標(biāo)準(zhǔn)RRT算法與RRT-Connect算法生成的曲線比較曲折,改進(jìn)后的RRT算法路徑比較平滑。如表1所示是在簡單環(huán)境下三種算法各方面性能的比較,可以看出改進(jìn)后的RRT算法比標(biāo)準(zhǔn)RRT、RRT-Connect算法規(guī)劃時(shí)間分別提高了86.3%、65.2%,節(jié)點(diǎn)數(shù)減少了85.1%、53.8%,路徑長度減少了15.5%、23.2%。

表1 簡單環(huán)境下各算法性能對(duì)比

在復(fù)雜環(huán)境下,對(duì)上述三種算法進(jìn)行仿真驗(yàn)證,得到最后的路徑圖如圖5所示。

(a) 標(biāo)準(zhǔn)RRT

由圖5可以看出改進(jìn)后的RRT算法趨向于目標(biāo)點(diǎn)擴(kuò)展,提高了收斂速度。

如表2所示為三種算法在復(fù)雜情況下各算法性能的比較,從表2中可以看出,改進(jìn)后的RRT算法比標(biāo)準(zhǔn)RRT、RRT-Connect算法的規(guī)劃時(shí)間提高了86.9%、63.4%,節(jié)點(diǎn)數(shù)減少了80.1%、43.0%,路徑長度減少了14.5%、13.2%。

通過上述的實(shí)驗(yàn)結(jié)果對(duì)比可得,不論是在簡單地圖環(huán)境下還是在復(fù)雜地圖環(huán)境下,改進(jìn)的RRT算法比其他兩種算法更具有優(yōu)勢性,驗(yàn)證了改進(jìn)后RRT算法的可行性。

4 結(jié) 論

針對(duì)標(biāo)準(zhǔn)RRT算法的一些不足進(jìn)行改進(jìn),基于改進(jìn)的RRT算法在隨機(jī)采樣中加入目標(biāo)偏置策略、節(jié)點(diǎn)拒絕策略和可變步長策略,增強(qiáng)了算法的收斂速度和機(jī)械臂的避障效果,再對(duì)生成的路徑調(diào)用貪心策略,對(duì)一些亢余節(jié)點(diǎn)刪除處理,并采用三次B樣條曲線對(duì)生成路徑的轉(zhuǎn)折點(diǎn)進(jìn)行平滑性處理,優(yōu)化了路徑長度。最后在不同環(huán)境地圖中將改進(jìn)的RRT算法同標(biāo)準(zhǔn)RRT算法、RRT-Connect算法進(jìn)行仿真實(shí)驗(yàn)對(duì)比,無論從規(guī)劃時(shí)間、生成節(jié)點(diǎn)數(shù)、路徑長度都驗(yàn)證了改進(jìn)后的RRT路徑規(guī)劃算法的可行性與優(yōu)越性。