多臂空間機器人的視覺伺服與協(xié)調(diào)控制

馮 驍，盧 山，侯月陽，王奉文，賈英宏

(1. 北京航空航天大學(xué)宇航學(xué)院，北京 100083；2. 上海航天控制技術(shù)研究所，上海 201109；3. 上海市空間智能控制技術(shù)重點實驗室，上海 201109)

0 引 言

由于太空環(huán)境的危險性，使用空間機器人代替航天員進行在軌操作已成為一個重要的研究課題[1]。具有多部機械臂的空間機器人能夠完成復(fù)雜的操作，已成為空間機器人設(shè)計的重要發(fā)展方向。通常，航天器與地面間存在通信延遲，增強空間機器人執(zhí)行任務(wù)的自動化程度將有助于提高其執(zhí)行任務(wù)的效率。因此，多臂空間機器人自主在軌操作的相關(guān)技術(shù)具有研究和應(yīng)用價值。關(guān)于空間機器人技術(shù)的詳細綜述可以參考文獻[1]和文獻[2]，下面僅介紹本文內(nèi)容相關(guān)的現(xiàn)有成果。

對多臂空間機器人本身及其與目標(biāo)間相對運動的建模是實現(xiàn)自主在軌抓捕的基礎(chǔ)。利用Kane方法導(dǎo)出多體系統(tǒng)動力學(xué)的過程簡單而有序，因此受到許多研究者的注意。文獻[3]利用Kane方程導(dǎo)出了任意樹形拓撲構(gòu)型柔性多體系統(tǒng)的動力學(xué)方程，其形式適合計算機編程實現(xiàn)。文獻[4]建立遞推運動學(xué)關(guān)系，文獻[5]引入偏速度、偏角速度矩陣，簡化了運動學(xué)變量求解公式的形式。以這些動力學(xué)模型為基礎(chǔ)編寫的軟件已在實踐中應(yīng)用。

融入視覺信息的軌跡規(guī)劃是實現(xiàn)自主在軌抓捕的關(guān)鍵。由于目標(biāo)相對航天器平臺的運動存在不確定性，需要對運動的狀態(tài)進行測量，并將測量信息引入機械臂的軌跡規(guī)劃中，使機械臂能根據(jù)目標(biāo)運動的情況調(diào)整自身運動。使用手眼相機拍攝目標(biāo)[6]并采用視覺伺服算法[7]是實現(xiàn)上述功能的一種較為成熟的方案。文獻[6]將基于位置的視覺伺服用于雙臂空間機器人的在線軌跡規(guī)劃，文獻[8]則在工程項目中驗證了類似方法在單臂空間機器人系統(tǒng)中的適用性。僅使用視覺伺服進行規(guī)劃得到的機械臂運動會對航天器平臺造成較大的姿態(tài)擾動。文獻[9]提出冗余機械臂在反作用零空間中運動時不會造成平臺姿態(tài)擾動，這樣的運動稱為零反作用機動。但對單臂空間機器人而言，進行零反作用機動時末端作用器的運動范圍十分有限。文獻[10]利用受限最小干擾圖對雙臂空間機器人進行軌跡規(guī)劃，使平臺姿態(tài)在機械臂運動過程中基本保持不變?？臻g多臂機器人在線軌跡規(guī)劃的一個新進展是將視覺伺服與零反作用機動相結(jié)合，使機械臂在對平臺無擾動的約束下抓捕目標(biāo)，這一方法使用基于圖像的視覺伺服，直接使用圖片特征作為測量量[11]。將視覺伺服的另一類方法(基于位置的視覺伺服)與零反作用機動結(jié)合的研究則比較少見。

