李 洋 徐 達(dá) 周 誠(chéng)

(陸軍裝甲兵學(xué)院兵器與控制系， 北京 100072)

0 引言

RRT作為具有概率完備性的規(guī)劃方法[1-4]廣泛應(yīng)用于機(jī)器人的路徑規(guī)劃。該方法省略了在C-space(構(gòu)型空間)中建立障礙物模型的過(guò)程，只需在節(jié)點(diǎn)處進(jìn)行碰撞檢測(cè)，其運(yùn)行速度快，對(duì)于高維空間的路徑規(guī)劃優(yōu)勢(shì)更加明顯。目前，國(guó)內(nèi)外學(xué)者針對(duì)RRT提出了多種改進(jìn)方法，LIN等[5]提出了一種自適應(yīng)步長(zhǎng)RRT，該方法可根據(jù)障礙物自動(dòng)調(diào)整隨機(jī)樹(shù)的生長(zhǎng)方向，可快速生成避障路徑。WANG等[6]提出了變步長(zhǎng)RRT方法，在隨機(jī)樹(shù)的生長(zhǎng)過(guò)程中，加入了貪婪思想，使隨機(jī)樹(shù)以固定步長(zhǎng)增量盡可能生長(zhǎng)，加快了RRT算法的收斂速度，但無(wú)法控制在W-space(工作空間)中的步長(zhǎng)大小。文獻(xiàn)[7-10]提出了自適應(yīng)維度RRT，通過(guò)降低C-space的局部維度，提高算法的運(yùn)行速度，但該方法沒(méi)有考慮節(jié)點(diǎn)之間的碰撞問(wèn)題，無(wú)法保證碰撞檢測(cè)的有效性。劉成菊等[11]將勢(shì)函數(shù)引入到RRT算法中，減少了RRT算法的隨機(jī)性，在局部搜索過(guò)程中具有良好的避障效果。

目前，RRT及其改進(jìn)方法的相關(guān)研究主要集中在移動(dòng)機(jī)器人的路徑規(guī)劃，在雙機(jī)器人的協(xié)同路徑規(guī)劃方面，RRT相關(guān)應(yīng)用較少。對(duì)于高維空間的運(yùn)動(dòng)規(guī)劃，RRT方法雖然省去了在C-space中建立障礙物模型的過(guò)程，但只在節(jié)點(diǎn)處進(jìn)行碰撞檢測(cè)難以保證其有效性[12-15]；利用RRT進(jìn)行雙機(jī)器人協(xié)同路徑規(guī)劃時(shí)，在每個(gè)節(jié)點(diǎn)處兩個(gè)機(jī)器人末端執(zhí)行器的相對(duì)位置必須保持固定[16-17]，而雙機(jī)器人的隨機(jī)樹(shù)卻在各自的C-space中隨機(jī)生長(zhǎng)，若要保持雙機(jī)器人之間的協(xié)調(diào)，需在不同的C-space中對(duì)機(jī)器人隨機(jī)樹(shù)的生長(zhǎng)進(jìn)行協(xié)調(diào)控制。

本文提出一種自適應(yīng)步長(zhǎng)RRT方法，通過(guò)構(gòu)建C-space和W-space中步長(zhǎng)的范數(shù)不等式，把隨機(jī)樹(shù)每一次生長(zhǎng)產(chǎn)生的位移約束在給定范圍內(nèi)，確保碰撞檢測(cè)的有效性，通過(guò)被動(dòng)生長(zhǎng)算法控制兩個(gè)機(jī)器人隨機(jī)樹(shù)的協(xié)調(diào)生長(zhǎng)，完成雙機(jī)器人在W-space中的協(xié)調(diào)運(yùn)動(dòng)，以實(shí)現(xiàn)雙機(jī)器人協(xié)同路徑規(guī)劃。

1 雙機(jī)器人運(yùn)動(dòng)學(xué)模型建立與求解

1.1 雙機(jī)器人旋量模型

