張紅文, 朱戰(zhàn)霞, 袁建平

(1.西北工業(yè)大學(xué) 航天學(xué)院, 陜西 西安 710072; 2.航天飛行動(dòng)力學(xué)技術(shù)國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室, 陜西 西安 710072)

FFSR具備節(jié)省燃料,延長(zhǎng)系統(tǒng)壽命等優(yōu)點(diǎn),被廣泛用于在軌服務(wù)。FFSR構(gòu)型參數(shù)中,關(guān)節(jié)角和基座姿態(tài)間存在一階導(dǎo)關(guān)系,需直接考慮微分約束進(jìn)行運(yùn)動(dòng)規(guī)劃。

FFSR運(yùn)動(dòng)規(guī)劃方法中,研究較多的包含3類。早期學(xué)者研究了系統(tǒng)的操控(steer)問題,即在不考慮障礙物的情況下,生成一系列的動(dòng)作,讓FFSR從初始構(gòu)型運(yùn)動(dòng)到目標(biāo)構(gòu)型。如Vafa等基于“虛擬機(jī)械臂(virtual manipulator,VM)”[1]的模型,發(fā)展了自校正運(yùn)動(dòng)法[1]及“擾動(dòng)圖法(disturbance map,DM)”[2]。Nakamura 和Mukherjee[3]基于李雅普諾夫函數(shù)設(shè)計(jì)了雙向方法。Fernandes等[4]研究了耦合剛體的運(yùn)動(dòng)規(guī)劃方法，此類方法著眼于利用關(guān)節(jié)運(yùn)動(dòng)操控基座姿態(tài);但未考慮碰撞,不能形成完整的運(yùn)動(dòng)規(guī)劃。第二類方法是基于多項(xiàng)式參數(shù)化的方法,即以FFSR的初始關(guān)節(jié)角和最終關(guān)節(jié)角為端點(diǎn),利用帶未知系數(shù)的多項(xiàng)式對(duì)關(guān)節(jié)軌跡進(jìn)行參數(shù)化,并以最終時(shí)刻基座姿態(tài)擾動(dòng)為適應(yīng)度函數(shù),將規(guī)劃問題轉(zhuǎn)換為優(yōu)化問題;利用遺傳算法[5]、粒子群算法[6]、微分進(jìn)化算法[7]等解優(yōu)化問題,可得最終軌跡。該類方法不易控制軌跡的形狀,因此很難考慮碰撞問題;此外,由于適應(yīng)度函數(shù)計(jì)算需進(jìn)行積分,因此效率低下。第三類方法是通過直接法或間接法將運(yùn)動(dòng)規(guī)劃問題轉(zhuǎn)換為非線性優(yōu)化問題[8-9]。Misra等[10]于2017年基于非線性優(yōu)化研究了空間機(jī)器人考慮任務(wù)約束的運(yùn)動(dòng)規(guī)劃問題,并將問題構(gòu)造為一個(gè)凸二次規(guī)劃問題。該類方法求解效率高,但也存在局部最優(yōu)問題。

基于采樣的運(yùn)動(dòng)規(guī)劃方法,能高效解決有高自由度構(gòu)型空間的機(jī)器人運(yùn)動(dòng)規(guī)劃問題。其中RRT(rapidly-exploring random tree)對(duì)有微分約束運(yùn)動(dòng)規(guī)劃有很強(qiáng)的適用性[11]。一般情況下,運(yùn)動(dòng)規(guī)劃多采用“先求逆運(yùn)動(dòng)學(xué),再規(guī)劃路徑/軌跡”的框架;但這一框架存在逆運(yùn)動(dòng)學(xué)所求目標(biāo)構(gòu)型與初始構(gòu)型未必在同一連通域內(nèi)的隱患。針對(duì)這一問題,Bertram、Weghe等[12-13]基于有目標(biāo)偏置的RRT(RRT with Goal Bias,GB-RRT)給出了解決方案,即不求逆運(yùn)動(dòng)學(xué),而是直接引入目標(biāo)末端位姿導(dǎo)引生長(zhǎng)。在該方案中,用于目標(biāo)末端位姿導(dǎo)引生長(zhǎng)的局部規(guī)劃含一個(gè)迭代過程,每次迭代通過雅克比矩陣的轉(zhuǎn)置,將末端位姿誤差映射到構(gòu)型空間,從而產(chǎn)生一系列越來越接近目標(biāo)末端位姿的構(gòu)型，但該方案針對(duì)固定基座機(jī)械臂的路徑規(guī)劃問題,而FFSR的運(yùn)動(dòng)規(guī)劃需直接考慮微分約束。此外,固定基座機(jī)械臂執(zhí)行機(jī)構(gòu)自由度大于末端位姿自由度,但FFSR末端末端位姿導(dǎo)引生長(zhǎng),除要求具備最終收斂到目標(biāo)末端位姿的能力外,還需保證基座姿態(tài)擾動(dòng)不能過大,因此執(zhí)行機(jī)構(gòu)自由度不足。

