通过互补滤波器来实现的传感器融合技术

王腾 牛桠枫

摘要：该文在分析了Android设备相关传感器后，对其工作中会出现的问题提出了新的方案，并通过图表说明了其工作方式，提出了通过互补滤波器来实现传感器融合，只要对加速感应器和磁场感应器作类似低通滤波的处理，而对陀螺仪作类似高通滤波的处理，然后整合它们的结果数据，就可以得到相对精确的结果，且通过软件的形式来实现这一技术。

关键词：互补滤波器；Android传感器；传感器融合

中图分类号：TP391 文献标识码：A 文章编号：1009-3044（2014）25-5950-05

Sensor Fusion Technology to Achieve by Complementary Filter

WANG Teng1，NIU Ya-feng2

（1.Yangtze University College of Arts and Sciences， Jingzhou 434020， China；2.Tourism and Environment College of Shaanxi Normal University， xi 'an 710119，China）

Abstract： Based on the analysis of Android equipment related sensor， a new scheme to appear the problem in the work presented， and explained its ways of working through the chart， the complementary filter to realize sensor fusion technology， as long as similar low pass filter on the acceleration sensor and magnetic sensor， and similar high pass filtering of the gyroscope， and then integrate them data that can get relatively accurate results， but also by the form of software to realize this one technology.

Key words： complementary filter； Android sensor； sensor fusion

随着科技的发展，智能设备的应用越来越普及，越来越多的人开始使用智能手机，智能手机为我们的学习和生活带来了太多的方便，也早就渗入人们的衣食住行，人们享受着它给我们带来的快捷和方便，可是为什么智能手机如此强大呢？这跟里面的各种传感器密切相关。当今市场上，智能手机中，谷歌的android系统占据了很大一部分的市场，大多数的android设备都有内置的测量运动、方向、和各种环境条件的传感器。这些传感器具有提供高精度和准确度的原始数据的能力，可用于监视设备在三维方向的移动和位置、或者监视设备周围环境的变化。例如，一个游戏可能要从重力传感器中读取轨迹，以便推断出复杂的用户手势和意图，如倾斜、振动、旋转或摆动等；同样，有关天气的应用程序可能要使用设备的温度传感器和湿度传感器来计算并报告露点；有关旅行的应用程序可能要使用地磁场传感器和加速度传感器来报告罗盘方位。

1 安卓设备中陀螺仪的分析

android获取方向是通过磁场感应器和加速度感应器共同获得的，至于具体的算法SDK已经封装好了。也就是说现在获取用户方向有两种方式，一是官方推荐的，通过SensorManager.getOrientation（）来获取，这个方法表面看似容易，但实际上需要用到两个感应器共同完成工作，特点是更加的准确。第二种方法非常简单，直接得到三个轴上的数据。

获取安卓设备翻转动态的普通方法都是调用SensorManager.getOrientation（） 函数，这个函数可以通过所获取设备的3个平面角度来得到安卓设备的翻转动态，其中2个角度数据是基于加速感应器和磁场感应器来获取的。简而言之，就是加速感应器可以得到重力矢量数据（这个矢量是指向地心的方向），与此同时磁场感应器作为指南针来做辅助工作。这两个传感器提供的信息数据就完全可以计算出设备的平面数据了。但是这两个传感器的数据时常都不够准确，特别是磁场感应器很容易受到干扰。

很多安卓设备中都有陀螺仪这个模块，它的测量精度较高并且设备模块响应时间也很短。陀螺仪的底部被托住，但是能够各个方向旋转。它能够提供XYZ三个轴方向的角位移速度值。这3个值通过整合得到当前设备的真实方向，其计算方法是角速度乘以设备从开始移动到某一相对状态的时间间隔，它的结果就得到一个旋转增量。所有产生的旋转增量之和就是设备的绝对方向。以上的处理过程看起来很好，但是陀螺仪的数据处理是一个循环往复的过程，在每一次的循环过程中都会产生细微的数据偏差，虽然这些偏差很细微，但是当它们积累以后的结果，往往会导致陀螺仪的数据计算出错，这种错误也就是我们常说的陀螺漂移。

为了避免以上两种方法各自的弊端，即陀螺漂移和磁场干扰。就需要寻找新的方法来进行计算测量，通过总结这三个模块的特点，可知陀螺仪适合在方向角度迅速发生改变时做测量计算，但是在一个较长的使用周期内，其数据会有错误；而加速感应器和磁场感应器适合周期更长的测量。所以，只要对加速感应器和磁场感应器作类似低通滤波的处理，而对陀螺仪作类似高通滤波的处理，然后整合它们的结果数据，就可以得到相对精确的结果。3个传感器融合并滤波的过程如图1所示。

那么，传感器的高通和低通滤波数据到底是什么意思呢？传感器定时（这个时间间隔可以人为设置）传送数据。这些数据值可以在一个以时间为单位的X轴的二维图中显示出来，这个图的界面和声波信号类似。加速感应器和重力感应器干扰信号的低通滤波是分布在一个连续时间窗口全方位角度之中的。

