智能手机中的MEMS传感器可用于室内定位等功能。在利用手机进行联合定位的时候，陀螺仪会起到很大的作用，而加速度计只起到辅助的作用。陀螺仪的误差关乎整个定位系统的精度^[1]。但是智能手机中的惯性传感器成本低，质量普遍较差，所输出的数据往往夹杂着大量噪声。如何在不增加硬件成本的同时有效降低低成本MEMS陀螺仪的噪声具有很大的工程意义。MEMS陀螺仪的噪声主要有确定噪声和漂移噪声两类。确定噪声可以通过特定补偿进行消除，本研究不作深入探讨。国内外对陀螺仪的漂移误差进行了大量研究，主要是先建立陀螺仪的误差漂移模型，然后使用滤波进行降噪处理；这里的误差漂移模型建模方法主要有小波分析、时间序列分析和神经网络等^[2-5]。滤波技术主要有非线性滤波、Kalman滤波和这些滤波方法的组合改进等^[6-11]。在本研究中，单独利用加速度计输出的原始数据可以直接计算出俯仰角和横滚角，但会受到响应时间的制约，在运动时相应的数据传输会滞后从而形成误差。尽管陀螺仪的动态性能好，由陀螺仪的原始数据经过积分可以得到载体转过的角度，但积分运算会使得原始的误差积累起来，最后误差越积越多，无限发散。采用卡尔曼滤波方法融合加速度计和陀螺仪的测量信息，可以根据它们各自的优缺点提高姿态角的测量精度，为手机联合定位提供准确的基本信息。

1 基于卡尔曼滤波器的信息融合

本文的核心方法是使用卡尔曼滤波器对解算出的姿态角进行滤波降噪处理。原始数据是加速度计测出的加速度和陀螺仪测出的角速度。将两种信息经过滤波器进行解算、融合，得到更加准确的姿态角信息。通过加速度计可以测得配备该传感器的物体的运动方向。一般情况下加速度计是用于计算位移量的，但是也可以用于修正角度^[12]，这也是本文研究的内容。

1.1 姿态角解算

图 1显示了手机中陀螺仪的工作原理，使用其获取的角速度值直接积分就可获得角度值，但是噪声非常显著，获得的结果精度较差，因此需要利用加速度计的数据对其进行修正。

根据经典力学公式，如果载体同时发生空间平移姿态变化，单纯根据3个轴的加速度不能求解姿态角变化量。但是一般运动控制系统中平移加速控制和姿态角控制是分离的，因此在一定区间内，可以假定载体匀速运动的，这时就能利用加速度计的数据对角加速度做线性估计^[13]。需要说明的是这种估计不是十分精确，但是可以满足一般应用的需求，具体分析和计算如下：

将图 1中的俯仰角定义为θ，横滚角定义为γ。加速度计获取的3轴数据用f_x, f_y, f_z表示。假定载体在当前运动区间内做匀速运动，那么物体姿态发生变化时，加速度计数值的变化就可以认为是由角度变化引起的^[14]，此时可以按照下式近似计算出俯仰角和横滚角：

$ \left\{ {\begin{array}{*{20}{l}} {\gamma = a\sin \left( {{f_z}/ \text{sqrt} \left( {f_y^2 + f_z^2} \right)} \right)}\\ {\theta = - a\sin \left( {{f_x}/ \text{sqrt} \left( {f_x^2 + f_y^2 + f_z^2} \right)} \right)} \end{array}} \right. $

(1)

1.2 低成本陀螺仪零漂误差分析

在进行手机联合定位的时候，陀螺仪会起到很大的作用，而加速度计只起到辅助作用。陀螺仪误差关乎整个定位系统的精度。陀螺仪的误差主要是静态误差，这个静态误差一般被称为零点漂移。

陀螺仪有两个工作状态，分别是开环工作状态和闭环工作状态^[15]。陀螺仪处于开环工作状态时信噪比小，漂移噪声较为严重，此时陀螺仪的漂移误差会随着时间逐渐积累，最终造成严重的零点漂移。陀螺仪处于闭环工作状态中时信噪比虽然会增大，但是噪声的漂移现象仍然会在一段时间后出现。在干扰力矩作用下自转轴相对惯性空间的偏差角变化速度被称作陀螺漂移率(即测量角的偏差在单位时间内的变化率)。它是衡量陀螺仪精度的主要性能指标，漂移率越小，陀螺仪精度越高。

1.3 卡尔曼滤波原理

卡尔曼滤波方法最大优势就是在测量方差已知的情况下能够从一系列存在测量噪声的数据中估计动态系统的状态，卡尔曼滤波器的实现需要使用到两组数据，这两组数据命名为系统的状态量和系统的观测量。使用加速度计测量的姿态角数值作为观测值，以陀螺仪的零偏作为状态值.并建立状态方程和量测方程^[16]：

$ {\mathit{\boldsymbol{X}}_k} = \mathit{\boldsymbol{A}}{\mathit{\boldsymbol{X}}_{k - 1}} + \mathit{\boldsymbol{B}}{\mathit{\boldsymbol{U}}_k} + \mathit{\boldsymbol{W}} $

$ {\mathit{\boldsymbol{Z}}_k} = \mathit{\boldsymbol{H}}{\mathit{\boldsymbol{X}}_k} + \mathit{\boldsymbol{V}} $

当系统初始状态以及对应的协方差矩阵已知后相应的Kalman滤波系统可以迭代运行，对连续过程中系统的任一时刻系统的状态进行最优估计，达到降噪的效果。

1.4 低成本陀螺仪降噪建模

陀螺仪输出的角速度存在固定偏差^[17]，设观测角度值φ对应的偏差为b，那么该系统根据角度和角速度可以获得如下方程：

$ \left[ {\begin{array}{*{20}{c}} {\dot \varphi }\\ {\dot b} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} 0&{ - 1}\\ 0&0 \end{array}} \right]\left[ {\begin{array}{*{20}{l}} \varphi \\ b \end{array}} \right] + \left[ {\begin{array}{*{20}{l}} 1\\ 0 \end{array}} \right]\omega + \left[ {\begin{array}{*{20}{l}} n\\ 0 \end{array}} \right] $

