CN110702113B - 基于mems传感器的捷联惯导***数据预处理和姿态解算的方法 - Google Patents

Abstract

本发明公开了一种基于MEMS传感器的捷联惯导***数据预处理和姿态解算的方法，为得到准确的姿态信息，将惯性器件测得数据分别两个阶段进行处理，第一阶段，考虑MEMS传感器的漂移和随机误差等影响，对测量数据进行预处理，建立漂移的ARMA模型，接着使用卡尔曼滤波和滑动均值滤波进行相关计算；第二阶段，先利用姿态融合算法求出初始姿态角，再建立姿态角状态方程和观测方程，利用无迹卡尔曼滤波的高精度算法进行姿态整合。该方法经预处理，能明显抑制噪声、漂移误差和消除野点等，使用UKF算法进一步提高姿态解算精度。

Description

基于MEMS传感器的捷联惯导***数据预处理和姿态解算的 方法

技术领域

本发明涉及传感器和惯性导航领域，更具体地，涉及一种基于MEMS传感器的捷联惯导***数据预处理和姿态解算的方法。

背景技术

近年来，随着MEMS技术的飞快发展，MEMS传感器以其成本低、尺寸小、功耗低等优势逐渐被人们用到IMU领域，如在无人机、航姿参考***、水下机器人、单兵导航等领域都有较为广泛的应用，因此研究MEMS传感器的捷联惯导***的姿态解算具有重要意义。

捷联惯性导航***(SINS)是一种不依赖外部信息，也不向外辐射信息的自主式导航***，理论上惯导***根据惯性传感器测得信息可以求出姿态角，但是MEMS惯性器件测得值易受到误差的干扰，造成姿态解算精度下降，这些误差主要分为***误差和随机误差，如陀螺仪的常值漂移，加速度计的零偏，磁力计易受到周围环境的干扰等等，长时间工作这些器件会有较大的累积误差。为获得准确的姿态角信息，必须滤除惯性传感器的各种误差。

针对惯性器件误差问题，常用的解决方法是建立IMU的误差模型，使用卡尔曼滤波等算法求出误差模型参数，以达到误差补偿目的，但建立准确的误差模型和滤波时选对参数，存在计算量大的问题。为提高姿态解算的精度，可以使用扩展卡尔曼滤波(EKF)来进行姿态解算，建立姿态角的状态方程和观测方程，选对参数可以滤除环境干扰和过程噪声，但是推导的姿态角的误差方程比较繁琐，而且当状态方程或观测方程是非线性时，使用EKF需要计算雅可比矩阵，计算量加大，不利于实际使用。

发明内容

针对上述现有技术存在的缺陷或不足，本发明专利目的在于提供一种基于MEMS传感器的捷联惯导***数据预处理和姿态解算的方法。

为了解决上述所描述的问题，本发明采用一下的技术解决方案：

基于MEMS传感器的捷联惯导***数据预处理和姿态解算的方法，为得到准确的姿态信息，将惯性器件测得数据分别两个阶段进行处理，第一阶段，考虑MEMS传感器的漂移和随机误差等影响，对测量数据进行预处理，建立漂移的ARMA模型，接着使用卡尔曼滤波(KF)和滑动均值滤波进行相关计算；第二阶段，先利用姿态融合算法求出初始姿态角，再建立姿态角状态方程和观测方程，利用无迹卡尔曼滤波(UKF)的高精度算法进行姿态整合。

优选的，具体按以下步骤进行：

步骤1：陀螺仪测得数据的预处理，首先去除偏移量和进行均值滤波，把传感器固定好得到一组输出数据，求这组数据的算术平均值,即偏移量，接着建立陀螺漂移的ARMA(n,m)***模型，根据该模型建立状态方程和观测方程，使用卡尔曼滤波实现；

步骤2：使用卡尔曼滤波器(KF)对加速度数据进行预处理；

步骤3：磁力计测得数据的预处理，建立一个数据缓冲区，依次存放八个采样数据，每采样一个新数据，就将最早采集的数据丢掉，而后求包括新数据在内的八个数据的算术平均值；

步骤4：使用预处理后的惯性器件测得数据，利用姿态融合算法求出初始姿态角，公式如下：

φ＝arctan(ay/az),[-π,π]

H_Y＝my*cos(θ)+mx*sin(θ)*sin(φ)-mz*cos(φ)*sin(θ)

H_X＝mx*cos(φ)+mz*sin(φ)

ψ＝arctan(H_Y/H_X),[-π,π]

其中，ф、θ和ψ分别是滚动角、俯仰角和航向角，(ax，ay，az)、(mx，my，mz)分别是加速度计和磁力计测得值；

步骤5：建立姿态角的状态方程和观测方程，选择三个姿态角当作是状态空间模型的观测量X_k＝[φ_kθ_kψ_k]，建立滤波状态方程为：

式中，[w_θ(k-1) w_φ(k-1) w_ψ(k-1)]^T为***噪声向量，而观测方程为：

式中，

v_k＝[v_θk v_φk v_ψk]^T是观测噪声。

