張 軍，胡海霞

（1.北京控制工程研究所，北京100190；2.空間智能控制技術(shù)國家級(jí)重點(diǎn)實(shí)驗(yàn)室，北京100190）

空間機(jī)器人動(dòng)力學(xué)建模與模型驗(yàn)證*

張 軍1，2，胡海霞1，2

（1.北京控制工程研究所，北京100190；2.空間智能控制技術(shù)國家級(jí)重點(diǎn)實(shí)驗(yàn)室，北京100190）

建立帶任意節(jié)機(jī)械臂的空間機(jī)器人動(dòng)力學(xué)模型，并驗(yàn)證了模型的正確性.采用Kane方程建立了各體間轉(zhuǎn)鉸具有三自由度的空間多體系統(tǒng)的動(dòng)力學(xué)模型，利用投影算子將所建立的模型退化為各節(jié)機(jī)械臂間具有單自由度轉(zhuǎn)鉸連接的空間機(jī)器人動(dòng)力學(xué)模型，依據(jù)動(dòng)力學(xué)三大基本定理，即動(dòng)量定理、動(dòng)量矩定理、動(dòng)能定理設(shè)計(jì)了模型驗(yàn)證方案，通過數(shù)學(xué)仿真驗(yàn)證了所建立模型的正確性.

空間機(jī)器人；動(dòng)力學(xué)建模；Kane方程

符號(hào)表

上標(biāo)× 列陣的反對(duì)稱陣

t 當(dāng)前時(shí)間

x｜t＝0變量x的初值

n 機(jī)械臂的節(jié)數(shù)

N 描述系統(tǒng)自由度的變量的個(gè)數(shù)

i 空間機(jī)器人各體的編號(hào)，取0時(shí)代表基座

p 廣義坐標(biāo)變量的編號(hào)，取值1，2，…，N

Fe慣性坐標(biāo)系oexeyeze

Fii體的本體坐標(biāo)系oixiyizi，oi取i體與第i－1體的鉸接處，i＝0時(shí)oi為基座上任意點(diǎn)

Eii體的動(dòng)能

mii體的質(zhì)量

e Fe的3軸單位方向矢量

biFi的3軸單位方向矢量

Ae，i從Fi到Fe的坐標(biāo)轉(zhuǎn)換矩陣

Aij從Fj體到Fi的坐標(biāo)轉(zhuǎn)換矩陣

qi單自由度轉(zhuǎn)鉸連接情況下第i節(jié)機(jī)械臂相對(duì)i－1節(jié)機(jī)械臂的轉(zhuǎn)角

ii

Sijj體相對(duì)oi的靜矩

Jijj體相對(duì)oi的轉(zhuǎn)動(dòng)慣量系統(tǒng)所受總的外力

M→ii～n體所受外力相對(duì)oi的力矩與i～n體所受外力矩之和

空間機(jī)器人由衛(wèi)星基座及搭載的機(jī)械臂組成，主要用于在軌服務(wù)，包括自主交會(huì)對(duì)接、失效衛(wèi)星的搶救及零部件安裝與更換等.隨著國際空間站［1］、ETS-Ⅶ［2］、XSS-10［3］、XSS-11［4］、“軌道快車”［5］的相繼發(fā)射，空間機(jī)器人正成為人類開發(fā)利用太空資源的重要手段.相對(duì)于地面固定基座的機(jī)器人，空間機(jī)器人具有漂浮基座，衛(wèi)星本體與機(jī)械臂的運(yùn)動(dòng)相互耦合，其動(dòng)力學(xué)特性更復(fù)雜［6］.對(duì)空間機(jī)器人進(jìn)行動(dòng)力學(xué)建模并確保模型的正確是控制系統(tǒng)設(shè)計(jì)與方案設(shè)計(jì)的基礎(chǔ).

