空間機(jī)器人抓捕目標(biāo)后的載荷分配

周逸群，羅建軍，王明明,*

1. 西北工業(yè)大學(xué) 深圳研究院，深圳 518057 2. 西北工業(yè)大學(xué) 航天動(dòng)力學(xué)國家重點(diǎn)實(shí)驗(yàn)室，西安 710072

隨著航天技術(shù)的發(fā)展，人們未來需要在太空中進(jìn)行更多復(fù)雜的艙外任務(wù)。與航天員相比，空間機(jī)器人在執(zhí)行在軌任務(wù)方面具有諸多優(yōu)勢(shì)。自20世紀(jì)80年代以來，空間機(jī)器人已被用于大型航天器上的許多航天任務(wù)，并成為實(shí)現(xiàn)在軌服務(wù)任務(wù)最重要的途徑之一。典型實(shí)例有“工程試驗(yàn)衛(wèi)星7號(hào)(Engineering Test Satellite VII, ETS-VII)”[1]、“軌道快車(Orbit Express, OE)”[2]、“鳳凰計(jì)劃(Phoenix)”[3]、“德國軌道服務(wù)任務(wù)(Deutsche Orbitale Servicing Mission, DEOS)”[4]等。由單臂到多臂、由遙操作到自主、由合作目標(biāo)到非合作目標(biāo)是空間機(jī)器人技術(shù)發(fā)展的主要趨勢(shì)[5]。

與單臂機(jī)器人相比，多臂機(jī)器人所能處理的載荷更多，靈活性也更高。但與此同時(shí)，多臂協(xié)調(diào)操作的復(fù)雜性也更大。為操作所抓捕的目標(biāo)，機(jī)器人需要對(duì)目標(biāo)施加適當(dāng)?shù)耐饬?，以?shí)現(xiàn)目標(biāo)的期望運(yùn)動(dòng)。由于多臂操作的冗余性，各機(jī)械臂所施加的外力組合是不唯一的。因此可以按照一定指標(biāo)，將目標(biāo)的期望合外力合理地分配到各機(jī)械臂。機(jī)械臂施加力可分為操作力和內(nèi)力兩部分，前者驅(qū)動(dòng)目標(biāo)運(yùn)動(dòng)，后者對(duì)目標(biāo)運(yùn)動(dòng)沒有影響。Alberts和Soloway[6]將載荷分配轉(zhuǎn)化為線性約束二次優(yōu)化問題，并提出權(quán)重因子以平衡分配末端力，在載荷分配問題中引入了權(quán)重廣義逆。Walker等[7]提出了一種避免操作力在目標(biāo)內(nèi)部產(chǎn)生擠壓，從而消除內(nèi)力的載荷分配方案，并指出這種抓取矩陣廣義逆是惟一的。Bonitz和Hsia[8]將這種方法應(yīng)用于相互作用力的分析，以實(shí)現(xiàn)操作力和內(nèi)力的解耦。但Chung等[9]對(duì)這一結(jié)果提出了質(zhì)疑，認(rèn)為使用Moore-Penrose逆作為權(quán)重廣義逆才可以消除內(nèi)力。Kumar和Waldron[10]則將內(nèi)力定義為任意2個(gè)機(jī)械臂末端施加力在它們幾何連線上的投影之差，但沒能將這一概念擴(kuò)展至內(nèi)力矩。Yoshikawa和Nagai[11]提出了一種幾何啟發(fā)式的內(nèi)力定義，試圖解決使用廣義逆不一致的問題。Williams和Khatib[12]提出內(nèi)力的另一種表征，其中機(jī)械臂的組合近似為鉸接機(jī)制，內(nèi)力被解釋為鎖定這一機(jī)制所需的驅(qū)動(dòng)力，但沒有考慮末端力對(duì)產(chǎn)生的扭矩的影響。Erhart和Hirche[13]提出了一種新定義，認(rèn)為內(nèi)力的產(chǎn)生是由于對(duì)運(yùn)動(dòng)約束的違反，并指出無擠壓的載荷分配是不唯一的，這為選擇合適的載荷分配方案提供了額外的自由度。

