多通道魯棒故障衛(wèi)星參數(shù)估計(jì)算法

侯翔昊, 袁建平

(西北工業(yè)大學(xué) 航天學(xué)院, 陜西 西安 710072)

自主交會(huì)對(duì)接技術(shù)是當(dāng)前在軌服務(wù)任務(wù)中的關(guān)鍵技術(shù)之一[1-2]。尤其是針對(duì)故障衛(wèi)星的在軌服務(wù)技術(shù)不僅能夠再次利用這些空間資源，也可以減少空間碎片，維護(hù)太空環(huán)境。這類(lèi)故障衛(wèi)星由于燃料耗盡或者部件失效導(dǎo)致其成為空間中的一種非合作目標(biāo)，并且不受控制地在空間中做自由翻滾運(yùn)動(dòng)。因此，為了對(duì)這類(lèi)目標(biāo)進(jìn)行空間操作，必須對(duì)其參數(shù)進(jìn)行預(yù)先識(shí)別。對(duì)故障衛(wèi)星參數(shù)準(zhǔn)確識(shí)別對(duì)進(jìn)一步的在軌服務(wù)操作有著重大意義。由于故障衛(wèi)星是一種空間非合作目標(biāo)，其自身并不能提供充分的先驗(yàn)信息，因此目前只能夠采用非接觸式的測(cè)量設(shè)備(單目相機(jī)、雙目相機(jī)、LIDAR、激光測(cè)距儀等)對(duì)其相對(duì)姿態(tài)與位置進(jìn)行測(cè)量。然而，由于空間環(huán)境十分復(fù)雜，導(dǎo)致這類(lèi)基于視覺(jué)的敏感器測(cè)量精度普遍較低，并且測(cè)量信息有限。因此，如何基于這些測(cè)量信息對(duì)空間故障衛(wèi)星進(jìn)行準(zhǔn)確且魯棒的參數(shù)識(shí)別是目前研究的重點(diǎn)。

基于對(duì)目標(biāo)的測(cè)量，文獻(xiàn)[3]采用了基于圖像處理算法的技術(shù)對(duì)空間故障衛(wèi)星進(jìn)行了參數(shù)識(shí)別。然而，這類(lèi)方法局限在圖像處理的方法框架之中，并沒(méi)有考慮目標(biāo)在空間中的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型。因此，這類(lèi)方法計(jì)算量大，且估計(jì)能力十分有限，一般只能估計(jì)目標(biāo)衛(wèi)星的相對(duì)姿態(tài)參數(shù)。因此，在目標(biāo)衛(wèi)星參數(shù)識(shí)別任務(wù)中，這類(lèi)方法的理論價(jià)值遠(yuǎn)高于其實(shí)際價(jià)值。針對(duì)這類(lèi)方法的不足，一些研究者采用視覺(jué)測(cè)量結(jié)果，同時(shí)也利用了目標(biāo)衛(wèi)星的相對(duì)運(yùn)動(dòng)學(xué)方程來(lái)描述其運(yùn)動(dòng)特征，從而估計(jì)目標(biāo)衛(wèi)星的姿態(tài)參數(shù)。文獻(xiàn)[4]利用視覺(jué)敏感器的測(cè)量，設(shè)計(jì)了一種基于EKF的姿態(tài)估計(jì)算法。文獻(xiàn)[5]也采用了類(lèi)似的方法。然而，上述研究針對(duì)的目標(biāo)衛(wèi)星是合作的航天器。這樣，相比于對(duì)故障衛(wèi)星的測(cè)量，上述研究中的測(cè)量量都是準(zhǔn)確且不受空間環(huán)境干擾的，因此上述研究所采用的參數(shù)估計(jì)方法并不考慮算法的魯棒性與容錯(cuò)性。并且，由于上述研究并沒(méi)有使用動(dòng)力學(xué)方程，因此對(duì)目標(biāo)衛(wèi)星的動(dòng)力學(xué)參數(shù)無(wú)法做出估計(jì)。

