蔡文濤， 鄧 屹， 張 靜,2， 張永波， 饒 爽， 陽 康

(1.西南科技大學(xué) 信息工程學(xué)院，四川 綿陽 621010；2.中國科學(xué)技術(shù)大學(xué) 信息科學(xué)技術(shù)學(xué)院，安徽 合肥 230026)

0 引 言

多關(guān)節(jié)串聯(lián)機(jī)械臂由于運(yùn)動空間大、作業(yè)靈活，被廣泛應(yīng)用在工業(yè)制造、軍事醫(yī)療等領(lǐng)域。而機(jī)械臂規(guī)劃算法是實現(xiàn)機(jī)械臂高效、靈巧、柔順作業(yè)的技術(shù)關(guān)鍵，一直是國內(nèi)外相關(guān)專家和技術(shù)人員的研究焦點。

快速探索隨機(jī)樹(rapidly-exploring random tree，RRT)算法因能夠有效地搜索高維空間,解決機(jī)械臂非完整約束限制而被廣泛采用。但由于RRT算法導(dǎo)向性差，規(guī)劃空間增大時，存在所規(guī)劃的路徑平滑度差,且算法時間復(fù)雜度高的問題[1]。近年來，研究者們不斷地改進(jìn)RRT算法力求解決這些問題，Burget F等人提出雙向隨機(jī)搜索樹搜索算法提高搜索效率，提高了其收斂速率[2]；孫豐財?shù)热颂岢霾捎蒙呻S機(jī)點向量組的形式對隨機(jī)點選取策略進(jìn)行了優(yōu)化，改善RRT的不確定性，加快搜索速度[1～5]。

本文引入目標(biāo)概率偏置策略，結(jié)合步長控制方法改進(jìn)傳統(tǒng)RRT算法，并在MATLAB和機(jī)器人操作系統(tǒng)(robot operation system,ROS)平臺上搭建實驗，通過實驗驗證了算法的有效性。

1 碰撞檢測與機(jī)械臂建模

1.1 機(jī)械臂模型與D-H參數(shù)

本文研究對象為6自由度串聯(lián)機(jī)械臂，其D-H坐標(biāo)建模如圖1所示，相關(guān)D-H參數(shù)見表1。

圖1 6自由機(jī)械臂模型

θi/(°)αi-1/(°)ai-1/cmdi/cm關(guān)節(jié)角/(°)θ1000-360～360θ2907.90-105～90 θ308.10-100～70 θ4905.35.3-360～360θ5905.00-80～70θ6901.21.2-360～360

1.2 機(jī)械臂運(yùn)動學(xué)

根據(jù)D-H建模方法，串聯(lián)機(jī)械臂相鄰兩關(guān)節(jié)間的齊次變換矩陣

(1)

(2)

式中n,o,a3個列向量的前3行表示機(jī)械臂的姿態(tài)[4]；p向量的前3行代表末端執(zhí)行器在基坐標(biāo)系下的三維空間坐標(biāo)。

1.3 碰撞檢測法

本文采用幾何包絡(luò)法簡化機(jī)械臂與障礙物的碰撞檢測模型，如圖2所示。

圖2 碰撞檢測模型示意

機(jī)械臂連桿簡化為圓柱體模型，不規(guī)則障礙物簡化為球體模型，b0(x,y,z)為球心世界坐標(biāo)，球半徑為R0，機(jī)械臂連桿i的截面半徑為r0。簡化后碰撞檢測問題轉(zhuǎn)換為障礙物球心到連桿軸心到距離d與r0+R0的數(shù)量關(guān)系：當(dāng)d≥r0+R0時，連桿i與障礙物未發(fā)生碰撞，否則，當(dāng)d

2 基于RRT的改進(jìn)路徑規(guī)劃算法

2.1 RRT算法基本原理

