王一全，王 敏，楊 聞，王興龍

（中國(guó)空間技術(shù)研究院 通信衛(wèi)星事業(yè)部，北京 100094）

0 引言

隨著世界各國(guó)空間利用的廣度和深度逐漸拓展，對(duì)航天器的研制需求不斷提高。一方面，運(yùn)載火箭整流罩包絡(luò)和發(fā)射重量的限制，很難滿(mǎn)足超大型衛(wèi)星的研制需要；另一方面，傳統(tǒng)衛(wèi)星從發(fā)射到壽命結(jié)束的一次性工作方式，已難適應(yīng)對(duì)航天器的靈活性和發(fā)射效益要求。在這種情況下，空間在軌組裝衛(wèi)星系統(tǒng)成為未來(lái)空間系統(tǒng)發(fā)展的一個(gè)重要方向。相對(duì)于傳統(tǒng)衛(wèi)星平臺(tái)而言，在軌組裝衛(wèi)星系統(tǒng)的主要特點(diǎn)是可重構(gòu)，包括整星構(gòu)型的重構(gòu)、功能的重構(gòu)以及業(yè)務(wù)重構(gòu)等。空間機(jī)械臂是航天器實(shí)現(xiàn)在軌重組技術(shù)的關(guān)鍵執(zhí)行部件，用于實(shí)現(xiàn)目標(biāo)部件的捕捉、轉(zhuǎn)移和釋放等功能，同時(shí)也可用于空間碎片清理以及空間環(huán)境探測(cè)。但是在空間微重力環(huán)境下，空間機(jī)械臂在執(zhí)行任務(wù)時(shí)會(huì)對(duì)星本體產(chǎn)生耦合擾動(dòng)，且受到空間環(huán)境中不確定因素的干擾。國(guó)內(nèi)外學(xué)者對(duì)空間機(jī)械臂模型等進(jìn)行了大量研究[1-6]，但給出的具體動(dòng)力學(xué)模型以及相關(guān)實(shí)驗(yàn)結(jié)論較少。

本文以空間三自由度機(jī)械臂為研究對(duì)象，采用適用于空間微重力環(huán)境的拉格朗日方程，推導(dǎo)機(jī)械臂系統(tǒng)的動(dòng)力學(xué)方程，仿真得到不同關(guān)節(jié)力矩下各關(guān)節(jié)轉(zhuǎn)角的動(dòng)力學(xué)響應(yīng)，分析并驗(yàn)證動(dòng)力學(xué)模型的準(zhǔn)確性，旨在為空間在軌服務(wù)的機(jī)械臂系統(tǒng)設(shè)計(jì)提供參考。

1 空間機(jī)械臂動(dòng)力學(xué)建模

1.1 動(dòng)力學(xué)建?；炯僭O(shè)

建模過(guò)程中對(duì)于機(jī)械臂的處理過(guò)程需要用到以下基本假設(shè)：

1）空間機(jī)械臂的臂桿視為剛性桿件，不考慮臂桿的柔性變形和振動(dòng)。

2）空間機(jī)械臂的關(guān)節(jié)視為鉸鏈連接，不考慮關(guān)節(jié)間隙損耗等影響，不研究關(guān)節(jié)內(nèi)部結(jié)構(gòu)，只計(jì)算關(guān)節(jié)的輸出轉(zhuǎn)角和驅(qū)動(dòng)力矩；關(guān)節(jié)質(zhì)量作為相應(yīng)臂桿質(zhì)量的一部分進(jìn)行計(jì)算，臂桿之間的關(guān)節(jié)處可視為機(jī)械臂末端的一個(gè)質(zhì)點(diǎn)。

3）整個(gè)機(jī)械臂系統(tǒng)（含基座）均視為處在空間微重力的環(huán)境中，故在動(dòng)力學(xué)建模中不考慮重力梯度力矩和其他環(huán)境力矩的影響。

4）空間目標(biāo)捕捉過(guò)程分為逼近階段、捕獲階段以及穩(wěn)定調(diào)整階段。由于在捕獲階段，機(jī)械臂系統(tǒng)會(huì)與目標(biāo)物接觸碰撞，碰撞產(chǎn)生的影響不僅與目標(biāo)物的參數(shù)特性有關(guān)，也會(huì)對(duì)整個(gè)機(jī)械臂產(chǎn)生一個(gè)反向作用力，使所研究問(wèn)題無(wú)法討論，且該碰撞過(guò)程不在軌跡規(guī)劃任務(wù)需求中，故所討論情況只研究機(jī)械臂從初始位置平穩(wěn)到達(dá)目標(biāo)位置的逼近階段，不考慮運(yùn)動(dòng)過(guò)程中的外部碰撞等干擾。

