置信检验自适应联邦卡尔曼滤波及其水下机器人组合导航应用

2022-03-19陈帅王宁陈廷凯杨毅田嘉禾

中国舰船研究 2022年1期

陈帅，王宁*，陈廷凯，杨毅，田嘉禾

1 大连海事大学船舶电气工程学院，辽宁大连 116026

2 中车大连电力牵引研发中心有限公司，辽宁大连 116085

0 引言

水下机器人作为一种智能化设备，已逐渐成为人类开发利用海洋资源的重要工具和海工装备的中坚力量。水下机器人在执行任务时，精准定位是极其重要的一环。因此，提升机器人组合导航系统的精度有着重大的研究意义。目前，GPS/INS是应用最为广泛的组合导航方式，其本质是一种误差特性互补的组合导航，通常与卡尔曼滤波相结合。具体而言，一方面利用GPS信息修正INS的导航结果，以调整其随时间积累的误差；另一方面，利用INS短时间内的高精度定位优势，解决GPS信号在受遮挡、中断等不利条件下的定位问题[1]。需要注意的是，当载体受到外界强扰动或量测信息不可靠情况下，传统卡尔曼滤波效果将会受到影响，不可避免地导致GPS/INS组合导航的精度受到影响。

高精度的组合导航技术引起了国内外学者的广泛关注。Yang等[2]提出了一种基于预测残差协方差阵的联邦卡尔曼滤波自适应因子改进方法，有效提高了联邦卡尔曼滤波的滤波精度；周先林等[3]提出了一种基于新息的自适应卡尔曼滤波算法，该算法通过卡方检验检测出量测异常值，在量测异常值处调整量测噪声方差阵，有效解决了滤波器中出现不可靠测量时滤波发散的问题；吕建新等[4]将里程计与GPS/INS组合导航系统相结合，提出了自适应信息分配因子的策略，提升了组合导航系统的抗扰动能力；Li等[5]针对GPS和INS更新速率不同步问题，提出了一种针对GPS量测值的观测性扩展方法，提升了组合导航系统在载体低速率时的导航精度；Jiang等[6]将澳大利亚的Locata定位系统、精准点定位全球卫星导航系统（PPP-GNSS）和INS相结合，并利用随机向量空间方法设计了一种全局最优滤波器，有效提升了导航精度；Ma等[7]将电磁罗盘（magnetic compass）、地形辅助导航（terrain aided navigation）、多普勒测速仪（Doppler velocity log）与INS相结合，并且利用残差提出了一种自适应联邦卡尔曼滤波算法，有效提升了组合导航系统的稳定性；Liu等[8]提出了鲁棒卡尔曼滤波算法并将其用于联邦卡尔曼滤波器，有效抑制了传感器误差对滤波器结果的影响；Malleswaran等[9]将输入延迟动态神经网络与GPS/INS组合导航系统相结合，在GPS信号不可用时利用神经网络对INS进行补偿，有效提升了组合导航系统的导航精度；Sung等[10]将数字罗盘与GPS相结合，并提出了一种简化卡尔曼滤波器，一定程度上提高了导航精度；Abosekeen等[11]利用简化的惯性传感器系统替代INS，并利用离散余弦变换对数据进行处理，提高了GPS信号被遮挡时的滤波精度；García等[12]通过构造可信度量判断量测信息的可靠性，解决了量测信息不可靠问题对滤波结果的影响。需要注意的是，上述方法在量测值发生异常时，无法解决组合导航系统稳定性变差和滤波精度降低的问题。

为解决上述问题，本文将提出一种置信检验自适应联邦卡尔曼滤波算法，通过构造高斯分布对量测值进行可靠性检验，保证量测值的有效性；根据残差协方差设计局部滤波器的自适应调节因子，对系统噪声协方差进行实时调整，有效提高滤波精度和系统稳定性。

1 INS/GPS/EC联邦滤波

