伪距及伪距率与加表组合导航及漂移估计

王献忠 张 肖 张丽敏 施常勇

1.上海航天技术研究院，上海201109 2.空间智能控制技术重点实验室，上海201109 3.上海航天控制技术研究所，上海201109

0 引言

加表能够连续输出非惯性力产生的加速度，基于加表惯导可以确定卫星在惯性空间的位置和速度，但加表存在漂移，不能长时间使用；高轨兼容机可能受地球遮挡和探测灵敏度影响，接收的导航星数有限，不能连续输出卫星在惯性空间的位置和速度。

基于兼容机估计加表漂移，对加表漂移修正后惯导解算输出卫星在惯性空间的位置和速度，既可以确保位置和速度输出的连续性，又可以抑制兼容机观测噪声对组合导航精度的影响。

国内外学者对SINS/GNSS、INS/GNSS组合导航进行了广泛的研究，文献[1]介绍INS/CNS/兼容机组合导航的信息融合和滤波方法，分析了INS/CNS/兼容机组合导航技术的发展和应用现状。众多学者基于卡尔曼滤波及其扩展型进行SINS/GNSS、INS/GNSS组合导航滤波算法研究，文献[2]基于伪距/伪距率与SINS组合导航，利用卫星系统的星历数据与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深组合导航系统中的应用研究。

本文首先推导了基于兼容机伪距及伪距率估计位置和速度误差算法；接着给出了基于兼容机伪距信息作为观测量的组合导航和加表漂移估计算法；最后针对基于兼容机伪距组合导航输出速度精度不高的问题，进行了基于兼容机伪距及伪距率信息作为观测量的组合导航，提高了速度精度。仿真验证表明算法有效，兼容机接收到2颗导航星就能够稳定组合导航及估计加表漂移。

本文兼容机伪距及伪距率与加表组合导航算法简单有效，易于工程实现。针对目前基于伪距组合导航速度精度不高的情况，工程应用时可以采用兼容机伪距及伪距率信息作为观测量与加表进行组合导航。

1 基于J2000惯性系的卫星惯导解算算法

设加表坐标系到J2000惯性系的姿态转换矩阵为Aia，求得J2000惯性系下卫星非惯性加速度aa,i：

aa,i=Aia·aa,a

(1)

其中：aa,a为加表测得的加表坐标系下的加速度。

ai=ag,i+aa,i

(2)

在J2000惯性系进行惯导解算位置ri和速度vi：

(3)

(4)

设加表加速度漂移转换到J2000系为Δai，根据式(3)和(4)惯导解算算法求得加表漂移引起的位置/速度误差Δvi/Δri：

(5)

(6)

在惯导积分过程中逐步增加位置/速度误差修正量，可以确保误差修正的平稳性。在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导航星和卫星相对位置关系如图1所示。

图1 卫星与GNSS导航星相对位置关系

(9)

(10)

其中：

rn,j=(xj,yj,zj)，xj，yj和zj为第j颗导航星位置。

设GNSS兼容机输出卫星相对第j颗GNSS导航星伪距测量值为ρj，求得伪距误差Δρj：

(11)

由式(10)得：

(12)

其中：

对式(12)二边微分求得：

(13)

ρj为GNSS兼容机输出的第j颗导航星到卫星的伪距，x，y和z为卫星的真实位置。

如GNSS兼容机接收到3颗导航星，得：

(14)

即：

AΔri=B

(15)

其中：

如果已有卫星粗精度位置，且3颗导航星不在同一直线上，即A-1存在，基于三颗导航星可以确定卫星位置误差：

Δri=A-1B

(16)

高轨卫星受地球遮挡和探测灵敏度影响，低轨卫星在姿态异常天线指向地球或受地面人为干扰时，GNSS兼容机不能捕获3颗导航星，只有1颗或2颗导航星时，无法解算位置误差。

(17)

n颗导航星伪距估计位置误差Δri如下：

(18)

其中：kpr为位置误差估计比例系数。

2.2 基于兼容机伪距率估计速度误差

第j颗GNSS导航星伪距率如图2所示。

图2 第j颗GNSS导航星伪距率

(19)

n颗导航星伪距率估计速度误差Δvi：

(20)

kpv为速度误差估计比例系数。