5）在捕捉過(guò)程中，假設(shè)機(jī)械臂系統(tǒng)初始的角度和角速度均為0，運(yùn)動(dòng)到目標(biāo)點(diǎn)時(shí)刻的速度也為0，此假設(shè)可作為動(dòng)力學(xué)模型的邊界條件。

1.2 研究構(gòu)型描述

由于不同構(gòu)型機(jī)械臂所建模型不同，本文采用類(lèi)似人手的三自由度機(jī)械臂。如圖1所示，該機(jī)械臂為具有三自由度的鏈?zhǔn)浇Y(jié)構(gòu)，該三自由度機(jī)械臂構(gòu)型3個(gè)關(guān)節(jié)分別用3個(gè)直流力矩電機(jī)驅(qū)動(dòng)，電機(jī)軸與旋轉(zhuǎn)方向已在圖中標(biāo)出。

圖1 三自由度機(jī)械臂構(gòu)型示意Fig.1 Schematic diagram of the 3-DOF manipulator

臂桿1：繞著豎直方向旋轉(zhuǎn)，其轉(zhuǎn)角記為θ1，運(yùn)動(dòng)范圍為180°，桿長(zhǎng)記為l1，質(zhì)心位置記為l1c，質(zhì)量記為m1，轉(zhuǎn)動(dòng)慣量記為I1；

臂桿2：在紙面內(nèi)旋轉(zhuǎn)，其轉(zhuǎn)角記為θ2，運(yùn)動(dòng)范圍為180°，桿長(zhǎng)記為l2，質(zhì)心位置記為l2c，質(zhì)量記為m2，轉(zhuǎn)動(dòng)慣量記為I2；

臂桿3：在紙面內(nèi)旋轉(zhuǎn)，其轉(zhuǎn)角記為θ3，運(yùn)動(dòng)范圍為180°，桿長(zhǎng)記為l3，質(zhì)心位置記為l3c，質(zhì)量記為m3，轉(zhuǎn)動(dòng)慣量記為I3。

機(jī)械臂的主要技術(shù)參數(shù)和質(zhì)量參數(shù)分別參見(jiàn)表1和表2。

表1 機(jī)械臂主要技術(shù)參數(shù)Table 1 Main technical parameters of the manipulator

表2 機(jī)械臂質(zhì)量參數(shù)特性Table 2 Quality characteristics of the manipulator

1.3 拉格朗日方程推導(dǎo)

拉格朗日方程是基于能量平衡的方程，相對(duì)于牛頓-歐拉公式，適合于相互約束下的多連桿運(yùn)動(dòng)的分析，利用廣義坐標(biāo)系以功和能來(lái)表達(dá)系統(tǒng)運(yùn)動(dòng)，不做功的力和內(nèi)部約束力將會(huì)自動(dòng)消除，可以直接導(dǎo)出動(dòng)力學(xué)完整形式的方程式。通常把拉格朗日函數(shù)L定義為系統(tǒng)的動(dòng)能T和位能P之差，即

T和P可以用任何方便的坐標(biāo)系來(lái)表示。由于本文研究對(duì)象為空間剛性機(jī)械臂，處于空間微重力環(huán)境下，重力不對(duì)系統(tǒng)產(chǎn)生作用，故而可不考慮其系統(tǒng)的位能和勢(shì)能P，只考慮動(dòng)能T，因此根據(jù)式(1)求拉格朗日函數(shù)

通過(guò)推導(dǎo)可得最后方程形式

其中Ti為第i桿的動(dòng)能；q為廣義變量；I為轉(zhuǎn)動(dòng)慣量。

1.4 機(jī)械臂的動(dòng)力學(xué)建模

已知各個(gè)臂桿的質(zhì)心速度和質(zhì)心角速度，帶入到拉格朗日方程中，可得系統(tǒng)的動(dòng)力學(xué)方程。計(jì)算機(jī)械臂系統(tǒng)動(dòng)能為

整理后可得簡(jiǎn)化結(jié)果為

臂桿1的關(guān)節(jié)驅(qū)動(dòng)力矩為

臂桿2的關(guān)節(jié)驅(qū)動(dòng)力矩為

臂桿3的關(guān)節(jié)驅(qū)動(dòng)力矩為