由于零反作用機動的運動范圍有限，當(dāng)不能在軌跡規(guī)劃中消除機械臂對平臺的擾動時，需要協(xié)調(diào)地控制機械臂與平臺。文獻[12]從角動量層面推導(dǎo)協(xié)調(diào)關(guān)系，在平臺和機械臂各自獨立控制系統(tǒng)的基礎(chǔ)上建立協(xié)調(diào)控制器，通過前饋補償機械臂對平臺的擾動。這一方法的工程實現(xiàn)可參見文獻[13]，文獻[14]也采用了類似的方法。這種協(xié)調(diào)控制方法一方面可以保留平臺和機械臂已有的(經(jīng)過實踐檢驗的)控制器設(shè)計，另一方面又在控制器中引入了平臺與機械臂的相互作用，十分適合工程應(yīng)用。除了從角動量層面建立協(xié)調(diào)控制器，還可以基于系統(tǒng)整體動力學(xué)模型導(dǎo)出協(xié)調(diào)控制機械臂與平臺的逆動力學(xué)控制器，其思路更簡明，控制性能更容易預(yù)測。另外，對于空間機器人插拔模塊的過程，文獻[15]研究了其力/位置混合控制方法。對于多臂空間機器人協(xié)調(diào)操作載荷的任務(wù)，文獻[16]則提出了位置/內(nèi)力的混合控制策略。

雖然空間機器人的建模、視覺伺服軌跡規(guī)劃和協(xié)調(diào)控制都有較為成熟的方法，但適用于多臂空間機器人系統(tǒng)工程應(yīng)用的綜合方案尚不多見。如何將這些方法結(jié)合在一起解決多臂航天器的自主在軌操作問題并不十分清晰。另一方面，有必要建立結(jié)合基于位置的視覺伺服和零反作用機動的方法。本文提出一種適用于多臂空間機器人的建模、視覺伺服與協(xié)調(diào)控制的方案，并結(jié)合基于位置的視覺伺服與零反作用機動建立一種在線軌跡規(guī)劃方法。為了驗證算法和進行仿真分析，基于Matlab編寫了多臂空間機器人仿真(Multi-arm space-robot simulation,MASS)軟件。

1 系統(tǒng)描述與運動學(xué)、動力學(xué)建模

1.1 系統(tǒng)描述

多臂空間機器人自主抓捕的示意圖如圖 1所示。其中多臂空間機器人由一個航天器平臺和L部安裝在其上的機械臂構(gòu)成，平臺具有力、力矩執(zhí)行機構(gòu)和一部測量目標(biāo)位置的相機，第i部機械臂有Ni個電機驅(qū)動的旋轉(zhuǎn)關(guān)節(jié)，每部機械臂的末端安裝有一部用于視覺測量的手眼相機。

1.2 運動學(xué)建模

航天器平臺的運動學(xué)為

(1)

式中：rb為Fb原點在FI中的位置；Rb為Fb到FI的坐標(biāo)轉(zhuǎn)換陣；vb為Fb原點相對FI的速度在Fb中的投影；Θb為Fb相對FI的姿態(tài)角，本文使用3-1-2姿態(tài)角(ψ,φ,θ)；G(Θb)由姿態(tài)角的轉(zhuǎn)軸列陣排列而成，表示姿態(tài)角變化率與角速度間的線性關(guān)系；ωb為Fb相對FI的角速度在Fb中的投影；變量上的點表示時間導(dǎo)數(shù)。

Ft原點相對Fb運動的關(guān)系為

(2)

(3)

(4)

(5)

(6)

對第i部機械臂引入其關(guān)節(jié)角列陣qi。rb,Θb和qi(i=1,2,…,L)組成了多臂空間機器人的廣義坐標(biāo)y,

(7)

(8)

1.3 動力學(xué)建模

Kane方法通過將力矢量向偏速度方向投影而消去多體系統(tǒng)中的約束力，導(dǎo)出多體的動力學(xué)方程，適合用于多臂空間機器人的動力學(xué)建模。

使用文獻[5]中矩陣形式的Kane方法，可將系統(tǒng)的動力學(xué)方程寫為

(9)

式中：FA為作用于系統(tǒng)上的廣義主動力,M為廣義質(zhì)量矩陣,FIt為非線性廣義慣性力。不考慮環(huán)境對空間機器人的作用時，有

(10)

式中：Fb為作用于平臺的主動力系的主矢在Fb中的投影,Tb為作用于平臺的主動力系對Fb原點的主矩在Fb中的投影,τi為第i部機械臂的關(guān)節(jié)力矩組成的列陣。M,FIt可由系統(tǒng)中各體的貢獻求和得到：

(11)

(12)

各體對質(zhì)量陣和非線性廣義慣性力貢獻的求解方法可參考文獻[5]。

