一种微惯性与磁组合测量单元的姿态解算方法

严丹，邓志红，张雁鹏

(1.北京理工大学 自动化学院,北京 100081；2.中国广核集团 苏州热工研究院,江苏 苏州 215004)

0 引言

基于微惯性测量单元(MIMU)或微惯性与磁组合测量单元(MIMMU)的姿态解算系统，具有自主性强、受外界环境影响小、体积小、质量轻、性价比高的优点，因此被广泛应用于无人机、机器人、人体动作捕捉等领域。

MIMU由三轴陀螺仪和三轴加速度计组成，可用于测量载体的角速率和加速度。MIMMU相比于MIMU增加了三轴磁力计，可用于测量外界磁感应强度。利用角速率积分可以获得载体姿态，但由于微陀螺仪随机漂移较严重，在积分过程中解算误差会随时间累积，长时间工作很可能造成发散。当载体处于静止或匀速运动时，加速度计只测得载体坐标系下重力加速度分量，可用于校正横滚角和俯仰角，但当载体加速运动时会影响校正效果。当载体附近无铁磁干扰时，磁力计只测得载体坐标系下地球磁场分量，可用于校正偏航角，但当磁力计受外界铁磁干扰较严重时会影响校正效果。由于利用后二者解算姿态角只基于一个解算周期内的加速度计数据和磁数据，解算误差不随时间积累，因此使用MIMU或MIMMU数据进行信息融合，共同解算载体姿态，一方面可以有效地提高解算精度，另一方面可以使系统长时间工作而误差不发散。

基于MIMU或MIMMU解算载体姿态的信息融合算法可分为三大类：Kalman滤波(KF)法[1-9]、互补滤波法[10-12]和最优化补偿方法[13-17]。KF法通过建立状态方程和观测方程，实现多传感器数据融合。Luinge等[1]提出了一种基于IMU数据解算人体骨骼姿态的线性KF算法。该算法以姿态角估计误差和陀螺仪补偿误差作为状态量，通过建立误差模型推导出Kalman滤波器的状态方程和观测方程。Roetenberg等在文献[1]算法基础上增加了磁扰动误差作为状态量，实现了基于IMMU数据的信息融合[2]，相对于文献[1]可以有效预防航向漂移和外界磁干扰。但上述算法存在计算量较大的问题，Sabatelli等[3]提出一种两级扩展Kalman滤波(EKF)算法，相比于使用Kalman滤波器同时融合加速度计和磁力计数据，该算法的计算量更小。针对载体运动加速度或外界铁磁干扰过大、影响系统姿态解算问题，近年来一部分文献使用自适应Kalman滤波器来增加系统的鲁棒性。例如：Sabatini[4]设计了一种门限式自适应因子，当运动加速度或者磁扰动大于某个阈值时，不使用该时刻的相应数据修正状态量。Ren等[5]提出一种改进的自适应因子，随运动加速度或磁扰动的增大线性加大观测噪声方差阵中对应元素的值，从而实现逐渐减小Kalman滤波器对加速度计或磁力计数据的置信度。Tong等[6]基于传感器误差模型建立乘性扩展Kalman滤波器(MEKF)，并提出了使用隐马尔可夫模型对当前加速度以及磁干扰状态进行判断，以作为自适应调整MEKF中观测噪声方差阵的依据。另一部分文献使用较新的KF算法，例如无迹Kalman滤波器[7-8]、协方差膨胀的乘法扩展Kalman滤波器(CI-MEKF)[9]。该类方法虽然解算效果较好，但计算量相比于后两类算法较大。

根据文献[10]的理论推导，互补滤波法可通过比例积分(PI)控制器对陀螺仪所估计姿态进行高通滤波，对加速度计或磁力计所估计姿态进行低通滤波。该类算法计算量小，但解算效果受参数影响较大。

