R-T-S平滑算法与抗差Kalman在数据后处理中的应用

，，，

(北京理工大学 自动化学院，北京 100081)

0 引言

GPS/INS组合导航系统的发展随着GPS技术的普及，在军用和民用各领域产生了深远的影响。GPS虽然具有长航时精度高、不受天气影响的优点，但其缺点也显而易见，那就是卫星信号在有些地方受遮挡造成丢星从而影响定位，以及抗干扰性差等因素。INS则是一种完全自主式的导航系统，导航过程中不向外辐射电磁信号，和外界不发生任何光、电的联系，因此具有隐蔽性好、抗干扰能力强,工作不受气象条件限制等优点[1]。但是，纯惯性导航系统也有一个致命的缺点，就是其导航误差随时间增长。组合导航常以INS 作为主导航系统，而将其他导航定位误差不随时间积累的导航系统，如多普勒导航、地形匹配导航等系统作为辅助导航系统，来修正INS在导航过程中的偏差。同时，在算法上采用Kalman滤波技术，对组合系统的状态变量进行最优估计，获得修正信息，从而提高导航精度[2]。这样，既保持了INS系统的自主性，又防止了导航定位误差随时间积累。由于GPS的低动态、窄带宽、高精度与SINS的高动态、宽频带、误差慢漂移特性形成强烈的互补性，所以 GPS/INS 组合在航空、航天、航海、陆地战车等导航领域已得到越来越广泛的应用，具有巨大的发展潜力。

近年来，由于微惯性器件的迅速发展，使得 MINS/GPS 组合导航系统己经在某些领域投入应用，它克服了低精度惯导系统单独工作时误差随时间积累的缺点和GPS系统固有的局限性，并且由于体积小、质量小，大大拓宽了其应用领域。GPS能提供高精度的导航信息，如位置和速度数据，并连续地对准和校准MINS系统[3]。MINS可提供短期的高精度的速度数据，并能辅助接收机在丢星时缩短捕获和重新捕获卫星信号的时间，故在丢星期间可直接采用其进行导航，并提供加速度和姿态数据。因此，MINS/GPS组合导航精度在一定程度上高于2个系统单独工作的导航精度，同时也提高了系统的鲁棒性[4]。

本文提出一种基于R-T-S(Rauch-Tung-Striebel)平滑算法和抗差Kalman滤波的数据后处理算法，应用于测绘领域。其基本原理是利用抗差估计原理，构造等价协方差矩阵，降低含有粗差观测值的利用程度[5]，从而提高Kalman滤波的抗差能力，并在此前向滤波的基础上，进行后向R-T-S最优固定区间平滑处理，并对该算法进行了实物仿真。结果表明，与传统Kalman滤波相比，R-T-S平滑算法不仅可以提高位置、姿态精度，而且在卫星信号失锁的情况下精度也得到显著改善，并且在不丢星的时刻，抗差Kalman滤波可以有效处理GPS信号中的异常观测值，遏制滤波发散，是一种有效的数据后处理方法。

1 组合导航数据后处理算法流程

在数据后处理过程中，以GPS设备所得数据为基准，用来估计惯导设备的偏差，在卫星信号失锁时，惯导设备所提供的信息可以用来估计载体的运动状态，从而减少卫星信号重新捕获所需的时间[6]。

图1 数据后处理算法流程Fig.1 The flow diagram of data post-processing algorithm

后处理算法主要包含2个过程：首先利用IMU测量的加速度信息、角速度信息进行惯性导航解算，将获得的位置信息、速度信息与GPS信息同步作差(为提高精度通常采用差分GPS)，作为前向Kalman滤波的观测量。其次利用前向Kalman滤波过程中获得的协方差矩阵和状态转移矩阵，对状态估值进行反向R-T-S平滑处理，根据平滑结果对INS输出的位置、速度和姿态进行校正，作为系统的最终输出[7]。在本文中，前向Kalman滤波过程采用抗差Kalman滤波，通过滤波增益矩阵来抑制粗差的影响。

2 抗差Kalman技术

在MINS/GPS组合导航中，由于GPS容易受到外界环境的遮挡和干扰，从而产生的异常观测值，称为粗差。粗差作用与Kalman滤波，会引起系统测量模型与实际测量值之间的偏差，从而影响滤波算法的稳定性，甚至导致滤波发散[8]。在GPS导航中，处理GPS观测粗差的一种行之有效的方法是抗差估计，其目的是在检测到观测值存在粗差时，引入等价权，通过降低含有粗差的观测值的权重来提高估计精度。

在本文中，采用抗差最小二乘估计。在紧组合中，假设某一时刻观测有N颗卫星(N≥4)，不需要抗差最小二乘的解算结果，只需要计算观测向量的等价协方差矩阵即可。对GPS卫星的观测可以认为独立、等权，故观测值得相对权矩阵为单位矩阵。对紧组合观测方程进行最小二乘估计，并用最小二乘残差结果计算观测值的权因子可得

αi=diag[α1α2…αn] 0<αi≤1，i=1,2…n

(1)

相应的抗差等价协方差矩阵为

