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

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

0 引言

加表能夠連續(xù)輸出非慣性力產(chǎn)生的加速度，基于加表慣導(dǎo)可以確定衛(wèi)星在慣性空間的位置和速度，但加表存在漂移，不能長(zhǎng)時(shí)間使用；高軌兼容機(jī)可能受地球遮擋和探測(cè)靈敏度影響，接收的導(dǎo)航星數(shù)有限，不能連續(xù)輸出衛(wèi)星在慣性空間的位置和速度。

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

國(guó)內(nèi)外學(xué)者對(duì)SINS/GNSS、INS/GNSS組合導(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/GNSS、INS/GNSS組合導(dǎo)航濾波算法研究，文獻(xiàn)[2]基于偽距/偽距率與SINS組合導(dǎo)航，利用衛(wèi)星系統(tǒng)的星歷數(shù)據(jù)與SINS給出的位置/速度計(jì)算偽距及偽距率，再與接收機(jī)輸出的偽距與偽距率作差獲得測(cè)量值，通過卡爾曼濾波估計(jì)導(dǎo)航誤差；文獻(xiàn)[3]基于偽距/偽距率與SINS組合導(dǎo)航，對(duì)UKF與卡爾曼濾波及自適應(yīng)濾波方法進(jìn)行了比較；文獻(xiàn)[4]基于偽距/偽距率與SINS組合導(dǎo)航，通過卡爾曼濾波提高組合導(dǎo)航精度；文獻(xiàn)[5]基于偽距/偽距率與INS組合導(dǎo)航，通過漸消因子的自適應(yīng)卡爾曼濾波提高組合導(dǎo)航精度；文獻(xiàn)[6]基于偽距與INS組合導(dǎo)航，采用間接的反饋校正設(shè)置，通過廣義卡爾曼濾波提高組合導(dǎo)航精度；文獻(xiàn)[7]基于EKF進(jìn)行SINS/兼容機(jī)深組合導(dǎo)航的應(yīng)用研究；文獻(xiàn)[8]基于卡爾曼濾波進(jìn)行運(yùn)載火箭SINS/兼容機(jī)組合導(dǎo)航的研究；文獻(xiàn)[9]基于集中式卡爾曼濾波結(jié)構(gòu)進(jìn)行提高SINS/兼容機(jī)組合導(dǎo)航定位精度方法的研究；文獻(xiàn)[10]基于UKF進(jìn)行MIMU/GPS/DVS組合導(dǎo)航方法的研究。也有一些學(xué)者基于新型濾波方法進(jìn)行SINS/GPS組合導(dǎo)航濾波算法的研究；文獻(xiàn)[11]進(jìn)行改進(jìn)高斯粒子濾波算法及其在SINS/GPS深組合導(dǎo)航系統(tǒng)中的應(yīng)用研究。

本文首先推導(dǎo)了基于兼容機(jī)偽距及偽距率估計(jì)位置和速度誤差算法；接著給出了基于兼容機(jī)偽距信息作為觀測(cè)量的組合導(dǎo)航和加表漂移估計(jì)算法；最后針對(duì)基于兼容機(jī)偽距組合導(dǎo)航輸出速度精度不高的問題，進(jìn)行了基于兼容機(jī)偽距及偽距率信息作為觀測(cè)量的組合導(dǎo)航，提高了速度精度。仿真驗(yàn)證表明算法有效，兼容機(jī)接收到2顆導(dǎo)航星就能夠穩(wěn)定組合導(dǎo)航及估計(jì)加表漂移。

本文兼容機(jī)偽距及偽距率與加表組合導(dǎo)航算法簡(jiǎn)單有效，易于工程實(shí)現(xiàn)。針對(duì)目前基于偽距組合導(dǎo)航速度精度不高的情況，工程應(yīng)用時(shí)可以采用兼容機(jī)偽距及偽距率信息作為觀測(cè)量與加表進(jìn)行組合導(dǎo)航。

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

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

aa,i=Aia·aa,a

(1)

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

ai=ag,i+aa,i

(2)

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

(3)

(4)

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

(5)

(6)

在慣導(dǎo)積分過程中逐步增加位置/速度誤差修正量，可以確保誤差修正的平穩(wěn)性。在J2000慣性系扣除位置/速度誤差修正量，應(yīng)用簡(jiǎn)化積分算法進(jìn)行慣導(dǎo)解算如下：

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

(7)

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

(8)

其中：

ai,k-1為第k-1步加速度，ai,k為第k步加速度，Δvi,k-1為第k-1步估計(jì)的速度誤差修正量，vi,k-1為第k-1步扣除速度誤差修正量的速度，vi,k為第k步扣除速度誤差修正量的速度，Δri,k-1為第k-1步估計(jì)的位置誤差修正量，ri,k-1為第k-1步扣除位置誤差修正量的位置，ri,k為第k步扣除位置誤差修正量的位置，T為導(dǎo)航解算周期。

式(7)和式(8)組合導(dǎo)航解算過程對(duì)位置和速度誤差修正量進(jìn)行積分，為防止積分飽和，需要對(duì)積分進(jìn)行限幅。

2 基于兼容機(jī)偽距及偽距率估計(jì)位置和速度誤差

2.1 基于兼容機(jī)偽距估計(jì)位置誤差

忽略導(dǎo)航星位置誤差，第j顆GNSS導(dǎo)航星和衛(wèi)星相對(duì)位置關(guān)系如圖1所示。

圖1 衛(wèi)星與GNSS導(dǎo)航星相對(duì)位置關(guān)系

(9)

(10)

其中：

rn,j=(xj,yj,zj)，xj，yj和zj為第j顆導(dǎo)航星位置。

設(shè)GNSS兼容機(jī)輸出衛(wèi)星相對(duì)第j顆GNSS導(dǎo)航星偽距測(cè)量值為ρj，求得偽距誤差Δρj：

(11)

由式(10)得：

(12)

其中：

對(duì)式(12)二邊微分求得：

(13)

ρj為GNSS兼容機(jī)輸出的第j顆導(dǎo)航星到衛(wèi)星的偽距，x，y和z為衛(wèi)星的真實(shí)位置。

如GNSS兼容機(jī)接收到3顆導(dǎo)航星，得：

(14)

即：

AΔri=B

(15)

其中：

如果已有衛(wèi)星粗精度位置，且3顆導(dǎo)航星不在同一直線上，即A-1存在，基于三顆導(dǎo)航星可以確定衛(wèi)星位置誤差：

Δri=A-1B

(16)

高軌衛(wèi)星受地球遮擋和探測(cè)靈敏度影響，低軌衛(wèi)星在姿態(tài)異常天線指向地球或受地面人為干擾時(shí)，GNSS兼容機(jī)不能捕獲3顆導(dǎo)航星，只有1顆或2顆導(dǎo)航星時(shí)，無法解算位置誤差。

(17)

n顆導(dǎo)航星偽距估計(jì)位置誤差Δri如下：

(18)

其中：kpr為位置誤差估計(jì)比例系數(shù)。

2.2 基于兼容機(jī)偽距率估計(jì)速度誤差

第j顆GNSS導(dǎo)航星偽距率如圖2所示。

圖2 第j顆GNSS導(dǎo)航星偽距率

(19)

n顆導(dǎo)航星偽距率估計(jì)速度誤差Δvi：

(20)

kpv為速度誤差估計(jì)比例系數(shù)。

3 基于兼容機(jī)偽距組合導(dǎo)航和估計(jì)加表漂移

高軌兼容機(jī)由于受地球遮擋和探測(cè)靈敏度影響，接收到導(dǎo)航星數(shù)量有限，兼容機(jī)不能連續(xù)解算輸出位置信息；加表加速度存在漂移，基于加表的純慣導(dǎo)不能長(zhǎng)時(shí)間使用；因此用兼容機(jī)輸出的少數(shù)導(dǎo)航星偽距信息估計(jì)慣導(dǎo)位置/速度誤差和加表漂移，并修正慣導(dǎo)位置/速度誤差。

(21)

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

(22)

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

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

Δva,k=AaiΔvi,k

(23)

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

(24)

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

設(shè)加表加速度漂移估計(jì)為Δaa,k，求得J2000慣性系下扣除加表漂移的衛(wèi)星非慣性加速度ai,k：

ai,k=Aia(aa,k-Δaa,k)

(25)

其中：Aia為加表坐標(biāo)系到J2000慣性系的姿態(tài)轉(zhuǎn)換矩陣，aa,k為加表第k步測(cè)得的加表坐標(biāo)系下的加速度。

4 基于兼容機(jī)偽距及偽距率組合導(dǎo)航

(26)

(27)

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

位置誤差反映速度誤差，位置誤差修正量可以估計(jì)速度誤差修正量，因此工程應(yīng)用時(shí)可以僅基于兼容機(jī)輸出的偽距與加表進(jìn)行組合導(dǎo)航，并對(duì)加表加速度漂移進(jìn)行估計(jì)。

高軌兼容機(jī)接收的導(dǎo)航星數(shù)有限，基于偽距誤差估計(jì)速度誤差導(dǎo)致慣導(dǎo)輸出的速度精度較低，基于偽距率估計(jì)速度誤差，可以獲得較高精度的速度，位置誤差和加速度漂移仍基于偽距估計(jì)。

5 仿真驗(yàn)證

地球同步靜止軌道衛(wèi)星(GEO)受地球?qū)?dǎo)航星遮擋影響，且為了降低對(duì)兼容機(jī)接收靈敏度需求，兼容機(jī)只接收GPS/GLONASS /BD三種導(dǎo)航星波束主瓣，不探測(cè)導(dǎo)航星波束副瓣，因此兼容機(jī)接收的導(dǎo)航星數(shù)有限，可見導(dǎo)航星數(shù)如圖3所示。