除多臂協(xié)調(diào)操作外，載荷分配在多指手抓取中也發(fā)揮著重要作用。在上述研究中，機(jī)械臂末端與目標(biāo)表面通常設(shè)定為固聯(lián)形式，但在實(shí)際中更類似于多指手抓取物體，機(jī)械臂末端的運(yùn)動(dòng)與目標(biāo)上的抓捕點(diǎn)并非完全相符，施加于目標(biāo)的接觸力必須符合接觸點(diǎn)處的摩擦約束，這就要求合理地規(guī)劃抓捕點(diǎn)布局和各臂所施加的抓捕力，以保持抓取穩(wěn)定并實(shí)現(xiàn)期望的目標(biāo)運(yùn)動(dòng)。抓取力的優(yōu)化可以描述為非線性規(guī)劃問題，即按照所設(shè)的目標(biāo)函數(shù)，尋找既能平衡目標(biāo)所受外力又能滿足接觸點(diǎn)處的摩擦錐約束條件的最優(yōu)接觸力。Nakamura等[14]用Lagrange乘子法進(jìn)行抓取力的非線性規(guī)劃，將摩擦錐作為非線性約束條件，摩擦力的最小范數(shù)作為優(yōu)化目標(biāo)。由于摩擦錐約束的高度非線性，這一問題的求解計(jì)算量往往很大，為此人們通常利用其特殊的幾何特性將約束線性化。Buss等[15]將非線性的摩擦約束條件替換為一個(gè)對(duì)稱矩陣的正定性判定條件，從而將優(yōu)化問題轉(zhuǎn)換成具有線性約束的黎曼流形的最優(yōu)化問題。王濱[16]和Borgstrom[17]等分別利用Lagrange乘子法和懲罰函數(shù)法調(diào)節(jié)法向接觸力的權(quán)值，以使抓取力滿足摩擦錐約束，并通過梯度流算法優(yōu)化抓取力。Cornellà等[18]提出了一種基于對(duì)偶原理的非線性規(guī)劃方法。陳棟金等[19]將任意外力分解為單位力的線性組合，對(duì)基礎(chǔ)抓取力按同樣規(guī)律組合，得到滿足約束的抓取力初值。陳金寶等[20]提出了基于序列二次規(guī)劃算法的fmincon函數(shù)，并采用關(guān)節(jié)阻抗控制算法控制各指接觸力，能夠快速進(jìn)行抓取力非線性規(guī)劃。王新慶等[21]將外力分為12個(gè)單位力向量的線性組合，先離線計(jì)算得到力優(yōu)化問題的初值，再結(jié)合半正定規(guī)劃算法進(jìn)行動(dòng)態(tài)力優(yōu)化。Kerr和Roth[22]最早提出用棱錐代替摩擦錐，將非線性的摩擦錐約束轉(zhuǎn)化為接觸力基向量的線性組合約束，提出抓取力優(yōu)化的線性規(guī)劃算法。Sinha[23]和Barkat[24]等采用二次規(guī)劃算法計(jì)算滿足非線性摩擦約束的最小接觸力。Jia等[25]建立了一種基于關(guān)鍵約束集的實(shí)時(shí)有效算法，直接優(yōu)化關(guān)節(jié)力矩，便于靈巧手的控制。Cloutier和Yang[26]對(duì)抓取力優(yōu)化的線性法、非線性法和線性矩陣不等式法進(jìn)行了比較研究?？傮w來說，抓取力規(guī)劃的非線性規(guī)劃方法的計(jì)算結(jié)果比較準(zhǔn)確，但計(jì)算量大，通常是離線進(jìn)行的。線性規(guī)劃方法的計(jì)算量較小，但由于對(duì)摩擦模型進(jìn)行了近似處理，所得結(jié)果較為保守。

