采用單目視覺的非合作目標(biāo)星狀態(tài)估計(jì)

郭新程，孟中杰，黃攀峰

(1. 西北工業(yè)大學(xué)航天學(xué)院智能機(jī)器人研究中心，西安 710072；2. 西北工業(yè)大學(xué)航天飛行動(dòng)力學(xué)技術(shù)國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室，西安 710072)

0 引 言

航天器相對(duì)導(dǎo)航技術(shù)可以應(yīng)用在各種空間任務(wù)場(chǎng)景，例如編隊(duì)飛行、在軌服務(wù)、失效衛(wèi)星和空間碎片清除、彗星和小行星探索等，具有重要的研究意義。根據(jù)任務(wù)場(chǎng)景和目標(biāo)合作程度的不同，采用不同的方案和傳感器進(jìn)行相對(duì)導(dǎo)航[1]。對(duì)于合作目標(biāo)和模型已知的非合作目標(biāo)，相對(duì)狀態(tài)估計(jì)問(wèn)題已經(jīng)得到了很好的解決。然而，空間物體大多屬于模型未知的非合作目標(biāo)，即不能與主動(dòng)航天器進(jìn)行信息交互，未安裝光學(xué)標(biāo)志器且?guī)缀文Ｐ臀粗?。?duì)于此類目標(biāo)，由于缺乏目標(biāo)的運(yùn)動(dòng)、表面結(jié)構(gòu)和慣性參數(shù)等信息，相對(duì)狀態(tài)估計(jì)問(wèn)題變得尤為復(fù)雜。

實(shí)際上，空間未知非合作目標(biāo)的相對(duì)狀態(tài)估計(jì)問(wèn)題和地面機(jī)器人的同時(shí)定位與建圖(Simult-aneously localization and mapping, SLAM)問(wèn)題類似，即在估計(jì)目標(biāo)相對(duì)位姿的同時(shí)，恢復(fù)目標(biāo)的表面結(jié)構(gòu)，對(duì)于翻滾運(yùn)動(dòng)的目標(biāo)，還需要估計(jì)目標(biāo)質(zhì)心，慣量主軸等狀態(tài)。對(duì)此，國(guó)內(nèi)外學(xué)者主要采用光學(xué)敏感器作為測(cè)量傳感器并設(shè)計(jì)相應(yīng)的估計(jì)方法對(duì)非合作目標(biāo)的相對(duì)狀態(tài)進(jìn)行估計(jì)。采用的光學(xué)敏感器主要包括主動(dòng)式的激光雷達(dá)和被動(dòng)式的相機(jī)及由多個(gè)相機(jī)組成的立體視覺測(cè)量系統(tǒng)等。文獻(xiàn)[2]利用一組協(xié)同工作的三維傳感器(假設(shè)這些傳感器均勻分布在目標(biāo)周圍)來(lái)估計(jì)目標(biāo)的相對(duì)位姿和表面結(jié)構(gòu)。文獻(xiàn)[3-5]等利用立體視覺，并設(shè)計(jì)相應(yīng)的濾波或者平滑算法對(duì)非合作目標(biāo)相對(duì)狀態(tài)進(jìn)行估計(jì)。但是應(yīng)該注意到平臺(tái)衛(wèi)星對(duì)傳感器的限制，一方面要考慮平臺(tái)星的載荷、電氣和計(jì)算能力，另一方面也要考慮衛(wèi)星系統(tǒng)的設(shè)計(jì)和開發(fā)成本。尤其是針對(duì)小衛(wèi)星，如立方星參與的在軌服務(wù)任務(wù)，單目相機(jī)無(wú)疑是具有很大優(yōu)勢(shì)。和激光雷達(dá)相比，單目相機(jī)結(jié)構(gòu)簡(jiǎn)單，質(zhì)量小，功耗和經(jīng)濟(jì)成本低；和立體視覺系統(tǒng)相比，單目相機(jī)工作范圍更遠(yuǎn)，并且不受搭載平臺(tái)大小的限制。實(shí)際上，在目前的立方星等微小衛(wèi)星平臺(tái)上，受體積、質(zhì)量、功耗等限制，僅適合搭載單目相機(jī)。