文獻(xiàn)[6]首次采用了目標(biāo)的運(yùn)動(dòng)學(xué)方程以及動(dòng)力學(xué)方程來(lái)對(duì)目標(biāo)衛(wèi)星進(jìn)行建模，同時(shí)采用LIDAR視覺(jué)系統(tǒng)的測(cè)量值作為輸入對(duì)目標(biāo)衛(wèi)星進(jìn)行參數(shù)估計(jì)。文獻(xiàn)[7-8]采用了相同的測(cè)量手段對(duì)一個(gè)空間失效且自由翻滾的衛(wèi)星進(jìn)行了參數(shù)估計(jì)研究。通過(guò)使用目標(biāo)的運(yùn)動(dòng)學(xué)方程以及動(dòng)力學(xué)方程，文獻(xiàn)[7-8]設(shè)計(jì)了一種基于EKF框架的參數(shù)估計(jì)方法對(duì)故障目標(biāo)的位姿參數(shù)以及質(zhì)量參數(shù)做出了一體化的參數(shù)估計(jì)。文獻(xiàn)[9]采用了基于單目視覺(jué)相機(jī)以及LIDAR的測(cè)量作為輸入，同時(shí)使用了基于UKF的參數(shù)估計(jì)方法對(duì)空間故障衛(wèi)星的位姿參數(shù)進(jìn)行了估計(jì)。然而，上述的研究都采用了基于四元數(shù)的傳統(tǒng)運(yùn)動(dòng)學(xué)模型以及傳統(tǒng)動(dòng)力學(xué)模型。在這種情況下，姿軌耦合現(xiàn)象并沒(méi)有得到考慮。因此，上述方法只能夠用于輕度姿軌耦合的參數(shù)識(shí)別任務(wù)中。

為了解決上述由于采用傳統(tǒng)方法建模而產(chǎn)生的問(wèn)題，一些學(xué)者采用了對(duì)偶四元數(shù)來(lái)對(duì)目標(biāo)的運(yùn)動(dòng)學(xué)以及動(dòng)力學(xué)進(jìn)行建模。與傳統(tǒng)建模方法不同，對(duì)偶四元數(shù)將姿態(tài)參數(shù)與位置參數(shù)同時(shí)考慮，并將這兩者結(jié)合在一起。這樣，采用對(duì)偶四元數(shù)對(duì)目標(biāo)的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)方程進(jìn)行建模就能夠考慮姿軌耦合效應(yīng)，同時(shí)還使得目標(biāo)的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型表達(dá)更為簡(jiǎn)潔與緊湊。不僅如此，由于對(duì)偶四元數(shù)與四元數(shù)的數(shù)學(xué)表達(dá)相似性，對(duì)偶四元數(shù)在應(yīng)用中也更容易被接受。文獻(xiàn)[10]采用了基于對(duì)偶四元數(shù)的運(yùn)動(dòng)學(xué)方程對(duì)空間目標(biāo)的位姿參數(shù)進(jìn)行了估計(jì)。文獻(xiàn)[11]采用了基于誤差四元數(shù)的對(duì)偶四元數(shù)運(yùn)動(dòng)學(xué)模型，并設(shè)計(jì)了基于EKF框架的參數(shù)估計(jì)算法對(duì)空間目標(biāo)的參數(shù)進(jìn)行了一體化估計(jì)。針對(duì)空間中的復(fù)雜環(huán)境以及為了對(duì)空間非合作目標(biāo)作出全面的參數(shù)估計(jì)，文獻(xiàn)[12]采用了基于矢量對(duì)偶四元數(shù)的相對(duì)運(yùn)動(dòng)學(xué)與相對(duì)動(dòng)力學(xué)方程，對(duì)空間非合作目標(biāo)的位姿參數(shù)與質(zhì)量參數(shù)做出了自適應(yīng)估計(jì)，具有一定的魯棒性。然而，上述研究只針對(duì)單一測(cè)量敏感器設(shè)計(jì)了參數(shù)估計(jì)算法，當(dāng)復(fù)雜空間環(huán)境使測(cè)量敏感器較長(zhǎng)時(shí)間內(nèi)處于失效環(huán)境中或者某一測(cè)量敏感器損壞時(shí)，上述研究中的參數(shù)估計(jì)算法并不能得出有效的參數(shù)估計(jì)結(jié)果。

