華 冰, 梁瑩瑩, 倪 瑞

(南京航空航天大學(xué)航天學(xué)院, 江蘇 南京 211000)

0 引 言

航天器的姿態(tài)確定指航天器依靠自身攜帶的敏感器的測(cè)量信息或者其他信息,通過(guò)數(shù)據(jù)處理獲得航天器本身相對(duì)于某一坐標(biāo)系姿態(tài)參數(shù)的過(guò)程[1],姿態(tài)確定的精度會(huì)直接影響航天器整體的控制和導(dǎo)航精度[2],是航天器在軌平穩(wěn)運(yùn)行、完成多種任務(wù)的關(guān)鍵。

目前小衛(wèi)星常用姿態(tài)敏感器包括磁強(qiáng)計(jì)、紅外地平儀、星敏感器、太陽(yáng)敏感器、陀螺等,單個(gè)傳感器運(yùn)用姿態(tài)確定往往缺乏足夠的穩(wěn)定性和可靠性,精度也不高,因此一般采用組合測(cè)姿方法進(jìn)行姿態(tài)確定。如:星敏和陀螺姿態(tài)確定[3]、星敏與紅外地平儀進(jìn)行組合測(cè)姿[4]、采用分布式融合方式進(jìn)行組合測(cè)姿[5]。傳統(tǒng)的組合姿態(tài)確定常采用聯(lián)邦濾波的形式,將各個(gè)傳感器作為子濾波,分布式計(jì)算,然后分配權(quán)值進(jìn)行融合,該方法結(jié)構(gòu)簡(jiǎn)單,具有計(jì)算量小,精度較高等優(yōu)點(diǎn)。在實(shí)際任務(wù)中,通常各個(gè)傳感器的更新頻率不一致,因此往往需要進(jìn)行數(shù)據(jù)同步處理,而上述傳統(tǒng)方法容易造成數(shù)據(jù)浪費(fèi);同時(shí),若某一傳感器失效,需要增加或者替換新的傳感器時(shí),則需要復(fù)雜的系統(tǒng)重構(gòu)。

因子圖[6-7]是圖模型中的一種,用于表示函數(shù)的因式分解,其在統(tǒng)計(jì)推斷、譯碼編碼、消息傳遞、濾波平滑等方面都有所應(yīng)用。目前在一些小型場(chǎng)景中機(jī)器人的定位導(dǎo)航與建圖[8-9]中有廣泛的運(yùn)用。Kschischang[10]最早提出了因子圖的基本概念并總結(jié)了因子圖和積算法,給出了詳細(xì)的數(shù)學(xué)理論過(guò)程;由于其強(qiáng)大的包容性,常被用于解決不同場(chǎng)景下的多源導(dǎo)航傳感器融合問(wèn)題,如Chiu等[11]在美國(guó)Georgia理工學(xué)院的研究基礎(chǔ)上,將因子圖模型用于包含57種不同的傳感器的機(jī)器人自主導(dǎo)航問(wèn)題;德國(guó)Lange[12]等將基于因子圖模型應(yīng)用到裝配有慣性測(cè)量單元、光流傳感器和無(wú)線電高度表的四旋翼無(wú)人機(jī)中;因子圖模型也被用于航天領(lǐng)域,Tweddle[13]等建立了空間非合作目標(biāo)剛體動(dòng)力學(xué)因子圖,用于估計(jì)和預(yù)測(cè)非合作目標(biāo)的各種狀態(tài)參數(shù);Takeishi等將因子圖應(yīng)用在小行星探測(cè)背景[14-15]中;因子圖在水下航行器[16]、無(wú)人機(jī)[17]、室內(nèi)機(jī)器人[18]、航天器上都有廣泛的應(yīng)用,基于因子圖的慣性/視覺(jué)信息融合方法是因子圖算法一大方向,國(guó)內(nèi)也將因子圖用于組合導(dǎo)航[19]、檢驗(yàn)失效航天器[20]、航天器故障診斷[21]、姿態(tài)估計(jì)[22]、室內(nèi)導(dǎo)航等。

