復(fù)雜環(huán)境下六自由度機(jī)械臂路徑規(guī)劃的Biased-RRT修正算法

陳志勇，黃澤麟，曾德財，于瀟雁

(福州大學(xué)機(jī)械工程及自動化學(xué)院，福建 福州 350108)

0 引言

機(jī)械臂路徑規(guī)劃是指機(jī)械臂在滿足系統(tǒng)運(yùn)動學(xué)約束的前提下，在所有關(guān)節(jié)運(yùn)動范圍內(nèi)規(guī)劃出一條機(jī)械臂自身不發(fā)生碰撞且不與外界障礙物碰撞的運(yùn)動路徑.目前機(jī)械臂的避障路徑規(guī)劃算法主要有： A*搜索算法[1-3]、 人工勢場法[4-5]、 快速擴(kuò)展隨機(jī)樹(rapidly exploring random tree, RRT)算法、 智能算法[6-8]等.

A*搜索算法采用啟發(fā)函數(shù)來確定路徑搜索方向，如賈慶軒等[9]把障礙物模型映射到機(jī)械臂關(guān)節(jié)空間，運(yùn)用A*算法完成路徑規(guī)劃，不過該算法在面對多自由度機(jī)械臂避障路徑問題時，計算效率較低，難以滿足實(shí)時性要求.人工勢場法[10]引入勢場思想，建立目標(biāo)引力和障礙物斥力，促使探索的路徑遠(yuǎn)離障礙物并接近目標(biāo)，從而完成路徑規(guī)劃，不過該算法對于障礙物較多的高維空間工況，其計算復(fù)雜度驟增且容易陷入局部極小值.馬培羽等[11]將人工勢場法運(yùn)用到機(jī)械臂上，人為添加虛擬勢場，推動其算法向目標(biāo)位置靠近，從而離開局部最優(yōu)值，但對于高維空間工況，依舊存在計算復(fù)雜的問題，難以處理多障礙物環(huán)境.此外，包括蟻群算法、 遺傳算法、 粒子群算法等在內(nèi)的各類智能算法雖然各有優(yōu)點(diǎn)，但均面臨著算法計算效率不高、 實(shí)時性差等突出問題.RRT算法[12]采用隨機(jī)采樣的方式擴(kuò)展樹狀結(jié)構(gòu)來探索空間，只對擴(kuò)展的節(jié)點(diǎn)進(jìn)行碰撞檢測，避免障礙物的空間描述，因此對于高維空間也有良好的效果，被越來越多地應(yīng)用于機(jī)械臂路徑規(guī)劃中[13-16].但隨機(jī)采樣的擴(kuò)展方式更傾向于探索整個空間，計算效率不穩(wěn)定.近年來，許多學(xué)者嘗試從不同方面對RRT算法進(jìn)行改進(jìn).如文獻(xiàn)[17]引入目標(biāo)偏置思想，提出Biased-RRT算法，在隨機(jī)采樣的基礎(chǔ)上有一定小概率采樣到目標(biāo)區(qū)域，引導(dǎo)路徑搜索向目標(biāo)區(qū)域進(jìn)行，從而提高算法效率，但算法并未對路徑進(jìn)行優(yōu)化，所得路徑并非最優(yōu).文獻(xiàn)[18]提出結(jié)合路徑長度和路徑安全性建立路徑代價函數(shù)來約束隨機(jī)采樣點(diǎn)，并用貪婪策略來彌補(bǔ)RRT算法局部擴(kuò)展緩慢的缺點(diǎn)，但隨著節(jié)點(diǎn)擴(kuò)展，路徑代價函數(shù)的計算量明顯增多，算法規(guī)劃時間無法保證.Karaman等[19]提出了RRT*算法，在RRT算法中添加重新布線和代價函數(shù)，通過不斷地更新路徑，可以找到一條代價較小或者最小的路徑，但需要耗費(fèi)的計算資源大幅度提升.

