基于MEMS-IMU 的动态大失准角STUKF 算法

顾元鑫，吴文启，王茂松

（国防科技大学 智能科学学院，长沙 410073）

微机电惯性测量单元（Micro-electro Mechanical Inertial Measurement Unit,MEMS-IMU）凭借其体积小、功耗低、价格低廉等优势，已经在各类无人平台的组合导航、动态姿态测量等领域中得到广泛应用[1-3]，相关滤波算法的研究一直受到关注[4,5]。由于微机电陀螺、加速度计精度相对低，且MEMS-IMU应用中经常存在大失准角姿态误差情况[6]，在剧烈的角运动环境中，误差影响显著增大。对于导航级的惯导系统，可以采用惯性系粗对准[7]的方法避免初始大失准角情况。由于MEMS-IMU 陀螺精度较低，并不能通过惯性系粗对准避免初始大失准角情况，因此针对这种情况下的非线性滤波算法的研究是十分必要的。扩展卡尔曼滤波器（Extended Kalman Filter,EKF）是应用最为广泛的传统非线性滤波算法，但EKF 的一阶截断误差较大，在大失准角等强非线性应用场景下EKF 会产生较大误差甚至失效[8]。

针对大失准角问题，无迹卡尔曼滤波（Unscented Kalman Filter,UKF）采用无迹变换（Unscented Transformation,UT）变换，应用一系列精心选择的点对系统的统计特性进行近似，不需要像EKF 一样将非线性模型进行线性化，无需计算雅可比矩阵。与EKF相比，UKF 的模型更加准确，在大失准角等强非线性应用场景中，其状态估计的精度也就更高[9]；与粒子滤波相比，UKF 通过一定规则产生采样点，不会出现粒子滤波中的粒子退化情况，且无迹卡尔滤波算法的计算量更小、更便于工程实现[10]。由于UKF 仍采用传统惯导误差模型，速度误差微分方程中包含姿态矩阵与比力的乘积项。在剧烈的角运动环境中，姿态矩阵误差和比力测量误差会造成误差状态协方差矩阵的计算误差，进而导致状态量估计不准确。

文献[11]提出了状态变换扩展卡尔曼滤波（State Transformation EKF,STEKF），使传统惯导误差模型中速度误差微分方程中的比力项变换成较为稳定的重力项，从而避免在动态环境或惯性器件的比力误差较大时，出现协方差矩阵计算不准确所导致的状态估计精度变差的情况[12,13]。但由于其误差微分方程仍是在小失准角的假设前提下推导出来的，本质上仍存在截断误差，当在动态大初始失准角等强非线性应用场景中，由于其模型不准确，仍会导致状态估计精度变差。

本文提出了一种基于状态变换的无迹卡尔曼滤波（State Transformation UKF,STUKF）算法，其基本思想是采用状态变换的方法将UKF 速度误差微分方程中的比力相关项转换成了相对稳定的重力项，避免了剧烈运动应用场景中比力计算不准确导致滤波精度下降的情形。相较于STEKF，由于本算法的误差微分方程都是在大失准角假设下推导获得的，因此在初始大失准角应用场景中，其模型的准确性以及滤波结果的可信性均优于STEKF。

本文以基于MEMS-IMU 的小型无人船载航姿系统为例，经海况数学仿真及三轴转台动态实验验证，在初始大失准角情况下，STUKF 的姿态角收敛精度及快速性均优于现有的STEKF 及UKF。

1 大失准角状态变换非线性误差微分方程

1.1 等效旋转矢量非线性姿态误差微分方程

理想情况下，捷联惯导系统的姿态矩阵微分方程为[14]：

在实际应用中，姿态矩阵微分方程是存在误差的，即：

则大失准角情况下的方向余弦误差微分方程为：

将式(1)(2)代入式(4)，有：

又因为：

令式(5)(6)右端相等，有：

大失准角情况下等效旋转矢量的非线性姿态误差微分方程[15]如式(11)所示：

与传统的欧拉角非线性姿态误差微分方程[14]相比，没有俯仰误差角小于90°的限制；且当姿态误差角收敛到小角度时，非线性误差模型可自然过渡到线性误差模型。

1.2 状态变换非线性速度误差微分方程

将式(15)移项并代入式(16)，有：

其中，δ fb表示加速度计测量误差，又有：