最优化补偿算法是通过构建并优化损失函数进行信息融合的方法，其计算量介于前两类之间，且可以对非高斯噪声所引入的误差进行校正。Bachmann等[14]提出一种使用加速度计和磁力计数据，对基于角速率更新的先验四元数进行补偿的算法。它通过上一时刻后验四元数获得当前加速度计和磁力计数据的估计值，将测量值与估计值的均方误差看作代价函数，以姿态四元数作为自变量，利用高斯牛顿法对该代价函数进行最优化，以获得后验四元数。Madgwick等在文献[15-16]中提出了一种基于梯度下降的滤波方法。相比于文献[14]，在代价函数不变的前提下，该算法改变了最优化方法，有效降低了计算复杂度。此外，它利用误差积分反馈进行陀螺仪漂移补偿，利用惯性坐标系下磁感应强度在y轴没有分量的特点进行磁补偿。该算法解算精度与KF算法类似且计算量更小。

综上所述，基于最优化补偿的微惯性与磁组合姿态测量方法，相比于KF法计算量更小，相比于互补滤波法鲁棒性更强。但目前的最优化补偿算法未考虑外界铁磁物质对于磁力计的干扰，当铁磁干扰较大时该算法无法正常工作。另外，该算法使用同一个滤波器对MIMMU数据进行融合，加速度计数据会对偏航角的解算造成不利影响，磁力计数据会对横滚角和俯仰角的解算造成不利影响。因此，本文提出一种基于梯度下降法的两级姿态更新算法：在第1级滤波中通过陀螺仪和加速度计数据更新俯仰角和横滚角，在第2级滤波中通过陀螺仪和磁力计数据更新偏航角，以提高系统解算精度。针对磁力计易受外界铁磁干扰问题，该算法在第2级滤波中对铁磁干扰进行实时估计并补偿，使算法具有一定抗铁磁干扰能力。最后对所提方法进行了仿真和实测实验验证。

1 传感器建模

1.1 坐标系建立

取地心惯性坐标系Oixiyizi(简称i系)为惯性坐标系，其原点Oi位于地球地心，xi轴指向春分点，zi轴沿地球自转轴，yi轴与xi、zi轴构成右手直角坐标系。取载体坐标系Obxbybzb(简称b系)与传感器模块固连，其原点Ob位于传感器模块重心，xb轴沿传感器模块横轴指向右侧，zb轴沿传感器模块竖轴指向上方，yb轴与xb、zb轴构成右手直角坐标系。设初始时刻的载体坐标系与导航坐标系Onxnynzn(简称n系)重合。

1.2 姿态表示与测定

目前，常用的姿态更新方法有四元数法[1-10,12-17]、欧拉角法[11]、旋转矢量法[18-19]等。其中，欧拉角法存在万向节死锁问题，无法适用于全姿态解算。旋转矢量法采用多子样算法对不可交换误差进行有效补偿，计算量相比于四元数法较大，适用于角机动频繁或存在严重角振动的运载体的姿态更新。由于机器人、无人机、人体动作捕捉等一般不存在严重角振动，因此本文选用四元数法进行载体姿态更新。

1.2.1 四元数姿态更新

四元数可用于表示刚体旋转。设刚体相对于n系做定点运动，采用四元数表示刚体的定点运动：

(1)

式中：Q为从n系到b系的旋转四元数；un为旋转轴和旋转方向；α为转过的角度。通过四元数表示可以将b系看作由n系经过无中间过程的一次性等效旋转形成的。

利用四元数表示旋转矩阵：

(2)

四元数微分方程：

(3)

由于陀螺仪测量需要一定时间，所测得角速率是离散数据，需要对(3)式进行基于1阶近似的离散化处理，可得

(4)

式中：I4为4阶单位矩阵；T为陀螺仪采样时间；ωk为k时刻陀螺仪量测值；Qk为k时刻n系到b系的旋转四元数；Z(·)为ωk的反对称矩阵，定义为

(5)

ωk,x、ωk,y、ωk,z分别为向量ωk的3个元素。

