基于序列圖像的航天器自主導航降維濾波方法

2021-07-05 13:46:44孫博文王大軼王炯琦周海銀葛東明董天舒

航空學報 2021年4期

關鍵詞：特征

孫博文，王大軼，王炯琦，周海銀，葛東明，董天舒

1. 國防科技大學文理學院，長沙 410073 2.中國空間技術研究院北京空間飛行器總體設計部，北京 100094

空間飛行器在軌服務與維護是航天技術領域的重要發展方向，是延長衛星、平臺、空間站等空間飛行器壽命和能力，減少衛星運行成本的重要保證，對我國航天領域的發展具有重要意義[1-4]。

在軌服務對象大多屬于“非合作目標”，比如，① 未安裝輔助測量標識的空間飛行器，② 無法向外傳輸自身信息的空間飛行器，③ 由于故障等原因失效處于自由運動狀態的空間飛行器，④ 空間碎片等等[5-8]。空間機器人利用機械臂抓捕空間非合作目標時，為減小操作過程中與非合作發生碰撞的風險，精準快速地估計非合作目標的運動狀態是前提和基礎，即空間機器人和空間非合作目標之間相對運動狀態的估計[9-12]。

通常情況下，執行在軌服務與維護的衛星通常攜帶雙目相機對非合作目標進行立體視覺觀測，依據圖像信息，選取非合作目標若干特征點，從而構建特征點觀測坐標系。然而，對于空間自由運動的物體，其姿態運動學模型均在非合作目標本體坐標系下構建，但通過雙目相機的觀測值無法直接解算出非合作目標本體坐標系，一般采用擴展卡爾曼濾波算法對非合作目標姿態進行高精確估計。國內外學者為此做了大量研究工作[13-17]。

Segal等[18]在狀態變量中估計一組特征點在非合作目標本體坐標系下的位置，通過跟蹤目標特征點的方式，并利用迭代擴展卡爾曼濾波觀測模型來估計相對位置，姿態，旋轉和平移速度。在實驗中利用最大后驗識別方案確定最可能的慣性張量，無需在濾波中估計慣性比。考慮到基于光照對視覺的影響，Peng等[19]提出了一種基于激光雷達和雙目相機的非合作目標姿態估計方法，該方法能夠適應惡劣的光照條件，且具有較好的魯棒性和較高的精度。但是星上資源有限，無法進行高負荷運算同時也無法攜帶過多測量設備。

Aghili和Parsa[20]提出了一種自適應噪聲的卡爾曼濾波方法，狀態變量包括相對位置速度，姿態四元數，非合作目標相對慣性系的角速度，自身慣量比，非合作目標本體坐標系與特征點觀測坐標系的位置與姿態關系等。為了避免濾波發散，將四元數估計變為對四元數誤差的估計。Zhang等[21]針對翻滾航天器提出了一種相對位置和姿態的估計方法，采用了一般偏心軌道相對方程描述相對位置動力學，并將狀態變量擴展為主副航天器相對于軌道系的四元數，得到了較為精確的結果。

為了不損失測量新息，Ge等[22]在狀態變量中加入非合作目標特征點在非合作目標本體坐標系下的位置矢量進行估計，測量值加入非合作目標特征點在相機系下的位置矢量，并進行仿真實驗，表明該方法具有較高的實際應用價值。然而，以上方法都存在待估狀態變量維數高，從而影響狀態濾波收斂速度的問題，進而影響空間飛行器的在軌服務與維護。

針對這一問題，本文提出了一種基于序列圖像的非合作目標相對導航方法，該方法由兩個濾波算法組成，首先不需要考慮非合作目標的位置變量，僅對非合作目標的姿態進行精確估計，然后在非合作目標姿態估計的基礎上對非合作目標的相對位置與速度進行估計。本文在非合作目標自由運動滿足的姿態運動學模型基礎上，依據雙目相機測量原理，分別分析了非合作目標姿態、相對位置速度與測量的關系，構建了基于序列圖像的測量模型，分別建立了不含有非合作目標質心位置的狀態方程和基于非合作目標位置、速度矢量的狀態方程，并利用擴展卡爾曼濾波對非合作目標的狀態變量進行估計。數學仿真實驗表明，本文提出的方法可以在50次采樣之內收斂，同時可以得到高精度的姿態估計結果。