建立的雙機(jī)器人模型如圖1所示，雙機(jī)器人系統(tǒng)由機(jī)器人R1、R2組成，其基座坐標(biāo)系分別為S1和S2，取S1為慣性坐標(biāo)系，S2在慣性坐標(biāo)系下的描述為

1S2=Rz(π)Tx(lx)Ty(ly)S2

(1)

其中

圖1 雙機(jī)器人模型Fig.1 Models of dual-robot

通過(guò)1S2可以將機(jī)器人R1和R2的運(yùn)動(dòng)學(xué)統(tǒng)一在慣性坐標(biāo)系S1中表達(dá)，故只需在機(jī)器人R1、R2各自的基座坐標(biāo)系下建立運(yùn)動(dòng)學(xué)模型即可。建立機(jī)器人R1的運(yùn)動(dòng)旋量模型，如圖2所示，R1的慣性坐標(biāo)系S與基座坐標(biāo)系重合，T為工具坐標(biāo)系，關(guān)節(jié)1和5繞z軸方向旋轉(zhuǎn)，其余關(guān)節(jié)繞y軸方向旋轉(zhuǎn)，ri為各關(guān)節(jié)軸線所在位置，建立關(guān)節(jié)的運(yùn)動(dòng)旋量坐標(biāo)

(2)

圖2 機(jī)器人R1的運(yùn)動(dòng)旋量模型Fig.2 Twist model of robot R1

將關(guān)節(jié)的旋量坐標(biāo)映射為指數(shù)形式

1.2 正向運(yùn)動(dòng)學(xué)

根據(jù)串聯(lián)機(jī)器人的POE公式[18]，計(jì)算R1的正向運(yùn)動(dòng)學(xué)方程

(3)

其中r11=-c6(s1s5-c1c5c234-c1s6s234)

r21=-c6(c1s5-s1c5c234-s1s6s234)
r31=-s6c234-c5c6s234
r12=-c5s1-c1s5c234r22=c1c5-s1c5c234
r32=s5s234r13=c1c6s234-s6(s1s5-c1c5c234)
r23=c6s1s234-s6(c1s5+s1c5c234)
r33=c6c234-c5s6s234
p1=l5s1+l2c1s2+l3(c1c2c3+c1s2c3)+
l4(c1c2c3s4+c1c2s3c4+c1s2c3c4-c1s2s3s4)
p2=-l5c1+l2s1s2+l3(s1c2s3+s1s2c3)+
l4(s1c2c3s4+s1c2s3c4+s1s2c3c4-s1s2s3s4)

p3=l1+l2c2+l3c23+l4c234

式中ci=cosθi，si=sinθi，cij=cos(θi+θj)，sij=sin(θi+θj)，下文同。

為實(shí)現(xiàn)雙機(jī)器人協(xié)同路徑規(guī)劃，須將機(jī)器人R1和R2的正向運(yùn)動(dòng)學(xué)統(tǒng)一在同一個(gè)坐標(biāo)系下描述，由于機(jī)器人R1和R2具有相同的結(jié)構(gòu)，所以機(jī)器人R2的正向運(yùn)動(dòng)學(xué)g2(θ)在S2中描述與g1(θ)相同，通過(guò)式(1)可將g2(θ)在慣性坐標(biāo)系S下表述為

g2(θ)=1S2g1(θ)

(4)

1.3 逆向運(yùn)動(dòng)學(xué)

機(jī)器人R1和R2后3個(gè)關(guān)節(jié)的軸線不相交于一點(diǎn)，因此無(wú)法求解逆向運(yùn)動(dòng)學(xué)的解析解[19]。本文采用Newton-Raphson迭代法[20]，首先給出一組估計(jì)解θ0和誤差Δ，通過(guò)多次迭代找到一組接近θ0且誤差小于Δ的數(shù)值解θt，θ0的選取在下文中會(huì)進(jìn)行詳細(xì)的說(shuō)明。

1.4 雅可比矩陣

