李宜鵬，解永春,2,3

(1. 北京控制工程研究所，北京100190；2. 空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京100094； 3. 天津市微低重力環(huán)境模擬技術(shù)重點(diǎn)實(shí)驗(yàn)室，天津300301)

0 引 言

為了能夠安全實(shí)施在軌維修、燃料加注等一系列在軌服務(wù)，在近距離接近空間目標(biāo)時需要估計(jì)相對位置和姿態(tài)(相對位姿)，對于安裝有合作標(biāo)志器的合作目標(biāo)和未安裝合作標(biāo)志器但模型已知的非合作目標(biāo)，該問題已經(jīng)得到了很好的解決。而空間目標(biāo)大多數(shù)為模型未知的非合作目標(biāo)(例如失效衛(wèi)星、空間垃圾等)，未安裝合作標(biāo)志器且缺乏關(guān)于幾何結(jié)構(gòu)和慣量參數(shù)的先驗(yàn)知識，這就給此類目標(biāo)接近操作過程中的相對位姿估計(jì)帶來了很大的困難，通常無法直接獲取目標(biāo)本體坐標(biāo)系的相對位姿。對于無旋轉(zhuǎn)運(yùn)動或繞單軸旋轉(zhuǎn)的目標(biāo)，其慣量參數(shù)及質(zhì)心位置是不完全可觀的；只有處于章動狀態(tài)時，目標(biāo)的慣量參數(shù)及質(zhì)心位置才是可觀的[1-2]。因此本文以模型未知且處于章動運(yùn)動的非合作目標(biāo)為研究對象，研究接近過程中目標(biāo)質(zhì)心位置、慣量主軸、線速度、角速度及轉(zhuǎn)動慣量比等相對狀態(tài)的估計(jì)方法。

未知非合作目標(biāo)的相對狀態(tài)估計(jì)問題可以用典型的同時定位與建圖(Simultaneously localization and mapping，SLAM)問題來描述[2-5]，即在恢復(fù)目標(biāo)三維結(jié)構(gòu)的同時，對目標(biāo)的相對位姿進(jìn)行估計(jì)，對于旋轉(zhuǎn)運(yùn)動的目標(biāo)，還需要估計(jì)質(zhì)心、慣量主軸等相對狀態(tài)。對此國內(nèi)外學(xué)者主要采用視覺敏感器作為測量手段，并設(shè)計(jì)相應(yīng)的估計(jì)方法對目標(biāo)的相對狀態(tài)進(jìn)行估計(jì)。根據(jù)原理的不同，文獻(xiàn)中的方法主要分為三類：基于運(yùn)動學(xué)的方法、建立輔助坐標(biāo)系的方法和直接估計(jì)的方法。

基于運(yùn)動學(xué)的方法[1,6-10]一般使用雙目相機(jī)或激光雷達(dá)獲取目標(biāo)特征點(diǎn)的位置以及相鄰時刻目標(biāo)位姿的變化量，通過差分運(yùn)算近似得到目標(biāo)的角速度以及特征點(diǎn)的線速度，并以此作為測量值，根據(jù)目標(biāo)的角動量守恒方程和特征點(diǎn)的旋轉(zhuǎn)-平移耦合運(yùn)動方程，使用最小二乘法分別估計(jì)出目標(biāo)的慣量參數(shù)、質(zhì)心位置及速度。這種方法的主要缺點(diǎn)是差分運(yùn)算引入的噪聲對估計(jì)結(jié)果影響較大。

建立輔助坐標(biāo)系的方法的基本原理是通過人為定義一個固定于目標(biāo)上的坐標(biāo)系，以該坐標(biāo)系的相對位姿作為觀測量，再根據(jù)目標(biāo)的動力學(xué)模型估計(jì)目標(biāo)的慣量參數(shù)[2,11-12]。在建立目標(biāo)的動力學(xué)模型時，需要將未知的轉(zhuǎn)動慣量比、輔助坐標(biāo)系與目標(biāo)本體系之間的偏移量做為待估計(jì)狀態(tài)變量，通過UKF[11]、EKF[12]或者增量平滑與建圖(incremental Smoothing and mapping，iSAM)[13]進(jìn)行估計(jì)。此類方法的優(yōu)點(diǎn)是能夠更精確地估計(jì)目標(biāo)的線速度、角速度等狀態(tài)，缺點(diǎn)是建立輔助坐標(biāo)系導(dǎo)致待估計(jì)變量的數(shù)量增加。