將以上3個(gè)臂桿的驅(qū)動(dòng)力矩方程整理在一起，即得到系統(tǒng)的動(dòng)力學(xué)方程。該方程可以表達(dá)系統(tǒng)各個(gè)關(guān)節(jié)的驅(qū)動(dòng)力矩與各個(gè)關(guān)節(jié)轉(zhuǎn)角的角度、角速度和角加速度間的顯式函數(shù)關(guān)系。

2 動(dòng)力學(xué)模型仿真

基于1.4節(jié)求出的動(dòng)力學(xué)方程，可將其在MatLab軟件中進(jìn)行動(dòng)力學(xué)仿真，步驟為：1）設(shè)置仿真參數(shù)；2）設(shè)置仿真時(shí)間為5 s；3）設(shè)置仿真步長(zhǎng)為0.1 s；4）設(shè)置各個(gè)初始值為0。

2.1 已知驅(qū)動(dòng)力矩，求解角度和角速度

2.1.1 只存在臂桿1的驅(qū)動(dòng)力矩的仿真結(jié)果

給定臂桿1的關(guān)節(jié)驅(qū)動(dòng)力矩，同時(shí)令臂桿2和臂桿3的關(guān)節(jié)驅(qū)動(dòng)力矩為0，所得仿真結(jié)果如圖2所示。

圖2 只存在驅(qū)動(dòng)力矩1條件下臂桿轉(zhuǎn)動(dòng)角度和角速度隨時(shí)間的變化Fig.2 The changes of angle and angular velocity against the time in the mere presence of driving force 1

在該種情況下，只存在驅(qū)動(dòng)力矩1，可以看出臂桿1的轉(zhuǎn)動(dòng)角速度是均勻遞增的，此時(shí)臂桿2與臂桿3的轉(zhuǎn)動(dòng)角度和角速度均不發(fā)生變化，因?yàn)楸蹢U1的轉(zhuǎn)角所在平面與臂桿2和臂桿3的轉(zhuǎn)角所在平面正交，所以轉(zhuǎn)角間不會(huì)發(fā)生動(dòng)力學(xué)耦合，故而不會(huì)產(chǎn)生相互影響，與實(shí)際相符合。

2.1.2 只存在臂桿2的驅(qū)動(dòng)力矩的仿真結(jié)果

給定臂桿2的關(guān)節(jié)驅(qū)動(dòng)力矩，同時(shí)令臂桿1和臂桿3的關(guān)節(jié)驅(qū)動(dòng)力矩保持0，所得仿真結(jié)果如圖3所示。

圖3 只存在驅(qū)動(dòng)力矩2條件下臂桿轉(zhuǎn)動(dòng)角度和角速度隨時(shí)間的變化Fig.3 The changes of angle and angular velocity against the time in the mere presence of driving force 2

在該種情況下，只存在驅(qū)動(dòng)力矩2，可以看出臂桿1的轉(zhuǎn)動(dòng)角速度不變，而臂桿2與臂桿3的轉(zhuǎn)動(dòng)角度和角速度均發(fā)生變化，而關(guān)節(jié)2與關(guān)節(jié)3的角度和角加速度互相影響；并且從圖3可以看出驅(qū)動(dòng)力矩2對(duì)臂桿3的影響較大，因此臂桿2的運(yùn)動(dòng)很大程度上直接影響臂桿3的運(yùn)動(dòng)。仿真結(jié)果分析與實(shí)際相符合，這也體現(xiàn)了動(dòng)力學(xué)模型的準(zhǔn)確性。

2.1.3 只存在臂桿3的驅(qū)動(dòng)力矩的仿真結(jié)果

給定臂桿3的關(guān)節(jié)驅(qū)動(dòng)力矩，同時(shí)令臂桿1和臂桿2的關(guān)節(jié)驅(qū)動(dòng)力矩為0，仿真結(jié)果如圖4所示。

圖4 只存在驅(qū)動(dòng)力矩3條件下臂桿轉(zhuǎn)動(dòng)角度和角速度隨時(shí)間的變化Fig.4 The changes of angle and angular velocity against the time in the mere presence of driving force 3

在該種情況下，只存在驅(qū)動(dòng)力矩3，可以看出臂桿1的轉(zhuǎn)動(dòng)角度和角速度同樣是不變的，此時(shí)臂桿2與臂桿3的轉(zhuǎn)動(dòng)角度和轉(zhuǎn)角速度均發(fā)生變化，且轉(zhuǎn)角間存在相互影響。從圖4可以看出，驅(qū)動(dòng)力矩3對(duì)臂桿3的影響較大，對(duì)臂桿2的運(yùn)動(dòng)也產(chǎn)生一定程度的影響，這體現(xiàn)出臂桿2與臂桿3間動(dòng)力學(xué)耦合運(yùn)動(dòng)的結(jié)果。