雅可比矩陣定義了機(jī)器人單個(gè)關(guān)節(jié)速度對(duì)末端執(zhí)行器速度的貢獻(xiàn)度，將機(jī)器人C-space中的關(guān)節(jié)速度映射到W-space的矩陣函數(shù)，是構(gòu)建C-space和W-space中步長(zhǎng)關(guān)系的關(guān)鍵。機(jī)器人R1末端執(zhí)行器速度與關(guān)節(jié)速度的映射關(guān)系為

V=J(θ)θ′

(5)

(6)

式中θ′——關(guān)節(jié)速度

J(θ)——雅可比矩陣

機(jī)器人R1的6個(gè)關(guān)節(jié)皆為轉(zhuǎn)動(dòng)副，因此有

(7)

2 傳統(tǒng)RRT算法

采用固定步長(zhǎng)RRT方法對(duì)機(jī)器人路徑進(jìn)行規(guī)劃，需要在C-space中確定步長(zhǎng)，對(duì)于多自由度的關(guān)節(jié)型機(jī)器人，C-space通常為6維，因此無(wú)法在C-space中進(jìn)行碰撞檢測(cè)，碰撞檢測(cè)只能在W-space中的節(jié)點(diǎn)處進(jìn)行，雖然在C-space中RRT的步長(zhǎng)為固定的Δθ，但是通過(guò)正向運(yùn)動(dòng)學(xué)將Δθ映射到W-space中所產(chǎn)生的位移Δp卻無(wú)法固定。

如圖3所示，在C-space中隨機(jī)樹(shù)以固定步長(zhǎng)Δθ從節(jié)點(diǎn)1生長(zhǎng)到節(jié)點(diǎn)2所引起的位移Δp大于障礙物W-obs的尺寸，而在W-space中節(jié)點(diǎn)1和節(jié)點(diǎn)2并沒(méi)有與障礙物發(fā)生碰撞，RRT算法會(huì)通過(guò)這次碰撞檢測(cè)，但在機(jī)器人從節(jié)點(diǎn)1向節(jié)點(diǎn)2運(yùn)動(dòng)的過(guò)程中卻發(fā)生了碰撞，由此可見(jiàn)，利用固定步長(zhǎng)RRT方法對(duì)多自由度關(guān)節(jié)型機(jī)器人進(jìn)行路徑規(guī)劃，無(wú)法確保生成一條避障路徑。

圖3 C-space和W-space中的步長(zhǎng)關(guān)系Fig.3 Relationship of stepsize in C-space and W-space

3 自適應(yīng)步長(zhǎng)RRT

3.1 C-space和W-space的范數(shù)不等式構(gòu)建

RRT方法以固定步長(zhǎng)Δθ在C-space中進(jìn)行隨機(jī)樹(shù)的生長(zhǎng)無(wú)法控制在W-space中所產(chǎn)生的位移Δp，為了解決這一問(wèn)題引入算子范數(shù)，構(gòu)建C-space和W-space中步長(zhǎng)的范數(shù)不等式，實(shí)現(xiàn)在C-space中控制W-space中Δp的目的。給定兩個(gè)向量范數(shù)‖·‖a∈Rm，‖·‖b∈Rn和矩陣A∈Rm×n，從屬于兩個(gè)向量范數(shù)的矩陣算子范數(shù)為

(8)

可得到范數(shù)不等式

‖Ax‖a≤‖A‖a,b·‖x‖b

(9)

機(jī)器人R1的C-space中步長(zhǎng)Δθ∈R6，W-space中所對(duì)應(yīng)的位移Δp∈R3，因此只需構(gòu)造從屬于向量Δp和Δθ的矩陣算子范數(shù)，就可建立C-space和W-space的范數(shù)不等式，即構(gòu)造一個(gè)矩陣A∈R3×6，滿足Δp=AΔθ即可通過(guò)不等式約束Δp的最大值，達(dá)到在C-space中控制步長(zhǎng)的目的。矩陣A的構(gòu)造過(guò)程如下：

(10)

其中

