基于簡化超球體SRUKF的FOG和SS組合定姿算法

聶高勝，朱高興

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

0 引 言

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

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

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

光纖陀螺和星敏感器組合算法原理如圖1所示。其中：φFOG、θFOG、γFOG為三軸光纖陀螺系統(tǒng)解算得到的三維姿態(tài)；φSS、θSS、γSS為星敏感器解算得到的三維姿態(tài)；εX、εY、εZ為濾波過程中估算的三維陀螺誤差。光纖陀螺系統(tǒng)輸出三維慣性姿態(tài)信息，星敏感器輸出三維星敏姿態(tài)信息，利用簡化的SUKF算法，將兩者姿態(tài)角信息之差作為觀測量進行最優(yōu)濾波，并用濾波過程中估算的陀螺誤差修正慣導(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)的誤差方程相對應(yīng)，Cbn為捷聯(lián)姿態(tài)矩陣。

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

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

2.2 量測模型

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

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

3 SUKF濾波算法簡介

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

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

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

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

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

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

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

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

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

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

再次，通過時間更新方程更新時間信息，具體為：

最后，量測更新方程為：

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

4 仿真試驗驗證

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

圖2 3種算法解算的航向角誤差對比

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

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

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

5 結(jié) 論

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