2.2 已知運(yùn)動(dòng)軌跡反解力矩的仿真結(jié)果

1）首先給出目標(biāo)點(diǎn)位置坐標(biāo)，進(jìn)行運(yùn)動(dòng)學(xué)反解，求出對(duì)應(yīng)于目標(biāo)點(diǎn)處各個(gè)關(guān)節(jié)轉(zhuǎn)角的目標(biāo)值。

2）由給定的初值條件和運(yùn)動(dòng)學(xué)反解計(jì)算得到的終止條件，求出各個(gè)關(guān)節(jié)的關(guān)節(jié)轉(zhuǎn)角插值函數(shù)。

3）將所得的關(guān)節(jié)轉(zhuǎn)角插值函數(shù)進(jìn)行求導(dǎo)得到各個(gè)關(guān)節(jié)角速度的插值函數(shù)，再求導(dǎo)即可得到各個(gè)關(guān)節(jié)角加速度的插值函數(shù)。

4）將所得到的各個(gè)關(guān)節(jié)角度、角速度和角加速度代入動(dòng)力學(xué)模型方程(6)～(8)中進(jìn)行仿真計(jì)算。

5）給出仿真時(shí)間、步長(zhǎng)及仿真環(huán)境變量，即可得到各個(gè)關(guān)節(jié)的力矩曲線(xiàn)變化圖，按照該曲線(xiàn)控制各個(gè)關(guān)節(jié)的力矩即可使機(jī)械臂按照所設(shè)計(jì)的路徑進(jìn)行運(yùn)動(dòng)到達(dá)目標(biāo)位置。

仿真結(jié)果如圖5所示，可以看出機(jī)械臂可按照預(yù)期約束條件平穩(wěn)到達(dá)指定目標(biāo)點(diǎn)。

圖5 根據(jù)已知運(yùn)動(dòng)軌跡求解出的各關(guān)節(jié)驅(qū)動(dòng)力矩控制曲線(xiàn)Fig.5 The driving torque for each joint extrapolated from specific motion conditions

3 空間點(diǎn)到點(diǎn)路徑規(guī)劃

3.1 直線(xiàn)插補(bǔ)法

通常情況的笛卡兒點(diǎn)到點(diǎn)路徑的直線(xiàn)插補(bǔ)法原理是：在已知初始點(diǎn)和目標(biāo)點(diǎn)的情況下，根據(jù)插補(bǔ)次數(shù)和取點(diǎn)個(gè)數(shù)進(jìn)行等間距插補(bǔ)。如從起始點(diǎn)P0(x0,y0,z0)到終點(diǎn)Pf(xf,yf,zf)的直線(xiàn)路程中間進(jìn)行n次插補(bǔ)，中間坐標(biāo)變化量記為 (Δx,Δy,Δz)，則有：

當(dāng)已知時(shí)間間隔和運(yùn)行速度時(shí)，插補(bǔ)的次數(shù)為

式中：v為運(yùn)行速度；Ts為時(shí)間間隔。因此直線(xiàn)上各個(gè)插補(bǔ)點(diǎn)的坐標(biāo)可表示為

3.2 二分逐點(diǎn)逼近插補(bǔ)法

由于等間距插補(bǔ)所需的插補(bǔ)次數(shù)多，且終止位置附近的控制精度相對(duì)低。為了提高路徑運(yùn)動(dòng)效率，且使得末端在逼近位置附近能更加平穩(wěn)準(zhǔn)確地捕捉目標(biāo)，可采取前半段路徑快速到位，后半段路徑盡可能平穩(wěn)、小擾動(dòng)的路徑規(guī)劃方法。故本文提出一種新的插補(bǔ)方法——二分逐點(diǎn)逼近法，用于空間環(huán)境下快速精確地實(shí)現(xiàn)任務(wù)的路徑軌跡策略，也可以更好地適應(yīng)空間機(jī)械臂的捕捉要求。

已知起始點(diǎn)P0(x0,y0,z0)和終點(diǎn)Pf(xf,yf,zf)，中間路徑點(diǎn)記為Pi(xi,yi,zi)，則有二分逐點(diǎn)逼近插補(bǔ)法的遞推表達(dá)式

式中 ξ為捕捉精度。