(2)

式中:ω为由陀螺仪获取的角速度值；n为其对应噪声误差。

设系统采样周期为T，式(2)可重写为

$ \begin{array}{*{20}{c}} {\left[ {\begin{array}{*{20}{c}} {\varphi (k) - \varphi (k - 1)}\\ {b(k) - b(k - 1)} \end{array}} \right] = }\\ {T\left( {\left[ {\begin{array}{*{20}{c}} 0&{ - 1}\\ 0&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\varphi (k - 1)}\\ {b(k - 1)} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} 1\\ 0 \end{array}} \right]\omega + \left[ {\begin{array}{*{20}{c}} n\\ 0 \end{array}} \right]} \right)} \end{array} $

设状态向量:

$ \mathit{\boldsymbol{X}}(k) = {[\varphi (k)b(k)]^{\rm{T}}} $

则可得系统的状态方程为：

$ \mathit{\boldsymbol{X}}(k) = \left[ {\begin{array}{*{20}{c}} 1&{ - T}\\ 0&1 \end{array}} \right]\mathit{\boldsymbol{X}}(k - 1) + \left[ {\begin{array}{*{20}{l}} T\\ 0 \end{array}} \right]\omega (k - 1) + \left[ {\begin{array}{*{20}{l}} T\\ 0 \end{array}} \right]n $

系统的观测方程基于加速度计建立，根据前述计算方法由式(1)可以直接利用3个加速度值计算出角度值，因此观测方程为：

$ \mathit{\boldsymbol{Z}}(k) = \left[ {\begin{array}{*{20}{l}} 1&0 \end{array}} \right]\mathit{\boldsymbol{X}}(k) + \mathit{\boldsymbol{m}} $

式中m为加速度计噪声误差。参照卡尔曼滤波的实现原理，只需再获取2个噪声的协方差阵Q和R即可进行滤波。在本文中，Q和R分别为：

$ \mathit{\boldsymbol{Q}} = \left[ {\begin{array}{*{20}{l}} {{q_a}}&0\\ 0&{{q_g}} \end{array}} \right],\mathit{\boldsymbol{R}} = \left[ {{r_a}} \right] $

式中Q和R矩阵中的参数通过元器件的给定参数确定^[18]。

以上是针对单个角度计算，当需要同时计算俯仰角θ和横滚角γ时，对各向量进行扩展，系统方程涉及的矩阵A、B、Q分别为：

$ \begin{array}{*{20}{c}} {\mathit{\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}} 1&{ - T}&0&0\\ 0&1&0&0\\ 0&0&1&{ - T}\\ 0&0&0&1 \end{array}} \right],\mathit{\boldsymbol{B}} = \left[ {\begin{array}{*{20}{l}} T&0\\ 0&0\\ 0&T\\ 0&0 \end{array}} \right],}\\ {\mathit{\boldsymbol{Q}} = \left[ {\begin{array}{*{20}{c}} {{q_{a\theta }}}&0&0&0\\ 0&{{q_{g\theta }}}&0&0\\ 0&0&{{q_{a\gamma }}}&0\\ 0&0&0&{{q_{g\gamma }}} \end{array}} \right]} \end{array} $

状态向量X和控制向量U分别为

$ \mathit{\boldsymbol{X}}(k) = \left[ {\begin{array}{*{20}{l}} {\theta (k)}\\ {{b_\theta }(k)}\\ {\gamma (k)}\\ {{b_\gamma }(k)} \end{array}} \right],\mathit{\boldsymbol{U}}(k) = \left[ {\begin{array}{*{20}{c}} {{\omega _{g\theta }}(k - 1)}\\ {{\omega _{g\gamma }}(k - 1)} \end{array}} \right] $

观测方程设计的矩阵H和R以及观测值向量Z分别为：