盡管已有的成熟商業(yè)化軟件（如ADAMS）能進(jìn)行多體系統(tǒng)的建模，并在前期工作中可用于系統(tǒng)的初步動(dòng)力學(xué)分析，但為進(jìn)行控制系統(tǒng)的設(shè)計(jì)及詳細(xì)的動(dòng)力學(xué)分析，建立空間機(jī)器人模型的顯式無疑是基本的，目前對(duì)空間機(jī)器人建模的主要方法包括Lagrange方程法［7］、虛擬機(jī)械臂法［8］、等價(jià)機(jī)械臂法［9］、廣義Jacobian矩陣法等［10］.采用Lagrange方程建模，需要對(duì)Lagrange函數(shù)進(jìn)行二階求導(dǎo)，計(jì)算非常復(fù)雜；采用虛擬機(jī)械臂方法建模時(shí)，首先假設(shè)在系統(tǒng)質(zhì)心相對(duì)慣性系鉸接，然后采用固定基座機(jī)器人的建模方法建模；等價(jià)機(jī)械臂繼承虛擬機(jī)械臂的建模方法，將空間機(jī)器人的模型與地面機(jī)器人的模型進(jìn)行等價(jià)轉(zhuǎn)換；廣義Jacobian矩陣法本質(zhì)上采用的是New ton-Euler法.上述方法都是將系統(tǒng)的質(zhì)心平動(dòng)與轉(zhuǎn)動(dòng)解耦，而空間機(jī)器人實(shí)際上是變慣量系統(tǒng)，物理上并不存在與系統(tǒng)質(zhì)心對(duì)應(yīng)的確定的點(diǎn).實(shí)際任務(wù)操縱及控制系統(tǒng)設(shè)計(jì)時(shí)，人們常要關(guān)心系統(tǒng)上確定位置的運(yùn)動(dòng)狀態(tài)，如交會(huì)對(duì)接過程中就要考慮衛(wèi)星基座上交會(huì)測(cè)量相機(jī)的運(yùn)動(dòng)狀態(tài)，如果采用上述方法建模，則需要將系統(tǒng)質(zhì)心的位置變化轉(zhuǎn)化為相機(jī)的位置變化，然后再設(shè)計(jì)相應(yīng)的控制策略.

與上述方法不同，本文采用Kane方程建立空間機(jī)器人的動(dòng)力學(xué)模型，盡管各種多體動(dòng)力學(xué)建模方法都很復(fù)雜，但采用Kane方程建模，過程煩而不難，并且Kane方程采用衛(wèi)星基座上任意一點(diǎn)（而非系統(tǒng)質(zhì)心）的平動(dòng)及各體間的相對(duì)轉(zhuǎn)動(dòng)來描述整個(gè)系統(tǒng)的運(yùn)動(dòng)狀態(tài)，物理意義明確，所建立的動(dòng)力學(xué)模型可直接用于控制系統(tǒng)的設(shè)計(jì).

此外，從工程的角度，針對(duì)目前文獻(xiàn)都沒有對(duì)所建立的模型進(jìn)行驗(yàn)證的問題，本文利用動(dòng)力學(xué)三大基本定理，即動(dòng)量定理、動(dòng)量矩定理、動(dòng)能定理，對(duì)所建立的模型設(shè)計(jì)了驗(yàn)證方案，通過數(shù)學(xué)仿真驗(yàn)證了所建立模型的正確性.

1 基于Kane方程的動(dòng)力學(xué)建模

從結(jié)構(gòu)上來看，空間機(jī)器人是具有漂浮基的多體樹形系統(tǒng)，其拓?fù)浣Y(jié)構(gòu)如圖1所示.不失一般性，這里只考慮單只機(jī)械臂的情況，含多只機(jī)械臂的動(dòng)力學(xué)模型可以很容易地由單只機(jī)械臂的情況演化得到.

圖1 空間機(jī)器人的樹形拓?fù)浣Y(jié)構(gòu)

1.1 機(jī)械臂的運(yùn)動(dòng)描述

dmi的加速度為

1.2 廣義坐標(biāo)的選取與系數(shù)矩陣的確定

為了用Kane方程對(duì)空間機(jī)器人進(jìn)行建模，將系統(tǒng)中各點(diǎn)的速度表示成如下形式：

式中，Wi為系統(tǒng)的廣義速度，為相對(duì)Wi的系數(shù)矩陣表示其元素為矢量.

對(duì)空間機(jī)械臂取廣義速度

根據(jù)式（1）和式（3）可得

1.3 動(dòng)力學(xué)方程的建立

暫不考慮系統(tǒng)中的撓性部件，則系統(tǒng)彈性勢(shì)能為0，由Kane方程知，相對(duì)第p個(gè)廣義坐標(biāo)的動(dòng)力學(xué)方程可由下式得到：

式中，fen，p為廣義慣性力，fex，p為廣義外力為作用在i體上的分布式系統(tǒng)外力［11］.

對(duì)于空間機(jī)器人，不考慮空間環(huán)境干擾力矩時(shí)，系統(tǒng)所受力與力矩不是分布式的，為集中力或集中力矩，不需用上面式子中的積分就可直接得到各階廣義外力.則由式（6）和式（7）可得到關(guān)于、ω0、ωi的動(dòng)力學(xué)方程分別為