卡爾曼濾波是組合定姿中常用的方法。該方法通過(guò)計(jì)算卡爾曼增益,僅使用上一時(shí)刻狀態(tài)進(jìn)行實(shí)時(shí)的狀態(tài)估計(jì)[23],其姿態(tài)確定的精度依賴于模型的準(zhǔn)確度,適用于線性系統(tǒng),難以應(yīng)對(duì)復(fù)雜多變的情況,缺乏自主優(yōu)化手段;相比于卡爾曼濾波,基于因子圖模型的方法可以通過(guò)圖優(yōu)化保留過(guò)去的狀態(tài),當(dāng)新的信息引入之后,對(duì)過(guò)去的信息一起進(jìn)行優(yōu)化[24]。另外,由于各傳感器的工作頻率不同,基于卡爾曼濾波的姿態(tài)確定算法在進(jìn)行多傳感器融合的數(shù)據(jù)同步時(shí),會(huì)造成數(shù)據(jù)浪費(fèi);另一方面,當(dāng)出現(xiàn)傳感器的增減、或者出現(xiàn)失效傳感器的時(shí)候,由于各傳感器間的誤差特性不同和測(cè)量誤差非高斯分布等特點(diǎn),基于卡爾曼濾波的算法需要重新進(jìn)行復(fù)雜的系統(tǒng)重構(gòu),這會(huì)大幅增加系統(tǒng)設(shè)計(jì)的復(fù)雜性,不利于新算法的拓展和多傳感器的使用。因子圖模型在復(fù)雜環(huán)境中有較好的表現(xiàn)力,傳感器的增刪可以方便地通過(guò)傳感器因子的增刪實(shí)現(xiàn),從而合理而充分地利用其他姿態(tài)測(cè)量信息實(shí)現(xiàn)即插即用,避免了復(fù)雜的系統(tǒng)重構(gòu),具有較好的包容性和易擴(kuò)展性,為實(shí)現(xiàn)高精度、魯棒性強(qiáng)的自主導(dǎo)航技術(shù)奠定了基礎(chǔ)。

本文提出一種基于改進(jìn)因子圖模型的航天器組合姿態(tài)確定方法,針對(duì)典型的航天器姿態(tài)確定系統(tǒng),通過(guò)建立因子圖模型,將多傳感器測(cè)量信息作為因子節(jié)點(diǎn),即插即用地加入因子圖模型。針對(duì)多測(cè)量信息頻率不一致、存在數(shù)據(jù)冗余和誤差不一等問(wèn)題,該方法對(duì)多源觀測(cè)數(shù)據(jù)集進(jìn)行了觀測(cè)蒸餾,通過(guò)提煉出高質(zhì)量的觀測(cè)值實(shí)現(xiàn)增量化構(gòu)建因子節(jié)點(diǎn),對(duì)基于因子圖進(jìn)行完善;然后通過(guò)自適應(yīng)調(diào)整觀測(cè)數(shù)據(jù)權(quán)值降低數(shù)據(jù)量,同時(shí)分析了觀測(cè)數(shù)據(jù)對(duì)整體姿態(tài)確定精度的影響,并篩除其中的不利觀測(cè)數(shù)據(jù),從而提高航天器的姿態(tài)確定精度。

1 典型地磁/星敏/陀螺姿態(tài)確定系統(tǒng)

星敏感器是常用的姿態(tài)確定敏感器,精度高，但更新頻率低;磁強(qiáng)計(jì)性價(jià)比高、穩(wěn)定性強(qiáng)、更新頻率也較高,但是精度較低;陀螺通過(guò)積分可以測(cè)量姿態(tài)的角速度,但長(zhǎng)時(shí)間會(huì)存在較大的累計(jì)誤差。星敏與陀螺組合可以彌補(bǔ)星敏頻率低的缺點(diǎn),但是一旦星敏故障,則無(wú)法保證精度。多傳感器組合姿態(tài)確定的方法常采用聯(lián)邦濾波的方式,如圖1所示。該方法需要進(jìn)行數(shù)據(jù)同步,而此過(guò)程中往往更新頻率取決于星敏感器,因此通常會(huì)造成一定的數(shù)據(jù)浪費(fèi)。當(dāng)需要加入新的傳感器時(shí),則需要重新設(shè)計(jì)系統(tǒng)構(gòu)型。本節(jié)將對(duì)典型的姿態(tài)傳感器模型進(jìn)行介紹,該部分也是進(jìn)行建立傳感器因子節(jié)點(diǎn)的基礎(chǔ)。

