自由漂浮空間機器人逆運動學閉環(huán)控制

張傳海，袁建平

(西北工業(yè)大學航天飛行動力學技術(shù)國家重點實驗室，陜西西安710072)

0 引言

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

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

1 運動學模型

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

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

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

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

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

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

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

對式(3)兩邊求導，并結(jié)合式(2)可得:

其中:

角速度關(guān)系:

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

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

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

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

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

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

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

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

則式(12)可以表示為:

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

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

2 閉環(huán)形式的逆運動學控制

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

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

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

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

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

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

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

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

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

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

兩邊對時間求導，結(jié)合式(26)和式(23)，可得:

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

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

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

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

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

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

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

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

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

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

4.2 期望軌跡

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

4.3 仿真結(jié)果

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

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

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

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

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

圖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é)束語

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

［1］ 張文輝，葉曉平，季曉明，等.國內(nèi)外空間機器人技術(shù)發(fā)展綜述［J］.飛行力學，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］.自動化學報，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.