2 視覺伺服

2.1 基于位置的視覺伺服

機械臂對平臺的擾動及目標(biāo)本身的運動會導(dǎo)致目標(biāo)相對Fb運動，因此需在機械臂的軌跡規(guī)劃中引入對目標(biāo)的測量，并進行在線的軌跡規(guī)劃，視覺伺服是一種比較成熟的實現(xiàn)方法。本文參照文獻[7]，使用基于位置的視覺伺服方法。

(13)

(14)

(15)

結(jié)合機械臂的幾何雅克比矩陣，有

(16)

式中：

(17)

(18)

式中:iJ(qi)為第i部機械臂的幾何雅克比矩陣。

對于無冗余的機械臂(6自由度)，可以使用速率分解規(guī)劃公式

(19)

式中：Ki為對稱正定矩陣。在關(guān)節(jié)角控制較好，即qi≈qr(i)時，有

(20)

抓捕誤差將收斂到零。

2.2 零反作用軌跡規(guī)劃

對于具有冗余的機械臂，軌跡規(guī)劃可使用偽逆解

(21)

對于無外力作用的多臂空間機器人系統(tǒng)，在初始靜止的條件下，其對平臺質(zhì)心的角動量L0有表達式[12]

(22)

式中：Is為與平臺運動相關(guān)的慣量，Im(i)為與機械臂相關(guān)的慣量。由于Is始終可逆，機械臂不對平臺產(chǎn)生擾動的條件是

(23)

觀察式(23)可知其并不要求各項單獨為零，這一數(shù)學(xué)表述的物理意義是，多臂航天器的零反作用機動是協(xié)調(diào)進行的，其中存在各臂間擾動抵消的情況。

由于系統(tǒng)冗余自由度數(shù)目及機械臂幾何、質(zhì)量參數(shù)和構(gòu)型的限制，一般很難使用零反作用運動實現(xiàn)對目標(biāo)的精確抓捕。因此，在每一時刻構(gòu)造、求解約束最小二乘問題：

(24)

可見，約束要求多臂航天器的機械臂嚴格在零反作用空間中運動；指標(biāo)函數(shù)則是對精確抓捕的放松，其物理意義是盡量使機械臂末端與目標(biāo)間的誤差減小。

(25)

3 協(xié)調(diào)控制器設(shè)計

零反作用機動能夠使平臺姿態(tài)不受機械臂運動的擾動，但利用這一機動可達到的空間一般是有限的。為了精確地抓捕目標(biāo)，需在抓捕的最后階段關(guān)閉零反作用機動功能，使用式(19)或(21)而不是式(25)的結(jié)果進行軌跡規(guī)劃。這時，機械臂運動將對平臺產(chǎn)生不可忽視的擾動，需要協(xié)調(diào)地控制機械臂和平臺的運動。在工程應(yīng)用中，平臺的控制系統(tǒng)和機械臂的控制系統(tǒng)往往是相互獨立設(shè)計的，由于機械臂運動對平臺的擾動難以被常規(guī)的平臺控制系統(tǒng)補償，需要建立在獨立控制系統(tǒng)之上的協(xié)調(diào)控制器，使平臺控制力矩及時補償擾動。若希望建立平臺、機械臂的一體化控制器，基于多臂航天器系統(tǒng)的動力學(xué)模型設(shè)計逆動力學(xué)控制器則是一種常用的方法，可以充分考慮機械臂與平臺間的運動耦合關(guān)系。本文采用以上兩種方法構(gòu)建多臂空間機器人的協(xié)調(diào)控制器。

3.1 基于角動量前饋補償?shù)膮f(xié)調(diào)控制算法

假設(shè)平臺與機械臂的獨立控制系統(tǒng)已經(jīng)設(shè)計完成(平臺控制系統(tǒng)包括姿態(tài)控制與目標(biāo)相對位置控制，基于平臺動力學(xué)模型和式(1)～(4)的運動學(xué)關(guān)系設(shè)計)。但基于反饋的姿態(tài)控制系統(tǒng)往往不能及時補償機械臂運動對姿態(tài)造成的擾動，需要在考慮機械臂與平臺運動耦合的基礎(chǔ)上進行補償。與建立零反作用機動的方法類似，為了導(dǎo)出協(xié)調(diào)控制算法，仍由角動量表達式出發(fā)，并假設(shè)系統(tǒng)初始時靜止，但此時平臺上有主動控制力矩的作用，即