基于上述研究的優(yōu)點(diǎn)與不足，本文提出了一種針對(duì)空間中的失效衛(wèi)星的多通道魯棒參數(shù)估計(jì)方法(DVQ-REKF)。首先，本文采用新型的矢量對(duì)偶四元數(shù)對(duì)目標(biāo)的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)進(jìn)行建模。其次，本文所設(shè)計(jì)的服務(wù)航天器采用2個(gè)相互獨(dú)立的單目相機(jī)以及公用的激光測(cè)距儀來(lái)測(cè)量故障衛(wèi)星的相對(duì)姿態(tài)與距離信息。針對(duì)空間環(huán)境尤其是復(fù)雜的空間光照環(huán)境，射線(xiàn)環(huán)境可能對(duì)CCD相機(jī)造成的干擾甚至損毀，本文采用2路相互獨(dú)立的單目相機(jī)測(cè)量結(jié)果，當(dāng)其中一路測(cè)量信息失效時(shí)，另外一路測(cè)量信息仍然能夠提供可靠的測(cè)量信息。同時(shí)，考慮到激光在空間中的穩(wěn)定特性，本文只采用一路激光測(cè)距信息，從而有效降低服務(wù)航天器的能耗以及成本?；谏鲜瞿Ｐ团c測(cè)量信息，服務(wù)航天器首先進(jìn)行相互獨(dú)立的基于EKF框架的單通道目標(biāo)參數(shù)估計(jì)(DVQ-EKF)。之后，采用聯(lián)邦濾波器作為多通道信息融合算法，本文設(shè)計(jì)了多通道魯棒參數(shù)估計(jì)方法(DVQ-REKF)，從而對(duì)目標(biāo)進(jìn)行魯棒參數(shù)估計(jì)?；诒疚乃O(shè)計(jì)的多通道魯棒參數(shù)估計(jì)方法(DVQ-REKF)，故障衛(wèi)星的位姿參數(shù)以及質(zhì)量參數(shù)均能得到準(zhǔn)確且魯棒的估計(jì)。

1 四元數(shù)與矢量對(duì)偶四元數(shù)

1.1 四元數(shù)

四元數(shù)具有如下形式：

q=(q0,q1,q2,q3)=q0+q1i+q2j+q3k

(1)

式中,q0是標(biāo)量部分,q1i+q2j+q3k是矢量部分。其共軛表示為:(q0,-q1,-q2,-q3)。

同時(shí),四元數(shù)與實(shí)數(shù)域的映射關(guān)系表示為:[·]:Q4→R4。在這種映射定義下,四元數(shù)叉乘可以定義在實(shí)數(shù)域內(nèi):

[a×b]=[a×][b]

(2)

類(lèi)似的,四元數(shù)內(nèi)積可以定義為:

[ab]=[a]L[b]

(3)

式中:

并且

1.2 對(duì)偶四元數(shù)與矢量對(duì)偶四元數(shù)

對(duì)偶四元數(shù)有如下形式:

(4)

式中,q以及q′均為四元數(shù),并且對(duì)偶運(yùn)算符ε2=0,ε≠0。

如果只采用四元數(shù)的矢量部分,那么矢量對(duì)偶四元數(shù)可以表示為:

(5)

1) 矢量對(duì)偶四元數(shù)加法:

(6)

2) 矢量對(duì)偶四元數(shù)乘法:

(7)

3) 矢量對(duì)偶四元數(shù)共軛:

(8)

4) 矢量對(duì)偶四元數(shù)交換:

(9)

其余有關(guān)矢量對(duì)偶四元數(shù)的運(yùn)算可以參考獻(xiàn)[12]。

2 基于矢量對(duì)偶四元數(shù)的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型

2.1 模型綜述

