張傳海，袁建平

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

0 引言

隨著空間技術(shù)的發(fā)展和空間任務(wù)的需求，各航天大國(guó)都加大力度支持空間機(jī)器人技術(shù)研究［1］?？臻g機(jī)器人主要由空間機(jī)器人本體及搭載在本體上的機(jī)械臂組成。當(dāng)本體的位置和姿態(tài)均不受控時(shí)，其處于自由漂浮狀態(tài)，稱為自由漂浮空間機(jī)器人。采用這種工作狀態(tài)不但可以節(jié)約大量寶貴的能源，延長(zhǎng)空間機(jī)器人的服役期限，而且可以避免當(dāng)機(jī)械臂接近目標(biāo)航天器時(shí)，位置姿態(tài)控制系統(tǒng)工作時(shí)突然點(diǎn)火造成的機(jī)械臂末端執(zhí)行器與目標(biāo)航天器的撞擊。因而，自由漂浮空間機(jī)器人在空間任務(wù)中具有更多的優(yōu)勢(shì)。由于自由漂浮空間機(jī)器人滿足動(dòng)量守恒，所以機(jī)械臂與本體的運(yùn)動(dòng)存在動(dòng)力學(xué)耦合，如何協(xié)調(diào)本體和機(jī)械臂的運(yùn)動(dòng)使機(jī)器人系統(tǒng)完成預(yù)定任務(wù)就成了首要問(wèn)題，國(guó)內(nèi)外學(xué)者在這方面開(kāi)展了大量研究［2-8］。

本文通過(guò)選取合適的位置和姿態(tài)誤差信息，給出適用于自由漂浮空間機(jī)器人的閉環(huán)形式運(yùn)動(dòng)控制方法，最后利用系統(tǒng)冗余特性實(shí)現(xiàn)了本體姿態(tài)零擾動(dòng)控制。

1 運(yùn)動(dòng)學(xué)模型

圖1給出了自由漂浮空間機(jī)器人的示意圖。圖中，B0為機(jī)器人本體;B1～Bn為機(jī)械臂連桿;Ji(i=1，2，…，n)為連接部件。

假設(shè):系統(tǒng)各組成部分均為剛體，剛體間的連接均為單自由度轉(zhuǎn)動(dòng)鉸;整個(gè)系統(tǒng)不受任何外力及外力矩作用;系統(tǒng)初始的線動(dòng)量及角動(dòng)量均為零?；谏鲜黾僭O(shè)，易知系統(tǒng)的質(zhì)心位置在慣性空間中保持不變，可以作為慣性系OIxIyIzI的原點(diǎn)OI，各連體坐標(biāo)系Oixiyizi的原點(diǎn)取在各剛體的質(zhì)心Ci處，坐標(biāo)軸與剛體的慣性主軸方向一致。

圖1 自由漂浮空間機(jī)器人示意圖Fig.1 Free－floating space robot

本文采用 Roberson－Wittenburg 方法［9］推導(dǎo)自由漂浮空間機(jī)器人的運(yùn)動(dòng)學(xué)模型。Roberson－Wittenburg方法引入圖論對(duì)剛體的連接關(guān)系進(jìn)行了描述，與以往的推導(dǎo)方法［6-8，10］相比，具有形式整潔、易于程式化和適用廣泛的優(yōu)點(diǎn)。

引入增廣體矢量 bji(j=0，…，n;i=0，…，n)，當(dāng)j=i時(shí)，它是由增廣體質(zhì)心指向該剛體質(zhì)心的矢量;當(dāng)j≠i時(shí)，它是由增廣體質(zhì)心指向該剛體上鉸接點(diǎn)的矢量，直接或間接地指向剛體i。進(jìn)而，剛體Bi(i=0，…，n)在慣性系下的質(zhì)心位置ρi可以用如下緊湊的形式表示:

剛體Bi質(zhì)心的速度:

末端執(zhí)行器的位置矢量rE可以表示為:

對(duì)式(3)兩邊求導(dǎo)，并結(jié)合式(2)可得:

其中:

角速度關(guān)系:

式中:Hi為轉(zhuǎn)軸方向矢量矩陣，且:

