低成本惯导系统预处理和姿态解算

毛红瑛，陈至坤，张瑞成

(华北理工大学电气工程学院，河北唐山 063210)

0 引言

惯性传感器和磁阻传感器进行姿态解算最早被应用在导航领域中。近年来，惯性传感器的发展速度日益加快，不断向更小的体积，更轻的质量、更便捷，以及更低的功耗方向发展。由于其价格低廉，也被大量的应用于无人机，游戏机和虚拟现实等众多领域中。

惯性传感器姿态测量系统是由三部分组成，分别为三轴加速度计、三轴陀螺仪和三轴电子磁罗盘[1-2]，可以通过采集传感器输出的信息，利用惯性导航系统原理来测量运动载体的俯仰角、横滚角和偏航角。目前常用的姿态测量方法有很多，本文基于惯性测量单元对姿态进行求解。由于惯性测量单元有着不受气候和空间条件的限制，同时适用于对测量精度和动态性能较高的领域，因此与其他姿态测量方法相比有着较大的优势，但是由于选用的是低成本传感器，陀螺仪和加速度计存在零偏和温漂，三轴磁罗盘又容易受环境影响，因此长时间工作必定会产生较大的累积误差，针对MEMS惯性传感器的误差问题，文献[3]提出了一套零滞后补偿的测量方法来对自回归滑动平均系统的参数进行估计，同时也提出了一种减小噪声的算法来降低残差自适应滤波器的噪声影响。文献[4]中提出了一种随机误差建模补偿方法，主要从确定建模样本的长度、求解模型参数、处理卡尔曼滤波器输出结果和降低模型阶数这些方面出发。由于每种传感器都有自身的局限性，为了消除信息冗余，我们需要对多传感信息融合进行多层次多级别的信息自动处理过程。在对姿态求解方面，最常用的方法有3种，分别是互补滤波法、卡尔曼滤波法和梯度下降法等，用不同的算法得到的解算效果也不尽相同。文献[5]中介绍了自适应互补滤波算法、梯度下降和互补滤波结合的这两种算法，并分别从静态和动态两方面对姿态解算的结果进行比较。文献[6]中设计了一种新的卡尔曼滤波方法，从测量方程出发，对其进行处理。用一个线性测量方程表示，最后基于卡尔曼滤波方法对姿态进行解算。文献[7]中讨论了2种姿态算法，梯度下降法和扩展卡尔曼滤波算法来求解姿态，先利用梯度下降法对三轴加速度计和三轴磁罗盘进行四元数的求解，通过陀螺仪可以求得一组姿态四元数，最后对两者进行融合，进而取得最优姿态。文献[8]对Sage-Husa自适应滤波算法进行改进，使算法结构变得简单，通过遗忘因子对噪声协方差进行估计提高了速度和姿态角的解算效率和精度。文献[9]介绍了Sage-Husa自适应扩展卡尔曼滤波算法，通过对测量噪声的取值进行改进，提高了算法的鲁棒性，对车辆的行驶状态进行了准确的估计。

本文设计了一种IMU误差模型，对惯性传感器数据进行静态预处理的基础上，通过构建两级噪声方差阵的自适应扩展卡尔曼滤波融合方法降低了有害加速度的影响，提高了姿态角的解算精度。

1 姿态表示与解算过程

利用三轴加速度计来对载体的加速度进行测量，三轴磁罗盘对磁场变化进行测量，三轴陀螺仪对载体三个轴的角速率进行测量。为了使求得的姿态角较为准确，先对惯性器件的测得值进行预处理，建立姿态角解算模型，最后使用自适应扩展卡尔曼算法进行姿态角的计算。

在惯性导航系统中，为了更好的说明一下载体的姿态，在此建立相应的空间坐标系，将载体坐标系设为b系，地心惯性坐标系设为i系，地理坐标系设为n系，选取东北天为地理坐标系，其中地理坐标系的OnXn轴正方向指向正东方向，OnYn轴正方向指向正北方向，OnZn轴正方向垂直地面指向天空，并且符合右手定则原理，通常情况下，导航坐标系就是地理坐标系，姿态实际上就是一个旋转过程，因此载体的姿态就是载体坐标系相对于地理坐标系东北天之间关系，通常使用四元数法、欧拉角法、方向余弦矩阵、旋转矢量等方法对旋转运动进行描述，现规定偏航角用φ表示，是指载体坐标系绕其Z轴旋转方向的角度，范围是0°～360°；俯仰角用θ表示，是指载体坐标系绕其Y轴旋转方向的角度，范围从-90°～90°；横滚角用γ表示，是指载体坐标系绕其X轴旋转方向的角度，范围从-180°～180°。方向余弦矩阵表示的坐标变换为

