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

1. 國(guó)防科技大學(xué) 文理學(xué)院，長(zhǎng)沙 410073 2.中國(guó)空間技術(shù)研究院 北京空間飛行器總體設(shè)計(jì)部， 北京 100094

空間飛行器在軌服務(wù)與維護(hù)是航天技術(shù)領(lǐng)域的重要發(fā)展方向，是延長(zhǎng)衛(wèi)星、平臺(tái)、空間站等空間飛行器壽命和能力，減少衛(wèi)星運(yùn)行成本的重要保證，對(duì)我國(guó)航天領(lǐng)域的發(fā)展具有重要意義[1-4]。

在軌服務(wù)對(duì)象大多屬于“非合作目標(biāo)”，比如，① 未安裝輔助測(cè)量標(biāo)識(shí)的空間飛行器，② 無(wú)法向外傳輸自身信息的空間飛行器，③ 由于故障等原因失效處于自由運(yùn)動(dòng)狀態(tài)的空間飛行器，④ 空間碎片等等[5-8]。空間機(jī)器人利用機(jī)械臂抓捕空間非合作目標(biāo)時(shí)，為減小操作過程中與非合作發(fā)生碰撞的風(fēng)險(xiǎn)，精準(zhǔn)快速地估計(jì)非合作目標(biāo)的運(yùn)動(dòng)狀態(tài)是前提和基礎(chǔ)，即空間機(jī)器人和空間非合作目標(biāo)之間相對(duì)運(yùn)動(dòng)狀態(tài)的估計(jì)[9-12]。

通常情況下，執(zhí)行在軌服務(wù)與維護(hù)的衛(wèi)星通常攜帶雙目相機(jī)對(duì)非合作目標(biāo)進(jìn)行立體視覺觀測(cè)，依據(jù)圖像信息，選取非合作目標(biāo)若干特征點(diǎn)，從而構(gòu)建特征點(diǎn)觀測(cè)坐標(biāo)系。然而，對(duì)于空間自由運(yùn)動(dòng)的物體，其姿態(tài)運(yùn)動(dòng)學(xué)模型均在非合作目標(biāo)本體坐標(biāo)系下構(gòu)建，但通過雙目相機(jī)的觀測(cè)值無(wú)法直接解算出非合作目標(biāo)本體坐標(biāo)系，一般采用擴(kuò)展卡爾曼濾波算法對(duì)非合作目標(biāo)姿態(tài)進(jìn)行高精確估計(jì)。國(guó)內(nèi)外學(xué)者為此做了大量研究工作[13-17]。

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

Aghili和Parsa[20]提出了一種自適應(yīng)噪聲的卡爾曼濾波方法，狀態(tài)變量包括相對(duì)位置速度，姿態(tài)四元數(shù)，非合作目標(biāo)相對(duì)慣性系的角速度，自身慣量比，非合作目標(biāo)本體坐標(biāo)系與特征點(diǎn)觀測(cè)坐標(biāo)系的位置與姿態(tài)關(guān)系等。為了避免濾波發(fā)散，將四元數(shù)估計(jì)變?yōu)閷?duì)四元數(shù)誤差的估計(jì)。Zhang等[21]針對(duì)翻滾航天器提出了一種相對(duì)位置和姿態(tài)的估計(jì)方法，采用了一般偏心軌道相對(duì)方程描述相對(duì)位置動(dòng)力學(xué)，并將狀態(tài)變量擴(kuò)展為主副航天器相對(duì)于軌道系的四元數(shù)，得到了較為精確的結(jié)果。

為了不損失測(cè)量新息，Ge等[22]在狀態(tài)變量中加入非合作目標(biāo)特征點(diǎn)在非合作目標(biāo)本體坐標(biāo)系下的位置矢量進(jìn)行估計(jì)，測(cè)量值加入非合作目標(biāo)特征點(diǎn)在相機(jī)系下的位置矢量，并進(jìn)行仿真實(shí)驗(yàn)，表明該方法具有較高的實(shí)際應(yīng)用價(jià)值。然而，以上方法都存在待估狀態(tài)變量維數(shù)高，從而影響狀態(tài)濾波收斂速度的問題，進(jìn)而影響空間飛行器的在軌服務(wù)與維護(hù)。

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

