空間包絡式抓捕機構非接觸/接觸一體化導納控制

劉 磊，張 濤

（1.空間智能控制技術全國重點實驗室，北京 100094；2.清華大學自動化系，北京 100084）

0 引言

隨著人類航天活動更加頻繁，失效衛(wèi)星、火箭上面級等空間碎片占據著重要的軌道資源，并對在軌運行航天器的安全性造成重大威脅［1-4］。這類目標通常不具備專門的抓捕對接裝置，幾何形狀和運動狀態(tài)均未知，屬于典型的非合作目標。非合作目標的接觸式剛性捕獲方法包括單臂抓捕、多臂抓捕以及并聯(lián)機構抓捕等。單臂抓捕通常依賴末端夾爪，僅適用于抓捕存在夾持點的空間碎片。多臂抓捕機構靈活性好，能夠實現多樣性的抓捕策略，如指尖抓捕、多指節(jié)抓捕等。指尖抓捕僅通過多臂的末端（指尖）與目標形成接觸點，需要較為精細的控制［5］。多指節(jié)抓捕涉及不同臂桿（指節(jié)）與目標表面的接觸，能更好地適應目標形狀［6-7］。隨著近年來地面類人手的發(fā)展，一些欠驅動類人手的設計方案也被應用到空間碎片抓捕領域，其模仿人手的結構與運動，能夠自然地抓取大型目標［8］。多指節(jié)抓捕與類人手抓捕在一定程度上都可視作包絡式抓捕方法，通過將目標完全包圍從而提高抓取穩(wěn)定性。然而，通過優(yōu)化方法快速精確求解接觸點與接觸力仍然是極具挑戰(zhàn)性的任務。

除了多臂形成的包絡抓捕，有學者提出了一種空間包絡式的并聯(lián)抓捕機構，該機構通過多個臂桿鉸接形成包絡體，依靠機構與目標的多點多次接觸，完成對目標的牢固捕獲［9］。與目前常見的飛網柔性抓捕、多臂抓捕等方式相比，該方案具有適用性強（對目標的大小、形狀要求不高，且無需專門的對接裝置）、捕獲難度低（無需測量或估計目標的運動狀態(tài)）、捕獲牢固（通過多點加持，實現牢固捕獲）的特點［9］。但是，該機構的構型復雜，受到多個閉環(huán)約束，導致動力學和運動學建模難度大，進一步給控制設計帶來困難。相應的建模方法包括變拓撲動力學［10］、旋量理論等。本文以空間包絡式抓捕機構為研究對象，建立其動力學模型，針對捕獲過程的控制，提出一種非接觸/接觸一體化的導納控制方案，實現捕獲過程的柔順控制。

目前，在機器人的主動柔順控制領域，力/位置混合控制［11］與阻抗控制［12］是應用較為廣泛的兩種控制策略。其中，廣義的阻抗控制包括基于力的阻抗控制（阻抗控制）以及基于位置的阻抗控制（導納控制）。在導納控制中，控制器輸入力信號并輸出位置修正信號［13］。有許多研究成果驗證了將柔順控制應用于空間機器人的可行性［14-16］。Palma 等［17］利用仿真驗證了柔順控制在處理機器人接觸問題中的優(yōu)勢。Uyama等［18］提出一種基于阻抗控制的自由飛行空間機器人的接觸控制策略，并執(zhí)行了利用柔性機械臂捕獲非合作目標的數值仿真。Hirano等［19］提出了一種魯棒控制方案，驗證了控制策略在捕獲大型旋轉目標在線路徑規(guī)劃和柔順控制的可行性。Liu 等［20］關注柔性關節(jié)空間機器人的柔順控制問題，提出了一種非奇異復合阻抗控制器。聶佳偉等［21］分別采用位置控制與位置型阻抗控制，證明了阻抗控制能夠更加柔順地控制機械臂與接觸面進行接觸。此外，還有學者關注了捕獲后操作目標的柔順控制。王明明等［22］針對抓捕非合作目標衛(wèi)星的運動穩(wěn)定問題，提出了一種空間機器人最優(yōu)消旋策略和協(xié)調控制方法。劉磊等［23］針對空間大質量目標雙臂轉位操作，提出了一種協(xié)調控制方法。韓冬等［24］針對空間機器人抓捕目標后構成的組合航天器的穩(wěn)定控制問題，提出了基于反作用輪重配的反步積分滑?？刂品椒?。樊茂等［25］針對空間機器人抓捕目標中產生的碰撞沖擊問題，提出了一種低能耗、小擾動的鎮(zhèn)定控制方法。