圖1展示了服務(wù)航天器以及空間中自由翻滾的故障衛(wèi)星模型。{I}是慣性系。{B}是故障衛(wèi)星的本體系,坐標(biāo)原點(diǎn)為故障衛(wèi)星的質(zhì)心。不考慮服務(wù)航天器的自身尺寸,服務(wù)航天器上有2個(gè)相互獨(dú)立的單目相機(jī)以及一個(gè)激光測(cè)距儀。假設(shè){B′}是抓捕位置坐標(biāo)系,坐標(biāo)原點(diǎn)位于抓捕位置幾何中心。由于目標(biāo)是我方故障航天器,抓捕位置的安裝矩陣已知,因此,假設(shè)且{B}與{B′}平行。抓捕位置的幾何中心與目標(biāo)質(zhì)心之間的位移為ρ。

{B}相對(duì)于{I}的姿態(tài)四元數(shù)為:qB/I,{B}與{I}相對(duì)距離為rB/I,{B′}與{I}的距離為rm。同時(shí),本文假設(shè)故障衛(wèi)星相對(duì)服務(wù)航天器的線(xiàn)速度為v,角速度為ω。

圖1 服務(wù)航天器與故障航天器模型圖

2.2 基于矢量對(duì)偶四元數(shù)的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型

由文獻(xiàn)[10],并只采用其矢量部分,{B}相對(duì)于{I}的矢量對(duì)偶四元數(shù)為:

(10)

同時(shí),定義矢量對(duì)偶速度如下:

(11)

由文獻(xiàn)[12],并取其矢量部分,基于矢量對(duì)偶四元數(shù)的運(yùn)動(dòng)學(xué)方程可以表示為:

(12)

由文獻(xiàn)[12],采用誤差矢量對(duì)偶四元數(shù)的運(yùn)動(dòng)學(xué)方程為:

(13)

由于只有當(dāng)誤差姿態(tài)為180°時(shí),誤差四元數(shù)才會(huì)出現(xiàn)奇異。而當(dāng)參數(shù)估計(jì)算法有效時(shí),誤差將遠(yuǎn)遠(yuǎn)小于180°。因此,當(dāng)參數(shù)估計(jì)算法有效時(shí),可以認(rèn)為誤差矢量四元數(shù)不會(huì)奇異。

由文獻(xiàn)[12],基于矢量對(duì)偶四元數(shù)的動(dòng)力學(xué)方程為:

(14)

相關(guān)運(yùn)算參考文獻(xiàn)[13]。

3 基于矢量對(duì)偶四元數(shù)的故障衛(wèi)星容錯(cuò)參數(shù)估計(jì)算法

本章基于每個(gè)單目相機(jī)設(shè)計(jì)了測(cè)量正常時(shí)獨(dú)立參數(shù)估計(jì)算法(DVQ-EKF),根據(jù)每個(gè)通道的DVQ-EKF輸出,進(jìn)一步設(shè)計(jì)了多通道魯棒參數(shù)估計(jì)算法(DVQ-REKF)。

3.1 測(cè)量方程及其線(xiàn)性化

每個(gè)單目相機(jī)能夠獲得目標(biāo)的抓捕點(diǎn)相對(duì)于服務(wù)航天器的相對(duì)姿態(tài)測(cè)量量,激光測(cè)距系統(tǒng)能夠獲得目標(biāo)的抓捕點(diǎn)與服務(wù)航天器的相對(duì)位置,則在同一時(shí)刻,每一路獨(dú)立的測(cè)量方程為:

(15)

式中,qm是每一個(gè)單目相機(jī)測(cè)量的相對(duì)姿態(tài)。rm是激光測(cè)距儀測(cè)量的相對(duì)距離。v是測(cè)量噪聲,假設(shè)其為高斯白噪聲,且R=E[vvT]。

由文獻(xiàn)[8],qm可以表示為:

(16)

式中

(17)

同時(shí),由圖1,可得:

(18)

式中,A(qB/I)是{B′}到{I}的姿態(tài)轉(zhuǎn)移矩陣。根據(jù)3.1的假設(shè),測(cè)量值qm=q。

(18)式可以采用一階泰勒級(jí)數(shù)展開(kāi)為:

(19)

并且

(20)

考慮(19)式、(20)式,觀測(cè)方程可以表示為:

(21)

定義系統(tǒng)狀態(tài)為:

(22)

因此,觀測(cè)方程的線(xiàn)性化形式為:

(23)

