无人机微机电系统惯性传感器卡尔曼滤波技术

孙立志

在消费级和工业级无人机中，微机电系统惯性传感器是其飞控系统十分重要的传感器。微机电系统信号噪声的存在降低了传感器的精度，对此本文提出了利用Allan方差估计分析噪声特性的方法，研究了基于卡尔曼滤波的MEMS信号降噪方法，解决了在无人机飞控系统中某些去噪方法计算复杂的问题，通过实验验证了该方法抑制微机电系统噪声的有效性。

微机电系统（Micro-ElectroMechanical System，MEMS）是指尺寸在毫米级别甚至更小的传感器系统。微机电系统传感器广泛配备于多种无人机系统，特别是消费级和工业级无人机，这得益于微机电系统传感器所具有的重量轻、功耗低、集成度高同时成本低等一系列优点。微机电系统传感器包括运动传感器、压力传感器、光传感器等，本文涉及的微机电系统惯性传感器是运动传感器的一种，集成了三轴陀螺仪和三轴加速度计，是该级别无人机飞控系统最重要的传感器之一，应用十分广泛。但微机电系统惯性传感器自身也有不可避免的缺点，即输出信号噪声成分多，若不进行滤波处理则无法使用，因此本文提出一种基于卡尔曼滤波的微机电系统惯性传感器信号降噪方法，使该类传感器应用于无人机时具有较高的精度。

微机电系统惯性传感器原理

微机电系统以哥氏效应为基本原理，是一种振动式的角速率传感器。哥氏效应是指在一个作旋转运动的坐标系中，当某一点相对该动坐标系作直线运动时，该动点即具有哥氏加速度，对此可得哥氏加速度的表达式：

a_c= 2×ω×v （1）

式中：ω——动坐标系的角速度;

v——质点相对动坐标系的线速度。

微机电系统工作时，通过电驱动，使微机械的振动框架达到谐振状态，而产生一定的速度，此时当微机电系统随平台框架旋转时，即产生科氏力;科氏力与检测框架发生关联，即形成检测信号，经放大输出得到电信号。据此微机电系统将无人机机体的旋转角速度信息转换为与之成比例的电压信号，通过模数转换得到角速度的数字信息，将角速度进行积分可得框架的角度信息。

本文采用MPU6050惯性传感器，其内部集成三轴陀螺仪和三轴加速度计，MPU6050的参考坐标系为右手坐标系，图中标注了3个测量轴的正向旋转方向，符合右手螺旋定则。当MPU6050围绕其测量轴旋转时，即可输出相应轴向的角速度信息。

微机电系统的性能指标主要有刻度因数、偏差、分辨率和随机漂移，其中分辨率是选择和使用微机电系统时重点关注的参数指标，它指的是微机电系统所能敏感到的最小角速度增量。

不考虑微机电系统的结构形式和制造工艺的不同，微机电系统的系统模型由物理模型、误差模型以及量测模型三部分组成。其中微机电系统信号的零点漂移和随机噪声可由误差模型和量测模型分析。误差模型中包括对确定性误差和随机噪声的分析，确定性误差是由模型中的参数变化和微机电系统所在的工作环境决定，随机噪声则是由于其他不确定的因素引起，典型的随机误差包括速度随机游走、白化速度噪声等;量测模型中包括对测量哚声的分析。

应用于无人机的微机电系统惯性传感器噪声特性

微机电系统的噪声可以分为确定性漂移和随机噪声两类。其中前者理论上能够完全补偿，但需要通过复杂的实验方法实现，本文不做深入研究;对于随机噪声，一般不能完全消除，而只能通过滤波的手段进行抑制。

应用于无人机时，微机电系统中的漂移和噪声主要来源于两个方面。一是微机电系统自身原理、结构和制造工艺带来的缺陷，即来源于微机电系统内部;二是外部因素，主要指微机电系统受到环境因素的影响而产生噪声。外部因素包括温湿度、无人机结构振动、空域内电磁辐射等。具体来说，当温度变化时，微机电系统的材料发生对温度敏感的变化，影响内部振动元件的状态;当外界的电磁环境较恶劣时，微机电系统内部电信号的转化、传输受到影响。