現(xiàn)有的有關(guān)載荷分配的研究大都針對(duì)地面機(jī)械臂或多指手，在空間機(jī)器人領(lǐng)域尚未得到應(yīng)用。已有的研究大多僅考慮目標(biāo)的幾何特性，沒有考慮機(jī)械臂所能施加的接觸力等限制條件。抓取的對(duì)象多為靜止物體，難以適用于多臂操作目標(biāo)運(yùn)動(dòng)的情況。本文旨在提出一種綜合考慮目標(biāo)與機(jī)械臂特性，應(yīng)用于多臂空間機(jī)器人抓捕目標(biāo)后的載荷分配方法。在給定目標(biāo)期望運(yùn)動(dòng)的條件下，計(jì)算各臂滿足摩擦約束和機(jī)械臂能力約束的最小抓捕力。為降低計(jì)算量，采用線性規(guī)劃方法，將軟指接觸模型線性化。根據(jù)目標(biāo)與機(jī)器人的動(dòng)力學(xué)特性和運(yùn)動(dòng)約束，建立關(guān)節(jié)扭矩約束。將求解最優(yōu)接觸力的非線性規(guī)劃問題轉(zhuǎn)化為線性規(guī)劃問題。

本文組織如下：第1節(jié)介紹抓捕后階段空間機(jī)器人及目標(biāo)組合體的動(dòng)力學(xué)。第2節(jié)建立軟指接觸條件下的摩擦力約束及運(yùn)動(dòng)約束。第3節(jié)將摩擦錐約束線性化，并推導(dǎo)組合體中關(guān)節(jié)扭矩約束的形式，建立載荷分配的線性規(guī)劃方法。第4節(jié)展示對(duì)于目標(biāo)各種形式的期望運(yùn)動(dòng)，所提接觸力優(yōu)化方法的有效性。第5節(jié)給出結(jié)論。

1 動(dòng)力學(xué)模型

1.1 空間機(jī)器人動(dòng)力學(xué)

在抓捕后階段，空間機(jī)器人與目標(biāo)構(gòu)成一個(gè)組合體。如圖1所示，雙臂空間機(jī)器人系統(tǒng)由基座和2條n自由度的機(jī)械臂組成，共包含2n+1個(gè)剛體。

表1給出了空間機(jī)器人系統(tǒng)中運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)符號(hào)的含義。在本文中，如無特別說明，所有參數(shù)都表示在與之固聯(lián)的剛體坐標(biāo)系中。En和0n分別表示n×n的單位矩陣和零矩陣。參數(shù)的上標(biāo)a或b分別表示與臂a或臂b相關(guān)的項(xiàng)。下標(biāo)b、m、e、t分別表示與基座、機(jī)械臂、末端執(zhí)行器或目標(biāo)相關(guān)的項(xiàng)。上標(biāo)l(l=a,b)為接觸點(diǎn)編號(hào)，下標(biāo)i(i=1,2,…,n)為連桿編號(hào)。

表1 本文所用運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)符號(hào)Table 1 Kinematic and dynamic symbols used in this paper

根據(jù)Lagrange力學(xué)，空間機(jī)器人動(dòng)力學(xué)方程表示為[27]

(1)

或簡(jiǎn)寫為

(2)

對(duì)于自由漂浮空間機(jī)器人，fb=0，動(dòng)力學(xué)方程可以改寫為

(3)

(4)

1.2 目標(biāo)動(dòng)力學(xué)

目標(biāo)的動(dòng)力學(xué)方程表示為[27]

(5)

(6)

×為3維叉乘因子，其具體形式為

(7)

(8)

2 機(jī)械臂與目標(biāo)的接觸模型