直接估計(jì)的方法是以目標(biāo)表面特征點(diǎn)的投影作為觀測量，不需要顯式地估計(jì)出目標(biāo)的相對姿態(tài)和位置，通過建立特征點(diǎn)運(yùn)動模型及非合作目標(biāo)的動力學(xué)模型，設(shè)計(jì)濾波算法對目標(biāo)的相對狀態(tài)進(jìn)行估計(jì)。比如文獻(xiàn)[14-15]使用單目相機(jī)觀測到的特征點(diǎn)單位視線方向作為觀測量，通過追蹤航天器的軌道機(jī)動、姿態(tài)機(jī)動或相機(jī)安裝位置偏置來保證特征點(diǎn)距離的可觀性，再通過濾波算法估計(jì)出目標(biāo)的相對狀態(tài)；特征點(diǎn)深度值的可觀性問題可以通過使用雙目相機(jī)來克服，不需要額外的軌道或姿態(tài)機(jī)動就可實(shí)現(xiàn)，文獻(xiàn)[16]使用雙目相機(jī)對目標(biāo)進(jìn)行測量，針對慣量矩陣不確定的問題，設(shè)計(jì)了多個IEKF濾波器，通過最大后驗(yàn)概率的準(zhǔn)則選取最優(yōu)的估計(jì)值；文獻(xiàn)[17]在文獻(xiàn)[16]的基礎(chǔ)上增加了特征點(diǎn)的深度信息和光流作為額外的觀測量，并使用文獻(xiàn)[2]中的方法對轉(zhuǎn)動慣量進(jìn)行參數(shù)化。

直接估計(jì)的方法兼顧了前兩種方法的缺點(diǎn)，但是相關(guān)文獻(xiàn)中所提出方法存在的問題是目標(biāo)特征點(diǎn)與目標(biāo)質(zhì)心之間的相對位置無法確定(無論是在目標(biāo)本體系下還是在追蹤器本體系下都如此)，在濾波初始化時很難給出準(zhǔn)確的初始值及協(xié)方差矩陣，因此會造成濾波算法性能的降低。針對這一問題，本文以雙目相機(jī)對目標(biāo)特征點(diǎn)的投影作為觀測量，定義不同目標(biāo)特征點(diǎn)位置的差值為相對特征，通過分析發(fā)現(xiàn)相對特征的運(yùn)動只與相對旋轉(zhuǎn)運(yùn)動有關(guān)，而與相對平移運(yùn)動無關(guān)，且通過雙目相機(jī)可以直接測量相對特征在追蹤器本體系下的表示，在此基礎(chǔ)上進(jìn)一步分析了相對特征的投影模型，以相對特征作為觀測量，根據(jù)非合作目標(biāo)的旋轉(zhuǎn)動力學(xué)模型對目標(biāo)慣量主軸、慣量比、角速度進(jìn)行估計(jì)。以目標(biāo)特征點(diǎn)的位置作為觀測量，結(jié)合目標(biāo)本體坐標(biāo)系相對姿態(tài)的估計(jì)值，通過相對平移動力學(xué)模型對目標(biāo)的質(zhì)心位置、線速度進(jìn)行估計(jì)。最后，通過數(shù)值仿真校驗(yàn)所提出解耦估計(jì)方法的有效性。

1 運(yùn)動模型

1.1 坐標(biāo)系定義

1.2 動力學(xué)模型

非合作目標(biāo)本體系相對于追蹤器本體系的相對姿態(tài)用四元數(shù)qct表示，在此分別引入兩種四元數(shù)乘法算子為[18]：

(1)