為此，本研究提出一種六自由度機(jī)械臂避障路徑規(guī)劃的Biased-RRT修正算法.該算法通過對傳統(tǒng)目標(biāo)偏置策略中的隨機(jī)點(diǎn)采樣規(guī)則的目標(biāo)導(dǎo)向設(shè)計，來確保搜索樹盡可能快地向目標(biāo)點(diǎn)擴(kuò)展，且避免了算法陷入局部極小值問題； 借鑒RRT*算法中的父節(jié)點(diǎn)重選操作及多余路徑節(jié)點(diǎn)的剔除操作，來獲得路徑代價最小的路徑點(diǎn)及其簡化路徑.再者，采用3次樣條插值技術(shù)為機(jī)械臂各關(guān)節(jié)規(guī)劃出一條光滑、 連續(xù)且無障的運(yùn)動路徑曲線.系統(tǒng)數(shù)值仿真及3D模擬仿真結(jié)果，均證實(shí)所提Biased-RRT修正算法的有效性.

1 六自由度機(jī)械臂路徑規(guī)劃算法設(shè)計

1.1 問題描述

要實(shí)現(xiàn)復(fù)雜環(huán)境下六自由度機(jī)械臂的避障路徑規(guī)劃，實(shí)際上是要在滿足六自由度機(jī)械臂運(yùn)動學(xué)約束的前提下，在其運(yùn)動起點(diǎn)到運(yùn)動終點(diǎn)之間規(guī)劃出一條復(fù)雜環(huán)境下無碰撞、 合理的可行路徑，并在此基礎(chǔ)上盡可能做到加快規(guī)劃時間、 優(yōu)化路徑質(zhì)量，讓所提路徑規(guī)劃算法能夠更好地適用于六自由度機(jī)械臂的實(shí)時控制.為此，本節(jié)提出一種復(fù)雜環(huán)境下六自由度機(jī)械臂路徑規(guī)劃的Biased-RRT修正算法，并分別從機(jī)械臂逆運(yùn)動學(xué)分析，Biased-RRT修正算法的采樣規(guī)則設(shè)計、 新節(jié)點(diǎn)擴(kuò)展、 路徑優(yōu)化的父節(jié)點(diǎn)重選, 以及算法整體流程等方面進(jìn)行詳細(xì)闡述.

1.2 六自由度機(jī)械臂的逆運(yùn)動學(xué)分析

不失一般性，研究如圖1所示的六自由度機(jī)械臂，該機(jī)械臂各關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié).由于本研究討論的Biased-RRT修正算法擬在關(guān)節(jié)空間下對系統(tǒng)進(jìn)行路徑規(guī)劃，故在獲取機(jī)械臂末端位置信息后，需將笛卡爾空間下的機(jī)械臂末端位置轉(zhuǎn)換成機(jī)械臂關(guān)節(jié)空間下的6個關(guān)節(jié)轉(zhuǎn)角位置.

(1)

1.3 采樣規(guī)則的目標(biāo)導(dǎo)向設(shè)計

傳統(tǒng)Biased-RRT算法使隨機(jī)采樣有小概率采樣到目標(biāo)點(diǎn)qgoal，但為了讓搜索樹盡可能向目標(biāo)點(diǎn)qgoal擴(kuò)展、 提高搜索效率，本修正算法擬對采樣規(guī)則進(jìn)行目標(biāo)導(dǎo)向設(shè)計.該設(shè)計涵蓋3個部分： 1) 若算法采樣時滿足目標(biāo)偏置條件，即目標(biāo)概率Pgoal小于隨機(jī)數(shù)rand時，則將目標(biāo)點(diǎn)qgoal直接作為采樣點(diǎn)； 2) 若算法采樣時不滿足目標(biāo)偏置條件，即目標(biāo)概率Pgoal大于隨機(jī)數(shù)rand時，則在整個工作空間隨機(jī)采樣10次，依據(jù)曼哈頓距離，從中挑選出距離目標(biāo)點(diǎn)qgoal最近的采樣點(diǎn)qmin； 3) 若搜索樹擴(kuò)展中出現(xiàn)被障礙物阻擋，導(dǎo)致無法繼續(xù)擴(kuò)展的情況，則算法在搜索樹擴(kuò)展被障礙物阻擋時，在工作空間中進(jìn)行單次隨機(jī)采樣，增加擴(kuò)展方向多樣性，繼而讓搜索樹可以向其他方向擴(kuò)展，以便順利離開障礙物區(qū)域.本設(shè)計利用目標(biāo)位置為搜索樹擴(kuò)展增加引導(dǎo)信息，減少搜索樹對無效區(qū)域的擴(kuò)展，將擴(kuò)展范圍縮小至目標(biāo)點(diǎn)附近，從而提高了算法的搜索效率.

