基于PI濾波估計加表漂移的兼容機與加表組合導航

王獻忠 張 肖 張麗敏 施常勇

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

加表能夠連續(xù)輸出非慣性力產(chǎn)生的加速度，基于加表慣導可以確定衛(wèi)星在慣性空間的位置和速度，但加表存在漂移，不能長時間使用；兼容機能夠直接提供衛(wèi)星在慣性空間的位置和速度，但兼容機可能受單粒子翻轉、姿態(tài)機動或翻滾時天線被地球遮擋、地面局部區(qū)域人為干擾等影響，不能連續(xù)輸出衛(wèi)星在慣性空間的位置和速度，特別是在高能粒子導致兼容機復位或重加載期間。

基于兼容機估計加表漂移，對加表漂移修正后慣導解算輸出衛(wèi)星在慣性空間的位置和速度，既可以確保位置和速度輸出的連續(xù)性，又可以抑制兼容機觀測噪聲對組合導航精度的影響。

國內外學者對SINS/GPS組合導航進行了廣泛的研究，文獻[1]介紹了INS/CNS/兼容機組合導航的信息融合和濾波方法，分析了INS/CNS/兼容機組合導航技術的發(fā)展和應用現(xiàn)狀。眾多學者基于卡爾曼濾波及擴展型進行SINS/GPS組合導航研究，文獻[2]基于EKF進行SINS/兼容機深組合導航應用研究；文獻[3] 基于卡爾曼濾波進行運載火箭SINS/兼容機組合導航研究；文獻[4] 基于集中式卡爾曼濾波結構進行提高SINS/兼容機組合導航定位精度方法研究；文獻[5]基于UKF進行MIMU/GPS/DVS組合導航方法研究。也有一些學者基于新型濾波方法進行SINS/GPS組合導航濾波算法研究，文獻[6]基于小波技術進行GPS/SINS組合導航方法研究；文獻[7]進行改進高斯粒子濾波算法及其在SINS/GPS深組合導航系統(tǒng)中的應用研究。

本文首先推導了基于兼容機位置/速度誤差估計加表加速度漂移的PI濾波方法；接著給出了兼容機與加表組合導航算法，利用估計得到的加速度漂移分別對位置和速度進行修正；其次進行了工程應用分析，在無法獲取高精度速度測量信息時僅利用兼容機位置對加速度漂移進行估計；最后針對僅用兼容機速度信息作為觀測量的濾波方法進行仿真驗證，仿真結果表明采用僅基于兼容機位置信息作為觀測量的PI濾波組合導航能準確估計加速度漂移，方法簡單有效，可應用于工程實踐。

1 基于J2000慣性系的衛(wèi)星慣導解算算法

(1)

(2)

(3)

設加表坐標系到J2000慣性系的姿態(tài)轉換矩陣為Aia，求得J2000慣性系下衛(wèi)星非慣性加速度aa,i：

aa,i=Aia·aa,a

(4)

其中：aa,a為加表測得的加表坐標系下的加速度。

ai=ag,i+aa,i

(5)

在J2000慣性系進行慣導解算位置ri和速度vi：

(6)

(7)

2 兼容機與加表PI濾波組合導航

2.1 基于兼容機修正慣導誤差和估計加表漂移

兼容機由于受自然遮擋或人為干擾影響，尤其是高軌兼容機由于靈敏度原因，收到導航星數(shù)量有限，導致兼容機不一定能連續(xù)輸出位置信息；加表加速度存在漂移，基于加表的純慣導不能長時間使用；因此可以用兼容機信息修正慣導位置/速度誤差，并估計加表漂移。

設加表加速度漂移轉換到J2000系為dai，根據(jù)式(6)和(7)慣導解算算法求得加表漂移引起的位置/速度誤差dvi/dri：

(8)

(9)

設慣導第k步解算輸出的位置/速度為ri,k/vi,k，兼容機經(jīng)時間對齊后輸出的位置/速度為ri,G/vi,G，求得慣導解算位置/速度誤差Δri,k/Δvi,k：

Δri,k=ri,k-ri,G

(10)

Δvi,k=vi,k-vi,G

(11)

基于兼容機求得的慣導解算位置/速度誤差Δri,k/Δvi,k，結合式(6)和(7)慣導解算積分過程，在慣導積分過程中逐步增加位置/速度誤差修正量，可以確保誤差修正的平穩(wěn)性。第k步慣導解算位置/速度誤差修正量dri,k/dvi,k：

dri,k=kp,r·Δri,k

(12)

dvi,k=kp,v·Δvi,k

(13)

其中：kp,r和kp,v為位置/速度誤差修正量估計比例系數(shù)，kp,r和kp,v為3×3對角陣，三軸可以獨立估計位置/速度誤差修正量。

將兼容機求得的慣導解算速度誤差Δvi,k轉換到加表坐標系：

Δva,k=Aai·Δvi,k

(14)

其中：Aai為J2000慣性系到加表坐標系的姿態(tài)轉換矩陣。

基于PI濾波估計加表加速度漂移算法如下：

(15)

其中：daa,k為第k步估計的加表漂移；kp,a為PI濾波估計比例系數(shù)；ki,a為PI濾波估計積分系數(shù)，kp,a和ki,a為3×3對角陣，三軸可以獨立估計加速度漂移。