1 雙目相機(jī)測(cè)量原理

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

圖1 雙目相機(jī)測(cè)量示意圖Fig.1 Schematic diagram of binocular stereo vision

(1)

則在相機(jī)系下特征點(diǎn)P的坐標(biāo)向量為[22]

在觀測(cè)衛(wèi)星利用雙目相機(jī)對(duì)非合作目標(biāo)進(jìn)行觀測(cè)時(shí)，可同時(shí)觀測(cè)多個(gè)特征點(diǎn)。由于非合作目標(biāo)為空間中小天體、非合作衛(wèi)星或天空垃圾等等，可以看作剛性物體，故各個(gè)特征點(diǎn)在非合作目標(biāo)本體坐標(biāo)系的相對(duì)位置矢量不變，所以由特征點(diǎn)構(gòu)造的坐標(biāo)系相對(duì)于非合作目標(biāo)本體坐標(biāo)系的姿態(tài)不隨非合作目標(biāo)運(yùn)動(dòng)而改變。由兩組不同特征點(diǎn)構(gòu)造的坐標(biāo)系之間的姿態(tài)也不隨非合作目標(biāo)的運(yùn)動(dòng)而改變，所以無(wú)妨假設(shè)雙目相機(jī)可以一直觀測(cè)某幾個(gè)特征點(diǎn)(無(wú)法觀測(cè)時(shí)也可根據(jù)特征點(diǎn)構(gòu)造坐標(biāo)系之間的相互轉(zhuǎn)化實(shí)現(xiàn))。下面介紹根據(jù)觀測(cè)的特征點(diǎn)構(gòu)造的特征點(diǎn)觀測(cè)坐標(biāo)系Om-xmymzm。

依據(jù)以上假設(shè)，選取非合作目標(biāo)上的3個(gè)非共線的特征點(diǎn)P1,P2,P3，建立特征點(diǎn)觀測(cè)坐標(biāo)系Om-xmymzm。定義特征點(diǎn)觀測(cè)坐標(biāo)系原點(diǎn)位于P1點(diǎn)，xm軸方向?yàn)槭噶縋1P2方向，單位矢量為

(2)

zm軸方向?yàn)槭噶縋1P2和矢量P1P3所在平面的法線方向，故其單位矢量為

(3)

建立右手坐標(biāo)系，則ym軸方向單位矢量為

ry=rz×rx

(4)

從而，可以得到相機(jī)系到特征點(diǎn)觀測(cè)坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣為

(5)

然而，對(duì)于翻滾狀態(tài)的非合作目標(biāo)，其姿態(tài)與非合作目標(biāo)本體坐標(biāo)系密切相關(guān)。如圖1所示，坐標(biāo)系OT-xTyTzT為非合作目標(biāo)本體坐標(biāo)系，通過雙目相機(jī)觀測(cè)非合作目標(biāo)特征點(diǎn)并不能直接解算出非合作目標(biāo)本體坐標(biāo)系，但是由于雙目相機(jī)觀測(cè)的特征點(diǎn)在非合作目標(biāo)本體坐標(biāo)系的位置矢量為常量，所以非合作目標(biāo)本體坐標(biāo)系與特征點(diǎn)觀測(cè)坐標(biāo)系之間的姿態(tài)關(guān)系固定不變。

為了更好描述非合作目標(biāo)的運(yùn)動(dòng)姿態(tài)(即非合作目標(biāo)本體坐標(biāo)系的姿態(tài))，同時(shí)避免奇異性，本文采用四元數(shù)對(duì)姿態(tài)進(jìn)行表述。故可以假設(shè)特征點(diǎn)觀測(cè)坐標(biāo)系相對(duì)于非合作目標(biāo)本體坐標(biāo)系的四元數(shù)為η，即η為常量。

