张 涛,王 帅，刘兴华

（1.东南大学仪器科学与工程学院,南京 210096;2.微惯性仪表与先进导航技术教育部重点实验室,南京 210096）

现代舰船都装备有各种高精度导航设备，为了能使这些设备正常协调工作，必须有一个统一的空间坐标基准系统。但是，由于舰船并不是一个绝对刚体以及船体自身重量和温度、海浪冲击等外部环境的影响，使得船体和甲板产生微小变形。根据俄罗斯学者A.V.Mochalov所述，舰船甲板变形角最高可达1°～1.5°，动态挠曲变形也可达角分量级[1]。由于船体变形的影响，使舰船设备获得的姿态信息不再准确，而船载设备一般都是高精度设备，微小的变形也会使设备性能受到严重影响。因此，对船体变形角进行实时测量和补偿，以保证船载设备在统一的基准坐标系下完成高精度工作成为近年来国内外研究的一个热点问题[2]。

目前，国内外对船体变形角采用的有效测量手段有光学测量法、惯性测量法、GPS测量法等。其中光学测量法使用光学设备对船体甲板变形角进行实时监测，测量精度高，理论精度能够达到5角秒以内，测量结果可以近似为船体的实际变形[3]。但是光学测量法成本高昂，结构复杂且实施困难，不能大规模使用。由于惯性系统体积小、精度高、隐蔽性好等优点，使惯性测量匹配法成为目前最具有发展潜力的研究方向，其思想是通过在中心主惯导系统附近和局部战位点附近分别安装一套惯性系统，两个惯导之间输出的信息差能够反映二者失准角的大小，通过二者输出的信息进行动态匹配实现船体变形的测量。

基于姿态匹配的船体变形测量方法最早由国防科技大学提出[4]，之后对激光陀螺的误差进行补偿[5][6]以及对模型参数的辨识展开了研究[7]。但是其采用的动态变形角模型是非时变二阶马尔科夫模型，没有考虑模型偏差带来的误差，而实际船体动态变形模型复杂多变且惯性系统受到的噪声不单一。针对变形角模型不确定的问题，哈尔滨工程大学提出一种交互式多模型滤波的船体变形测量方法[8]，但是由于先验信息的不准确使得状态转移概率很难定义，而且多次更新卡尔曼滤波方程需要大量的计算时间。文献[9]提出一种基于姿态四元数匹配的神经网络船体变形测量方法，并对卡尔曼滤波中的时间延迟进行了补偿[10]，避免了历史变形数据对实时变形影响导致的误差。但是计算神经网络系统最优参数需要大量的训练数据并且可能会导致过度学习。

基于多重渐消因子的强跟踪卡尔曼滤波（Strong Tracking Kalman Filter，STKF）采用使残差序列正交的方法确定最佳的多个渐消因子,在模型失配的情况下仍能很好地跟踪状态量的变化，该算法因有较强的鲁棒性并且得到广泛应用[11]。在实际的船体变形测量中，由于环境的复杂性，惯性设备的误差通常是各种噪声的叠加，它们极少服从正态分布，有的幅值较大甚至还有奇异点。而传统的卡尔曼滤波以最小均方误差（MMSE）为最优准则，不能反映真实的噪声情况，会产生较大误差。最大互相关熵卡尔曼滤波以最大相关熵准则（Maximum Correntropy Criterion，MCC）为最优准则，不仅能捕获通常的二阶统计信息，而且还可以获得更高阶的统计信息，因此能够获得更好的估计效果[12]。本文设计了一种强跟踪最大互相关熵卡尔曼滤波（Strong Tracking Maximum Correntropy Kalman Filter，STMCKF）算法，与传统卡尔曼滤波相比，当模型不确定或存在未知干扰时，能够更有效的估计船体变形角。

1 船体变形测量模型