該方法運(yùn)動(dòng)效率高，且省略路徑前半段中不必要的插補(bǔ)點(diǎn)，在靠近目標(biāo)位置的關(guān)鍵路徑點(diǎn)上更加細(xì)化精確，不僅能提高機(jī)械臂任務(wù)執(zhí)行的運(yùn)動(dòng)效率，且在末端捕捉目標(biāo)時(shí)有更高的精度。該方法同樣適用于捕捉非靜態(tài)目標(biāo)物，通過(guò)逐點(diǎn)二分法逐漸逼近目標(biāo)點(diǎn)直至滿(mǎn)足捕捉精度要求，即可捕捉該目標(biāo)物。

3.3 兩種方法的仿真對(duì)比

將上述2種方法在MatLab中進(jìn)行仿真，設(shè)置初始點(diǎn)、終止點(diǎn)以及規(guī)定距離，給出捕捉精度并設(shè)置相同的仿真步長(zhǎng)，可分別得到其路徑散點(diǎn)圖。從起始點(diǎn)(0, 0, 0)運(yùn)動(dòng)到目標(biāo)點(diǎn)(10, 10, 10)，路徑插補(bǔ)點(diǎn)計(jì)數(shù)為100，仿真時(shí)間以到達(dá)精度或到達(dá)目標(biāo)時(shí)刻為準(zhǔn)，步長(zhǎng)均為1 s，仿真結(jié)果如圖6、圖7所示。

圖6 等間距直線(xiàn)插補(bǔ)法的MatLab仿真Fig.6 MatLab simulation of equi-distance linear interpolation

圖7 二分逐點(diǎn)逼近直線(xiàn)插補(bǔ)法的MatLab仿真Fig.7 MatLab simulation of point by point approximation for linear interpolation

對(duì)比2種軌跡規(guī)劃方法的仿真圖，等間距直線(xiàn)插補(bǔ)法的插補(bǔ)精度為0.17，二分逐點(diǎn)逼近插補(bǔ)法的精度數(shù)量級(jí)遠(yuǎn)遠(yuǎn)小于等間距直線(xiàn)插補(bǔ)法。若以精度0.1為參考，二分逐點(diǎn)逼近法只需要8個(gè)插補(bǔ)計(jì)數(shù)點(diǎn)精度已可達(dá)到0.067，滿(mǎn)足距離目標(biāo)點(diǎn)精度要求，相比于等間距直線(xiàn)插補(bǔ)法，在運(yùn)動(dòng)時(shí)間和插補(bǔ)個(gè)數(shù)上均有顯著優(yōu)化和縮減。因此不難看出：二分逐點(diǎn)逼近插補(bǔ)法具有運(yùn)動(dòng)效率高（即不需要很多插補(bǔ)點(diǎn)即可快速接近目標(biāo)點(diǎn)），且越接近目標(biāo)位置控制精度越高等優(yōu)點(diǎn)，適用于高精度捕捉路徑規(guī)劃設(shè)計(jì)。

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

本文給出了空間三自由度機(jī)械臂系統(tǒng)的拉格朗日動(dòng)力學(xué)方程，利用MatLab軟件對(duì)其進(jìn)行了仿真建模，并給出已知驅(qū)動(dòng)力矩求各個(gè)關(guān)節(jié)運(yùn)動(dòng)曲線(xiàn)以及已知運(yùn)動(dòng)情況求解各個(gè)關(guān)節(jié)驅(qū)動(dòng)力矩的仿真結(jié)果。此外，根據(jù)仿真模型研究了空間環(huán)境下在軌機(jī)械臂系統(tǒng)的軌跡規(guī)劃問(wèn)題，分別介紹了等間距直線(xiàn)插補(bǔ)法和二分逐點(diǎn)逼近直線(xiàn)插補(bǔ)法2種逼近策略，并對(duì)這2種軌跡規(guī)劃方法進(jìn)行了仿真分析對(duì)比，結(jié)果驗(yàn)證了本文所提出的二分逐點(diǎn)逼近直線(xiàn)插補(bǔ)法性能更優(yōu)。該方法是一種創(chuàng)新性的路徑插補(bǔ)方法，經(jīng)實(shí)踐驗(yàn)證可用作一些空間任務(wù)中路徑逼近的插補(bǔ)策略，具有工程意義和價(jià)值。

以上針對(duì)空間三自由度機(jī)械臂系統(tǒng)所開(kāi)展的軌跡規(guī)劃仿真分析工作，為后續(xù)機(jī)械臂路徑規(guī)劃研究工作提供了理論和模型基礎(chǔ)。