1.1 联邦卡尔曼滤波

联邦卡尔曼滤波的状态方程和量测方程为：

式中：X(k+1)为系统k时刻对k+1时刻的状态估计值；F(k)，X(k) 和G(k)分别为系统在k时刻的状态转移矩阵、状态变量和控制矩阵；w(k)为系统的白噪声矩阵；i=1,2,···,n，为第i个局部滤波器；Zi(k)，Hi(k)和vi(k)分别为系统局部滤波器i在k时刻的量测值、量测矩阵和量测噪声。

联邦卡尔曼滤波器滤波机制主要包括4个步骤：信息分配、信息时间更新、信息量测更新和最优信息融合[13]。

1）信息分配。

假设在初始时刻全局状态的初始值为X0，其协方差矩阵为P0，系统噪声方差矩阵为Q0。将这一信息通过信息分配因子按以下规则分配到各局部滤波器和全局滤波器：

式中：βi为信息分配因子，必须满足信息守恒原则，即 β1+β2+···+βn+βm=1,0≤βi≤1；下标n代表第n个局部滤波器，下标m代表主滤波器。

2）信息时间更新。

时间更新包括2个部分：状态预测和协方差预测。

式中：Pi(k+1|k)，Xˆi(k+1|k)，Fi(k+1|k)和Qi(k+1|k)分别为局部滤波器i的一步预测误差协方差矩阵、状态一步预测、一步转移矩阵和一步预测系统噪声方差矩阵。

3）信息量测更新。

量测更新包括3个部分：卡尔曼滤波增益计算、状态最优估计以及协方差修正。

式中，Ki(k)和Ri(k)分别为局部滤波器i的增益矩阵和量测噪声方差矩阵。

4）最优信息融合。

最优信息融合则是利用各个局部滤波器的协方差和估计值，在主滤波器中进行最优状态估计。

上面4个步骤即为联邦卡尔曼滤波的基本步骤，通过以上步骤，可以实现多测量值系统对状态的最优估计。

1.2 组合导航建模

通常而言，选取惯性导航系统的位置、速度和姿态等15维误差信息作为状态变量，分别为位置误差、速度误差、姿态角误差、陀螺仪漂移误差以及加速度计的零偏误差[14]。故状态变量X(k)为：

式常速度值中计漂：ε 零移bx，偏；εΔb误yb，x，差εbΔz；b分yw，别(Δkb)z为分=[东别ωg、为x北 ω东g、y、天 ω北g方z、 ω向天ax的方ω陀向ay螺的ωaz仪加]T为白噪声矩阵，其中ωgx，ωgy，ωgz分别为东、北、天方向的陀螺仪白噪声，ωax，ωay，ωaz为东、北、天方向的加速度计一阶马尔科夫过程白噪声；状态转移矩阵F(k)和控制矩阵G(k)可参考文献[15]。

在建立组合导航系统的状态方程后，需建立观测方程对组合导航系统的位置、速度、姿态信息进行观测。惯性导航系统能够测量水下机器人的位置、姿态、速度信息，全球定位系统能够测量水下机器人的位置、速度信息，电子罗盘能够测量水下机器人的姿态信息。以惯性导航系统作为公共系统，分别求取与电子罗盘、全球定位系统的相同信息差值，选取惯性导航系统和全球定位系统的速度信息和位置信息作为观测变量建立INS/GPS局部滤波器，选取惯性导航系统和电子罗盘的姿态信息作为观测变量建立INS/EC局部滤波器[16]。

1.2.1 INS/GPS局部滤波器

因惯性导航系统和全球定位系统均可直接测得水下机器人在某一时刻的位置和速度信息，故选取惯性导航系统和全球定位系统的位置和速度测量值作为量测值。惯性导航系统和全球定位系统的位置信息可表示为：