RRT算法是從空間中1個起始節(jié)點Xs出發(fā)，通過隨機(jī)采樣，不斷增加新節(jié)點Xnw，生成1個隨機(jī)擴(kuò)展樹。當(dāng)增加的新節(jié)點中有目標(biāo)節(jié)點Xt時，Xs到Xt就會至少有1條路徑[6]。首先，以Xs作為樹根，隨機(jī)采樣得到Xr；然后，找到隨機(jī)樹中離Xr最近的節(jié)點Xnr，在Xr指向Xn方向上以一定步長生成Xnw，若Xnw沒碰到障礙物，則加入隨機(jī)樹，若碰到障礙物，則重新采樣；最后，若Xnw等于Xt，則算法結(jié)束，否則繼續(xù)生長。RRT樹生長過程如圖3所示，Xs到Xt間加粗線條為路徑，空白區(qū)域為Cfree空間[6,9]。

圖3 RRT樹生長的過程

由于RRT算法采樣的隨機(jī)性，新生成的節(jié)點通常遠(yuǎn)離目標(biāo)節(jié)點，隨著規(guī)劃空間變大，隨機(jī)樹盲目生長和低效擴(kuò)展的弊端會更明顯。

2.2 目標(biāo)概率偏置與步長優(yōu)化策略

基于隨機(jī)采樣的RRT算法缺乏導(dǎo)向性，借鑒啟發(fā)式算法的思想，在隨機(jī)樹的生長過程中設(shè)置一個偏置概率參數(shù)，可使隨機(jī)采樣點更導(dǎo)向于目標(biāo)點，提高RRT算法的導(dǎo)向性[3]。若不受障礙物干涉，節(jié)點樹將繼續(xù)向目標(biāo)點方向擴(kuò)展，應(yīng)用此方法可在很大程度上縮短RRT路徑規(guī)劃的時間。但當(dāng)障礙物較多時，RRT算法擴(kuò)展樹則無法繞過障礙物到達(dá)目標(biāo)點，此時需要使用歩長控制策略[5]，控制目標(biāo)點方向的步長，改善隨機(jī)樹的避障能力。

RRT的原始新節(jié)點位置的計算

(3)

式中p為RRT的生長最小單位，稱為步長。

步長控制RRT算法(S-RRT)新節(jié)點位置的計算

(4)

新節(jié)點位置公式通過添加目標(biāo)點Xt方向上的分量p2，使不同步長對新節(jié)點的位置會有影響。當(dāng)p2>p1時，新節(jié)點偏向于Xt，擴(kuò)展樹向目標(biāo)點方向擴(kuò)展。當(dāng)p2p1，使擴(kuò)展樹盡可能向目標(biāo)點方向擴(kuò)展。若遇到障礙取p2

2.3 改進(jìn)RRT路徑規(guī)劃算法描述

結(jié)合目標(biāo)概率偏置與步長優(yōu)化策略，使隨機(jī)樹生長時以P概率偏置目標(biāo)節(jié)點采樣，以1-P概率隨機(jī)采樣，并進(jìn)行碰撞檢測，若檢p2>p1測到碰撞則減小目標(biāo)節(jié)點方向的步長否則增加目標(biāo)節(jié)點方向步長。改進(jìn)RRT(I-RRT)基本算法流程如下：

1)初始化Xs，Xt，p2，p2，n；

2)若節(jié)點樹上新生節(jié)點Xnw等于Xt，則結(jié)束生長，否則進(jìn)行下一步；

3)以概率P使Xr=Xt，1-P概率隨機(jī)采樣生成隨機(jī)節(jié)點Xr；

4)進(jìn)行碰撞檢測，若連線(Xnr,Xr)與空間障礙物碰撞則回到步驟(2)，且變量n的值累加，p2=p2+n，p1=p1-n，必須限定n值大小，若無碰撞則下一步；

6)從目標(biāo)節(jié)點到起始節(jié)點搜索一條無碰撞路徑。

3 實驗仿真

3.1 MATLAB實驗搭建與結(jié)果分析