在實(shí)際情況下，機(jī)械臂與所抓捕目標(biāo)之間往往并非固聯(lián)，而是類似于多指手抓取的形式。這種情況下，機(jī)械臂末端與目標(biāo)的運(yùn)動(dòng)并不完全一致，指尖只能在某些方向上對(duì)目標(biāo)施加力和力矩，接觸力受到摩擦約束的限制。指尖和物體有3種基本的接觸形式：無摩擦點(diǎn)接觸(Frictionless Point Contact, FPC)、有摩擦點(diǎn)接觸(Point Contact with Friction, PCwF)和軟指接觸(Soft-Finger Contact, SFC[25])。其中，軟指接觸最接近實(shí)際情況。令接觸點(diǎn)l(l=a,b)坐標(biāo)系的z軸沿目標(biāo)表面法向并指向其內(nèi)部，x軸和y軸組成的平面垂直于目標(biāo)表面法向。

(9)

此時(shí)接觸力的方向局限于接觸點(diǎn)的法向軸線，并指向目標(biāo)內(nèi)部。

(10)

由式(10)表示的法向壓力與切向摩擦力的約束在幾何上可以表示為一個(gè)頂點(diǎn)位于接觸點(diǎn)，以法向壓力為軸的圓錐，稱為摩擦錐。

(11)

或

(12)

式(11)和式(12)分別稱為軟指接觸的線性模型和橢球模型。在法向壓力固定的情況下，切向摩擦力和摩擦力矩在幾何上可分別表示為一個(gè)3維雙錐體和3維橢球體。

圖2展示了三類接觸模型。

在軟指接觸的情形下，機(jī)械臂末端施加于目標(biāo)的6維力螺旋為

(13)

(14)

和

(15)

(16)

3 抓捕力優(yōu)化問題

3.1 優(yōu)化問題

多臂抓捕目標(biāo)的載荷分配問題可以表述為一個(gè)多約束優(yōu)化問題。機(jī)械臂末端與目標(biāo)間為軟指接觸形式。為避免接觸力過大損壞目標(biāo)，這里設(shè)定各抓捕點(diǎn)的法向壓力之和最小化作為優(yōu)化目標(biāo)。約束條件包括式(5)所代表的動(dòng)力學(xué)約束、式(11) 或式(12)所代表的摩擦約束，以及由關(guān)節(jié)扭矩上限所決定的機(jī)械臂能力約束等。具體形式為

(17)

為減少計(jì)算量，下面將摩擦約束線性化，并建立 機(jī)械臂能力約束形式。

3.2 摩擦約束的線性化

由于摩擦錐約束的非線性，在抓捕力規(guī)劃問題中直接運(yùn)用起來比較困難。此時(shí)可以通過線性化方法，將其轉(zhuǎn)化為線性約束。

(18)

(19)

或

(20)

式中：nl=2JK-J+2；Sl∈R4×nl為接觸力基向量的并列形式；λl∈Rnl為其系數(shù)向量。各抓捕點(diǎn)接觸力的并列可表示為

(21)

式中：S=diag(Sa,Sb)；λ=[(λa)T,(λb)T]T為其系數(shù)向量。

將接觸力轉(zhuǎn)換到目標(biāo)坐標(biāo)系，得到：

(22)

(23)

式中：W=[Wa,Wb]∈R6×(na+nb)為所有接觸點(diǎn)原始力螺旋組成的矩陣。

3.3 機(jī)械臂能力約束

在空間機(jī)器人運(yùn)動(dòng)過程中，關(guān)節(jié)扭矩既要為末端抓捕目標(biāo)提供期望的接觸力，又要驅(qū)動(dòng)機(jī)器人的運(yùn)動(dòng)。因此，機(jī)械臂能力約束同時(shí)受到機(jī)器人和目標(biāo)的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)因素的影響。

對(duì)式(16)兩邊求導(dǎo)，可得機(jī)械臂與目標(biāo)間的加速度級(jí)運(yùn)動(dòng)約束：

(24)

由式(14)和式(15)分別可得

(25)

(26)

將式(25)和式(26)代入式(24)，得到