分析微机电系统的随机漂移，目前主要有Allan方差法功率谱密度法和1σ方法等方法，其中因Allan方法对随机噪声的各成分有较强的辨识度，更利于对各噪声成分进行分析和提取，故更常用于微机电系统的随机噪声分析。Allan方法用于分析随机噪声成分，其目的是对随机误差的各个误差项进行辨识并确定出各误差项的系数，对微机电系统的各项性能指标进行估计;本文的AR模型用于建立随机漂移的数学模型，将非平稳的随机序列处理后进行AR建模，进而设计卡尔曼滤波器。

随着制造工艺的进步、安装精度的提高，微机电系统噪声信号的抑制程度也随之提高。但是受限于微机电系统自身的结构原理和生产工艺水平，且考虑微机电系统的成本等因素，单从微机电系统本身考虑噪声抑制是远远不够的，因此基于软件滤波的方法，即数字滤波方法更要结合应用。基于软件滤波的方法中，先建立微机电系统噪声的数学模型，再根据模型的特点进行数字滤波器的设计不失为一种较好地思路。但是考虑到微机电系统噪声的随机性、非线性等因素，建立较为准确、精确的数学模型有困难。因此，对微机电系统输出信号进行不过分依赖其信号模型的直接数字滤波较为方便。

数字滤波器是随机数字信号处理技术中的常用方法，其具有灵活性强、可靠性高等优点，广泛应用于工程实际中。根据数字滤波的理论，数字信号的输入序列和输出序列之间满足线性组合的关系，即任意时刻的输出值可由该时刻的输入值和过去时刻的输入和输出值表示，其差分方程为

数字滤波器的类型可以分为非递归型和递归型两种，其类型根据信号输出序列的系数值分类。a_j（j≥1）的取值均为0时式（2）表示的是非递归型数字滤波器，其特点是当前时刻输出序列与过去时刻的输入值无关，只和当前时刻的输入值有关。反之则式（2）表示递归型数字滤波器。针对随机误差，目前研究较多的滤波算法有低通滤波、卡尔曼滤波、小波去哚、维纳滤波等。其中小波分析方法因其模型階数较高、算法复杂，并不适合在嵌入式处理器中应用。

微机电系统惯性传感器随机噪声的数学模型

微机电系统随机哚声的成分是非平稳的有色噪声，因其主要受工作环境的影响，所以没有确定的误差模型。时间序列法和相关函数估计是用于建立随机噪声的模型常用的方法，其中相关函数法容易引入额外误差，而时间序列法对信号的模型要求不高，常用于微机电系统信号的处理。

所谓时间序列法，是指利用统计的手段对随机过程的随机序列进行分析的方法，其目的是根据测得的随机数据，进行相应的分析，得出其数学模型及相关的参数。时间序列分析法的适用范围是带限的白噪声，因此需要对微机电系统具有非平稳特性的随机噪声进行预处理。所谓平稳过程，是一种在工程中常见的随机过程，如通信中常见的高斯白噪声、无人机受到空气湍流而产生的波动。平稳过程的特点是其统计特性不仅与现在的状态有关，还与过去时间的状态有关。预处理的方法一般是一阶差分法或二阶差分法。一般经过一阶差分处理后，微机电系统的随机噪声序列即成为具有平稳性、非周期性、正态性的随机差分序列，可以对其进行模型估计。

对于这样的平稳随机序列，可以利用的自回归模型（AR（p）模型）进行分析，这种模型具有建模简单的优点，且在工程应用中其模型的阶次不高，适合处理器运算。利用自回归模型对随机噪声建模后，测量某时刻的数据是该时刻之前p个时刻数据的线性拟合与噪声的叠加。因此本文建立AR（1）模型，即一阶的AR模型，其结构见式（3）。

x_k=φ₁x_k-1+a_k（3）

式中，x_k——随机序列;

φ₁——AR（1）模型系数;

aφ_k——噪声序列，且aφ₁～。

该AR模型即为系统的状态方程，其中的系数通过微机电系统实际的参数进行计算。

基于卡尔曼滤波的微机电系统惯性传感器信号降噪方法

在无人机控制系统中，由于控制器硬件的限制，以及受限于系统对实时性的要求，上述提及的各种数字滤波方法并不一定适合应用于工程实际中。在实际应用中，需要综合考虑滤波器滤波算法的复杂程度和滤波效果。卡尔曼滤波方法是一种模型清晰，算法简洁同时濾波效果较好，滤波输出标准偏差较小的滤波方法，适合在消费级和工业级无人机系统中应用。