圖1 典型地磁/星敏/陀螺姿態(tài)確定系統(tǒng)結(jié)構(gòu)圖

1.1 測(cè)量模型

1.1.1 磁強(qiáng)計(jì)測(cè)量模型

對(duì)于近地航天器,在外部磁場(chǎng)影響可忽略的情況下,可以采用高斯球諧函數(shù)將地磁場(chǎng)勢(shì)函數(shù)描述為

V=

(1)

地磁場(chǎng)強(qiáng)度矢量可以表示成地磁場(chǎng)勢(shì)函數(shù)的負(fù)梯度,在地理坐標(biāo)系中,為磁場(chǎng)V在北、東、地3個(gè)方向的分量,由此可以得到地磁場(chǎng)矢量與航天器所在位置的關(guān)系式,即地球固聯(lián)坐標(biāo)系e中地磁場(chǎng)矢量:

(2)

式中:Be=[Bex,Bey,Bez]表示航天器地理系三軸磁場(chǎng)矢量;將本體系三軸磁場(chǎng)矢量小量作為觀測(cè)量[25],可以表示為

Z1=ΔBb=A(Q)δQ+A(Q)ΔB0+v1

(3)

式中:B0=Aoi(orbit)Be,Aoi表示軌道系到慣性系的轉(zhuǎn)換矩陣，與航天器位置orbit相關(guān)；v1為測(cè)量誤差；A(Q)為軌道系到本體系姿態(tài)轉(zhuǎn)換矩陣；Bo，Bb為航天器軌道系、本體系的三軸磁場(chǎng)矢量，具體轉(zhuǎn)換方式同方法[23]。

1.1.2 星敏感器測(cè)量模型

(4)

選取觀測(cè)量為測(cè)量誤差四元數(shù)矢量部分：

Z2=Csδqs+ν2

(5)

式中：Cs=diag{1+μ,1+μ,1+μ},μ是均值為μμ的白噪聲；ν2為測(cè)量誤差,是均值為0的白噪聲。

1.1.3 陀螺測(cè)量模型

陀螺是衛(wèi)星姿態(tài)控制系統(tǒng)中一種極為重要的敏感器,其輸出衛(wèi)星三軸相對(duì)于慣性空間的角速率,與姿態(tài)敏感器聯(lián)合使用進(jìn)行姿態(tài)確定,可以實(shí)現(xiàn)對(duì)姿態(tài)角速度的測(cè)量,輸出相對(duì)于參考坐標(biāo)系的角速度,不考慮安裝誤差和陀螺指數(shù)漂移,其測(cè)量模型如下所示:

ωg=ω+b+ηg

(6)

1.2 姿態(tài)運(yùn)動(dòng)學(xué)模型

用姿態(tài)四元數(shù)來(lái)對(duì)航天器的姿態(tài)運(yùn)動(dòng)進(jìn)行描述,可以表示為

(7)

式中:Q=[q0，q]表示姿態(tài)四元數(shù)，q0為姿態(tài)四元數(shù)標(biāo)量部分,q為矢量部分；ω=[ωx，ωy，ωz]為姿態(tài)角速度;ω×表示向量的叉乘反對(duì)稱陣。對(duì)式(7)進(jìn)行求導(dǎo),并忽略二階小量可得

(8)

由于四元數(shù)存在奇異的問(wèn)題,針對(duì)小角度的機(jī)動(dòng)的航天器,采用誤差四元數(shù)對(duì)姿態(tài)進(jìn)行動(dòng)態(tài)描述,可以有效避免該問(wèn)題。航天器在外力矩的作用下,發(fā)生姿態(tài)的改變。外力矩一般包括航天器的控制力矩與空間擾動(dòng)力矩。姿態(tài)動(dòng)力學(xué)方程用于描述轉(zhuǎn)動(dòng)角速度和所受外力矩之間的關(guān)系,即

(9)

式中:J為航天器的轉(zhuǎn)動(dòng)慣性量;h為動(dòng)量輪角動(dòng)量;Nτ為空間擾動(dòng)力矩;Nr為控制力矩。不考慮角動(dòng)量輪的作用,空間擾動(dòng)力矩,控制力矩均為0情況下,進(jìn)行求導(dǎo)得

(10)