因?yàn)棣谽=ωn，則根據(jù)式(7)可得空間機(jī)器人末端執(zhí)行器的角速度為:

進(jìn)而將式(6)帶入式(4)，可得末端執(zhí)行器的線速度為:

記3×3單位陣為E，則可以將式(8)與式(9)寫成更為緊湊的形式:

式(10)給出了末端執(zhí)行器的運(yùn)動(dòng)與本體及關(guān)節(jié)運(yùn)動(dòng)的關(guān)系。

考慮假設(shè)，分別根據(jù)線動(dòng)量和角動(dòng)量守恒有:

根據(jù)式(2)，上式中的第二項(xiàng)可以表示為:

式(13)中雙叉乘可以寫為張量與矢量乘積的形式:

則式(12)可以表示為:

將式(6)帶入式(15)，可得:

式(17)給出了本體的運(yùn)動(dòng)與關(guān)節(jié)運(yùn)動(dòng)的關(guān)系，將其帶入式(10)，便可得到末端執(zhí)行器的運(yùn)動(dòng)與關(guān)節(jié)運(yùn)動(dòng)的關(guān)系:

2 閉環(huán)形式的逆運(yùn)動(dòng)學(xué)控制

物體的位移可以用位置矢量r描述，而姿態(tài)的描述方法卻不唯一。修正羅德里格斯參數(shù)(Modified Rodrigues Parameters，MRP)因其與其他姿態(tài)描述方式相比具有無(wú)冗余、無(wú)奇異的優(yōu)點(diǎn)，廣泛應(yīng)用于姿態(tài)估計(jì)、姿態(tài)控制領(lǐng)域［11-12］。

根據(jù)歐拉有限轉(zhuǎn)動(dòng)定理，MRP定義為:

MRP表示的方向余弦矩陣為:

分別用rd(t)和re(q)表示末端執(zhí)行器的期望位置和實(shí)際位置，定義末端執(zhí)行器的位置誤差為:

分別用σd(t)，σe(q)表示末端執(zhí)行器期望姿態(tài)和實(shí)際姿態(tài)，其對(duì)應(yīng)的旋轉(zhuǎn)矩陣分別為Rd和Re，則相對(duì)姿態(tài)旋轉(zhuǎn)矩陣為:

可以求得與Rde對(duì)應(yīng)的σde為:選取σde作為姿態(tài)誤差，構(gòu)造閉環(huán)形式的逆運(yùn)動(dòng)學(xué)方程:

定理1:對(duì)于系統(tǒng)式(18)，控制規(guī)律式(24)能夠保證閉環(huán)系統(tǒng)的輸出漸近跟蹤參考輸入。

證明:將式(24)帶入式(18)，則系統(tǒng)的控制誤差為:

式中:Δωde=ωd－ωe。易知位置誤差Δrde是指數(shù)收斂的。

構(gòu)造如下的正定Lyapunov函數(shù):

兩邊對(duì)時(shí)間求導(dǎo)，結(jié)合式(26)和式(23)，可得:

可見(jiàn)姿態(tài)誤差σde也是收斂的，故系統(tǒng)式(18)在控制規(guī)律式(24)下，能夠漸近跟蹤參考輸入。

3 本體姿態(tài)零擾動(dòng)控制

設(shè)ms和me分別代表空間機(jī)器人本體和末端執(zhí)行器的任務(wù)空間維數(shù)，根據(jù)文獻(xiàn)［7］對(duì)于空間機(jī)器人冗余性的分析，當(dāng)關(guān)節(jié)數(shù)n≥ms+me時(shí)，機(jī)械臂的運(yùn)動(dòng)可以協(xié)調(diào)末端執(zhí)行器和本體的運(yùn)動(dòng)。引入任務(wù)優(yōu)先級(jí)的方法［13］，基于以上閉環(huán)控制形式的討論，對(duì)于同時(shí)存在末端執(zhí)行器任務(wù)和本體任務(wù)約束的情形，關(guān)節(jié)角速率的計(jì)算如下:

自由漂浮空間機(jī)器人有時(shí)為完成復(fù)雜的空間作業(yè)，保證本體姿態(tài)的零擾動(dòng)是非常必要的，即要求在機(jī)械臂的運(yùn)動(dòng)過(guò)程中，不對(duì)本體的姿態(tài)產(chǎn)生影響根據(jù)上述討論，此時(shí)的關(guān)節(jié)角速率可按下式求解:

4 仿真結(jié)果及分析

為了驗(yàn)證上述閉環(huán)逆運(yùn)動(dòng)學(xué)算法的控制效果，針對(duì)1個(gè)具有7關(guān)節(jié)機(jī)械臂的自由漂浮機(jī)器人設(shè)計(jì)數(shù)值仿真，設(shè)機(jī)械臂編號(hào)分別為i0～i7，質(zhì)量分別為500.00，20.00，20.00，40.00，40.00，20.00，20.00，40.00 kg;長(zhǎng)度分別為 6.00，0.35，0.35，4.00，4.00，0.35，0.35，1.20 m。機(jī)械臂構(gòu)型參考加拿大2號(hào)機(jī)械臂［14］。 機(jī) 械 臂 的 轉(zhuǎn) 動(dòng) 慣 量 及 D－H(Denavit－Hartenberg)參數(shù)如表1所示。

表1 機(jī)械臂的轉(zhuǎn)動(dòng)慣量及D－H參數(shù)Table 1 Moment of inertia and the D-H parameters of the robot arm

4.1 初始及目標(biāo)狀態(tài)

假設(shè)初始本體的體坐標(biāo)系與慣性系平行，即σoi=0，末端執(zhí)行器的初始狀態(tài):

任務(wù)期望的目標(biāo)狀態(tài)為:

4.2 期望軌跡

考慮到滿足初始和末端時(shí)刻速度與加速度均為0，采用五次多項(xiàng)式插值生成期望軌跡。任務(wù)時(shí)間設(shè)計(jì)為2 s。

4.3 仿真結(jié)果

利用Matlab(R2008a)進(jìn)行數(shù)值仿真，積分采用定步長(zhǎng)四階龍格-庫(kù)塔算法，積分步長(zhǎng)h=0.002 s。

算例1僅存在末端執(zhí)行器任務(wù)約束，即控制末端執(zhí)行器由初始狀態(tài)按期望軌跡到達(dá)目標(biāo)狀態(tài)，而不考慮其對(duì)本體姿態(tài)的影響，按式(24)計(jì)算，增益矩陣KPr=10E，KPo=20E。為了考察初始誤差對(duì)控制的影響，設(shè)末端執(zhí)行器初始位姿分別存在10－2，10－3量級(jí)的隨機(jī)誤差，仿真結(jié)果如圖2～圖5所示。

圖2給出了各關(guān)節(jié)角速率隨時(shí)間變化的規(guī)律，可以看出控制輸入光滑平緩。圖3和圖4分別給出了末端執(zhí)行器的位置和姿態(tài)誤差，可以看出，在存在初始誤差的情況下，誤差隨時(shí)間是收斂的，且在0.6 s誤差已經(jīng)減小到10－4量級(jí)。關(guān)節(jié)運(yùn)動(dòng)對(duì)本體的姿態(tài)干擾如圖5所示，可以看出關(guān)節(jié)運(yùn)動(dòng)對(duì)本體姿態(tài)產(chǎn)生了擾動(dòng)。

算例2同時(shí)存在末端執(zhí)行器和本體任務(wù)約束，即實(shí)現(xiàn)末端執(zhí)行器任務(wù)的同時(shí)保證了本體姿態(tài)零擾動(dòng)。為了滿足冗余性的要求，末端執(zhí)行器的任務(wù)僅考慮姿態(tài)約束，按式(30)計(jì)算，假設(shè)初始無(wú)誤差，增益矩陣 KP=100E，KC=40E，結(jié)果如圖 6～圖 8所示。

從圖6可以看出，控制輸入仍然能夠保證光滑平緩。圖7給出了末端執(zhí)行器的姿態(tài)控制誤差，由于本體姿態(tài)任務(wù)的存在，其誤差為10－3量級(jí)，相比于算例1有所增大。從圖8的關(guān)節(jié)運(yùn)動(dòng)對(duì)本體的姿態(tài)干擾情況可以看出，干擾在10－6量級(jí)以下，可以看作姿態(tài)零擾動(dòng)。

