西北工業(yè)大學(xué) 航天學(xué)院,西安 710072

空間機(jī)器人抓捕目標(biāo)過程中涉及兩個(gè)空間物體之間的接觸，對(duì)于自由飛行空間機(jī)器人來說，接觸碰撞造成的影響主要有三個(gè)方面：一是在基座可控范圍內(nèi)，碰撞力越大，碰撞次數(shù)越多，維持系統(tǒng)穩(wěn)定所消耗的推進(jìn)燃料越多；二是當(dāng)碰撞力超過基座控制范圍，會(huì)造成機(jī)器人系統(tǒng)失穩(wěn)；三是接觸力增大到一定程度，會(huì)對(duì)抓捕機(jī)構(gòu)造成損壞。因此接觸碰撞階段是整個(gè)抓捕過程中危險(xiǎn)系數(shù)最高的階段。接觸碰撞與抓捕控制涉及抓捕機(jī)構(gòu)的空間構(gòu)型、接觸表面的形狀與材料特性，抓捕機(jī)構(gòu)狀態(tài)的計(jì)算、目標(biāo)速度與位置的測(cè)量與目標(biāo)的質(zhì)量特性等諸多因素。真實(shí)的接觸碰撞動(dòng)力學(xué)模型是個(gè)高度非線性的復(fù)雜模型，其精確建模的難度極大，因此抓捕接觸碰撞力控制是在軌抓捕技術(shù)中的難點(diǎn)和重點(diǎn)。

針對(duì)抓捕接觸碰撞控制問題，Wee等[1]提出了一種梯度投影方法來進(jìn)行笛卡爾空間的路徑規(guī)劃以使得接觸時(shí)的沖擊力最小化，但該方法要求機(jī)器人系統(tǒng)機(jī)構(gòu)具有運(yùn)動(dòng)學(xué)冗余，并且該梯度算法容易陷入局部最優(yōu)。Zheng等[2-3]給出了“沖擊系數(shù)”和“沖擊橢圓”碰撞沖擊力特性，并在動(dòng)量守恒的基礎(chǔ)上，基于虛擬轉(zhuǎn)子慣量的概念提出了擴(kuò)展逆慣性張量(Ex-IIT)的概念，這樣就可以在速度級(jí)上描述問題而無需測(cè)量碰撞力。Nenchev等[4-6]對(duì)如何利用“零反作用空間”進(jìn)行了充分的討論，采用耦合慣量矩陣的零空間解決了本體與機(jī)械臂之間的動(dòng)力學(xué)耦合問題。作者同時(shí)指出，利用該方法獲得的關(guān)節(jié)速度不會(huì)影響動(dòng)量的分布。Shibli,M等[7-8]建立了空間機(jī)器人和目標(biāo)衛(wèi)星模型，設(shè)計(jì)了機(jī)器人抓捕目標(biāo)的自適應(yīng)逆動(dòng)力學(xué)控制器。徐文福等[9]提出了一種將經(jīng)典的選擇矩陣進(jìn)行加權(quán)處理的改進(jìn)力/位控制方法，使力控制和位置控制的切換更加平滑。危清清等[10]將抓捕過程中的接觸碰撞力簡(jiǎn)化為作用在抓捕機(jī)構(gòu)上的脈沖力進(jìn)行研究，但是實(shí)際中的接觸力要復(fù)雜的多。MAO[11]利用仿真器對(duì)微重力環(huán)境下的接觸動(dòng)力學(xué)進(jìn)行了研究。DIAO等[12]提出了減小碰撞力對(duì)空間機(jī)器人系統(tǒng)影響的算法。MAO等[13]又根據(jù)末端執(zhí)行器的幾何構(gòu)型、抓捕機(jī)構(gòu)的狀態(tài)和觀測(cè)得到的目標(biāo)狀態(tài)來預(yù)估接觸力的方向，并基于此優(yōu)化得到恰當(dāng)?shù)牟东@時(shí)間和捕獲位置，使得接觸碰撞對(duì)系統(tǒng)的影響最小。魏承等[14-16]提出了一種空間機(jī)器人“動(dòng)態(tài)抓捕域”的概念，并將其運(yùn)用于捕獲浮游目標(biāo)的抓取控制 ，但他們是將浮游目標(biāo)假設(shè)為質(zhì)點(diǎn)，目標(biāo)姿態(tài)可控，沒有考慮到目標(biāo)及抓捕機(jī)構(gòu)的空間構(gòu)造。陳鋼等[17]針對(duì)空間機(jī)器人目標(biāo)捕獲過程中的碰撞問題，提出了一種碰撞運(yùn)動(dòng)分析算法，并且證明了該算法的正確性和適用性。劉厚得等[18]針對(duì)航天器抓捕后復(fù)合體系統(tǒng)失穩(wěn)問題，提出了關(guān)節(jié)阻尼控制和關(guān)節(jié)函數(shù)參數(shù)化等兩種協(xié)調(diào)穩(wěn)定控制方法，實(shí)現(xiàn)了系統(tǒng)角動(dòng)量的管理和重分配 。Yoshida等[19-23]研究了空間機(jī)器人系統(tǒng)之間的接觸動(dòng)力學(xué)，在捕獲非合作目標(biāo)過程中基于阻抗控制試驗(yàn)分析了機(jī)械臂與被動(dòng)飛行器之間的碰撞過程 ，同時(shí)在如何減小沖擊力上提出有效控制策略。Matsumoto 等[24]針對(duì) Hyper-OSV 進(jìn)行了抓捕策略研究，并對(duì)這些策略進(jìn)行了分類討論 。

