Android 传感器融合教程

• 下载 SensorFusion1.zip - 70.8 KB

前言

本文是从我已停用的 former blog on thousand-thoughts.com 上的原始文章转载的。该文章的请求量非常高，所以我决定将其保存在 Code Project 上。 原始文章发布于 2012 年 3 月 16 日，因此使用了过时的 Android API。但是，这种方法仍然是一个有趣的话题。从那时起，代码已被许多开发人员使用。Jose Collas 使用原始代码创建了一个更用户友好的框架，并将其发布到 github 存储库。

引言

在撰写硕士论文期间，我对 Android 设备上的传感器进行了一些实践，并决定与其他 Android 开发者分享。在我的工作中，我为原型系统开发了一个头部跟踪组件。由于它需要根据用户头部的方向调整音频输出，因此要求它同时具有快速响应和准确性。

我使用了我的三星 Galaxy S2，并决定结合使用陀螺仪、加速度计和磁场传感器，以快速准确地测量用户的头部旋转。为了实现这一点，我实现了一个互补滤波器来消除陀螺仪漂移以及加速度计和磁力计的信号噪声。以下教程将详细介绍如何实现。

本教程基于 Android API 版本 10 (platform 2.3.3)。

本文分为两部分。第一部分介绍了 Shane Colton (MIT) 描述的传感器信号互补滤波器的理论背景，在此处。第二部分描述了使用 Java 编程语言的实现。任何认为理论枯燥并想立即开始编程的人都可以直接跳到第二部分。

通过互补滤波器进行传感器融合

获取 Android 设备姿态的常用方法是使用 SensorManager.getOrientation() 方法获取三个方向角。这两个角度基于加速度计和磁力计的输出。简单来说，加速度计提供重力矢量（指向地球中心的矢量），磁力计充当指南针。来自这两个传感器的信息足以计算设备的朝向。但是，两个传感器的输出都不准确，尤其是磁场传感器的输出，它包含大量噪声。

设备中的陀螺仪精度远高于此，响应时间非常短。它的缺点是可怕的陀螺仪漂移。陀螺仪提供所有三个轴的角速度。为了获得实际的朝向，需要随时间积分这些速度值。这是通过将角速度乘以上次传感器输出和当前传感器输出之间的时间间隔来完成的。这将产生一个旋转增量。所有旋转增量的总和产生设备的绝对朝向。在此过程中，每次迭代都会引入小的误差。这些小的误差会随着时间的推移而累积，导致计算出的朝向持续缓慢旋转，这就是陀螺仪漂移。

为了避免陀螺仪漂移和嘈杂的方向信息，陀螺仪的输出仅用于短时间内的方向变化，而磁力计/加速度计数据则在长时间内用作支持信息。这等同于对加速度计和磁场传感器信号进行低通滤波，对陀螺仪信号进行高通滤波。整体传感器融合和滤波如下所示：

那么，对传感器数据进行高通和低通滤波究竟意味着什么？传感器在（或多或少）规则的时间间隔内提供数据。它们的数值可以显示为图形中的信号，其中时间是 x 轴，类似于音频信号。嘈杂的加速度计/磁力计信号（上图中为 accMagOrientation）的低通滤波是指在恒定时间窗口内随时间平均的方向角度。

稍后在实现中，这可以通过缓慢地将加速度计/磁力计的新值引入绝对方向来完成：

// low-pass filtering: every time a new sensor value is available
// it is weighted with a factor and added to the absolute oriantation
accMagOrientation = ( 1 - factor ) * accMagOrientation+ factor * newAccMagValue;  

通过用陀螺仪方向值替换 accMagOrientation 中的高频分量，可以对集成陀螺仪数据进行高通滤波。

fusedOrientation =
(1 - factor) * newGyroValue // high-frequency component
+ factor * newAccMagValue;  // low-frequency component

实际上，这已经是我们完成的互补滤波器了。

假设设备在一个方向上旋转了 90°，并在短时间内转回初始位置，滤波过程中的中间信号看起来会像这样：

请注意集成陀螺仪信号中的陀螺仪漂移。这是由于原始角速度中的微小不规则性造成的。这些微小的偏差在积分过程中会累加，并导致基于陀螺仪的朝向产生不希望的额外缓慢旋转。

实施

现在，让我们开始在实际的 Android 应用程序中实现。

