陀螺與磁強(qiáng)計(jì)組合定姿及漂移估計(jì)算法優(yōu)化

王獻(xiàn)忠 張 肖 劉 艷

1.上海市空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室, 上海 201109 2.上海航天技術(shù)研究院, 上海 201109 3.上海航天控制技術(shù)研究所, 上海 201109

0 引言

低軌衛(wèi)星在軌運(yùn)行期間通過(guò)三軸磁強(qiáng)計(jì)測(cè)得的地磁場(chǎng)矢量與應(yīng)用國(guó)際地磁場(chǎng)模型(IGRF)計(jì)算得到的地磁場(chǎng)矢量比較估計(jì)衛(wèi)星姿態(tài)，在某一時(shí)刻單獨(dú)利用磁強(qiáng)計(jì)只能確定二軸姿態(tài)，要確定三軸姿態(tài)至少需要具有一定夾角的雙矢量觀(guān)測(cè)，如與太陽(yáng)敏感器測(cè)得的太陽(yáng)矢量、地平儀測(cè)得的地心矢量等組合定姿。

單獨(dú)利用磁強(qiáng)計(jì)不能連續(xù)確定三軸姿態(tài)，但可以基于軌道運(yùn)動(dòng)斷續(xù)獲取三軸姿態(tài)，單獨(dú)利用磁強(qiáng)計(jì)定姿在應(yīng)用上有一定的局限性，通常將磁強(qiáng)計(jì)與陀螺組合進(jìn)行定姿。

隨著微電子、微機(jī)械等新技術(shù)的發(fā)展，航天產(chǎn)品越來(lái)越小型化，如具備三軸角速率測(cè)量功能的硅微陀螺只有幾十克，具備三軸磁場(chǎng)強(qiáng)度測(cè)量功能的磁強(qiáng)計(jì)甚至更輕，這些小型化的產(chǎn)品在小衛(wèi)星上得到了廣泛應(yīng)用，出現(xiàn)了納星、皮星等微小衛(wèi)星。

對(duì)陀螺測(cè)得的三軸姿態(tài)角速率積分也可以確定衛(wèi)星姿態(tài)，但其測(cè)量精度受陀螺角速率漂移影響，只能用于短期姿態(tài)確定。將陀螺與磁強(qiáng)計(jì)組合，利用磁強(qiáng)計(jì)的測(cè)量值估算和修正陀螺角速率漂移，同時(shí)利用陀螺彌補(bǔ)磁強(qiáng)計(jì)定姿實(shí)時(shí)性差的缺點(diǎn)。

文獻(xiàn)[1]基于陀螺、太陽(yáng)敏感器、磁強(qiáng)計(jì)和星敏感器4種敏感器，研究了微小衛(wèi)星在速率阻尼模式、太陽(yáng)捕獲模式、對(duì)日對(duì)地定向及維持模式、試驗(yàn)?zāi)Ｊ较碌南到y(tǒng)建模方法,以及基于Unscented卡爾曼濾波(UKF)的組合定姿方法。文獻(xiàn)[2]基于衛(wèi)星運(yùn)動(dòng)學(xué)建立狀態(tài)方程,應(yīng)用EKF進(jìn)行陀螺與磁強(qiáng)計(jì)組合定姿研究。文獻(xiàn)[3]基于低成本低精度的MEMS陀螺、太陽(yáng)敏感器、磁強(qiáng)計(jì)組合，設(shè)計(jì)了Unscented Kalman融合濾波算法。文獻(xiàn)[4-5]僅利用三軸磁強(qiáng)計(jì)作為測(cè)量設(shè)備，分別通過(guò)EKF和UKF處理測(cè)量數(shù)據(jù),實(shí)時(shí)地估計(jì)無(wú)陀螺小衛(wèi)星的姿態(tài)角與角速度。文獻(xiàn)[6]提出了一種利用梯度下降法對(duì)加速度計(jì)和磁強(qiáng)計(jì)的數(shù)據(jù)進(jìn)行姿態(tài)解算，并采用互補(bǔ)濾波算法將其與陀螺儀積分得到的結(jié)果進(jìn)行融合的定姿方法。文獻(xiàn)[7]基于四元數(shù)的卡爾曼濾波進(jìn)行陀螺與磁強(qiáng)計(jì)組合定姿。