2 基于改進(jìn)因子圖模型的航天器組合姿態(tài)確定算法

航天器的姿態(tài)運(yùn)動(dòng)是一個(gè)典型的馬爾可夫過(guò)程,是一個(gè)多變量控制系統(tǒng)。如果采用因子圖模型建模,狀態(tài)的傳遞,以及姿態(tài)的觀測(cè),可以非常方便地通過(guò)作為因子變量的方式加入到模型之中[27]。在因子圖模型中,變量節(jié)點(diǎn)到因子節(jié)點(diǎn)或者因子節(jié)點(diǎn)到變量節(jié)點(diǎn)的是通過(guò)信息交互的,因此節(jié)點(diǎn)的變化和增刪可以很靈活地通過(guò)信息傳遞反應(yīng),從而實(shí)現(xiàn)傳感器的即插即用。

一般線性系統(tǒng)的因子圖表示如圖2所示。其中,方塊表示函數(shù)關(guān)系;圓圈表示狀態(tài)量。狀態(tài)通過(guò)類似馬爾可夫鏈進(jìn)行傳遞。

圖2 一般線性系統(tǒng)的因子圖表示

考慮航天器穩(wěn)定時(shí)期的姿態(tài)運(yùn)動(dòng)為小角度運(yùn)動(dòng),常用姿態(tài)四元數(shù)描述航天器姿態(tài)運(yùn)動(dòng),誤差四元數(shù)的動(dòng)態(tài)變化可以直接反應(yīng)航天器姿態(tài)變化,基于因子圖的航天器姿態(tài)運(yùn)動(dòng)模型利用誤差四元數(shù)傳遞姿態(tài)變化,然后通過(guò)四元數(shù)計(jì)算得到航天器的最后估計(jì)姿態(tài),可簡(jiǎn)化表示為圖3所示。

圖3 基于因子圖的航天器姿態(tài)運(yùn)動(dòng)模型

由于傳感器制造工藝、使用環(huán)境、精度大小以及本身觀測(cè)方式的不同,不同傳感器獲取的姿態(tài)信息質(zhì)量不一,基于改進(jìn)因子圖模型的航天器多傳感器組合姿態(tài)確定算法的基本思想是:利用先驗(yàn)信息,采用觀測(cè)蒸餾法,計(jì)算觀測(cè)殘差,對(duì)觀測(cè)數(shù)據(jù)集進(jìn)行提煉,同時(shí)對(duì)提煉的信息進(jìn)行自適應(yīng)調(diào)整,將調(diào)整的觀測(cè)量作為節(jié)點(diǎn)增量更新因子圖,構(gòu)建滑動(dòng)窗口的航天器姿態(tài)因子圖狀態(tài)模型[28],從而高效合理利用多傳感器信息,提高整個(gè)系統(tǒng)的精度。系統(tǒng)整體如圖4所示。

圖4 基于改進(jìn)因子圖模型的航天器多傳感器組合姿態(tài)確定算法結(jié)構(gòu)圖

接下來(lái)對(duì)算法進(jìn)行具體介紹:算法的核心在于構(gòu)建某一時(shí)間段內(nèi)的因子圖模型,將系統(tǒng)狀態(tài)與姿態(tài)測(cè)量信息相關(guān)聯(lián),再基于后驗(yàn)估計(jì)理論實(shí)現(xiàn)數(shù)據(jù)的融合[29]。即在給定所有可用量測(cè)值后,計(jì)算所有狀態(tài)的聯(lián)合概率分布函數(shù)的最大后驗(yàn)概率估計(jì),其計(jì)算方法如下所示:

(11)

P(Zi|Xi)P(Xi|Xi-1)

(12)

根據(jù)最大后驗(yàn)概率的公式和全概率貝葉斯公式,求解滿足最大后驗(yàn)概率最大的狀態(tài)變?yōu)樽罱K的估計(jì)值,則一段時(shí)間tj時(shí)刻到tk時(shí)刻之內(nèi)的后驗(yàn)概率可以表示為

(13)

式中:P(Zj:k|Xj:k)表示tj時(shí)刻到tk時(shí)刻之間的ti時(shí)刻的觀測(cè)量相對(duì)于ti時(shí)刻的狀態(tài)量Xj:k的先驗(yàn)概率;P(Xj:k|Xj:k)表示ti時(shí)刻的狀態(tài)量Xj:k相對(duì)于前一時(shí)刻狀態(tài)量的先驗(yàn)概率。