船体变形测量系统由两套惯性系统（Inertial navigation system，INS）组成，如图1所示。在中心惯导附近安装一个惯性系统（SINS1），局部战位点安装另一个惯性系统（SINS2），定义坐标系分别为ox1y1z1和ox2y2z2，x轴沿着船体的右舷，y轴指向船艏方向，z轴与船体甲板平面垂直并指向上方。xyz构成右手直角坐标系，即为右前上坐标系。

图1 船体变形测量系统构成Fig.1 Composition of hull deformation measurement system

令SINS1的载体坐标系为b1，并选择初始时刻t0的载体坐标系b1作为SINS1的惯性坐标系i1。同样，令SINS2的载体坐标系为b2，选择初始时刻t0的载体坐标系b2作为SINS2的惯性坐标系i2。则表示i2系到i1系的坐标变换矩阵，反映的是t0时刻的变形角。为系到b1系的坐标变换矩阵，反映t时刻的变形角。由于陀螺偏移，导致解算出的惯性系与真实的惯性系i1，i2之间存在偏差，把解算得到的惯性系称为计算惯性系，简记为i1′，i2′。则根据坐标系的定义，旋转矩阵之间的关系为：

记t时刻的变形为φ=[φx,φy,φz]T，当φ较小时，近似为：

记t0时刻的失准角为φ0=[φ0x,φ0y,φ0z]T，其包含有初始对准误差和初始变形角，当φ0较小时，近似有：

记对应的欧拉角分别为θi1和θi2，当θi1和θi2较小时，近似有：

将式（2）（3）（4）代入（1），即得：

记和分别为：

将式（6）代入式（5），根据矩阵两边的元素(1,2)、(3,1)、(3,2)相等分别建立等式，化简并忽略二阶小量得到：

式中，矩阵Z和A由SINS1、SINS2的陀螺输出和决定并随其改变，具体表示为：

,θi2表示由陀螺漂移引起i′系与i系的偏差，惯性空间失准角与陀螺漂移之间的关系为：

式中ε1、ε2分别为SINS1和SINS2的陀螺漂移。

根据船体变形频谱分布的不同将其分为静态变形和动态变形[8]，其中静态变形在短时间内可以看作常值，动态变形角满足二阶Markov过程，其模型为：

式中，βi是挠曲变形的模型参数，其大小与相关时间有关，具体表述为：

τi是船体变形的相关时间，ηi是白噪声，其方差Qηi的大小为：

σi是动态变形角θi的方差。

影响船体变形测量精度的一个主要因素是陀螺偏移，根据陀螺漂移的特性，一般将其分为随机常值误差和随机游走误差，分别建立模型为：

式中，w(t)～(0,σ2)，T为相关时间，由于相关时间很大，随机游走误差可以简化为：

选取系统状态向量为18维，即：

式中：Φ为静态变形角，θ为动态变形角，为动态变形角速度，φ0为t0时刻的失准角，ε1、ε2分别为SINS1和SINS2的陀螺漂移。

将式（7）中的Z作为观测量，则系统的观测方程可表示为：

考虑到φ=Φ+θ及式(8)，观测矩阵可表示为：

2 强跟踪最大熵卡尔曼滤波算法

2.1 模型误差对估计结果的影响

实际船体变形模型中，由于导致船体变形的因素复杂多变，没有一个固定的形式，采用一个时不变的动态变形模型必然会产生误差。在先验估计中引入偏差项推导模型偏差对动态变形角的估计误差[13]，具体表述为：

式中：ΔXk代表先验估计和真实状态Xk的动态模型偏差，代入卡尔曼滤波标准公式：

得到：

与式（18）相比，动态模型偏差ΔXk导致的后验估计偏差为：

从式（20）可以看出，由ΔXk导致的估计误差与增益矩阵K和观测矩阵H是紧密相连的，如果在时变观测系统中不考虑ΔXk导致的误差，可能会对测量精度带来较大误差，甚至在有些特殊情况会导致系统不可观测。因此，在进行观测更新之前，需要对模型偏差进行修正。

2.2 多渐消因子强跟踪滤波算法