(26)

(27)

式中：T0為作用于平臺的主動力系對平臺質(zhì)心的主矩在Fb中的投影。為了保持平臺姿態(tài)不受擾動，應(yīng)使外力矩的積分與機械臂運動的角動量完全相同，即

(28)

在實際中，為了防止前饋導(dǎo)致的不穩(wěn)定，可以通過前饋系數(shù)Kff對前饋角動量進行調(diào)節(jié)，最終取前饋補償規(guī)律為

(29)

對于可以接受角動量指令的執(zhí)行機構(gòu)，可以將前饋角動量直接加入指令中

hd=hff+hf b

(30)

式中：hd為包含平臺反饋控制與前饋補償作用的總角動量指令，hf b為平臺姿態(tài)控制系統(tǒng)(反饋控制)產(chǎn)生的角動量指令。對于接受力矩指令的執(zhí)行機構(gòu),則可以再設(shè)置一個角動量控制回路，使執(zhí)行機構(gòu)提供的角動量跟蹤期望值。

(31)

式中：Km為對稱正定的控制增益矩陣，h為T0作用積累的角動量。

3.2 逆動力學(xué)控制算法

基于角動量前饋補償?shù)膮f(xié)調(diào)控制在平臺和機械臂相互獨立的控制系統(tǒng)上構(gòu)建了基于角動量關(guān)系的前饋補償機制，使得平臺的姿態(tài)控制系統(tǒng)能夠獲取機械臂運動產(chǎn)生的擾動量并進行補償。更進一步，可以在多臂空間機器人系統(tǒng)動力學(xué)模型的基礎(chǔ)上設(shè)計同時控制平臺和機械臂運動的逆動力學(xué)控制器。

由系統(tǒng)的動力學(xué)方程出發(fā)

(32)

(33)

4 仿真校驗

為了校驗上述視覺伺服與協(xié)調(diào)控制算法，對雙臂空間機器人自主抓捕目標(biāo)航天器上兩個對稱目標(biāo)點的任務(wù)進行仿真。圖2為系統(tǒng)的示意圖。航天器平臺質(zhì)量接近2 t。兩機械臂的構(gòu)型相同，對稱安裝在平臺z軸的正、負方向，其中+z方向的稱為機械臂1，-z方向的稱為機械臂2。每部機械臂重約70 kg，長約4 m，有7個關(guān)節(jié)。Ft的原點不運動，姿態(tài)相對FI作姿態(tài)角3°,角速度0.3°/s的晃動(Ft，F(xiàn)I的定義參見圖 1)。

首先考慮不對平臺施加主動控制，而使機械臂在視覺伺服下進行自主抓捕的情況(稱為算例1)，圖3為其系統(tǒng)框圖。這一情況將被作為對比的基線。在軌跡規(guī)劃中，對每個關(guān)節(jié)施加2°/s的關(guān)節(jié)轉(zhuǎn)速限制。仿真結(jié)果如圖4～圖11所示。由圖4～圖7可知，機械臂1和機械臂2均能準(zhǔn)確完成抓捕目標(biāo)的任務(wù)。但是，目標(biāo)航天器在Fb內(nèi)的位置距期望值(Fb中的一個固定位置)有較大的偏移，且Fb的姿態(tài)也有比較大的擺動。從這一結(jié)果可以看出使用零反作用軌跡規(guī)劃對平臺進行控制的必要性。