于是，基于目標(biāo)導(dǎo)向設(shè)計后的采樣規(guī)則為

(2)

其中：Pgoal為預(yù)設(shè)的目標(biāo)偏置概率； rand為在[0, 1]內(nèi)生成的隨機(jī)數(shù)；qmin表示距離目標(biāo)點(diǎn)qgoal最近的隨機(jī)采樣點(diǎn)；qrand為選取的隨機(jī)采樣點(diǎn)； collision表示搜索樹擴(kuò)展時遇到障礙物的情況.

1.4 基于變步長的新節(jié)點(diǎn)擴(kuò)展方式

鑒于隨機(jī)采樣是在系統(tǒng)關(guān)節(jié)空間下進(jìn)行的，為便于后續(xù)規(guī)劃算法的設(shè)計及路徑代價度量，將機(jī)械臂從節(jié)點(diǎn)qA運(yùn)動至節(jié)點(diǎn)qB的曼哈頓距離

(3)

定義為節(jié)點(diǎn)qA與qB之間的運(yùn)動路徑代價.

圖2為Biased-RRT修正算法所采用的擴(kuò)展步長選取策略示意圖.一般來說，傳統(tǒng)RRT算法采樣時常以固定步長來擴(kuò)展搜索樹的新節(jié)點(diǎn)qnew，大步長雖可加快搜索速度，但若步長選取過大，有可能會導(dǎo)致搜索樹臨近目標(biāo)點(diǎn)時，新節(jié)點(diǎn)qnew在目標(biāo)點(diǎn)qgoal附近

來回振蕩，容易陷入局部死循環(huán)，無法準(zhǔn)確擴(kuò)展到目標(biāo)點(diǎn)qgoal； 但若步長選取過小，則搜索樹擴(kuò)展緩慢、 搜索效率低下.為此，Biased-RRT修正算法在搜索樹擴(kuò)展初期，選取較大的步長st1，加快搜索樹的探索； 為了解決較大步長的缺陷，以R0為半徑，目標(biāo)點(diǎn)qgoal為中心生成目標(biāo)區(qū)域，在新節(jié)點(diǎn)qnew生成之前，需要在搜索樹上搜尋距離隨機(jī)采樣點(diǎn)qrand路徑代價最小的節(jié)點(diǎn)qnearest，之后判斷節(jié)點(diǎn)qnearest與目標(biāo)點(diǎn)qgoal的曼哈頓距離Len(qnearest,qgoal)是否小于閾值R0？若qnearest在目標(biāo)區(qū)域內(nèi)，則將步長調(diào)整為較小的步長st2.

擴(kuò)展步長選取策略為

(4)

其中： st表示算法新節(jié)點(diǎn)擴(kuò)展所選取的步長，st1為大步長，st2為小步長；R0表示目標(biāo)區(qū)域半徑.

1.5 基于路徑代價最小的父節(jié)點(diǎn)重選

傳統(tǒng)Biased-RRT算法在整個空間中搜索較為盲目，所得到的路徑質(zhì)量往往較差.RRT*算法雖可通過在路徑節(jié)點(diǎn)的擴(kuò)展中依據(jù)路徑代價來重選父節(jié)點(diǎn)和重新規(guī)劃路徑，使算法具有漸進(jìn)最優(yōu)，但由于需要重新規(guī)劃路徑，該算法的實(shí)時性受到較大影響.為此，本研究僅將RRT*算法中的重選父節(jié)點(diǎn)操作融入Biased-RRT修正算法中，以實(shí)現(xiàn)對路徑進(jìn)行優(yōu)化.具體做法如下：