式中：q=[Tη]T，為四元數(shù)的向量部分，η為四元數(shù)的標(biāo)量部分，I為單位矩陣，[a]×為向量a=[a1,a2,a3]T的反對稱矩陣，即

(2)

與四元數(shù)q相對應(yīng)的旋轉(zhuǎn)矩陣為：

C(q)=(2η2-1)I+2T+2η[]×

(3)

ωr=ωt-CT(qct)ωc

(4)

相對旋轉(zhuǎn)的運(yùn)動學(xué)方程為：

(5)

(6)

非合作目標(biāo)與追蹤器之間的相對旋轉(zhuǎn)動力學(xué)模型可以由下式描述：

(7)

(8)

(9)

1.3 相對特征運(yùn)動模型

首先建立非合作目標(biāo)表面特征點(diǎn)的運(yùn)動模型，假設(shè)有N個表示在非合作目標(biāo)本體系下的特征點(diǎn)，記為fi(i=1,…,N)，特征點(diǎn)固定在非合作目標(biāo)上，即滿足

(10)

則特征點(diǎn)fi在追蹤器本體系下的觀測向量ri為：

ri=rct+C(qct)fi

(11)

對式(11)求一價(jià)導(dǎo)數(shù)可得：

(12)

由式(12)可以看出非合作目標(biāo)上固定特征點(diǎn)在追蹤器下的觀測向量受旋轉(zhuǎn)和平移運(yùn)動的耦合影響。直接以ri=[xiyizi]T在雙目相機(jī)像平面的投影作為觀測向量進(jìn)行濾波運(yùn)算的難點(diǎn)是fi在非合作目標(biāo)本體系下的坐標(biāo)無法得知，因此很難選取合適的初始值及初始協(xié)方差矩陣，這也是文獻(xiàn)[16-17]中仿真結(jié)果出現(xiàn)估計(jì)偏差的原因。

為解決這一問題，本文提出相對特征的概念，將相對旋轉(zhuǎn)運(yùn)動估計(jì)和相對平移運(yùn)動估計(jì)進(jìn)行解耦。考慮非合作目標(biāo)表面上兩個不同的特征點(diǎn)fi和fj(j≠i)，則相對特征的定義為：

(13)

則相對特征在追蹤器本體系下的觀測量為：

(14)

則相對特征在追蹤器本體系下的一階導(dǎo)數(shù)為：

(15)

2 觀測模型

如圖2所示，假設(shè)在追蹤器上安裝有固定的雙目相機(jī)，兩相機(jī)的光軸平行，兩相機(jī)的焦距均為f，兩相機(jī)之間的基線長度記為b,兩相機(jī)的像主點(diǎn)分別為[luclvc]T和[rucrvc]T,以左相機(jī)的坐標(biāo)系作為雙目相機(jī)的參考系，為簡單起見，假設(shè)左相機(jī)的坐標(biāo)系與追蹤器的本體坐標(biāo)系重合。

非合作目標(biāo)上的特征點(diǎn)fi在左右相機(jī)上的投影分別為

(16)

其中,b=[b0 0]T,π(·)為相機(jī)投影函數(shù)：

(17)

其中a=[a1a2a3]T。

(18)

(19)

對式(19)進(jìn)一步變換可得相對特征橫坐標(biāo)的計(jì)算方程：

(20)

使用雙目相機(jī)求特征點(diǎn)深度信息的函數(shù)為：

式中:f為相機(jī)焦距，b為左右相機(jī)之間的基線長度，du=lu-ru為特征點(diǎn)在左右相機(jī)像平面投影橫坐標(biāo)的差值，將上式代入式(20)可得：

(21)

同樣，可以得到相對縱坐標(biāo)的計(jì)算方程為：

(22)