以上對(duì)于抓捕接觸碰撞控制研究的側(cè)重點(diǎn)都是在研究接觸碰撞的基礎(chǔ)上尋求減小末端沖擊的方法，鮮有對(duì)抓捕過程滿足平穩(wěn)性要求的碰撞力控制研究。為此本文提出基于任務(wù)優(yōu)先級(jí)方法求解機(jī)器人系統(tǒng)運(yùn)動(dòng)逆解，采用顯式力控制的接觸碰撞力控制方法，通過與基于外力控制方法進(jìn)行比較分析，驗(yàn)證所提方法對(duì)抓捕接觸碰撞力控制的有效性，為解決空間機(jī)器人目標(biāo)抓捕接觸碰撞力控制問題提供技術(shù)支撐。

1 空間機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型

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

如圖 1所示，空間機(jī)器人由基座、4自由度操作臂以及操作手爪構(gòu)成。將慣性系記作ΣI，體坐標(biāo)系記作Σb，其中體坐標(biāo)系原點(diǎn)位于基座質(zhì)心，x軸為機(jī)械臂的安裝方向，z軸根據(jù)推進(jìn)器安裝位置確定，y軸指向與另外兩個(gè)軸構(gòu)成右手直角坐標(biāo)系。一般情況下為方便計(jì)算，使初始時(shí)刻體坐標(biāo)系與慣性坐標(biāo)系平行。本文運(yùn)動(dòng)學(xué)建模采用DH參數(shù)法，即將基座看做連桿0，然后從機(jī)械臂第一節(jié)連桿開始，依次遞推，直到最后一節(jié)機(jī)械臂，依次記作1,2,3,…,n。在基座上，機(jī)械臂的安裝位置坐標(biāo)系記作Σ0，為了方便，一般讓其與體坐標(biāo)系平行；連桿i末端的坐標(biāo)系記作Σi，其x軸與該連桿的方向相同，從該連桿的頭部指向尾部，其z軸的方向?yàn)樵撨B桿與上一節(jié)連桿之間的關(guān)節(jié)轉(zhuǎn)軸方向，y軸指向與另外兩個(gè)軸構(gòu)成右手直角坐標(biāo)系。連桿i+1繞Σi的z軸旋轉(zhuǎn)過的角度記作θn+1。連桿坐標(biāo)系之間的變換利用DH參數(shù)依次遞推得到。本文將圖 1中位于機(jī)械臂末端的坐標(biāo)系Σ4另記作Σe。

圖1 空間機(jī)器人DH坐標(biāo)Fig.1 DH coordinate diagram of space robot

定義慣性系下體坐標(biāo)系的位置和姿態(tài)為：

(1)