1 雙目相機測量原理

觀測衛星攜帶雙目相機對目標進行觀測。如圖1所示，兩相機平行放置，構成平行式立體視覺模型。假設相機系Oc-xcyczc原點位于左相機光心處，yc軸指向相平面中心，xc軸平行相平面指向右相機，zc軸構成右手坐標系。兩相機內部參數一致，焦距為f，基線長度為b。當觀測特征點P時，假設特征點P在左右兩相機相平面成像坐標分別為(ul,vl)和(ur,vr)，由三角形幾何關系可以得到

圖1 雙目相機測量示意圖Fig.1 Schematic diagram of binocular stereo vision

(1)

則在相機系下特征點P的坐標向量為[22]

在觀測衛星利用雙目相機對非合作目標進行觀測時，可同時觀測多個特征點。由于非合作目標為空間中小天體、非合作衛星或天空垃圾等等，可以看作剛性物體，故各個特征點在非合作目標本體坐標系的相對位置矢量不變，所以由特征點構造的坐標系相對于非合作目標本體坐標系的姿態不隨非合作目標運動而改變。由兩組不同特征點構造的坐標系之間的姿態也不隨非合作目標的運動而改變，所以無妨假設雙目相機可以一直觀測某幾個特征點(無法觀測時也可根據特征點構造坐標系之間的相互轉化實現)。下面介紹根據觀測的特征點構造的特征點觀測坐標系Om-xmymzm。

依據以上假設，選取非合作目標上的3個非共線的特征點P1,P2,P3，建立特征點觀測坐標系Om-xmymzm。定義特征點觀測坐標系原點位于P1點，xm軸方向為矢量P1P2方向，單位矢量為

(2)

zm軸方向為矢量P1P2和矢量P1P3所在平面的法線方向，故其單位矢量為

(3)

建立右手坐標系，則ym軸方向單位矢量為

ry=rz×rx

(4)

從而，可以得到相機系到特征點觀測坐標系的姿態轉換矩陣為

(5)

然而，對于翻滾狀態的非合作目標，其姿態與非合作目標本體坐標系密切相關。如圖1所示，坐標系OT-xTyTzT為非合作目標本體坐標系，通過雙目相機觀測非合作目標特征點并不能直接解算出非合作目標本體坐標系，但是由于雙目相機觀測的特征點在非合作目標本體坐標系的位置矢量為常量，所以非合作目標本體坐標系與特征點觀測坐標系之間的姿態關系固定不變。

為了更好描述非合作目標的運動姿態(即非合作目標本體坐標系的姿態)，同時避免奇異性，本文采用四元數對姿態進行表述。故可以假設特征點觀測坐標系相對于非合作目標本體坐標系的四元數為η，即η為常量。

2 基于立體視覺測量的目標姿態估計

2.1 四元數基礎

定義非合作目標本體坐標系相對于慣性坐標系的四元數為

(6)

式中：該四元數表示為慣性坐標系繞單位矢量軸e旋轉φ角度到非合作目標本體坐標系；qv、q0分別表示四元數的矢量和標量部分，記qv=vec(q)，易知四元數的模長為1。則用四元數表示慣性坐標系(用字母i表示)到非合作目標本體坐標系的旋轉矩陣為

(7)

式中：S(qv)的表達式為

(8)

(9)

則有

q?q-1=q-1?q=[0,0,0,1]T

(10)

2.2 姿態運動學方程

假設非合作目標本體系主慣量分別為Ixx,Iyy,Izz，則定義慣量比l=[lx,ly,lz]T為

(11)

對于非合作目標，其主慣量與慣量比均未知。假設非合作目標本體坐標系相對于慣性系的旋轉角速度在非合作目標本體坐標系下的表達式為ω=[ωx,ωy,ωz]T，則非合作目標做自由翻滾運動時的動力學方程為

(12)

非合作目標本體系相對于慣性系的四元數運動學方程為[23]

(13)