$ \mathit{\boldsymbol{H}} = \left[ {\begin{array}{*{20}{c}} 1&0&0&0\\ 0&0&1&0 \end{array}} \right],\mathit{\boldsymbol{R}} = \left[ {\begin{array}{*{20}{c}} {{r_{a\theta }}}&0\\ 0&{{r_{a\gamma }}} \end{array}} \right],\mathit{\boldsymbol{Z}}(k) = \left[ {\begin{array}{*{20}{c}} {{\theta _a}}\\ {{\gamma _a}} \end{array}} \right] $

2 滤波模型的程序实现

根据上述模型编写了滤波程序文件。滤波程序分为：1)读取实验获取的数据；2)由加速度算出姿态角，并作为观测值；3)进行卡尔曼滤波；4)得到状态预估值4个步骤。

图 2为卡尔曼滤波的子程序流程。

3 滤波模型在智能手机上的实现 3.1 数据收集

MATLAB可以从Android设备中获取传感器数据。需要在安卓系统上安装能够测量传感器数据的应用程序sensorUDP，其界面如图 3所示，同时要求运行MATLAB的计算机与安装sensorUDP应用程序的Android设备处于同一个WiFi环境下。SensorUDP是一款由Takashi SASAKI开发的APP, 允许用户获得手机传感器的各种原始数据如加速度、角速度、线速度和经纬度等。

sensorUDP需要和sensorgroup组件配合使用，该组件可以从MATLAB的官网下载，将其解压至安装目录下的toolbox文件夹中。sensorgroup.m文件提供了一组函数可供调用，用户可以利用这些函数读取相应的传感器数据。

在使用时，首先在计算机上设置无线网络，在手机端连接创建好的无线网络。保证两个设备在同一个网络环境下，MATLAB就能接收到手机发出的UDP数据包，并通过函数命令解析数据包。函数命令可以由mathwork官网获得并保存在MATLAB工作路径中。函数命令obj=sensorgroup(deviceType)会提示用户进行相关设置从而建立通信, 如图 4所示。需要在dest.host对应编辑框中输入要接收数据的电脑的IP地址(10.3.136.90)，端口值一般设置为默认值50000。

3.2 静止条件下手机陀螺仪误差分析

将手机平放在地面上，保持静止状态，收集陀螺仪和加速度计的原始数据(陀螺仪原始数据为载体的三轴角速度，加速度计的原始数据为载体的三轴加速度)。

由图 1所示，手机的X轴和Y轴具有相似性，所以只需要单独分析其中一个轴的静态误差即可。以陀螺仪Y轴为例进行误差分析。

先将静止状态下的数据进行滤波仿真，图 6和图 7中虚线为滤波前结果，实线为滤波后结果。

由Y轴静止状态下的滤波结果可得表 1。

在单独使用陀螺仪原始数据解算姿态角时，100 s后由积分运算造成的漂移误差达到大约28.3°(动态漂移率为0.28°/s)，随着时间推移误差积累，数据逐渐发散，最后所得的结果不能正常使用。可以看出陀螺仪数据的漂移问题严重。由图 6和表 1可以看出滤波很好地抑制了陀螺仪静止时的零点漂移噪声，且解算出的姿态角平稳，可供一般应用直接使用。

3.3 动态条件下手机陀螺仪滤波降噪

将手机按照依次绕X轴Y轴旋转90°的顺序进行数据收集。将数据保存为mat文件，和编写的滤波程序放置到同一目录下，然后运行滤波程序，所得结果如图 7所示。分析陀螺仪运动状态下的相关数据可得表 2(其中误差计算方式以旋转到90°时的数据为标准)。

分析滤波前的姿态角信息可知在动态条件下：俯仰角在200 s的漂移误差约为28.6°(动态漂移率约为0.143°/s)，横滚角在200 s内的漂移误差大约为34.4°(动态漂移率约为0.17°/s)。

对比分析滤波前后数据可明显发现，滤波前的角度存在显著误差，已经无法使用。而经滤波后，姿态角数据平稳，误差大小控制在4%左右，误差在可接受范围内，综合降噪效果在70%左右，滤波结果可以在一般的手机应用中直接使用。

4 结论

利用手机陀螺仪输出的角速度数据对其在静止状态下的零漂做出了定量分析，实验表明零漂造成的误差严重影响了姿态角的测量精度。当载体处于运动状态时，将误差较大的陀螺仪原始数据和由加速度计解算出的动态响应较差的姿态角数据进行信息融合、滤波降噪处理。实验证明当载体处于运动状态时，采用卡尔曼滤波能有效降低陀螺仪的漂移误差、提高姿态角测量的精度。此方法达到了预期的降噪目标，并且可以将经过滤波处理后的姿态角信息用于手机的一些简单应用如定位和平衡类小游戏等。但本文只涉及了平面方向的六轴自由度，在未来的工作中可以将航向角信息和磁力计信息加以利用，实现在空间九轴上的姿态角测量。