3.2 DVQ-EKF設(shè)計(jì)

基于(22)式的狀態(tài),以及(13)式、(14)式,本小節(jié)先推導(dǎo)狀態(tài)方程的線(xiàn)性化形式,繼而設(shè)計(jì)DVQ-EKF。

假設(shè)在參數(shù)識(shí)別過(guò)程中,質(zhì)量參數(shù)不變,即:

(24)

(25)

結(jié)合(13)式、(14)式、(24)式、(25)式,系統(tǒng)狀態(tài)方程為:

(26)

因此,線(xiàn)性化后的狀態(tài)方程為:

Xk+1=ΦkXk+wk

(27)

式中,Φk?I+FΔt,Δt為采樣時(shí)間,并且:

(28)

式中:

(29)

式中:

是過(guò)程噪聲的強(qiáng)度。

由文獻(xiàn)[14]，DVQ-EKF可以設(shè)計(jì)為:

1) 時(shí)間更新:

XK/K-1=ΦkXk-1/k-1

(30)

(31)

2) 測(cè)量更新:

(32)

δXk=Kk(Zk-h(Xk/k-1))

(33)

Pk=[I-KkHk]Pk/k-1

(34)

3) 狀態(tài)更新:

(35)

Xk/k=Xk-1/k-1+δXk

(36)

3.3 DVQ-REKF設(shè)計(jì)

考慮空間中的復(fù)雜環(huán)境可能會(huì)對(duì)單目相機(jī)造成干擾,導(dǎo)致測(cè)量不準(zhǔn)確或者測(cè)量失效的問(wèn)題,本小節(jié)設(shè)計(jì)了多通道魯棒參數(shù)估計(jì)算法(DVQ-REKF)。

圖2表示了DVQ-REKF的算法結(jié)構(gòu)圖。首先,服務(wù)航天器分別接收同一時(shí)刻每一個(gè)獨(dú)立通道的測(cè)量量,并進(jìn)行雙通道相互獨(dú)立的DVQ-EKF參數(shù)估計(jì)。之后,參數(shù)估計(jì)結(jié)果輸入基于聯(lián)邦濾波器框架的數(shù)據(jù)融合系統(tǒng)并進(jìn)行數(shù)據(jù)處理,生成經(jīng)過(guò)融合的魯棒參數(shù)估計(jì)。

圖2 DVQ-REKF結(jié)構(gòu)圖

假設(shè)Xi,Pi(i=1,2)代表分別表示每一通道的DVQ-EKF參數(shù)估計(jì)量與其協(xié)方差,由聯(lián)邦濾波器信息分配原理可知,融合后的協(xié)方差P以及狀態(tài)估計(jì)X為:

(37)

(38)

通過(guò)(37)式、(38)式可以發(fā)現(xiàn),當(dāng)某一通道因?yàn)閱文肯鄼C(jī)的測(cè)量失效從而導(dǎo)致其DVQ-EKF參數(shù)估計(jì)出現(xiàn)偏差時(shí),可以通過(guò)平衡兩個(gè)通道各自參數(shù)估計(jì)的結(jié)果,進(jìn)行信息融合,從而獲得更為可靠的多通道魯棒參數(shù)估計(jì)。

4 仿真驗(yàn)證

本章通過(guò)Matlab仿真驗(yàn)證平臺(tái)驗(yàn)證了本文所設(shè)計(jì)的DVQ-EKF以及DVQ-REKF的效果。

同時(shí),假設(shè)外界干擾為高斯白噪聲,且:

(39)

(40)

分別表示外力對(duì)角速度和線(xiàn)速度的干擾。

假設(shè)視覺(jué)測(cè)量系統(tǒng)的采樣頻率為2 Hz,測(cè)量噪聲也為高斯白噪聲,且協(xié)方差為:

R=diag((0.01)I4×4,(0.1)I4×4)m

(41)

待估計(jì)參數(shù)的初始值如下表設(shè)定:

表1 參數(shù)初始值

并且,初始化的狀態(tài)協(xié)方差矩陣為:P(0)=I24×24,仿真總時(shí)間為T(mén)=200 s。