將式（2）和式（5）代入式（8）～（10），經(jīng)過推導(dǎo)可得到關(guān)于、ω0、ωi的動(dòng)力學(xué)方程，進(jìn)而得到如下形式的系統(tǒng)動(dòng)力學(xué)方程：

式中，

式（11）建立的動(dòng)力學(xué)模型中各體間具有3個(gè)轉(zhuǎn)動(dòng)自由度，而一般空間機(jī)器人各節(jié)機(jī)械臂只具有單自由度.不失一般性，這里假設(shè)該單自由度沿Fi的zi方向，此時(shí)空間機(jī)器人動(dòng)力學(xué)模型可由式（11）經(jīng)過投影算子變形得到

式中，

P為Fi中在zi上的投影算子.

2 模型驗(yàn)證方案

理論上采用成熟方法建立的動(dòng)力學(xué)模型都是正確的，但由于空間機(jī)器人動(dòng)力學(xué)模型十分復(fù)雜，所以必須對(duì)所建立的動(dòng)力學(xué)模型進(jìn)行驗(yàn)證，實(shí)踐也證明，未進(jìn)行驗(yàn)證和修正的模型常常存在錯(cuò)誤，而其導(dǎo)致的后果則是災(zāi)難性的.對(duì)模型的驗(yàn)證可采用兩種方法：一種是與采用另外方法（如商業(yè)化軟件ADAMS等）建立的模型進(jìn)行仿真結(jié)果的比對(duì)；另一種是采用物理定律進(jìn)行驗(yàn)證.采用第一種方法時(shí)，首先需要另外建立一套模型，而在相同的輸入與參數(shù)設(shè)置下，一旦兩套模型輸出不一致，進(jìn)行問題定位就非常困難，因此是一種理論上簡(jiǎn)單而實(shí)際操作比較困難的方法.

本文采用第二種方法，利用動(dòng)力學(xué)系統(tǒng)三大基本定理，即動(dòng)量定理、動(dòng)量矩定理、動(dòng)能定理，對(duì)模型進(jìn)行驗(yàn)證，只要所建立的模型在任意參數(shù)設(shè)置與任意輸入下，始終滿足三大基本定理，就認(rèn)為所建立的模型是正確的.不同的參數(shù)與不同的輸入可以暴露空間機(jī)器人各子體的運(yùn)動(dòng)特性，因此驗(yàn)證方案均針對(duì)整個(gè)空間機(jī)器人系統(tǒng)設(shè)計(jì).

2.1 動(dòng)量定理驗(yàn)證方案

動(dòng)量定理描述的是系統(tǒng)質(zhì)心的線運(yùn)動(dòng)規(guī)律.對(duì)于任意一個(gè)動(dòng)力學(xué)系統(tǒng)，其質(zhì)心的運(yùn)動(dòng)規(guī)律由所受的外力決定，與系統(tǒng)內(nèi)力及內(nèi)外力矩?zé)o關(guān)，其運(yùn)動(dòng)規(guī)律滿足牛頓第二定律.

定義j體相對(duì)oi的靜矩Sij：

可得系統(tǒng)質(zhì)心在F0中坐標(biāo)為

則系統(tǒng)質(zhì)心在Fe中的坐標(biāo)可表示為

由牛頓第二定律可知

2.2 動(dòng)量矩定理驗(yàn)證方案

i體的質(zhì)心相對(duì)Fe的線速度在Fe中的坐標(biāo)可表示為

i體的質(zhì)心相對(duì)系統(tǒng)質(zhì)心的矢量在F0中的坐標(biāo)可表示為

則系統(tǒng)總的角動(dòng)量為

式中：Mom1為系統(tǒng)各體由于平動(dòng)導(dǎo)致的相對(duì)系統(tǒng)質(zhì)心的角動(dòng)量；Mom2為各體由于轉(zhuǎn)動(dòng)導(dǎo)致的角動(dòng)量.

j體上外作用力相對(duì)oi的矢量坐標(biāo)為

相應(yīng)外力在Fi中的坐標(biāo)為

則動(dòng)力學(xué)方程中與各體相應(yīng)的外力矩可表示為

系統(tǒng)所受相對(duì)系統(tǒng)質(zhì)心的總的外力矩可表示為

由動(dòng)量矩定理，系統(tǒng)應(yīng)始終存在如下等式關(guān)系：

積分號(hào)上標(biāo)tnow表示當(dāng)前時(shí)間。

2.3 動(dòng)能定理驗(yàn)證方案

系統(tǒng)總的動(dòng)能為

式中

外力與外力矩作用的功為

由動(dòng)能定理，系統(tǒng)應(yīng)始終存在如下等式關(guān)系：

