一種參數(shù)化非合作目標(biāo)相對(duì)位姿和慣量估計(jì)方法

馮 乾，潘 泉，侯曉磊，楊家男

(1. 西北工業(yè)大學(xué)自動(dòng)化學(xué)院，西安 710072；2. 西北工業(yè)大學(xué)信息融合教育部重點(diǎn)實(shí)驗(yàn)室，西安 710072)

0 引 言

近些年來，隨著人類空間活動(dòng)日益增加，在外太空滯留大量的失效航天器、廢棄衛(wèi)星及火箭末級(jí)等空間碎片，這些空間碎片對(duì)其它航天器的正常運(yùn)行構(gòu)成巨大的潛在威脅[1-2]。據(jù)美國空間監(jiān)測(cè)網(wǎng)絡(luò)(Space surveillance network, SSN)報(bào)告，截止2020年，環(huán)繞在地球周圍的航天器(包括主動(dòng)/被動(dòng)衛(wèi)星、衛(wèi)星碎片、火箭外殼等)總數(shù)大約為20000個(gè)，其中包含約11000個(gè)解體碎片，2000個(gè)任務(wù)相關(guān)碎片[3]。為了維護(hù)空間安全，降低空間碎片與正常航天器碰撞的概率，亟需采用主動(dòng)清除手段來減少這些碎片[4]。為了實(shí)現(xiàn)對(duì)空間碎片的有效抓捕/清除，對(duì)空間碎片的相對(duì)狀態(tài)(包括相對(duì)位置、相對(duì)線速度、相對(duì)姿態(tài)和角速度)和慣量參數(shù)進(jìn)行有效估計(jì)具有重要的現(xiàn)實(shí)意義[5-6]。

與傳統(tǒng)交會(huì)對(duì)接任務(wù)中的合作目標(biāo)相比，大部分空間碎片屬于非合作目標(biāo)，即目標(biāo)的幾何參數(shù)和慣量參數(shù)未知，沒有可以直接測(cè)量角速度的傳感器，缺乏航天器間的相互通信，這些特征給非合作目標(biāo)的相對(duì)導(dǎo)航帶來了很大挑戰(zhàn)[7]。CCD/CMOS等視覺傳感器具有非接觸、體積小、功耗低等優(yōu)勢(shì)，廣泛應(yīng)用于非合作目標(biāo)的相對(duì)位姿(位置和姿態(tài))測(cè)量。文獻(xiàn)[8]提出一種基于單目視覺的迭代算法來求解非合作目標(biāo)相對(duì)位姿參數(shù)，該方法假設(shè)目標(biāo)的形狀及幾何尺寸已知，通過迭代方法求解目標(biāo)的相對(duì)位姿。文獻(xiàn)[9]通過前后不同時(shí)刻非合作目標(biāo)旋轉(zhuǎn)軸的中線中點(diǎn)來近似非合作目標(biāo)質(zhì)心位置，對(duì)于高速旋轉(zhuǎn)且低速平移的非合作目標(biāo)能夠較好地近似質(zhì)心位置。文獻(xiàn)[10]假設(shè)先驗(yàn)已知目標(biāo)的CAD結(jié)構(gòu)，采用迭代最近點(diǎn)(Iterative closest point,ICP)方法來解算目標(biāo)的相對(duì)位姿信息，同時(shí)還引入狀態(tài)預(yù)測(cè)反饋來提高ICP算法的魯棒性。文獻(xiàn)[11]提出一種基于雙目立體視覺的相對(duì)位姿測(cè)量方法，該方法以航天器上太陽帆板支架作為識(shí)別對(duì)象，借助支架上的三個(gè)頂點(diǎn)來解算非合作目標(biāo)的相對(duì)位姿參數(shù)，但在第一幀識(shí)別過程中需要借助地面遙測(cè)通過人機(jī)交互方式來選擇支架的一組內(nèi)外點(diǎn)。文獻(xiàn)[12]提出一種基于幾何特征綜合匹配的雙目視覺相對(duì)位姿測(cè)量方法，該方法以非合作目標(biāo)的太陽帆板為幾何特征，經(jīng)過特征提取及特征匹配獲取太陽帆板上的若干特征點(diǎn)信息，進(jìn)而解算出非合作目標(biāo)的相對(duì)位姿參數(shù)。

