空間非合作目標(biāo)多指包絡(luò)抓捕路徑設(shè)計(jì)

萬文婭，孫沖，袁建平

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

空間非合作目標(biāo)主要包括太空中殘留的火箭末級、失效衛(wèi)星、航天器任務(wù)拋棄物、航天器解體及碰撞衍生物等[1]。通常，這類目標(biāo)的幾何外形復(fù)雜多樣且不具有專門的抓捕對接口，慣性參數(shù)和運(yùn)動參數(shù)等信息存在不確定性甚至是未知的，運(yùn)動規(guī)律也較為復(fù)雜。空間非合作目標(biāo)的這些特性造成了在抓捕過程中，抓捕機(jī)構(gòu)和抓捕對象之間存在高動態(tài)相對運(yùn)動，使得難以確定固定抓捕點(diǎn)，因此空間非合作目標(biāo)在軌抓捕難度很大[2-3]。

針對空間非合作目標(biāo)抓捕，已經(jīng)進(jìn)行了大量的研究，提出了諸如機(jī)械臂固定點(diǎn)抓捕、繩系機(jī)器人抓捕、柔性機(jī)構(gòu)抓捕之類的方法[4]。本文借鑒多指包絡(luò)抓捕的思想，利用多指機(jī)構(gòu)在目標(biāo)物體周圍形成一個(gè)“包圍圈”，以限制目標(biāo)物體的運(yùn)動[5]。采用這種方式對空間非合作目標(biāo)進(jìn)行抓捕，不僅對空間非合作目標(biāo)的幾何外形具有更好的適應(yīng)性，還可以提升抓捕的魯棒性。相比于傳統(tǒng)的抓捕方法[6-7]，這種方式不需要考慮接觸等力學(xué)特性，放松了抓捕中對控制的要求。

基于包絡(luò)的思想，孫沖等提出了一種具有魯棒性的外包絡(luò)抓捕方法及抓捕路徑優(yōu)化方案[8]。韓亮亮等提出了一種基于仿章魚充氣軟體機(jī)器人的碎片捕獲裝置[9]。然而，由于多指機(jī)構(gòu)自身高自由度的問題以及目標(biāo)的動態(tài)性問題，使得多指機(jī)構(gòu)的包絡(luò)算法設(shè)計(jì)還存在諸多難題[5]。

包絡(luò)問題是由Kuperberg率先提出的，他將包絡(luò)問題描述為尋找包絡(luò)點(diǎn)的構(gòu)型使得多邊形目標(biāo)物體不能任意遠(yuǎn)離其初始位置[10]。隨后，Rimon和Blake將“包絡(luò)”概念引入到機(jī)器人領(lǐng)域，并提出了光滑平面物體的兩指包絡(luò)算法[11]。從此，包絡(luò)問題成為機(jī)器人領(lǐng)域的一個(gè)熱門研究，相繼提出了針對平面目標(biāo)物體的兩指/三指包絡(luò)算法。兩指包絡(luò)是最簡單的包絡(luò)形式，其適用于凹目標(biāo)抓捕[12-13]。然而針對凸目標(biāo)物體，兩指機(jī)構(gòu)無法提供封閉的包絡(luò)構(gòu)型。由于存在冗余自由度，三指機(jī)構(gòu)可用于包絡(luò)凸目標(biāo)，但是其包絡(luò)構(gòu)型設(shè)計(jì)相較于雙指機(jī)構(gòu)更為復(fù)雜，相關(guān)研究人員通常固定某些自由度來簡化三指包絡(luò)問題[14-15]。由于手指數(shù)量增加會導(dǎo)致計(jì)算量增加，超過三指的多指包絡(luò)鮮有研究[5]。但是，對于復(fù)雜外形的平面物體以及空間物體，則需要更多數(shù)量的手指[16]。