将式(15)移项并代入式(20)有：

其中，位置误差向量定义为：

式(22)中，Lδ为纬度误差；δλ为经度误差；hδ为高度误差。

式(23)(24)中，RE及RN分别表示卯酉圈及子午圈曲率半径；h表示当地高程；L为当地纬度。则：

式(26)中，ωie为地球自转角速度。

位置误差微分方程为：

2 STUKF 算法

2.1 状态方程

根据上节中提出的大失准角情况下基于等效旋转矢量的状态变换非线性误差微分方程，可得STUKF系统的状态方程：

状态量x中的变量包括：

由于状态方程为非线性，其状态量与参数不可分，因此其状态转移矩阵无法明确写出，以状态转移方程的形式给出：

系统噪声矩阵为：

2.2 观测方程

选取观测量为：

观测方程可表示为：

其中，υ为量测噪声向量；H(x)为量测矩阵。有：

STUKF 的滤波过程与传统的UKF 一致，与其相比，STUKF 将传统的速度误差替换成状态变换后的速度误差。变换后的误差方程中不再含有与姿态及加速度计相关的比力项fn=，而是替换成相对稳定的当地重力项gn，减小了由于MEMS-IMU 姿态误差和比力测量误差大对协方差矩阵及状态估计结果的影响。

STEKF 通过对非线性函数进行泰勒展开，忽略其高阶项，从而实现线性化，而STUKF 是通过对非线性函数的概率分布进行近似，并且整个模型的推导过程均是在大失准角的假设下完成的，因此大失准角情况下STUKF 的模型准确性要优于STEKF。

3 实验结果与分析

3.1 摇摆仿真实验

以小型无人船载MEMS-IMU/卫星组合航姿系统为应用场景，利用Matlab 生成仿真的载体姿态、速度和位置数据以及陀螺、加速度计理论采样值，结合如表1 所示的实际MEMS-IMU 陀螺零偏及噪声、加速度计零偏及噪声，以及卫星导航接收机的速度、位置测量噪声，生成更符合真实情况的IMU 数据及速度位置观测数据，选取的卫星接收机的水平定位精度为2.5 m（1σ），测速精度为0.05 m/s（1σ）。

表1 模拟MEMS-IMU 性能指标参数Tab.1 Specifications of the simulated MEMS-IMU

为提高MEMS-IMU 水平加速度计零偏及等效东向陀螺零偏的可观测性，实验中采用IMU 绕方位轴旋转调制的方式，即 MEMS-IMU 绕方位轴采用0°→180°→ 0°每个位置停留5 min 的转停方案，实现对MEMS-IMU 陀螺仪及加速度计零偏的估计和补偿。

按照现行电工电子产品环境试验国家标准中典型摇摆实验严酷等级示例表中的指导参数进行实验，载体摇摆方式中各参数定义如下：滚动角正弦摇摆幅值为Aγ，正弦摇摆频率为fγ；俯仰角正弦摇摆幅值为Aθ，正弦摇摆频率为fθ；航向角正弦摇摆幅值为Aφ，正弦摇摆频率为fφ。实验1-3 的参数选取如表2 所示。

表2 摇摆仿真实验Tab.2 Swing simulation experiments

记初始滚动失准角为Δγ，初始俯仰失准角为Δθ，初始航向失准角为Δφ。在实际应用中，由于加速度计的精度一般较高，因此水平大失准角估计相较于航向大失准角更容易，所以实验中更侧重航向大失准角的情况。采用100 s 的惯性系粗对准[7]时，三个陀螺5°/h 的零偏造成的航向误差也不会超过60°。按照表3 所给定的大失准角取值范围均匀、随机地选取50组初始失准角进行蒙特卡洛实验，实验中按失准角较为恶劣的情况进行假设，因此φN、φE、φD的滤波初始方差设置为[(45°)2,(45°)2,(60°)2]T，各次蒙特卡洛仿真实验的时长为2000 s。根据表2 中实验2 的摇摆条件及表3 中小失准角取值范围均匀、随机地选取50 组初始小失准角进行蒙特卡洛仿真，φN、φE、φD的滤波初始方差设置为[(0.1°)2,(0.1°)2,(0.5°)2]T，记为实验4。

表3 摇摆仿真实验初始失准角设定Tab.3 Initial misalignment angle setting for swing simulation experiments