為了實(shí)現(xiàn)對(duì)非合作翻滾目標(biāo)的精確近場(chǎng)操作，需要估計(jì)出非合作目標(biāo)的慣量參數(shù)，進(jìn)而預(yù)測(cè)出非合作目標(biāo)的旋轉(zhuǎn)狀態(tài)[13]。文獻(xiàn)[14]采用立體視覺追蹤目標(biāo)上若干特征點(diǎn)，預(yù)先給定目標(biāo)慣性張量的若干模態(tài)，在每種模態(tài)下采用迭代擴(kuò)展卡爾曼濾波(Iterated extended Kalman filter, IEKF)估計(jì)出相對(duì)位姿參數(shù)和特征點(diǎn)位置，然后基于各模態(tài)下的位姿參數(shù)和立體視覺量測(cè)采用最大后驗(yàn)概率估計(jì)獲得目標(biāo)的最優(yōu)慣性張量和位姿參數(shù)，但該方法得到的慣性張量只能是預(yù)先給出各模態(tài)中的某一模態(tài)。文獻(xiàn)[15]由立體視覺獲得的非合作目標(biāo)上三個(gè)特征點(diǎn)建立非合作目標(biāo)幾何坐標(biāo)系，然后基于該幾何坐標(biāo)系量測(cè)先后設(shè)計(jì)了姿態(tài)測(cè)量和相對(duì)導(dǎo)航濾波器，從而估計(jì)出非合作目標(biāo)的相對(duì)狀態(tài)和慣量比率，同時(shí)借助粘附衛(wèi)星實(shí)現(xiàn)對(duì)非合作目標(biāo)質(zhì)量和轉(zhuǎn)動(dòng)慣量的辨識(shí)，但在慣量比率的估計(jì)過程中沒有考慮各慣量比率之間的不等式約束。文獻(xiàn)[16]通過引入目標(biāo)的慣量比率來傳播目標(biāo)的角速度，設(shè)計(jì)了基于擴(kuò)展卡爾曼濾波(Extended Kalman filter, EKF)的單目視覺相對(duì)導(dǎo)航方法，但所引入的三個(gè)慣量比率彼此不獨(dú)立且存在區(qū)間約束。文獻(xiàn)[17]提出了一種基于即時(shí)定位與地圖構(gòu)建(Simultaneous localization and mapping,SLAM)技術(shù)的非合作目標(biāo)相對(duì)位姿估計(jì)方法，該方法通過反復(fù)迭代來求解目標(biāo)的相對(duì)位姿、質(zhì)心位置、主慣性軸及慣量比率等參數(shù)，但該方法計(jì)算量大，很難滿足實(shí)時(shí)性要求。文獻(xiàn)[18]建立了一種非合作目標(biāo)相對(duì)特征的運(yùn)動(dòng)模型和投影模型，然后對(duì)非合作目標(biāo)的旋轉(zhuǎn)運(yùn)動(dòng)和平移運(yùn)動(dòng)進(jìn)行解耦，最終設(shè)計(jì)EKF算法對(duì)非合作目標(biāo)的相對(duì)狀態(tài)進(jìn)行估計(jì)，其中慣量比率采用冪指數(shù)形式來參數(shù)化，雖然取值范圍沒有限制，但彼此之間需滿足三角約束，即任意兩個(gè)軸主轉(zhuǎn)動(dòng)慣量之和大于第三軸主轉(zhuǎn)動(dòng)慣量。文獻(xiàn)[19]以非合作目標(biāo)固有特征的像素位置為觀測(cè)輸入，提出一種基于單目視覺的非合作目標(biāo)相對(duì)狀態(tài)估計(jì)方法，其中慣量參數(shù)采用文獻(xiàn)[18]的參數(shù)化方法，也存在相應(yīng)的不等式約束。