设置和初始化

首先，我们需要用所需的成员设置我们的 Android 应用程序，获取 SensorManager 并初始化我们的传感器监听器，例如，在 onCreate 方法中。

public class SensorFusionActivity extends Activity implements SensorEventListener{
    private SensorManager mSensorManager = null;
 
    // angular speeds from gyro
    private float[] gyro = new float[3];
 
    // rotation matrix from gyro data
    private float[] gyroMatrix = new float[9];
 
    // orientation angles from gyro matrix
    private float[] gyroOrientation = new float[3];
 
    // magnetic field vector
    private float[] magnet = new float[3];
 
    // accelerometer vector
    private float[] accel = new float[3];
 
    // orientation angles from accel and magnet
    private float[] accMagOrientation = new float[3];
 
    // final orientation angles from sensor fusion
    private float[] fusedOrientation = new float[3];
 
    // accelerometer and magnetometer based rotation matrix
    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;
 
        // initialise gyroMatrix with identity matrix
        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;
 
        // get sensorManager and initialise sensor listeners
        mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
        initListeners();
    }
 
    // ...
} 

请注意，该应用程序实现了 SensorEventListener 接口。因此，我们将不得不实现 onAccuracyChanged 和 onSensorChanged 这两个方法。由于本教程不需要，因此我将 onAccuracyChanged 留空。更重要的方法是 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);
} 

获取和处理传感器数据

在初始化监听器后，每当有新的传感器数据可用时，onSensorChanged 方法会自动调用。然后会相应地复制或处理数据。

public void onSensorChanged(SensorEvent event) {
    switch(event.sensor.getType()) {
    case Sensor.TYPE_ACCELEROMETER:
        // copy new accelerometer data into accel array
        // then calculate new orientation
        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 参考页面显示了如何从陀螺仪数据获取旋转矢量（参见 Sensor.TYPE_GYROSCOPE）。我只是重用了建议的代码并添加了一些参数，使其看起来像这样：

public static final float EPSILON = 0.000000001f;
 
private void getRotationVectorFromGyro(float[] gyroValues,
                                       float[] deltaRotationVector,
                                       float timeFactor)
{
    float[] normValues = new float[3];
 
    // Calculate the angular speed of the sample
    float omegaMagnitude =
        (float)Math.sqrt(gyroValues[0] * gyroValues[0] +
        gyroValues[1] * gyroValues[1] +
        gyroValues[2] * gyroValues[2]);
 
    // Normalize the rotation vector if it's big enough to get the axis
    if(omegaMagnitude > EPSILON) {
        normValues[0] = gyroValues[0] / omegaMagnitude;
        normValues[1] = gyroValues[1] / omegaMagnitude;
        normValues[2] = gyroValues[2] / omegaMagnitude;
    }
 
    // Integrate around this axis with the angular speed by the timestep
    // in order to get a delta rotation from this sample over the timestep
    // We will convert this axis-angle representation of the delta rotation
    // into a quaternion before turning it into the rotation matrix.
    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;
} 

上述函数创建一个类似于四元数的旋转矢量。它表示自上次陀螺仪测量以来设备在这段时间内的旋转间隔。将旋转速度乘以自上次测量以来经过的时间间隔——这里是参数 timeFactor。然后，此函数在实际的 gyroFunction 中用于陀螺仪传感器数据处理。在这里，陀螺仪旋转间隔会添加到绝对陀螺仪方向。但由于我们得到的是旋转矩阵而不是角度，因此无法通过简单地添加旋转间隔来完成。我们需要通过矩阵乘法来应用旋转间隔。

private static final float NS2S = 1.0f / 1000000000.0f;
private float timestamp;
private boolean initState = true;
 
public void gyroFunction(SensorEvent event) {
    // don't start until first accelerometer/magnetometer orientation has been acquired
    if (accMagOrientation == null)
        return;
 
    // initialisation of the gyroscope based rotation matrix
    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;
    }
 
    // copy the new gyro values into the gyro array
    // convert the raw gyro data into a rotation vector
    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);
    }
 
    // measurement done, save current time for next interval
    timestamp = event.timestamp;
 
    // convert rotation vector into rotation matrix
    float[] deltaMatrix = new float[9];
    SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);
 
    // apply the new rotation interval on the gyroscope based rotation matrix
    gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);
 
    // get the gyroscope based orientation from the rotation matrix
    SensorManager.getOrientation(gyroMatrix, gyroOrientation);
}  