1.2.2 欧拉角

载体的空间姿态可以看作是导航坐标系Onxnynzn依次绕航向轴、俯仰轴、横滚轴旋转ψ、θ、γ后的复合结果。

当旋转顺序设定为z-x-y时，从n系到b系的旋转矩阵为

(6)

1.3 传感器模型

本文所使用的传感器MIMMU由微陀螺仪、微加速度计及微磁力计组成。设t时刻微陀螺仪所测数据为yg,t，其中包含b系相对于i系的角速度信息、陀螺零偏、测量噪声；设t时刻微加速度计所测数据为ya,t，其中包含载体运动加速度、b系下所测得的重力加速度、系统固有偏置、测量噪声；设t时刻微磁力计所测数据为ym,t，其中包含b系下所测得的地球磁场、外界铁磁干扰、系统固有偏置、测量噪声。在不考虑传感器非正交误差、灵敏度误差前提下，其具体量测模型如下：

(7)

为了简化计算，本文将ds,t和dh,t合并为t时刻磁力计所受铁磁干扰矢量dt，原磁力计量测模型可简化为

(8)

2 数据融合算法设计

图1 本文提出的姿态解算算法框图Fig.1 Block diagram of the proposed attitude determination method

由于Kg、Ka、Km、bg、ba、bm属于传感器系统误差，可通过数据预处理校正。假设当前传感器已经过校正，设Kg、Ka、Km均为3阶单位矩阵以及bg、ba、bm均为零向量，可得

yg,t=ωt+vg,t,

(9)

(10)

(11)

若初始时刻传感器水平放置，即初始姿态的z轴(即zn轴)与重力加速度方向重合，则加速度计所测x轴与y轴数据均为0，即g=[0 0gz]T。随后，当传感器旋转到某一姿态并保持静止或匀速运动时，可通过(6)式、(10)式求得加速度计量测值ya：

(12)

此时传感器的俯仰角θ和横滚角γ为

θ=arcsinya,y,

(13)

(14)

但无法通过加速度计数据算出偏航角ψ.

一方面，由于磁力计相比于加速度计更容易受到外界环境干扰(如电子设备等)，其数据可信度相对较小。另一方面，无论传感器初始姿态如何，磁力计所测数据都与偏航角ψ有关，可以利用磁力计数据修正载体偏航角。因此本文采用两级姿态更新法，在第1级更新中将陀螺仪与加速度计数据融合，对载体的俯仰角和横滚角进行更新；在第2级更新中将陀螺仪与磁力计数据融合，对载体的偏航角进行更新。

2.1 第1级姿态更新

由于本文所提算法主要应用于人体运动捕捉、小型机器人、无人机等设备姿态解算，载体运动加速度相对于重力加速度较小，因此忽略运动加速度部分，加速度计量测模型可简化为

(15)

t时刻加速度的实际测量值与估计值之间误差可以表示为

(16)

由此可知，当估计四元数q越接近t时刻由n系到b系旋转四元数的真实值qt时，ε1(q)越接近[0 0 0]T。利用ε1(·)的欧几里得范数作为代价函数：

F1(q)=ε1(q)Tε1(q),

(17)

将t-1时刻由n系到b系的后验估计四元数作为估计四元数的初始值，利用梯度下降法对F1(q)进行最优化，沿损失函数的负梯度方向更新变量，有助于减小损失函数：

(18)

设q=[q0q1q2q3]T、g=[gxgygz]T，由(2)式、(16)式、(17)式可得

(19)

(20)

利用陀螺仪数据更新旋转四元数的1阶离散公式为

(21)

(22)

式中：μ1为置信于a,t的权重。

(23)

式中：ρ1=μ1β1，为可调参数。

2.2 第2级姿态更新

由于地球地表磁场强度在25～65 μT范围内，比较微弱，铁磁干扰数值常大于地球地表磁场，因此无法忽略铁磁干扰部分，本文选择对铁磁干扰dt进行估计。