同理可以得到相對特征在右相機(jī)像平面的虛擬投影，經(jīng)過驗(yàn)證相對特征在左右相機(jī)的虛擬投影是相等的，因此僅將相對特征在左相機(jī)的投影作為觀測量即可，值得注意的是,在構(gòu)造相對特征時需要首先計(jì)算特征點(diǎn)的三維坐標(biāo)，并對相對特征進(jìn)行初始化，相對特征的虛擬投影可以直接根據(jù)式(21)和式(22)計(jì)算得出。為避免出現(xiàn)奇異，所選擇特征點(diǎn)的深度值不能太接近。

3 相對狀態(tài)估計(jì)

第1節(jié)和第2節(jié)分別描述了非合作目標(biāo)與追蹤器之間相對運(yùn)動的動力學(xué)模型和使用雙目相機(jī)作為測量設(shè)備時的觀測模型，并在此基礎(chǔ)上提出了相對特征的概念，用于對非合作目標(biāo)相對旋轉(zhuǎn)估計(jì)和相對平移估計(jì)進(jìn)行解耦。本節(jié)主要描述使用相對特征對非合作目標(biāo)相對旋轉(zhuǎn)運(yùn)動進(jìn)行估計(jì)的方法以及使用特征點(diǎn)位置對相對平移運(yùn)動進(jìn)行估計(jì)的方法。

3.1 相對旋轉(zhuǎn)運(yùn)動估計(jì)

非合作目標(biāo)相對旋轉(zhuǎn)運(yùn)動系統(tǒng)的待估計(jì)參數(shù)包括慣量主軸的姿態(tài)四元數(shù)、角速度、轉(zhuǎn)動慣量比以及相對特征在左相機(jī)坐標(biāo)系下的三維坐標(biāo)表示。由于非合作目標(biāo)的慣量參數(shù)未知，需要對轉(zhuǎn)動慣量進(jìn)行參數(shù)化，并將其作為待估計(jì)變量，另外直接使用加性姿態(tài)四元數(shù)進(jìn)行濾波有一定的局限性，因此本文使用乘性誤差四元數(shù)作為狀態(tài)變量進(jìn)行估計(jì)。

3.1.1轉(zhuǎn)動慣量參數(shù)化

由于非合作目標(biāo)的慣量參數(shù)未知，進(jìn)行狀態(tài)估計(jì)時無法直接使用式(7)，需要對轉(zhuǎn)動慣量比進(jìn)行參數(shù)化，文獻(xiàn)[2,10,11,15]分別使用了幾種不同的方法對轉(zhuǎn)動慣量比進(jìn)行參數(shù)化，但參數(shù)化后的慣量比取值范圍還有一定的約束，因此本文提出一種新的參數(shù)化方法使慣量比為相互獨(dú)立的變量，非合作目標(biāo)的姿態(tài)動力學(xué)方程為：

(23)

由于非合作目標(biāo)本體系原點(diǎn)在質(zhì)心上，且坐標(biāo)軸與慣量主軸平行，因此轉(zhuǎn)動慣量矩陣Jt為對角矩陣，記Jt=diag(J1,J2,J3)，假設(shè)非合作目標(biāo)的轉(zhuǎn)動慣量矩陣滿足J1

(24)

式中:

(25)

現(xiàn)對式(25)中的慣量比做如下變換：

(26)

式中:ea=exp(a), -∞

將式(25)中的慣量參數(shù)代入式(24)可得：

(27)

(28)

3.1.2乘性誤差四元數(shù)

直接使用四元數(shù)進(jìn)行狀態(tài)估計(jì)有一定的局限性，首先是參數(shù)的自由度，描述姿態(tài)的參數(shù)自由度為3，而四元數(shù)有4個變量；其次四元數(shù)有范數(shù)為1的約束條件。因此，在使用四元數(shù)描述姿態(tài)時，通常使用乘性誤差四元數(shù)設(shè)計(jì)估計(jì)算法，記非合作目標(biāo)本體坐標(biāo)系姿態(tài)四元數(shù)的誤差為：

(29)

(30)

3.1.3濾波算法

定義該系統(tǒng)的狀態(tài)向量為：

其中,1≤s≤S，此處為了描述方便，假設(shè)有S個相對特征，xr對應(yīng)的動力學(xué)模型可以用下式來描述：

(31)