(2)

故抗差Kalman滤波的基本方程可以表述为：

(3)

抗差Kalman滤波通过调节观测噪声协方差矩阵调整Kalman滤波的增益。当GPS观测值中含有粗差时，利用抗差最小二乘算法构造等价权矩阵的方法构造观测值的等价协方差矩阵，在Kalman滤波中，观测噪声方差的增大意味着相应观测值增益的减小。因此，可以通过滤波增益矩阵抑制粗差对系统的影响[9]。当观测值中不存在粗差时，观测值的权因子αi=1(i=1,2…n)，此时抗差Kalman滤波转变为常规Kalman滤波。

3 平滑技术

平滑技术作为事后或准实时数据处理的一种方法，可以在一定程度上提高数据处理的精度，在测绘领域获得了广泛的应用。平滑技术总的来说分为三类：固定区间平滑(Fixed-Interval Smoothing)、固定点平滑(Fixed-Point Smoothing)和固定滞后平滑(Fixed-Lag Smoothing)。其中在数据后处理中应用最为广泛的方法就是固定区间平滑，其原理图如图2所示。它是在前向Kalman滤波的基础上进行的反向滤波处理，充分利用区间内所有时刻的测量值对某一时刻的状态进行估计。该方法能提供比单向滤波更高的精度，同时在卫星信号失锁段也是一种很好的桥接算法[10]。

图2 固定区间平滑算法原理图Fig.2 Schematic diagram of fixed-interval smoothing algorithm

R-T-S算法是一种固定区间最优平滑算法。许多文献中将R-T-S固定区间最优平滑算法应用到GPS/INS组合导航系统中，结果R-T-S固定区间平滑算法的精度比Kalman滤波的精度高，且该方法计算简单，易于实现，是一种有效的事后处理方法[11]。由于该方法的优势主要在于GPS失锁时刻，而在不丢星时并无显著改进，故本文在该算法的基础上引入了抗差Kalman滤波，使其在不丢星时的精度得到进一步改善，从而得到更好的数据后处理结果。

R-T-S平滑算法由前向滤波和后向滤波组成，前向滤波是经典的Kalman滤波器，用来估计每一时刻的状态，后向滤波是在前向滤波的基础上重复利用部分数据，以获取更精确的状态估计值[12]。

由于R-T-S平滑算法主要体现在后向滤波处理上，故在执行该算法前，应首先在时间区间 [0,N]内对系统进行前向抗差Kalman滤波，抗差Kalman滤波的递推公式如下。

状态向量和方差阵的一步预测：

(4)

PF(k|k-1)=Φk,k-1PF(k-1|k-1)×

(5)

式中，下标F表示Kalman滤波。

滤波增益为

(6)

状态向量和方差阵更新：

(7)

抗差Kalman滤波过程结束后，利用前向滤波过程中存储的数据进行R-T-S固定区间最优平滑。在平滑前需要先对平滑器进行初始化，令k=N，则有：

PS(N|N)=PF(N|N)

(8)

式中，下标S表示最优平滑。在时间区间[N-1,0]内R-T-S固定区间平滑算法的递推公式如下。

平滑增益为

(9)

平滑的状态向量和方差阵的更新为：

(10)

(11)

由式(10)、式(11)可见平滑递推公式是一个由k=N-1到k=0的倒推过程，且平滑递推过程中要用到的状态向量与方差阵的估计值和预测值均为前向Kalman滤波过程中所得的值[14]。故R-T-S算法是前向滤波算法和后向滤波算法的融合，充分利用了区间[0,N] 内的每一组数据，因而精度高于一般Kalman滤波。

4 组合导航系统的数学模型

本文导航坐标系选取东北天地理坐标系，其状态变量为

XI=[φEφNφUδvEδvNδvUδLδλδh

εxεyεzxyzδtuδtru]T

(12)

其中，φE、φN、φU为数学平台失准角；δvE、δvN、δvU分别为载体的东向、北向和天向速度误差；δL、δλ、δh分别为纬度误差、经度误差和高度误差；εx、εy、εz、x、y、z分别为陀螺随机常值漂移和加速度计随机常值零偏；δtu、δtru分别是与时钟误差等效的距离误差和与时钟频率误差等效的距离率误差。

系统采用紧组合的方式，其状态方程为

(13)

基于伪距、伪距率的量测方程为

(14)

式中各系数矩阵均可通过导航关系解算出来，由于篇幅关系不再赘述。

5 跑车实验介绍

为了验证R-T-S算法和抗差Kalman滤波在数据后处理中的应用，进行了跑车试验，车载3套MIMU，共进行了3组试验，每组实验的安装方式如图3所示。

图3 实验安装方法Fig.3 The experimental installation method

按照第一种安装方式，在基准点J01停止，采集MIMU的输出，约10min。沿北清路行进、掉头返回，继续行进，采集该过程中MIMU的输出数据。行进路径如图4所示。按照第二种、第三种安装方式重复试验。

图4 实验地段北清路Fig.4 The Beiqing Road in experiment

