郭新程，孟中杰，黃攀峰

(1. 西北工業(yè)大學航天學院智能機器人研究中心，西安 710072；2. 西北工業(yè)大學航天飛行動力學技術(shù)國家級重點實驗室，西安 710072)

0 引 言

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

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

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

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

1 追蹤星/目標星相對運動模型

1.1 坐標系定義

為建立追蹤星與目標星之間的相對運動模型，首先定義以下坐標系。

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

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

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

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

相機坐標系C：坐標原點固定在相機光心上，z軸沿相機光軸方向，x軸y軸平行于成像平面。

為簡化模型，假設(shè)LVLH坐標系和追蹤星本體坐標系重合，相機坐標系和追蹤星本體坐標系各坐標軸平行。

1.2 相對位置運動模型

(1)

(2)

(3)

(4)

1.3 相對姿態(tài)運動模型

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

(5)

(6)

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

(7)

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

(8)

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

(9)

相對旋轉(zhuǎn)的動力學方程為：

(10)

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

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

(11)

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

(12)

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

(13)

式中：

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

(14)

2 目標星觀測方程

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

(15)

(16)

(17)

(18)

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

(19)

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

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

狀態(tài)選擇為：

(20)

(21)

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

(22)

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

推導離散模型為：

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

(23)

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

(24)

測量模型的雅克比矩陣可以通過對觀測方程式(19)求導得到，

(25)

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

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

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

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

4 仿真校驗

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

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

表1 特征點的位置分布Table 1 The distribution of feature points’ position

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

初始協(xié)方差:

相對位置估計誤差為：

(26)

相對速度估計誤差為：

(27)

相對角速度估計誤差為：

(28)

k1,k2估計誤差為：

(29)

eθ=2arccos(qe4)

(30)

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

目標上特征點的位置估計誤差為：

(31)

(32)

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

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

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

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

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

5 結(jié) 論

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