當(dāng)然，使用單目相機(jī)進(jìn)行目標(biāo)星狀態(tài)估計(jì)也存在一些缺點(diǎn)，例如：被動(dòng)式的測(cè)量對(duì)光照條件敏感；無(wú)法直接獲得關(guān)于目標(biāo)三維測(cè)量等。特別是后者限制了單目相機(jī)的應(yīng)用，因此一些研究通過(guò)引入額外信息來(lái)解決。文獻(xiàn)[6]將單目相機(jī)和距離傳感器獲得的測(cè)量數(shù)據(jù)融合，通過(guò)UKF估計(jì)目標(biāo)狀態(tài)；文獻(xiàn)[7]雖然只使用了單目相機(jī)，但是假設(shè)了目標(biāo)的幾何模型是已知的。對(duì)于僅用單目相機(jī)對(duì)未知非合作目標(biāo)進(jìn)行相對(duì)位姿估計(jì)的文獻(xiàn)較少。文獻(xiàn)[8]提出了一種純單目視覺SLAM算法，使用特征點(diǎn)的圖像投影作為觀測(cè)，并設(shè)計(jì)濾波算法進(jìn)行相對(duì)狀態(tài)的估計(jì)。但是該文獻(xiàn)假設(shè)目標(biāo)以恒定角速度旋轉(zhuǎn)，因此無(wú)法估計(jì)目標(biāo)的轉(zhuǎn)動(dòng)慣量信息。文獻(xiàn)[9]使用特征點(diǎn)視線方向作為觀測(cè)量，通過(guò)EKF算法估計(jì)出目標(biāo)的狀態(tài)。為了說(shuō)明濾波器對(duì)位置相關(guān)狀態(tài)的可觀性，一方面使用基于Lie導(dǎo)數(shù)的可觀性秩條件法進(jìn)行分析；另一方面，采用文獻(xiàn)[10]的方法，在假設(shè)目標(biāo)姿態(tài)相關(guān)狀態(tài)已知的條件下，證明了相機(jī)偏置安裝時(shí)位置相關(guān)狀態(tài)是可觀的。此外，對(duì)于自由翻滾運(yùn)動(dòng)的目標(biāo)，還需要估計(jì)目標(biāo)的轉(zhuǎn)動(dòng)慣量。考慮到在缺乏激勵(lì)的條件下無(wú)法獲得轉(zhuǎn)動(dòng)慣量的絕對(duì)值，通常的處理方式是將轉(zhuǎn)動(dòng)慣量的比值作為參數(shù)一起進(jìn)行估計(jì)。文獻(xiàn)[5,9,11]采用了不同的轉(zhuǎn)動(dòng)慣量參數(shù)化方式。

利用直接估計(jì)[12]的方法(即以目標(biāo)表面特征點(diǎn)的投影作為觀測(cè)量，設(shè)計(jì)濾波算法估計(jì)目標(biāo)相對(duì)狀態(tài))可以很好地處理非合作未知目標(biāo)的狀態(tài)估計(jì)問(wèn)題，文獻(xiàn)[3-4,9,12]都是利用這種方法。直接估計(jì)的方法依賴于對(duì)目標(biāo)特征的跟蹤能力，并且由于待估計(jì)狀態(tài)包含每個(gè)特征的視線或三維位置矢量，因此計(jì)算量隨著特征點(diǎn)數(shù)量的增加而增加。而對(duì)于單目相機(jī)而言，很難直接得到未知非合作目標(biāo)的相對(duì)位置和姿態(tài)，因此使用直接估計(jì)的方式就成了首選。對(duì)此，本文針對(duì)立方星等小衛(wèi)星逼近觀測(cè)非合作目標(biāo)星任務(wù)，使用單目相機(jī)，以目標(biāo)特征點(diǎn)的投影像素位置作為觀測(cè)量，通過(guò)EKF估計(jì)未知非合作目標(biāo)的相對(duì)位置、相對(duì)線速度、相對(duì)姿態(tài)、角速度、轉(zhuǎn)動(dòng)慣量比和特征點(diǎn)位置。最后，以簡(jiǎn)化的ENVISAT衛(wèi)星模型為目標(biāo)，通過(guò)數(shù)值仿真校驗(yàn)所提出估計(jì)方法的有效性。