3 數(shù)學(xué)仿真

針對(duì)所建立的空間機(jī)器人動(dòng)力學(xué)模型與驗(yàn)證方案，采用Matlab／Simulink進(jìn)行數(shù)學(xué)編程，建立空間機(jī)器人仿真系統(tǒng)，下面給出一組仿真算例.

仿真中假設(shè)空間機(jī)器人有6節(jié)機(jī)械臂，此時(shí)機(jī)械臂末端可以以任意姿態(tài)定位在空間中任意點(diǎn).

一般來講，空間機(jī)器人的機(jī)械臂質(zhì)量及慣量值要遠(yuǎn)小于基座本體，但在進(jìn)行模型驗(yàn)證時(shí)，為了放大機(jī)械臂的動(dòng)力學(xué)影響，仿真中將機(jī)械臂的質(zhì)量及慣量參數(shù)設(shè)成與基座的同等量級(jí)，其中各體相對(duì)其質(zhì)心的轉(zhuǎn)動(dòng)慣量及質(zhì)量取為Jc5＝1．2Jc0，Jc6＝0．8Jc0，m0＝440kg，m1＝350kg，m2＝300kg，m3＝280kg，m4＝260kg，m5＝320kg，m6＝340kg.下面的仿真中，為了具有一般性與代表性，在空間機(jī)器人各體上都施加外力與外力矩，具體值由Matlab隨機(jī)生成，記Me＝，rf，取值：

在10s≤t≤30s時(shí)，外力與外力矩取式（31）中各值；在40s≤t≤60s時(shí)，外力與外力矩反向，但值保持不變；其余時(shí)間內(nèi)外力與外力矩取0.空間機(jī)器人初始狀態(tài)為在慣性空間中靜止，各變量初始值取0.圖2為星體基座的三軸角速度與歐拉角，從圖中可以看到，前10s基座處于靜止?fàn)顟B(tài)，從第10秒開始，基座出現(xiàn)非零角速度，至到仿真結(jié)束；圖3為6節(jié)機(jī)械臂的6節(jié)關(guān)節(jié)角，也是從第10秒開始出現(xiàn)運(yùn)動(dòng)，至到仿真結(jié)束；圖4中給出的是空間機(jī)器人系統(tǒng)質(zhì)心的速度與受到的總外力曲線，由于外力在10s≤t≤30s及40s≤t≤60s中都是常值，因此系統(tǒng)質(zhì)心在慣性空間中先后分別作勻加速與勻減速運(yùn)動(dòng)，60s后系統(tǒng)質(zhì)心重新處于靜止?fàn)顟B(tài)，其運(yùn)動(dòng)規(guī)律符合動(dòng)量定理；圖5（a）中給出空間機(jī)器人的角動(dòng)量曲線，圖5（b）中給出的曲線為將仿真中外力撤除后，即只作用Me時(shí)空間機(jī)器人的角動(dòng)量曲線，圖5（a）中第60秒后，系統(tǒng)存在殘余角動(dòng)量，而圖5（b）中第60秒后，系統(tǒng)角動(dòng)量完全為0，因?yàn)樵趹T性空間中前后兩段時(shí)間外力矩作用的大小相等、方向相反，則最后系統(tǒng)的總角動(dòng)量必然為0；圖6則為與圖5相對(duì)應(yīng)的系統(tǒng)的總動(dòng)能；在圖7和圖8中給出了在Simulink下設(shè)置不同的相對(duì)容許誤差，容許誤差為式（27）與式（31）中等號(hào)左邊項(xiàng)減去右邊項(xiàng)后的值，圖（a）、（b）、（c）分別將容許誤差設(shè)為10－9、10－12、10－15，從仿真曲線可以看到，式（27）與式（31）左右兩項(xiàng)相減后的殘差隨著設(shè)置的容許誤差的減小而減小，因此可認(rèn)為仿真中式（27）與式（31）的左右兩邊是完全相等的，相減后的殘差由仿真誤差引起，即動(dòng)量矩定理與動(dòng)能定理得到了驗(yàn)證.

圖2 基座3軸角速度與歐拉角

圖3 6節(jié)機(jī)械臂的相對(duì)轉(zhuǎn)角

圖4 系統(tǒng)的質(zhì)心速度與所受總的外力

圖5 系統(tǒng)的3軸總角動(dòng)量

圖6 系統(tǒng)的3軸總動(dòng)能

