陀螺與磁強計組合定姿及漂移估計算法優化

王獻忠 張 肖 劉 艷

1.上海市空間智能控制技術重點實驗室, 上海 201109 2.上海航天技術研究院, 上海 201109 3.上海航天控制技術研究所, 上海 201109

0 引言

低軌衛星在軌運行期間通過三軸磁強計測得的地磁場矢量與應用國際地磁場模型(IGRF)計算得到的地磁場矢量比較估計衛星姿態，在某一時刻單獨利用磁強計只能確定二軸姿態，要確定三軸姿態至少需要具有一定夾角的雙矢量觀測，如與太陽敏感器測得的太陽矢量、地平儀測得的地心矢量等組合定姿。

單獨利用磁強計不能連續確定三軸姿態，但可以基于軌道運動斷續獲取三軸姿態，單獨利用磁強計定姿在應用上有一定的局限性，通常將磁強計與陀螺組合進行定姿。

隨著微電子、微機械等新技術的發展，航天產品越來越小型化，如具備三軸角速率測量功能的硅微陀螺只有幾十克，具備三軸磁場強度測量功能的磁強計甚至更輕，這些小型化的產品在小衛星上得到了廣泛應用，出現了納星、皮星等微小衛星。

對陀螺測得的三軸姿態角速率積分也可以確定衛星姿態，但其測量精度受陀螺角速率漂移影響，只能用于短期姿態確定。將陀螺與磁強計組合，利用磁強計的測量值估算和修正陀螺角速率漂移，同時利用陀螺彌補磁強計定姿實時性差的缺點。

文獻[1]基于陀螺、太陽敏感器、磁強計和星敏感器4種敏感器，研究了微小衛星在速率阻尼模式、太陽捕獲模式、對日對地定向及維持模式、試驗模式下的系統建模方法,以及基于Unscented卡爾曼濾波(UKF)的組合定姿方法。文獻[2]基于衛星運動學建立狀態方程,應用EKF進行陀螺與磁強計組合定姿研究。文獻[3]基于低成本低精度的MEMS陀螺、太陽敏感器、磁強計組合，設計了Unscented Kalman融合濾波算法。文獻[4-5]僅利用三軸磁強計作為測量設備，分別通過EKF和UKF處理測量數據,實時地估計無陀螺小衛星的姿態角與角速度。文獻[6]提出了一種利用梯度下降法對加速度計和磁強計的數據進行姿態解算，并采用互補濾波算法將其與陀螺儀積分得到的結果進行融合的定姿方法。文獻[7]基于四元數的卡爾曼濾波進行陀螺與磁強計組合定姿。

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

1 軌道地磁場推算及變化規律

1.1 基于軌道推算的慣性系地磁場強度

基于軌道根數計算地固系磁場強度推算用軌道位置參數：

(1)

(2)

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

(3)

其中：a為軌道半長軸;e為軌道偏心率;f為真近點角;Ω為升交點赤經;u為軌道緯度幅角，i軌道傾角;Sg為格林尼治恒星時。

考慮星載計算機的計算能力，地磁場的球諧波模型取一階近似如下：

(4)

(5)

(6)

(7)

(8)

(9)

將地固系磁場強度轉換到慣性系：Bci=Aie·Bce；其中，Aie為地固系到慣性系轉換矩陣。

1.2 陀螺推算的本體系磁場強度

假定陀螺坐標系與衛星本體系一致，基于修正漂移后的陀螺角速率，求得前后節拍姿態變化四元數ΔQω：

(10)

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

(11)

(12)

其中：ωk為陀螺輸出的當前節拍本體系角速率，ωk-1為陀螺輸出的前一節拍本體系角速率。

基于前一節拍修正后的本體相對慣性系姿態四元數Qbi,k-1，推算當前節拍本體相對慣性系姿態四元數Qbi,k/k-1：

1.3 磁場強度變化及規律

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

(13)

By=0

(14)

(15)