到目前為止，由立體視覺系統(tǒng)同時(shí)估計(jì)非合作目標(biāo)的相對(duì)狀態(tài)和慣量參數(shù)仍然是一個(gè)研究難點(diǎn)，現(xiàn)有的參數(shù)化方法所描述的慣量參數(shù)都存在等式約束或不等式約束，不能作為獨(dú)立的自由變量進(jìn)行狀態(tài)估計(jì)。針對(duì)非合作目標(biāo)缺乏先驗(yàn)?zāi)Ｐ?，無法直接獲得其角速度以及慣量參數(shù)的約束問題，本文采用立體視覺測(cè)量系統(tǒng)跟蹤目標(biāo)上的若干特征點(diǎn)，用反雙曲正切函數(shù)來描述非合作目標(biāo)的慣量參數(shù)，結(jié)合自由航天器姿態(tài)動(dòng)力學(xué)模型來傳播非合作目標(biāo)的角速度，進(jìn)而設(shè)計(jì)EKF算法以同時(shí)估計(jì)非合作目標(biāo)的相對(duì)狀態(tài)和慣量參數(shù)。其中，本文所提的慣量參數(shù)化方法僅含兩個(gè)獨(dú)立變量，且充分考慮了慣量參數(shù)之間的相互約束。

本文其余部分安排如下：第1節(jié)給出本文用到的各類坐標(biāo)系，并推導(dǎo)出非合作目標(biāo)的相對(duì)平移動(dòng)力學(xué)方程、相對(duì)姿態(tài)運(yùn)動(dòng)學(xué)方程、姿態(tài)動(dòng)力學(xué)方程以及立體視覺量測(cè)方程；第2節(jié)設(shè)計(jì)了估計(jì)非合作目標(biāo)相對(duì)狀態(tài)和慣量參數(shù)的EKF算法；第3節(jié)給出了數(shù)值仿真驗(yàn)證；第4節(jié)總結(jié)了本文所提的算法。

1 系統(tǒng)模型

本節(jié)主要介紹濾波器設(shè)計(jì)所需的各類系統(tǒng)模型，包括非合作目標(biāo)的相對(duì)平移動(dòng)力學(xué)方程、相對(duì)姿態(tài)運(yùn)動(dòng)學(xué)方程、非合作目標(biāo)姿態(tài)動(dòng)力學(xué)方程以及立體視覺量測(cè)方程。尤其在非合作目標(biāo)姿態(tài)動(dòng)力學(xué)模型中，創(chuàng)造性地引入了反雙曲正切函數(shù)來描述慣量參數(shù)。

本文用到的坐標(biāo)系包括地心慣性坐標(biāo)系、本體坐標(biāo)系、軌道坐標(biāo)系和相機(jī)坐標(biāo)系，各坐標(biāo)系的示意圖如圖1所示。具體定義如下：

圖1 各坐標(biāo)系示意圖Fig.1 Schematic diagram for the coordinate systems

1.1 相對(duì)平移動(dòng)力學(xué)方程

(1)

(2)

1.2 相對(duì)姿態(tài)運(yùn)動(dòng)學(xué)方程

R(qtc)=ΞT(qtc)Ψ(qtc)

(3)

其中:

式中:In表示n×n的單位矩陣;a×是向量a=[a1,a2,a3]T對(duì)應(yīng)的叉乘矩陣，可表示為:

(4)

用四元數(shù)描述的相對(duì)姿態(tài)運(yùn)動(dòng)學(xué)方程可表示為:

(5)

式(5)的一種離散化描述為:

(6)

其中

(7)

(8)

1.3 非合作目標(biāo)航天器姿態(tài)動(dòng)力學(xué)方程

在不考慮主動(dòng)力矩控制和噪聲情況下，非合作目標(biāo)航天器的姿態(tài)動(dòng)力學(xué)模型可表示為:

(9)

式中:Jt=diag(Jtxx,Jtyy,Jtzz)為非合作目標(biāo)航天器的主慣性張量矩陣，Jtxx、Jtyy和Jtzz分別對(duì)應(yīng)各軸主轉(zhuǎn)動(dòng)慣量，展開式(9)可得:

(10)

由式(10)可以看出，當(dāng)非合作目標(biāo)航天器的主轉(zhuǎn)動(dòng)慣量等比擴(kuò)大或縮小相同比例時(shí)，其姿態(tài)動(dòng)力學(xué)方程保持不變，因此在沒有對(duì)非合作目標(biāo)航天器施加主動(dòng)力矩的情況下，基于視覺的測(cè)量方法只能解算出轉(zhuǎn)動(dòng)慣量的相對(duì)值。定義以下慣量比率:

(11)

由定義(11)可得各慣量比率之間存在以下約束:

px+py+pz+pxpypz=0

(12)

顯然，式(11)定義的三個(gè)慣量比率并不獨(dú)立，由等式約束(12)可知慣量比率只有兩個(gè)獨(dú)立變量，即任一慣量比率可由其它兩個(gè)慣量比率來表示，如:

(13)

此外，由主轉(zhuǎn)動(dòng)慣量的物理意義可得以下約束:

(14)

對(duì)應(yīng)的慣量比率滿足約束:

(15)

如果直接將式(11)定義的慣量比率作為狀態(tài)量進(jìn)行估計(jì)，存在以下問題：1)其取值范圍應(yīng)為(-1,1)，不是取值任意的高斯隨機(jī)變量；2)采用三個(gè)非獨(dú)立的變量描述慣量比率，最終估計(jì)出的慣量比率不再滿足等式約束式(12)；3)在某些時(shí)刻，所估計(jì)的慣量比率因不滿足不等式約束式(15)而失去物理意義。為此，本文提出一種用反雙曲正切函數(shù)來描述慣量參數(shù)的方法，定義慣量參數(shù)kω=[k1,k2]T，使得:

(16)

此時(shí)慣性張量可表示為:

(17)

由定義(16)可知參數(shù)kω各分量的取值范圍為(-∞， +∞)，由式(17)可知kω也滿足慣量參數(shù)之間不等式約束式(14)。如果將式(17)代入式(11)，可以得到:

(18)

顯然，此時(shí)各慣量比率可表示為參數(shù)kω的雙曲正切函數(shù)形式，由雙曲正切函數(shù)的值域?yàn)?-1,1)可知慣量比率也滿足式(15)的約束。因此，用反雙曲正切函數(shù)定義的含兩個(gè)獨(dú)立變量的慣量參數(shù)kω能很好地滿足等式約束式(12)和不等式約束式(14)，慣量參數(shù)都符合物理意義，且kω各分量的取值范圍為(-∞，+∞)，可直接作為無約束的狀態(tài)變量進(jìn)行估計(jì)。通過這種潛在包含約束的參數(shù)化方法，其實(shí)質(zhì)是引入了新信息(約束信息)，從而提高狀態(tài)估計(jì)的性能。

考慮到重力梯度、太陽光壓及空間氣壓等對(duì)非合作目標(biāo)航天器產(chǎn)生的干擾力矩t=[tx,ty,tz]T該干擾力矩服從均值為零,協(xié)方差為的正態(tài)分布，則式(10)可重新寫為:

(19)

將參數(shù)k1和k2代入式(19)，得:

(20)

其中:

1.4 立體視覺測(cè)量方程

圖2 立體視覺測(cè)量系統(tǒng)Fig.2 Stereo vision measurement system

(21)

式中:[uiL,viL]T和[uiR,viR]T分別是左右相機(jī)的圖像坐標(biāo)，f是相機(jī)的焦距，b是相機(jī)的基線。

(22)

(23)

(24)

式中:N為特征點(diǎn)數(shù)目。

圖3 特征點(diǎn)坐標(biāo)幾何關(guān)系圖Fig.3 Geometric relationship of some feature point’s coordinates

(25)

2 濾波器設(shè)計(jì)

基于以上各運(yùn)動(dòng)模型和立體視覺量測(cè)模型，本節(jié)主要對(duì)各狀態(tài)方程和量測(cè)方程進(jìn)行線性化處理，并設(shè)計(jì)EKF算法以估計(jì)各狀態(tài)量。

2.1 線性化狀態(tài)方程

根據(jù)誤差四元數(shù)的定義得:

(26)

對(duì)式(26)兩端求導(dǎo)得:

(27)

四元數(shù)估計(jì)量的姿態(tài)運(yùn)動(dòng)學(xué)方程為:

(28)

分別將式(5)、(26)和(28)代入式(27)得:

(29)

(30)

(31)

將式(31)代入式(29)并忽略二階項(xiàng)，得:

(32)

(33)

其中:

對(duì)于剛體目標(biāo)，其慣性張量是常量，對(duì)應(yīng)的慣量參數(shù)也是常量，即慣量參數(shù)kω的誤差方程滿足:

(34)

由式(2)可得相對(duì)平移誤差運(yùn)動(dòng)學(xué)方程為:

(35)

式中:

綜上，線性化的狀態(tài)誤差運(yùn)動(dòng)學(xué)方程可表示為:

(36)

式中:

對(duì)應(yīng)的系統(tǒng)噪聲序列方差陣為:

(37)

2.2 線性化量測(cè)方程

結(jié)合式(21)，對(duì)量測(cè)方程(22)進(jìn)行線性化可得:

1≤i≤N

(38)

式中:

(39)

基于EKF的相對(duì)狀態(tài)和慣量參數(shù)估計(jì)算法總結(jié)見表1。

表1 基于EKF的相對(duì)狀態(tài)和慣量參數(shù)估計(jì)算法Table 1 Relative state and inertia parameters estimation algorithm based on EKF

其中，上標(biāo)“-”和“+”分別對(duì)應(yīng)各分量的預(yù)測(cè)值和更新值。

3 仿真校驗(yàn)

表2 特征點(diǎn)位置坐標(biāo)Table 2 Location of the feature points

表3 仿真參數(shù)Table 3 Simulations parameters

在仿真中，相對(duì)姿態(tài)的濾波初值是在真值上加[1°, -1°, 1°]T的姿態(tài)誤差；非合作目標(biāo)航天器角速度、相對(duì)位置和線速度的濾波初值均在真值上加1σ誤差；慣量參數(shù)的濾波初值設(shè)為零。濾波器的初始協(xié)方差矩陣為：

非合作目標(biāo)的真實(shí)相對(duì)位置軌跡如圖4所示，200次蒙特卡洛仿真的誤差和3σ邊界結(jié)果如圖5～9所示，其中，3σ是3倍的200次蒙特卡洛仿真樣本標(biāo)準(zhǔn)差。

圖4 非合作目標(biāo)真實(shí)相對(duì)位置Fig.4 The true relative position of the non-cooperative target

圖5 相對(duì)姿態(tài)估計(jì)誤差和3σ邊界Fig.5 Relative attitude errors and 3σ bounds

圖6 相對(duì)角速度估計(jì)誤差和3σ邊界Fig.6 Relative angular velocity errors and 3σ bounds

圖7 慣量參數(shù)估計(jì)誤差和3σ邊界Fig.7 Inertia parameter errors and 3σ bounds

圖8 相對(duì)位置估計(jì)誤差和3σ邊界Fig.8 Relative position errors and 3σ bounds

圖9 相對(duì)線速度估計(jì)誤差和3σ邊界Fig.9 Relative linear velocity errors and 3σ bounds

為了進(jìn)一步量化仿真誤差參數(shù)，定義時(shí)間平均均方根誤差(Root time-average mean square error, RTMSE)為:

(40)

eθ=2arccos(δqtc4)

(41)

在200次蒙特卡洛仿真中，由最后50 min仿真結(jié)果求得各狀態(tài)量的eRTSME值見表4。

表4 各狀態(tài)量時(shí)間平均均方根誤差Table 4 RTSME for each variable

考慮特征點(diǎn)數(shù)目、目標(biāo)尺寸(即特征點(diǎn)分布)以及噪聲水平對(duì)算法性能的影響，仿真對(duì)比以下三種場(chǎng)景：1)特征點(diǎn)數(shù)目分別為3、4、6、9和12，目標(biāo)尺寸為1.0 m，噪聲水平為10-5rad； 2)特征點(diǎn)數(shù)目為4，目標(biāo)尺寸分別為0.2 m、0.6 m、1.0 m、2.0 m和3.0 m，噪聲水平為10-5rad；3)特征點(diǎn)數(shù)目為4，目標(biāo)尺寸為0.6 m，噪聲水平分別為10-5rad、 2×10-5rad、 4×10-5rad、 6×10-5rad和8×10-5rad。對(duì)應(yīng)200次蒙特卡洛結(jié)果的eRTSME見表5～7。

