基于簡(jiǎn)化超球體SRUKF的FOG和SS組合定姿算法

聶高勝，朱高興

（江西師范高等專科學(xué)校 物聯(lián)網(wǎng)學(xué)院，江西 鷹潭 335000）

0 引 言

衛(wèi)星一般采用光纖陀螺實(shí)現(xiàn)定姿。光纖陀螺是一種基于Sagnac效應(yīng)的角速度傳感器，與傳統(tǒng)的機(jī)械陀螺相比具有重量輕、體積小、動(dòng)態(tài)范圍大、實(shí)時(shí)性好以及抗干擾能力強(qiáng)等優(yōu)點(diǎn)，但存在解算姿態(tài)誤差隨時(shí)間積累的缺陷。星敏感器是一種通過(guò)測(cè)量恒星而實(shí)現(xiàn)定姿的天文傳感儀器，具有測(cè)量精度高、不受外界干擾等優(yōu)點(diǎn)，但實(shí)時(shí)性較差。兩者組合到一起定姿，可優(yōu)勢(shì)互補(bǔ)，形成一種長(zhǎng)航時(shí)衛(wèi)星定姿方式[1]。

光纖陀螺與星敏感器的信息融合一般采用卡爾曼濾波算法。卡爾曼濾波較適用于線性系統(tǒng)，對(duì)非線性系統(tǒng)的處理效果較差，但現(xiàn)實(shí)中的系統(tǒng)基本都是非線性系統(tǒng)。文獻(xiàn)[2]將簡(jiǎn)化無(wú)跡卡爾曼濾波（Simple Unscented Kalman Filter，SUKF）應(yīng)用到慣導(dǎo)系統(tǒng)的非線性初始對(duì)準(zhǔn)，取得了很好的效果；文獻(xiàn)[3-4]分別將SUKF應(yīng)用到慣導(dǎo)與GPS和慣導(dǎo)與地磁的非線性組合導(dǎo)航中，實(shí)現(xiàn)了更高精度的定位定向。本文則首次將SUKF算法應(yīng)用到光纖陀螺與星敏感器的組合定姿中，通過(guò)建立簡(jiǎn)化組合系統(tǒng)模型，采用反饋陀螺漂移誤差校正姿態(tài)的方式，實(shí)現(xiàn)了更高精度的光纖陀螺和星敏感器組合定姿。

1 姿態(tài)解算算法原理

光纖陀螺和星敏感器組合算法原理如圖1所示。其中：φFOG、θFOG、γFOG為三軸光纖陀螺系統(tǒng)解算得到的三維姿態(tài)；φSS、θSS、γSS為星敏感器解算得到的三維姿態(tài)；εX、εY、εZ為濾波過(guò)程中估算的三維陀螺誤差。光纖陀螺系統(tǒng)輸出三維慣性姿態(tài)信息，星敏感器輸出三維星敏姿態(tài)信息，利用簡(jiǎn)化的SUKF算法，將兩者姿態(tài)角信息之差作為觀測(cè)量進(jìn)行最優(yōu)濾波，并用濾波過(guò)程中估算的陀螺誤差修正慣導(dǎo)輸出，從而提高組合系統(tǒng)的姿態(tài)解算精度。

圖1 光纖陀螺和星敏感器組合定姿算法原理框圖

2 光纖陀螺和星敏感器組合系統(tǒng)模型建立

2.1 狀態(tài)模型

選擇東北天地理坐標(biāo)系（E、N、U）為導(dǎo)航坐標(biāo)系，建立卡爾曼濾波的狀態(tài)方程為：

式（1）中，組合導(dǎo)航系統(tǒng)的狀態(tài)量為：

式中，φE、φN、φU為三維姿態(tài)誤差角，εE、εN、εU為三維陀螺常值漂移。

系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣Fk(t)為：

式（3）中，F(xiàn)N中的元素與慣導(dǎo)系統(tǒng)的誤差方程相對(duì)應(yīng)，Cbn為捷聯(lián)姿態(tài)矩陣。

系統(tǒng)的噪聲轉(zhuǎn)移矩陣為：

系統(tǒng)噪聲矢量為：

2.2 量測(cè)模型

根據(jù)量測(cè)信息建立的量測(cè)方程為：

其中，Y(t)=[φx,φy,φz]T，量測(cè)矩陣為H(t)=[I3×303×3]，v(t)為量測(cè)噪聲矩陣。

3 SUKF濾波算法簡(jiǎn)介

無(wú)跡卡爾曼濾波是一種非線性濾波方法。對(duì)于非線性系統(tǒng)，它的濾波性能優(yōu)于卡爾曼濾波。

假設(shè)一個(gè)維數(shù)為n的離散化非線性系統(tǒng)方程和量測(cè)方程為：

