基于PI滤波估计加表漂移的兼容机与加表组合导航

王献忠 张 肖 张丽敏 施常勇

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

加表能够连续输出非惯性力产生的加速度，基于加表惯导可以确定卫星在惯性空间的位置和速度，但加表存在漂移，不能长时间使用；兼容机能够直接提供卫星在惯性空间的位置和速度，但兼容机可能受单粒子翻转、姿态机动或翻滚时天线被地球遮挡、地面局部区域人为干扰等影响，不能连续输出卫星在惯性空间的位置和速度，特别是在高能粒子导致兼容机复位或重加载期间。

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

国内外学者对SINS/GPS组合导航进行了广泛的研究，文献[1]介绍了INS/CNS/兼容机组合导航的信息融合和滤波方法，分析了INS/CNS/兼容机组合导航技术的发展和应用现状。众多学者基于卡尔曼滤波及扩展型进行SINS/GPS组合导航研究，文献[2]基于EKF进行SINS/兼容机深组合导航应用研究；文献[3] 基于卡尔曼滤波进行运载火箭SINS/兼容机组合导航研究；文献[4] 基于集中式卡尔曼滤波结构进行提高SINS/兼容机组合导航定位精度方法研究；文献[5]基于UKF进行MIMU/GPS/DVS组合导航方法研究。也有一些学者基于新型滤波方法进行SINS/GPS组合导航滤波算法研究，文献[6]基于小波技术进行GPS/SINS组合导航方法研究；文献[7]进行改进高斯粒子滤波算法及其在SINS/GPS深组合导航系统中的应用研究。

本文首先推导了基于兼容机位置/速度误差估计加表加速度漂移的PI滤波方法；接着给出了兼容机与加表组合导航算法，利用估计得到的加速度漂移分别对位置和速度进行修正；其次进行了工程应用分析，在无法获取高精度速度测量信息时仅利用兼容机位置对加速度漂移进行估计；最后针对仅用兼容机速度信息作为观测量的滤波方法进行仿真验证，仿真结果表明采用仅基于兼容机位置信息作为观测量的PI滤波组合导航能准确估计加速度漂移，方法简单有效，可应用于工程实践。

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

(1)

(2)

(3)

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

aa,i=Aia·aa,a

(4)

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

ai=ag,i+aa,i

(5)

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

(6)

(7)

2 兼容机与加表PI滤波组合导航

2.1 基于兼容机修正惯导误差和估计加表漂移

兼容机由于受自然遮挡或人为干扰影响，尤其是高轨兼容机由于灵敏度原因，收到导航星数量有限，导致兼容机不一定能连续输出位置信息；加表加速度存在漂移，基于加表的纯惯导不能长时间使用；因此可以用兼容机信息修正惯导位置/速度误差，并估计加表漂移。

设加表加速度漂移转换到J2000系为dai，根据式(6)和(7)惯导解算算法求得加表漂移引起的位置/速度误差dvi/dri：

(8)

(9)

设惯导第k步解算输出的位置/速度为ri,k/vi,k，兼容机经时间对齐后输出的位置/速度为ri,G/vi,G，求得惯导解算位置/速度误差Δri,k/Δvi,k：

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

(10)

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

(11)

基于兼容机求得的惯导解算位置/速度误差Δri,k/Δvi,k，结合式(6)和(7)惯导解算积分过程，在惯导积分过程中逐步增加位置/速度误差修正量，可以确保误差修正的平稳性。第k步惯导解算位置/速度误差修正量dri,k/dvi,k：

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

(12)

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

(13)

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

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

Δva,k=Aai·Δvi,k

(14)

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

基于PI滤波估计加表加速度漂移算法如下：

(15)

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

2.2 兼容机与加表组合导航

考虑到兼容机输出信息可能不连续，组合导航的输出并不直接采用兼容机信息，而是基于加速度计测量得到的加速度，扣除采用兼容机估计得到的加速度常值漂移后积分得到惯性系位置、速度，既保证长时高精度，又能保证数据的连续性。

设加表坐标系到J2000惯性系的姿态转换矩阵为Aia；aa,a为加表测得的加表坐标系下的加速度；加表加速度漂移估计为daa；求得J2000惯性系下扣除加表漂移的卫星非惯性加速度aai：

aa,i=Aia·aa,a-daa

(16)

3 工程应用

兼容机基于伪距测量滤波估计位置和速度，其输出的速度相当于对位置微分，速度精度远低于位置精度，且观测噪声较大；为防止低精度的速度观测量影响组合导航精度，且考虑到位置误差也反映速度误差，位置误差修正量可以估计速度误差修正量，因此工程应用时可以仅基于兼容机输出的位置与加表进行组合导航，并对加表加速度漂移进行估计。

(17)

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

将速度误差修正量dvi,k转换到加表坐标系：

dva,k=Aai·dvi,k

(18)

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

(19)

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

求得J2000惯性系下扣除加表漂移的卫星非惯性加速度aai如式(20)：

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

(20)

采用式(5)求解J2000惯性系下卫星加速度ai，应用简化积分算法进行惯导解算如下：

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步估计的速度误差修正量；vi,k-1为第k-1步扣除速度误差修正量的速度，vi,k为第k步扣除速度误差修正量的速度；dri,k-1为第k-1步估计的位置误差修正量；ri,k-1为第k-1步扣除位置误差修正量的位置；ri,k为第k步扣除位置误差修正量的位置；T为导航周期。

4 仿真验证

加表三轴加速度常值漂移分别为0.005m/s2、0.008m/s2和-0.005m/s2；随机漂移0.0005m/s2；兼容机三轴位置常值偏差分别为5m、10m和15m，速度误差小于0.5m/s(3σ)。

对兼容机与加表组合导航算法进行仿真验证，并分别对仅用兼容机输出的位置作为观测量，同时采用兼容机输出的位置/速度作为观测量进行对比仿真。

1)仅基于兼容机位置信息作为观测量,如图1～3；

2)同时基于兼容机位置/速度作为观测信息,如图4～6。

从仿真结果可以看出，基于兼容机位置信息作为观测量，或基于兼容机位置/速度信息作为观测量均可以有效估计加表漂移；其中基于位置信息作为观测量时，位置估计误差波动优于0.1 m，速度估计精度优于0.005 m/s；同时基于位置/速度信息作为观测量时，位置估计误差波动优于0.5 m，速度估计精度优于0.05 m。这是由于兼容机速度测量精度较低，其在引入系统滤波时影响了位置估计精度。

图1 基于兼容机位置观测信息加表漂移估计值

图2 基于位置观测信息组合导航位置误差曲线

图3 基于位置观测信息组合导航速度误差曲线

图4 基于兼容机位置/速度观测信息加表漂移估计值

图5 基于位置/速度观测信息组合导航位置误差曲线

图6 基于位置/速度观测信息组合导航速度误差曲线

5 结论

基于PI滤波估计加表加速度漂移，在此基础上进行兼容机与加表组合导航算法研究，尤其针对兼容机输出速度精度不高的问题进行了仅基于兼容机位置信息作为观测量，对加表加速度漂移进行估计。仿真验证表明该算法有效，组合导航位置、速度估计精度均优于同时引入兼容机速度信息作为观测量的情况。针对目前兼容机速度测量精度不高的情况，工程应用时可以仅采用兼容机位置信息作为观测量与加表进行组合导航。

基于PI滤波估计加表漂移的兼容机与加表组合导航算法简单有效，易于工程实现，并已通过在轨考核验证。