1 追蹤星/目標(biāo)星相對(duì)運(yùn)動(dòng)模型

1.1 坐標(biāo)系定義

為建立追蹤星與目標(biāo)星之間的相對(duì)運(yùn)動(dòng)模型，首先定義以下坐標(biāo)系。

地心慣性坐標(biāo)系I：原點(diǎn)位于地球質(zhì)心，z軸指向地球北極，x軸指向春分點(diǎn)，y軸由右手法則確定。

LVLH坐標(biāo)系H：坐標(biāo)系原點(diǎn)位于追蹤星質(zhì)心，x軸由地心指向追蹤星質(zhì)心，z軸垂直于軌道面且與軌道角動(dòng)量方向一致，y軸由右手法則確定。

追蹤星本體坐標(biāo)系A(chǔ)：坐標(biāo)原點(diǎn)固定在追蹤星質(zhì)心上，坐標(biāo)軸與追蹤星慣量主軸重合。

目標(biāo)本體坐標(biāo)系B：坐標(biāo)原點(diǎn)固定在目標(biāo)質(zhì)心上，坐標(biāo)軸與目標(biāo)的慣量主軸重合，規(guī)定最小慣量主軸為x軸，最大慣量主軸為z軸。

相機(jī)坐標(biāo)系C：坐標(biāo)原點(diǎn)固定在相機(jī)光心上，z軸沿相機(jī)光軸方向，x軸y軸平行于成像平面。

為簡(jiǎn)化模型，假設(shè)LVLH坐標(biāo)系和追蹤星本體坐標(biāo)系重合，相機(jī)坐標(biāo)系和追蹤星本體坐標(biāo)系各坐標(biāo)軸平行。

1.2 相對(duì)位置運(yùn)動(dòng)模型

(1)

(2)

(3)

(4)

1.3 相對(duì)姿態(tài)運(yùn)動(dòng)模型

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

(5)

(6)

定義四元數(shù)運(yùn)算：

(7)

定義追蹤星和目標(biāo)星的角速度在各自本體坐標(biāo)系下的表示分別為ωc,ωt，將兩者的相對(duì)速度表示在目標(biāo)本體系下可得：

(8)

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

(9)

相對(duì)旋轉(zhuǎn)的動(dòng)力學(xué)方程為：

(10)

式中：Jc,Jt分別為追蹤星和目標(biāo)的轉(zhuǎn)動(dòng)慣量矩陣，Mc,Mt分別為追蹤星和目標(biāo)所受的外力矩。

假設(shè)目標(biāo)做自由翻滾運(yùn)動(dòng)，則目標(biāo)所受外力矩Mt=0。一方面使用式(10)估計(jì)相對(duì)旋轉(zhuǎn)角速度比較復(fù)雜;另一方面，假設(shè)追蹤星角速度是可以獲得的。因此，本文使用目標(biāo)星的姿態(tài)動(dòng)力學(xué)方程進(jìn)行估計(jì)。目標(biāo)的姿態(tài)動(dòng)力學(xué)方程為：

(11)

由于目標(biāo)的轉(zhuǎn)動(dòng)慣量矩陣未知，因此需要對(duì)轉(zhuǎn)動(dòng)慣量進(jìn)行參數(shù)化處理。采用類似于文獻(xiàn)[5]的參數(shù)化方式，定義轉(zhuǎn)動(dòng)慣量比：

(12)

式中：k1=-ln(Jxx/Jzz),k2=-ln(Jyy/Jzz)，代入式(11)得：

(13)

式中：

考慮到目標(biāo)星為剛體，短期內(nèi)質(zhì)量及其分布不發(fā)生變化，其轉(zhuǎn)動(dòng)慣量比為常量，應(yīng)滿足：

(14)

2 目標(biāo)星觀測(cè)方程