以上算法多是采用EKF、或UKF或其它比較復(fù)雜的濾波算法進(jìn)行姿態(tài)估計(jì)，工程應(yīng)用中還必須考慮姿態(tài)確定算法復(fù)雜度對(duì)軟件運(yùn)行存貯空間和運(yùn)行時(shí)間的影響，基于相同的敏感器獲得同等的姿態(tài)確定精度，姿態(tài)確定算法越簡(jiǎn)單越適合于工程應(yīng)用；對(duì)于僅采用磁強(qiáng)計(jì)作為觀(guān)測(cè)量的算法由于其是基于軌道運(yùn)動(dòng)斷續(xù)獲取三軸姿態(tài)，在使用上有一定的局限性。本文是在文獻(xiàn)[8]的基礎(chǔ)上進(jìn)一步改進(jìn)，首先利用磁強(qiáng)計(jì)測(cè)得的當(dāng)前時(shí)刻磁場(chǎng)強(qiáng)度，與衛(wèi)星軌道和姿態(tài)角速率推算的當(dāng)前時(shí)刻磁場(chǎng)強(qiáng)度比較，估計(jì)陀螺積分姿態(tài)誤差；接著利用磁強(qiáng)計(jì)測(cè)得的前后時(shí)刻磁場(chǎng)強(qiáng)度，與基于衛(wèi)星軌道和姿態(tài)角速率推算的前后時(shí)刻磁場(chǎng)強(qiáng)度比較，估計(jì)姿態(tài)推算誤差；然后基于前后時(shí)刻磁場(chǎng)強(qiáng)度估計(jì)的姿態(tài)誤差校正陀螺積分姿態(tài)，并基于PI濾波估計(jì)陀螺漂移；最后基于當(dāng)前磁強(qiáng)計(jì)的一般測(cè)量精度并同時(shí)考慮常值誤差和噪聲進(jìn)行仿真驗(yàn)證。

1 軌道地磁場(chǎng)推算及變化規(guī)律

1.1 基于軌道推算的慣性系地磁場(chǎng)強(qiáng)度

基于軌道根數(shù)計(jì)算地固系磁場(chǎng)強(qiáng)度推算用軌道位置參數(shù)：

(1)

(2)

η=π/2-arcsin(sinu·sini)

(3)

其中：a為軌道半長(zhǎng)軸;e為軌道偏心率;f為真近點(diǎn)角;Ω為升交點(diǎn)赤經(jīng);u為軌道緯度幅角，i軌道傾角;Sg為格林尼治恒星時(shí)。

考慮星載計(jì)算機(jī)的計(jì)算能力，地磁場(chǎng)的球諧波模型取一階近似如下：

(4)

(5)

(6)

(7)

(8)

(9)

將地固系磁場(chǎng)強(qiáng)度轉(zhuǎn)換到慣性系：Bci=Aie·Bce；其中，Aie為地固系到慣性系轉(zhuǎn)換矩陣。

1.2 陀螺推算的本體系磁場(chǎng)強(qiáng)度

假定陀螺坐標(biāo)系與衛(wèi)星本體系一致，基于修正漂移后的陀螺角速率，求得前后節(jié)拍姿態(tài)變化四元數(shù)ΔQω：

(10)

Δqω=[(ωk-ωk-1)/2+ωk-1]·T/2

(11)

(12)

其中：ωk為陀螺輸出的當(dāng)前節(jié)拍本體系角速率，ωk-1為陀螺輸出的前一節(jié)拍本體系角速率。

基于前一節(jié)拍修正后的本體相對(duì)慣性系姿態(tài)四元數(shù)Qbi,k-1，推算當(dāng)前節(jié)拍本體相對(duì)慣性系姿態(tài)四元數(shù)Qbi,k/k-1：

1.3 磁場(chǎng)強(qiáng)度變化及規(guī)律

考慮星載計(jì)算機(jī)的計(jì)算能力，地磁場(chǎng)的球諧波模型取一階近似得到地固系下地磁場(chǎng)矢量Bce，近似如下：

(13)

By=0

(14)

(15)

衛(wèi)星從南緯最高緯度到北緯最高緯度升軌飛行，X軸和Z軸磁場(chǎng)強(qiáng)度都變化了1周，衛(wèi)星從北緯最高緯度到南緯最高緯度降軌飛行，X軸和Z軸磁場(chǎng)強(qiáng)度也都變化了1周。

姿態(tài)保持慣性定向，地磁場(chǎng)每軌平均以2倍的軌道角速率變化；姿態(tài)保持對(duì)地定向，姿態(tài)按軌道角速率變化，地磁場(chǎng)每軌平均以1倍的軌道角速率變化。地磁場(chǎng)在衛(wèi)星本體系指向隨衛(wèi)星軌道運(yùn)動(dòng)和姿態(tài)轉(zhuǎn)動(dòng)變化。

2 基于磁場(chǎng)強(qiáng)度估計(jì)姿態(tài)誤差

2.1 基于當(dāng)前時(shí)刻磁場(chǎng)強(qiáng)度估計(jì)姿態(tài)誤差

假定磁強(qiáng)計(jì)坐標(biāo)系與本體系一致，磁強(qiáng)計(jì)輸出的本體系磁場(chǎng)強(qiáng)度和基于軌道及陀螺推算的本體系磁場(chǎng)強(qiáng)度如圖1，求得姿態(tài)四元數(shù)誤差Δqe1，其矢量部分ΔQe1由式(16)計(jì)算得到：

圖1 基于當(dāng)前節(jié)拍磁場(chǎng)強(qiáng)度估計(jì)姿態(tài)誤差

(16)

其中：Bcb,k/k-1為基于軌道及姿態(tài)推算的磁場(chǎng)強(qiáng)度;Bmb,k為當(dāng)前節(jié)拍磁強(qiáng)計(jì)測(cè)得的磁場(chǎng)強(qiáng)度。