本文以GB-RRT為框架設(shè)計(jì)規(guī)劃算法,解決FFSR從初始構(gòu)型到目標(biāo)末端位姿的運(yùn)動(dòng)規(guī)劃問題。為適應(yīng)FFSR直接考慮微分約束的需求,在Weghe等[13]提出的方法基礎(chǔ)上,在每次迭代中引入積分機(jī)制;為克服FFSR執(zhí)行機(jī)構(gòu)自由度不足的問題,提出適時(shí)調(diào)節(jié)基座姿態(tài)的目標(biāo)末端位姿導(dǎo)引生長(zhǎng)局部規(guī)劃器,以調(diào)節(jié)末端位姿為主,同時(shí)在必要時(shí)兼顧考慮基座姿態(tài)的調(diào)節(jié)。

1 自由漂浮空間機(jī)器人的運(yùn)動(dòng)學(xué)方程

FFSR如圖1所示,符號(hào)定義與文獻(xiàn)[14]相同。

圖1 FFSR模型簡(jiǎn)圖

1.1 僅考慮幾何關(guān)系的運(yùn)動(dòng)學(xué)方程

1.1.1 姿態(tài)方程

IRe=feR(q)

(1)

FFSR的EE姿態(tài)角Ψe與IRe對(duì)應(yīng),因此

Ψe=feΨ(q)

(2)

1.1.2 位置方程

FFSR末端執(zhí)行器位置pe為

(3)

1.1.3 速度方程

FFSR末端執(zhí)行器角速度ωe為

(4)

末端執(zhí)行器線速度ve度為

(5)

(6)

Jb,Jm的表達(dá)式見文獻(xiàn)[14]。

1.2 動(dòng)量守恒方程

FFSR的動(dòng)量方程為

(7)

式中，P與L分別為FFSR的線動(dòng)量、角動(dòng)量,相關(guān)矩陣的定義間文獻(xiàn)[14]。令

1.3 考慮動(dòng)量守恒后的運(yùn)動(dòng)學(xué)方程

1.3.1 基座姿態(tài)角一階導(dǎo)方程

(8)

取上述方程后3行,可得基座角速度ωb為

(9)

(10)

式中

(11)

式中，JbΨ為與基座姿態(tài)角相關(guān)的雅克比矩陣。

1.3.2 狀態(tài)轉(zhuǎn)移方程

考慮(11)式,則FFSR的狀態(tài)轉(zhuǎn)移方程為

(12)

式中，Jx為與狀態(tài)轉(zhuǎn)移相關(guān)的雅克比矩陣。

1.3.3 廣義雅克比矩陣

考慮(6)式以及(8)式,可得

(13)

Jg為廣義雅克比矩陣。

1.3.4 考慮線動(dòng)量守恒積分形式后的方程

FFSR的線動(dòng)量可積分為位置形式

(14)

(15)

進(jìn)而

(16)

給出構(gòu)型參數(shù)q,可計(jì)算在慣性空間之中的表示Ibi,Iai,進(jìn)而可計(jì)算r0,即q為構(gòu)型參數(shù)q的函數(shù)。再考慮(3)式,可知pe為q的函數(shù)

pe=fep(q)