(27)

忽略與速度相關(guān)的偏差項(xiàng)，可得：

(28)

將式(28)代入式(3)，得

(29)

(30)

經(jīng)過以上處理，優(yōu)化問題式(17)可以改寫為

(31)

通過對(duì)摩擦約束和機(jī)械臂能力約束的處理，原接觸力非線性規(guī)劃問題轉(zhuǎn)化為原始力螺旋系數(shù)的線性規(guī)劃問題。

3.4 優(yōu)化算法

如式(31)所示，載荷分配問題可轉(zhuǎn)化為一個(gè)包含等式約束和不等式約束的線性規(guī)劃問題。本文采用原始-對(duì)偶內(nèi)點(diǎn)法[28]解決該優(yōu)化問題。

對(duì)于線性規(guī)劃問題的標(biāo)準(zhǔn)形式：

(32)

式中:A和b分別為不等式約束系數(shù);Aeq和beq分別為等式約束系數(shù);l和u分別為優(yōu)化對(duì)象的下、上邊界。引入松弛變量s和t，將不等式約束轉(zhuǎn)化為等式約束：

(33)

式中：

(34)

初始值x0設(shè)為下邊界值。設(shè)Lagrange乘子分別為y、v、w，得到Lagrange方程：

(35)

系統(tǒng)的KTT條件為

(36)

算法首先根據(jù)Newton-Raphson公式預(yù)測(cè)一步，然后計(jì)算校正步。Newton-Raphson步驟為

(37)

式中：X、V、W、T分別為對(duì)應(yīng)x、v、w、t的對(duì)角矩陣，最右項(xiàng)為剩余向量。式(37)可轉(zhuǎn)化為對(duì)稱矩陣形式：

(38)

式中：

(39)

式(38)經(jīng)轉(zhuǎn)換可得到：

(40)

求解該方程得到Δy后，代入式(37)和式(38)即可得到剩余迭代向量，通過逐步迭代得到目標(biāo)函數(shù)變量的收斂值。

4 仿真驗(yàn)證

4.1 仿真模型

為了驗(yàn)證所提載荷分配方法的實(shí)用性，本節(jié)給出采用冗余雙臂空間機(jī)器人抓捕目標(biāo)的仿真實(shí)例?？臻g機(jī)器人模型如圖1所示，兩臂各有7個(gè)自由度，基座有6個(gè)自由度。空間機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)參數(shù)如表2所示，其中ai、bi和Ii均表示在其本體坐標(biāo)系中。

表2 空間機(jī)器人的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)參數(shù)Table 2 Kinematic and dynamic parameters of space robot

所抓捕的目標(biāo)的質(zhì)量mt=40 kg，轉(zhuǎn)動(dòng)慣量It=diag(16.80,24.36,25.68) kg·m2，2個(gè)抓捕點(diǎn)的位置為rg=[0,±1.006,0]Tm。以上參數(shù)均表示在目標(biāo)坐標(biāo)系中。

兩機(jī)械臂末端與目標(biāo)間的摩擦力系數(shù)和摩擦力矩系數(shù)分別為μ=0.3,γ=0.4。線性化摩擦錐參數(shù)為J=20,K=1。關(guān)節(jié)扭矩幅值τmax=20。

數(shù)值仿真的基本步驟如下：

步驟1根據(jù)式(18)～式(23)，離線計(jì)算矩陣Jt、S和W。

步驟2對(duì)目標(biāo)進(jìn)行運(yùn)動(dòng)規(guī)劃，獲得目標(biāo)的期望運(yùn)動(dòng)信息(位姿、速度、加速度)。

步驟4根據(jù)式(5)計(jì)算期望合外力。

步驟5通過3.4節(jié)所示原始-對(duì)偶內(nèi)點(diǎn)法求解優(yōu)化問題式(31)，得到最優(yōu)接觸力。

4.2 仿真實(shí)例

