王獻(xiàn)忠 張 肖

1.上海航天技術(shù)研究院，上海 201109 2.上海航天控制技術(shù)研究所，上海 201109 3.空間智能控制技術(shù)重點實驗室，上海 201109

陀螺能夠連續(xù)輸出衛(wèi)星相對慣性空間的角速率，對陀螺角速率積分可以確定衛(wèi)星姿態(tài)，但陀螺存在漂移，不能長時間使用；星敏感器能夠直接提供衛(wèi)星姿態(tài)，但可能受月亮、地球和太陽等干擾不能連續(xù)輸出姿態(tài)，特別是衛(wèi)星姿態(tài)機(jī)動期間。

基于星敏感器感器估計陀螺漂移，對陀螺漂移修正后積分輸出衛(wèi)星姿態(tài)，既可以確保姿態(tài)輸出的連續(xù)性，又可以抑制星敏感器觀測噪聲對姿態(tài)確定精度的影響。

一般基于卡爾曼濾波(Kalman)及其擴(kuò)展型進(jìn)行星敏感器與陀螺組合定姿研究，參考文獻(xiàn)[1]推導(dǎo)了星敏感器觀測矩陣及誤差四元數(shù)狀態(tài)方程，使用卡爾曼濾波方法進(jìn)行星敏感器與陀螺組合定姿研究；文獻(xiàn)[2]基于擴(kuò)展卡爾曼濾波進(jìn)行星敏感器與陀螺組合定姿研究，并對陀螺漂移進(jìn)行實時補(bǔ)償；文獻(xiàn)[3]和[4]利用擴(kuò)展卡爾曼濾波進(jìn)行陀螺與星敏感器組合定姿研究；文獻(xiàn)[5]基于擴(kuò)展卡爾曼濾波進(jìn)行星敏感器與陀螺組合定姿態(tài)研究，采用離線計算次優(yōu)增益矩陣的方法減少計算量。

也有基于其它濾波算法進(jìn)行星敏感器與陀螺組合定姿研究，文獻(xiàn)[6]基于改進(jìn)輔助邊緣粒子濾波(IAMPF) 進(jìn)行星敏感器與陀螺組合定姿研究，并與UKF進(jìn)行比對；文獻(xiàn)[7]推導(dǎo)了一種能在線估計過程噪聲和量測噪聲協(xié)方差陣的自適應(yīng)采樣濾波算法，該算法不僅能對姿態(tài)確定系統(tǒng)的非線性誤差進(jìn)行高精度估計，而且估計結(jié)果具有較強(qiáng)的魯棒性；文獻(xiàn)[8]將慣導(dǎo)確定的載體相對慣性空間的四元數(shù)姿態(tài)信息與星敏感器輸出的高精度四元數(shù)姿態(tài)進(jìn)行信息融合，實時估計出陀螺漂移及失準(zhǔn)角，并對慣導(dǎo)進(jìn)行在線修正。

基于PI濾波估計陀螺角速率漂移進(jìn)行星敏感器與陀螺組合定姿算法研究。首先推導(dǎo)了陀螺角速率漂移與姿態(tài)偏差四元數(shù)之間的關(guān)系，在穩(wěn)態(tài)小姿態(tài)角變化和小角速率條件下陀螺角速率漂移與姿態(tài)偏差四元數(shù)滿足線性關(guān)系，并基于姿態(tài)偏差四元數(shù)和PI濾波估計陀螺角速率漂移；接著給出了分別基于J2000慣性系和軌道坐標(biāo)系的星敏感器與陀螺組合定姿算法和姿態(tài)偏差四元數(shù)求解算法；最后進(jìn)行了工程應(yīng)用方式分析和仿真驗證。

1 基于偏差四元數(shù)估計陀螺漂移

1.1 偏差四元數(shù)導(dǎo)數(shù)與陀螺角速率關(guān)系

本文中2個四元數(shù)p和q的乘法定義為：

定義姿態(tài)偏差四元數(shù)如下：

(1)

對式(1)求導(dǎo)可以得到：

(2)

由姿態(tài)運動學(xué)方程：

(3)

(4)

將式(3)和(4)代入式(2)并推導(dǎo)整理得到：

(5)

考慮衛(wèi)星姿態(tài)偏差為小角度：

(6)

將上式代入式(5)，并整理得：

(7)

其中,

1.2 推導(dǎo)偏差四元數(shù)與陀螺角速率漂移關(guān)系

(8)

(9)

設(shè)陀螺量測噪聲為ng，則星體的真實角速率可表示為：

(10)

將式(10)代入式(7)，并考慮陀螺漂移估計誤差Δb和陀螺量測噪聲ng為小量，忽略高階小量后可得：

(11)

由上式得：

(12)

設(shè)ng為白噪聲，由式(8)和(12)得：

(13)

(14)

式(14)積分項為三軸之間的耦合項，穩(wěn)態(tài)控制時角速率和姿態(tài)誤差都為小量，忽略高階小量后可得：

(15)

1.3 基于PI濾波估計陀螺角速率漂移

由式(15)可知陀螺漂移和姿態(tài)偏差四元數(shù)之間存在線性關(guān)系，基于PI濾波估計陀螺漂移如下：