衛星從南緯最高緯度到北緯最高緯度升軌飛行，X軸和Z軸磁場強度都變化了1周，衛星從北緯最高緯度到南緯最高緯度降軌飛行，X軸和Z軸磁場強度也都變化了1周。

姿態保持慣性定向，地磁場每軌平均以2倍的軌道角速率變化；姿態保持對地定向，姿態按軌道角速率變化，地磁場每軌平均以1倍的軌道角速率變化。地磁場在衛星本體系指向隨衛星軌道運動和姿態轉動變化。

2 基于磁場強度估計姿態誤差

2.1 基于當前時刻磁場強度估計姿態誤差

假定磁強計坐標系與本體系一致，磁強計輸出的本體系磁場強度和基于軌道及陀螺推算的本體系磁場強度如圖1，求得姿態四元數誤差Δqe1，其矢量部分ΔQe1由式(16)計算得到：

圖1 基于當前節拍磁場強度估計姿態誤差

(16)

其中：Bcb,k/k-1為基于軌道及姿態推算的磁場強度;Bmb,k為當前節拍磁強計測得的磁場強度。

2.2 基于前后時刻磁場強度估計姿態誤差

磁強計測得的前后時刻磁場強度如圖2，求得前后節拍磁場強度等效的姿態變化四元數Δqm，其矢量部分ΔQm由式(17)計算得到：

(17)

其中：Bmb,k-1為前一節拍磁強計測得的磁場強度;Bmb,k為當前節拍磁強計測得的磁場強度。

圖2 磁強計測得的前后節拍磁場強度

基于軌道位置推算的前后節拍磁場強度如圖3，求得前后節拍軌道位置變化導致的姿態變化四元數Δqr，其矢量部分ΔQr由式(18)計算得到：

圖3 軌道位置導致的前后節拍磁場強度變化

(18)

其中：Bce,k-1為基于軌道推算的前一節拍地固系磁場強度;Bce,k為基于軌道推算的當前節拍地固系磁場強度。

軌道位置和姿態角速率導致的前后節拍姿態變化四元數ΔQc：

ΔQc=ΔQω?ΔQr

(19)

磁強計測得的前后節拍姿態變化由軌道位置、姿態運動和姿態推算誤差導致，求得姿態推算誤差四元數ΔQe2：

(20)

2.3 姿態四元數誤差修正

基于磁強計前后時刻磁場強計估計的姿態誤差四元數，求得姿態誤差四元數ΔQe：

ΔQe=ΔQe1?ΔQe2

(21)

考慮誤差為小量，簡化為：

(22)

姿態四元數誤差修正量：

(23)

3 陀螺漂移估計

PI濾波估計陀螺漂移如下：

(24)

其中：kp為比例系數;ki為積分系數。

陀螺角速率漂移修正：

ωbi,k=ωbi-Δω

(25)

其中：ωbi為陀螺當前節拍測量值。

4 算例仿真驗證

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

圖4 姿態角測量誤差曲線

圖5 姿態角測量誤差曲線末端放大曲線

圖6 陀螺漂移估計曲線

圖7 陀螺漂移估計放大曲線

磁強計常值偏差1000nT，噪聲1000nT，噪聲增大濾波作用要相應加強；三軸初始姿態誤差分別為50°、50°、50°，磁強計增大測量誤差在初始大姿態誤差下仍能收斂，如圖8和圖9所示。

圖8 姿態角測量誤差曲線

圖9 姿態角測量誤差曲線末端放大曲線

5 結束語

利用磁強計測得的前后時刻磁場強度，與衛星軌道和姿態角速率推算的磁場強度比較，估計陀螺積分姿態誤差；基于磁強計估計的姿態誤差校正陀螺積分姿態，并基于PI濾波估計陀螺漂移，設計了適合于星上應用的陀螺與磁強計組合定姿算法。仿真試驗結果表明姿態確定精度1°左右，適合在納星、皮星等微小衛星上應用。