基于模糊鲁棒自适应CKF算法的MEMS-IMU姿态估计

乔美英，高翼飞，李宛妮，姚文豪

（河南理工大学 电气工程与自动化学院，焦作 454000）

随着无人机、虚拟现实器件（Virtual Reality, VR）、可穿戴医疗设备等技术的不断发展，低成本、小体积、低功耗的微机电系统（Micro-Electro-Mechanical System, MEMS）惯导设备也得到了广泛的发展应用[1,2]。其内部惯性测量单元（Inertial Measurement Unit,IMU）是实现惯性导航的基本组成单元，根据MEMS-IMU系统不同传感器的测量数据，通过算法融合并实时解算出的姿态参数，实现对载体的定位和跟踪。因此快速、准确地解算载体实时的姿态信息是实现惯性导航的关键。

目前对惯性导航系统姿态解算的算法有很多，其中最具有代表性的是其估计结果具有无偏、一致最优的卡尔曼滤波（Kalman Filter, KF）及其扩展算法：扩展卡尔曼滤波（Extended Kalman Filter, EKF），无迹卡尔曼滤波（Unscented Kalman Filter, UKF）及容积卡尔曼滤波（Cubature Kalman filter, CKF）[3]等，均广泛应用于航空航天，船舶和汽车的自主导航或辅助导航等方面。

KF滤波及其扩展算法的无偏估计、一致最优是在假设量测噪声严格服从高斯分布的条件下成立的，但实际应用中由于存在各种干扰情况，常常导致假设不成立，使估计结果达不到一致最优。为消除这些干扰，文献[4]针对目标运动时出现理论模型和实际模型不匹配、模型失配的问题，在传统渐消滤波的基础上提出了一种多重渐消因子的强跟踪CKF算法。郭士荦等[5]在多重渐消因子的强跟踪CKF算法的基础上引入χ2检验条件，使多重渐消因子的引入时机更加合理。徐博等[6]针对水下复杂环境，设计了一种基于马氏距离的自适应CKF算法，使用滤波新息序列的马氏距离判断系统模型失准情况。文献[7]则针对系统状态跃变以及非高斯量测噪声干扰等问题，提出了一种基于m估计的鲁棒卡尔曼滤波器，根据Huber的m估计方法重新定义了新息序列，使滤波的鲁棒性得到提升。文献[8]则借鉴Huber等价权函数的思想，构造了逼近函数以抑制观测野值的影响，提高了算法的鲁棒性。但上述这些算法在处理滤波异常情况时，是独立考虑算法的自适应性和鲁棒性，而所针对的也是仅单一异常情况。然而实际惯导系统的算法在不同环境下运行时，需要考虑多种干扰混合共存的情况，同时考虑算法的自适应性和鲁棒性。

基于此，本文提出了一种模糊鲁棒自适应容积卡尔曼滤波（Fuzzy Robust Adaptive - Cubature Kalman Filter, FRA-CKF）算法，借鉴滤波新息序列的2χ检验，根据2χ分布上分位点与假设检验原理设置了不同异常的修正门限，构造了模糊修正边界并提出了相应的模糊修正准则。将算法的自适应性和鲁棒性进行了有机融合，提高了滤波性能。实验结果表明，本文所提出的算法在受到不同干扰时能够及时对异常进行修正，准确地估计系统姿态。

1 姿态估计模型与滤波算法

1.1 姿态估计模型

本文以IMU系统中的陀螺仪为对象，用四元数方式描述姿态参数，建立IMU系统的状态方程。令四元数q为系统状态，则根据陀螺仪的输出角速度和四元数的微分方程有：

其中 q=[q0,q1, q2,q3]T且为单位四元数，Ω(ω)是由陀螺仪测量角速度构成的矩阵，其矩阵形式为：

将式(1)进行离散化处理，以方便迭代计算：

