基于抗差自适应SITAN算法的水下重力匹配导航方法

付林威 赵东明 付 林

1 信息工程大学地理空间信息学院,郑州市科学大道62号,450001

重力匹配惯性导航是实现水下运载体长时间高精度导航定位的重要手段之一,能有效减弱惯性导航系统(inertial navigation system,INS)随时间推移产生的误差累积[1]。水下重力匹配惯性导航精度依赖于INS精度、水下重力传感器测量精度、重力异常基准图精度以及匹配算法等要素,其中,匹配算法是核心,主要包括基于相关分析技术的地形轮廓匹配(terrain contour matching,TERCOM)算法、迭代最近等值线点(iterated closest contour point,ICCP)算法和基于递推滤波技术的桑迪亚惯性地形辅助导航(Sandia inertial terrain aided navigation,SITAN)算法[2]等。近年来,有学者提出矢量匹配算法[3]、粒子滤波算法[4]、具有约束条件的匹配算法[5]和三角形约束算法[6]等。SITAN算法综合了卡尔曼滤波(Kalman filter,KF)技术和地形随机线性化技术,具有实时性好、可操作性强等特点,在量测值不含粗差且量测误差服从正态分布时能够求得最优无偏估值,但在实际情况下可能出现异常观测值和动力学模型误差等问题,严重时甚至会导致滤波发散。为此,学者们提出多种针对动力学模型误差[7-10]和观测粗差[11-12]的算法,取得了较好的效果。

本文在SITAN算法的基础上,提出一种基于抗差自适应扩展卡尔曼滤波(robust adaptive extended Kalman filter,RAEKF)的SITAN算法,针对IMU运动学模型误差问题,计算自适应因子调节状态预测协方差矩阵;在此基础上,针对自适应因子的失效问题,通过求解抗差因子剔除粗差;最后利用重力异常模型数据开展仿真实验。

1 SITAN算法数学模型

SITAN算法首先根据惯导指示位置,在重力异常模型中得到该处的重力异常值;然后利用搭载的水下重力传感器测量得到运载体航行位置处的真实重力异常值,将二者的差值作为Kalman滤波的量测输入;最后结合惯导误差方程和重力异常随机线性化技术,构建状态方程和量测方程。

1.1 状态方程

汪凤林等[13]对重力匹配惯性导航系统的可观测性进行研究,结果表明,系统状态量需要取平面位置误差。基于此,本文取水下运载体的平面位置误差X=(δφ,δλ)T作为滤波状态量,δφ、δλ分别为纬度、经度误差。

INS位置误差微分方程为:

(1)

式中,δvN、δvE分别为N、E向的速度误差,RM、RN分别为子午圈、卯酉圈曲率半径,δφ、δλ和δh分别为纬度、经度和高度误差。

若不考虑速度误差和高度误差的影响,式(1)可简化为:

(2)

令

(3)

则式(2)可表示为:

(4)

对式(3)进行离散化,得到SITAN算法的滤波状态方程:

Xk=Φk/k-1Xk-1+Wk-1

(5)

式中,Φk/k-1为一步转移矩阵,Φk/k-1=I+F(tk-1)Ts,Ts为离散化时间间隔,Xk、Xk-1分别为k、(k-1)时刻的状态向量,Wk-1为(k-1)时刻的状态噪声。

1.2 量测方程

采用SITAN算法进行匹配时,首先要建立重力异常与状态位置的线性关系,即重力异常随机线性化。在量测更新时刻,对水下运载体航行位置(φ,λ)处的真实重力异常在惯导指示位置(φi,λi)处进行一阶泰勒级数展开:

Δg(φ,λ)=ΔgM(φi,λi)+

(6)

式中,Δg(φ,λ)为水下运载体航行位置处的真实重力异常,ΔgM(φi,λi)为根据惯导指示位置在重力异常基准图上读取的重力异常,∂ΔgM/∂φi为邻域内重力异常纬向梯度,∂ΔgM/∂λi为邻域内重力异常径向梯度,vi为泰勒级数展开过程中的截断误差和重力异常基准图误差。

水下运载体真实位置处的重力异常可表示为实测重力异常与量测噪声之和:

Δg(φ,λ)=Δgs(φt,λt)+vs

(7)

式中,Δgs(φt,λt)为水下运载体实际观测重力异常,vs为水下重力传感器量测噪声。

由式(6)和式(7)可得:

ΔgM(φi,λi)-Δgs(φt,λt)=

(8)

令

(9)

整理式(9)可得SITAN算法的滤波量测方程为:

Zk=HkXk+Vk

(10)

2 基于RAEKF的SITAN算法

SITAN算法的滤波递推方程如下:

(11)

精确的滤波模型和误差统计特性是卡尔曼滤波最优估计的基础。但在实际的水下组合导航应用中,滤波数学模型与真实模型存在偏差,且观测值会受到外界环境的影响,使得异常观测值的存在不可避免,若直接进行解算将会导致导航解次优甚至滤波发散[14]。为此,本文采用基于RAEKF的SITAN算法,其原理是利用自适应因子和抗差因子调节预测信息和观测信息对滤波结果贡献的权重[15]:

(12)

(13)

(14)

(15)

抗差因子选择IGGⅢ函数模型:

(16)

(17)