通過(guò)式(13)可看出,該先驗(yàn)概率可以表示為多個(gè)函數(shù)相乘的形式,表示為如下形式:

(14)

2.1 姿態(tài)測(cè)量信息因子

在衛(wèi)星姿態(tài)確定系統(tǒng)之中,傳感器的測(cè)量模型一般為線性模型,可以表示為

Zi=hi(Xi)+Vi

(15)

式中：hi(·)表示傳感器的測(cè)量函數(shù),給定估計(jì)狀態(tài)可以通過(guò)測(cè)量函數(shù)得到傳感器的預(yù)測(cè)值;Vi表示為均值為0的白噪聲。把誤差函數(shù)描述為傳感器的實(shí)際測(cè)量值與預(yù)測(cè)值誤差,即err=Zi-hi(Xi)。其中,Zi表示實(shí)測(cè)值,定義傳感器因子節(jié)點(diǎn)f(Xi)為ti時(shí)刻每個(gè)觀測(cè)量相對(duì)于狀態(tài)量Xi的誤差函數(shù),因子節(jié)點(diǎn)fi與誤差函數(shù)err(Xi,Zi)有關(guān),可表示為

f(Xi)=d[err(Xi,Zi)]

(16)

式中：d(·)表示對(duì)應(yīng)的代價(jià)函數(shù)。

針對(duì)第1.1節(jié)中不同傳感器的從測(cè)量模型,磁強(qiáng)計(jì)因子節(jié)點(diǎn)可以具體表示為

(17)

星敏感器因子節(jié)點(diǎn)可以表示為

(18)

對(duì)于高斯分布,式(18)的代價(jià)函數(shù)可以表示為指數(shù)形式,即

(19)

2.2 航天器姿態(tài)運(yùn)動(dòng)學(xué)因子

航天器姿態(tài)運(yùn)動(dòng)學(xué)因子節(jié)點(diǎn)函數(shù)可表示為

err(Xi,Xi-1)=g(Xi-1)-Xi

(20)

式中：g(·)為狀態(tài)轉(zhuǎn)移函數(shù)。由于狀態(tài)量的轉(zhuǎn)移滿足姿態(tài)動(dòng)力學(xué)方程,如第1.2節(jié)所述。同樣,概率密度P(Xi|Xi-1)可以表示為指數(shù)形式:

(21)

(22)

式中：Xi-1和Xi表示為ti時(shí)刻狀態(tài)和前一時(shí)刻態(tài)。

2.3 改進(jìn)因子圖模型的航天器組合姿態(tài)確定算法

利用建立的傳感器因子節(jié)點(diǎn)和姿態(tài)動(dòng)力學(xué)因子節(jié)點(diǎn),帶入并對(duì)式(12)左右取對(duì)數(shù),從而將乘積變成和的形式,將式(14)實(shí)際上變成了一個(gè)優(yōu)化問(wèn)題。

(23)

因此,基于改進(jìn)因子圖模型的多傳感器姿態(tài)確定的過(guò)程可以描述為:在已知所有觀測(cè)量Zj,Zj+1,…,Zk下估計(jì)狀態(tài)Xj,Xj+1,…,Xk的概率函數(shù),并使之最大。假設(shè)所述的姿態(tài)運(yùn)動(dòng)系統(tǒng)的狀態(tài)轉(zhuǎn)移和觀測(cè)噪聲都符合高斯白噪聲分布,則根據(jù)馬爾可夫過(guò)程的概率密度函數(shù)的定義,可以根據(jù)則全概率將系統(tǒng)的概率密度函數(shù)定義為

P(Xj:k|Zj:k)∝

(24)

通過(guò)取對(duì)數(shù),式(11)求解最大后驗(yàn)概率問(wèn)題轉(zhuǎn)化成求下列解式代價(jià)函數(shù)最小問(wèn)題:

(25)