(1)

(2)

在惯性导航系统中，方向余弦矩阵法运算量大，欧拉角法存在万向节死锁的现象。为了更方便的描述刚体的运动，通常使用四元数法，由于四元数法具有最佳性能：不仅可以避免绕固定坐标轴旋转顺序所造成的万向节死锁现象，而且在精度高的同时还可以简化计算量，因此将载体坐标系相对于地理坐标系的旋转变化通过四元数用符号q表示，具体形式为：

q=q0+q1i+q2j+q3k

(3)

(4)

2 传感器数据的预处理

由于传感器的测量结果受环境和其精度等因素的影响，其精度影响主要体现在安装误差、零偏误差、刻度因数，同时在测量中会出现不可避免的噪声和野值，若是直接进行计算会出现较大的误差，因此数据的预处理就显得至关重要。

2.1 陀螺仪误差模型

ARMA全称为自回归滑动平均模型，是一种基于时间序列的分析方法，将MEMS陀螺仪随机误差用该方法进行建模分析，首先要满足平稳正态、零均值的条件。然而通过采样得到的随机噪声序列并不完全满足，因此建模之前先对采集数据进行去除偏移量和滤波处理。MEMS随机误差时序模型阶次越高真实性就越高，计算难度和计算误差也会随着阶数的升高而增大[10]。用p，q来表示模型阶数，故ARMA(p,q)模型表示为：

x(t)=φ1x(t-1)+φ2x(t-2)+…+φpx(t-p)+
ε(t)-θ1ε(t-1)-θ2ε(t-2)-…-θqε(t-q)

(5)

式中：φ1,φ2,…,φp为自回归系数；θ1,θ2,…,θq为移动平均数；ε(t)为输入的白噪声。

综合上述阶次对模型的影响，通常将随机误差模型定在3阶以内，因此本文取p=2，q=1，在静止状态下采集6 000个样本数据，则ARMA(2,1)为

x(t)=φ1x(t-1)+φ2x(t-2)+

ε(t)-θ1ε(t-1)

(6)

根据公式：

(7)

式中：ρk为样本自相关系数。

对样本数据用最小二乘法进行曲线拟合得到最终表达式：

ω(t)=-0.172ω(t-1)+0.135ω(t-2)-
0.571ε(t-1)+ε(t)

(8)

2.2 加速度误差模型

利用最小二乘法对不同阶的一元高阶模型进行拟合[11]，在静止状态下采集6000个左右的加速度计误差信号点，本文采用一元三阶模型对加速度计误差进行补偿，最终表达式如下所示：

a(t)=5.1×104a3(t-1)-530a2(t-1)+

1.59a(t-1)+0.01

(9)

3 扩展卡尔曼滤波解算过程

卡尔曼滤波算法实际上是由预测和校正(或更新和修正)2部分组成。在预测过程中，当前状态的预测是通过滤波器对上一时刻的状态估计求得。在校正过程中，为了获得更准确，接近真实值的新估计量，需要滤波器利用当前时刻状态的观测量来修正预测阶段获得的预测量。

在实际应用中，惯性导航系统是一个非线性系统[13]，因此此扩展卡尔曼滤波算法的方程如下所示：

(10)

式中：xk为状态向量；Zk为测量向量；Hk为三维常系数测量矩阵；B为已知的系统结构参数；ωk-1为k-1时刻过程噪声；Vk为k时刻测量噪声。

根据以上建立的状态方程和观测方程，利用扩展卡尔曼算法对状态向量和协方差进行预测，然后通过递归过程求得最优四元数，最后对四元数进行归一化处理，计算姿态角。

根据陀螺仪在载体坐标系下输出的数据，和由初始姿态角计算得到初始四元数，列写如下四元数的微分方程[14]：

(11)

式中ωx,ωy,ωz为陀螺仪输出的三轴角速率。

通过式(12)得到实时更新的四元数为

(12)

式中T为采样周期。

为了得到较为准确的姿态角，将姿态四元数q0(k)、q1(k)、q2(k)、q3(k)作为状态量，列写状态方程为

x(k) =[q0(k)q1(k)q2(k)q3(k)]T

=f(x(k-1),k-1)+w(k-1)

(13)

对函数f(x(k-1),k-1)求偏导得到的雅克比矩阵为A(x,k-1)，表示如下：

(14)

将加速度计的姿态解算方程作为测量方程，则观测方程列写为：

(15)

式中：

(16)

对函数求h(x(k),k)求偏导得到的雅克比矩阵为

(17)

扩展卡尔曼滤波算法的核心公式[15]如下：

(18)