圖2 關(guān)節(jié)角速率Fig.2 Joint velocity

圖3 末端執(zhí)行器的位置誤差Fig.3 Position errors of the end－effector

圖4 末端執(zhí)行器的姿態(tài)誤差Fig.4 Attitude errors of the end－effector

圖5 本體姿態(tài)Fig.5 Base attitude

圖6 關(guān)節(jié)角速率Fig.6 Joint velocity

圖7 末端執(zhí)行器的姿態(tài)誤差Fig.7 Attitude errors of the end－effector

圖8 本體姿態(tài)Fig.8 Base attitude

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

本文針對(duì)自由漂浮空間機(jī)器人的控制特點(diǎn)，通過(guò)引入適當(dāng)?shù)姆答佇畔ⅲo出了一種閉環(huán)形式的逆運(yùn)動(dòng)學(xué)控制算法。在此基礎(chǔ)上，討論了利用系統(tǒng)的冗余特性實(shí)現(xiàn)本體姿態(tài)零擾動(dòng)的方法。通過(guò)仿真表明，該方法可以有效解決自由漂浮機(jī)器人運(yùn)動(dòng)控制問(wèn)題，并且可以用于本體和末端執(zhí)行器任務(wù)約束同時(shí)存在的情形，具有易于實(shí)現(xiàn)、穩(wěn)定性好的優(yōu)點(diǎn)。

［1］ 張文輝，葉曉平，季曉明，等.國(guó)內(nèi)外空間機(jī)器人技術(shù)發(fā)展綜述［J］.飛行力學(xué)，2013，31(3):198－202.

［2］ Vafa Z，Dubowsky S.On the dynamics of space manipulators using the virtual manipulator approach，with application to path planning［J］.Journal of Astronautica Sciences 1990，38(4):441－472.

［3］ 梁斌，劉良棟，李庚田，等.空間機(jī)器人的動(dòng)力學(xué)等價(jià)臂［J］.自動(dòng)化學(xué)報(bào)，1998，24(6):761－767.

［4］ Nenchev D，Yoshida K，Umetani Y.Analysis，design and control of free－flying space robots using fixed－attitude－restricted Jacobian Matrix［C］∥Proceeding of the Fifth International Symposiumon Robotics Research.MIT Press Cambridge，1990:251－258.

［5］ Liu X D，Baoyin H X，Ma X R.Optimal path planning of redundant free－floating revolute－jointed space manipulators with seven links［J］.Multibody Systems Dynamic，2013，29(1):41－56.

［6］ Umetani Y，Yoshida K.Resolved motion rate control of space manipulators with generalized Jacobian matrix［J］.IEEE Transactions on Robotics and Automation，1989，5(3):303－314.

［7］ Nenchev D，Umetani Y，Yoshida K.Analysis of a redundant free－flying spacecraft/manipulator system［J］.IEEE Transactions on Robotics and Automation，1992，8(1):1－6.

［8］ Siciliano B.A closed－loop inverse kinematic scheme for online joint－based robot control［J］.Robotica，1990，8(3):231－243.

［9］ Wittenburg J.Dynamics of systems of rigid bodies［M］.Teubner，Stuttgart，1977:1－224.

［10］Kreutz－Delgado K，Long M，Seraji H.Kinematic analysis of 7 DOF manipulators［J］.The International Journal of Robotics Research，1992，11(5):469－481.

［11］ Akella M R.Rigid body attitude tracking without angular velocity feedback［J］.Systems and Control Letters，2001，42(4):321－326.

［12］ Gohary A E，Tawfik T E S.Attitude stabilization using modified Rodriguez parameters without angular velocity measurements［J］.World Journal of Mechanics，2011，1(2):57－63.

［13］ Nakamura Y，Hanafusa H，Yoshikawa T.Task－priority based redundancy control of robot manipulators［J］.International Journal of Robotics Research，1987，6(2):3－15.

［14］ Gibbs G，Sachdev S.Canada and the international space station program:overview and status［J］.Acta Astronaut，2002，51(1－9):591－600.