式中：X1，Y1，Z1，X2，Y2，Z2分别为惯性导航系统和全球定位系统的经度、纬度、高度的测量值；λ，L，h分别为水下机器人经度、纬度、高度的真实值；δλ ，δL， δh， δX2，δY2，δZ2分别为惯性导航系统和全球定位系统的经度、纬度、高度的测量误差；R为地球赤道半径，约为 6.37×106m。

定义位置观测方程为，

式中，O1×13为零矩阵。

惯性导航系统和全球定位系统的速度信息可表示为

式中：vx，vy，vz，vx2，vy2，vz2分别为惯性导航系统和全球定位系统的东、北、天方向速度的测量值；vxr，vyr，vzr分别为水下机器人东、北、天方向速度的真实值；δvx，δvy，δvz， δvx2， δvy2， δvz2分别为惯性导航系统和全球定位系统的东、北、天方向速度的测量误差。

定义速度观测方程为

1.2.2 INS/EC局部滤波器

通过惯性导航系统和电子罗盘可直接获得水下机器人的三轴姿态角信息，故以两者之差作为观测量，惯性导航系统和电子罗盘的姿态信息可表示为

式中：φx，φy，φz，φx2，φy2，φz2分别为惯性导航系统和电子罗盘的东、北、天方向姿态的测量值；φxr，φyr，φzr分别为水下机器人东、北、天方向姿态的真实值；δφx，δφy，δφz， δφx2， δφy2， δφz2分别为惯性导航系统和电子罗盘的东、北、天方向姿态的测量误差。

姿态观测方程为

2 置信检验自适应联邦卡尔曼滤波

将本文提出的置信检验自适应联邦卡尔曼滤波器应用于水下机器人的INS/GPS/EC组合导航系统中，构成2个局部滤波器和1个主滤波器，INS/GPS/EC组合导航系统结构图如图1所示。

传统联邦滤波器的各子滤波器因为采用了相同的状态方程，若状态方程出现扰动会影响子滤波器的性能，在载体发生扰动异常或存在较大动力学模型误差时，滤波的效果往往不太理想。由图1可知，在本文设计的联邦滤波器中，INS，GPS和罗盘的量测信息需先经过置信检验，可靠的量测值Zi被送入局部滤波器，基于INS/GPS和INS/EC的自适应局部滤波器并行运行，利用自适应调节因子调节系统噪声协方差，得到预测误差协方差矩阵Pi和局部估计值Xˆi，并将其送入主滤波器并进行融合，得到系统状态的最优估计值Pg和Xˆg。同时，为进一步提升滤波精度，本系统选用了有重置结构的联邦卡尔曼滤波器，Xˆg和被放大为β−i1Pg的最优估计值被反馈到局部滤波器，对局部滤波器的估计值进行重置。

图1 INS/GPS/EC组合导航系统结构图Fig.1 Structure diagram of INS/GPS/EC integrated navigation system

2.1 置信检验

由卡尔曼滤波器的结构可知，量测值对下一阶段的估计值具有直接影响，而对于联邦卡尔曼滤波器而言，当任意一个传感器出现故障或不可靠测量时，主滤波器输出的最优估计值的估计精度都会受损。针对这一问题，本文提出CC-AFKF框架，对每一时刻的量测值均进行一次置信检验，满足条件的量测值认为是可靠测量，可用于进行下一时刻的预测；而不满足条件的量测值利用量测方程进行更改，保证最优估计值的精度。

对任何一个局部滤波器的任意一个时刻而言，下一时刻状态的预测值为

为判定量测值的可靠度，将预测向量中第t维的预测值Xˆit(k+1|k)表示为。计算G(k)w(k)第t维的方差。G(k)w(k)第t维参数的均值 µ为

由系统噪声w(k)的高斯特性，可认为下一时刻斯的分最布优，即估Xˆ计it(k值|k均)～服N从(X¯均it,σ值2)为。X引¯it，入方一差个为置σ2信的概高率α，为使得Zti(k)处于置信区间内，根据标准正态分布的分布函数 Φ(x)，量测值的边界取值、均值、方差和置信概率的关系可表示为