在搜索樹生成新節(jié)點(diǎn)qnew后，將以qnew為圓心，以r為半徑，r隨著搜索樹T擴(kuò)展選取不同的數(shù)值，具體設(shè)置為r=k_rrg_×log(cardDbl)；k_rrg_=e+e/dimDbl，cardDbl=n0+1；n0表示當(dāng)前搜索樹T擴(kuò)展的總節(jié)點(diǎn)數(shù)，e是自然對數(shù)的底，dimDbl是關(guān)節(jié)空間維度.然后尋找當(dāng)前搜索樹上距離qnew小于r的所有節(jié)點(diǎn)，記qnears[i0]為滿足條件的第i0個節(jié)點(diǎn)，并計算從起始點(diǎn)qinit途經(jīng)qnears[i0]并運(yùn)動到qnew的總路徑代價，選取總代價最小且與qnew之間的運(yùn)動不發(fā)生碰撞的節(jié)點(diǎn)替換掉qnew原本的父節(jié)點(diǎn)qnearest，刪除原本的qnew和qnearest的路徑.在選取出路徑代價最小的父節(jié)點(diǎn)以后，搜索樹進(jìn)入下一輪采樣，繼續(xù)擴(kuò)展.重選父節(jié)點(diǎn)的簡單演示過程如圖3和圖4所示，其中帶有圓圈的數(shù)字表示搜索樹擴(kuò)展的各節(jié)點(diǎn)，相鄰兩節(jié)點(diǎn)連線上的數(shù)值則表示為兩點(diǎn)之間的運(yùn)動路徑代價值.

于是，從起始點(diǎn)qinit途經(jīng)qnears[i0]并運(yùn)動到qnew的路徑代價可表示為

qnew[i0].cost=qnears[i0].cost+Len(qnears[i0]，qnew)

(5)

1.6 路徑簡化

為簡化路徑點(diǎn)數(shù)量、 縮短路徑長度，本節(jié)擬對Biased-RRT修正算法規(guī)劃出來的路徑進(jìn)行多余節(jié)點(diǎn)、 拐點(diǎn)的剔除處理.圖5為路徑簡化的簡單示意圖，其中黑色方塊表示障礙物，虛線表示初期規(guī)劃出的路徑，黑色表示簡化后的路徑(即原路徑qinit→q1→q2→q3→q4→qgoal被簡化為qinit→q1→q4→qgoal).將Biased-RRT修正算法規(guī)劃出來的所有離散節(jié)點(diǎn)的集合記為點(diǎn)集{N}, 從起始點(diǎn)qinit開始，依次遍歷點(diǎn)集{N}中的節(jié)點(diǎn)qL.若點(diǎn)集{N}有節(jié)點(diǎn)qL與起始點(diǎn)qinit之間的連線，沒有碰撞發(fā)生且可以直達(dá)，則直接連接該節(jié)點(diǎn)qL和起始點(diǎn)qinit，刪除qinit和qL間的路徑點(diǎn)，同時將qinit和qL保存至新的點(diǎn)集{P0}中； 若發(fā)生碰撞，起始點(diǎn)qinit則與該節(jié)點(diǎn)的父節(jié)點(diǎn)qL-1連接，將qinit和qL-1保存至點(diǎn)集{P0}中，然后從qL-1開始繼續(xù)往后遍歷節(jié)點(diǎn).重復(fù)以上過程，直至遍歷到目標(biāo)點(diǎn)qgoal，最終所得點(diǎn)集{P0}即為簡化后的新路徑點(diǎn)的集合.

1.7 路徑規(guī)劃的實(shí)現(xiàn)步驟

路徑規(guī)劃的實(shí)現(xiàn)具體可分為以下12個步驟.

步驟1設(shè)qinit為初始起點(diǎn)，qgoal為目標(biāo)終點(diǎn)； 以終點(diǎn)位置qgoal為圓心、R0為半徑，劃定目標(biāo)區(qū)域C，節(jié)點(diǎn)初始擴(kuò)展大步長為st1.

步驟2將qinit作為搜索樹T的樹根，當(dāng)搜索樹T擴(kuò)展到qgoal時，結(jié)束搜索樹T擴(kuò)展.

步驟3當(dāng)隨機(jī)數(shù)rand小于預(yù)先設(shè)定的目標(biāo)偏置概率Pgoal時，進(jìn)入步驟4； 反之，算法跳到步驟5.

步驟4將隨機(jī)采樣點(diǎn)qrand直接設(shè)定為終點(diǎn)qgoal，進(jìn)入步驟6.