2.2 兼容機與加表組合導航

考慮到兼容機輸出信息可能不連續(xù)，組合導航的輸出并不直接采用兼容機信息，而是基于加速度計測量得到的加速度，扣除采用兼容機估計得到的加速度常值漂移后積分得到慣性系位置、速度，既保證長時高精度，又能保證數(shù)據(jù)的連續(xù)性。

設加表坐標系到J2000慣性系的姿態(tài)轉換矩陣為Aia；aa,a為加表測得的加表坐標系下的加速度；加表加速度漂移估計為daa；求得J2000慣性系下扣除加表漂移的衛(wèi)星非慣性加速度aai：

aa,i=Aia·aa,a-daa

(16)

3 工程應用

兼容機基于偽距測量濾波估計位置和速度，其輸出的速度相當于對位置微分，速度精度遠低于位置精度，且觀測噪聲較大；為防止低精度的速度觀測量影響組合導航精度，且考慮到位置誤差也反映速度誤差，位置誤差修正量可以估計速度誤差修正量，因此工程應用時可以僅基于兼容機輸出的位置與加表進行組合導航，并對加表加速度漂移進行估計。

(17)

其中：kp,v為速度誤差修正量估計比例系數(shù)；kp,v為3×3對角陣，三軸可以獨立估計速度誤差修正量。

將速度誤差修正量dvi,k轉換到加表坐標系：

dva,k=Aai·dvi,k

(18)

其中：Aai為J2000慣性系到加表坐標系的姿態(tài)轉換矩陣。

(19)

其中：daa,k為第k步估計的加表漂移;kp,a為PI濾波估計比例系數(shù);ki,a為PI濾波估計積分系數(shù)；kp,a和ki,a為3×3對角陣，三軸可以獨立估計加速度漂移。

求得J2000慣性系下扣除加表漂移的衛(wèi)星非慣性加速度aai如式(20)：

aa,i=Aia·(aa,a-daa)

(20)

采用式(5)求解J2000慣性系下衛(wèi)星加速度ai，應用簡化積分算法進行慣導解算如下：

vi,k=vi,k-1+[ai,k-1+(ai,k-ai,k-1)/2]·
T-dvi,k-1

(21)

ri,k=ri,k-1+[vi,k-1+(vi,k-vi,k-1)/2]·
T-dri,k-1

(22)

其中：ai,k-1為第k-1步扣除加表漂移的加速度；ai,k為第k步扣除加表漂移的加速度；dvi,k-1為第k-1步估計的速度誤差修正量；vi,k-1為第k-1步扣除速度誤差修正量的速度，vi,k為第k步扣除速度誤差修正量的速度；dri,k-1為第k-1步估計的位置誤差修正量；ri,k-1為第k-1步扣除位置誤差修正量的位置；ri,k為第k步扣除位置誤差修正量的位置；T為導航周期。

4 仿真驗證

加表三軸加速度常值漂移分別為0.005m/s2、0.008m/s2和-0.005m/s2；隨機漂移0.0005m/s2；兼容機三軸位置常值偏差分別為5m、10m和15m，速度誤差小于0.5m/s(3σ)。

對兼容機與加表組合導航算法進行仿真驗證，并分別對僅用兼容機輸出的位置作為觀測量，同時采用兼容機輸出的位置/速度作為觀測量進行對比仿真。

1)僅基于兼容機位置信息作為觀測量,如圖1～3；

2)同時基于兼容機位置/速度作為觀測信息,如圖4～6。

從仿真結果可以看出，基于兼容機位置信息作為觀測量，或基于兼容機位置/速度信息作為觀測量均可以有效估計加表漂移；其中基于位置信息作為觀測量時，位置估計誤差波動優(yōu)于0.1 m，速度估計精度優(yōu)于0.005 m/s；同時基于位置/速度信息作為觀測量時，位置估計誤差波動優(yōu)于0.5 m，速度估計精度優(yōu)于0.05 m。這是由于兼容機速度測量精度較低，其在引入系統(tǒng)濾波時影響了位置估計精度。

圖1 基于兼容機位置觀測信息加表漂移估計值

圖2 基于位置觀測信息組合導航位置誤差曲線

圖3 基于位置觀測信息組合導航速度誤差曲線

圖4 基于兼容機位置/速度觀測信息加表漂移估計值

圖5 基于位置/速度觀測信息組合導航位置誤差曲線

圖6 基于位置/速度觀測信息組合導航速度誤差曲線

5 結論

基于PI濾波估計加表加速度漂移，在此基礎上進行兼容機與加表組合導航算法研究，尤其針對兼容機輸出速度精度不高的問題進行了僅基于兼容機位置信息作為觀測量，對加表加速度漂移進行估計。仿真驗證表明該算法有效，組合導航位置、速度估計精度均優(yōu)于同時引入兼容機速度信息作為觀測量的情況。針對目前兼容機速度測量精度不高的情況，工程應用時可以僅采用兼容機位置信息作為觀測量與加表進行組合導航。

基于PI濾波估計加表漂移的兼容機與加表組合導航算法簡單有效，易于工程實現(xiàn)，并已通過在軌考核驗證。