在上述给定初始失准角取值规则及摇摆方式设定的基础上，进行50 次蒙特卡洛仿真。由于传统EKF算法在动态大失准角情况下发散，不能正常工作，因此选取STUKF、STEKF、UKF 三种算法进行比较，用姿态角估计误差收敛后的值（600 s～2000 s）求取均方根值（Root Mean Square,RMS），判定算法的姿态角估计精度，实验结果如图1-4 所示。

图1 摇摆仿真实验1 的姿态误差RMSFig.1 RMS of attitude error of swing simulation experiment 1

图2 摇摆仿真实验2 的姿态误差RMSFig.2 RMS of attitude error of swing simulation experiment 2

图3 摇摆仿真实验3 的姿态误差RMSFig.3 RMS of attitude error of swing simulation experiment 3

综合图1-3，在大初始失准角情况下：在水平角方面，三种滤波算法的最终收敛精度相近，收敛值可维持0.01°（1σ）；在航向角方面，STUKF 的航向角估计精度的收敛值总可以优于0.5°（1σ），UKF 的航向角估计精度的收敛值总可以优于2°（1σ），STEKF的航向角估计精度的收敛值总可以优于3°（1σ）。从总体上来说，STUKF 的姿态估计精度优于UKF，UKF优于STEKF。由图4 可知，在小初始失准角情况下，三种滤波算法的姿态估计精度均有提升：在水平角方面，三种算法的精度相近；在航向角方面，STUKF 与UKF 的姿态估计精度相近，均优于STEKF，造成这一现象的主要原因是大陀螺零偏及加速度计零偏依然会导致实际系统模型非线性，进而影响STEKF 的姿态估计精度。

图4 摇摆仿真实验4 的姿态误差RMSFig.4 RMS of attitude error of swing simulation experiment 4

选取实验2 中的摇摆条件及表1 中零偏初值进行实验，比较UKF 与STUKF 的误差状态方差一致性，然后增大惯性器件误差，设置x、y、z轴的陀螺及加速度计零偏为[3.5°/h,4°/h,4.5°/h]T和[4.5 mg,5 mg,5.5 mg]T，进行误差状态方差一致性分析。实验中先进行惯性系粗对准，在粗对准后大失准角基础上应用UKF 及STUKF 进行精对准，以航向角误差为例进行分析。由于惯性系粗对准后初始水平失准角很小，为更好地观察误差状态方差一致性，φN、φE、φD的滤波初始方差设置为[(1°)2,(1°)2,(60°)2]T，实验结果如图5-8 所示。

图5 增大惯性器件误差前由滤波协方差阵得到的航向角误差标准差估计曲线Fig.5 Estimation curves of yaw angle error standard deviation obtained in the filter covariance matrix before increasing the inertial sensor errors

图6 增大惯性器件误差前航向角误差估计曲线Fig.6 Estimation curves of yaw angle error before increasing the inertial sensor errors

图7 增大惯性器件误差后由滤波协方差阵得到的航向角误差标准差估计曲线Fig.7 Estimation curves of yaw angle error standard deviation obtained in the filter covariance matrix after increasing the inertial sensor errors

图8 增大惯性器件误差后航向角误差估计曲线Fig.8 Estimation curves of yaw angle error after increasing the inertial sensor errors

从图5-8 可知，在陀螺及加速度计零偏加大前后的两组实验中，UKF 航向角误差标准差估计曲线的收敛速度快于STUKF，但其实际误差收敛速度却比STUKF 慢，导致UKF 航向角误差还未收敛但其误差标准差估计值就已过度收敛，与实际误差标准差不一致，表现出误差标准差过度优化现象，进而导致UKF航向角误差收敛精度及速度都比STUKF 差。

由于实际工作中航向失准角的估计困难更大，且其收敛速度更慢，因此选取航向失准角的误差完全收敛到[-1°,1°] 范围内时记为算法航向误差的收敛时间（若该次实验中航向误差最终都未收敛到[-1°,1°]范围内，则记航向误差收敛时间为2000 s）。取每组实验中50 次蒙塔卡洛实验收敛时间的均值作为该组实验滤波算法的收敛时间，实验结果如表4 所示。

表4 蒙特卡洛实验平均收敛时间Tab.4 Average convergence time of Monte Carlo experiments

由表4 可知，在大初始失准角情况下，STUKF 航向角估计误差的收敛速度优于 UKF，UKF 优于STEKF。