其中 qk=[ q0,k, q1,k, q2,k, q3,k]T为k时刻的四元数系统状态。由于陀螺仪在测量过程中会受噪声和自身漂移影响，所以陀螺仪实际输出的角速度应包含这部分误差。设陀螺仪所受干扰引起的角速度影响为 ωΔ ，则实际输出的角速度应为： ωs= ωt+Δω，其中 ωt为真实的角速度。将 ωs= ωt+Δω带入式(3)有：

IMU的加速度计通过测量重力矢量能够获得载体的俯仰信息和横滚信息，磁强计通过测量磁场矢量能够得到载体的航向信息，所以本文选用加速度计和磁强计为系统的量测，对陀螺仪漂移进行修正。

加速度计的输出模型为：

其中上标b表示载体坐标系(右前上)，上标n表示导航坐标系(东北天)；an=［ 0 0g］T为导航坐标系下的重力加速度，为k时刻载体坐标系下的加速度测量值，为k时刻从导航坐标系(n系)到载体坐标系(b系)的旋转矩阵，加速度计噪声面垂直，因此使用加速度计量测系统的俯仰角信息和由于重力平面始终和系统的航向平横滚角信息，旋转矩阵形式为：

将式(6)中第一项的计算结果拆分出四元数qk，得到k时刻的加速度计量测矩阵：

将式(8)带入式(6)，可得加速度计输出模型的线性形式：

磁强计的输出模型为：

其中 hn= [ B · cosδ 0 -B ·sinδ]T为地磁场强度B在导航坐标系的投影，δ为磁偏角，磁强计噪声Vh,k~N(0, Rh,k)，且Wk，Va,k和Vh,k三种噪声是相互独立的。同理，将式(10)中第一项的计算结果拆分出四元数qk，可得矩阵：

其中 B1= B ·cosδ， B2= B ·sinδ。将式(11)带入式(10)，可得磁强计输出模型的线性形式：

联立式(5)(9)(12)得到IMU系统的状态方程和量测方程。

1.2 滤波算法

1）渐消自适应滤波

KF及其扩展算法估计结果的无偏、最优是在假设量测噪声服从高斯白噪声分布的前提下才成立的。系统量测值和量测预测值之间的残差称为新息kη，在此假设成立的前提下，滤波的新息序列kη服从均值为零，协方差理论值为的高斯序列。

当量测受到非高斯噪声干扰时假设不成立，滤波估计精度将下降，传统方式是通过新息序列协方差的理论值和计算值进行判断：非高斯噪声影响时，新息序列协方差的实际计算值和理论值是不相等的，新息序列协方差估计值的计算式为：

渐消自适应KF是一种抑制滤波新息异常的简单有效方法[4]，如果新息序列为高斯序列，则滤波新息序列的马氏距离应服从卡方分布。根据假设检验原理和 χ2检验原理，选取以 χ2分布显著性水平为α的上分位点作为滤波评判标准。例如，若量测为三维时，选取的上分位点= 11.345，通过查表得也就是说，在滤波正常的情况下，新息序列的马氏距离大于的概率只有1%。根据假设检验原理，若新息序列的马氏距离大于则在99%的置信度下可以认为滤波异常。将此判断方式应用到CKF中，则有：

其中dm为新息序列的马氏距离。

在正常噪声情况下 dm＜，按照CKF滤波对系统状态进行估计；当 dm＞时，对CKF算法进行修正，通过自适应的方式调节这一现象：引入一个时变的标量因子 kλ，仅针对该时刻的新息序列进行估计，对状态预测误差协方差Pk/k-1的第一项进行调整：

通过高斯牛顿法进行迭代求解，可以得到λk的递推式[12]：

2）膨胀鲁棒滤波

当外部的测量信息不可靠时，通过提高量测的权重从而抑制模型失配带来的误差，当量测含有较大噪声时，若继续使用渐消自适应的方式进行调整，则会影响滤波估计的精度。文献[9]提出了一种与渐消自适应滤波类似的方法，在KF的基础上引入一个时变的标量膨胀因子对新息协方差的第二项进行调整，对量测噪声进行膨胀，削弱量测对滤波的修正作用。