如圖1所示，在追蹤星上安裝有固定的單目相機(jī)，單目相機(jī)在追蹤星本體坐標(biāo)系下的位置坐標(biāo)為d=[dx,dy,dz]T。相機(jī)成像采用小孔成像模型，不考慮畸變，單目相機(jī)內(nèi)參矩陣為：

(15)

(16)

(17)

(18)

將式(17)代入式(18)即可得到觀測(cè)方程，其形式為：

(19)

3 基于單目視覺的相對(duì)狀態(tài)估計(jì)

在本文第1、第2節(jié)中分別描述了非合作目標(biāo)星與追蹤星的相對(duì)運(yùn)動(dòng)方程和狀態(tài)觀測(cè)方程，本節(jié)設(shè)計(jì)了通過(guò)EKF估計(jì)目標(biāo)相對(duì)姿態(tài)、角速度、轉(zhuǎn)動(dòng)慣量比、相對(duì)質(zhì)心位置、相對(duì)質(zhì)心速度及特征點(diǎn)位置的方法。

狀態(tài)選擇為：

(20)

(21)

根據(jù)式(3)、(9)、(13)、(14)和(21)可得系統(tǒng)狀態(tài)方程：

(22)

式(22)描述的是連續(xù)非線性系統(tǒng)，需要先轉(zhuǎn)換為離散模型，之后再按照標(biāo)準(zhǔn)的擴(kuò)展卡爾曼濾波公式進(jìn)行估計(jì)。

推導(dǎo)離散模型為：

X(k+1)=X(k)+f(X(k))Δt+

(23)

由矩陣Ak可以得到系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣的一階近似φ(k)=I15+3N+AkΔt, Δt為濾波周期。過(guò)程噪聲矩陣近似為：

(24)

測(cè)量模型的雅克比矩陣可以通過(guò)對(duì)觀測(cè)方程式(19)求導(dǎo)得到，

(25)

該雅克比矩陣為2N×(15+3N)維，具體形式參見文獻(xiàn)[13]。

使用擴(kuò)展卡爾曼濾波對(duì)式(23)和式(19)所表示的過(guò)程模型和測(cè)量模型進(jìn)行狀態(tài)估計(jì)。注意由于直接用了四元數(shù)進(jìn)行姿態(tài)估計(jì)，因此姿態(tài)四元數(shù)每一步預(yù)測(cè)和更新后都需要單位化處理。

具體算法流程如圖2所示。詳細(xì)的相對(duì)狀態(tài)估計(jì)流程為：

步驟3.獲得觀測(cè)Zk后根據(jù)觀測(cè)模型和狀態(tài)預(yù)測(cè)值及預(yù)測(cè)誤差方差陣計(jì)算觀測(cè)雅克比矩陣Hk、增益矩陣Kk和預(yù)測(cè)殘差rk。

4 仿真校驗(yàn)

本節(jié)通過(guò)非合作目標(biāo)星與追蹤星相對(duì)運(yùn)動(dòng)的動(dòng)力學(xué)模型模擬兩航天器的相對(duì)運(yùn)動(dòng)軌跡。以目標(biāo)星表面的固有特征為觀測(cè)特征點(diǎn)，根據(jù)特征點(diǎn)的運(yùn)動(dòng)和小孔成像模型產(chǎn)生特征點(diǎn)的圖像投影像素坐標(biāo)，并加入測(cè)量噪聲，作為系統(tǒng)觀測(cè)輸入。

以失效的近地軌道地球觀測(cè)衛(wèi)星Envisat為目標(biāo)星，其軌道半長(zhǎng)軸a=7143 km，真近點(diǎn)角速率約為n=0.001 rad/s。在本文仿真中，為便于選擇特征點(diǎn)，將Envisat衛(wèi)星主體部分簡(jiǎn)化為一個(gè)12 m×4 m×4 m的長(zhǎng)方體，其余部分如太陽(yáng)能電池板和雷達(dá)暫不考慮。目標(biāo)主慣量矩陣設(shè)為Jt=diag(16960,120570,124970)。選擇其幾何頂點(diǎn)作為觀測(cè)特征點(diǎn)，特征點(diǎn)在非合作目標(biāo)上的位置分布如表1所示。

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