在上述研究中[10-16]，目標(biāo)物體都是靜態(tài)的，而空間非合作目標(biāo)通常是動態(tài)的，目前關(guān)于動態(tài)物體包絡(luò)的研究較少。在已有的文獻(xiàn)中，Wang等[17-18]和Wan等[19]研究了多機(jī)器人對運(yùn)動目標(biāo)的包絡(luò)。與此類似，文獻(xiàn)[20-21]研究了多個(gè)機(jī)器人對動態(tài)物體的追捕問題。但是，多指機(jī)構(gòu)不同于多機(jī)器人群體，它的各個(gè)手指不是獨(dú)立的，受到多指機(jī)構(gòu)的結(jié)構(gòu)約束。因此，需要設(shè)計(jì)符合多指機(jī)構(gòu)自身特性的動態(tài)物體包絡(luò)算法。

針對上述提到的多指包絡(luò)中需要解決的問題，本文提出了一種適用于空間非合作目標(biāo)的 “主-從”式多指包絡(luò)算法，不僅具有處理運(yùn)動目標(biāo)的能力，而且在包絡(luò)構(gòu)型搜索過程中還簡化了系統(tǒng)自由度進(jìn)而提升了計(jì)算效率，可以快速地搜索到一條有效的空間非合作目標(biāo)多指包絡(luò)抓捕路徑。

1 問題描述

針對空間非合作目標(biāo)的多指包絡(luò)抓捕問題，本文旨在設(shè)計(jì)一種“主-從”式包絡(luò)算法，在給定初始相對構(gòu)型的條件下，尋找能夠約束住空間非合作目標(biāo)的有效包絡(luò)構(gòu)型。

1.1 多指機(jī)構(gòu)結(jié)構(gòu)模型

如圖1所示，多指機(jī)構(gòu)由手掌和n個(gè)手指組成。其中，每個(gè)手指由m個(gè)連桿和旋轉(zhuǎn)關(guān)節(jié)組成。受諸如手掌尺寸和關(guān)節(jié)允許轉(zhuǎn)動范圍等結(jié)構(gòu)約束，多指機(jī)構(gòu)的各個(gè)手指間存在內(nèi)在約束，作為一個(gè)整體工作。

圖1 多指機(jī)構(gòu)示意圖Fig.1 Multi-fingered mechanism

(1)

其次，對于第i個(gè)手指的具體構(gòu)型Ci，它是由自身的關(guān)節(jié)角θi=[θi,1,θi,2,…,θi,m]T決定的：

Ci=f(θi)=f(θi,1,θi,2,…,θi,m)

i=1,2,…,n

(2)

1.2 空間非合作目標(biāo)包絡(luò)條件

本文中空間非合作目標(biāo)包絡(luò)條件是指多指機(jī)構(gòu)能夠?qū)⒖臻g非合作目標(biāo)約束到某一范圍內(nèi)時(shí)，多指機(jī)構(gòu)和空間非合作目標(biāo)之間應(yīng)當(dāng)滿足的約束條件。多指機(jī)構(gòu)有效包絡(luò)住空間非合作目標(biāo)意味著空間非合作目標(biāo)不能從多指機(jī)構(gòu)的約束中逃離。具體來說，就是不能從多指機(jī)構(gòu)相鄰手指的間隙以及各個(gè)手指的指尖間隙中逃離。不失一般性，將多指機(jī)構(gòu)的各個(gè)手指的連桿和關(guān)節(jié)分別簡化為線段和點(diǎn)(圖1中的黑色線段和圓點(diǎn))。

圖2 空間非合作目標(biāo)包絡(luò)條件推導(dǎo)示意圖Fig.2 Derivation of caging conditions for space non-cooperative target

首先，計(jì)算第i個(gè)手指和第(i+1)個(gè)手指連桿間的間隙。如圖2(a)所示，由第i個(gè)手指和第i+1 個(gè)手指的第k個(gè)連桿組成的圖形為空間四邊形Ji,kJi,k+1Ji+1,k+1Ji+1,k(i=1,2,…,n;k=1,2,…,m;Ji,m+1=Ji,e,Ji,e表示指尖)。需要指出，當(dāng)i=n時(shí)，Jn,kJn,k+1Jn+1,k=Jn,kJn,k+1J1,k+1J1，k，手指相鄰。如果Ji,kJi,k+1Ji+1,k+1Ji+1,k對邊間的距離小于目標(biāo)相應(yīng)的特征長度lcl，那么目標(biāo)則不能從第i個(gè)手指和第(i+1)個(gè)手指的間隙中逃離：