在下面的代码中，通过引用从加速感应器和重力感应器到对地定向的值来实现上面所述的情况。

// 低通滤波，需要加一个方向变量factor的权值

accMagOrientation = （ 1 - factor ） * accMagOrientation+ factor * newAccMagValue；

陀螺仪数据的高通滤波数据是使用相应的陀螺仪方位数据替换accMagOrientation中已经过滤过的高频率部分。

fusedOrientation =

（1 - factor） * newGyroValue // high-frequency component+ factor * newAccMagValue；

事实上，这个结果就已经可以作为混合滤波器了。

假设安卓设备在某方向上翻转90度然后迅速复位，这其中的信号通过滤波的过程如图2所示。注意在陀螺仪的陀螺漂移信号。

图2 信号滤波过程（翻转90度并迅速复位）

2 融合技术的实现

下面通过一个安卓应用程序来看一下它的使用。

2.1初始化

首先通过成员变量的赋值来建立这个APP，获取SensorManager，通过OnCreate函数来初始化传感器监听器。

public class SensorFusionActivity extends Activity implements SensorEventListener{

private SensorManager mSensorManager = null

private float[] gyro = new float[3]；

private float[] gyroMatrix = new float[9]；

private float[] gyroOrientation = new float[3]；

private float[] magnet = new float[3]；

private float[] accel = new float[3]；

private float[] accMagOrientation = new float[3]；

private float[] fusedOrientation = new float[3]；

private float[] rotationMatrix = new float[9]；

public void onCreate（Bundle savedInstanceState） {

super.onCreate（savedInstanceState）；

setContentView（R.layout.main）；

gyroOrientation[0] = 0.0f；

gyroOrientation[1] = 0.0f；

gyroOrientation[2] = 0.0f；

gyroMatrix[0] = 1.0f； gyroMatrix[1] = 0.0f； gyroMatrix[2] = 0.0f；

gyroMatrix[3] = 0.0f； gyroMatrix[4] = 1.0f； gyroMatrix[5] = 0.0f；

gyroMatrix[6] = 0.0f； gyroMatrix[7] = 0.0f； gyroMatrix[8] = 1.0f；

// 获取sensorManager，初始化传感器监听器

mSensorManager = （SensorManager） this.getSystemService（SENSOR_SERVICE）；

initListeners（）；}}

注意程序中使用的SensorEventListener接口，下面会使用2个函数onAccuracyChanged和onSensorChanged。这里的重点是onSensorChanged函数，它可以持续更新传感器数据。传感器监听器的初始化通过下面的initListeners函数。

public void initListeners（）{

mSensorManager.registerListener（this，

mSensorManager.getDefaultSensor（Sensor.TYPE_ACCELEROMETER），

SensorManager.SENSOR_DELAY_FASTEST）；

mSensorManager.registerListener（this，

mSensorManager.getDefaultSensor（Sensor.TYPE_GYROSCOPE），

SensorManager.SENSOR_DELAY_FASTEST）；

mSensorManager.registerListener（this，

mSensorManager.getDefaultSensor（Sensor.TYPE_MAGNETIC_FIELD），

SensorManager.SENSOR_DELAY_FASTEST）；}

2.2获取和处理传感器数据

监听器初始化完成后，如果有新的可用传感器数据产生，onSensorChanged函数会被自动调用，onSensorChanged函数代码如下所示。

public void onSensorChanged（SensorEvent event） {

switch（event.sensor.getType（）） {

case Sensor.TYPE_ACCELEROMETER：

// 把加速传感器的数据复制给accel数组，然后计算新的方向数据

System.arraycopy（event.values， 0， accel， 0， 3）；

calculateAccMagOrientation（）；

break；

case Sensor.TYPE_GYROSCOPE：

// process gyro data

gyroFunction（event）；

break；

case Sensor.TYPE_MAGNETIC_FIELD：

// copy new magnetometer data into magnet array

System.arraycopy（event.values， 0， magnet， 0， 3）；

break；}}

android API提供了很多函数可以很方便的通过加速感应器和磁场感应器来获取对地定向。

public void calculateAccMagOrientation（） {

if（SensorManager.getRotationMatrix（rotationMatrix， null， accel， magnet）） {

SensorManager.getOrientation（rotationMatrix， accMagOrientation）；}}

如上所述，陀螺仪数据是不能直接用来做计算的，它需要进行处理，其具体的处理方法在android帮助中已经给出，这里就不再详述，下面就是对这段代码的引用，其中添加了一些参数。

public static final float EPSILON = 0.000000001f；

private void getRotationVectorFromGyro（float[] gyroValues，

float[] deltaRotationVector，

float timeFactor）

