0 引言

    近年来，以MEMS(Micro-Electro-Mechanical System)陀螺仪、加速度计、磁强计为组合的姿态测量系统在车辆自主驾驶、微型飞行器、人体运动分析等领域得到广泛应用^[1-3]。低成本MEMS器件由于漂移及高噪声的特点，需要组合传感器通过姿态解算得到稳定的姿态信息^[4]。而当存在长时间的机动加速度和磁场干扰时，无法获得准确的姿态信息。因此，采用合理的姿态描述模型及特定的解算方法实现信息融合一直是个棘手的问题。 

    针对以上问题，国内外学者提出了一些改进算法^[4-8]。文献[4]提出修正卡尔曼滤波算法，由于无法补偿外部干扰，估计精度下降明显；文献[5]采用扩展卡尔曼滤波融合方法，由于磁力计耦合干扰而影响倾角精度，另外EKF存在高阶项忽略带来的误差问题，且维数高、计算复杂；文献[6]采用残差的思想识别机动加速度，但是没有考虑偏航角；文献[7]采用旋转矩阵EKF，该算法实现了机动加速度补偿和降低了算法复杂度，但是无法实现全姿态估计。

    为解决在机动加速度干扰下精度下降及耦合干扰问题，本文通过状态反馈估计机动加速度，实现对加速度补偿，并利用模糊规则动态调整协方差阵，以降低机动加速度干扰，同时串级结构消除了磁场信息对倾角的影响，最后设计实验验证本文方法的有效性。

1 姿态描述与传感器模型

1.1 坐标系和旋转矩阵

    本文按照Z-Y-X旋转方向从载体坐标系(B系)旋转到大地坐标系(E系，定义X/Y/Z正轴分别指向北/东/天)得到旋转矩阵为：

式中，c和s分别代表cos和sin，Ψ、θ、φ表示偏航角、俯仰角及翻滚角。φ、θ和Ψ的获取表达如下：

1.2 传感器模型

    MEMS传感器输出模型：

式中，ω为陀螺仪理想角速率输出，b_a、b_g为加速度和陀螺仪零偏，G=(0，0，g)^T为重力向量，g取9.8 m/s²，a为机动加速度，h为磁场参考向量，e_g、e_a、e_m分别是陀螺仪、加速度计及磁强计互不相关的高斯随机白噪声。

2 串级LKF滤波设计

    本文主要利用旋转矩阵中第三行向量与倾斜角相关而与偏航角无关的特性，采取分层设计两个线性卡尔曼滤波器，以逐级获得倾斜角和偏航角的最优估计。

2.1 倾角卡尔曼滤波

式中Δt为采样时间。状态在递推过程中会出现发散情况，必须通过加速度数据进行校正。然而在机动条件下加速度受到机动加速度的很大干扰，必须对机动加速度进行估计，以提升动态条件下的估计精度。

    当载体朝着一定方向运动时，不存在突变转动条件下机动加速度a在相邻短暂时刻之间不能突变，所以本文采用文献[7]的一阶次低通连续过程模型。加速度模型如图1所示。上一时刻机动加速度的后验估计a⁺(k-1)可根据上时刻量测加速度和最优理论加速度之差获得，即：a⁺(k-1)=y_a(k-1)-b_a-gx₁(k-1)。根据加速度模型可得下一时刻的先验估计：

其中，a⁺(k-1)为上时刻后验估计，a^-(k)为本时刻先验估计，c_a为量纲。

    由式(3)的第二项方程得到观测方程：

    对角阵R_acc往往用于克服不存在突变转动曲线运动导致的加速度干扰。当存在突变转动加速度的情况下，对协方差阵采用模糊规则进行调整，协方差阵模型如下：

    利用模糊准则设定err1、err2为模糊输入变量，K_a、K_g为模糊输出变量。err1、err2={NB，NS，ZE，PS，PB}，元素分别代表负大、负小、零、正小、正大。本文考虑到三角形运算简单，对模糊变量的隶属度函数采用对称三角形，err1、err2模糊化的隶属度函数μ(x)如图2。模糊规则如表1所示。

    由表1K_a、K_g取值，可得噪声协方差阵：