其中,xr中相應(yīng)狀態(tài)變量的動力學(xué)方程在第1節(jié)中給出，Gr=diag(03×3,Gω,03S×3S)，相對旋轉(zhuǎn)運(yùn)動估計(jì)時以相對特征作為觀測量，即

(32)

對過程模型的線性化如下：

(33)

其中,M和N可以從式(27)分別對角速度和轉(zhuǎn)動慣量比求導(dǎo)得出。

測量模型的Jacobian矩陣可以通過對式(32)求一階導(dǎo)數(shù)得到：

(34)

其中,

最后使用標(biāo)準(zhǔn)的擴(kuò)展卡爾曼濾波器對式(33)和式(34)所表示的過程模型和測量模型進(jìn)行狀態(tài)估計(jì)。注意由于使用了乘性誤差四元數(shù)來描述目標(biāo)旋轉(zhuǎn)動力學(xué)，因此姿態(tài)四元數(shù)的更新使用四元數(shù)乘法實(shí)現(xiàn)。

3.2 相對平移運(yùn)動估計(jì)

由于非合作目標(biāo)表面特征點(diǎn)的運(yùn)動受旋轉(zhuǎn)耦合影響，根據(jù)所建立目標(biāo)質(zhì)心、特征點(diǎn)運(yùn)動方程以及所估計(jì)的旋轉(zhuǎn)運(yùn)動量針對質(zhì)心及其速度設(shè)計(jì)估計(jì)算法， 以特征點(diǎn)的位置作為觀測量

(35)

對應(yīng)的過程模型為：

式中:G0=diag(03×3,I3×3,03N×3N)，第i個特征點(diǎn)的觀測量構(gòu)成的觀測方程為：

yt,i=[I0…C(qct)…]xt+νf

(36)

式(35)和式(36)所表示的非合作目標(biāo)質(zhì)心過程模型和測量模型均為線性函數(shù)，因此使用標(biāo)準(zhǔn)的線性卡爾曼濾波器即可估計(jì)出目標(biāo)的相對平移運(yùn)動，此處不再贅述。

4 仿真分析

為了校驗(yàn)本文所提出方法的有效性，本節(jié)通過非合作目標(biāo)相對運(yùn)動的動力學(xué)模型模擬相對運(yùn)動軌跡，并根據(jù)相對運(yùn)動軌跡及雙目相機(jī)成像模型產(chǎn)生特征點(diǎn)在圖像上的位置，最后根據(jù)雙目相機(jī)的模擬觀測信息。為了說明本文方法的優(yōu)點(diǎn)，分別使用本文所提出方法和文獻(xiàn)[16]中的方法對非合作目標(biāo)的相對狀態(tài)進(jìn)行估計(jì)，并對仿真結(jié)果進(jìn)行對比分析。

4.1 仿真條件

追蹤器的軌道近似為圓軌道，其半徑設(shè)置為7170 km，由此可以計(jì)算出軌道真近點(diǎn)角速率n=0.0001 rad·s-1，追蹤器的轉(zhuǎn)動慣量矩陣Jc=diag(500,600,700) kg·m2，非合作目標(biāo)的轉(zhuǎn)動慣量矩陣Jt=diag(410,578,751) kg·m2。左右相機(jī)的焦距均為f=8 mm，像元尺寸為5.5 μm×5.5 μm，圖像分辨率為2048×2048，兩相機(jī)的像主點(diǎn)均為[1024 1024]，相機(jī)的之間的基線長度b=0.3 m。固定在非合作目標(biāo)上的特征點(diǎn)分布如表1所示。

表1 特征點(diǎn)位置分布Table 1 The distribution of feature points’ position

動力學(xué)仿真時間為0～200 s，步長為0.1 s，仿真初始條件設(shè)置如下：

4.2 仿真結(jié)果分析