定義體坐標(biāo)系下體坐標(biāo)系相對(duì)于慣性系的線速度和角速度為：

(2)

則：

(3)

定義體坐標(biāo)系下空間機(jī)器人系統(tǒng)速度為：

(4)

可以將式(4)連同關(guān)節(jié)角寫成更加緊湊的形式：

(5)

定義慣性系下末端坐標(biāo)系的位置和姿態(tài)為：

(6)

末端位姿ηee可表示為體坐標(biāo)系位姿η和機(jī)械臂關(guān)節(jié)角q的函數(shù)，即：

ηee=k(η,q)

(7)

定義體坐標(biāo)系下末端坐標(biāo)系線速度和角速度為：

(8)

慣性系下機(jī)械臂末端的位置可以表示為：

(9)

對(duì)式(9)求導(dǎo)可以得到：

(10)

(11)

定義：

(12)

將式(11)和式(12)代入式(10)可以得到：

(13)

同時(shí)，有：

(14)

(15)

(16)

(17)

慣性系下機(jī)械臂末端的角速度可以表示為：

(18)

利用固定基座末端的雅克比矩陣可以將式(18)寫為：

(19)

(20)

則式(19)最終可以寫為：

(21)

式中：

(22)

最終得到表示機(jī)械臂末端六自由度完整運(yùn)動(dòng)學(xué)方程：

(23)

(24)

1.2 動(dòng)力學(xué)建模

本文采用牛頓-歐拉迭代法建立空間機(jī)器人動(dòng)力學(xué)模型。安裝在基座上機(jī)械臂上的任意連桿i的慣性力和慣性力矩可以表示為：

(25)

(26)

(27)

(28)

根據(jù)式(25)～式(28)可以進(jìn)行相應(yīng)的外向和內(nèi)向迭代計(jì)算。

(29)

式中：fdi和fvi是關(guān)節(jié)的干摩擦系數(shù)和粘性摩擦系數(shù)。

定義：

τq=[τq,1…τq,n]T

(30)

τ=[τvτq]T

(31)

式中：τv為空間機(jī)器人系統(tǒng)受到的合外力或力矩；τq為關(guān)節(jié)控制力矩。

空間機(jī)器人系統(tǒng)的動(dòng)力學(xué)方程最終可以表示為：

(32)

2 基于顯式力控制的碰撞力控制方法

圖 2所示為本文所涉及的操作手及目標(biāo)結(jié)構(gòu)。其中，O為末端D-H坐標(biāo)系的原點(diǎn)，x、y、z分別為末端D-H坐標(biāo)軸,操作手安裝在機(jī)械臂末端，e1,e2,e3分別為3個(gè)指與手掌的鉸鏈連接點(diǎn)，抓捕過程中手指e1a1b1,e2a2b2,e3a3b3分別繞e1,e2,e3同時(shí)轉(zhuǎn)動(dòng)實(shí)現(xiàn)手爪的開合，完成目標(biāo)抓捕；平面c為手眼相機(jī)前端平面，該平面與手掌平面(即e1,e2,e3組成的平面)平行。為簡(jiǎn)化模型，假設(shè)抓捕目標(biāo)為單獨(dú)圓柱桿，d1d2為圓柱桿中心線，半徑為r1。手指橫截面為橢圓，對(duì)應(yīng)的短軸半徑為r2。

顯式力控制方法是一種力位混合控制方法[26]，它將運(yùn)動(dòng)控制和力控制分開，是一種直接力控制方法，相比與通過控制機(jī)器人系統(tǒng)位置實(shí)現(xiàn)力控制目標(biāo)的外力控制方法(間接力控制方法)，其控制思想簡(jiǎn)單，容易理解?；陲@式力控制的碰撞力系統(tǒng)控制框圖如圖3所示。

圖2 手爪示意Fig.2 Schematic diagram of the claw

圖3 基于顯式力控制的碰撞力控制系統(tǒng)框圖Fig.3 Block diagram of collision force control system based on explicit force control

2.1 碰撞模型

本文采用赫茲接觸力碰撞模型計(jì)算作用在目標(biāo)上的接觸力Fcmi，

(33)