除了上述仿真算例，還采用其他輸入條件與參數(shù)進(jìn)行了各種數(shù)學(xué)仿真，也都得到了與上述一致的仿真結(jié)果，證明了所建立的模型都能滿足所設(shè)計(jì)的模型驗(yàn)證方案.條件成熟后，還采用相同的輸入與初始條件，與NASDA進(jìn)行ETS-7方案論證時(shí)采用的模型進(jìn)行了仿真結(jié)果比對(duì)，結(jié)果完全一致，也進(jìn)一步證實(shí)了本文所建模型及驗(yàn)證方案的正確性.

4 結(jié) 論

采用Kane方程建立了空間多體系統(tǒng)的動(dòng)力學(xué)模型，利用投影算子將所建立的動(dòng)力學(xué)模型退化為采用單自由度鉸接連接的空間機(jī)器人動(dòng)力學(xué)模型，接著依據(jù)動(dòng)力學(xué)三大基本定理，即動(dòng)量定理、動(dòng)量矩定理、動(dòng)能定理設(shè)計(jì)了模型的驗(yàn)證方案，通過仿真驗(yàn)證了所建立的動(dòng)力學(xué)模型的正確性及驗(yàn)證方案的有效性.本文建立的空間機(jī)器人的動(dòng)力學(xué)模型采用衛(wèi)星基座上固定點(diǎn)的平動(dòng)及相鄰體間的轉(zhuǎn)動(dòng)運(yùn)動(dòng)來描述系統(tǒng)的狀態(tài)，物理意義明確，特別適合于空間機(jī)器人的任務(wù)規(guī)劃及控制系統(tǒng)的設(shè)計(jì).

圖7 不同容許誤差下的角動(dòng)量仿真誤差

圖8 不同容許誤差下的動(dòng)能仿真誤差

［1］ Mah H W，Modi V J.A relatively general formulation for studying dynamics of the space station based arms with applications［R］.AIAA-88-0674，1988

［2］ Oda M.On the Dynamics and control of ETS-7 satellite and its robot arm［C］.The IEEE／RSJ／GI International Conference on Intelligent Robots and Systems，Tokyo，Japan，1994

［3］ Barnhart D A，Hunter R C，Weston A R，et al.XSS-10 micro-satellite demonstration［R］.AIAA-98-5298，1998

［4］ 聞新，王秀麗，鄧寶忠.美國試驗(yàn)小衛(wèi)星XSS-11系統(tǒng)［J］.中國航天，2006（7）：22-25

［5］ Shanes，Pejmun M.Orbital express capture system：concept to reality［C］.SPIE，Bellingham，WA，USA，2004

［6］ Dubowskys，Papadopoulos E.The kinematics，dynamics and control of free-flying and free-floating space robotics systems［J］.IEEE Transactions on Robotics and Automation，1993，9（5）：531-543

［7］ Papadopoulos E，Dubowskys.On the nature of control algorithms for free-floating space manipulators［J］.IEEE Transactions Robotics and Automation，1991，7（5）：750-758

［8］ Vafa Z，Dubowskys.On the Dynamics of space manipulator using the virtual manipulator with application to path planning［J］.The Journal of the Astronautical Science，1990，38（4）：441-472

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

［10］ 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

［11］ 黃文虎，邵成勛.多柔體系統(tǒng)動(dòng)力學(xué)［M］.北京：科學(xué)出版社，1996：53

［12］ 張軍.空間機(jī)器人動(dòng)力學(xué)建模與仿真驗(yàn)證［R］.北京控制工程研究所，2007

Dynamics Modeling and Simulation Verification for Space Robot

ZHANG Jun1，2，HU Haixia1，2
（1.Beijing Institute of Control Engineering，Beijing 100190，China；2.National Key Laboratory of Science and Technology on Space Intelligent Control，Beijing 100190，China）

The dynamicsmodel of a free-flying space robotwith arbitrary joints connected manipulator is established，and verified by numerical simulation.Firstly，the model of a space multi-body system with three degrees of freedom in adjacent bodies is established using Kane equation.Then，projective operators are used to transform general equations into the dynam icsmodel of a space robot with one degree of freedom between the bodies of itsmanipulator.Three fundamental dynamical theorems，i.e.momentum theorem，momentum moment theorem，kinetic energy theorem are used to design the algorithm to verify the dynamicsmodel.Finally，numerical simulation verifies the dynamicsmodel of the space robot.

space robot；dynamicsmodeling；Kane equation

*國家自然科學(xué)基金（10872028）資助項(xiàng)目.

2009-04-07

張 軍（1980—），男，湖北人，工程師，研究方向?yàn)榭臻g站、空間機(jī)器人及在軌服務(wù)GNC技術(shù)（e-mail：zhangjun10＠gmail.com）.

TP24

A

1674-1579（2009）06-0006-07