步驟5在工作空間中隨機(jī)采樣10次，選取與終點(diǎn)qgoal距離最小的點(diǎn)作為qrand.

步驟6在搜索樹T上搜尋一個距離qrand最小的節(jié)點(diǎn)qnearest.

步驟7計算qnearest與qgoal之間的距離Len(qnearest,qgoal)，若Len(qnearest,qgoal)≤R0，則qnearest進(jìn)入目標(biāo)區(qū)域C范圍，步長選用小步長st2; 若Len(qnearest,qgoal)>R0，則仍選用初始的大步長st1.

步驟8若Len(qnearest,qrand)≥st，依據(jù)步長在qnearest和qrand之間的連線上擴(kuò)展新的節(jié)點(diǎn)qnew； 若Len(qnearest,qrand)

步驟9在qnew附近半徑r的范圍內(nèi)，尋找qnew的所有鄰近節(jié)點(diǎn)qnears[i0]，作為用來替換qnew的父節(jié)點(diǎn)qnearest的備選； 判斷qnew和鄰近節(jié)點(diǎn)qnears[i0]之間是否有障礙物？ 若有，則刪除這個鄰近節(jié)點(diǎn)； 若沒有，計算從起始點(diǎn)qinit途經(jīng)qnears[i0]并運(yùn)動到qnew的路徑代價，選取代價最小的鄰近節(jié)點(diǎn)作為qnew新的父節(jié)點(diǎn).

步驟10循環(huán)以上步驟，直至搜索樹T擴(kuò)展至終點(diǎn)qgoal.

步驟11從qgoal開始，依據(jù)節(jié)點(diǎn)擴(kuò)展順序，找到從qinit到qgoal的所有路徑點(diǎn)，得出規(guī)劃的路徑.

步驟12從起點(diǎn)qinit依次遍歷路徑點(diǎn)，刪除多余節(jié)點(diǎn)，簡化路徑.

2 關(guān)節(jié)路徑曲線的生成

Biased-RRT修正算法是在機(jī)械臂的關(guān)節(jié)空間中規(guī)劃，因此所規(guī)劃出的路徑節(jié)點(diǎn)結(jié)構(gòu)為q=(θ1,θ2, …,θ6)T.利用3次樣條插值技術(shù)對路徑點(diǎn)進(jìn)行插值，以便為機(jī)械臂各關(guān)節(jié)分別生成一條光滑、 連續(xù)的路徑曲線.下面將對機(jī)械臂第i(i=1, 2, …, 6)個關(guān)節(jié)路徑曲線生成進(jìn)行分析.

設(shè)Biased-RRT修正算法規(guī)劃出的路徑點(diǎn)有n+1個，則各關(guān)節(jié)在關(guān)節(jié)空間下各有n+1個路徑節(jié)點(diǎn)； 若定義各關(guān)節(jié)運(yùn)行到各自已規(guī)劃的關(guān)節(jié)路徑點(diǎn)所需時間為tk(k=0, 1, …,n)，則第i(i=1, 2, …, 6)個關(guān)節(jié)的所有路徑點(diǎn)可表示為： (t0,θi0), (t1,θi1), …, (tn-1,θi(n-1)), (tn,θin), 其中:θik(i=1, 2, …, 6;k=0, 1, …,n)表示機(jī)械臂第i個關(guān)節(jié)在時間tk下的實(shí)際位置.

對機(jī)械臂第i(i=1, 2, …, 6)個關(guān)節(jié)兩相鄰路徑點(diǎn)(tj,θij)和(tj+1,θi(j+1))進(jìn)行如下3次樣條插值，即

Sij(t)=aij+bij(t-tj)+cij(t-tj)2+dij(t-tj)3

(6)

(7)

(8)

3 仿真實(shí)驗與結(jié)果分析

3.1 仿真實(shí)驗概述

