基于状态变换卡尔曼滤波的DVL/SINS组合导航算法

靳凯迪, 柴洪洲,*, 宿楚涵, 向民志, 李 明

(1. 战略支援部队信息工程大学地理空间信息学院, 河南 郑州 450001;2. 郑州航天宏图北斗应用技术研究院有限公司, 河南 郑州 450001)

0 引 言

水下无人自主航行器(unmanned underwater vehicle, UUV)在海洋测绘、水下资源探索和水下目标探测等任务中发挥着越来越重要的作用。获取高精度的姿态、速度和位置等导航参数是UUV高效和安全完成任务的重要前提[1-3]。捷联惯性导航系统(strapdown inertial navigation system, SINS)具有自主性、隐蔽性和主动性等优势,可为UUV提供丰富的导航参数[4-5]。然而,受惯性测量元件(inertial measurement unit, IMU)误差的影响,SINS定位误差会随时间迅速累积,因此必须使用辅助传感器修正SINS累积误差。

多普勒计程仪(Doppler velocity logger,DVL)/SINS组合导航系统是应用最为广泛的UUV水下定位系统[6-7]。根据传感器耦合方式的不同,DVL/SINS组合导航可分为松组合和紧组合[8],二者分别使用DVL三维速度和波束频移作为Kalman滤波的量测信息[9-12]。然而,由于当前大部分DVL仅能提供三维速度,在实际情况中应用最为广泛的仍为松组合方式。近年来,许多学者针对DVL/SINS组合导航Kalman滤波中的DVL误差模型、非完整性约束和自适应滤波等问题展开了广泛研究,有效提升了DVL/SINS组合导航系统的定位性能[13-16]。

DVL/SINS组合导航卡尔曼滤波中的状态方程由SINS误差方程构成。根据姿态误差表达方式的不同,SINS误差方程可分为欧拉角误差模型和四元数误差模型[17-18]。针对传统SINS误差模型不严密的问题,文献[19]提出了一种基于矢量运算坐标系一致的误差构建思想。在其基础上,文献[20]指出,传统SINS速度误差的定义只考虑了实际速度与理论速度大小的差异,而忽略了矢量坐标系不一致的影响。进一步,文献[21]通过定义新的速度误差提出了状态变换卡尔曼滤波,并将其成功应用于全球卫星导航(global navigation satellite system, GNSS)系统/SINS组合导航、视觉/SINS组合导航和SINS初始对准等陆地场景[22-23]。文献[24]和文献[25]将速度误差定义在计算导航系中,分别推导了GNSS阻尼SINS非线性误差模型的欧拉角形式和四元数形式,并将其应用于GNSS/SINS大失准角对准中。

本文延续统一矢量运算中坐标系的思想,将状态变换卡尔曼滤波引入DVL/SINS组合导航。首先,指出传统SINS误差方程中姿态误差方程存在的坐标系不一致问题,进而构建了更为严密的SINS误差方程;进一步,基于新定义的速度误差推导了改进的DVL/SINS组合导航卡尔曼滤波状态方程和量测方程;最后,利用仿真数据和船载实测数据验证了本文所提算法的有效性。

1 坐标系定义

本文坐标系定义如下:

(1) 导航坐标系(n系):定义“东-北-天(E-N-U)”当地水平坐标系为导航坐标系;

(2) 计算导航坐标系(n′系):SINS实际计算时使用的导航坐标系,n′系与n系间的欧拉角称为失准角φ;

(4) DVL坐标系(d系):原点为DVL的几何中心,x-y-z轴分别指向DVL框架的“右-前-上”方向,d系与b系间的欧拉角称为DVL的安装偏差角η,b系原点至d系原点的位移称为DVL的杆臂误差δl;

(5) 地球坐标系(e系):原点位于地球质心,x轴指向赤道与本初子午线的交点,z轴指向北极点,y轴与x轴和z轴构成右手坐标系;

(6) 地心惯性坐标系(i系):原点位于地球质心,x轴指向春分点,z轴指向北极点,y轴与x轴和z轴构成右手坐标系,i系不随地球自转而转动。

2 状态变换SINS误差模型

2.1 传统SINS误差模型

SINS姿态可表示为欧拉角或四元数。在小失准角假设下,SINS的欧拉角误差模型和四元数误差模型均为线性,但四元数各分量的物理意义不明确,而且增加了误差模型的维度,因此本文选择欧拉角(失准角φ)描述SINS的姿态误差。

传统SINS姿态误差模型为

(1)

速度误差模型为

