偽距及偽距率與加表組合導航及漂移估計

王獻忠 張 肖 張麗敏 施常勇

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

0 引言

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

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

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

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

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

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

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

aa,i=Aia·aa,a

(1)

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

ai=ag,i+aa,i

(2)

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

(3)

(4)

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

(5)

(6)

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

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為導航解算周期。

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

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

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

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

圖1 衛(wèi)星與GNSS導航星相對位置關系

(9)

(10)

其中：

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

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

(11)

由式(10)得：

(12)

其中：

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

(13)

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

如GNSS兼容機接收到3顆導航星，得：

(14)

即：

AΔri=B

(15)

其中：

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

Δri=A-1B

(16)

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

(17)

n顆導航星偽距估計位置誤差Δri如下：

(18)

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

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

第j顆GNSS導航星偽距率如圖2所示。

圖2 第j顆GNSS導航星偽距率

(19)

n顆導航星偽距率估計速度誤差Δvi：

(20)

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

3 基于兼容機偽距組合導航和估計加表漂移

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

(21)

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

(22)

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

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

Δva,k=AaiΔvi,k

(23)

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

(24)

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

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

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

(25)

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

4 基于兼容機偽距及偽距率組合導航

(26)

(27)

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

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

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

5 仿真驗證

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

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

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

仿真結果表明：

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

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

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

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

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

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

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

6 結論

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

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