本研究首先對傳統(tǒng)RRT算法、 傳統(tǒng)Biased-RRT算法及Biased-RRT修正算法進(jìn)行二維仿真規(guī)劃實(shí)驗，通過不同算法在平均擴(kuò)展節(jié)點(diǎn)數(shù)目、 路徑質(zhì)量、 規(guī)劃時間上的結(jié)果對比，來證實(shí)Biased-RRT修正算法的有效性.其次，在ROS平臺上將Biased-RRT修正算法應(yīng)用于復(fù)雜環(huán)境下六自由度機(jī)械臂的路徑規(guī)劃實(shí)驗，以進(jìn)一步檢驗Biased-RRT修正算法的可行性.

3.2 改進(jìn)算法的有效性驗證實(shí)驗

不失一般性，將系統(tǒng)位置節(jié)點(diǎn)q*簡化為二維向量，先行實(shí)施改進(jìn)算法在有復(fù)雜障礙物的二維環(huán)境下的路徑規(guī)劃實(shí)驗.仿真環(huán)境所采用的地圖大小為800 px × 800 px, 且設(shè)置有大小不等、 形狀不同的障礙物，仿真要求算法在起始點(diǎn)qinit=(10, 10)T到目標(biāo)點(diǎn)qgoal=(720, 750)T之間快速規(guī)劃出一條無障、 便捷的運(yùn)動路徑.仿真先后采用傳統(tǒng)RRT算法、 Biased-RRT算法及Biased-RRT修正算法對系統(tǒng)路徑分別進(jìn)行規(guī)劃.仿真時，傳統(tǒng)RRT算法和Biased-RRT算法均采用定步長40，Biased-RRT修正算法采用變步長方式，即初始大步長st1設(shè)置為50，目標(biāo)區(qū)域半徑R0為20，進(jìn)入目標(biāo)區(qū)域后的小步長st2設(shè)置為10； 傳統(tǒng)Biased-RRT算法和Biased-RRT修正算法的目標(biāo)偏置概率均設(shè)置為0.1.圖6～8分別描述了傳統(tǒng)RRT算法、 Biased-RRT算法及Biased-RRT修正算法所生成的搜索樹及其規(guī)劃出來的路徑.在各圖中，紅色點(diǎn)表示起始點(diǎn)，綠色點(diǎn)表示目標(biāo)點(diǎn)，空心藍(lán)點(diǎn)為搜索樹擴(kuò)展的節(jié)點(diǎn)； 藍(lán)色實(shí)線表示算法初步規(guī)劃出的路徑，圖8中紅色實(shí)線表示經(jīng)過Biased-RRT修正算法簡化后的路徑.

表1 平均擴(kuò)展節(jié)點(diǎn)數(shù)對比

對于路徑規(guī)劃問題，需要考慮算法對計算資源的消耗，以便算法可以應(yīng)用于更多的環(huán)境中.傳統(tǒng)RRT算法、 Biased-RRT算法及Biased-RRT修正算法都是以樹結(jié)構(gòu)探索空間，采用節(jié)點(diǎn)的方式記錄路徑點(diǎn)位，依據(jù)擴(kuò)展順序來生成路徑.3種算法在相同環(huán)境下算法運(yùn)行30次后的平均擴(kuò)展節(jié)點(diǎn)數(shù)如表1所示.表1中Biased-RRT修正算法采用路徑簡化前的路徑節(jié)點(diǎn)數(shù)目來對比.算法搜索樹中記錄的節(jié)點(diǎn)越多，需要的計算資源越多，路徑所使用的節(jié)點(diǎn)數(shù)目越少，意味著算法計算資源有效使用率越低，因此可用多余的節(jié)點(diǎn)占比來衡量算法的額外開銷.

從表1可知，在同等環(huán)境下，傳統(tǒng)RRT算法是對整個空間隨機(jī)探索，即使是對RRT算法改進(jìn)后的Biased-RRT也是小概率地向目標(biāo)區(qū)域擴(kuò)展，從而導(dǎo)致算法產(chǎn)生許多額外開銷.相比之下，所提Biased-RRT修正算法搜索樹節(jié)點(diǎn)數(shù)目較少，節(jié)點(diǎn)利用率較高，可有效減少算法的額外開銷.這是因為Biased-RRT修正算法并不在整個空間隨機(jī)采樣，而是利用目標(biāo)位置為指引，通過多次采樣來選取出靠近目標(biāo)點(diǎn)的采樣點(diǎn)，減少算法對不必要空間的探索，讓搜索樹生成趨向性明顯，避免對無效方向的擴(kuò)展生成.其次，相比固定步長擴(kuò)展而言，變步長的擴(kuò)展方式讓搜索樹可以用更少的節(jié)點(diǎn)和擴(kuò)展次數(shù)來快速通過前期區(qū)域，從而減少了搜索樹節(jié)點(diǎn)數(shù).