3.2 三轴转台实验

由于MEMS-IMU 在三轴角运动模拟转台上存在姿态安装偏差，且由于三轴角运动模拟转台与实验系统未完全同步，因此选取初始小失准角情况下长时间对准所获得的姿态值作为姿态基准。在实验中发现，三种算法在初始小失准角情况下的姿态对准误差相近，只是在IMU 方位旋转调制的转位过程中每个转停周期内姿态误差的具体变化存在一定差异，因此为避免各滤波算法误差的具体变化对姿态基准精度产生影响，选取三种滤波算法在初始小失准角情况下且去除了初始陀螺、加速度计零偏的滤波结果均值作为姿态基准。利用三轴角运动模拟转台进行摇摆实验，实验中三轴转台的运动参数设置与仿真实验中实验2 的设置一致。实验在x、y、z轴陀螺及加速度计标定后剩余零偏的基础上分别加以[4.9°/h,5°/h,5.1°/h]T和[4.9 mg,5 mg,5.1 mg]T的常值零偏误差。由于惯性系粗对准后水平失准角较小，因此φN、φE、φD的滤波初始方差设置为[(5°)2,(5°)2,(60°)2]T。首先采用惯性系粗对准，然后在惯性系粗对准后大失准角误差基础上分别采用三种算法进行精对准。在三轴转台摇摆实验中，由于陀螺和加速度计的标度因数误差、温度影响、残余的杆臂误差、速度观测信息不准确以及姿态基准值本身不准确等问题，导致三轴转台动态实验中姿态角误差值会比仿真实验中大。

滤波算法中陀螺及加速度计零偏估计值的滤波初值均设置为0。经过滤波，陀螺及加速度计零偏的估计值逐渐逼近零偏误差的实际值，图9-10 即为估计所得的零偏值。从图中可知，三种滤波算法可实现对除天向陀螺外的两个水平陀螺零偏及三个加速度计零偏的有效估计，且三种滤波算法估计得到的零偏值相近。

图9 转台实验陀螺零偏误差估计曲线Fig.9 Gyroscope bias error estimation curves of turntable experiments

图10 转台实验加速度计零偏误差估计曲线Fig.10 Accelerometer bias error estimation curves of turntable experiments

由图11-12 可知，在精对准900 s 后，STUKF 的滚动角和俯仰角估计误差分别收敛在0.03°（1σ）和0.02°（1σ）；STEKF 的滚动角和俯仰角估计误差分别收敛在0.05°（1σ）和0.02°（1σ）；UKF 的滚动角和俯仰角估计误差分别收敛在0.03°（1σ）和0.04°（1σ）。由图13 可知，在航向角方面，STUKF 精对准900 s 后，航向角的估计误差收敛到1°（1σ）以内；UKF精对准900 s后，航向角的估计误差收敛到2°（1σ）以内；STEKF 精对准900 s 后，航向角的估计误差收敛到3°（1σ）以内。STUKF 对航向角估计误差的收敛速度及精度优于UKF，UKF 优于STEKF。

图12 转台实验俯仰角误差Fig.12 Pitch angle error of turntable experiments

图13 转台实验航向角误差Fig.13 Yaw angle error of turntable experiments

综上可以得出结论：在初始大失准角动态姿态对准中，STUKF 的对准精度及快速性优于UKF，UKF 优于STEKF，与仿真实验的结论一致，验证了仿真实验的可信性。

4 结论

以基于MEMS-IMU 的小型无人船载航姿系统为应用背景，本文提出了一种STUKF 算法。通过状态变换的方法将UKF 速度误差微分方程中的比力项转换为较为稳定的重力项，克服了UKF 在剧烈运动情况下，姿态误差与比力测量误差导致的状态估计不准确的问题。相较于现有的STEKF 而言，在初始大失准角以及MEMS-IMU 中陀螺和加速度计零偏较大的应用场景中，考虑了非线性误差项的STUKF 模型更加准确，滤波精度更高。通过蒙特卡洛摇摆模拟仿真实验与实际三轴转台动态实验验证了STUKF算法在初始大失准角应用场景中的优越性，其收敛速度及姿态估计精度优于UKF 及STEKF。同时，本文提出的等效旋转矢量惯导大失准角误差方程也可推广用于中、高精度惯导系统。