3 基于兼容机伪距组合导航和估计加表漂移

高轨兼容机由于受地球遮挡和探测灵敏度影响，接收到导航星数量有限，兼容机不能连续解算输出位置信息；加表加速度存在漂移，基于加表的纯惯导不能长时间使用；因此用兼容机输出的少数导航星伪距信息估计惯导位置/速度误差和加表漂移，并修正惯导位置/速度误差。

(21)

其中：kp,r为位置误差修正量估计比例系数，kp,r为3×3对角阵，可以独立估计位置误差修正量。

(22)

其中：kp,v为速度误差修正量估计比例系数，kp,v为3×3对角阵，可以独立估计速度误差修正量。

将基于兼容机伪距求得的惯导解算速度误差Δvi,k转换到加表坐标系：

Δva,k=AaiΔvi,k

(23)

其中：Aai为J2000惯性系到加表坐标系的姿态转换矩阵。

(24)

其中：Δaa,k为第k步估计的加表漂移，kp,a为PI滤波估计比例系数，ki,a为PI滤波估计积分系数，kp,a和ki,a为3×3对角阵，三轴可以独立估计加速度漂移。

设加表加速度漂移估计为Δaa,k，求得J2000惯性系下扣除加表漂移的卫星非惯性加速度ai,k：

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

(25)

其中：Aia为加表坐标系到J2000惯性系的姿态转换矩阵，aa,k为加表第k步测得的加表坐标系下的加速度。

4 基于兼容机伪距及伪距率组合导航

(26)

(27)

其中：kp,r和kp,v为位置/速度误差修正量估计比例系数，kp,r和kp,v为3×3对角阵，三轴可以独立估计位置/速度误差修正量。

位置误差反映速度误差，位置误差修正量可以估计速度误差修正量，因此工程应用时可以仅基于兼容机输出的伪距与加表进行组合导航，并对加表加速度漂移进行估计。

高轨兼容机接收的导航星数有限，基于伪距误差估计速度误差导致惯导输出的速度精度较低，基于伪距率估计速度误差，可以获得较高精度的速度，位置误差和加速度漂移仍基于伪距估计。

5 仿真验证

地球同步静止轨道卫星(GEO)受地球对导航星遮挡影响，且为了降低对兼容机接收灵敏度需求，兼容机只接收GPS/GLONASS /BD三种导航星波束主瓣，不探测导航星波束副瓣，因此兼容机接收的导航星数有限，可见导航星数如图3所示。

图3 GEO卫星可见GPS/GLONASS /BD导航星数

加表三轴加速度常值漂移分别为-0.006m/s2、-0.004m/s2、0.005m/s2，随机漂移为0.0005m/s2；兼容机伪距常值误差和随机误差均为4m，兼容机伪距率常值误差和随机误差均为0.04m/s。

仿真结果表明：

1)基于兼容机伪距估计加表漂移如图4所示，组合导航位置误差和速度误差如图5～6所示，收敛后位置误差为200m，速度误差为0.5m/s，均出现在兼容机仅可见1颗导航星时；

2)基于兼容机伪距和伪距率估计速度误差如图7所示，导航收敛后速度误差为0.2m/s，优于仅基于伪距组合导航，也出现在兼容机接收到1颗导航星时；

3)兼容机伪距与加表组合导航允许短期没有导航星，可基于加表惯导；当兼容机接收到2颗导航星时能够稳定组合导航，长时间只有1颗导航星时导航精度受影响。

图4 基于兼容机伪距估计加表漂移

图5 基于兼容机伪距组合导航位置误差放大曲线

图6 基于兼容机伪距组合导航速度误差放大曲线

图7 基于兼容机伪距及伪距率率组合导航速度误差放大曲线

6 结论

基于兼容机伪距估计加表漂移，进行兼容机伪距及伪距率组合导航算法研究，尤其针对兼容机伪距组合导航输出速度精度不高的问题，进行了以兼容机伪距及伪距率信息为观测量的组合导航，提高了速度精度。仿真验证表明算法有效，兼容机接收到2颗导航星就能够稳定组合导航及估计加表漂移。

该算法简单有效，易于工程实现。针对目前基于伪距组合导航速度精度不高的情况，工程应用时可以采用兼容机伪距及伪距率信息作为观测量与加表进行组合导航。