因传感器每次测量时，真实值均服从正态分布[17]，所以根据传感器的测量特性，x1和x2应满足x1+x2=2X¯it，当Zti(k)∈(x1,x2]时，可认为下一时刻的Zti(k)属于可靠测量，能够用于接下来的预测环节；当Zti(k)∉(x1,x2]时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=HX(k)+v(k)获得。

同时，根据正态分布的“3σ”法则，当α设置为0.27%时，置信检验可以过滤掉传感器在出厂误差范围之外的量测值。但在实际应用场合中，为提高融合精度，可通过重复试验合理选取α值。

2.2 自适应局部滤波器

在对量测值进行了置信检验后，确保了每次用于下一阶段预测的量测值的可靠性，保证了滤波器的稳定性，进而考虑设计自适应调节因子对滤波器中的相关变量进行实时调整，提高滤波器的滤波精度。

式中，P(k+1|k)为预测误差协方差矩阵在计算区间内的估计值。kt−k0一般取20，当残差的实际协方差与理论协方差保持一致时，卡尔曼滤波算法的精度最高[18]。根据残差的实际协方差和理论协方差，可用两者的迹tr的比值η来衡量真实值与理论值的偏差程度：

考虑到滤波器结构中包含多个局部滤波器，调整Ri(k)会不可避免地增加系统的运算量，又因为Pi(k+1|k)=Fi(k+1|k)Pi(k|k)FiT(k+1|k)+Qi(k)，故调整残差的实际协方差可通过调整带有调节因子 τ的Qi(k)来实现：

式中，e为自然常数，每次迭代时令Qi(k)=τQi(k−1)，不断调整每次更新过程中的系统噪声协方差，实现自适应状态估计。

本文所提出的置信检验自适应联邦卡尔曼滤波算法流程图如图2所示。

图2 置信检验自适应联邦卡尔曼滤波算法流程图Fig.2 Flow process of confidence check-adaptive federated Kalman filter algorithm

首先设定好滤波器的初值，包括状态初值、系统噪声协方差和量测噪声协方差，其次根据预测值和置信概率进行量测值的置信检验，当量测值属于不可靠测量时，利用量测方程修正量测值，然后计算自适应调节因子实时调节系统噪声协方差矩阵，利用调节后的系统噪声协方差进行状态估计，最后得到卡尔曼滤波结果。

3 试验结果分析

为验证本文所提出算法的有效性和优越性，将本文所提出的CC-AFKF框架与经典的KF，FKF框架在融合精度层面相比较。本次试验地点为大连市凌海港，试验所用的组合导航模块信息如表1所示，水下机器人试验平台如图3所示。

图3 水下机器人试验平台Fig.3 Test platform of underwater vehicle

表1 组合导航模块器件清单Table 1 Parts list of integrated navigation module

试验初始参数设置如下：本次试验仅考虑东向和北向速度，并忽略水下机器人的天向速度[19]，水下机器人的初始位置为东经121.546°，北纬38.872°，初始东向和北向速度均为0 m/s，试验将第三方SBG Ekinox 2的组合导航输出结果（理论定位精度为0.02 m）作为参考值，试验分析的各项定位测速误差都是以该参考值为基准得出。水下机器人真实运动轨迹如图4所示。

图4 水下机器人的真实运动轨迹Fig.4 Real trajectory of underwater vehicle

根据GPS，INS和EC所测得的数据，得到GPS，INS和使用本文算法CC-AFKF融合后的经纬度和速度数据，分别如图5和图6所示。

图5 原始经纬度信息对比图Fig.5 Comparison of original latitude and longitude information

图6 原始北、东向速度信息对比图Fig.6 Comparison of original information of eastward and northward speed