由于復(fù)雜的空間環(huán)境的干擾,考慮2個(gè)單目相機(jī)中的一個(gè)測(cè)量失效或者兩者均測(cè)量失效的情況。假設(shè)復(fù)雜環(huán)境對(duì)測(cè)量的影響體現(xiàn)在測(cè)量噪聲上:對(duì)qm的影響為對(duì)其附加一均值為0,方差為0.1的白噪聲;對(duì)rm的影響為對(duì)其附加一均值為0,方差為1的白噪聲。設(shè)置單目相機(jī)1的測(cè)量失效時(shí)間為仿真開(kāi)始后的第25～第35 s,設(shè)置單目相機(jī)2的測(cè)量失效時(shí)間為方針開(kāi)始后的第60～第70 s,設(shè)置2個(gè)單目相機(jī)均測(cè)量失效的時(shí)間為方針開(kāi)始后的第120～第130 s。

圖3 單目相機(jī)測(cè)量失效時(shí)DVQ-EKF參數(shù)估計(jì)誤差2-范數(shù) 圖4 四元數(shù)誤差仿真結(jié)果 圖5 相對(duì)距離誤差仿真結(jié)果

圖9 故障衛(wèi)星質(zhì)心位置誤差仿真結(jié)果

圖10 綜合誤差2-范數(shù)仿真結(jié)果

如圖3所示，當(dāng)某一通道測(cè)量失效情況發(fā)生時(shí)，由于DVQ-EKF沒(méi)有容錯(cuò)機(jī)制，因此不能在失效測(cè)量的情況下對(duì)故障衛(wèi)星的參數(shù)進(jìn)行獨(dú)立估計(jì)。

圖4～圖10展示了采用DVQ-REKF對(duì)故障衛(wèi)星進(jìn)行參數(shù)估計(jì)的結(jié)果?？梢钥吹剑捎贒VQ-REKF能夠分配雙通道參數(shù)估計(jì)的權(quán)重，因此，DVQ-REKF能夠在測(cè)量失效發(fā)生時(shí)，仍然對(duì)故障衛(wèi)星做出有效的參數(shù)估計(jì)。圖10顯示了情況2下，DVQ-REKF的參數(shù)估計(jì)整體效果。可以看到，在仿真時(shí)間為300 s時(shí)，該誤差的2-范數(shù)略大于0.2，滿(mǎn)足實(shí)際任務(wù)的精度要求。

同時(shí)，可以看出，當(dāng)測(cè)量失效發(fā)生時(shí)，盡管采用了具有魯棒功能的DVQ-REKF，相比于姿態(tài)參數(shù)，位置參數(shù)的參數(shù)估計(jì)量仍然具有相對(duì)大的誤差，這也是由于姿軌耦合效應(yīng)所致。因此，通過(guò)仿真分析，本文設(shè)計(jì)的DVQ-REKF雖然能夠在考慮姿軌耦合因素的前提下做到一體化的參數(shù)估計(jì)，然而并不能消除姿軌耦合所帶來(lái)的影響。

5 結(jié) 論

本文針對(duì)空間中的故障衛(wèi)星，利用搭載2個(gè)單目相機(jī)以及一個(gè)激光測(cè)距儀作為相對(duì)信息測(cè)量敏感器的服務(wù)航天器，設(shè)計(jì)了在測(cè)量失效情況下利用雙通道測(cè)量信息進(jìn)行數(shù)據(jù)融合的DVQ-REKF魯棒參數(shù)估計(jì)算法。通過(guò)仿真驗(yàn)證，可以看到本文所設(shè)計(jì)的位姿質(zhì)量參數(shù)一體化參數(shù)估計(jì)算法有效，且DVQ-REKF能夠在測(cè)量失效的條件下對(duì)目標(biāo)參數(shù)做出合理估計(jì)。盡管本文所設(shè)計(jì)的算法不能完全解決姿軌耦合問(wèn)題，然而相對(duì)于傳統(tǒng)算法而言，本文所設(shè)計(jì)DVQ-REKF仍然有著一定優(yōu)勢(shì)，為空間故障衛(wèi)星維修，航天器測(cè)控方法提供一種新的思路。