陀螺仪数据直到加速度计和磁力计的方向角可用（在成员变量 accMagOrientation 中）才进行处理。这些数据对于陀螺仪数据的初始方向是必需的。否则，我们的方向矩阵将包含未定义的值。将设备的当前方向和计算出的陀螺仪旋转矢量转换为旋转矩阵。

gyroMatrix 是从所有迄今为止处理过的陀螺仪测量计算出的总方向。deltaMatrix 保存需要在下一步应用于 gyroMatrix 的上次旋转间隔。这通过将 gyroMatrix 与 deltaMatrix 相乘来完成。这等同于 gyroMatrix 绕 deltaMatrix 的旋转。matrixMultiplication 方法将在下面进一步描述。请勿交换矩阵乘法的两个参数，因为矩阵乘法不是可交换的。

可以通过调用 SensoManager 中的 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]);
 
    // rotation about x-axis (pitch)
    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;
 
    // rotation about y-axis (roll)
    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;
 
    // rotation about z-axis (azimuth)
    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;
 
    // rotation order is y, x, z (roll, pitch, azimuth)
    float[] resultMatrix = matrixMultiplication(xM, yM);
    resultMatrix = matrixMultiplication(zM, resultMatrix);
    return resultMatrix;
}

我必须承认，这个函数不是最优的，在性能方面还有改进的空间，但对于本教程来说，它已经足够了。它基本上为每个轴创建一个旋转矩阵，并按正确的顺序（在本例中为 y、x、z）将矩阵相乘。

这是矩阵乘法的函数：

private float[] matrixMultiplication(float[] A, float[] B) {
    float[] result = new float[9];
 
    result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
    result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
    result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];
 
    result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
    result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
    result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];
 
    result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
    result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
    result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];
 
    return result;
}

互补滤波器

最后，我们可以实现互补滤波器。为了更好地控制其输出，我们在单独的定时线程中执行滤波。传感器信号的质量在很大程度上取决于采样频率，即每秒调用滤波器方法的次数。因此，我们将所有计算放在一个 TimerTask 中，稍后定义每次调用之间的时间间隔。

class calculateFusedOrientationTask extends TimerTask {
    public void run() {
        float oneMinusCoeff = 1.0f - FILTER_COEFFICIENT;
        fusedOrientation[0] =
            FILTER_COEFFICIENT * gyroOrientation[0]
            + oneMinusCoeff * accMagOrientation[0];
 
        fusedOrientation[1] =
            FILTER_COEFFICIENT * gyroOrientation[1]
            + oneMinusCoeff * accMagOrientation[1];
 
        fusedOrientation[2] =
            FILTER_COEFFICIENT * gyroOrientation[2]
            + oneMinusCoeff * accMagOrientation[2];
 
        // overwrite gyro matrix and orientation with fused orientation
        // to comensate gyro drift
        gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
        System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
    }
}

如果您阅读了本教程的第一部分，这应该看起来有些熟悉。但是，有一个重要的修改。我们在每次滤波迭代中都会覆盖基于陀螺仪的方向和旋转矩阵。这用“改进”的传感器数据替换了陀螺仪方向，并消除了陀螺仪漂移。

当然，信号质量的第二个重要因素是 FILTER_COEFFICIENT。我通过旋转我手机的 3D 模型使用我实际设备上的传感器来启发式地确定了这个值。值为 0.98，采样率为 33Hz（这产生 30ms 的时间周期）对我来说效果很好。您可以增加采样率以获得更好的时间分辨率，但然后您必须调整 FILTER_COEFFICIENT 来提高信号质量。

因此，这些是我们传感器融合代码的最终补充：

public static final int TIME_CONSTANT = 30;
public static final float FILTER_COEFFICIENT = 0.98f;
private Timer fuseTimer = new Timer();
public void onCreate(Bundle savedInstanceState) {
 
    // ...
 
    // wait for one second until gyroscope and magnetometer/accelerometer
    // data is initialised then scedule the complementary filter task
    fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(),
                                  1000, TIME_CONSTANT);
}

希望本教程能够充分解释 Android 设备上的自定义传感器融合。 

历史 

2014 年 2 月 18 日：文章从原版转载。