本发明能够实现姿态解算，而且能够明显抑制漂移误差、随机噪声和消除野点对姿态解算的影响。

附图说明

图1是本发明一个实施例的流程图；

图2是本发明一个实施例的MEMS陀螺仪经去除偏移量和滤波后的静止状态下数据分布高斯图；

图3是本发明一个实施例的MEMS陀螺仪实测值和经预处理后的静态角速度对曲线的比图；

图4是本发明一个实施例的MEMS加速度计实测值和经预处理后的静态加速度曲线的对比图；

图5是本发明一个实施例的MEMS磁力计实测值和经预处理后的静态磁感应强度曲线的对比图；

图6是本发明一个实施例的静态试验下姿态融合算法和采用无迹卡尔曼滤波的姿态解算算法得到的滚动角和俯仰角误差曲线的对比图。

具体实施方式

下面将详细描述本发明的实施例，说明下，本实施例是事例性的，仅仅是用来解释本发明的，而不能理解为对本发明的限制。下面将参照说明书附图对本发明的一种基于MEMS传感器的捷联惯导***数据预处理和姿态解算的方法进行以下详细的说明：

如图1所示，可以看出整个流程主要分为两个阶段：第一阶段是对采集到的MEMS惯性器件数据的预处理；第二阶段是使用第一阶段预处理后的数据，分别采用姿态融合算法和UKF算法进行姿态解算。

第一阶段是对惯性器件的数据进行预处理，具体按以下步骤实施：

1、对陀螺仪的数据进行处理，首先去除偏移量，方法是使用飞控减震架把惯性传感器固定好，得到一组角速度数据，求这组数据的算术平均值,即偏移量，之后对陀螺仪数据进行滤波，得到数据见图2，经过统计分析和预处理后的陀螺样本序列可以利用时间序列分析法的ARMA模型建立陀螺仪的随机漂移误差模型；

(1)建立陀螺漂移的ARMA模型，从图2可以看出，经过去除偏移量后，此时陀螺仪数据符合平稳、服从正态分布的、零均值的时间序列，假设为：{x_k}，k＝1,2,...n，则它的ARMA(n,m)模型有如下形式：

式中x_k为时间序列；

为模型系数(自回归系数)；θ_k为模型系数(滑动平均系数)；a_k为残差(白噪声)；n，m为ARMA(n,m)模型阶次数；

可以简写为：

x_k＝A(B)X_t+C(B)a_t

式中：

C(B)＝1+θ₁B+···+θ_mB^m

通常陀螺漂移误差模型阶数比较低，一般不会大于三阶，取n＝2,m＝1。

(2)使用卡尔曼滤波(KF)，根据陀螺随机漂移ARMA模型以及卡尔曼滤波的理论，可设定***状态方程为：

X_k＝AX_k-1+BV_k

其中，

B＝[1 θ₁]，V_k＝[a_k a_k-1]^T。

假设W_k为测量噪声序列，那么***的输出方程为：

Z_k＝CX_k+W_k

卡尔曼滤波分为预测和更新两步骤，如下：

根据卡尔曼滤波算法流程，进行状态预测：

式中，

为先验状态估计值，即利用k-1时刻的***最优估计值来估算k时刻***的状态变量。

为先验估计误差的协方差，Q为***的过程噪声矩阵。

根据卡尔曼滤波算法流程，进行状态更新：

其中，K为卡尔曼增益，

为后验状态估计值，即***的状态变量在k时刻的最优估计值。P_k为后验估计误差的协方差，R为***的测量噪声矩阵。

2、对加速度进行预处理，考虑在测量加速度数据时，肯定是会受到重力加速度的影响，假设在求载体的姿态角时忽略重力加速度的影响，所以使用卡尔曼滤波器(KF)直接对加速度数据进行预处理。

3、对磁力计测得值的预处理的方法是滑动均值滤波，因为磁力计极易受环境的影响，若采用KF算法，所处环境不同，初值选取是一个问题。具体方法是建立一个数据缓冲区，依次存放八个采样数据，每采样一个新数据，就将最早采集的数据丢掉，而后求包括新数据在内的八个数据的算术平均值。

为了验证所使用的预处理方法的有效性，本发明进行了静态实验。实验采用了STM32系列的单片机，使用基于MPU6050和MAG3110微传感器的捷联惯导***，固定于一块开发板上，电脑和SINS之间的通信使用RS232串口线，在室外远离磁干扰的地方进行下面实验。为验证预处理的效果，把惯导***固定好，滤波器每秒滤波5次，滤波时间为300s。实验结果如图3、图4、图5为预处理前后的角速度、加速度和磁感应强度的数据，可以看出预处理效果明显，野点等误差基本被消除。

第二阶段是利用第一阶段预处理后的数据，进行姿态解算，具体实施如下：

4、姿态融合的计算方法是首先通过加速度计测得信号(ax，ay，az)求取滚动角φ和俯仰角θ：

φ＝arctan(ay/az),[-π,π]