预测残差只有在观测值不包含粗差的情况下才能反映状态预测信息的异常,即在量测信息可靠的情况下自适应因子才有效。因此,在计算自适应因子之前,应判别观测值是否包含粗差,若包含粗差,则直接选择抗差滤波进行解算;若不包含粗差,则选择自适应滤波进行解算。当预测残差统计量大于阈值时,采用自适应因子抑制IMU运动学推算误差。

基于RAEKF的SITAN算法流程见图1。

图1 算法流程Fig.1 Flow chart of algorithm

3 实验验证与分析

3.1 仿真实验设计

为验证匹配算法的有效性,选取南海一块重力异常变化丰富的海域开展仿真实验。重力异常基准图采用DTU17重力异常模型,分辨率为1′×1′,中误差为2～3 mGal。初始状态量为X0=(0,0)T,P0=10-5I,Qk=0.03I,Rk=0.25。通过在重力异常基准图数据上添加中误差为3 mGal的高斯白噪声,可以得到水下运载体的重力异常观测数据。图2为实验海域的重力异常三维模型图。

图2 实验海域重力异常三维模型Fig.2 3D model of gravity anomaly in the test area

利用航迹仿真算法生成参考航迹,时长为24 h,初始位置为(7°N,113°E,-100 m),初始速度为(0 m/s, 0 m/s,0 m/s),初始姿态为(0°,0°,45°)。本次实验水下运载体真实航迹运动过程如下:首先以0.008 6 m/s2的加速度加速600 s,速度达到10 kn后匀速行驶28 110 s;然后左转45°,匀速行驶28 800 s;最后右转45°,匀速行驶28 890 s。设置惯导采样频率为100 Hz,基于生成的真实航迹,利用捷联惯导反演算法生成航迹上各采样点处的惯性器件输出,即角增量和速度增量。将生成的惯性器件输出表1所示的误差参数,模拟真实环境下的惯导元器件输出,利用双子样算法进行惯导解算,生成惯导航迹。

表1 惯性器件误差参数Tab.1 Inertial device error parameters

结合载体航行速度和重力异常基准图分辨率,设置量测更新间隔为180 s,分别利用常规SITAN算法(EKF算法)和抗差自适应SITAN算法(RAEKF算法)展开重力匹配仿真实验。为对比2种算法的抗差效果,在第160～240次量测更新之间,每隔5次在原始观测值上加入30 mGal的粗差,对应滤波时段为8 ～12 h。

3.2 常规SITAN算法仿真

真实航迹、惯导航迹以及EKF算法匹配航迹见图3。利用EKF算法处理带有粗差观测值的匹配航迹(EKF-Outlier)、惯导航迹(INS)与EKF算法匹配航迹的位置误差见图4。由图3和图4可知,在观测值未加入粗差的情况下,EKF算法能够有效修正惯导整体航迹,其匹配结果定位精度优于INS精度;在观测值加入粗差的情况下,滤波时长小于8 h,匹配航迹能收敛于真实航迹,观测值加入粗差后,滤波在纬度和经度方向上逐渐发散,导航定位精度与INS精度相当,滤波器失效。

图3 常规SITAN算法匹配航迹Fig.3 The matching tracks of ordinary SITAN algorithm

图4 航迹位置误差Fig.4 Position error of tracks

对EKF算法匹配结果的位置误差进行统计,并与INS的位置误差进行对比,结果见表2(单位n mile)。由表2可知,观测值存在粗差的情况下,EKF算法导航定位误差最大值为9.93 n mile,RMS为5.01 n mile;INS导航定位误差最大值为10.24 n mile,RMS为6.16 n mile,滤波结果失真。由此可知,EKF算法无法抵抗异常观测值的影响。因此,需要构建自适应因子和抗差因子实现状态预测信息的自适应控制以及粗差剔除。

表2 误差统计Tab.2 Error statistics

3.3 基于RAEKF的SITAN算法仿真

RAEKF算法匹配航迹以及航迹位置误差见图5和图6,其中,RAEKF-Outlier为利用RAEKF算法处理带有粗差观测值的匹配航迹。由图5和图6可知,在观测值未加入粗差的情况下,RAEKF算法能显著降低惯导累积误差的影响,修正惯导整体轨迹,且RAEKF算法的匹配精度远优于INS。相较于EKF算法,RAEKF算法的导航定位精度也有提升。在观测值加入粗差的情况下,RAEKF算法的效果更加显著。由此可见,在自适应因子和抗差因子的作用下,系统的稳健性和精度均有提高。

图5 基于RAEKF的SITAN算法匹配航迹Fig.5 The matching tracks of SITAN algorithm based on RAEKF

图6 航迹位置误差Fig.6 Position error of tracks

RAEKF算法匹配结果位置误差统计见表3(单位n mile)。由表2和表3可知,在观测值未加入粗差的情况下,RAEKF算法相较于EKF算法的导航定位精度提升了1.6%;在观测值加入粗差的情况下,RAEKF算法相较于EKF算法在纬向和经向上的定位精度分别提高了49.5%和81.6%,整体导航定位精度提高了73.9%,滤波器运行平稳。由此可见,RAEKF算法能有效抑制观测粗差的影响。

表3 误差统计Tab.3 Error statistics

4 结 语

针对水下重力匹配导航中存在观测粗差和动力学模型误差的问题,提出一种抗差自适应SITAN算法,并利用重力异常模型数据开展水下重力匹配仿真实验。结果表明,在观测值存在粗差的情况下,抗差自适应SITAN算法的定位精度可达到1.31 n mile,较常规SITAN算法的精度提升73.9%。