传统卡尔曼滤波具有记忆性，综合利用了所有的量测值Zk，过去的量测数据会直接影响现在时刻状态的估计精度，由于模型存在偏差，会使方差阵Pk不能正确反映状态的估计精度，造成较大误差。多渐消因子强跟踪滤波通过对各数据通道以不同的速率进行渐消，更准确地调整增益矩阵，使系统具有较强的跟踪能力。

当模型有偏差时，会使式（21）中等号两边不相等，导致新息方差矩阵失衡。为使式（21）等号两边相等，将Pk,k-1重写为：

式中Λk=diag(λ1λ2…λk)表示状态变量相应的渐消因子组成的对角阵。

则式（21）可以重新表述为：

式中为k时刻新息序列γk的协方差矩阵估计值，可以根据式(24)进行估算：

式中ρ为遗忘因子，0 ＜ρ≤1，通常ρ= 0.95。

进一步，令

故可得

若Hk满秩，即m=rank(Hk)，且Hk有j1,j2,…,jm共m列元素不全为0，则Mk可以简化为：

式中，S为Hk中不全为0的m列元素组成的矩阵，Jm为Jk中第j1,j2,…,jm行和第j1,j2,…,jm列元素组成的矩阵。

将式（27）代入式（26）并整理可得：

令βk=S-1Nk(ST)-1，因此多重渐消因子求法为：

式中βk(i,i)和Jk(ji,ji)分别为矩阵βk和Jk的对角线元素值。

2.3 最大相关熵卡尔曼滤波

对于两个服从联合概率密度为FX,Y(x,y)的随机变量X,Y∈R，其互相关熵定义为：

其中，E(·)表示求期望，κ(·)表示高斯核函数，即

式中，e=x-y，σ＞ 0为核函数宽度。

将系统状态方程和量测方程重新表述为：

式中：

分别将Pk,k-1和Rk进行Cholesky分解得到Bp,(k,k-1)和Br,k，则可得：

在式（32）左右两边同时乘，并重写为：

式中：

定义基于信息最大熵准则下的代价函数为：

式中，n+m是Dk的维数，di,k和wi,k分别是Dk与Wk的第i个元素，Xk的最优解可以通过使代价函数最大求得[14]，即

因此，可以通过式(38)得出Xk的最优解：

求解式(28)可得

式中xi,k是Xk的第i个元素。

进一步，定义：

将式（38）写为矩阵形式为：

从式（40）可以看出，基于信息最大熵准则的滤波方法利用Ck对误差协方差重新加权并重新构建量测信息，具体表述为：

3 仿真实验与结果分析

3.1 仿真条件

（1）舰船以10 m/s的速度匀速直线航行，以正弦规律绕横摇轴、纵摇轴、航向轴作三轴摇摆，相应的摆动幅值分别为3 °、4 °、5 °，摆动周期分别为10 s、8 s、6 s，随机选取初始航向角为北偏东60 °，采样频率为100 Hz。仿真轨迹如图2所示。

（2）认为船体静态变形角是常值，分别设置为0.2 °、0.1 °、0.05 °。船体动态变形角为二阶马尔科夫随机过程，变形角方差σ为5 ″、6 ″、7 ″，相关时间τ为1 s、1.5 s、1.8 s。

（3）SINS1、SINS2的陀螺常值漂移都取为0.05°/h,随机游走误差都取为

图2 仿真轨迹图Fig.2 Simulation trajectory

3.1 仿真结果与分析

设仿真时间为900 s，为验证本文提出算法的有效性，在三种环境下进行仿真验证，环境设置如表1所示。

表1 三种环境设置表Tab.1 Three environment settings

在第一种情况中，假设在300～600 s的时间段内，系统的真实噪声变为25Q，强跟踪最大熵卡尔曼滤波和传统卡尔曼滤波的变形角估计误差如图3～5所示。在第二种情况中，假设在300～400 s的时间段内，系统的动态变形角模型发生变化，将动态变形角方差σ改为2σ，对应的变形角估计误差如图6～8所示。在第三种情况中，假设惯性系统噪声不是高斯白噪声，而是重尾的混合高斯分布，SINS1、SINS2的随机噪声设置为：