2 基于立體視覺測(cè)量的目標(biāo)姿態(tài)估計(jì)

2.1 四元數(shù)基礎(chǔ)

定義非合作目標(biāo)本體坐標(biāo)系相對(duì)于慣性坐標(biāo)系的四元數(shù)為

(6)

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

(7)

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

(8)

(9)

則有

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

(10)

2.2 姿態(tài)運(yùn)動(dòng)學(xué)方程

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

(11)

對(duì)于非合作目標(biāo)，其主慣量與慣量比均未知。假設(shè)非合作目標(biāo)本體坐標(biāo)系相對(duì)于慣性系的旋轉(zhuǎn)角速度在非合作目標(biāo)本體坐標(biāo)系下的表達(dá)式為ω=[ωx,ωy,ωz]T，則非合作目標(biāo)做自由翻滾運(yùn)動(dòng)時(shí)的動(dòng)力學(xué)方程為

(12)

非合作目標(biāo)本體系相對(duì)于慣性系的四元數(shù)運(yùn)動(dòng)學(xué)方程為[23]

(13)

(14)

(15)

(16)

(17)

(18)

2.3 測(cè)量方程

當(dāng)相機(jī)對(duì)非合作目標(biāo)進(jìn)行觀測(cè)得到如式(5)所示的姿態(tài)旋轉(zhuǎn)矩陣時(shí)，可得到特征點(diǎn)觀測(cè)坐標(biāo)系相對(duì)于相機(jī)系的四元數(shù)qr為[12]

(19)

μ=qr?qc

(20)

假設(shè)特征點(diǎn)觀測(cè)坐標(biāo)系相對(duì)于非合作目標(biāo)本體坐標(biāo)系的四元數(shù)為η，非合作目標(biāo)本體坐標(biāo)系相對(duì)于慣性系的四元數(shù)為q，則

μ=η?q

(21)

根據(jù)文獻(xiàn)[19]可知，針對(duì)四元數(shù)μ的矢量部分有

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

(22)

則分別對(duì)δqv,δηv求偏導(dǎo)可得[19]

(23)

2.4 濾波過程

定義擴(kuò)展卡爾曼濾波的狀態(tài)變量為

(24)

根據(jù)上述推導(dǎo)得到如下離散方程:

(25)

式中：δx為狀態(tài)估計(jì)誤差變量；εk、vk分別為過程噪聲和測(cè)量噪聲，分別滿足均值為零，方差為Q和R的正態(tài)分布；測(cè)量值z(mì)為四元數(shù)δμ的矢量部分，即z=vec(δμ)；Φk、Hk分別為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣和測(cè)量矩陣，表達(dá)式為

Φk=I12+AkT

(26)

式中：

(27)

(28)

下面給出濾波步驟：

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

(29)

步驟3狀態(tài)變量協(xié)方差一步預(yù)測(cè)

(30)

步驟4計(jì)算增益矩陣

(31)

步驟5狀態(tài)變量更新

(32)

(33)

(34)

(35)

(36)

(37)

(38)

步驟6狀態(tài)協(xié)變量方差更新

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

(39)

3 基于姿態(tài)的非合作目標(biāo)位置估計(jì)

3.1 狀態(tài)方程

圖2為非合作目標(biāo)相對(duì)位置示意圖。坐標(biāo)系Oc-xcyczc為觀測(cè)衛(wèi)星軌道坐標(biāo)系，原點(diǎn)位于航天器質(zhì)心，xc軸為觀測(cè)衛(wèi)星的向徑方向并指向軌道外，zc軸為觀測(cè)衛(wèi)星軌道面正法線方向，yc軸構(gòu)成右手坐標(biāo)系。

圖2 非合作目標(biāo)相對(duì)位置Fig.2 Relative position of non-cooperative target