當(dāng)新的狀態(tài)與因子引入之后,對(duì)新的因子圖進(jìn)行全局優(yōu)化不需要全部重新計(jì)算,而是利用之前優(yōu)化過(guò)程中的信息,即因子圖的增量特性,從而可以大大減少因子圖的優(yōu)化問(wèn)題中的計(jì)算量。在考慮磁強(qiáng)計(jì)星敏感器、陀螺組合導(dǎo)航情況下,姿態(tài)確定的問(wèn)題可以描述成求解下列最小值為問(wèn)題。

(26)

可以看出,如果有新的節(jié)點(diǎn)的引入,目標(biāo)函數(shù)對(duì)應(yīng)增加了新的多項(xiàng)式和,由此傳感器的增刪變得簡(jiǎn)單,但是計(jì)算量由此增加。由于傳感器制造工藝、使用環(huán)境、本身觀測(cè)方式的不同,不同傳感器獲取的姿態(tài)信息質(zhì)量不一,因此本文提出的基于改進(jìn)因子圖模型的多源信息融合技術(shù),提出了一種觀測(cè)蒸餾法,通過(guò)計(jì)算觀測(cè)殘差,設(shè)定閾值,將觀測(cè)數(shù)據(jù)集進(jìn)行提煉,信息提煉成兩部分,一部分為高質(zhì)量觀測(cè)信息,另一部分為低質(zhì)量的觀測(cè)信息,為了降低低質(zhì)量觀測(cè)信息的影響,將低質(zhì)量觀測(cè)信息的均值、協(xié)方差矩陣和權(quán)值進(jìn)行自適應(yīng)調(diào)整,實(shí)現(xiàn)對(duì)觀測(cè)不確定模型調(diào)整修正,將修正的觀測(cè)量作為節(jié)點(diǎn)增量更新因子圖,從而減少整個(gè)系統(tǒng)的計(jì)算量,提高整個(gè)系統(tǒng)的精度。其具體流程如圖5所示。

圖5 采用觀測(cè)蒸餾的改進(jìn)因子圖模型流程圖

具體方法如下:計(jì)算窗口M內(nèi)的觀測(cè)殘差表示為

R=[r1,r2,…,rM]ri=|Zi-hn(Xi)|

假設(shè)觀測(cè)不確定模型滿足均值為μ，均方差為σ的正態(tài)分布,將觀測(cè)殘差偏移值為G(ri,μ,Λ)=(ri-μ)/σ,其中Λ為觀測(cè)殘差的協(xié)方差矩陣。計(jì)算新的觀測(cè)信息殘差系列,給定閾值Td,將滿足觀測(cè)殘差閾值的部分表示為:Ro={r|r∈R,G(r,μ,Λ)

為了定量的描述低質(zhì)量觀測(cè)的可靠性,首先對(duì)Rn進(jìn)行cholesky分解得到新的目標(biāo)變量Y,目標(biāo)變量Y表示為

Y=[L-1r1,L-1r2,…,L-1rm]

(27)

式中：μy和Λy為轉(zhuǎn)置的觀測(cè)殘差Y的均值與協(xié)方差矩陣。

為了分析低質(zhì)量觀測(cè)值的特性,對(duì)其均值和協(xié)方差進(jìn)行檢驗(yàn),構(gòu)建值：

F(d,n-d)。

對(duì)其均值和協(xié)方差真進(jìn)檢驗(yàn)卡方檢驗(yàn)和F檢驗(yàn)。如果均值和協(xié)方差與高質(zhì)量模型一致,那么自適應(yīng)調(diào)整不確定模型的均值協(xié)方差以及權(quán)值:

(28)

如果低質(zhì)量觀測(cè)信息的均值和協(xié)方差不一致,與則調(diào)整該觀測(cè)源的權(quán)值;如果觀測(cè)殘差的均值超過(guò)閾值則將權(quán)值置零,調(diào)整方式如下所示:

(29)

改進(jìn)的因子圖模型可以描述為

(30)

通過(guò)求解得到狀態(tài)的誤差四元數(shù),通過(guò)誤差四元數(shù)計(jì)算姿態(tài)四元數(shù)的方式如下:

(31)

在進(jìn)行多傳感器組合測(cè)姿時(shí),將基于不同模型需要進(jìn)行的步驟進(jìn)行對(duì)比[30],如表1所示。

表1 不同方式重新進(jìn)行組合測(cè)姿方案步驟對(duì)比

基于因子圖模型的地磁姿態(tài)確定可具有較強(qiáng)的擴(kuò)展性,可以合理充分地利用其他測(cè)量信息,在面對(duì)復(fù)雜情況具有較好的包容性,可以實(shí)現(xiàn)即插即用,但是由于保存了一頓時(shí)間內(nèi)所有狀態(tài)和觀測(cè)量,使得相對(duì)計(jì)算量較大,隨著計(jì)算力的不斷提高及增量?jī)?yōu)化算法的改進(jìn),計(jì)算量不是限制問(wèn)題?；谝蜃訄D模型的地磁姿態(tài)確定可為應(yīng)對(duì)未來(lái)復(fù)雜的空間環(huán)境提供較好的模型平臺(tái)。