4.2.1 場(chǎng)景1：目標(biāo)的平移運(yùn)動(dòng)

首先考慮空間機(jī)器人操作目標(biāo)進(jìn)行平移的情形。目標(biāo)運(yùn)動(dòng)軌跡可采用5階Bézier曲線參數(shù)化：

(41)

兩機(jī)械臂末端接觸力的仿真結(jié)果如圖4所示。未發(fā)生違反摩擦約束或機(jī)械臂能力約束的情況。在目標(biāo)的平移過程中，兩臂施加的力矩接近于零。由于目標(biāo)僅在x方向運(yùn)動(dòng)，所施加的y方向的摩擦力為零，x方向的摩擦力則與加速度變化趨勢(shì)一致。在z方向(即目標(biāo)表面法向)，兩機(jī)械臂需要施加一定的法向壓力以產(chǎn)生期望的摩擦力，其變化趨勢(shì)與摩擦力的幅值基本一致。

4.2.2 場(chǎng)景2：目標(biāo)的自旋運(yùn)動(dòng)

空間機(jī)器人抓捕非合作目標(biāo)后，通常需要施加力矩消除目標(biāo)的自旋運(yùn)動(dòng)。本文采用x-y-z順序的歐拉角φ=[α,β,γ]T表示目標(biāo)自旋運(yùn)動(dòng)，其軌跡可采用歸一化時(shí)間的初等函數(shù)表示：

φi(τ)=kiexp(-τ)+aiτ2+biτ+ci

(42)

(43)

仿真結(jié)果如圖6所示。未發(fā)生違反約束的情況。在目標(biāo)自旋過程中，兩臂所施加力矩的變化趨勢(shì)與目標(biāo)角加速度相同。為產(chǎn)生期望的摩擦力矩，兩機(jī)械臂需要施加一定的法向壓力和切向摩擦力以保持接觸，并呈指數(shù)衰減趨勢(shì)。隨著目標(biāo)自旋運(yùn)動(dòng)的消除，兩臂接觸力均收斂至零。

4.2.3 場(chǎng)景3：目標(biāo)的混合運(yùn)動(dòng)

圖8給出了兩機(jī)械臂末端接觸力的仿真結(jié)果。此時(shí)所需的合外力較大，在個(gè)別時(shí)刻出現(xiàn)了接近或達(dá)到關(guān)節(jié)扭矩約束邊界的情況，接觸力矩和接觸力出現(xiàn)了振動(dòng)。同時(shí)，由于目標(biāo)的平移與自旋，兩臂所施加的力矩與力不再對(duì)稱，呈現(xiàn)更為復(fù)雜的變化。

5 結(jié) 論

載荷分配是空間多臂機(jī)器人在軌操控的前提條件，其主要任務(wù)為按照一定的衡量指標(biāo)，將抓捕目標(biāo)的期望合外力轉(zhuǎn)化為各機(jī)械臂施加的滿足約束條件的抓捕力。本文關(guān)于空間機(jī)器人載荷分配的研究成果包括：

1) 在地面機(jī)械臂及多指手抓取的基礎(chǔ)上，將載荷分配研究應(yīng)用至空間機(jī)器人，進(jìn)行抓捕目標(biāo)后的抓捕力規(guī)劃。

2) 載荷分配問題同時(shí)考慮了摩擦約束和機(jī)械臂能力約束，將摩擦約束線性化，并建立綜合考慮目標(biāo)與機(jī)械臂因素的關(guān)節(jié)扭矩約束，將原非線性規(guī)劃問題簡(jiǎn)化為線性規(guī)劃問題。

3) 對(duì)雙臂空間機(jī)器人抓捕目標(biāo)后，目標(biāo)平移、自旋等不同運(yùn)動(dòng)狀態(tài)下的數(shù)值仿真，驗(yàn)證了在滿足約束的前提下所提方法針對(duì)復(fù)雜運(yùn)動(dòng)狀態(tài)的有效性。