其次，考慮將零反作用機動引入在線軌跡規(guī)劃的情況(稱為算例2)，圖12為其系統(tǒng)框圖。在機械臂初始構(gòu)型、目標(biāo)均對稱的情況下，機械臂1與機械臂2對平臺產(chǎn)生的擾動會比較精確地相互抵消，這使得精確抓捕任務(wù)在零反作用機動下就可以完成。但這種情況在機械臂任務(wù)中是比較特殊和少見的。因此，為了使結(jié)果更具有一般化的意義，在仿真中將機械臂1與機械臂2的初始構(gòu)型作不同的設(shè)置。仿真結(jié)果如圖13～圖18所示。因為零反作用軌跡規(guī)劃優(yōu)先保證對平臺的無擾動，而將機械臂對目標(biāo)的抓捕作為優(yōu)化目標(biāo)，所以引入零反作用軌跡規(guī)劃后，機械臂對目標(biāo)的抓捕出現(xiàn)了一定的偏差，而平臺的姿態(tài)則不受擾動(注意到姿態(tài)角誤差數(shù)量均在10-5(°)級別)。在實際中，可以先使機械臂沿零反作用軌跡運動，靠近目標(biāo)后再切換成常規(guī)的視覺伺服，完成精確抓捕。

最后，考慮不使用零反作用軌跡規(guī)劃時對平臺進行控制?？刂颇繕?biāo)為機械臂精確抓捕目標(biāo)，同時平臺保持其相對目標(biāo)航天器系原點的位置并鎮(zhèn)定自身的姿態(tài)。

一方面，使用基于角動量前饋補償?shù)膮f(xié)調(diào)控制方法(稱為算例3)，圖19為系統(tǒng)框圖，角動量前饋系數(shù)取為0.8。機械臂抓捕的結(jié)果與基線情況類似，但不包含平臺隨機械臂運動時機械臂進行的附加運動。平臺的控制結(jié)果如圖20～圖25所示。目標(biāo)航天器相對平臺的位置誤差和平臺的姿態(tài)誤差相比基線仿真明顯降低，說明了這一協(xié)調(diào)控制方法的有效性。從機械臂對平臺的擾動力矩和平臺控制力矩的對比可以看出，大部分擾動力矩及時地被平臺控制力矩補償，其對平臺姿態(tài)造成的影響被有效降低。

另一方面，在逆動力學(xué)控制下(見圖26)，機械臂的控制效果與基于角動量前饋補償?shù)膮f(xié)調(diào)控制基本一致，而由于控制器是在整個系統(tǒng)的動力學(xué)模型上設(shè)計的全狀態(tài)反饋控制器，其可以完全消除機械臂運動帶來的平臺擾動，平臺控制力、力矩完全補償機械臂對平臺的擾動力、力矩。

5 MASS軟件設(shè)計

在上述理論的基礎(chǔ)上，使用Matlab編寫了仿真軟件MASS。其具有多臂空間機器人動力學(xué)、運動學(xué)仿真能力，并可由用戶自定義控制作用和輸出(第2、第3節(jié)的軌跡規(guī)劃與控制算法已通過這一接口實現(xiàn))。

MASS的核心是多臂空間機器人的系統(tǒng)常微分方程求解器，求解動力學(xué)、運動學(xué)方程

(34)

(35)

和用戶自定義控制器

FA=fc(t,y,u,z)

(36)

(37)

組成的一階常微分方程系統(tǒng)。另外，用戶可自定義輸出變量，對仿真結(jié)果作詳細的分析。

6 結(jié) 論

本文研究多臂空間機器人自主抓捕技術(shù)中的視覺伺服與協(xié)調(diào)控制問題。研究結(jié)果表明：多臂空間機器人可實現(xiàn)各臂對各自目標(biāo)的抓捕，利用視覺伺服進行在線軌跡規(guī)劃、進而實現(xiàn)自主抓捕具有可行性；零反作用機動可以有效消除機械臂對平臺姿態(tài)的擾動，對提高平臺姿態(tài)控制精度具有顯著作用；基于角動量前饋補償?shù)膮f(xié)調(diào)控制器和逆動力學(xué)控制器均可實現(xiàn)對系統(tǒng)的協(xié)調(diào)控制，前者相對簡單且更接近工程應(yīng)用，后者更易理解但計算相對復(fù)雜。

[1] Flores-Abad A, Ma O, Pham K, et al. A review of space robotics technologies for on-orbit servicing [J]. Progress in Aerospace Sciences, 2014, 68: 1-26.

[2] Xu W, Liang B, Xu Y. Survey of modeling, planning, and ground verification of space robotic systems [J]. Acta Astronautica, 2011, 68(11): 1629-1649.