本文構(gòu)建基于MATLAB平臺的實驗，比較本文I-RRT算法、目標(biāo)偏置P-RRT、步長控制S-RRT與RRT的路徑規(guī)劃過程，驗證本文改進(jìn)方法的有效性。仿真環(huán)境硬件信息：Intel Core i5—6400 CPU，主頻2.7 GHz，內(nèi)存4 G。實驗各項參數(shù)單位均為米(m)，初始設(shè)定如：偏置概率P=0.2，初始步長p1=p2=20，變量n=10，起始點為(0,0)，目標(biāo)點為(850,300)。

實驗結(jié)果圖范圍為900×900，圓形區(qū)域為障礙物，方點為起始點，圓點為目標(biāo)點，窄線條為擴(kuò)展節(jié)點樹，寬線條為路徑。對比圖4(a)，(c)，(d)可得：I-RRT與P-RRT比RRT擴(kuò)展節(jié)點數(shù)少，導(dǎo)向性好，但P-RRT的避障能力不強(qiáng)；比較圖4(a)，(b)，(d)可知：I-RRT與S-RRT比RRT的節(jié)點樹簡單，路徑更平滑。由實驗結(jié)果得：I-RRT算法導(dǎo)向性好，隨機(jī)樹更簡單，路徑平滑度也更好。

圖4 改進(jìn)的RRT與RRT比較

表2為以上仿真實驗的1 000次實驗統(tǒng)計值結(jié)果記錄。由表2知，I-RRT算法，較P-RRT,S-RRT,RRT等算法平均耗時大大降低，其中I-RRT算法較RRT算法時間復(fù)雜度優(yōu)化至30 %；同時I-RRT算法導(dǎo)向性增強(qiáng)，在迭代次數(shù)與擴(kuò)展節(jié)點數(shù)上較前3種算法也都有明顯減少，路徑規(guī)劃成功率明顯提高。

表2 仿真實驗環(huán)境下的數(shù)據(jù)比較

3.2 ROS機(jī)械臂避障規(guī)劃實驗

本文采用I-RRT末端所規(guī)劃的路徑反解到關(guān)節(jié)空間下的方法[7]實現(xiàn)機(jī)械臂控制，基于公式(2)，機(jī)械臂逆運(yùn)動學(xué)求解使用KDL(The Kinematics and Dynamics Library)求解器完成[8,9]。其中，機(jī)械臂避障實驗任務(wù)描述如圖5所示。仿真場景包含1個機(jī)械臂，1個工作臺，1個球形包絡(luò)的障礙物。實驗要求機(jī)械臂從初始位姿完成任務(wù)1，并從任務(wù)1狀態(tài)避開障礙完成任務(wù)2。

圖5 機(jī)械臂避障任務(wù)描述

如圖6所示，機(jī)械臂完成任務(wù)1，并順利地越過避障完成任務(wù)2。

圖6 改進(jìn)RRT算法避障規(guī)劃仿真過程

由實驗可知，利用本文的改進(jìn)算法實現(xiàn)了機(jī)械臂避障規(guī)劃，保證機(jī)械臂順利地避開障礙物并完成任務(wù)位姿，證明了算法的有效性。另外，本文使用I-RRT算法進(jìn)行連續(xù)500次運(yùn)行，機(jī)械臂490次成功完成避障任務(wù)，結(jié)果表明了I-RRT算法有效且可靠。

4 結(jié) 論

仿真實驗表明：改進(jìn)后的算法(I-RRT)，導(dǎo)向性明顯增強(qiáng)，較RRT時間復(fù)雜度降低至30 %，且低于S-RRT和P-RRT等算法的時間復(fù)雜度，該算法的路徑規(guī)劃成功率也有明顯提升，達(dá)99.65 %。進(jìn)一步提升該算法在動態(tài)障礙物環(huán)境中的避障能力，將是后續(xù)研究工作的重點。