i=1,2,…,n;k=1,2,…,m-1

(3)

式中：di→i+1,k表示線段Ji,kJi,k+1和線段Ji+1,kJi+1,k+1之間的距離；di,k→k+1表示線段Ji,kJi+1,k和線段Ji,k+1Ji+1,k+1之間的距離。

接著，計(jì)算各個(gè)手指的指尖之間的距離。如圖2(b)所示，多指機(jī)構(gòu)的各個(gè)手指的指尖形成了一個(gè)空間n邊形，第i個(gè)手指的指尖Ji,e和第j個(gè)手指的指尖Jj,e(i,j=1,2,…,n;i≠j)之間的距離di,j為

i,j=1,2,…,n;i≠j

(4)

式中：d(·,·)表示空間中任意兩點(diǎn)間距離計(jì)算的函數(shù)，本文中采用歐幾里得距離，此時(shí)p=2。為了保證目標(biāo)不從指尖的間隙中逃離，應(yīng)當(dāng)滿足如下關(guān)系：

(5)

進(jìn)一步地，結(jié)合方程(3)和(5)，運(yùn)動物體包絡(luò)條件可以表示為

g(Θ):=max(di,i→i+1,di,k→k+1,{di,j})-lcl<0

(6)

式中：i,j=1,2,…,n(i≠j)；n+1=1；k=1,2,…,m-1。

注1特別地，當(dāng)包絡(luò)問題可以簡化為二維平面包絡(luò)時(shí)(空間非合作目標(biāo)為扁平型)，多指機(jī)構(gòu)的各個(gè)手指可以簡化為點(diǎn)手指，即用指尖表示。此時(shí)，多指機(jī)構(gòu)可以用一系列存在位置約束的點(diǎn)表示(例如圖1中的指尖)。進(jìn)一步，包絡(luò)條件可以只用方程(5)來描述。

2 “主-從”式包絡(luò)算法設(shè)計(jì)

為解決多指機(jī)構(gòu)對空間非合作目標(biāo)實(shí)施包絡(luò)抓捕時(shí)存在的多指機(jī)構(gòu)高自由度以及待包絡(luò)物體具有動態(tài)性的問題，本文提出了一種“主—從”式包絡(luò)算法。總體思想是：將多指機(jī)構(gòu)的手指劃分為一根主手指和其余從手指，并控制主手指的基關(guān)節(jié)與包絡(luò)點(diǎn)之間的運(yùn)動同步，接著調(diào)整主手指的關(guān)節(jié)角使得主手指的構(gòu)型與包絡(luò)邊盡可能地匹配，然后根據(jù)多指機(jī)構(gòu)的結(jié)構(gòu)模型確定其余從手指的構(gòu)型，最后根據(jù)包絡(luò)條件選擇能夠約束住空間非合作目標(biāo)運(yùn)動的有效包絡(luò)構(gòu)型。

圖3 包絡(luò)元素示意圖Fig.3 Caging elements diagram

度εb內(nèi)的相對于目標(biāo)靜止的所有點(diǎn)的集合，其中包絡(luò)點(diǎn)Pb∈Cb的選取是隨機(jī)的。為方便起見，將包絡(luò)邊、包絡(luò)裕度、包絡(luò)點(diǎn)集以及包絡(luò)點(diǎn)統(tǒng)稱為包絡(luò)元素。

(7)

在進(jìn)行具體的包絡(luò)算法設(shè)計(jì)之前，假設(shè)多指機(jī)構(gòu)具有諸如底座之類的支撐部件，它可以提供多指機(jī)構(gòu)所需的控制力和控制力矩。