当 dm＞时，同样以CKF滤波为对象，对新息序列协方差中的量测噪声协方差R进行处理：

膨胀因子 μk也需要满足：

同样使用高斯牛顿法进行迭代求解，得到膨胀因子 μk的递推式[10]：

其中膨胀因子 μk的初值为1。

与渐消自适应滤波相反，膨胀鲁棒滤波通过对量测噪声的膨胀作用，减小了滤波增益，削弱了量测的权重，从而提高系统自身的鲁棒性。

这两种方法分别针对不同异常情况提出了各自的调整方式，自适应滤波更加相信量测信息的修正作用，而鲁棒滤波则更看重系统模型的预测信息。两种调节滤波的方式，就像“杠杆”一样，是互相对立的调节方式，同一时刻在滤波算法中难以实现平衡[11]。在实际应用中，系统不可避免地会出现状态不连续变化时的模型失准，以及受外界干扰影响使量测含有非高斯噪声的情况[12]。

因此，针对此情况提出了一种模糊修正准则，将CKF、自适应修正和鲁棒修正相互贯通，将这根“杠杆”转变为“桥梁”，使算法兼顾鲁棒性和自适应性。

2 基于模糊修正的鲁棒自适应CKF

2.1 修正门限值

新息序列的统计特性是判断滤波是否异常的关键[9]。本文同样利用2χ检验原理对滤波异常判断。并将设置为修正起始门限，以二维平面为例展示修正域，设原点为圆心，以修正门限为半径作圆如图1所示(参数β＞0)。若k时刻没有出现异常，新息序列的马氏距离应在小圆内，此时无需修正；若滤波出现异常，新息序列的马氏距离超出修正半径，此时需要根据模糊修正准则执行相应的修正。

图1 修正门限圆Fig.1 Modifiedthreshold circle

2.2 模糊修正准则

根据引起滤波异常的原因，需要使用不同方法进行修正，但是如果直接设置自适应修正的边界和鲁棒修正的边界，严格按照界内界外进行修正，显然是不合理的。如果dm出现在自适应修正边界内但距离鲁棒修正边界很近的情况，或是在鲁棒修正边界内但距离自适应修正边界很近的情况，严格按照界内界外的标准可能会引起错误修正，出现相反的修正结果。

为避免这种现象发生，合理修正异常情况，本文提出了模糊修正准则，通过将修正边界模糊化把边界转化为域，在域内通过设置不同修正方式的隶属度函数，用隶属程度这一概念将不修正、自适应修正和鲁棒修正三种修正方式融合，根据模糊修正准则对滤波异常进行合理修正。模糊修正准则具体由三部分组成：

1) 设置模糊边界

在设置不同异常的模糊边界时，考虑到系统状态出现跃变导致模型失准情况时，实际量测值和量测预测值之间的差异较大，滤波新息会大幅度变化；而系统受到噪声干扰或累积误差引起的新息变化相对较小。因此，在修正门限的基础上选定4个参数a，b，c，d分别作为CKF、自适应修正和鲁棒修正边界，其中

2) 构造隶属度函数

三种算法标准CKF、鲁棒修正（RCKF）和自适应修正（ACKF）的隶属度函数分别为：

三种隶属度函数的选取原因是：当新息序列的马氏距离超过修正门限且不断增大时，不修正的隶属度应快速下降，所以选取了抛物线型为不修正的隶属度函数；此时鲁棒修正的隶属度应逐渐增大，因此选取了增长趋势为线性的半梯形隶属度函数。当新息序列的马氏距离增长过鲁棒修正边界时，鲁棒修正的隶属度也应逐渐下降，若不选择梯形而选择抛物线型，则鲁棒修正的程度会下降得过快从而使修正不到位。自适应修正的隶属度函数应先缓慢增长再快速上升，因此自适应修正的隶属度函数选择半抛物线型。以a=2,b= 4,c=6,d=8为例，三种修正的隶属度函数如图2所示。