(2)

位置误差模型为

(3)

式中:δp=[δL,δλ,δh]T为SINS的位置误差;L、λ和h分别为SINS的纬度、经度和高程;Mpv和Mpp的具体形式为

(4)

(5)

式中:RMh=RM+h;RMh=RN+h;RM和RN分别为SINS所在位置的子午圈曲率半径与卯酉圈曲率半径。

2.2 改进的SINS误差模型

(6)

(7)

其中,

(8)

(9)

根据式(6)定义的速度误差,可将传统速度误差表示为

(10)

将式(10)代入式(7)可得

(11)

(12)

将式(11)代入式(1),可得

(13)

式中:

(14)

将式(14)代入式(13),得到新的姿态误差方程:

(15)

式中:Map=M1+M2。

对式(6)两侧微分,可得

(16)

结合式(2)、式(13)和速度微分方程,式(16)可改写为

(17)

将式(10)和式(11)代入式(17),有

(18)

由于

(19)

将式(14)和式(19)代入式(18)中并化简,得到新的速度误差方程:

(20)

将式(10)代入式(3),得到新的位置误差方程:

(21)

3 DVL/SINS组合导航卡尔曼滤波

3.1 卡尔曼滤波状态方程

DVL测速精度受水体温度、盐度和波束宽度等因素影响,在实际应用中一般将DVL观测值建模为

(22)

式中:δk是建模为常值的刻度因子;wd为高斯白噪声。

考虑DVL和SINS的误差参数以及d系与b系间的安装偏差角和杆臂误差,在DVL/SINS组合导航卡尔曼滤波中选择22维状态向量:

(23)

因此,Kalman滤波状态方程为

(24)

式中:Ft是由SINS误差方程构成的状态转移矩阵;Gt为系统噪声驱动矩阵;Wb为由陀螺和加速度计的白噪声构成的状态噪声。

根据式(15)、式(20)和式(21),状态转移矩阵和系统噪声驱动矩阵的具体形式为

--------------------

(25)

--------------------

3.2 卡尔曼滤波量测方程

DVL/SINS松组合中以三维速度作为卡尔曼滤波的量测信息。考虑DVL的安装偏差角与杆臂误差,由式(22)可得

(26)

由式(24),得到DVL/SINS组合导航量测方程为

(27)

综上,DVL/SINS组合导航卡尔曼滤波数据融合过程如图1所示。

图1 DVL/SINS组合导航数据融合过程

4 实验结果与分析

4.1 仿真实验

为验证DVL/SINS状态变换Kalman滤波算法的有效性,首先设计如图2所示的仿真轨迹进行实验验证。仿真轨迹总时长为800 s,包含如表1所示的加速、减速、匀速、横滚和转向等典型UUV运动状态。轨迹初始位置为北纬30°、东经120°、高程0 m,初始速度为0 m/s,初始姿态角为[0°,0°,0°]。轨迹的初始水平失准角设为5′,初始天向失准角为30′,E-N-U3个方向的初始速度误差均为1 m/s。

表1 仿真轨迹运动状态

图2 UUV仿真轨迹

为说明本文算法的有效性,使用常规DVL/SINS组合导航Kalman滤波(以“常规算法”表示)作为对比[28-29]。在相同条件下执行两种算法,分别进行50次蒙特卡罗仿真实验。

在姿态误差上,由于两种算法的水平姿态角估计精度均较高,在此仅对比天向失准角的估计效果。图3给出了50次蒙特卡罗实验中两种算法的天向失准角平均值曲线。从图3可见,本文算法的收敛速度更快。在0.5°初始天向失准角下,本文算法的平均天向失准角在280 s左右收敛到0.1°以下,而常规方式在560 s左右收敛到0.1°以下。导航结束时,常规算法的天向失准角为0.045°,本文算法的天向失准角为0.024°。因此,本文算法在一定程度上提高了DVL/SINS组合导航的姿态精度。

图3 天向失准角平均值

DVL/SINS松组合直接以三维速度作为量测值,组合导航系统中速度误差具有较强的可观测性。图4给出了两种方案的E-N-U 3个方向速度误差的平均误差曲线。从图4可见,两种算法的速度误差在DVL的约束下迅速收敛,且本文算法的水平速度估计效果优于常规算法,在测速精度和稳定性上均有一定程度的提升。经过初始收敛阶段,在E-N方向上,常规算法的最大速度误差分别为0.214 m/s和-0.069 m/s,而本文算法分别为0.142 m/s和-0.031 m/s,本文算法在导航前期有较高的速度精度。随着导航进行,二者差异逐渐减小,在对准结束时,两种算法在E-N方向上的速度误差均为0.003 m/s和0.009 m/s。对比式(2)和式(20)可见,由于在新的速度误差方程中由更稳定的重力矢量代替了式(2)中的比力信息,因此Kalman滤波状态转移矩阵更加稳定。