式中：kg為接觸碰撞剛性系數(shù)；kc為接觸碰撞阻尼系數(shù)；δ為形變量，目標(biāo)與手指碰撞的產(chǎn)生的形變量δi=di-(r1+r2),i=1,2,…,6，與相機(jī)平面碰撞產(chǎn)生的形變量δ7=d7-r1；ni為作用在目標(biāo)上的碰撞接觸力方向。在末端D-H坐標(biāo)下，根據(jù)設(shè)定的手爪閉合速度，計(jì)算當(dāng)前手指位置，并分別計(jì)算目標(biāo)軸線到手指e1a1b1(包括線段e1a1，線段a1b1)，e1a1b1(包括線段e2a2，線段a2b2)，e3a3b3(包括線段e3a3，線段a3b3)和c平面的距離d1,d2,d3,d4,d5,d6和d7。

作用在目標(biāo)上的碰撞接觸力合力為Fcm，

Fcm=∑Fcmi,i=1,2,…,7

(34)

作用在抓捕機(jī)構(gòu)上的合力Fcs=-Fcm。需要說明的是此處Fcmi,Fcm,Fcs均為在最后一個(gè)機(jī)械臂的坐標(biāo)系下。

2.2 任務(wù)優(yōu)先閉環(huán)運(yùn)動(dòng)學(xué)控制

本文空間機(jī)器人系統(tǒng)共10個(gè)自由度，大于末端位姿的6個(gè)自由度，因此是運(yùn)動(dòng)學(xué)冗余的。

任務(wù)優(yōu)先冗余解決方法[28-29]可以很好地實(shí)現(xiàn)優(yōu)先級(jí)高的任務(wù)，同時(shí)兼顧次優(yōu)的任務(wù)。本文將基于任務(wù)優(yōu)先方法的運(yùn)動(dòng)學(xué)閉環(huán)控制方法與接觸碰撞力控制融合，可獲得較好的接觸力控制效果。

對(duì)于任意任務(wù)a：

σa=σa(η,q)

(35)

相應(yīng)的雅可比矩陣是：

(36)

定義代價(jià)函數(shù)：

f=ζTζ

(37)

滿足式(23)的約束，通過使得代價(jià)函數(shù)f最小，可以得到：

(38)

(39)

3個(gè)任務(wù)的方程為：

(40)

當(dāng)利用上述算法積分求解基時(shí)，會(huì)造成數(shù)字漂移，使得實(shí)際得到的值與期望值有偏差。對(duì)此可以通過加入比例項(xiàng)盡量消除這些積分漂移，由此可得式(40)的閉環(huán)形式為:

(41)

式中：ka,kb,kc是選取的反饋系數(shù)，可以使相應(yīng)的積分漂移收斂到零。特別地，選取控制機(jī)械臂末端執(zhí)行器位姿閉環(huán)形式為：

(42)

得到閉環(huán)形式的ζr后，便可以利用逆運(yùn)動(dòng)學(xué)得到慣性系下的參考軌跡：

(43)

2.3 反饋線性化動(dòng)力學(xué)控制方法

空間機(jī)器人整個(gè)系統(tǒng)是一個(gè)典型的多體系統(tǒng)。本節(jié)利用反饋線性化、參數(shù)線性化的相關(guān)理論，設(shè)計(jì)相應(yīng)的控制器，控制基座推力、力矩以及關(guān)節(jié)力來跟蹤基于任務(wù)優(yōu)先方法的運(yùn)動(dòng)學(xué)閉環(huán)控制方法(第2.2小節(jié))給出的參考軌跡。

整個(gè)多體系統(tǒng)的動(dòng)力學(xué)方程為：

(44)

令控制律為：

(45)

式中：

(46)

而

(47)

(48)

(49)

2.4 接觸碰撞力控制律設(shè)計(jì)

由圖3及上述推導(dǎo)可得基于顯式力控制的接觸碰撞力控制方法的控制律：

τ=τM+τF

(50)

式中：τ為施加的運(yùn)動(dòng)控制力矩；τF為施加的力控制力矩，

(51)

(52)

基于外力控制的接觸碰撞力控制律為：

(53)

其中

(54)

3 接觸碰撞力控制仿真分析