[3] Singh R P, VanderVoort R J, Likins P W. Dynamics of flexible bodies in tree topology-a computer-oriented approach [J]. Journal of Guidance, Control, and Dynamics, 1985, 8(5): 584-590.

[4] Banerjee A K, Lemak M E. Multi-flexible body dynamics capturing motion-induced stiffness [J]. ASME, Transactions, Journal of Applied Mechanics, 1991, 58: 766-775.

[5] 胡權(quán), 賈英宏, 徐世杰. 多體系統(tǒng)動力學(xué) Kane 方法的改進 [J]. 力學(xué)學(xué)報, 2011, 43(5): 968-972. [Hu Quan, Jia Ying-hong, Xu Shi-jie. An improved Kane’s method for multibody dynamics [J]. Chinese Journal of Theoretical and Applied Mechanics, 2011, 43(5): 968-972.]

[6] 徐文福, 劉厚德, 李成, 等. 雙臂空間機器人捕獲運動目標(biāo)的自主路徑規(guī)劃 [J]. 機器人, 2012, 34(6): 704-714. [Xu Wen-fu, Liu Hou-de, Li Cheng, et al. Autonomous path planning of dual-arm space robot for capturing moving target [J]. Robot, 2012, 34(6): 704-714.]

[7] Siciliano B, Sciavicco L, Villani L, et al. Robotics: modelling, planning and control [M]. London: Springer-Verlag, 2009.

[8] Inaba N, Mitsushige O D A, Hayashi M. Visual servoing of space robot for autonomous satellite capture [J]. Transactions of the Japan Society for Aeronautical and Space Sciences, 2003, 46(153): 173-179.

[9] Yoshida K, Hashizume K, Abiko S. Zero reaction maneuver: flight validation with ETS-VII space robot and extension to kinematically redundant arm [C]. International Conference on Robotics and Automation, Seoul, Korea, May 21-26, 2001.

[10] 洪炳熔, 王鴻鵬. 雙臂自由飛行空間機器人的姿態(tài)控制方案 [J]. 宇航學(xué)報, 2000, 21(2): 21-28. [Hong Bing-rong, Wang Hong-peng. Scheme of attitude control of dual-arm free-flying space robot [J]. Journal of Astronautics, 2000, 21(2): 21-28.]

[11] Hafez A H A, Mithun P, Anurag V V, et al. Reactionless visual servoing of a multi-arm space robot combined with other manipulation tasks [J]. Robotics and Autonomous Systems, 2017, 91: 1-10.

[12] Yoshida K. Practical coordination control between satellite attitude and manipulator reaction dynamics based on computed momentum concept[C]. Intelligent Robots and Systems, Munich, Germany, September 12-16, 1994.

[13] Mitsushige O. Motion control of the satellite mounted robot arm which assures satellite attitude stability [J]. Acta Astronautica, 1997, 41(11): 739-750.

[14] 徐文福, 李成, 梁斌, 等. 空間機器人捕獲運動目標(biāo)的協(xié)調(diào)規(guī)劃與控制方法 [J]. 自動化學(xué)報, 2009, 35(9): 1216-1225. [Xu Wen-fu, Li Cheng, Liang Bin, et al. Coordinated planning and control method of space robot for capturing moving target [J]. Acta Automatic Sinica, 2009, 35(9): 1216-1225.]

[15] 徐文福, 周瑞興, 孟得山. 空間機器人在軌更換ORU的力/位混合控制方法 [J]. 宇航學(xué)報, 2013, 34(10): 1353-1361. [Xu Wen-fu, Zhou Rui-xing, Meng De-shan. A hybrid force/position control method of space robot performing on-orbit ORU replacement [J]. Journal of Astronautics, 2013, 34(10): 1353-1361.]

[16] 袁景陽, 柳長安, 李瑰賢. 多臂協(xié)調(diào)操作自由飛行空間機器人的位置和內(nèi)力混合控制 [J]. 宇航學(xué)報, 2007, 28(4): 1049-1052. [Yuan Jing-yang, Liu Chang-an, Li Gui-xian. Position and internal force hybrid control algorithm of multi-arm coordinated operate free flying space robot [J]. Journal of Astronautics, 2007, 28(4): 1049-1052.]