{float[] normValues = new float[3]；

// 计算角速度

float omegaMagnitude =

（float）Math.sqrt（gyroValues[0] * gyroValues[0] +

gyroValues[1] * gyroValues[1] +

gyroValues[2] * gyroValues[2]）；

if（omegaMagnitude > EPSILON） {

normValues[0] = gyroValues[0] / omegaMagnitude；

normValues[1] = gyroValues[1] / omegaMagnitude；

normValues[2] = gyroValues[2] / omegaMagnitude；}

float thetaOverTwo = omegaMagnitude * timeFactor；

float sinThetaOverTwo = （float）Math.sin（thetaOverTwo）；

float cosThetaOverTwo = （float）Math.cos（thetaOverTwo）；

deltaRotationVector[0] = sinThetaOverTwo * normValues[0]；

deltaRotationVector[1] = sinThetaOverTwo * normValues[1]；

deltaRotationVector[2] = sinThetaOverTwo * normValues[2]；

deltaRotationVector[3] = cosThetaOverTwo；}

上面这个函数可以计算出deltaRotationVector数组的值，这个值是一个旋转向量，这个向量包含4个值。它是android设备中陀螺仪的旋转状态（从一个开始时间点状态到一个结束时间点状态的时间间隔）值（相对角度）。旋转速度乘以最近的时间间隔（上面这个函数中的时间因子参数）。这个函数被用来在陀螺仪开始测量时的陀螺仪运算函数中调用。

private static final float NS2S = 1.0f / 1000000000.0f；

private float timestamp；

private boolean initState = true；

public void gyroFunction（SensorEvent event） {

if （accMagOrientation == null）

return；

if（initState） {

float[] initMatrix = new float[9]；

initMatrix = getRotationMatrixFromOrientation（accMagOrientation）；

float[] test = new float[3]；

SensorManager.getOrientation（initMatrix， test）；

gyroMatrix = matrixMultiplication（gyroMatrix， initMatrix）；

initState = false；}

float[] deltaVector = new float[4]；

if（timestamp ！= 0） {

final float dT = （event.timestamp - timestamp） * NS2S；

System.arraycopy（event.values， 0， gyro， 0， 3）；

getRotationVectorFromGyro（gyro， deltaVector， dT / 2.0f）；}

timestamp = event.timestamp；

float[] deltaMatrix = new float[9]；

SensorManager.getRotationMatrixFromVector（deltaMatrix， deltaVector）；

gyroMatrix = matrixMultiplication（gyroMatrix， deltaMatrix）；

SensorManager.getOrientation（gyroMatrix， gyroOrientation）；}

当加速感应器和重力感应器的方向角度数据产生之后（通过变量accMagOrientation来控制），陀螺仪的数据才能被处理。被处理之后的数据就可以作为陀螺仪的初始方向了。另外，函数中的方向矩阵也会包含很多未知值，android设备的当前方向和被计算过的陀螺仪旋转向量会被转化成旋转矩阵（gyroMatrix变量保存其值）。

gyroMatrix变量中包含了所有被处理过的陀螺仪的方向数据。deltaMatrix变量保存最新的旋转时间间隔值，这个数据在gyroMatrix变量的后面计算中会用到，相当于把gyroMatrix变量所表示的矩阵转置。如下所示的函数中会调用matrixMultiplication函数，注意给这个函数的两个形参赋值时，实参的次序不要颠倒了，因为这里涉及到两个矩阵相乘（左乘和右乘是不一样的）。调用getRotationMatrixFromVector函数可以把旋转向量转换成矩阵，代码如下所示。

private float[] getRotationMatrixFromOrientation（float[] o） {

float[] xM = new float[9]；

float[] yM = new float[9]；

float[] zM = new float[9]；

float sinX = （float）Math.sin（o[1]）；

float cosX = （float）Math.cos（o[1]）；

float sinY = （float）Math.sin（o[2]）；

float cosY = （float）Math.cos（o[2]）；

float sinZ = （float）Math.sin（o[0]）；

float cosZ = （float）Math.cos（o[0]）；

xM[0] = 1.0f； xM[1] = 0.0f； xM[2] = 0.0f；

xM[3] = 0.0f； xM[4] = cosX； xM[5] = sinX；

xM[6] = 0.0f； xM[7] = -sinX； xM[8] = cosX；

yM[0] = cosY； yM[1] = 0.0f； yM[2] = sinY；

yM[3] = 0.0f； yM[4] = 1.0f； yM[5] = 0.0f；

yM[6] = -sinY； yM[7] = 0.0f； yM[8] = cosY；

zM[0] = cosZ； zM[1] = sinZ； zM[2] = 0.0f；

zM[3] = -sinZ； zM[4] = cosZ； zM[5] = 0.0f；

zM[6] = 0.0f； zM[7] = 0.0f； zM[8] = 1.0f；

float[] resultMatrix = matrixMultiplication（xM， yM）；

resultMatrix = matrixMultiplication（zM， resultMatrix）；

return resultMatrix；}

3 結束语

在使用互补滤波器时，为了更好的控制它的输出，可以通过独立的线程来处理滤波。传感器信号的质量高低很大程度上取决于其采样频率，也就是滤波函数在单位时间被调用的次数。这也就是把所有计算放在TimerTask类中，但是把关于取样的时间间隔变量定义放在每次调用它的函数中。