王獻(xiàn)忠 張 肖 張麗敏 施常勇

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

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

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

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

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

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

(1)

(2)

(3)

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

aa,i=Aia·aa,a

(4)

其中：aa,a為加表測(cè)得的加表坐標(biāo)系下的加速度。

ai=ag,i+aa,i

(5)

在J2000慣性系進(jìn)行慣導(dǎo)解算位置ri和速度vi：

(6)

(7)

2 兼容機(jī)與加表PI濾波組合導(dǎo)航

2.1 基于兼容機(jī)修正慣導(dǎo)誤差和估計(jì)加表漂移

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

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

(8)

(9)

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

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

(10)

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

(11)

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

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

(12)

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

(13)

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

將兼容機(jī)求得的慣導(dǎo)解算速度誤差Δvi,k轉(zhuǎn)換到加表坐標(biāo)系：

Δva,k=Aai·Δvi,k

(14)

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

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

(15)

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

2.2 兼容機(jī)與加表組合導(dǎo)航

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

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

aa,i=Aia·aa,a-daa

(16)

3 工程應(yīng)用

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

(17)

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

將速度誤差修正量dvi,k轉(zhuǎn)換到加表坐標(biāo)系：

dva,k=Aai·dvi,k

(18)

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

(19)

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

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

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

(20)

采用式(5)求解J2000慣性系下衛(wèi)星加速度ai，應(yīng)用簡(jiǎn)化積分算法進(jìn)行慣導(dǎo)解算如下：

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步估計(jì)的速度誤差修正量；vi,k-1為第k-1步扣除速度誤差修正量的速度，vi,k為第k步扣除速度誤差修正量的速度；dri,k-1為第k-1步估計(jì)的位置誤差修正量；ri,k-1為第k-1步扣除位置誤差修正量的位置；ri,k為第k步扣除位置誤差修正量的位置；T為導(dǎo)航周期。

4 仿真驗(yàn)證

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

對(duì)兼容機(jī)與加表組合導(dǎo)航算法進(jìn)行仿真驗(yàn)證，并分別對(duì)僅用兼容機(jī)輸出的位置作為觀測(cè)量，同時(shí)采用兼容機(jī)輸出的位置/速度作為觀測(cè)量進(jìn)行對(duì)比仿真。

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

2)同時(shí)基于兼容機(jī)位置/速度作為觀測(cè)信息,如圖4～6。

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

圖1 基于兼容機(jī)位置觀測(cè)信息加表漂移估計(jì)值

圖2 基于位置觀測(cè)信息組合導(dǎo)航位置誤差曲線

圖3 基于位置觀測(cè)信息組合導(dǎo)航速度誤差曲線

圖4 基于兼容機(jī)位置/速度觀測(cè)信息加表漂移估計(jì)值

圖5 基于位置/速度觀測(cè)信息組合導(dǎo)航位置誤差曲線

圖6 基于位置/速度觀測(cè)信息組合導(dǎo)航速度誤差曲線

5 結(jié)論

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

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