图2 三种修正的隶属度函数Fig.2 Membership functions of three modified

3) 制定模糊修正准则

根据隶属度函数对滤波异常进行判断并修正：当dm在模糊域(0,a]之间时，滤波无异常，执行标准CKF算法。当dm在模糊域(a,b)之间时，滤波异常，执行修正，此时既执行CKF也执行鲁棒修正，但是隶属于CKF的“程度”逐渐减小，隶属于鲁棒修正的“程度”逐渐增大。当dm在模糊域[b,c)之间时，新息异常是滤波受到噪声干扰引起的，应对量测噪声做出调整降低量测的修正作用，因此执行鲁棒修正。当dm在模糊域[c,d)之间时，此时既执行鲁棒修正也执行自适应修正，但是隶属于鲁棒修正的“程度”逐渐减小，隶属于自适应修正的“程度”逐渐增大。当dm超过模糊修正边界d时，新息异常是由状态跃变或模型失配引起的，应对系统预测协方差进行调整，降低系统预测部分的权重，增大量测的修正作用，因此执行自适应修正。针对滤波异常时的模糊修正准则如表1所示。整体流程如图3所示。

表1 模糊修正准则Tab.1 Fuzzy correction criterion

图3 滤波异常判断和模糊修正准则Fig.3 Filter anomaly judgment and fuzzy correction criteri

3 仿真试验与分析

3.1 仿真试验

为了验证所提模糊修正准则在不同异常情况下对CKF算法的修正能力。本节在Matlab2018a环境下进行仿真试验验证，为了接近本文所用IMU系统的模型，设置系统的状态方程为：

式中：k =1，2，3…n为迭代次数，x为系统状态用四元数形式表示且初始值x(0)=[0,0,0,0]T，F为主对角线元素为1，其余元素为0.01的四阶矩阵，I为四阶单位矩阵，系统噪声协方差和量测噪声协方差分别为 Q = 10-4·I和 R = 10-1·I。量测噪声在迭代的第40次时以指数形式扩大一倍后不变，仿真了系统在不同环境不同场合运行时的非高斯噪声影响。在迭代至第40、41次时，给系统状态施加一个强度为50的正向脉冲信号；在迭代至第80、81次时给系统状态施加一个强度为150的反向脉冲信号；在迭代至第120-125时给系统施加强度为20的反向阶跃信号，使系统状态发生不连续跃变，仿真了系统运动状态发生变化导致的模型失准状况。仿真结果如图4所示（仅展示q0的仿真结果，其余元素与q0结果类似）。

图4 q0的迭代结果Fig.4 Iteration result of q0

图4 中本文所提的FRA-CKF算法有效地跟踪了系统实际情况，在系统状态发生跃变时做出了合理的自适应修正，准确地跟踪系统状态，并且在仅受噪声影响，未发生跃变时，能够进行鲁棒修正、渐消噪声影响，有效地验证了所提模糊修正准则。

为了进一步验证模糊修正鲁棒自适应CKF算法准确跟踪系统状态的能力，将该算法与渐消自适应CKF算法（ACFK），鲁棒CKF算法（RCFK）和CKF算法的估计结果进行对比。将四种算法的滤波参数设置相同，对系统状态进行估计，结果如图5所示。

图5 不同滤波算法对比Fig.5 Comparison of different filtering algorithms

图5 展示的滤波估计结果中能看出，所提FRA-CKF算法在系统状态仅受噪声干扰时的估计精度与鲁棒CKF算法相当，高于CKF算法，自适应CKF算法的估计精度最低；然而当系统状态发生跃变时，鲁棒CKF算法没有跟踪上系统的状态，CKF算法虽然勉强跟踪上了系统的状态，但与实际系统状态依然有较大的相差。只有自适应CKF算法和FRA-CKF算法能够跟踪系统的变化，但自适应CKF算法的估计精度受噪声干扰，估计的结果振荡不稳定。只有FRA-CKF既能够及时地跟踪系统状态，也能在噪声干扰下具有较高的精度和稳定的估计结果。