為了方便對比本文方法和文獻(xiàn)[16]方法對旋轉(zhuǎn)非合作目標(biāo)姿態(tài)角度的估計(jì)值，將姿態(tài)四元數(shù)轉(zhuǎn)換為軸角的形式。由于文獻(xiàn)[16]沒有直接對目標(biāo)轉(zhuǎn)動慣量比進(jìn)行估計(jì)，因此僅給出本文方法對轉(zhuǎn)動慣量比的估計(jì)曲線。

圖3～5分別為非合作目標(biāo)慣量主軸姿態(tài)四元數(shù)、角速度和轉(zhuǎn)動慣量比估計(jì)值與仿真所使用真實(shí)值之間的誤差隨時間的變化曲線。圖3中,本文方法所估計(jì)的姿態(tài)四元數(shù)轉(zhuǎn)換成軸角后的角度誤差范圍為[-1,1]°，圖4中本文方法所估計(jì)的角速度誤差范圍為[-0.3,0.3](°)·s-1，所估計(jì)的轉(zhuǎn)動慣量比誤差范圍為[-0.02,0.02]，說明本文所設(shè)計(jì)的相對旋轉(zhuǎn)運(yùn)動估計(jì)方法能夠有效地對非合作目標(biāo)的慣量主軸、角速度及轉(zhuǎn)動慣量比進(jìn)行估計(jì)。

圖6、圖7分別為非合作目標(biāo)質(zhì)心相對位置和速度與真實(shí)值的誤差隨時間的變化曲線。本文方法所估計(jì)的質(zhì)心位置的估計(jì)誤差范圍為[-0.02, 0.02] m，質(zhì)心速度的估計(jì)誤差范圍為[-0.001, 0.001] m·s-1，充分說明相對平移運(yùn)動估計(jì)算法的有效性，值得注意的是，對相對平移運(yùn)動進(jìn)行估計(jì)的時候，需要將非合作目標(biāo)慣量主軸姿態(tài)四元數(shù)的估計(jì)值作為輸入，由此也進(jìn)一步驗(yàn)證了相對旋轉(zhuǎn)運(yùn)動估計(jì)結(jié)果的有效性。

仿真結(jié)果表明，文獻(xiàn)[16]方法估計(jì)結(jié)果出現(xiàn)偏差，主要原因是非合作目標(biāo)特征點(diǎn)與目標(biāo)質(zhì)心的相對位置是未知的，因此初始化濾波器初始狀態(tài)時很難選取合適的初始值及初始協(xié)方差矩陣，導(dǎo)致濾波器的性能降低。本文所提方法使用相對特征實(shí)現(xiàn)了旋轉(zhuǎn)運(yùn)動和平移運(yùn)動估計(jì)的解耦，解決了狀態(tài)初始值不確定的問題，從而提高了濾波器的性能。

5 結(jié) 論

針對在軌旋轉(zhuǎn)非合作目標(biāo)相對狀態(tài)估計(jì)中的特征點(diǎn)位置不確定的問題，本文提出了一種使用相對特征對相對旋轉(zhuǎn)和相對平移運(yùn)動估計(jì)進(jìn)行解耦的方法。通過建立相對特征運(yùn)動模型及使用特征點(diǎn)計(jì)算相對特征投影的觀測方程，設(shè)計(jì)擴(kuò)展卡爾曼濾波器對目標(biāo)慣量主軸、角速度及轉(zhuǎn)動慣量比進(jìn)行估計(jì)；根據(jù)特征點(diǎn)的耦合運(yùn)動模型設(shè)計(jì)卡爾曼濾波器對目標(biāo)質(zhì)心及速度進(jìn)行估計(jì)。通過仿真的對比分析，證明本文所提出的解耦估計(jì)方法能夠有效地對旋轉(zhuǎn)非合作目標(biāo)的相對狀態(tài)進(jìn)行估計(jì)。

本文所提出的使用相對特征進(jìn)行解耦估計(jì)的方法需要通過雙目相機(jī)進(jìn)行測量才能實(shí)現(xiàn)，考慮到通過安裝位置偏置的單目相機(jī)也可以保證特征點(diǎn)深度信息可觀[14-15]，因此未來考慮研究基于單目相機(jī)的解耦估計(jì)方法。