圖3 GEO衛(wèi)星可見GPS/GLONASS /BD導(dǎo)航星數(shù)

加表三軸加速度常值漂移分別為-0.006m/s2、-0.004m/s2、0.005m/s2，隨機(jī)漂移為0.0005m/s2；兼容機(jī)偽距常值誤差和隨機(jī)誤差均為4m，兼容機(jī)偽距率常值誤差和隨機(jī)誤差均為0.04m/s。

仿真結(jié)果表明：

1)基于兼容機(jī)偽距估計(jì)加表漂移如圖4所示，組合導(dǎo)航位置誤差和速度誤差如圖5～6所示，收斂后位置誤差為200m，速度誤差為0.5m/s，均出現(xiàn)在兼容機(jī)僅可見1顆導(dǎo)航星時(shí)；

2)基于兼容機(jī)偽距和偽距率估計(jì)速度誤差如圖7所示，導(dǎo)航收斂后速度誤差為0.2m/s，優(yōu)于僅基于偽距組合導(dǎo)航，也出現(xiàn)在兼容機(jī)接收到1顆導(dǎo)航星時(shí)；

3)兼容機(jī)偽距與加表組合導(dǎo)航允許短期沒有導(dǎo)航星，可基于加表慣導(dǎo)；當(dāng)兼容機(jī)接收到2顆導(dǎo)航星時(shí)能夠穩(wěn)定組合導(dǎo)航，長(zhǎng)時(shí)間只有1顆導(dǎo)航星時(shí)導(dǎo)航精度受影響。