(14)

(15)

(16)

(17)

(18)

2.3 測量方程

當相機對非合作目標進行觀測得到如式(5)所示的姿態旋轉矩陣時，可得到特征點觀測坐標系相對于相機系的四元數qr為[12]

(19)

μ=qr?qc

(20)

假設特征點觀測坐標系相對于非合作目標本體坐標系的四元數為η，非合作目標本體坐標系相對于慣性系的四元數為q，則

μ=η?q

(21)

根據文獻[19]可知，針對四元數μ的矢量部分有

δμv=vec(δη?δq)

(22)

則分別對δqv,δηv求偏導可得[19]

(23)

2.4 濾波過程

定義擴展卡爾曼濾波的狀態變量為

(24)

根據上述推導得到如下離散方程:

(25)

式中：δx為狀態估計誤差變量；εk、vk分別為過程噪聲和測量噪聲，分別滿足均值為零，方差為Q和R的正態分布；測量值z為四元數δμ的矢量部分，即z=vec(δμ)；Φk、Hk分別為系統的狀態轉移矩陣和測量矩陣，表達式為

Φk=I12+AkT

(26)

式中：

(27)

(28)

下面給出濾波步驟：

步驟1利用四階龍格庫塔法根據式(13)和式(12)進行狀態變量qv和ω的一步預測，狀態變量l,η的一步預測取上一步的濾波值，即lk|k-1=lk-1，ηk|k-1=ηk-1。

(29)

步驟3狀態變量協方差一步預測

(30)

步驟4計算增益矩陣

(31)

步驟5狀態變量更新

(32)

(33)

(34)

(35)

(36)

(37)

(38)

步驟6狀態協變量方差更新

Pk=(I12-KkHk)Pk|k-1

(39)

3 基于姿態的非合作目標位置估計

3.1 狀態方程

圖2為非合作目標相對位置示意圖。坐標系Oc-xcyczc為觀測衛星軌道坐標系，原點位于航天器質心，xc軸為觀測衛星的向徑方向并指向軌道外，zc軸為觀測衛星軌道面正法線方向，yc軸構成右手坐標系。

圖2 非合作目標相對位置Fig.2 Relative position of non-cooperative target

當觀測衛星與非合作目標之間的距離遠小于觀測衛星軌道半徑rs時，短期內可以利用C-W方程描述兩者之間相對位置關系，在觀測衛星軌道坐標系下表示為

(40)

(41)

設置采樣時間間隔為T，則離散形式的C-W方程為

(42)

其中：

(43)

(44)

3.2 測量方程

簡便起見，假設觀測衛星軌道坐標系與相機系之間的旋轉矩陣為單位矩陣，即兩坐標系重合。

如圖2所示，非合作目標質心在觀測衛星軌道坐標系下的位置矢量為r，第i個觀測點Pi在相機系下的位置矢量為ri，在非合作目標本體系下的位置矢量為ρi，假設觀測衛星軌道坐標系相對于慣性系的四元數為qs，則非合作目標本體坐標系相對于觀測衛星軌道坐標系的四元數qr為

(45)

則觀測模型為

ri=r+R(qr)ρi

(46)

(47)

3.3 濾波過程

步驟2狀態變量協方差一步預測

(48)

步驟3計算增益矩陣

(49)

步驟4狀態變量更新

(50)

步驟5狀態變量協方差更新

(51)

4 仿真驗證

根據上述推導過程進行仿真驗證。仿真數據如下設置：假設非合作目標主慣量分別為Ixx=10 300 kg·m2，Iyy=5 390 kg·m2，Izz=9 190 kg·m2，計算得到慣量比分別為lx=-0.368 9，ly=-0.205 9，lz=0.534 3。目標質心初始相對位置為[1,0.1,0.1]Tm，初始相對速度為[0.005,-0.001, -0.005]Tm/s，初始姿態四元數為0,0,0,1]T，初始角速度為[2,2,2]Trad/s，3個特征點在非合作目標體坐標系下坐標分別為[0.2,0,0]Tm，[0,0.2,0]Tm，[0,0,0.2]Tm，采樣頻率為10 Hz。