2.2 基于前后時(shí)刻磁場(chǎng)強(qiáng)度估計(jì)姿態(tài)誤差

磁強(qiáng)計(jì)測(cè)得的前后時(shí)刻磁場(chǎng)強(qiáng)度如圖2，求得前后節(jié)拍磁場(chǎng)強(qiáng)度等效的姿態(tài)變化四元數(shù)Δqm，其矢量部分ΔQm由式(17)計(jì)算得到：

(17)

其中：Bmb,k-1為前一節(jié)拍磁強(qiáng)計(jì)測(cè)得的磁場(chǎng)強(qiáng)度;Bmb,k為當(dāng)前節(jié)拍磁強(qiáng)計(jì)測(cè)得的磁場(chǎng)強(qiáng)度。

圖2 磁強(qiáng)計(jì)測(cè)得的前后節(jié)拍磁場(chǎng)強(qiáng)度

基于軌道位置推算的前后節(jié)拍磁場(chǎng)強(qiáng)度如圖3，求得前后節(jié)拍軌道位置變化導(dǎo)致的姿態(tài)變化四元數(shù)Δqr，其矢量部分ΔQr由式(18)計(jì)算得到：

圖3 軌道位置導(dǎo)致的前后節(jié)拍磁場(chǎng)強(qiáng)度變化

(18)

其中：Bce,k-1為基于軌道推算的前一節(jié)拍地固系磁場(chǎng)強(qiáng)度;Bce,k為基于軌道推算的當(dāng)前節(jié)拍地固系磁場(chǎng)強(qiáng)度。

軌道位置和姿態(tài)角速率導(dǎo)致的前后節(jié)拍姿態(tài)變化四元數(shù)ΔQc：

ΔQc=ΔQω?ΔQr

(19)

磁強(qiáng)計(jì)測(cè)得的前后節(jié)拍姿態(tài)變化由軌道位置、姿態(tài)運(yùn)動(dòng)和姿態(tài)推算誤差導(dǎo)致，求得姿態(tài)推算誤差四元數(shù)ΔQe2：

(20)

2.3 姿態(tài)四元數(shù)誤差修正

基于磁強(qiáng)計(jì)前后時(shí)刻磁場(chǎng)強(qiáng)計(jì)估計(jì)的姿態(tài)誤差四元數(shù)，求得姿態(tài)誤差四元數(shù)ΔQe：

ΔQe=ΔQe1?ΔQe2

(21)

考慮誤差為小量，簡(jiǎn)化為：

(22)

姿態(tài)四元數(shù)誤差修正量：

(23)

3 陀螺漂移估計(jì)

PI濾波估計(jì)陀螺漂移如下：

(24)

其中：kp為比例系數(shù);ki為積分系數(shù)。

陀螺角速率漂移修正：

ωbi,k=ωbi-Δω

(25)

其中：ωbi為陀螺當(dāng)前節(jié)拍測(cè)量值。

4 算例仿真驗(yàn)證

磁強(qiáng)計(jì)三軸常值偏差100nT，噪聲100nT；陀螺三軸常值漂移分別為0.008(°)/s、0.004(°)/s、-0.006(°)/s，三軸噪聲0.001(°)/s；三軸初始姿態(tài)誤差都為5°。姿態(tài)角測(cè)量誤差如圖4和圖5所示，陀螺漂移估計(jì)如圖6和圖7所示。

圖4 姿態(tài)角測(cè)量誤差曲線(xiàn)

圖5 姿態(tài)角測(cè)量誤差曲線(xiàn)末端放大曲線(xiàn)

圖6 陀螺漂移估計(jì)曲線(xiàn)

圖7 陀螺漂移估計(jì)放大曲線(xiàn)

磁強(qiáng)計(jì)常值偏差1000nT，噪聲1000nT，噪聲增大濾波作用要相應(yīng)加強(qiáng)；三軸初始姿態(tài)誤差分別為50°、50°、50°，磁強(qiáng)計(jì)增大測(cè)量誤差在初始大姿態(tài)誤差下仍能收斂，如圖8和圖9所示。

圖8 姿態(tài)角測(cè)量誤差曲線(xiàn)

圖9 姿態(tài)角測(cè)量誤差曲線(xiàn)末端放大曲線(xiàn)

5 結(jié)束語(yǔ)

利用磁強(qiáng)計(jì)測(cè)得的前后時(shí)刻磁場(chǎng)強(qiáng)度，與衛(wèi)星軌道和姿態(tài)角速率推算的磁場(chǎng)強(qiáng)度比較，估計(jì)陀螺積分姿態(tài)誤差；基于磁強(qiáng)計(jì)估計(jì)的姿態(tài)誤差校正陀螺積分姿態(tài)，并基于PI濾波估計(jì)陀螺漂移，設(shè)計(jì)了適合于星上應(yīng)用的陀螺與磁強(qiáng)計(jì)組合定姿算法。仿真試驗(yàn)結(jié)果表明姿態(tài)確定精度1°左右，適合在納星、皮星等微小衛(wèi)星上應(yīng)用。