陀螺與星敏感器組合定姿及陀螺漂移估計(jì)

王獻(xiàn)忠 張 肖

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

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

基于星敏感器感器估計(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)行實(shí)時(shí)補(bǔ)償；文獻(xiàn)[3]和[4]利用擴(kuò)展卡爾曼濾波進(jìn)行陀螺與星敏感器組合定姿研究；文獻(xiàn)[5]基于擴(kuò)展卡爾曼濾波進(jìn)行星敏感器與陀螺組合定姿態(tài)研究，采用離線計(jì)算次優(yōu)增益矩陣的方法減少計(jì)算量。

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

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

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

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

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

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

(1)

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

(2)

由姿態(tài)運(yùn)動學(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，則星體的真實(shí)角速率可表示為：

(10)

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

(11)

由上式得：

(12)

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

(13)

(14)

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

(15)

1.3 基于PI濾波估計(jì)陀螺角速率漂移

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

(16)

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

(17)

(18)

(19)

其中,kpx，kpy和kpz為三軸PI濾波估計(jì)比例系數(shù)，kix，kiy和kiz為三軸PI濾波估計(jì)積分系數(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

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

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

基于式(9)求得的扣除陀螺漂移的角速率，在J2000慣性系下直接積分時(shí)在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í)差及數(shù)據(jù)采集延時(shí)修正后，扣除安裝矩陣，得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時(shí)，φ=arcsin(a23)，θ=0ψ=arctan(a31/-a32)

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

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

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

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

3.2 仿真驗(yàn)證

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

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

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

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

4 結(jié)束語

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