由(11)式得到t时刻磁力计的实际测量值与估计值之间误差为

(24)

由(11)式和(24)式可知，当估计四元数q越接近t时刻旋转四元数的真实值qt，且估计铁磁干扰d越接近t时刻铁磁干扰的真实值dt时，ε2(q,d)越接近[0 0 0]T。

令x=[qd]T，利用ε2(·)的欧几里得范数作为代价函数:

F2(x)=ε2(x)Tε2(x)，

(25)

利用梯度下降法对F2(x)进行最优化：

(26)

(27)

式中：Λ(x)为第2级代价函数F2(x)关于估计四元数q求偏导后转置的结果，可用于优化估计四元数。由于该公式太过复杂，在此省略展示。

(28)

式中：μ2为置信于m,t的权重，其数值较小；ρ2=μ2β2，为可调参数。

2.3 整体更新

根据磁力计量测模型(11)式，可得t时刻后验估计铁磁干扰为

(29)

3 实验验证

3.1 解算误差表示

令真实姿态四元数为qtrue，通过算法解算出的姿态四元数为q，q相对于qtrue的误差可通过Δq表示为

(30)

根据四元数的辐角公式(1)式，可得解算误差角Δθ为

(31)

式中：scalar(·)为提取四元数内标量的函数。本文在后续的实验中通过Δθ衡量解算误差。

3.2 仿真实验

通过轨迹发生器可自定义载体运动情况，并产生模拟陀螺仪、加速度计、磁力计数据以及真实姿态数据。

设计载体运动方式和外部环境的步骤如下：

1)载体静止20 s；

2)载体绕xn轴以3π rad/s角速率旋转40 s，沿xn、yn、zn轴分别施加10 μT的铁磁干扰；

3)载体静止20 s，外界铁磁干扰保持不变；

4)载体绕xn轴以3π rad/s角速率旋转40 s，沿xn、yn、zn轴分别施加30 μT的铁磁干扰；

5)载体静止20 s，外界铁磁干扰保持不变；

6)载体绕xn轴以3π rad/s角速率旋转40 s，沿xn、yn、zn轴分别施加50 μT的铁磁干扰；

7)载体静止20 s，外界铁磁干扰保持不变。

采样周期设为100 Hz，陀螺仪漂移设置为0.001°/s，加速度计漂移设置为8×10-6g/s，磁力计漂移设置为0.05 μT.地球局部参数均根据北京进行设置，重力加速度设为[0 m/s2，0 m/s2，-9.801 47 m/s2]，地球表面磁场设为[27.77 μT，-3.35 μT，46.97 μT].地球自转角速率设置为0.000 072 921 rad/s，地球近似半径设置为6 378 137 m.

利用上述数据对本文提出算法进行仿真测试。将本文提出算法的实验结果与角速率积分法、互补滤波法、传统优化法以及两级姿态更新法的运行结果进行对比。其中，角速率积分法仅利用微陀螺仪数据，通过离散化四元数微分方程更新姿态四元数。本文用作对比的互补滤波法由左国玉等[12]提出，同时对MIMMU数据进行融合。其余3种算法都是基于梯度下降法优化姿态四元数，具体来说：传统优化法由Madgwick等[16]提出，它使用一个滤波器同时对MIMMU数据进行融合，且在建模过程中未考虑外界铁磁干扰；两级姿态更新法是根据本文提出算法去除铁磁补偿部分获得的，它与前者区别在于采用了两级滤波器，先对陀螺仪与加速度计数据进行融合，更新载体的俯仰角和横滚角，再对陀螺仪与磁力计数据进行融合，更新载体的偏航角；本文提出算法相比于两级姿态更新法，在第2级滤波建模中考虑了铁磁干扰，增加了铁磁干扰补偿。

在本文提出算法中令ρ1=0.000 026 7，ρ2=0.000 030 7.各算法解算误差角Δθ的阶段均值与总体均值如表1所示，表中MeanΔθ为Δθ均值。各算法针对该数据的解算误差角Δθ随时间变化情况如图2所示。