式中：P(k|k-1)为x(k|k-1)预测误差对应的协方差；Q为系统噪声协方差；R为测量噪声协方差；Kk为卡尔曼增益；I为单位矩阵。

通过上述过程即可求得最优估计四元数，对其进行规范化处理得到：

(19)

再根据四元数转化欧拉角的式(4)，得到最终的姿态角，实现姿态数据的融合。

4 自适应扩展卡尔曼滤波解算过程

从理论上说，只有在准确的知道系统的结构参数和噪声特性才能通过扩展卡尔曼算法得到最优估计值，由于在惯性导航系统中，这2个参数会存在误差。因此通过自适应算法对测量噪声进行处理，提高算法的精度。

首先需要降低有害加速度，第一级测量噪声为

(20)

其中，通过试凑法确定权重因子ka，为k+1时刻的规范化加速度值‖a‖，当仅有重力作用时，‖a‖=1，I为3×3的单位矩阵。

在使用惯性传感器进行测量时，测量噪声并不是一成不变的，当这种不确定性存在时，就可能会造成估计精度的下降，甚至有可能会发散[16]。而Sage-Husa自适应卡尔曼滤波算法就是对运动过程中的载体的测量噪声进行较为准确的估计，从而对协方差矩阵更新，使得姿态解算的精度更高。在这里使用指数渐消记忆自适应卡尔曼滤波算法来实现测量噪声的改变，计算第二级测量噪声为

(21)

=ka(|‖a‖-1|)I+(1-βk)Rk-1+βk

(22)

因此，使用改进后的量测噪声，将其带入式(18)所示的过程进行解算，最终得到自适应扩展卡尔曼滤波算法，求得姿态角。

5 实验与结果分析

基于STM32F051K8微处理器系列芯片，选用ICM42605的三轴陀螺仪、三轴加速度计模块搭建了实验平台。将其安装在无人机3p平台进行验证，将无人机输出姿态角作为参考角度。整个实验放在远离磁场干扰的地方进行。

5.1 数据预处理实验

将三轴陀螺仪固定好，然后对信号进行除偏移和滤波处理，由式(8)的ARMA模型建立卡尔曼滤波器，将陀螺仪测量的实际漂移数据作为卡尔曼滤波器的状态输入，则有：

状态方程x(k|k-1)=Ax(k|k-1)+Bwk-1

观测方程yk=Ckyk+Vk

参数设置如下所示：

(23)

(24)

C=[1 0]

(25)

图1 陀螺仪随机漂移误差

对于加速度计的处理与陀螺仪类似，根据式(9)误差模型利用卡尔曼滤波进行处理，得到滤波前后的误差如图2所示。同时陀螺仪和加速度计在滤波前后的数据对比如表1所示，对均值和标准差分别进行说明。

图2 加速度计随机漂移误差

表1 滤波前后数据对比

由表1可以看出，陀螺仪的随机漂移误差均值由滤波前的0.008 62降到了0.0009 7，减小了一个数量级，同样加速度计的随机漂移误差在均值和标准差方面也有明显的降低。说明了数据的预处理效果比较好，随机漂移可以被有效抑制。

5.2 姿态解算实验

在预处理的基础上，进行扩展卡尔曼滤波算法和自适应扩展卡尔曼滤波的姿态融合算法，得到最终的姿态角。设置采样频率为100 Hz，时间为3 min左右，验证算法的实际运行效果。图3和图4分别为扩展卡尔曼滤波算法和自适应扩展卡尔曼滤波的计算结果，表2为上述2种姿态解算与参考值相比产生的误差数据分析。

表2 2种方法姿态误差数据对比(°)

由图3、图4和表2的数据可以看出采用自适应扩展卡尔曼滤波算法得到的姿态角最大误差都在3°左右，要比扩展卡尔曼滤波算法的误差小很多，因此，采用自适应扩展卡尔曼滤波算法可以得到较好的结果，提高了姿态角的解算精度。

(a)

(b)图3 扩展卡尔曼滤波算法结果对比

(a)

(b)图4 自适应扩展卡尔曼滤波算法结果对比

6 结束语

本文将实验数据导入MATLAB中，并对结果进行分析。主要针对惯性传感器的系统误差，易受环境干扰和陀螺仪的漂移等因素引起姿态发散的现象，在预处理的基础上，采用对测量噪声的两级自适应处理来提高计算精度。通过实验和仿真，将该算法与扩展卡尔曼滤波进行对比，结果表明，数据的预处理效果较好，基本上消除了加速度计和陀螺仪的干扰和野值，通过自适应扩展卡尔曼滤波算法的姿态解算，使得姿态角的精度进一步提高，有效的解决了累计误差的问题，说明该方法是一种高精度的姿态解算方法。