表2 路徑質(zhì)量對比

傳統(tǒng)RRT算法和Biased-RRT算法都不具備優(yōu)化路徑的能力，算法規(guī)劃出的路徑冗余節(jié)點(diǎn)多，路徑質(zhì)量往往不是最優(yōu)的，會增加機(jī)械臂的運(yùn)行時間，因此需要對路徑質(zhì)量優(yōu)化.一般以平均路徑節(jié)點(diǎn)數(shù)和平均路徑代價來考量算法規(guī)劃出的路徑質(zhì)量.3種算法在同等環(huán)境下所規(guī)劃的路徑質(zhì)量如表2所示，表2中Biased-RRT修正算法以路徑簡化后的節(jié)點(diǎn)數(shù)進(jìn)行對比.

由表2可知，Biased-RRT修正算法對路徑有一定的優(yōu)化能力，Biased-RRT修正算法平均節(jié)點(diǎn)數(shù)較之傳統(tǒng)RRT和Biased-RRT算法減少約72.5%，路徑縮短約10.65%.傳統(tǒng)RRT算法和Biased-RRT算法在搜索樹擴(kuò)展時，會向無效區(qū)域擴(kuò)展，所得的路徑并非是最優(yōu)路徑.為了盡可能優(yōu)化路徑質(zhì)量，Biased-RRT修正算法引入RRT*的重選父節(jié)點(diǎn)操作，在每次擴(kuò)展節(jié)點(diǎn)的同時，不依賴擴(kuò)展的先后順序確定父節(jié)點(diǎn)，而是揀選出路徑代價最小的鄰近節(jié)點(diǎn)作為父節(jié)點(diǎn)，修正節(jié)點(diǎn)擴(kuò)展路徑.最后對規(guī)劃出的路徑簡化處理，剔除冗余節(jié)點(diǎn)和拐點(diǎn)，減少不必要的曲折路徑，從而減少了路徑節(jié)點(diǎn)和代價.

在機(jī)械臂的實(shí)際應(yīng)用中，對路徑規(guī)劃時間有著較高的要求，算法規(guī)劃時間越短，機(jī)械臂運(yùn)行效率越高，更能滿足機(jī)械臂應(yīng)用要求.

表3 平均規(guī)劃時間對比

從表3可得，Biased-RRT修正算法相比RRT算法提升近74.71%，比之Biased-RRT算法，提高約44.67%，表明Biased-RRT修正算法搜索樹擴(kuò)展目的性明顯，算法的實(shí)時性更高.這是因為Biased-RRT修正算法里基于采樣規(guī)則的目標(biāo)導(dǎo)向設(shè)計充分利用了目標(biāo)點(diǎn)作為引導(dǎo)信息，可確保搜索樹擴(kuò)展方向不會過多偏離目標(biāo)點(diǎn)，在一定程度避免了搜索樹無效擴(kuò)展，從而加快算法搜索效率.

3.3 基于ROS實(shí)驗平臺的機(jī)器人仿真實(shí)驗驗證

為驗證所提Biased-RRT修正算法在六自由度機(jī)械臂路徑規(guī)劃上可行性，在ROS平臺上，選用MOTOMAN-GP7型六自由度機(jī)械臂作為仿真機(jī)械臂，機(jī)械臂的D-H參數(shù)如表4所示.

表4 MOTOMAN-GP7的D-H參數(shù)