图4 速度平均误差

图5给出了两种方案E-N-U 3个方向的平均位置误差曲线。由于DVL/SINS系统中位置误差不可观,因此解算的位置误差是速度误差累积的结果。从图5可见,常规算法的E-N-U 3个方向的最大位置误差分别为13.1 m、-8.9 m和-2.4 m,本文算法3个方向的最大位置误差分别为8.1 m,-5.3 m和-2.6 m。在导航初始阶段,本文算法的位置误差发散得到了一定程度的改善。在导航结束时,两种算法的定位误差的差异在毫米级。

图5 位置平均误差

图6～图8给出了两种方案的DVL的刻度系数、安装偏差角和杆臂误差的50次实验平均值曲线。从图中可见,对于可观性较强的状态参数,如x轴-z轴安装偏差角、刻度系数和杆臂误差,两种算法的估计效果较为接近,均可估计出添加的误差参数。对于可观性较差的状态参数,两种算法有一定差异,且均无法准确估计。

图6 DVL刻度系数平均值

图7 DVL安装偏差角平均值

图8 DVL杆臂误差平均值

4.2 船载实验

图9 船载平台及设备

实验使用高精度后处理软件Novatel Inertial Explorer(IE)的GNSS-RTK/SINS前后向平滑紧组合的输出作为参考基准。由IE结果获得轨迹的初始位置为北纬30.577°,东经114.472°,高程19 m。船载轨迹如图10所示。

图10 武汉湖试轨迹

为了避免DVL杆臂误差、安装误差角和刻度系数对导航参数的影响,使用文献[30]算法对实测数据中的DVL误差参数进行了预标定。实验中,在IE参考值的基础上,设置轨迹的初始水平失准角为0.5°,初始天向失准角为1°,E-N-U 3个方向速度误差均为1 m/s。

使用常规算法与本文算法分别对该轨迹进行处理。图11给出了两种算法估计的天向失准角对比。从图11可见,在导航前期二者的差异较为显著,本文算法的最大天向失准角误差为-2.390°,常规算法为-4.199°。随着导航进行,二者差异逐渐减小,在导航结束时常规算法和本文算法的天向失准角分别为-0.040°和0.017°。因此,在补偿了坐标系不一致误差后,组合导航系统估计的姿态精度更高。

图11 湖试中的天向失准角

图12给出了两种算法的速度误差估计曲线。从图12可见,两种算法的天向速度有基本一致的估计精度。在E-N方向上,常规算法的最大速度误差分别为-0.214 m/s和0.172 m/s,而本文算法的最大速度误差分别为-0.105 m/s和0.098 m/s。因此,本文算法在导航前期的测速精度较高,随着导航进行,两种算法速度误差的差异逐渐减小。

图12 湖试中的速度误差

图13给出了两种算法的位置误差估计曲线。从图13可见,常规算法和本文算法的水平最大位置误差分别为18.700 m和14.239 m。当导航结束时,常规算法和本文算法的水平位置误差分别为4.937 m和3.109 m。这是由于本文算法的速度误差较小,而位置误差受速度误差累积的影响。统计两种算法估计的姿态、速度和位置均方根差(root mean squared error, RMSE)如表2所示。从表2可见,本文算法的姿态、速度和位置的精度较常规算法均有一定程度的提升。其中,天向失准角RMSE降低了43%,东向速度RMSE降低了34%,北向速度RMSE降低了31%。本文算法的东向位置误差和北向位置误差的RMSE分别降低了28%和10%。

表2 湖试中导航参数RMSE统计结果

图13 湖试中的位置误差

5 结 论

为了提高DVL/SINS组合导航系统的定位精度,考虑SINS速度误差中坐标系不一致的问题,提出了一种基于状态变换Kalman滤波的DVL/SINS组合导航算法。在分析传统SINS误差方程中存在坐标系不一致误差的基础上,详细推导了改进的状态变换SINS误差方程;进一步地,构建了DVL/SINS组合导航Kalman滤波方程。实验结果表明,该算法可以有效处理传统速度误差中坐标系不一致的影响,在一定程度上提高了DVL/SINS组合导航系统的精度。