表5 特征點(diǎn)個(gè)數(shù)對(duì)算法性能的影響Table 5 The influence of the number of the feature points on the algorithm performance

由圖5～9的仿真結(jié)果可以看出，所設(shè)計(jì)的算法能夠有效估計(jì)出非合作目標(biāo)的相對(duì)位姿和慣量參數(shù)，且結(jié)果都收斂于各自的3σ邊界。在中間階段誤差和3σ邊界略有增大，對(duì)比圖3的y軸可知，這是由于這一階段非合作目標(biāo)深度增加而導(dǎo)致立體視覺測(cè)量誤差增大造成的。結(jié)合表4的平均時(shí)間均方根誤差的量化結(jié)果看，各估計(jì)量都能夠達(dá)到較高精度，相對(duì)姿態(tài)誤差小于0.1°，相對(duì)角速度誤差小于6×10-5rad/s，相對(duì)位置誤差不超過10 mm，相對(duì)線速度誤差低于10 μm，同時(shí)慣量參數(shù)也收斂到不超過0.02的誤差。

對(duì)比分析表5可知，隨著特征點(diǎn)數(shù)目的增加，各狀態(tài)量的估計(jì)精度逐漸提高，但計(jì)算量也隨之加重，權(quán)衡精度和計(jì)算量，選取4個(gè)特征點(diǎn)較為合適。對(duì)比分析表6可知，隨著目標(biāo)尺寸的增大，各狀態(tài)量的估計(jì)精度也逐漸提高，這是因?yàn)樘卣鼽c(diǎn)的分布更加分散，立體視覺量測(cè)信息差異性更大，量測(cè)信息也更豐富。對(duì)比分析表7可以看出，噪聲水平直接影響各狀態(tài)量的估計(jì)，在實(shí)際應(yīng)用中盡可能選取噪聲水平低的測(cè)量系統(tǒng)，但從表7也可以看出，即使在8×10-5rad的噪聲水平下，所提算法仍能達(dá)到較高的精度。此外，表5～7的慣量參數(shù)估計(jì)結(jié)果表明，其估計(jì)精度在各場(chǎng)景中變化不大，這是因?yàn)樽鳛槌Ａ康膽T量參數(shù)狀態(tài)信息不豐富，同時(shí)本文采用非線性的反雙曲正切函數(shù)來描述慣量參數(shù)，在所設(shè)計(jì)的EKF算法中，非線性是影響其估計(jì)精度的主要原因。

表6 目標(biāo)尺寸對(duì)算法性能的影響Table 6 The influence of the size of the targe on the algorithm performance

表7 噪聲水平對(duì)算法性能的影響Table 7 The influence of the measurement noise level on the algorithm performance

4 結(jié) 論

本文針對(duì)非合作目標(biāo)缺乏可直接量測(cè)的角速度信息和慣量參數(shù)約束問題，通過引入反雙曲正切函數(shù)來描述慣量參數(shù)，借助姿態(tài)動(dòng)力學(xué)模型來傳播非合作目標(biāo)的角速度，并結(jié)合立體視覺測(cè)量系統(tǒng)跟蹤測(cè)量非合作目標(biāo)上三個(gè)特征點(diǎn)的位置信息，設(shè)計(jì)了一種EKF算法來估計(jì)非合作目標(biāo)的相對(duì)位姿和慣量參數(shù)。該算法所引入的慣量參數(shù)只包含兩個(gè)獨(dú)立變量，且沒有區(qū)間約束，能夠直接作為濾波器的狀態(tài)隨機(jī)變量進(jìn)行估計(jì)。通過蒙特卡洛數(shù)值仿真結(jié)果表明，所設(shè)計(jì)算法在不同量測(cè)噪聲水平下都能夠有效估計(jì)出非合作目標(biāo)相對(duì)位姿和慣量參數(shù)，且隨特征點(diǎn)數(shù)目和目標(biāo)尺寸的增加，估計(jì)精度逐漸提高。對(duì)于引入反雙曲正切函數(shù)所增加的系統(tǒng)非線性，下一階段將考慮采用基于梯度或隨機(jī)優(yōu)化等方法來提高處理系統(tǒng)非線性的能力。