除接觸碰撞階段外，在空間機器人接近目標階段，同樣可以采用合適的控制策略降低機器人與目標的碰撞沖擊，以實現機器人的柔順性。例如，Suarez等［26］在一種柔性輕型雙機械臂系統(tǒng)中驗證了在沒有關節(jié)電機的情況下的可變阻抗技術，實現機械臂與外部環(huán)境的柔順接觸；Ramón 等［27］利用視覺伺服控制算法，實現視覺特征信息輔助機械臂的阻抗控制；Xu等［28］提出了一種基于圖像的雙自由度機械臂阻抗控制策略，并將控制過程分為運動控制與自適應視覺阻抗控制兩個階段。除了借助視覺特征的輔助的方法，Arai 等［29］最早提出虛擬阻抗的概念，通過在機器人與未接觸的目標之間產生虛擬阻抗力來實現機器人對環(huán)境的阻抗特性。但其聚焦于避障路徑規(guī)劃問題，本質是將系統(tǒng)視為機械阻抗，無法直接與接觸導納控制相統(tǒng)一。Tsuji 等［30］則在虛擬阻抗的基礎上利用視覺反饋信息設計了機器人與目標之間的非接觸式阻抗控制方法，實現避障或減小首次碰撞力。目前，非接觸與接觸導納控制一體化控制方法還沒有被提出，這種一體化控制方法能夠靈活應對不同的接觸工況與任務場景，而無需修改底層控制器結構與參數。

綜上，針對機械臂與目標接觸前后的柔順控制問題，提出一種非接觸與接觸導納控制的統(tǒng)一方法，在機械臂接近目標時采用非接觸式導納控制，降低接觸時刻機械臂受到的接觸力；在抓緊目標過程中采用接觸式導納控制，實現目標的柔順接觸，避免捕獲失敗。兩種階段的導納控制輸出量具有相同的形式，避免了控制器切換的困難。

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

1.1 空間包絡式抓捕機構系統(tǒng)描述

如圖1（a）所示，空間包絡式抓捕機構的主體由3RRS 并聯(lián)機構（Revolute-Revolute-Spherical 的鉸鏈配置，各臂桿編號分別為11、12、21、22、31、32）以及Bricard 機構（各臂桿編號12A、12B、22A、22B、32A、32B）兩部分組成［5］，并安裝于上平臺，再與衛(wèi)星本體連接。系統(tǒng)共包括14個剛體。3RRS和Bricard機構的運動學分析見文獻［31］。由于機構內部有復雜的閉環(huán)約束，為建立最小維數的系統(tǒng)動力學模型，選取Bricard機構內C1、C2、C3處的柱鉸作為切開點解除閉環(huán)約束，所得等效開環(huán)系統(tǒng)結構如圖1（b）所示。

圖1 空間包絡式抓捕機構與等效開環(huán)系統(tǒng)的結構示意圖Fig.1 Structural diagram of the spatial envelope capture mechanism and its equivalent open-loop system ［31］

圖1 中空間包絡式抓捕機構的自由度分析如下：星體b于慣性空間中自由漂浮，具有平動和轉動的6個自由度；上平臺與星體之間通過柱鉸連接，具有1個相對自由度；3RRS并聯(lián)機構中臂桿11、21、31分別與上平臺之間、臂桿12、22、32 分別與臂桿11、21、31之間都通過柱鉸連接，一共具有6個相對自由度；Bricard 機構與3RRS 并聯(lián)機構的三個臂桿連接順序為i2-i2A-i2B，其中i2-i2A 為球鉸連接，各具有3 個相對自由度，i2A-i2B 為柱鉸連接，各具有1 個相對自由度。整個系統(tǒng)的獨立自由度數量為11個。

1.2 考慮包絡閉環(huán)約束的動力學模型

為了完成系統(tǒng)的動力學建模，首先在Bricard 中間的三個鉸鏈C1，C2，C3處引入切開點，得到等效開環(huán)系統(tǒng)，該系統(tǒng)具有25個自由度，選取廣義速率為

針對等效的開環(huán)系統(tǒng)，可建立如下形式的動力學方程

在切開點處的約束方向上，兩端的速度、角速度相等，基于該物理事實可得系統(tǒng)受到的約束形式為

式中：A為約束矩陣，是系統(tǒng)的幾何參數、構型坐標的函數。對式（4）求導即為系統(tǒng)的加速度約束方程