根據(jù)雅可比矩陣式(10)可改寫(xiě)為

(11)

式(11)兩邊同乘以Δt可得

(12)

‖Δp‖w≤‖A‖w,c‖Δθ‖c

(13)

式中 ‖Δp‖w——W-space中的范數(shù)

‖Δθ‖c——C-space中的范數(shù)

3.2 C-space和W-space范數(shù)相容不等式改進(jìn)

在式(13)中Δp特指雙機(jī)器人末端執(zhí)行器的位移，當(dāng)在C-space中給定一個(gè)步長(zhǎng)Δθ機(jī)器人發(fā)生位移的最大位置不一定為末端執(zhí)行器，也有可能發(fā)生在關(guān)節(jié)處，所以僅對(duì)機(jī)器人末端執(zhí)行器的位移進(jìn)行約束無(wú)法保證碰撞檢測(cè)的有效性，因此需改進(jìn)范數(shù)不等式。

選用長(zhǎng)方體包圍盒對(duì)雙機(jī)器人的連桿進(jìn)行模型等效，設(shè)第i個(gè)連桿的頂點(diǎn)集合Li={pi1,pi2,…,pi8}，i=1,2,…,6，則產(chǎn)生最大位移的位置必然在集合Li中，所以有

‖Δpmax‖w=max‖Δpij‖w(Δpij∈Li)

(14)

將式(14)代入式(13)可得

max‖Δpij‖w≤max‖Aij‖w,c‖Δθ‖c

(15)

max‖Aij‖w,c∈R3×6

由此得到改進(jìn)C-space和W-space范數(shù)不等式為

‖Δpij‖w≤max‖Aij‖w,c‖Δθ‖c

(16)

式中w、c的取值參考3.1節(jié)。

通過(guò)式(16)可以控制機(jī)器人任意位置由Δθ所引起的最大位移Δpij，完成雙機(jī)器人的全局性避障。

3.3 隨機(jī)樹(shù)被動(dòng)生長(zhǎng)

為了保持機(jī)器人之間的協(xié)調(diào)，在W-space中，隨機(jī)樹(shù)每一個(gè)節(jié)點(diǎn)的機(jī)器人末端執(zhí)行器相對(duì)位置必須保持不變，因此兩個(gè)機(jī)器人在W-space中必須共享一顆隨機(jī)樹(shù)S_tree，但是在C-space中，機(jī)器人R1和R2的隨機(jī)樹(shù)卻是各自生長(zhǎng)，故RRT算法無(wú)法控制雙機(jī)器人隨機(jī)樹(shù)的生長(zhǎng)產(chǎn)生相同的S_tree。引入隨機(jī)樹(shù)被動(dòng)生長(zhǎng)方法，使機(jī)器人R2的隨機(jī)樹(shù)跟隨機(jī)器人R1隨機(jī)樹(shù)的生長(zhǎng)，在機(jī)器人R2的隨機(jī)樹(shù)生長(zhǎng)的同時(shí)，完成對(duì)S_tree的復(fù)制，如圖4所示，假設(shè)機(jī)器人R1的隨機(jī)樹(shù)為主動(dòng)生長(zhǎng)樹(shù)A_tree，機(jī)器人R2的隨機(jī)樹(shù)為被動(dòng)生長(zhǎng)樹(shù)P_tree，當(dāng)A_tree在C-space中生長(zhǎng)出一個(gè)新的節(jié)點(diǎn)A_node時(shí)，通過(guò)正向運(yùn)動(dòng)學(xué)將該節(jié)點(diǎn)映射到W-space中，完成共享隨機(jī)樹(shù)S_tree的生長(zhǎng)并產(chǎn)生新的共享節(jié)點(diǎn)S_node，而P_tree新節(jié)點(diǎn)P_node的正向運(yùn)動(dòng)學(xué)須滿足

g1(A_node)=1S2g2(P_node)=S_node

(17)