2.2 偏航角卡尔曼滤波

式中h_my，h_mx为无噪声下的理论磁场水平分量，e_mx、e_my为磁场水平噪声。

    由上可得观测方程：

    算法流程如图3所示。

3.1 静态测试

    采用加速度计解算、本文融合算法解算、陀螺仪解算3种方法获取静态姿态信息。

    图4中加速度计静止下获得的姿态角噪声干扰大，本文算法解算的姿态角消除了噪声及漂移导致的姿态累计误差，实际上平台放置存在小角度倾斜，解算的姿态角偏离零点，但达到稳定角度后无发散和超调现象，精度也有较大提升；图5可得通过陀螺仪得到的姿态角会由于噪声和陀螺仪漂移而逐渐发散，静止下也无法稳定到固定角度。

3.2 动态测试

    图6为机动加速度干扰下，本文算法和文献[5]EKF算法得到的姿态角与参考平台姿态角对比的误差值。

    表2为加速度估计、EKF与本文算法3种算法估计的姿态角与参考平台姿态角对比的误差值均方差数据对比。

    结果表明，本文算法能够提高在机动加速度干扰下的姿态估计精度。动态条件下本文算法得到的俯仰角和翻滚角均方差小于扩展卡尔曼滤波算法，而静态条件下主要为了克服累计误差，同样保持着高精度的特性。

3.3 磁场干扰测试

    在静止的采集平台附近添加硬铁等磁场干扰，姿态角信息输出如图7。图7表明，虽然EKF算法偏航角波动较小，但已影响到翻滚角精度，俯仰角产生角度波动；本文算法由于两级滤波器解耦合，第二级滤波无法影响水平姿态，保持正确角度不变，提高了水平姿态角精度。

4 结论

    本文针对在加速度干扰条件下姿态精度下降及干扰相互耦合等问题，提出基于模糊调整的串级LKF滤波方法，。该方法采用分层设计解决了磁场干扰对倾角的影响，并重点研究根据运动状态信息模糊调整协方差阵减小机动加速度干扰和对加速度进行补偿的方法。实验验证，该方法消除了累计误差，提高了机动加速度干扰下的姿态估计精度，消除了磁场干扰对倾角的影响，比较适合推广至低成本姿态系统中使用。

参考文献

[1] ZIHAJEHZADEH S，LOH D，LEE T J，et al.A cascaded Kalman filter-based GPS/MEMS IMU integration for sports applications[J].Measurement，2015，73：200-210.

[2] KANG C W，PARK C G.Attitude estimation with accelerometers and gyros using fuzzy tuned kalman filter[C].Proc.of ECC，2009：3713-3718.

[3] LIGORIO G，SABATINI A M.A novel Kalman filter for human motion tracking with an inertial based dynamic inclinometer[J].IEEE transactions on Biomedical Engineering，2015，62(8)：2033-2043.

[4] 吴杰，闫建国.基于修正的卡尔曼滤波的姿态估计算法研究[J].计算机仿真，2012，29(2)：54-57.

[5] SABATIN A M.Quaternion based extended Kalman filter fordetermining orientation by inertial and magnetic sensing[J].IEEE Transactions on Biomedical Engineering，2006，53(7)：1346-1356.

[6] SUH Y S.Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of externalacceleration[J].IEEE Transactions on Instrumentation and Measurement，2010，59(12)：3296-3305．

[7] LEE J K，PARK E J，NOBINOVITCH S.Estimation of attitude and external acceleration using inertial sensor measurement during various dynamic conditions[J].IEEE Transactions on Instrumentation and Measurement，2012，61(8)：2262-2273．

[8] 肖文建，路平，张曦郁.一种面向机动的航姿测量系统设计[J].电光与控制，2014，21(5)：83-87.

[9] MAHONY R，HAMEL T，PFLIMLIN J M.Nonlinear complementary filters on the special orthogonal group[J].IEEE Transactions on Automatic Control，2008，53：1203-1218.

[10] 秦永元.惯性导航(第二版)[M].北京：科学出版社，2015.

作者信息:

杜红彬，侯秋华，康小明

（华东理工大学 信息科学与工程学院，上海200237）