在Matlab环境下，先对GPS/MINS组合导航系统进行前向Kalman滤波，同时存储Kalman滤波器在各个时刻的估计值。事后利用R-T-S最优平滑算法对系统进行后向平滑处理，分别得到位置误差、姿态误差、组合位置的图像如图5～图7所示。

图5 位置误差Fig.5 The position error

图6 姿态误差Fig.6 The attitude error

图7 位置Fig.7 The position

从以上各图中可以看出，与标准Kalman滤波相比，R-T-S最优平滑滤波和抗差Kalman滤波的结合可以有效地减小系统的位置和姿态误差，提高系统的精度。基于标准Kalman滤波的导航结果在过程中出现了较大跳变，其原因主要是受到伪距、伪距率粗差观测值的影响。相比之下，抗差Kalman滤波降低了观测噪声方差较大项的滤波增益，可以有效减少噪声的影响，图像相比之下并没有出现较大的跳变。并且当GPS不失锁时，R-T-S算法对导航结果的平滑的优势主要体现在GPS测量更新的间隙，能够有效减少SINS预测过程中误差的积累，也能在一定程度上削弱随机误差的影响。但由于试验所选惯导精度较高，故对随机误差的削弱主要还是由抗差Kalman完成。但当可见卫星数小于4颗时，抗差Kalman滤波退化为普通Kalman滤波，此时导航解算精度的提高主要依赖于R-T-S算法，但由于本实验所选地段较为开阔，整个过程中仅有3座桥梁会造成丢星的发生，不存在丢星时刻的长时间积累，故R-T-S算法的优势并不能完全发挥。但经过R-T-S算法和抗差Kalman滤波的结合，整体数据后处理的精度仍有大幅提高，位置精度能够提高50%以上，姿态精度也有40%左右的提升。

6 结论

为了提高数据处理的精度，获得高精度的速度、位置和姿态信息，将R-T-S最优平滑算法和抗差Kalman滤波应用于数据后处理中。其中当可观测卫星数大于等于4颗时，抗差Kalman滤波模型成立，可以降低观测噪声方差较大项的滤波增益，通过滤波增益矩阵来抑制粗差的影响；R-T-S算法此时对后处理精度提高的优势主要体现在GPS测量更新的间隙，能够有效减少SINS预测过程中误差的积累，也能在一定程度上削弱随机误差的影响。当可观测卫星数小于4颗时，抗差Kalman滤波退化为普通Kalman滤波，此时后处理精度的提高主要依赖于R-T-S算法，在前向Kalman滤波的基础上进行的反向滤波，充分利用了区间内所有时刻的测量值对状态进行估计，显著提高数据后处理的精度。两者的结合可以在全时段提高数据后处理的精度，是一种有效的数据后处理方法。

参考文献

[1] Rochette P, Gattacceca J, Ivanov A V, et al. Mag-netic properties of lunar materials: Meteorites, Luna and Apollo returned samples[J]. Earth and Planetary Science Letters, 2010, 292(3-4): 383-391.

[2] 马云峰. MSINS/GPS组合导航系统及其数据融合技术研究[D]. 南京：东南大学, 2006.

[3] 付梦印，邓志红，闫莉萍. Kalman滤波理论及其在导航系统中的应用[M]. 北京：科学出版社, 2010.

[4] Shortelle K J, Graham W R, Rabourn C. F-16 flight tests of a rapid transfer alignment procedure[C]// Position Location and Navigation Symposium. IEEE, 1998: 379-383.

[5] 秦永元. 惯性导航[M]. 北京：科学出版社, 2005.

[6] Richmond N C, Hood L L. A preliminary global map of the vector lunar crustal magnetic field based on Lunar Prospector magnetometer data[J]. Journal of Ge-ophysical Research: Planets,2008, 113(E2).

[7] Purucker M E. A global model of the internal magn-etic field of the Moon based on Lunar Prospector ma-gnetometer observations[J].Icarus, 2008, 197(1): 19-22.

[8] 严恭敏. 捷联惯导算法及车载组合导航系统研究[D]. 西安：西北工业大学, 2004.

[9] 董明. 捷联惯性导航误差模型与初始对准技术研究[D]. 郑州：解放军信息工程大学, 2011.

[10] Nassar S. Different Algorithms for Bridging Kine-matic DGPS Outages Using SINS/DGPS Integration[C]// Institute of Navigation Satellite Division Technical Meeting. Portland, 2002: 1474-1480.

[11] Hood L L. Central magnetic anomalies of Nectarian-aged lunar impact basins: Probable evidence for an early core dynamo[J]. Icarus, 2011, 211(2):1109-1128.

[12] Xiao-Xu W, Quan P, LIANG Y, et al. Application of unscented transformation for nonlinear state smoo-thing[J].Acta Automatica Sinica, 2012, 38(7): 1107-1111.

[13] Liu Jianye, Yuan Xin. The application of smoothing filter to GPS/INS integrated navigation system[J]. Aerospace Control, 1995(4):36-44.

[14] 李靖松. 基于伪距、伪距率组合导航技术的研究[D]. 南京：南京理工大学, 2009.