2.1 包絡(luò)點(diǎn)追蹤

為了方便對空間非合作目標(biāo)實(shí)施包絡(luò)，應(yīng)當(dāng)首先實(shí)現(xiàn)多指機(jī)構(gòu)與空間非合作目標(biāo)之間的運(yùn)動同步，具體為多指機(jī)構(gòu)和空間非合作目標(biāo)之間的姿態(tài)運(yùn)動同步以及主手指的基關(guān)節(jié)和包絡(luò)點(diǎn)之間的平移運(yùn)動同步。雖然空間非合作目標(biāo)的運(yùn)動參數(shù)及慣性參數(shù)等參數(shù)通常未知或含有不確定性，但是已有大量研究來確定這些參數(shù)[22]，故本文假設(shè)空間非合作目標(biāo)物體的運(yùn)動參數(shù)和慣性參數(shù)已知。

關(guān)于兩航天器之間的相對姿態(tài)運(yùn)動同步控制算法，相關(guān)學(xué)者已經(jīng)進(jìn)行了大量研究，提出了諸如滑膜控制算法[23]之類的成熟算法。因此，可以直接假設(shè)在實(shí)施包絡(luò)時(shí)多指機(jī)構(gòu)有能力實(shí)現(xiàn)與空間非合作目標(biāo)之間的姿態(tài)運(yùn)動同步。故本文不再贅述多指機(jī)構(gòu)和空間非合作目標(biāo)之間的相對姿態(tài)運(yùn)動同步的控制算法，直接將其作為實(shí)施包絡(luò)操作的初始條件。

(8)

(9)

(10)

(11)

k2sign(x2e)|x2e|α

(12)

(13)

注3在包絡(luò)點(diǎn)追蹤的整個(gè)過程中，需要滿足多指機(jī)構(gòu)和空間非合作目標(biāo)物體間不發(fā)生碰撞的安全性要求。本文將每一時(shí)刻多指機(jī)構(gòu)Od(t)和空間非合作目標(biāo)物體Om(t)均不能存在交集作為約束來保證兩者之間不發(fā)生碰撞(式(14))。

Od(t)∩Om(t)=?

(14)

下面對式(14)做進(jìn)一步說明。本文中是根據(jù)空間非合作目標(biāo)與多指機(jī)構(gòu)之間的相對幾何位置關(guān)系來進(jìn)行碰撞檢測的，即通過實(shí)時(shí)地檢測空間非合作目標(biāo)所占的空間與多指機(jī)構(gòu)所占的空間之間是否有交集來進(jìn)行判斷。具體而言，如果空間非合作目標(biāo)所占的空間與多指機(jī)構(gòu)所占的空間之間沒有交集，則空間非合作目標(biāo)與多指機(jī)構(gòu)之間不會發(fā)生碰撞，是安全的；反之，如果空間非合作目標(biāo)所占的空間與多指機(jī)構(gòu)所占的空間之間有交集，則說明空間非合作目標(biāo)和多指機(jī)構(gòu)發(fā)生了相互穿插的現(xiàn)象，這顯然是不符合實(shí)際情況的，屬于發(fā)生碰撞的情形。式(14)簡單明了地表示空間非合作目標(biāo)與多指機(jī)構(gòu)之間不發(fā)生碰撞的安全約束。

2.2 包絡(luò)邊匹配

如圖4所示，主手指和包絡(luò)邊之間存在很多可能的匹配構(gòu)型，引入單向距離D(Cz,bl)來衡量主手指Cz和包絡(luò)邊bl匹配的相似度[25]：

D(Cz,bl)=

(15)

D(cz,i～i+1,bl)=

(16)

式中：Dd(p,bl)表示cz,i～i+1上任意一點(diǎn)到bl的最短距離：

Dd(p,bl)=minq∈blDEuclid(p,q)

(17)

此時(shí)，包絡(luò)邊匹配問題可以轉(zhuǎn)化為尋找使得Cz與bl間單向距離最小的主手指關(guān)節(jié)角θz：

(18)