表1 各算法MeanΔθ统计表Tab.1 MeanΔθ of each algorithm (°)

图2 各算法解算误差对比图(针对仿真数据)Fig.2 Comparison among the attitude determination errors of various algorithms (for simulated data)

根据仿真设定，实验载体在20～60 s、60～120 s、120～200 s阶段所受外界铁磁干扰逐渐增加，通过该仿真数据可对各个算法抗铁磁干扰能力和姿态解算精度进行测试。由表1可以看出，互补滤波法、传统优化法、两级姿态更新法以及本文提出算法的各阶段MeanΔθ始终低于角速率积分法，表明使用MIMMU数据解算载体姿态有利于提高解算精度。另外，本文所提算法各阶段的MeanΔθ相近，没有呈现递增状况，且都小于3°；而其余3种融合解算算法各阶段的MeanΔθ都呈现出明显递增，且均大于3°，表明本文提出算法相比于其他对比算法可以有效估计并补偿外界铁磁干扰，使系统具有一定的抗铁磁干扰能力且解算精度更高。

根据仿真设定，载体在60～80 s、120～140 s、180～200 s处于静止状态，且所受铁磁干扰递增。由图2可以看出，在这些时间段，角速率积分法解算误差不变，传统优化法、两级姿态更新法以及本文提出算法的解算误差随时间减少，而互补滤波法解算误差在60～80 s区间随时间减少、在120～140 s和180～200 s区间随时间增加。表明基于最优化的补偿算法相比于互补滤波法鲁棒性更强。通过对比传统优化法和两级姿态更新法的解算结果可以看出，两级姿态更新相比于单滤波器更有利于提高解算精度，当载体处于静止时其补偿解算误差的速度也更快。通过对比两级姿态更新法和本文提出算法的解算结果可以看出，本文所设计的铁磁干扰补偿算法效果明显，可以有效提高系统的鲁棒性，使之适用于各种铁磁干扰环境。

3.3 实测实验

使用荷兰Xsens公司生产的MTi-G-710作为传感器，其内部包含三轴微陀螺仪、三轴微加速度计、三轴微磁力计，相关参数如表2所示。MTi-G-710所配套的软件内置一套基于KF的姿态解算算法，当传感器处于均匀磁场中时，该算法解算结果的误差均方根(RMS)如表3所示。本文将该算法输出的姿态四元数看作真实姿态四元数，进行后续实验。

通过手持MIMMU模块，分别在铁磁干扰不同的3个地点进行自由晃动、获得3组数据，陀螺仪、加速度计、磁力计的采样周期都设置为100 Hz.由于MTi-G-710中磁力计的输出数据是单位化后的结果，即当外界无铁磁干扰时，每一时刻磁力计数据的范数都为1，因此外界铁磁干扰越大，每一时刻磁力计数据的范数越偏离1.根据(32)式量化该组数据所受铁磁干扰的大小：

表2 MTi-G-710内置传感器相关参数表Tab.2 Specifications of MTi-G-710

表3 MTi-G-710配套算法解算精度表Tab.3 Attitude determination performance (°)

(32)

式中：n为该组数据总时刻数。

利用(32)式对各数据受铁磁干扰大小进行计算，结果如表4所示。由表4可知，数据1所受铁磁干扰整体最小，数据2次之，数据3所受铁磁干扰最大。

利用上述3组数据进行算法检验，将本文提出算法与角速率积分法、互补滤波法、传统优化法以及两级姿态更新法进行对比。