对应的仿真结果如图9～11所示。三种环境下的均方根误差如表2所示（20 s之后稳定求得）。

表2 变形角均方根误差Tab.2 Deformation Angle root mean square error(")

图3 情况一纵摇角变形估计误差Fig.3 pitching angle deformation estimation error in case-1

图4 情况一横摇角变形估计误差Fig.4 rolling angle deformation estimation error in case-1

图5 情况一航向角变形估计误差Fig.5 yawing angle deformation estimation error in case-1

从图3～5可以看出，在情况一中，当系统噪声突然变大时，变形角估计误差变大，其中航向角误差变化最大，强跟踪最大熵卡尔曼滤波比传统卡尔曼滤波的估计精度有明显提高，尤其航向角最为明显。结合表2数据，纵摇角、横摇角和航向角的估计精度分别提高8.42%、22.41%和30.29%。由此可知，强跟踪最大熵卡尔曼滤波可以更有效地跟踪系统噪声突变状态，同时，由于系统噪声突变和滤波器惯性等原因，导致600 s后传统卡尔曼滤波航向角估计误差出现一个偏置，但强跟踪最大熵卡尔曼滤波的收敛误差可以减少30.41″，使偏置减小。

图6 情况二纵摇角变形估计误差Fig.6 pitching angle deformation estimation error in case-2

图7 情况二横摇角变形估计误差Fig.7 rolling angle deformation estimation error in case-2

图8 情况二航向角变形估计误差Fig.8 yawing angle deformation estimation error in case-2

从图6～8可以看出，在情况二中，变形角估计误差在300s时有突变的现象，这是因为在300～400s的时间内变形角模型突变使数据不连续导致的。结合表2数据，纵摇角、横摇角和航向角的估计精度分别提高11.78%、24.14%和12.82%，由此可知，强跟踪最大熵卡尔曼滤波比传统卡尔曼滤波的收敛精度提高较大，说明强跟踪最大熵卡尔曼滤波提高了系统的鲁棒性及环境适应性。

图9 情况三纵摇角变形估计误差Fig.9 pitching angle deformation estimation error in case-3

图10 情况三横摇角变形估计误差Fig.10 rolling angle deformation estimation error in case-3

图11 情况三航向角变形估计误差Fig.11 yawing angle deformation estimation error in case-3

在情况三中，仿真试验设置的σ为0.4和0.5，从图9～11可以看出，当σ为0.5时，其效果比σ为0.4时更接近传统卡尔曼滤波，这是因为σ越大，二阶误差矩越占主导地位。结合表2数据，纵摇角、横摇角和航向角的估计精度平均提高了14.17%、11.48%和9.25%，由此可知，当惯性系统的噪声为重尾的混合高斯分布时，通过调节核宽度σ，强跟踪最大熵卡尔曼滤波比传统卡尔曼滤波的估计精度明显提高。

4 结 论

本文针对船体变形测量过程中模型不确定以及受未知噪声干扰导致的误差问题，讨论了模型偏差对滤波估计的影响，设计了一种基于姿态匹配的强跟踪最大熵卡尔曼滤波算法。利用惯性系统输出的姿态信息建立船体变形测量模型，以最大互相关熵为最优准则，不仅可以获得二阶误差矩，还可以获得更高阶误差矩。通过自适应地调整多个渐消因子，即使在滤波达到稳态时，仍然可以调整滤波增益。本文在三种环境下对该算法进行仿真验证，仿真结果表明，相比较传统卡尔曼滤波，强跟踪最大熵卡尔曼滤波在模型不确定以及未知噪声情况下能够取得更好的效果，使其对复杂环境具有更好的适应性，提高了系统的估计精度和鲁棒性。