本節(jié)將對(duì)基于顯式力控制的抓捕接觸碰撞力控制方法和基于外力控制的抓捕接觸碰撞力控制方法進(jìn)行仿真對(duì)比分析，其中A組為后者的仿真結(jié)果，B組為前者的仿真結(jié)果。仿真條件如下：

1)初始時(shí)刻目標(biāo)在末端坐標(biāo)系中的坐標(biāo)為[0.07,0.01,0]；

2)手爪閉合旋轉(zhuǎn)速度為w=0.154 6 rad/s。

仿真結(jié)果如圖4～圖10所示。

圖4 兩組基座控制力仿真結(jié)果Fig.4 Simulation results of two groups of base control force

圖5 兩組基座控制力矩仿真結(jié)果Fig.5 Simulation results of control moment of two groups of bases

圖6 兩組關(guān)節(jié)控制力矩仿真結(jié)果Fig.6 Simulation results of two groups of joint control moments

由圖4～圖6的仿真結(jié)果可以看出，A組的基座控制力和力矩約為B組基座控制力和力矩的10倍，并且A組基座力和力矩變化曲線有多處突變，B組基座力和力矩變化曲線相對(duì)平緩。對(duì)于關(guān)節(jié)控制力矩來說，B組關(guān)節(jié)力矩在初始時(shí)刻較大，之后保持在較小的范圍內(nèi)，且變化較為規(guī)律，A組整體來講關(guān)節(jié)力矩較大且變化沒有規(guī)律。

從圖7可以看出，A組控制下基座的位姿擾動(dòng)量約為B組控制下機(jī)器人系統(tǒng)位姿擾動(dòng)擾動(dòng)量的2倍。

由圖8～圖10可見，A、B兩組碰撞接觸力的變化趨勢(shì)以及各個(gè)接觸力的最大值相同，碰撞合力的最大值相近，基于顯式力控制的過程呈現(xiàn)出振蕩收斂的特性，而基于外力控制下的曲線變化相對(duì)比較平滑。兩組目標(biāo)質(zhì)心軌跡趨勢(shì)大致相同，B組過程中存在小幅震蕩。

圖7 兩組空間機(jī)器人系統(tǒng)位置仿真結(jié)果Fig.7 Simulation results of two groups of space robot systems

圖8 兩組接觸碰撞合力仿真結(jié)果Fig.8 Simulation results of two groups of contact collision resultant force

圖9 兩組接觸碰撞力仿真結(jié)果Fig.9 Simulation results of two groups of contact collision forces

圖10 兩組目標(biāo)質(zhì)心運(yùn)動(dòng)軌跡仿真結(jié)果Fig.10 Simulation results of the motion trajectories of the two groups of target centroids

4 結(jié)束語

本文針對(duì)空間機(jī)器人抓捕漂浮目標(biāo)過程中的接觸碰撞力控制問題，提出了一種基于顯式力控制的空間機(jī)器人抓捕碰撞力控制方法。研究結(jié)果顯示：

1)基于顯式力控制的抓捕接觸碰撞力控制方法可以有效的控制接觸碰撞力，在抓捕過程中使系統(tǒng)處于平穩(wěn)狀態(tài)。

2)基于顯式力控制的抓捕接觸碰撞力控制方法所需的基座控制力和力矩遠(yuǎn)小于基于外力控制的抓捕接觸碰撞力控制方法所需的控制力和力矩，滿足空間機(jī)器人系統(tǒng)的穩(wěn)定和盡可能少的能量消耗要求。

分析碰撞力對(duì)目標(biāo)的影響時(shí)，忽略了目標(biāo)的旋轉(zhuǎn)運(yùn)動(dòng)，因此針對(duì)旋轉(zhuǎn)運(yùn)動(dòng)較大的目標(biāo)還需要進(jìn)一步考慮目標(biāo)旋轉(zhuǎn)運(yùn)動(dòng)的影響；此外，基于接觸表面的摩擦和阻滯較小的前提，所采用的碰撞模型沒有考慮接觸表面的摩擦和阻滯系數(shù)，這一點(diǎn)在其不可忽視的情況下也需要進(jìn)一步研究。