1. 背景介绍

基于惯性传感器(IMU)的姿态估计广泛应用于机器人、航空航天、无人机以及动作捕捉等众多领域。低成本九轴IMU通常由陀螺仪，加速度计及磁力仪组成，分别测量载体相对于惯性系的角速度，比力及当地磁通量密度。

图1. 姿态估计应用

载体的姿态可以由陀螺仪输出的角速度积分或进一步融合磁力仪和加速度计观测获得。Kalman滤波是目前最常用的多传感器融合算法，但磁力仪测量容易受到周围磁场干扰以及当地磁参考向量不准确的影响。如果不加处理，姿态估计误差可达到上百度。抑制外部磁干扰对姿态估计影响的方法包括：自适应滤波；检测磁干扰并剔除；磁干扰建模；隔离磁干扰对部分状态估计的影响。由于磁干扰的瞬时性与不确定性，自适应与建模的方法并不能完全剔除或建模磁干扰。

本文着重介绍课题组新提出的磁干扰检测与隔离的方法[1]。我们知道，加速度计观测通常可以很好地估计出水平角(俯仰角/横滚角)。若将磁力仪观测只用于航向角估计，可以抑制磁干扰对水平角的影响，提高水平角估计的精度。课题组工作[1]提出了一种部分状态更新Kalman滤波(PS-KF)算法，用于隔离外部磁干扰对水平角及传感器零偏估计的影响。同时还提出了基于向量的磁干扰检测算法，并与PS-KF结合，获得了更好的姿态估计精度。

2. 部分状态更新Kalman滤波(PS-KF)

考虑如下线性离散动态系统 [2]

其中, 分别是k时刻的n维状态与m维观测； 是k-1时刻到k时刻的状态转移矩阵； 是k时刻的观测矩阵;以及符合高斯分布的零均值过程与观测噪声。

传统Kalman滤波通过对系统预测与观测加权求和获得估计结果。其中，最优权值可以通过最小化验后方差的迹获得[2]。PS-KF是在传统Kalman滤波的基础上，通过调节权值，使得指定的某些状态分量不被更新，从而隔离观测对这些状态的影响。假设状态向量中的前p维不被更新，表1给出了PS-KF算法的步骤，及其与传统Kalman滤波的异同。

将表1中PS-KF的增益矩阵带入状态更新方程可以看到前p维状态的更新量为0，也就是说，PS-KF是在指定状态分量不被更新条件下的最优滤波。

3. 磁干扰抑制姿态解算算法

3.1 基于PS-EKF的姿态解算算法

姿态估计算法中，待估计的状态通常包括IMU姿态，陀螺与加速度计的零偏。其中零偏建模为随机游走，而姿态的预测由陀螺的输出积分获得。滤波的观测方程包括：

1. 加速度计观测: 当载体速度变化较小的时候，加速度计观测可以大概反应出当地重力。
2. 磁力仪观测: 利用PS-KF只更新航向角。带入PS-KF的更新方程，即可以隔离磁干扰对水平角估计的影响。

3.2 基于向量的磁干扰检测算法

观测更新之前，磁力仪原始测量需要进行磁干扰检测，以防止磁干扰对滤波器产生不可逆的负面影响。传统基于模值的磁干扰检测算法通过判断磁力仪输出与参考磁场的模值差异剔除受干扰的磁测量。文献[3]提出进一步考虑当地磁向量与重力方向的夹角。实际上，重力方向夹角检测并不能检测出所有磁干扰，比如磁干扰可能位于以重力向量为对称轴，以地磁向量与重力方向的夹角为半锥角的圆锥面上[1]。课题组在工作[1]中提出了一种向量磁干扰检测算法

其中 是Kalman滤波估计的姿态, 为磁力仪测量，为当地磁参考向量。上式第一个不等式约束了磁向量观测与参考磁场在载体系下的方向，第二个不等式约束了磁测量的模值，所以向量磁干扰算法理论上可以完全剔除磁干扰测量。

整个姿态估计算法的框架如图2所示：

图2. 隔离磁干扰的姿态估计算法框架(黄色为工作[1]提出的新算法)

其中，初始对准算法是为了得到初始时刻的姿态和零偏，而磁力仪标定是在使用磁力仪之前，对磁力仪的误差参数进行估计。初始对准算法读者可参考文献[4]，磁标定算法可参考[5,6]。

4. 实验结果与分析

对比如下几种算法：

1. Yaw-EKF：磁向量观测求得的磁航向角+加表观测+传统Kalman滤波；
2. Magnetic vector PS-EKF：加表与磁向量观测+部分更新Kalman滤波；
3. Magnetic vector EKF：加表与磁向量观测+传统Kalman滤波。

4.1 仿真

仿真消费级IMU绕自身旋转,整个数据时长100s，在33-66s时间段添加磁干扰。姿态和加速度计、陀螺仪零偏的估计如图3所示

图3. 状态估计误差

在有磁干扰时间段内的算法姿态估计误差如表2所示

可以看出，相对于Magnetic vector EKF和Yaw EKF，PS-EKF可以提高水平角的估计精度。另外由于更精确的水平角和零偏估计，PS-EKF的航向角估计也有提高。

对于不同磁干扰检测算法，下图在传统Magnetic vector EKF框架下对比了模值磁干扰检测(Norm)，磁观测与重力方向夹角法(Dip)，以及新提出的向量磁干扰检测算法(Vectorial)。不同检测算法估计的角度误差如图4所示。

图4. 不同磁干扰检测方法姿态解算结果对比

4.2 实验

使用Xsens MTi-G-700传感器采集数据，首先将IMU静止，然后手持IMU旋转，在大约80s-136s时间段，使用铁锤子接近IMU模拟产生磁场干扰。结束之后，将IMU放回起始位置。实验的姿态估计误差如图5所示：

图5. 状态估计误差

同样可以看到在无磁干扰的情况下，算法估计精度相当；但是在有磁干扰的环境下，PS-EKF能达到更好的状态估计结果。最后我们将本文提出的PS-EKF与基于向量的磁干扰检测算法结合，得到更好的估计结果，如图6所示。

图6. PS-EKF+基于向量的磁干扰检测法状态估计误差

详情参见全文[1]。

参考文献

[1]. M. Zhu, Y. Ou, W. Yu, “Orientation Estimation by Partial-State Updating Kalman Filter and Vectorial Magnetic Interference Detection” IEEE Tans. Aerospace Electronic Systems (Accepted)

[2]. J. L. Crassidis and J. L. Junkins, Optimal estimation of dynamic systems. Chapman and Hall/CRC, 2011.

[3]. J. K. Lee and E. J. Park, "Minimum-order Kalman filter with vector selector for accurate estimation of human body orientation," IEEE Transactions on Robotics, vol. 25, no. 5, pp. 1196-1201, 2009.

[4]. P. D. Groves, Principles of GNSS, inertial, and multisensor integrated navigation systems. Artech house, 2013.

[5]. Y. Wu, D. Zou, P. Liu, and W. Yu, "Dynamic magnetometer calibration and alignment to inertial sensors by Kalman filtering," IEEE Transactions on Control Systems Technology, vol. 26, no. 2, pp. 716-723, 2017.

[6]. M. Zhu, Y. Wu, and W. Yu, "An Efficient Method for Gyroscope-aided Full Magnetometer Calibration," IEEE Sensors Journal, vol. 19, no. 15, pp. 6355-6361, 2019.