仿真時(shí)間設(shè)計(jì)為200 s，仿真步長(zhǎng)為0.1 s，仿真初始條件設(shè)置如下：

初始協(xié)方差:

相對(duì)位置估計(jì)誤差為：

(26)

相對(duì)速度估計(jì)誤差為：

(27)

相對(duì)角速度估計(jì)誤差為：

(28)

k1,k2估計(jì)誤差為：

(29)

eθ=2arccos(qe4)

(30)

式中：qe4為姿態(tài)誤差四元數(shù)的標(biāo)量部分。

目標(biāo)上特征點(diǎn)的位置估計(jì)誤差為：

(31)

(32)

仿真結(jié)果如圖3～圖9所示。

圖3、圖4分別為目標(biāo)相對(duì)姿態(tài)和角速度估計(jì)值與仿真真值之間的誤差曲線。圖3中，將姿態(tài)誤差四元數(shù)轉(zhuǎn)換成軸角后估計(jì)誤差在2°以內(nèi)。圖4中，目標(biāo)角速度估計(jì)誤差在0.3°/s以內(nèi)。圖5、圖6分別為目標(biāo)質(zhì)心相對(duì)位置和相對(duì)速度的誤差變化曲線。相對(duì)位置估計(jì)誤差70 s后收斂在0.1 m以內(nèi),120 s后收斂在0.05 m附近,相對(duì)速度估計(jì)誤差30 s 后收斂在0.0005 m/s以內(nèi)。結(jié)果表明了本方法對(duì)目標(biāo)星相對(duì)位姿估計(jì)的有效性。

圖7、圖8分別為目標(biāo)轉(zhuǎn)動(dòng)慣量比k1，k2的誤差曲線，最終穩(wěn)定時(shí)k1誤差上界為0.005，k2誤差上界為0.001，這說(shuō)明本方法可以很好地估計(jì)出目標(biāo)星的轉(zhuǎn)動(dòng)慣量比。

圖9為所有特征點(diǎn)在目標(biāo)本體系上的位置估計(jì)誤差曲線，100 s后誤差曲線穩(wěn)定在0.25 m左右，特征點(diǎn)平均估計(jì)誤差小于0.04 m，說(shuō)明了本方法可以很好地估計(jì)目標(biāo)表面特征點(diǎn)位置。

綜上可知，本文設(shè)計(jì)的基于單目視覺的目標(biāo)星狀態(tài)估計(jì)方法能夠有效地實(shí)現(xiàn)對(duì)未知非合作目標(biāo)星相對(duì)狀態(tài)估計(jì)，估計(jì)方法收斂時(shí)間較短，估計(jì)精度較高，滿足近距離相對(duì)導(dǎo)航任務(wù)的要求。

5 結(jié) 論

針對(duì)空間未知非合作目標(biāo)星的相對(duì)狀態(tài)估計(jì)問(wèn)題，本文以微小衛(wèi)星搭載單目相機(jī)，設(shè)計(jì)了一種基于單目視覺的目標(biāo)星狀態(tài)估計(jì)方法，以目標(biāo)星的固有特征成像為輸入，設(shè)計(jì)了一種基于EKF的目標(biāo)星相對(duì)姿態(tài)、角速度、兩自由度轉(zhuǎn)動(dòng)慣量比、相對(duì)質(zhì)心位置、相對(duì)線速度和特征點(diǎn)位置的估計(jì)方法，并通過(guò)數(shù)值仿真校驗(yàn)了該方法的有效性。但是，值得注意的是，目標(biāo)星固有特征的提取和跟蹤是本文的狀態(tài)估計(jì)方法的前提，空間光照條件的變化會(huì)影響相機(jī)成像精度，從而使得觀測(cè)噪聲統(tǒng)計(jì)特性發(fā)生變化，因此有必要在下一步工作中研究適合不同統(tǒng)計(jì)特性噪聲的自適應(yīng)濾波算法，進(jìn)一步提高狀態(tài)估計(jì)精度。