基于ROS平臺對該機(jī)械臂進(jìn)行三維建模，并生成如圖9(a)所示的存在有復(fù)雜障礙物的仿真環(huán)境.圖中，仿真空間尺寸設(shè)置為2 m × 2 m × 2 m； 障礙物選取為圓柱體，圖中數(shù)字表示障礙物的序號； 以機(jī)械臂基座中心構(gòu)建坐標(biāo)系，障礙物1的位置為(0 m, -0.4 m, 0.3 m)、 高度0.6 m、 半徑0.2 m； 障礙物2的位置為(0.5 m, 0.5 m, 0.2 m)、 高度0.4 m、 半徑0.2 m； 障礙物3的位置為(0.8 m, 0 m, 0.35 m)、 高度0.3 m、 半徑0.1 m； 障礙物4的位置為(0.8 m, 0 m, 0.1 m)、 高度0.2 m、 半徑0.2 m； 障礙物5的位置為(0.8 m, -0.5 m, 0.1 m)、 高度0.2 m，半徑0.2 m； 機(jī)械臂末端起始位姿為P1=(0.55 m, 0 m, 0.81 m)T、Q1=(π/2(rad)，-π/2(rad)，π/2(rad))T； 所提改進(jìn)Biased-RRT修正算法規(guī)劃機(jī)械臂從末端起始位姿運(yùn)動到第1個指定位姿P2=(0.8 m, -0.3 m, 0.35 m)T、Q2=(π(rad), 0(rad), π(rad))T，緊接著再運(yùn)動到第2個指定位姿P3=(0.8 m, 0.3 m, 0.4 m)T、Q3=(π(rad), 0(rad), π(rad))T，最后返回到起始位姿的具體路徑； 仿真中，選用ROS平臺中的KDL逆運(yùn)動學(xué)求解器將機(jī)械臂末端位姿轉(zhuǎn)化為機(jī)械臂各關(guān)節(jié)位置，并利用3次樣條插值技術(shù)生成機(jī)械臂各關(guān)節(jié)路徑曲線，進(jìn)而引導(dǎo)機(jī)械臂仿真模型進(jìn)行避障運(yùn)動.仿真實(shí)驗中算法的目標(biāo)偏置概率設(shè)為0.05，搜索樹擴(kuò)展的大步長st1設(shè)置為7.614，小步長st2設(shè)置為3.807，目標(biāo)區(qū)域半徑R0設(shè)置為7.614.

圖9為機(jī)械臂整個仿真運(yùn)動過程示意圖，圖中的橙色機(jī)械臂表示起始狀態(tài)，藍(lán)色機(jī)械臂表示運(yùn)動狀態(tài)； 圖9(b)表示以機(jī)械臂基座為參考坐標(biāo)系，機(jī)械臂末端運(yùn)動至第一個位置運(yùn)動的過程； 圖9(c)表示機(jī)械臂運(yùn)動至第二個指定位置； 圖9(d)表示機(jī)械臂返回起始位置的過程.

從上述仿真結(jié)果可以觀察到： 所提Biased-RRT修正算法可使六自由度機(jī)械臂在復(fù)雜障礙物環(huán)境中有效地避開障礙物，依次到達(dá)指定位置，并完成預(yù)期運(yùn)動.圖10和圖11分別給出機(jī)械臂從起始位置平穩(wěn)運(yùn)動到第一個指定位置時，機(jī)械臂各關(guān)節(jié)角的運(yùn)動及其速度變化曲線.

4 結(jié)語

設(shè)計一種六自由度機(jī)械臂路徑規(guī)劃的Biased-RRT修正算法.相比于傳統(tǒng)RRT、 Biased-RRT算法，所提Biased-RRT修正算法利用目標(biāo)偏置、 多次隨機(jī)采樣取“最近”和單次隨機(jī)采樣相結(jié)合的多重采樣方式來生成節(jié)點(diǎn)，以減少搜索時長、 提高搜索效率； 其間，所采用的變步長節(jié)點(diǎn)擴(kuò)展方式將有助于修正算法生成的搜索樹快速、 穩(wěn)定地搜索到目標(biāo)節(jié)點(diǎn).此外，還通過引入基于路徑代價最小的父節(jié)點(diǎn)重選、 多余路徑節(jié)點(diǎn)剔除操作，來優(yōu)化規(guī)劃出來的路徑，并結(jié)合3次樣條插值技術(shù)來生成機(jī)械臂各關(guān)節(jié)光滑、 連續(xù)的路徑曲線.二維及三維情形下的仿真結(jié)果表明： 所提Biased-RRT修正算法在機(jī)械臂路徑規(guī)劃上是可行、 有效的.