3 仿真驗(yàn)證

GTSAM 是一個(gè)基于因子圖模型的開源的C++優(yōu)化庫(kù),用因子圖和貝葉斯網(wǎng)絡(luò)算最大化后驗(yàn)概率,使用增量平滑算法。本實(shí)驗(yàn)針對(duì)三軸穩(wěn)定狀態(tài)下的航天器進(jìn)行姿態(tài)確定,傳感器采用磁強(qiáng)計(jì)、星敏感器、陀螺進(jìn)行遞推估計(jì)和固定滯后平滑過(guò)程,觀測(cè)的蒸餾通過(guò)Matlab算法進(jìn)行實(shí)現(xiàn)。

3.1 仿真條件

針對(duì)500 km低軌對(duì)地三軸穩(wěn)定衛(wèi)星,其基本軌道和姿態(tài)參數(shù)如下。

(1)J2000慣性坐標(biāo)系下設(shè)置該衛(wèi)星的軌道六根數(shù)初始值如表2所示。

表2 衛(wèi)星軌道6根數(shù)初始值

(2)敏感器的參數(shù)配置及模型噪聲設(shè)置如下:磁強(qiáng)計(jì)的測(cè)量精度為10 nT;星敏感器的測(cè)量精度為10″;陀螺的漂移為1°/h;轉(zhuǎn)動(dòng)慣性為[2.683，2.326，1.897]kg2/m3;采樣間隔為1 s;

(3)改進(jìn)因子圖的參數(shù)設(shè)置:Td=10-5，Tr=10-2。

3.2 仿真結(jié)果與分析

首先利用擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)對(duì)基于星敏感器的航天器姿態(tài)確定進(jìn)行仿真,星敏感器的精度和其他參數(shù)設(shè)置同第3.1節(jié)所示,姿態(tài)確定后的歐拉角誤差如圖6所示。

圖6 基于EKF的星敏感器航天器姿態(tài)確定歐拉角誤差

利用歐拉角誤差的標(biāo)準(zhǔn)差來(lái)衡量精度的大小,其計(jì)算方式為

仿真結(jié)果表明,當(dāng)存在初始誤差時(shí),濾波器經(jīng)過(guò)10 s左右收斂到穩(wěn)定狀態(tài)。穩(wěn)定后基于卡爾曼濾波模型三軸歐拉角誤差為[6.126 5,6.393 0,6.760 9]″,整體精度為:11.140 7″。在利用因子圖框架并采用星敏感器進(jìn)行航天器姿態(tài)確定的情況下,可以看出基于因子圖模型的航天器姿態(tài)確定方法不需要前期動(dòng)態(tài)穩(wěn)定的過(guò)程,因此姿態(tài)確定的整體精度會(huì)更高。

為避免奇異性,本文采用誤差四元描述姿態(tài)運(yùn)動(dòng)變化,利用因子圖對(duì)誤差四元數(shù)進(jìn)行遞推估計(jì),利用誤差四元數(shù)使用四元數(shù)乘法計(jì)算得到姿態(tài)四元數(shù)。為驗(yàn)證本文所提出的方法在復(fù)雜條件下可以實(shí)現(xiàn)即插即用,且具有較好的動(dòng)態(tài)穩(wěn)定性,考慮復(fù)雜條件包括:

(1)敏感器出現(xiàn)階段性失效;

(2)敏感器測(cè)量噪聲非高斯分布。

進(jìn)行以下實(shí)驗(yàn)。

實(shí)驗(yàn) 1星敏感器進(jìn)行姿態(tài)確定，驗(yàn)證該方法的有效性。