因此機(jī)器人R2隨機(jī)樹(shù)P_tree只能根據(jù)式(17)進(jìn)行生長(zhǎng)，通過(guò)機(jī)器人R2的逆向運(yùn)動(dòng)學(xué)反解出P_node，為了避免機(jī)器人R2的關(guān)節(jié)位移產(chǎn)生跳動(dòng)，估計(jì)解θ0應(yīng)取在P_tree中A_node的父節(jié)點(diǎn)Anear_node所對(duì)應(yīng)的節(jié)點(diǎn)Pnear_node。此時(shí)P_node為P_tree的新節(jié)點(diǎn)，若P_node通過(guò)碰撞檢測(cè)，則把P_node加入到機(jī)器人R2隨機(jī)樹(shù)P_tree中，完成一次雙機(jī)器人隨機(jī)樹(shù)的生長(zhǎng)。

圖4 隨機(jī)樹(shù)被動(dòng)生長(zhǎng)Fig.4 Passive growing of random tree

3.4 雙機(jī)器人自適應(yīng)步長(zhǎng)RRT算法

基于3.2節(jié)所建立的范數(shù)不等式和3.3節(jié)提出的隨機(jī)樹(shù)被動(dòng)生長(zhǎng)方法，建立雙機(jī)器人自適應(yīng)步長(zhǎng)RRT算法，首先確定機(jī)器人在W-space中所允許的最大位移，令max‖Aij‖w,c‖Δθ‖c=Δpmax，代入式(16)可得

‖Δpijmax‖w≤max‖Aij‖w,c‖Δθ‖c=Δpmax

(18)

在構(gòu)型空間中的步長(zhǎng)為

(19)

式中‖Δpmax‖w為算法的初始設(shè)定值，隨機(jī)樹(shù)每一次生長(zhǎng)都會(huì)更新‖Aij‖w,c，從而使步長(zhǎng)Δθ自動(dòng)滿足式(19)，此時(shí)Δθ的取值即控制了機(jī)器人工作空間步長(zhǎng)Δpijmax又可以保證隨機(jī)樹(shù)的生長(zhǎng)速率。為了提高算法的搜索效率，采用雙向搜索方法[21]，雙機(jī)器人自適應(yīng)步長(zhǎng)RRT算法的偽代碼為

F_WTree=pint,S_WTree=pgoal

Whilen

Tree_Add(pnew,F_WTree)

return Path_J(S_JTree, F_JTree)

return Path_W(S_WTree, F_WTree)

else

ExchangeTrees(S_JTree1, S_JTree2, F_JTree1,F_JTree2)

end if

end if

end while

Return Null

在C-space中分別定義以機(jī)器人R1、R2起始點(diǎn)和目標(biāo)點(diǎn)為根的隨機(jī)樹(shù)F_JTree1、S_JTree1、F_JTree2、S_JTree2，在W-space中定義機(jī)器人R1、R2的共享樹(shù)F_WTree、S_WTree，在對(duì)隨機(jī)樹(shù)F_JTree1進(jìn)行生長(zhǎng)時(shí)，引入范數(shù)不等式，通過(guò)Stepsize( )計(jì)算并且約束步長(zhǎng)Δθ，Stepsize( )的偽代碼為

算法實(shí)質(zhì)上為式(19)的計(jì)算過(guò)程，其中涉及到w、c的取值，前文已有w=2，c=1，則Aij的算子范數(shù)定義如下

(20)

Aij的算子范數(shù)求解偽代碼為

max=0

J=Jacc(θnew)

fori= 1 to 6

forj= 1 to 8

fork= 1 toi

temp=‖ωi×pij+νi‖2

if temp>max

max=temp

end if

end for

end for

end for

Return max

算法中的第2行是雅可比矩陣的求解，ωi和νi已經(jīng)在前文中定義。

flag = true

pidx= GetpointOnF_WTree1(idx, F_WTree1)

d= GetDirection(pidx,pnew)

dist= GetDistance(pidx,pnew)

R= GetPose(pnew)

while flag = true

temp=pnew+s

