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

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

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

0 引言

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

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

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

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

1.1 問題描述

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

1.2 六自由度機械臂的逆運動學(xué)分析

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

(1)

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

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

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

(2)

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

1.4 基于變步長的新節(jié)點擴展方式

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

(3)

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

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

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

擴展步長選取策略為

(4)

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

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

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

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

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

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

(5)

1.6 路徑簡化

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(6)

(7)

(8)

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

3.1 仿真實驗概述

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

3.2 改進算法的有效性驗證實驗

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

表1 平均擴展節(jié)點數(shù)對比

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

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

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

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

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

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

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

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

3.3 基于ROS實驗平臺的機器人仿真實驗驗證

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

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

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

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

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

4 結(jié)語

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