當(dāng)觀測(cè)衛(wèi)星與非合作目標(biāo)之間的距離遠(yuǎn)小于觀測(cè)衛(wèi)星軌道半徑rs時(shí)，短期內(nèi)可以利用C-W方程描述兩者之間相對(duì)位置關(guān)系，在觀測(cè)衛(wèi)星軌道坐標(biāo)系下表示為

(40)

(41)

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

(42)

其中：

(43)

(44)

3.2 測(cè)量方程

簡(jiǎn)便起見，假設(shè)觀測(cè)衛(wèi)星軌道坐標(biāo)系與相機(jī)系之間的旋轉(zhuǎn)矩陣為單位矩陣，即兩坐標(biāo)系重合。

如圖2所示，非合作目標(biāo)質(zhì)心在觀測(cè)衛(wèi)星軌道坐標(biāo)系下的位置矢量為r，第i個(gè)觀測(cè)點(diǎn)Pi在相機(jī)系下的位置矢量為ri，在非合作目標(biāo)本體系下的位置矢量為ρi，假設(shè)觀測(cè)衛(wèi)星軌道坐標(biāo)系相對(duì)于慣性系的四元數(shù)為qs，則非合作目標(biāo)本體坐標(biāo)系相對(duì)于觀測(cè)衛(wèi)星軌道坐標(biāo)系的四元數(shù)qr為

(45)

則觀測(cè)模型為

ri=r+R(qr)ρi

(46)

(47)

3.3 濾波過程

步驟2狀態(tài)變量協(xié)方差一步預(yù)測(cè)

(48)

步驟3計(jì)算增益矩陣

(49)

步驟4狀態(tài)變量更新

(50)

步驟5狀態(tài)變量協(xié)方差更新

(51)

4 仿真驗(yàn)證

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

圖3 相對(duì)位置矢量估計(jì)值與真實(shí)值Fig.3 Estimation and true values of relative position vector

圖4 相對(duì)速度矢量估計(jì)值與真實(shí)值Fig.4 Estimation and true values of relative velocity vector

表1 相對(duì)位置矢量絕對(duì)值偏差

表2 相對(duì)速度矢量絕對(duì)值偏差

圖5 四元數(shù)qv估計(jì)值與真實(shí)值Fig.5 Estimation and truth value of quaternion qv

圖6 角速度估計(jì)值與真實(shí)值Fig.6 Estimation and truth value of angular velocity

表3 四元數(shù)qv絕對(duì)值偏差Table 3 Deviation of absolute value of quaternion qv

表4 角速度絕對(duì)值偏差Table 4 Deviation of absolute value of angular velocity 單位：rad/s

圖7 慣量比估計(jì)值與真值Fig.7 Estimation and true value of inertia ratio

表5 慣量比絕對(duì)值偏差Table 5 Deviation of absolute value of inertia ratio

由實(shí)驗(yàn)可以看出，在上述實(shí)驗(yàn)條件下，本文提出的方法與傳統(tǒng)方法相比可以在更短時(shí)間內(nèi)收斂到真值，并在真值附近進(jìn)行波動(dòng)。實(shí)驗(yàn)統(tǒng)計(jì)了偏差絕對(duì)值的均值，結(jié)果表明本文所提方法與傳統(tǒng)方法估計(jì)相比精度略高，在5個(gè)變量中的估計(jì)精度一致較好。

5 結(jié) 論

通過理論推導(dǎo)與仿真實(shí)驗(yàn)可得結(jié)論如下：

1) 該方法可以在50次采樣之內(nèi)收斂，收斂速度優(yōu)于傳統(tǒng)方法。

2) 本文所提出的改進(jìn)方法相比于傳統(tǒng)方法，在狀態(tài)變量估計(jì)精度中一致較好。

3) 該方法可用于非合作目標(biāo)的相對(duì)導(dǎo)航，有利于空間飛行器在軌服務(wù)與維護(hù)。