if CollisionFreeAndCheckJointLimits

pnew=temp

continue

else

break

end if

else

flag = false

end if

end while

圖5 隨機(jī)樹(shù)的融合Fig.5 Merging of random tree

算法首先找出隨機(jī)樹(shù)S_WTree1中距離pnew1最近的點(diǎn)pidx，并確定隨機(jī)樹(shù)的融合方向d，算法的9～12行是以自適應(yīng)步長(zhǎng)的方式，在pnew和pidx之間插入n個(gè)節(jié)點(diǎn)，而后檢測(cè)這些節(jié)點(diǎn)能否通過(guò)關(guān)節(jié)限制檢測(cè)和碰撞檢測(cè)，若通過(guò)檢測(cè)則隨機(jī)樹(shù)F_WTree、S_WTree融合，機(jī)器人R1和R2在工作空間中走出以pnew1、pidx為端點(diǎn)的直線軌跡；若其中有節(jié)點(diǎn)發(fā)生碰撞或者超出關(guān)節(jié)限制，則隨機(jī)樹(shù)F_WTree、S_WTree無(wú)法融合，繼續(xù)進(jìn)行隨機(jī)樹(shù)的生長(zhǎng)。

當(dāng)隨機(jī)樹(shù)F_WTree、S_WTree完成融合時(shí)，自適應(yīng)步長(zhǎng)RRT算法結(jié)束，得到雙機(jī)器人的協(xié)同運(yùn)動(dòng)路徑。

4 仿真

4.1 單次仿真

在Matlab中搭建雙機(jī)器人模型，如圖6所示，在雙機(jī)器人的W-space中分別設(shè)置4個(gè)球形障礙物，半徑分別為0.1、0.1、0.15、0.2 m，和一個(gè)長(zhǎng)方體障礙物，尺寸為1.6 m×0.4 m×0.4 m，最小半徑為0.1 m，故設(shè)置Δpmax為0.1 m，即機(jī)器人R1和R2在W-space中的步長(zhǎng)不能超過(guò)0.1 m，否則碰撞檢測(cè)將會(huì)失效，無(wú)法保證機(jī)器人有效避障，起點(diǎn)的坐標(biāo)為(0.15，0.6，0)，終點(diǎn)的坐標(biāo)為(-0.55，0.6，0.6)。在模型中分別利用自適應(yīng)步長(zhǎng)RRT算法和RRT算法進(jìn)行路徑搜索并進(jìn)行對(duì)比。

圖6 雙機(jī)器人仿真環(huán)境Fig.6 Simulation environment of dual-robot

圖7為自適應(yīng)步長(zhǎng)RRT算法和步長(zhǎng)為0.5 rad的RRT算法進(jìn)行一次路徑搜索的結(jié)果，其中以起始點(diǎn)為根的隨機(jī)樹(shù)為F_WTree，以目標(biāo)點(diǎn)為根的隨機(jī)樹(shù)為S_WTree，當(dāng)兩棵樹(shù)達(dá)到融合條件時(shí)，隨機(jī)樹(shù)停止生長(zhǎng)，經(jīng)過(guò)圓滑處理產(chǎn)生一條距離最短的最終路徑。由于RRT算法的步長(zhǎng)為0.5 rad，因此在工作空間中產(chǎn)生的步長(zhǎng)也相對(duì)較大，所用的迭代次數(shù)較少，雖然RRT算法成功地生成了一條路徑，但從圖7a可發(fā)現(xiàn)，RRT算法在工作空間中所產(chǎn)生的步長(zhǎng)具有明顯的跳動(dòng)，在W-space中的步長(zhǎng)最大值為0.155 7 m，大于Δpmax，無(wú)法保證碰撞檢測(cè)的有效性。自適應(yīng)步長(zhǎng)RRT所產(chǎn)生的步長(zhǎng)具有很強(qiáng)的穩(wěn)定性，且工作空間中最大步長(zhǎng)為0.023 6 m，小于Δpmax，實(shí)現(xiàn)了雙機(jī)器人的避障路徑規(guī)劃。

