偽距及偽距率與加表組合導(dǎo)航及漂移估計

王獻忠 張 肖 張麗敏 施常勇

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

0 引言

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

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

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

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

本文兼容機偽距及偽距率與加表組合導(dǎo)航算法簡單有效，易于工程實現(xiàn)。針對目前基于偽距組合導(dǎo)航速度精度不高的情況，工程應(yīng)用時可以采用兼容機偽距及偽距率信息作為觀測量與加表進行組合導(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為加表測得的加表坐標(biāo)系下的加速度。

ai=ag,i+aa,i

(2)

在J2000慣性系進行慣導(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)用簡化積分算法進行慣導(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步估計的速度誤差修正量，vi,k-1為第k-1步扣除速度誤差修正量的速度，vi,k為第k步扣除速度誤差修正量的速度，Δri,k-1為第k-1步估計的位置誤差修正量，ri,k-1為第k-1步扣除位置誤差修正量的位置，ri,k為第k步扣除位置誤差修正量的位置，T為導(dǎo)航解算周期。

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

2 基于兼容機偽距及偽距率估計位置和速度誤差

2.1 基于兼容機偽距估計位置誤差

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

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

(9)

(10)

其中：

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

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

(11)

由式(10)得：

(12)

其中：

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

(13)

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

如GNSS兼容機接收到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)星受地球遮擋和探測靈敏度影響，低軌衛(wèi)星在姿態(tài)異常天線指向地球或受地面人為干擾時，GNSS兼容機不能捕獲3顆導(dǎo)航星，只有1顆或2顆導(dǎo)航星時，無法解算位置誤差。

(17)

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

(18)

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

2.2 基于兼容機偽距率估計速度誤差

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

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

(19)

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

(20)

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

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

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

(21)

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

(22)

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

將基于兼容機偽距求得的慣導(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步估計的加表漂移，kp,a為PI濾波估計比例系數(shù)，ki,a為PI濾波估計積分系數(shù)，kp,a和ki,a為3×3對角陣，三軸可以獨立估計加速度漂移。

設(shè)加表加速度漂移估計為Δ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步測得的加表坐標(biāo)系下的加速度。

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

(26)

(27)

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

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

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

5 仿真驗證

地球同步靜止軌道衛(wèi)星(GEO)受地球?qū)?dǎo)航星遮擋影響，且為了降低對兼容機接收靈敏度需求，兼容機只接收GPS/GLONASS /BD三種導(dǎo)航星波束主瓣，不探測導(dǎo)航星波束副瓣，因此兼容機接收的導(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，隨機漂移為0.0005m/s2；兼容機偽距常值誤差和隨機誤差均為4m，兼容機偽距率常值誤差和隨機誤差均為0.04m/s。

仿真結(jié)果表明：

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

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

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

圖4 基于兼容機偽距估計加表漂移

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

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

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

6 結(jié)論

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

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