基于顯式力控制的空間機器人抓捕碰撞力控制方法

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

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

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

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

1 空間機器人運動學(xué)和動力學(xué)模型

1.1 運動學(xué)模型

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

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

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

(1)

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

(2)

則：

(3)

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

(4)

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

(5)

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

(6)

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

ηee=k(η,q)

(7)

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

(8)

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

(9)

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

(10)

(11)

定義：

(12)

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

(13)

同時，有：

(14)

(15)

(16)

(17)

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

(18)

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

(19)

(20)

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

(21)

式中：

(22)

最終得到表示機械臂末端六自由度完整運動學(xué)方程：

(23)

(24)

1.2 動力學(xué)建模

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

(25)

(26)

(27)

(28)

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

(29)

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

定義：

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

(30)

τ=[τvτq]T

(31)

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

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

(32)

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

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

顯式力控制方法是一種力位混合控制方法[26]，它將運動控制和力控制分開，是一種直接力控制方法，相比與通過控制機器人系統(tǒng)位置實現(xiàn)力控制目標(biāo)的外力控制方法(間接力控制方法)，其控制思想簡單，容易理解?；陲@式力控制的碰撞力系統(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 碰撞模型

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

(33)

式中：kg為接觸碰撞剛性系數(shù)；kc為接觸碰撞阻尼系數(shù)；δ為形變量，目標(biāo)與手指碰撞的產(chǎn)生的形變量δi=di-(r1+r2),i=1,2,…,6，與相機平面碰撞產(chǎn)生的形變量δ7=d7-r1；ni為作用在目標(biāo)上的碰撞接觸力方向。在末端D-H坐標(biāo)下，根據(jù)設(shè)定的手爪閉合速度，計算當(dāng)前手指位置，并分別計算目標(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)

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

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

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

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

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

σa=σa(η,q)

(35)

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

(36)

定義代價函數(shù)：

f=ζTζ

(37)

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

(38)

(39)

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

(40)

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

(41)

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

(42)

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

(43)

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

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

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

(44)

令控制律為：

(45)

式中：

(46)

而

(47)

(48)

(49)

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

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

τ=τM+τF

(50)

式中：τ為施加的運動控制力矩；τF為施加的力控制力矩，

(51)

(52)

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

(53)

其中

(54)

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

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

1)初始時刻目標(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組基座力和力矩變化曲線相對平緩。對于關(guān)節(jié)控制力矩來說，B組關(guān)節(jié)力矩在初始時刻較大，之后保持在較小的范圍內(nèi)，且變化較為規(guī)律，A組整體來講關(guān)節(jié)力矩較大且變化沒有規(guī)律。

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

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

圖7 兩組空間機器人系統(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ì)心運動軌跡仿真結(jié)果Fig.10 Simulation results of the motion trajectories of the two groups of target centroids

4 結(jié)束語

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

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

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

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