進(jìn)一步減小RRT算法的步長(zhǎng)并與自適應(yīng)步長(zhǎng)RRT算法進(jìn)行對(duì)比，其結(jié)果如圖8所示，圖8a為步長(zhǎng)為0.3 rad的RRT的路徑搜索結(jié)果，相對(duì)于步長(zhǎng)為0.5 rad的RRT，步長(zhǎng)為0.3 rad的RRT在W-space中產(chǎn)生的步長(zhǎng)平均值大幅減小，但仍然存在較大的步長(zhǎng)跳動(dòng)，步長(zhǎng)最大值為0.112 9 m，大于Δpmax，仍然無(wú)法保證碰撞檢測(cè)的有效性，圖8b為自適應(yīng)步長(zhǎng)RRT算法第2次的路徑搜索結(jié)果，保持了在W-space中步長(zhǎng)的穩(wěn)定性，最大步長(zhǎng)為0.032 7 m，小于Δpmax。

圖8 步長(zhǎng)為0.3 rad的RRT與自適應(yīng)步長(zhǎng)RRT路徑規(guī)劃對(duì)比Fig.8 Comparison of path planning by RRT with stepsize of 0.3 rad and self-adaptive stepsize RRT

將RRT算法的步長(zhǎng)縮小到0.1 rad并再次與自適應(yīng)步長(zhǎng)RRT算法進(jìn)行對(duì)比，結(jié)果如圖9所示，在C-space中0.1 rad是一個(gè)非常小的步長(zhǎng)，意味著機(jī)器人6個(gè)關(guān)節(jié)所產(chǎn)生的角度變化的總和為0.1 rad，因此隨機(jī)樹(shù)的每一次生長(zhǎng)在W-space中所產(chǎn)生的步長(zhǎng)會(huì)非常小，如圖9a所示，步長(zhǎng)最大值為0.001 9 m，雖然遠(yuǎn)小于Δpmax，但過(guò)小的步長(zhǎng)使隨機(jī)樹(shù)的生長(zhǎng)速度變慢，導(dǎo)致迭代次數(shù)劇增，是步長(zhǎng)為0.3 rad的12倍。在相對(duì)嚴(yán)格的融合條件下，兩棵隨機(jī)樹(shù)難以融合，在可接受的時(shí)間內(nèi)無(wú)法搜索出一條避障路徑。自適應(yīng)步長(zhǎng)RRT算法的第3次路徑搜索仍然保持了步長(zhǎng)穩(wěn)定的特性，且迭代次數(shù)約為步長(zhǎng)0.1 rad RRT算法的1/2，同時(shí)兼顧了隨機(jī)樹(shù)生長(zhǎng)速度和步長(zhǎng)大小的控制。

圖9 步長(zhǎng)為0.1 rad的RRT與自適應(yīng)步長(zhǎng)RRT路徑規(guī)劃對(duì)比Fig.9 Comparison of path planning by RRT with stepsize of 0.1 rad and self-adaptive stepsize RRT

通過(guò)對(duì)RRT算法步長(zhǎng)的多次調(diào)整，確定0.2 rad為RRT算法在當(dāng)前仿真環(huán)境下的步長(zhǎng)匹配值，進(jìn)行一次路徑搜索，其結(jié)果如圖10所示。

圖10 步長(zhǎng)為0.2 rad的RRT路徑規(guī)劃Fig.10 Path planning by RRT with stepsize of 0.2 rad