結合式（3）和式（5）可寫出空間包絡式抓捕機構的動力學方程為

式中：μ為拉格朗日乘子，其物理意義為切開點兩端所增加的未知約束力。一般情況下可由方程求解μ，其通解表示為

式中：z為元素可取任意值的15維向量。將式（7）代入式（6）可得系統(tǒng)動力學響應為

1.3 抓捕過程中的接觸模型

針對空間包絡抓捕機構的運動仿真，需要描述機構與目標之間的接觸碰撞過程。首先利用計算機圖形學方法進行碰撞檢測。假設各臂桿和空間目標外形均為圓柱體，各關節(jié)形狀為球體，則碰撞檢測問題簡化為兩圓柱體之間、圓柱體和球體之間的碰撞。當檢測到碰撞時，通過接觸碰撞模型計算當前時刻接觸碰撞力大小，使用Flores 模型作為機械臂末端與星體表面之間的法向接觸碰撞模型，此模型中接觸力的表達式為

滯后阻尼因子為

式中：K為接觸剛度系數；δ表示兩接觸剛體法向嵌入深度；n為力系數；Cr為恢復系數；表示本次碰撞開始時刻的二者接觸點表面切向相對速度。

選取靜摩擦、庫倫摩擦以及Stribeck 現象三者組合的模型用來描述接觸過程中切向摩擦力的情況，該組合模型表達式可寫為

表1 捕獲目標過程接觸碰撞的相關參數Table 1 Contact collision related parameters during the process of capturing targets

2 非接觸/接觸一體化導納控制

本文所提出的非接觸/接觸一體化導納控制框圖如圖2 所示。根據廣義碰撞力觀測器（2.2 節(jié)）的觀測結果，當空間包絡抓捕機構與非合作目標沒有發(fā)生接觸時，采用非接觸式虛擬導納控制（2.1 節(jié)），根據虛擬阻抗模型和獨立關節(jié)空間虛擬導納模型，得到關節(jié)運動的修正量，避免與目標發(fā)生較大的初次碰撞力；當空間包絡抓捕機構與非合作目標發(fā)生接觸后，控制器更改為接觸式導納控制（2.3 節(jié)），同樣輸出關節(jié)運動的修正量，實現期望的夾持狀態(tài)。由于兩種控制輸出相同，底層控制回路則采用軌跡跟蹤控制器，在切換前后無需調整控制參數。

圖2 非接觸式和接觸式一體化導納控制框圖Fig.2 Block diagram of non-contact and contact admittance control

2.1 非接觸式虛擬導納控制

空間包絡式抓捕機構在接近目標過程中，需要添加主動的減速過程，以降低首次與目標接觸時的碰撞力，避免機構的損壞。本節(jié)通過設計非接觸虛擬導納，實現接近目標過程的柔順控制。首先在可能發(fā)生碰撞的關節(jié)處設計虛擬球，利用虛擬阻抗模型計算得到虛擬阻抗力，并將其轉換為廣義虛擬阻抗力；其次，設計虛擬導納模型將此廣義虛擬阻抗力轉換為軌跡修正量對期望軌跡進行調節(jié)，使得各關節(jié)跟蹤修正后的軌跡。

考慮如圖3所示的機械臂第i個關節(jié)，定義半徑為r(i)(i=1，2，…，N)的虛擬球，每個虛擬球的球心均位于關節(jié)中心。虛擬阻抗控制器的輸入為機械臂臂桿與目標之間的位置、速度和加速度信息，輸出為虛擬阻抗力。

圖3 非接觸式阻抗控制原理圖Fig.3 Schematic diagram of non-contact impedance control

在以下推導中，若未做特殊說明，各位置、方向物理量均表示在機構安裝的上平臺坐標系中，見圖1（a）中的x1y1z1坐標系。假設將要接觸的物體位置坐標為Xo，當物體進入虛擬球i的內部時，定義從虛擬球表面到物體的法向為

式（14）即為非接觸阻抗模型，又稱虛擬阻抗模型。由式（13）與式（14）可以看出，當物體處于虛擬球球心處或未進入虛擬球時，虛擬阻抗力均為零；而當物體處于虛擬球內部且未與球心處關節(jié)接觸時，此虛擬阻抗力可根據虛擬球心所在關節(jié)處的雅克比矩陣轉換為各個關節(jié)的作用力矩。考慮到加速度反饋所帶來的噪聲影響，可忽略加速度項。獨立關節(jié)空間下的廣義虛擬阻抗力可寫為