圖5 最優(yōu)包絡(luò)邊匹配構(gòu)型搜尋算法Fig.5 Algorithm of optimal caging edge matching

考慮到求解最優(yōu)化問題(18)的過程中不僅需要實(shí)時(shí)進(jìn)行碰撞規(guī)避還希望計(jì)算效率盡可能高，所以本文采用了快速搜索隨機(jī)樹法來進(jìn)行求解。快速搜索隨機(jī)樹法是機(jī)器人路徑規(guī)劃領(lǐng)域的一個(gè)重要的方法，這種方法在機(jī)器人規(guī)劃領(lǐng)域，尤其是高維環(huán)境(如，機(jī)械臂)的規(guī)劃中，占有重要的位置，是基于采樣的規(guī)劃方法的一種。如圖5所示，首先，通過快速搜索隨機(jī)樹法隨機(jī)搜索有效匹配構(gòu)型，但是并不是每個(gè)有效匹配構(gòu)型都會被添加到搜索路徑中，只有當(dāng)前有效匹配構(gòu)型對應(yīng)的單向距離比上一個(gè)有效匹配構(gòu)型對應(yīng)的單向距離小時(shí)，才會被添加，這保證了快速搜索隨機(jī)樹法是朝著單向距離越來越小的方向進(jìn)行搜索的；其次，由于最優(yōu)匹配構(gòu)型對應(yīng)的主手指關(guān)節(jié)角值是未知的，而傳統(tǒng)快速搜索隨機(jī)樹法中目標(biāo)點(diǎn)是給定的，所以在圖5所示的最優(yōu)包絡(luò)邊匹配構(gòu)型搜索算法中，以快速搜索隨機(jī)樹法搜索到的相鄰2個(gè)有效匹配構(gòu)型所對應(yīng)的單向距離的差值作為算法是否終止的判斷條件；最后，當(dāng)單向距離收斂到一個(gè)穩(wěn)定值不再變化時(shí)，則說明找到了使得主手指與包絡(luò)邊最大程度地相似的主手指關(guān)節(jié)角取值。雖然快速搜索隨機(jī)樹法搜索每個(gè)有效匹配構(gòu)型的過程是隨機(jī)的，但是，通過搜索方向和算法終止的判斷條件這2個(gè)方面保證了優(yōu)化目標(biāo)的收斂性，即通過隨機(jī)搜索找到最好的解。

2.3 從手指構(gòu)型確定

i≠z,i=1,2,…n,z∈{1,2,…,n}

(19)

然后，根據(jù)式(2)，確定Ci的可能構(gòu)型：

(20)

2.4 有效包絡(luò)構(gòu)型確定

一旦主手指和所有從手指的構(gòu)型確定，多指機(jī)構(gòu)的構(gòu)型Cd也就隨之確定：

Cd={Cz，{Ci}}

i≠z；i=1,2,…,n；z∈{1,2,…,n}

(21)

本文所提的“主-從”式的多指機(jī)構(gòu)包絡(luò)算法，不僅具有處理運(yùn)動目標(biāo)的能力，而且在包絡(luò)構(gòu)型搜索過程中還簡化了系統(tǒng)自由度進(jìn)而提升了計(jì)算效率，所以可以實(shí)現(xiàn)對空間非合作目標(biāo)的快速有效包絡(luò)。

i≠z；i=1,2,…n；z∈{1,2,…,n}

(22)

3 仿真驗(yàn)證

為了驗(yàn)證所設(shè)計(jì)算法的有效性，采用圖1所示的四指三關(guān)節(jié)的多指機(jī)構(gòu)分別對可以簡化為二

表1 指尖距離的容許范圍Table 1 Allowable distances between fingertips

圖6 多指機(jī)構(gòu)結(jié)構(gòu)關(guān)系圖Fig.6 Structural relationship in multi-fingered mechanism

3.1 扁平型空間非合作目標(biāo)包絡(luò)

表2 仿真參數(shù)Table 2 Simulation parameters