(16)

其中,kp為PI濾波估計比例系數(shù);ki為PI濾波估計積分系數(shù)；kp和ki為3×3對角陣，可以獨立估計陀螺三軸角速率漂移。

(17)

(18)

(19)

其中,kpx，kpy和kpz為三軸PI濾波估計比例系數(shù)，kix，kiy和kiz為三軸PI濾波估計積分系數(shù)。

2 星敏感器與陀螺組合定姿

星敏感器與陀螺組合定姿可基于慣性坐標(biāo)系或軌道坐標(biāo)系。

2.1 陀螺積分四元數(shù)

應(yīng)用簡化積分算法求得四元數(shù)增量dq如下：

(20)

J2000慣性系或軌道系下積分四元數(shù)：

qbi/o,k/k-1=qbi/o,k-1?dq

其中，慣性系下組合定姿時qbi/o,k-1即為qbi,k-1，為第k-1步基于星敏感器修正后慣性系下陀螺積分姿態(tài)四元數(shù)；軌道系下組合定姿時qbi/o,k-1即為qbo,k-1，為第k-1步基于星敏感器修正后軌道系下陀螺積分姿態(tài)四元數(shù)。

2.1.1 基于慣性坐標(biāo)系組合定姿

基于式(9)求得的扣除陀螺漂移的角速率，在J2000慣性系下直接積分時在J2000慣性系下直接積分。

2.1.2 基于軌道坐標(biāo)系組合定姿

基于式(13)求得的扣除陀螺漂移的角速率，再扣除軌道角速率ωoi，在軌道系積分。

2.2 陀螺積分姿態(tài)四元數(shù)修正

基于星敏感器修正陀螺積分姿態(tài)四元數(shù)qbi,k如下：

(21)

其中，dQe表示偏差四元數(shù)dqe的向量部分，0≤ke≤1為星敏感器修正陀螺積分姿態(tài)四元數(shù)強(qiáng)度系數(shù)。

2.2.1 基于慣性坐標(biāo)系組合定姿

設(shè)星敏感器考慮曝光時差及數(shù)據(jù)采集延時修正后，扣除安裝矩陣，得J2000慣性系下姿態(tài)四元數(shù)為qbi,s，求得姿態(tài)偏差四元數(shù)如下：

(22)

星體相對軌道系姿態(tài)四元數(shù)qbo,k如下：

qbo,k=qio,k?qbi,k

其中：qio,k為由軌道參數(shù)求得的慣性系相對軌道系姿態(tài)四元數(shù)。

2.2.2 基于軌道坐標(biāo)系組合定姿

基于星敏感器求得相對軌道系姿態(tài)四元數(shù)如下：

(23)

得姿態(tài)偏差四元數(shù)dqe如下：

(24)

2.3 求相對軌道系姿態(tài)角

(25)

求得姿態(tài)角為：

若a23>0.9999時，φ=arcsin(a23)，θ=0ψ=arctan(a31/-a32)

3 工程應(yīng)用方式及仿真驗證

3.1 工程應(yīng)用方式

姿態(tài)偏差四元數(shù)用于估計陀螺角速率漂移，陀螺角速率測量值扣除陀螺角速率漂移后進(jìn)行角速率積分和姿態(tài)求解，基于陀螺積分求得的姿態(tài)用于控制，星敏感器姿態(tài)用于估計陀螺漂移和姿態(tài)修正，有利于抑制星敏感器觀測噪聲和消除長時間積分累積誤差。

基于J2000慣性系直接積分不受軌道角速率估計誤差影響；軌道角速率估計誤差會影響基于軌道坐標(biāo)系角速率積分，軌道角速率估計誤差導(dǎo)致陀螺角速率漂移估計值波動，陀螺角速率漂移估計值不能真實反映陀螺漂移狀態(tài)，不利于地面上注陀螺漂移修正量。

3.2 仿真驗證

陀螺積分三軸初始姿態(tài)誤差為-5°，陀螺三軸角速率常值漂移分別為0.005(°)/s,0.008(°)/s和-0.005(°)/s，隨機(jī)漂移0.0005(°)/s。陀螺角速率漂移估計仿真結(jié)果如圖1所示，星敏感器與陀螺組合定姿姿態(tài)角誤差仿真結(jié)果如圖2所示。

圖1 陀螺角速度漂移估計及局部放大曲線

圖2 陀螺與星敏感器組合定姿姿態(tài)誤差及局部放大曲線

從仿真結(jié)果可以看出：陀螺的常值漂移被準(zhǔn)確估計，漂移估計受隨機(jī)漂移影響波動，波動幅度不超過加入的隨機(jī)漂移幅度； 陀螺積分初始姿態(tài)誤差通過自主調(diào)整陀螺角速率漂移消除，PI濾波穩(wěn)定后姿態(tài)誤差小于 0.005°。

4 結(jié)束語

基于PI濾波估計陀螺角速率漂移，在此基礎(chǔ)上進(jìn)行星敏感器與陀螺組合定姿算法研究，仿真驗證了組合定姿算法的有效性。本文基于PI濾波估計陀螺漂移的星敏感器與陀螺組合定姿算法簡單，易于工程實現(xiàn)，并已通過在軌考核驗證。