采用本文所提出的方法的航天器姿態(tài)確定歐拉角誤差結(jié)果,如圖7所示。

圖7 改進(jìn)因子圖前后的航天器姿態(tài)確定歐拉角誤差

由仿真結(jié)果可知,穩(wěn)定時(shí)基于因子圖的航天器三軸歐拉角誤差為[6.504 2,7.994 8,7.451 2]″,整體精度為12.717 8″;基于改進(jìn)后因子圖的三軸歐拉角誤差為[1.187 9, 0.516 7,0.377 0]″,整體精度為1.349 2″。

將采用因子圖進(jìn)行組合姿態(tài)確定與采用EKF進(jìn)行姿態(tài)確定對(duì)比,卡爾曼濾波的初始設(shè)置如表3所示。

表3 卡爾曼濾波仿真參數(shù)初始設(shè)置

將基于因子圖、改進(jìn)因子圖與EKF的航天器姿態(tài)確定誤差進(jìn)行對(duì)比,如圖8所示。

圖8 3種姿態(tài)確定方案對(duì)比

圖9 3種姿態(tài)確定方案整體姿態(tài)歐拉角誤差

3種姿態(tài)確定方案對(duì)比如表4所示。

表4 3種姿態(tài)確定方案對(duì)比

通過(guò)實(shí)驗(yàn)仿真對(duì)比可知,改進(jìn)后因子圖模型的姿態(tài)估計(jì)算法具有較好的精度,該算法可以通過(guò)觀測(cè)蒸餾法、自適應(yīng)確定權(quán)重,來(lái)顯著降低低質(zhì)量觀測(cè)對(duì)姿態(tài)的影響,并通過(guò)平滑姿態(tài)曲線,提高姿態(tài)確定的精度。同時(shí),由于該方法具備一定的野值故障排除作用,并通過(guò)滑動(dòng)窗口的滯后優(yōu)化,緩解了狀態(tài)量過(guò)多時(shí)計(jì)算量大的問(wèn)題。

實(shí)驗(yàn) 2整體仿真時(shí)間為1 000 s,第一階段(0～500 s)采用磁強(qiáng)計(jì)定姿;第二階段(500～1 000 s)采用磁強(qiáng)計(jì)、星敏、太敏和陀螺組合定姿。驗(yàn)證該方法可以實(shí)現(xiàn)即插即用,并在動(dòng)態(tài)切換的時(shí)候,具有較好的動(dòng)態(tài)穩(wěn)定性。

圖10 有傳感器缺失的姿態(tài)誤差曲線

圖11 有傳感器缺失的姿態(tài)誤差曲線局部圖

在該實(shí)驗(yàn)中,第一階段采用磁強(qiáng)計(jì)進(jìn)行姿態(tài)確定,精度為0.2°,第二階段將星敏感器通過(guò)添加節(jié)點(diǎn)的方式引入組合定姿中去,實(shí)現(xiàn)多傳感器的融合,精度可達(dá)0.002°。實(shí)驗(yàn)結(jié)果表明,該方法可以從第一階段流暢地切換到第二階段,具有較好的動(dòng)態(tài)穩(wěn)定性,可以實(shí)現(xiàn)傳感器的即插即用。

4 結(jié) 論

本文提出的基于因子圖模型的航天器姿態(tài)確定方法是一種擴(kuò)展性較強(qiáng)的組合測(cè)姿方法,可以合理而充分地利用其他姿態(tài)測(cè)量信息,避免了復(fù)雜的系統(tǒng)重構(gòu)過(guò)程,減少了系統(tǒng)的設(shè)計(jì)復(fù)雜度,有利于新算法的拓展和多傳感器的使用。實(shí)驗(yàn)結(jié)果表明,基于改進(jìn)因子圖模型的姿態(tài)確定具有較好的動(dòng)態(tài)穩(wěn)定性,同時(shí),本文提出的方法針對(duì)復(fù)雜情況下的航天器姿態(tài)確定,可以對(duì)基于因子圖的姿態(tài)確定進(jìn)行平滑,精度為1.349 2″。另外,該方法具有較高的擴(kuò)展性,以及動(dòng)態(tài)穩(wěn)定性,可以應(yīng)用在復(fù)雜條件下的航天器多傳感器組合定姿任務(wù)中。