从图5和图6中可以看出，本文所提出的CC-AFKF算法在位置和速度信息融合方面具备有效性。其中，图5显示INS框架定位误差随着时间的增加而增加，在经度为121.546 8°时其测量结果就已开始明显偏离实际参考值，选取图5中INS发生明显偏差的一部分进行放大，通过观察可知，本文所提出的CC-AFKF算法所得到的经、纬度基本上与参考值保持一致；由于试验海域不存在遮挡物，因此GPS单独导航的效果与参考值相比也相差不大，但限于GPS单系统的不稳定性，可以发现GPS的数据存在较大波动。同理，图6也明显展示出无论是原始东向速度还是北向速度，INS框架的速度误差也在随时间累积；可以发现，在INS运行一段时间后，速度开始偏离参考值，而CC-AFKF框架和GPS框架在速度信息上与参考值进行对比时，展现的特性与经纬度信息几乎完全一致。

由此可知，本文提出的CC-AFKF算法在位置和速度的融合方面具备一定的可靠性，为证明置信检验环节的有效性，在150 s时对GPS的测量数据施加一定的扰动，得到AFKF和CC-AFKF的经纬度和速度对比图（图7和图8）。

从图7和图8可知，施加扰动前，AFKF框架下的融合结果与CC-AFKF和参考值几乎完全重合；但在施加扰动后，无论是位置信息还是速度信息，均发生了较大偏差；由此可见，本文所提出的具备置信检验环节的CC-AFKF框架具备一定的抗扰动性能。

图7 受扰动后不同框架的经纬度对比图Fig.7 Comparison of latitude and longitude between different frameworks due to perturbation

图8 受扰动后不同框架的北、东向速度对比图Fig.8 Comparison of northward and eastward speed between different frameworks after perturbation

现有组合导航方式多为运用KF的GPS/INS组合导航模式[20]，为充分体现本文算法的优越性，将本文所提出的CC-AFKF算法与经典的KF和FKF算法相比较，结果如图9至图14所示。其中，图9至图11分别是经纬度对比、误差对比和均方根误差对比；图12至图14分别是东北向速度对比、误差对比和均方根误差对比。

图9 不同框架的经纬度对比图Fig.9 Comparison of latitude and longitude between different frameworks

图10 不同框架的经纬度误差对比图Fig.10 Comparison of longitude and latitude errors between different frameworks

图11 不同框架的位置均方根误差对比图Fig.11 Comparison of root mean square error of position between different frameworks

图12 不同框架的北、东向速度对比图Fig.12 Comparison of northward and eastward speed between different frameworks

图13 不同框架下的北、东向速度误差对比图Fig.13 Comparison of northward and eastward speed errors between different frameworks

图14 不同框架的速度均方根误差对比图Fig.14 Comparison of root mean square error of speed between different frameworks

从图9至图14可以看出，在KF框架和FKF框架下速度和位置信息相较于参考值不会发生明显偏差，但本文所提出的CC-AFKF框架在速度和位置融合精度方面能够实现更优越的性能。从图9和图12的细节放大图可知，KF框架下的数据波动较大，KFK框架虽然相较于KF框架稳定度有所提高，但提升并不明显，CC-AFKF框架下的位置和速度信息与参考值最为接近，误差图对比也证明了这一点。在位置融合方面，经度和纬度误差能够稳定在 [−1,1]区间，与KF框架相比较，本文所提出的CC-AFKF框架在纬度和经度的融合精度上分别提升了71.4%和68.5%，相较于FKF则分别提升了32.5%和36.9%。在速度的融合方面，本文所提出的CC-AFKF框架使用自适应系统噪声协方差，能够更加精准地实现速度融合。具体而言，本文提出的CC-AFKF框架在北向速度和东向速度的融合精度上相较于KF分别提升了46.9%和59.2%，相较于FKF则分别提升了29.4%和42.7%。综上所述，由于KF和FKF缺少对系统噪声的跟随性，故其难以实现与本文所提出的CC-AFKF框架相一致的融合性能。