在实测中，惯导***是不可能完全水平放的，肯定是会有一点倾斜的，即此时存在滚动角和俯仰角，所以磁力计测得的磁场强度(mx，my，mz)就不是真正的水平轴上的分量，须根据mx，my，mz，ф和θ求出投影在XOY面上的两个量了，即通过滚动角、俯仰角和磁力计测得值修正磁力计的位置，根据修正后的信息算出航向角ψ，具体计算公式如下：

H_Y＝my*cos(θ)+mx*sin(θ)*sin(φ)-mz*cos(φ)*sin(θ)

H_X＝mx*cos(φ)+mz*sin(φ)

ψ＝arctan(H_Y/H_X),[-π,π]

5、基于无迹卡尔曼滤波的姿态解算，UKF假设状态变量满足高斯随机分布，UKF是基于2n+1个不同权值的采样点经非线性***后的结果计算***的状态均值和协方差。理论证明：后验状态均值和协方差是可达到泰勒展开式的三阶精度，UKF和EKF计算量相当，但是EKF在工作时要计算雅可比矩阵，故UKF较为简单。

为实现姿态角的解算，所以选择三个姿态角当作是状态空间模型的观测量X_k＝[φ_k θ_k ψ_k]，建立滤波状态方程为：

式中，[w_θ(k-1) w_φ(k-1) w_ψ(k-1)]^T为***的过程激励噪声向量。

通过姿态融合算法解算出来的姿态角为Z_k＝[θ_k φ_k ψ_k]，可以反映出姿态角的变化可是实时性较低，故当作观测量来更新***的状态，则观测方程为：

式中，

v_k＝[v_θk v_φk v_ψk]^T是观测噪声向量。

上式中的状态方程是非线性的，而观测方程是线性的，则***的UKF滤波器设计如下：

1)初始化：将过程激励噪声和观测噪声增广为状态变量，则有：

2)计算采样点和对应权值：

式中，n表示状态向量的维数，w_i表示权值，λ＝a²(n+l)-n为一个比例参数，常选取l＝0，a是控制采样点的分布，决定采样点与均值的离散程度。常取一个很小的正数，例如1e-3，避免状态方程非线性严重时，采样点的非局域性受到影响，适当调节a和l可以提高估计均值的精度。

3)时间更新

4)测量更新：

为验证本发明的有效性，进行静态实验。使用飞控减震架把惯导***固定好，通过采集静止惯导的数据，将本发明的方法与传统的姿态融合算法进行比较，比较结果见图6。从图中可以看出，当惯导***处于静止状态下时，姿态角解算精度高：对于传统的姿态融合算法，滚动角和俯仰角精度都在0.1°左右，由于预处理的效果好，此时姿态角精度已经很高；而采用基于UKF的姿态解算的方法，姿态解算精度进一步提高，相比于姿态融合算法，参考国军标GJB 729-1989计算得到解算精度分别提高了31％、10％。本发明能够实现姿态解算，而且能够明显抑制漂移误差、随机噪声和消除野点对姿态解算的影响。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (2)

1.基于MEMS传感器的捷联惯导***数据预处理和姿态解算的方法，其特征在于，将惯性器件测得数据分别两个阶段进行处理，第一阶段，考虑MEMS传感器的漂移和随机误差的影响，对测量数据进行预处理，建立漂移的ARMA模型，接着使用卡尔曼滤波和滑动均值滤波进行相关计算；第二阶段，先利用姿态融合算法求出初始姿态角，再建立姿态角状态方程和观测方程，利用无迹卡尔曼滤波的高精度算法进行姿态整合；

具体按以下步骤进行：

步骤1：陀螺仪测得数据的预处理，首先去除偏移量和进行均值滤波，把传感器固定好得到一组输出数据，求这组数据的算术平均值,即偏移量，接着建立陀螺漂移的ARMA(n,m)***模型，根据该模型建立状态方程和观测方程，使用卡尔曼滤波实现；

步骤2：使用卡尔曼滤波器对加速度数据进行预处理；

步骤3：磁力计测得数据的预处理，建立一个数据缓冲区，依次存放八个采样数据，每采样一个新数据，就将最早采集的数据丢掉，而后求包括新数据在内的八个数据的算术平均值；

步骤4：使用预处理后的惯性器件测得数据，利用姿态融合算法求出初始姿态角；

步骤5：建立姿态角的状态方程和观测方程，选择三个姿态角当作是状态空间模型的观测量，使用无迹卡尔曼滤波的高精度算法进行姿态整合。

2.根据权利要求1所述基于MEMS传感器的捷联惯导***数据预处理和姿态解算的方法，其特征在于，所述步骤5中建立姿态角的状态方程和观测方程，使用无迹卡尔曼滤波的高精度算法进行姿态整合，其步骤如下：

建立滤波状态方程为：

式中，[w_θ(k-1) w_φ(k-1) w_ψ(k-1)]^T为***噪声向量，而观测方程为：

式中，H_k＝I，v_k＝[v_θk v_φk v_ψk]^T是观测噪声。

Images