由于上述实验算法都需要进行参数设置，利用数据1对各算法进行调参，其中在本文提出算法中令ρ1=0.000 026 7，ρ2=0.000 030 7.各算法解算误差角Δθ的均值如表5所示。将互补滤波法与角速率积分法的MeanΔθ进行对比，通过表5可以看出：随着外界铁磁干扰的增大，该算法校正陀螺仪数据的效果逐渐变差，甚至在数据3时其解算误差超过单纯角速率积分法。由此表明互补滤波法的解算效果受参数影响大、鲁棒性较弱。而传统优化法、两级姿态更新法以及本文提出算法对于上述3组数据，它们的解算误差始终低于角速率积分法以及互补滤波法，表明最优化补偿法相对于互补滤波法解算效果更好，且鲁棒性较强，在不同铁磁干扰环境下都能对陀螺仪数据进行一定程度的有效校正。根据表5，对比传统优化法和两级姿态更新法的MeanΔθ可以看出，通过两级姿态更新可以有效提高姿态解算精度；对比两级姿态更新法与本文提出算法的MeanΔθ可以看出，增加铁磁干扰补偿有利于进一步提高算法解算精度，并使其可以很好地适应不同铁磁干扰环境。

表5 各算法MeanΔθ统计表Tab.5 MeanΔθ of each algorithm (°)

以数据1为例进行具体分析。数据1的角速率、加速度、磁力计数据范数随时间变化情况如图3所示，各算法针对数据1的解算误差角Δθ随时间变化情况如图4所示。通过对比图3、图4可以看出：0～25 s，传感器处于静止状态，此时角速率积分法、互补滤波法、传统优化法的解算误差都随时间累积，而两级姿态更新法和本文提出算法的解算误差先增大、后减小，表明两级姿态更新相比于单滤波器融合法可以更有效地补偿传感器静置时陀螺仪漂移带来的解算误差；40～50 s，陀螺仪发生较大漂移，各算法解算误差都出现不同程度的陡增；50～70 s，根据角速率积分法可以看出在这一时段陀螺仪漂移情况较平稳，本文提出算法相比于其他对比算法能更快地通过加速度计和磁力计数据降低解算误差；70～85 s，传感器恢复静止状态，前3种方法解算误差都随时间增加，两级姿态更新法虽能在一定程度上补偿此前解算误差，但补偿速率较慢，本文提出算法由于加入了铁磁补偿，可以有效估计外界铁磁干扰，以较快速率将解算误差降低到较低状态，这一特点使本算法更适用于长时间正常工作。

图3 陀螺仪、加速度计、磁力计数据范数随时间变化(针对数据1)Fig.3 Magnitudes of gyroscope data,accelerometer data and magnetometer data (for Data 1)

图4 各算法解算误差对比(针对数据1)Fig.4 Comparison among the attitude determination errors of various algorithms (for Data 1)

各算法针对数据2、数据3的解算误差角Δθ随时间变化情况分别如图5、图6所示。对比图4、图5、图6可以看出，外界铁磁干扰的增大会对互补滤波法、传统优化法、两级姿态更新法的解算精度有较大影响，此时本文所提铁磁补偿法发挥作用，使系统在较强铁磁干扰环境下仍能正常工作。

图5 各算法解算误差对比(针对数据2)Fig.5 Comparison among the attitude determination errors of various algorithms (for Data 2)

图6 各算法解算误差对比(针对数据3)Fig.6 Comparison among the attitude determination errors of various algorithms (for Data 3)

4 结论

本文使用MIMMU作为传感器，提出了一种抗铁磁干扰的两级姿态更新姿态解算方法：它基于姿态四元数更新，可用于全姿态解算；第1级通过陀螺仪和加速度数据更新俯仰角和偏航角，第2级在考虑铁磁干扰的情况下通过磁力计数据修正偏航角。

仿真和实测实验结果表明：两级姿态更新相比于单滤波器方法可以有效提高解算精度，而本文提出的利用梯度下降法估计并补偿铁磁干扰方法，可以使算法具有一定抗铁磁干扰能力，解决了在外界存在铁磁干扰的环境下低精度器件中高精度姿态获取的难题。本文方法可广泛应用于人体运动捕捉、无人机及机器人等领域，具有广阔的应用前景。