(17)

綜合考慮(1)式以及(17)式,可得端位姿xe與q間的映射關(guān)系

(18)

2 FFSR無逆運(yùn)動(dòng)學(xué)基于采樣的運(yùn)動(dòng)規(guī)劃

算法偽代碼如Algorithm 1所示,在每次迭代中,利用隨機(jī)采樣構(gòu)型qSample或目標(biāo)末端位姿xed來導(dǎo)引樹的生長(zhǎng),這2種生長(zhǎng)分別稱為隨機(jī)導(dǎo)引生長(zhǎng)(Extend-Randomly()函數(shù))與目標(biāo)末端位姿導(dǎo)引生長(zhǎng)(Extend-Toward-Goal())。每次迭代生成一個(gè)隨機(jī)數(shù)p∈[0,1],若p

algorithm 1 GB-RRT for FFSR1tree.V.init(qI)2for i=1 to K do3 if rand()

2.1 局部規(guī)劃器設(shè)計(jì)

2.2.1 隨機(jī)構(gòu)型導(dǎo)引生長(zhǎng)局部規(guī)劃器

隨機(jī)構(gòu)型導(dǎo)引生長(zhǎng)選擇離qsample最近的節(jié)點(diǎn)作為待擴(kuò)展點(diǎn)qextended,并從qextended向qsample進(jìn)行局部規(guī)劃,以生成局部軌跡以及新的節(jié)點(diǎn)。

algorithm 2 extend-randomly1qsample=random-config()2qextend=nearest-node(tree,qsample)3qpresent=qextend4last-checked-q=qpresent5shall-end=false6while all(shall-end=false)7 Jb=Jacobian(qpresent)8 Jx=[Jb,eye(n,n)]9 Δq=qsample-qpresent10 JV=inv(Jx)*Δq11 BV=Jb*JV12 qpresent=qpresent+[BV;JV]*time-step13 If max(qpresent-last-checked-q)>= collision-check-thresh14 is-collision=collision-check(qpresent)15 last-checked-q=qpresent17 shall-end=check-termination()19return

(19)

(20)

從而得到構(gòu)型速度

(21)

通過積分,更新構(gòu)型

(22)

對(duì)qpresent進(jìn)行碰撞檢測(cè),以保證所生成的局部軌跡處于自由構(gòu)型空間。若滿足下述任何一個(gè)條件,則停止迭代:①檢測(cè)出碰撞;②構(gòu)型超出限制,即基座姿態(tài)擾動(dòng)超出限制,或者關(guān)節(jié)角度超出范圍;③構(gòu)型參數(shù)之中最大擴(kuò)展值達(dá)到一定的閾值。

2.2.2 適時(shí)調(diào)節(jié)基座姿態(tài)的目標(biāo)末端位姿導(dǎo)引生長(zhǎng)局部規(guī)劃器

algorithm3 extend-toward-goal1qextend=nearest-node(tree,Xgoal) 2qpresent=qextend 3last-checked-q=qpresent 4shall-end=false 5while all(shall-end=false) 6 JB=Jacobian(qpresent) 7 JG=general-Jacobian(qpresent) 8 ΔX=Xgoal-f(qpresent) 9 ΔqB=qBgoal-qBpresent 10 JV-for-EE=pinv(JG)*ΔX 11 if ΔqB>B-adjust-thresh 12 JV-for-BA=pinv(JB*null(JG))*ΔqB 13 else 14 JV-for-BA=0 15 JV=JV-for-EE+null(JG)*JV-for-BA 16 BV=Jb*JV 17 qpresent=qpresent+[BV;JV]*time-step 18 if max(qpresent-last-checked-q)>=C-check-tresh 19 is-collision=collision-check(qpresent) 20 last-checked-q=qpresent 21 shall-end=check-termination() 22return