作为工程应用中常用的滤波方法，卡尔曼滤波方法因适用于对随机过程的估计，而成为微机电系统随机噪声抑制方法中最常用的一种。但在设计卡尔曼滤波器的过程中，首先要求建立系统噪声的数学模型。为了解决卡尔曼滤波器对噪声模型依赖程度高的问题，需要对非平稳的随机噪声进行模型估计。

卡尔曼滤波是一种递归型数字滤波器，它适用于线性动态系统，在应用时，需要分析系统的状态方程和量测方程，设X_k为系统在k时刻的状态，Wsub>k为过程噪声，则系统的状态方程为

Xsub>k=φXsub>k-1+HWsub>k （4）

式中：φ——为状态转移矩阵;

H——为噪声驱动矩阵，表征噪声对系统状态的影响程度。

因随机哚声的模型建立为AR（1）模型，即一阶自回归模型，则状态方程（4）中的各矩阵降阶为各个数字，由式（2）和式（4）可得，状态转移矩阵φ=φsub>1=1，噪声驱动矩阵H=1，Xsub>k=xsub>k，Wsub>k=ω（k）（k=0，1，2，3…）。

设Vsub>k为量测噪声，Zsub>k是系统的测量值，即微机电系统输出信号值，则系统的量测方程为

Zsub>k=CXsub>k+ Vsub>k（k=0，1，2，3…） （5）

式中C——为量测矩阵。

因系统的测量值为状态变量和量测噪声的叠加，即

Zsub>k=Xsub>k+Vsub>k （6）

则量测矩阵为C=1。

对于该系统，设Xsub>k为状态变量Xsub>k的滤波估计，则卡尔曼滤波的递推过程如下，其中状态一步预测为

协方差阵一步预测为

式中Q——为系统噪声的方差，其值由实验方法确定。

滤波增益为

式中R——为量测噪声的方差，其值由实验方法确定。

协方差阵估计为

状态估计为

系统输出为

可以看出，卡尔曼滤波由增益计算和滤波计算两部分组成，式（7）～（12）即为离散卡尔曼滤波器的计算过程，其中式（7）、（11）为滤波计算部分，由此可得卡尔曼滤波器滤波计算部分具体结构，式（8）、（9）、（10）为增益计算部分;其纵向可分为时间更新部分和测量更新部分，时间更新部分为式（7）、（8），测量更新部分为式（9）、（10）、（11）。其中滤波增益Ksub>k是影响滤波器性能的关键参数，系统哚声方差Q和量测噪声方差R对滤波器的影响即通过Ksub>k体现，正因为此，当系统噪声方差和量测噪声方差不易通过实验确定时，可直接对滤波增益进行估计，其范围为0k<1。

当给出初始状态时，即X和Psub>0确定时，即可根据时刻k的量测值Zsub>k递推得到时刻k的状态值Xsub>k。

卡尔曼滤波方法实验

针对MPU6050，将上述卡尔曼滤波方法应用于其输出信号的去哚。在验证滤波算法去哚效果的实验中，为了便于观察和对比实验波形，需要将微机电系统置于相对静止的实验环境中，因此利用光学平台来隔离地基的振动。实验平台选用常用于无人机飞控系统的嵌入式处理器对微机电系统数据进行滤波处理，以观察该滤波算法在嵌入式控制系统中的处理效率。

微机电系统处于静态时，将其输出作为卡尔曼滤波器的输入，验证其滤波效果。上图是微机电系统静态时原始输出，其噪声信号达到0.040°/s，经卡尔曼滤波器处理，可得下图中输出信号，对比可以看出微机电系统静态输出信号波动范围在±0.025°/s内，对随机噪声信号起到一定的抑制作用。同时对利用上位机接收的数据处理结果进行分析，证明了该滤波算法适于在本课题的嵌入式控制系统中应用，计算效率较高。

总结

本文分析了微机电系统惯性传感器输出信号的噪声特性，利用Allan方差估计方法对噪声建立了数学模型。在此基础上引入了基于卡尔曼滤波的陀螺去噪方法，设计了适用于无人机飞控系统的卡尔曼滤波器，对微机电系统的输出信号进行了滤波处理。通过实验验证，该方法应用于无人机的控制器中时获得了良好的滤波效果，为无人机的高精度飞行奠定了基础。