4 实测实验

为验证所提FRA-CKF算法在惯性导航姿态估计中的实际应用效果，利用MEMS-IMU JY901进行实测实验验证。

1)静态实验：将JY901接通好固定在无磁双轴转台上，使芯片x轴对准北向。转台开机后转置水平，在静置1小时之后开始采集数据，采样频率为60 Hz，采样时间为73 s，如图6所示。

图6 双轴转台和带有MEMS-IMU的测量载体Fig.6 Two axis turntable and carrier with MEMS-IMU

设置对比实验：由于渐消自适应CKF和鲁棒CKF在滤波无异常时不执行修正，与CKF算法为同一算法，而FRA-CKF算法通过设置修正边界和模糊修正准则在静态条件下依然具有修正作用。所以静态实验仅与CKF算法进行对比。分别使用两种算法对系统进行姿态估计，两种算法的姿态估计结果如图7-9所示。

图7 俯仰角估计结果Fig.7 Pitch angle estimation results

图8 横滚角估计结果Fig.8 Roll angle estimation results

图9 航向角估计结果Fig.9 Heading angle estimation results

从图7-9中可以看出，在静态条件下FRA-CKF算法相比CKF算法对姿态参数的估计精度更高，且波动范围小，具有稳定估计结果。为了进一步衡量两种算法，表2给出了两算法的均方根误差。

表2 两种算法的均方根误差Tab.2 Root mean square error of two algorithms

从表2中能看出FRA-CKF算法相比CKF算法，静态时姿态估计结果的均方根误差，俯仰角降低了24%，横滚角降低9%，航向角降低了80%，验证了算法的准确性。

2)动态实验：对带有MEMS-IMU JY901的测量载体进行车载实验，将载体连接好后固定在汽车后座上，记录汽车的运动姿态，如图10所示。采样频率为60 Hz，采样时间为13 min。行驶轨迹如图11所示。

图10 动态车载实验Fig.10 Dynamic vehicle experiment

图11 行驶轨迹Fig.11 Driving track

设置对比实验：由于动态车载实验测量数据会受外界干扰从而影响CKF算法，使算法出现异常情况。渐消自适应CKF和鲁棒CKF在异常情况将执行相应的修正，因此动态实验将FRA-CKF、渐消自适应CKF以及鲁棒CKF算法进行对比，进而验证算法的实用性能。分别使用三种算法对系统进行姿态估计，根据实测数据三种算法的姿态估计结果如图12-14所示。

图12 俯仰角估计结果Fig.12 Pitch angle estimation results

图13 横滚角估计结果Fig.13 Roll angle estimation results

图14 航向角估计结果Fig.14 Heading angle estimation results

图12-14中的姿态信息表明：渐消自适应CKF的俯仰信息和横滚信息随着航向信息的变化受到了较大干扰，而鲁棒CKF不随航向信息的变化而变化，始终稳定。但两种算法解算的航向信息在多次变化后的解算精度严重下降，第630 s后解算的航向严重偏离实际运动情况。而FRA-CKF算法在航向多次发生变化后仍能准确地估计，且俯仰信息和横滚信息不敏感外界干扰，验证了算法兼顾自适应与鲁棒性的特点和在实际应用中的有效性。

5 结 论

针对惯性导航系统受到状态突变、未知量测噪声等干扰，传统滤波算法因发散而无法准确估计系统姿态的问题，提出了一种模糊鲁棒自适应容积卡尔曼滤波算法。通过设置起始修正门限对滤波新息的异常进行判断，并根据误差来源设置了修正边界，构造隶属度函数提出模糊修正准则。根据模糊修正准则对状态预测协方差或量测噪声协方差修正，使算法兼顾鲁棒性和自适应性。实验结果表明，该算法在静态和动态条件下相比其他算法具有准确、稳定的姿态估计结果。