則修正的期望軌跡為

2.2 考慮閉環(huán)約束的動量觀測器

空間包絡式抓捕機構在與目標發(fā)生接觸后，需要進行碰撞檢測，根據接觸程度改變各個自由度的運動。一種解決方法是在各臂桿配置類人皮膚傳感器，然而這將大大增加機器人硬件系統(tǒng)的復雜度、降低可靠性。另一種思路則是利用安裝在機器人本體的運動傳感器和力學傳感器測量信息對機器人與外界間的接觸碰撞情況進行觀測，實時獲得碰撞力對各關節(jié)的廣義外力，即廣義碰撞力。目前大部分對廣義碰撞力的觀測僅限于樹形系統(tǒng)，本節(jié)針對空間包絡式抓捕機構，引入考慮閉環(huán)約束的動量觀測器，實現接觸識別以及接觸力的估計。

本文所研究的空間包絡式抓捕機構存在多閉環(huán)結構與冗余約束，其內部真實約束力難以得到，而且實時求解多處接觸碰撞點的位置和碰撞力信息較為困難，因此選擇直接利用觀測器所觀測到的廣義碰撞力信息設計各關節(jié)的接觸式阻抗控制。與傳統(tǒng)的開環(huán)或鏈式機械臂的動力觀測器不同，為了消除閉環(huán)約束造成的觀測難度，引入最小數目的傳感器測量閉環(huán)約束中的約束力，即式（7）中的μ。

由空間包絡式抓捕機構動力學方程（6）可知，在有碰撞情況下的動力學方程為

式中：β=Cu-。觀測器形式為

式中：KMO為觀測器增益上標“^”表示測量或計算得到的值。

由式（20）和式（21）可得

由式（22）可以得出觀測器的輸出γ能夠實現對FGC的指數跟蹤，因此空間包絡式抓捕機構廣義碰撞力的觀測可由觀測器（21）來實現，觀測器具體的結構如圖4所示。

圖4 空間包絡式抓捕機構廣義碰撞力觀測器Fig.4 Generalized collision force observer for spatial enveloping capture mechanism

2.3 接觸式導納控制

依靠碰撞觀測器，可以設計接觸式導納控制器，該控制器的輸入是碰撞力，輸出是軌跡修正量。

獨立關節(jié)空間下的廣義碰撞力可寫為

獨立關節(jié)空間下的阻抗模型為

則修正后的獨立關節(jié)期望軌跡為

觀察式（17）和（26），非接觸式虛擬導納控制和接觸式導納控制有一致的輸出形式，可自然地將兩種控制融合在一起，形成非接觸/接觸一體化導納控制。

3 數值仿真

對空間包絡式抓捕機構抓捕目標的柔順控制過程進行仿真分析。空間包絡式抓捕機構的質量和尺寸參數如表2所示。

表2 空間包絡式抓捕機構質量與尺寸參數Table 2 Mass and size parameters of spatial enveloping capture mechanism

本節(jié)仿真中空間目標的質量和尺寸參數在表3中定義，阻抗模型參數以及軌跡跟蹤控制器參數列于表4。其中，阻抗模型參數的數量級與原系統(tǒng)保持一致，在選取剛度矩陣后按二階系統(tǒng)特性近似選取質量矩陣與阻尼矩陣；獨立關節(jié)空間修正量只作用于3RRS 以及Bricard 機構的4 個獨立自由度；此外，3RRS的三個獨立關節(jié)阻抗參數應保持相同?？臻g包絡式抓捕機構將以六點式夾持方式完成對目標的柔順捕獲。抓捕任務的初始時刻與結束時刻系統(tǒng)構型如圖5所示。初始時刻星體速度、角速度及各關節(jié)角速度均為零；目標軸線垂直于抓捕網口、中心位于[0 0 -15]Tm，初始速度為[0 0 -0.05]Tm/s、角速度為零。期望抓捕狀態(tài)下，空間包絡式抓捕機構各關節(jié)角速度、角加速度均為零，期望的部分關節(jié)運動軌跡如圖6所示。

表3 空間目標質量與尺寸參數Table 3 Space target mass and size parameters

表4 控制器參數Table 4 Controller parameters

圖5 抓捕任務初始時刻與結束時刻系統(tǒng)狀態(tài)Fig.5 System state at the initial and terminal moment of the capture task