目標(biāo)末端位姿導(dǎo)引生長(zhǎng),即選擇離xed最近的點(diǎn)作為待擴(kuò)展點(diǎn)qextended,并從qextended向xed進(jìn)行局部規(guī)劃;其要求盡可能消除相對(duì)于xed的誤差,同時(shí)保證Ψb不超出取值范圍。本文提出適時(shí)調(diào)節(jié)基座姿態(tài)的目標(biāo)末端位姿導(dǎo)引生長(zhǎng)其局部規(guī)劃器,其偽代碼如Algorithm 3所示。作為執(zhí)行機(jī)構(gòu)的FFSR操作臂關(guān)節(jié)僅有7個(gè)自由度,而末端位姿任務(wù)為六自由度,基座姿態(tài)為三自由度。相對(duì)于到達(dá)xed以及保持Ψb處于一定取值范圍的要求,執(zhí)行機(jī)構(gòu)自由度不足;因此若在局部規(guī)劃的每一次迭代中,同時(shí)調(diào)節(jié)xe和Ψb,則末端位姿誤差無法收斂。

(23)

(24)

(25)

獲得動(dòng)作后,后續(xù)其他步驟與2.2.1節(jié)中相應(yīng)的步驟相同。本局部規(guī)劃器的終止條件包含:①檢測(cè)出碰撞;②構(gòu)型超出限制,即基座姿態(tài)擾動(dòng)超出限制,或者關(guān)節(jié)角度超出范圍;③到達(dá)目標(biāo)。

3 仿 真

表1 FFSR的動(dòng)力學(xué)以及幾何參數(shù)

表2 FFSR的D-H參數(shù)

3.1 場(chǎng)景1的結(jié)果

36次迭代后,FFSR的末端到達(dá)目標(biāo)位置,規(guī)劃結(jié)束。如圖2a)所示,圖中淡藍(lán)色為初始構(gòu)型,紅色為最終構(gòu)型。從初始構(gòu)型到目標(biāo)末端位置的運(yùn)動(dòng)歷程如圖2b)所示,整個(gè)過程中未發(fā)生碰撞。關(guān)節(jié)角、基座姿態(tài)隨時(shí)間的變化分別如圖3所示,從圖3b)可看出,基座姿態(tài)滿足取值范圍限制。

圖2 場(chǎng)景1：初始構(gòu)型與最終構(gòu)型，從初始圖3 場(chǎng)景1：關(guān)節(jié)角及基座姿態(tài)變化曲線構(gòu)型向目標(biāo)末端位置的運(yùn)動(dòng)歷程

3.2 場(chǎng)景2的結(jié)果

116次迭代后,到達(dá)目標(biāo)末端位姿,規(guī)劃成功;圖4a)中,紅色為最終構(gòu)型,青色為初始構(gòu)型。從初始構(gòu)型向目標(biāo)末端位姿的運(yùn)動(dòng)歷程如圖4b)所示,整個(gè)過程未發(fā)生碰撞。關(guān)節(jié)角、基座姿態(tài)隨時(shí)間的變化如圖5所示,可發(fā)現(xiàn)基座姿態(tài)處于取值范圍內(nèi)。

圖4 場(chǎng)景2：初始構(gòu)型與最終構(gòu)型，從初始圖5 場(chǎng)景2：關(guān)節(jié)角及基座姿態(tài)變化曲線構(gòu)型向目標(biāo)末端位姿的運(yùn)動(dòng)歷程

4 結(jié) 論

本文基于GB-RRT設(shè)計(jì)了FFSR無逆運(yùn)動(dòng)學(xué)的運(yùn)動(dòng)規(guī)劃算法,并提出適時(shí)調(diào)節(jié)基座姿態(tài)的目標(biāo)末端位姿導(dǎo)引局部規(guī)劃器,引入?yún)⒖寄繕?biāo)基座姿態(tài)、基座姿態(tài)誤差閾值。在該局部規(guī)劃器的每一次迭代中,只有相對(duì)于參考目標(biāo)基座姿態(tài)誤差達(dá)到閾值,才調(diào)節(jié)基座姿態(tài),否則只調(diào)節(jié)末端位姿。該局部規(guī)劃器所生成的軌跡能滿足基座姿態(tài)取值限制,且具備讓末端位姿誤差收斂的能力,克服了FFSR執(zhí)行機(jī)構(gòu)自由度不足的問題。