算法的最大步長(zhǎng)為0.042 3 m，迭代次數(shù)為30次，基本與自適應(yīng)步長(zhǎng)RRT算法相近，步長(zhǎng)仍有輕微跳動(dòng)，最大步長(zhǎng)已經(jīng)控制在Δpmax的范圍內(nèi)，可以滿足碰撞檢測(cè)的要求。使用RRT算法進(jìn)行路徑搜索時(shí)，從上述的仿真過(guò)程可以發(fā)現(xiàn)，找到合適的步長(zhǎng)通常需要多次調(diào)試才能夠確定，而自適應(yīng)步長(zhǎng)RRT只需要指定Δpmax，就可自動(dòng)調(diào)整C-space中的步長(zhǎng)來(lái)控制W-space中產(chǎn)生步長(zhǎng)，當(dāng)機(jī)器人的工作環(huán)境發(fā)生變化時(shí)，只需根據(jù)環(huán)境模型指定新的Δpmax就可再次進(jìn)行路徑搜索，而固定步長(zhǎng)RRT算法需再次對(duì)步長(zhǎng)進(jìn)行多次的調(diào)試。另外，傳統(tǒng)的RRT算法存在步長(zhǎng)過(guò)大所導(dǎo)致的碰撞檢測(cè)失效問(wèn)題，而自適應(yīng)步長(zhǎng)RRT算法，通過(guò)式(16)把機(jī)器人的關(guān)節(jié)、連桿和末端執(zhí)行器的位移全部約束在Δpmax內(nèi)，可得到機(jī)器人全局性的無(wú)碰撞路徑。

4.2 多次仿真

分別對(duì)自適應(yīng)步長(zhǎng)RRT和步長(zhǎng)0.5、0.3、0.1、0.2 rad的RRT進(jìn)行50次仿真，其結(jié)果如表1所示。表中pmax為W-space中步長(zhǎng)的最大值，當(dāng)pmax大于Δpmax時(shí)，算法失效。從表1可以看出，RRT算法隨著步長(zhǎng)的減小，pmax的最大值和平均值也隨之減小，但迭代次數(shù)會(huì)不斷增加。當(dāng)步長(zhǎng)為0.5 rad時(shí)，RRT算法的步長(zhǎng)較大，失效次數(shù)為37次，失效概率為74%，當(dāng)進(jìn)一步減小步長(zhǎng)至0.3 rad時(shí)，雖然pmax的平均值降到可接受的范圍內(nèi)，但仍有14次失效，失效概率為28%。當(dāng)步長(zhǎng)為0.1 rad時(shí)，失效次數(shù)雖然為0，但過(guò)小的步長(zhǎng)導(dǎo)致迭代次數(shù)的劇增，而自適應(yīng)步長(zhǎng)RRT既保證了算法的失效概率為0，同時(shí)又控制了算法的迭代次數(shù)。當(dāng)取RRT的步長(zhǎng)為0.2 rad時(shí)，可得到與自適應(yīng)步長(zhǎng)RRT相接近的最大步長(zhǎng)與迭代次數(shù)。

表1 不同步長(zhǎng)RRT與自適應(yīng)步長(zhǎng)RRT步長(zhǎng)、迭代次數(shù)和失效次數(shù)Tab.1 Maximum stepsize, iterations and invalid times of RRT and self-adaptive stepsize RRT

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

針對(duì)RRT算法無(wú)法控制雙機(jī)器人在W-space中步長(zhǎng)的問(wèn)題，提出了自適應(yīng)步長(zhǎng)RRT算法。通過(guò)構(gòu)建C-space和W-space的范數(shù)不等式，把隨機(jī)樹(shù)每一次生長(zhǎng)所產(chǎn)生的機(jī)器人位移約束在給定范圍內(nèi)，在利用自適應(yīng)步長(zhǎng)RRT進(jìn)行路徑規(guī)劃時(shí)，只需根據(jù)機(jī)器人的工作環(huán)境指定Δpmax，省去了RRT算法為確定步長(zhǎng)而進(jìn)行多次調(diào)試的耗時(shí)過(guò)程；提出了隨機(jī)樹(shù)被動(dòng)生長(zhǎng)方法，實(shí)現(xiàn)了雙機(jī)器人末端執(zhí)行器同位置不同構(gòu)型的路徑規(guī)劃，結(jié)合自適應(yīng)步長(zhǎng)RRT算法完成了雙機(jī)器人的協(xié)同路徑規(guī)劃。