圖4 基于兼容機(jī)偽距估計(jì)加表漂移

圖5 基于兼容機(jī)偽距組合導(dǎo)航位置誤差放大曲線

圖6 基于兼容機(jī)偽距組合導(dǎo)航速度誤差放大曲線

圖7 基于兼容機(jī)偽距及偽距率率組合導(dǎo)航速度誤差放大曲線

6 結(jié)論

基于兼容機(jī)偽距估計(jì)加表漂移，進(jìn)行兼容機(jī)偽距及偽距率組合導(dǎo)航算法研究，尤其針對(duì)兼容機(jī)偽距組合導(dǎo)航輸出速度精度不高的問題，進(jìn)行了以兼容機(jī)偽距及偽距率信息為觀測(cè)量的組合導(dǎo)航，提高了速度精度。仿真驗(yàn)證表明算法有效，兼容機(jī)接收到2顆導(dǎo)航星就能夠穩(wěn)定組合導(dǎo)航及估計(jì)加表漂移。

該算法簡(jiǎn)單有效，易于工程實(shí)現(xiàn)。針對(duì)目前基于偽距組合導(dǎo)航速度精度不高的情況，工程應(yīng)用時(shí)可以采用兼容機(jī)偽距及偽距率信息作為觀測(cè)量與加表進(jìn)行組合導(dǎo)航。