圖7 包絡(luò)正五邊形時(shí)的包絡(luò)元素Fig.7 Caging element when caging regular pentagonal object

3.2 一般性三維空間非合作目標(biāo)包絡(luò)

圖8 包絡(luò)正五邊形時(shí)包絡(luò)點(diǎn)追蹤結(jié)果隨時(shí)間變化圖Fig.8 Time responses of caging point tracking results when caging regular pentagonal object

[0,-0.285 1,-0.301 4]Tm。此時(shí)，主手指基關(guān)節(jié)和包絡(luò)點(diǎn)間的初始相對位置誤差為x1e0=[-5.659 5,-7.795 9,0]Tm、初始相對速度誤差為x2e0=[0.6,-1,-0.5]Tm/s。同樣地，設(shè)期望在時(shí)刻tb=150 s實(shí)現(xiàn)包絡(luò)點(diǎn)追蹤。

通過以上2個(gè)仿真案例可以得出，所設(shè)計(jì)的算法對可以簡化為二維平面物體的扁平型空間非合非合目標(biāo)包絡(luò)抓捕問題和一般性三維空間非合作目標(biāo)物體包絡(luò)抓捕問題均適用，可以高效地得到一條有效的空間非合作目標(biāo)多指包絡(luò)抓捕路徑。

圖9 包絡(luò)正五邊形時(shí)的多指機(jī)構(gòu)構(gòu)型Fig.9 Caging configurations of multi-fingered mechanism when caging regular pentagonal object

圖10 包絡(luò)長方體時(shí)的包絡(luò)點(diǎn)追蹤結(jié)果態(tài)隨時(shí)間變化圖Fig.10 Time responses of caging point tracking results when caging cuboid object

圖11 包絡(luò)邊匹配結(jié)果Fig.11 Results of caging edge matching

4 結(jié) 論

本文針對空間非合作目標(biāo)抓捕，首先提出了一種多指機(jī)構(gòu)包絡(luò)抓捕方法。然后，針對多指包絡(luò)抓捕中存在的多自由度導(dǎo)致的計(jì)算復(fù)雜問題，以及難以處理動態(tài)目標(biāo)包絡(luò)抓捕問題，提出了一種“主-從”式多指包絡(luò)算法，可以高效地得到一條有效的空間非合作目標(biāo)多指包絡(luò)抓捕路徑。以上抓捕方法和多包絡(luò)算法的主要?jiǎng)?chuàng)新有以下3點(diǎn)：

圖12 包絡(luò)長方體時(shí)的多指機(jī)構(gòu)構(gòu)型Fig.12 Caging configurations of multi-fingered mechanism when caging a cuboid object

1) 提出一種多指抓捕建模方法，將多指抓捕機(jī)構(gòu)分為一根主手指和其余從手指兩類，針對各類手指分步進(jìn)行包絡(luò)算法設(shè)計(jì)，仿真表明可降低包絡(luò)構(gòu)型設(shè)計(jì)中的自由度，從而可以快速求解包絡(luò)構(gòu)型。

2) 采用誤差跟蹤控制實(shí)現(xiàn)主手指的基關(guān)節(jié)與包絡(luò)點(diǎn)之間的運(yùn)動同步，實(shí)現(xiàn)了動態(tài)空間非合作目標(biāo)的抓捕。

3) 引入單向距離的概念來定量衡量主手指構(gòu)型與包絡(luò)邊的相似程度，并建立了考慮了多指機(jī)構(gòu)內(nèi)在約束的多指機(jī)構(gòu)結(jié)構(gòu)模型，可以利用該模型可以便捷地確定其他從手指的構(gòu)型。

通過對運(yùn)動的二維平面物體和三維物體的包絡(luò)仿真可以得出，所設(shè)計(jì)的算法不僅簡化了計(jì)算復(fù)雜度，還適用于動態(tài)目標(biāo)。本文所設(shè)計(jì)的算法可以為后續(xù)的對實(shí)際空間非合作目標(biāo)的多指包絡(luò)抓捕奠定基礎(chǔ)。