圖6 空間包絡式抓捕機構關節(jié)期望軌跡Fig.6 Expected trajectory of joint in spatial enveloping capture mechanism

對比使用傳統(tǒng)的位置控制和本文提出的非接觸/接觸一體化導納控制（后簡稱“柔順控制”）的效果。以臂桿12A 為例，分別給出位置控制與柔順控制作用下，空間包絡式抓捕機構與目標的接觸情況，如圖7所示。

圖7 空間包絡式抓捕機構與目標接觸情況（0：無接觸，1：動摩擦，2：靜摩擦）Fig.7 Contact between spatial enveloping capture mechanism and target（0：non-contact，1：dynamic friction，2：static friction）

可以看出，在位置控制下，Bricard 機構各個臂桿與目標之間發(fā)生碰撞后被碰撞力彈開，接觸面發(fā)生分離，之后二者經過若干次碰撞過程才最終保持接觸，但最終的穩(wěn)定狀態(tài)二者之間仍存在滑動摩擦，說明位置控制下空間包絡式抓捕機構對存在0.05 m/s 相對運動的10 m 尺寸目標并不能有效地進行捕獲。在柔順控制下，當廣義碰撞力觀測器觀測到抓捕機構與目標碎片的首次接觸后，控制器將立即調整為阻抗控制模式，對各關節(jié)實時施加阻抗作用、調整抓捕機構對外展現的阻抗特性，從而避免了抓捕機構與目標碎片接觸后的分離，以較快的速度實現了可靠的抓捕。

圖8分別給出了與空間包絡式抓捕機構關節(jié)運動對應的廣義碰撞力。位置控制下抓捕機構將與目標碎片發(fā)生多次、多點的接觸碰撞，在接觸穩(wěn)定前抓捕機構臂桿及各關節(jié)將受到多次沖擊；而柔順控制下機構在接觸目標碎片后不再與其分離。同時可以看出，在非接觸導納的作用下，首次碰撞的接觸力明顯減小，避免了對結構帶來較大的沖擊。此外，柔順控制下抓捕機構與目標碎片間最終的接觸力大于位置控制。原因在于，柔順控制中通過阻抗作用對抓捕關節(jié)的剛度進行了調整，從而產生了“抓緊”目標碎片的效果；而傳統(tǒng)的位置控制不具備此能力。

圖8 3RRS結構與Bricard機構對應的廣義碰撞力Fig.8 The generalized collision force corresponding to the 3RRS structure and the Bricard mechanism

圖9 以Bricard 機構中關節(jié)12B 為例，給出其期望軌跡、修正的期望軌跡以及兩類控制下實際轉角的圖線。該轉角的變化情況表征了空間包絡式抓捕機構的抓捕網口大小的變化情況?？梢钥闯?，柔順控制根據觀測的廣義碰撞力及阻抗模型對關節(jié)期望軌跡進行了修正，使其較快地收斂于接觸穩(wěn)定位置。同時，由于位置控制和柔順控制分別對原期望軌跡和修正后的期望軌跡進行跟蹤，因此兩類控制下關節(jié)12B 的實際轉角變化情況也有較大差別：位置控制下由于抓捕機構與目標的多次接觸碰撞，關節(jié)轉角12B 在首次接觸后將發(fā)生多次彈跳，而柔順控制下該轉角將較快地穩(wěn)定于穩(wěn)態(tài)值。

圖9 位置控制與柔順控制模式下關節(jié)12B轉角Fig.9 Angle of joint 12B in the traditional position control and the proposed control

4 結論

本文以帶有復雜閉環(huán)約束的空間包絡式抓捕機構為研究對象，提出了一種非接觸式與接觸式一體化的導納控制方法，實現對非合作目標的柔順捕獲。首先建立了系統(tǒng)顯式的動力學模型，其次設計了空間包絡式抓捕機構與目標的碰撞檢測和非接觸式與接觸式一體化的導納控制。一方面，在接近目標過程中，采用非接觸的導納控制策略，降低抓捕機構與目標首次發(fā)生碰撞的接觸力；另一方面，在首次接觸后轉換為接觸的導納控制策略，根據接觸力的觀測結果動態(tài)調整關節(jié)的期望軌跡，減少抓捕機構與目標的接觸次數，且能獲得更大的夾持力實現穩(wěn)定捕獲。數值結果表明，所提方法能夠有效降低空間包絡式抓捕機構與目標物體首次接觸的危險，同時更快地實現穩(wěn)定捕獲。