其中，xk為系統(tǒng)的狀態(tài)向量矩陣，fk為狀態(tài)變量的傳播函數(shù)，wk為系統(tǒng)的噪聲向量，yk為量測(cè)向量，hk為量測(cè)向量矩陣，vk為系統(tǒng)量測(cè)噪聲向量。

SUKF濾波的本質(zhì)是通過(guò)在的附近構(gòu)造一組Sigma采樣點(diǎn)，并計(jì)算Sigma采樣點(diǎn)，估計(jì)出狀態(tài)一步預(yù)測(cè)和一步預(yù)測(cè)均方差，從而進(jìn)行濾波，具體算法過(guò)程描述如下。

首先，設(shè)定濾波的初始值，即：

其次，計(jì)算Sigma采樣點(diǎn)。此時(shí)，根據(jù)i的取值，分3種情況進(jìn)行討論。

（1）當(dāng)i=0時(shí)，有

（2）當(dāng)i=1,2,…,n時(shí)，有

（3）當(dāng)i=n+1,…,2n時(shí)，有

其中，λ=α2(n+k)-n為標(biāo)量。α是很小的正數(shù)，可取10-4≤α≤1，k通常設(shè)置為零。

再次，通過(guò)時(shí)間更新方程更新時(shí)間信息，具體為：

最后，量測(cè)更新方程為：

其中，β的取值與x的分布有關(guān)，通常高斯分布的x取值為2。

4 仿真試驗(yàn)驗(yàn)證

試驗(yàn)選用三軸的光纖陀螺產(chǎn)品，陀螺常值漂移為0.05°/h（1σ），是衡量陀螺性能的關(guān)鍵指標(biāo)。陀螺置于帶有北向基準(zhǔn)的測(cè)試平臺(tái)上，通電穩(wěn)定后采集三向陀螺3 600 s靜態(tài)輸出原始數(shù)據(jù)；星敏感器的量測(cè)精度設(shè)為1''，計(jì)算機(jī)仿真生成導(dǎo)航坐標(biāo)系下3 600 s靜態(tài)的三向姿態(tài)數(shù)據(jù)用作星敏感器的靜態(tài)導(dǎo)航輸出數(shù)據(jù)。分別用純FOG、卡爾曼濾波（KF）和簡(jiǎn)化SUKF濾波算法進(jìn)行離線仿真計(jì)算，并對(duì)比分析解算結(jié)果。3種解算方法的姿態(tài)解算結(jié)果對(duì)比如圖2所示。可以看出，簡(jiǎn)化SUKF濾波算法解算的三向姿態(tài)角誤差明顯小于其他兩種方法。

圖2 3種算法解算的航向角誤差對(duì)比

由于3種算法的組合導(dǎo)航誤差曲線都比較平滑，可以用姿態(tài)的最大誤差和標(biāo)準(zhǔn)差來(lái)評(píng)價(jià)其精度水平。表1列出了3種算法的統(tǒng)計(jì)結(jié)果，其中航向角、俯仰角以及橫滾角是衡量姿態(tài)的3個(gè)重要指標(biāo)。

由仿真結(jié)果可見(jiàn)，在相同的3 600 s導(dǎo)航時(shí)間內(nèi)，簡(jiǎn)化SUKF算法解算的三向姿態(tài)最大誤差約是KF算法的20%，約是純FOG算法的5%，SUKF算法解算的三向姿態(tài)標(biāo)準(zhǔn)差約是KF算法的16%，約是純FOG算法的6%。可見(jiàn)，相比純FOG和KF算法，簡(jiǎn)化SUKF算法可得到更高精度的姿態(tài)角。因?qū)Ш秸`差有隨機(jī)性和隨時(shí)間發(fā)散性，最大誤差和標(biāo)準(zhǔn)差都是指3 600 s內(nèi)時(shí)間段內(nèi)所有數(shù)據(jù)對(duì)于同一方法同一指標(biāo)的結(jié)果。比如，SUKF方法“head”在3 600 s這個(gè)時(shí)間段的導(dǎo)航誤差最大誤差值是6.639，這段時(shí)間導(dǎo)航的標(biāo)椎差是1.602。

表1 3種算法解算的姿態(tài)誤差統(tǒng)計(jì)

5 結(jié) 論

仿真試驗(yàn)結(jié)果表明，將SUKF濾波算法應(yīng)用到光纖陀螺和星敏感器的組合定姿中，可很好地處理非線性系統(tǒng)，且在長(zhǎng)航時(shí)比卡爾曼濾波和純FOG解算的姿態(tài)角精度更高，證明了SUKF濾波算法的正確性和可行性，可為其今后在衛(wèi)星定姿中的進(jìn)一步應(yīng)用提供理論參考。