使用 Android 陀螺仪代替加速度计。我发现很多零碎的东西,但没有完整的代码

Posted

技术标签:

【中文标题】使用 Android 陀螺仪代替加速度计。我发现很多零碎的东西,但没有完整的代码【英文标题】:Using Android gyroscope instead of accelerometer. I find lots of bits and pieces, but no complete code 【发布时间】:2012-11-20 16:50:05 【问题描述】:

Sensor Fusion 视频看起来很棒,但没有代码: http://www.youtube.com/watch?v=C7JQ7Rpwn2k&feature=player_detailpage#t=1315s

这是我的代码,它只使用加速度计和指南针。我还在 3 个方向值上使用了卡尔曼滤波器,但是这里显示的代码太多了。最终,这可以正常工作,但结果要么过于紧张,要么过于滞后,具体取决于我对结果的处理方式以及我将过滤因子设为多低。

/** Just accelerometer and magnetic sensors */
public abstract class SensorsListener2
    implements
        SensorEventListener

    /** The lower this is, the greater the preference which is given to previous values. (slows change) */
    private static final float accelFilteringFactor = 0.1f;
    private static final float magFilteringFactor = 0.01f;

    public abstract boolean getIsLandscape();

    @Override
    public void onSensorChanged(SensorEvent event) 
        Sensor sensor = event.sensor;
        int type = sensor.getType();

        switch (type) 
            case Sensor.TYPE_MAGNETIC_FIELD:
                mags[0] = event.values[0] * magFilteringFactor + mags[0] * (1.0f - magFilteringFactor);
                mags[1] = event.values[1] * magFilteringFactor + mags[1] * (1.0f - magFilteringFactor);
                mags[2] = event.values[2] * magFilteringFactor + mags[2] * (1.0f - magFilteringFactor);

                isReady = true;
                break;
            case Sensor.TYPE_ACCELEROMETER:
                accels[0] = event.values[0] * accelFilteringFactor + accels[0] * (1.0f - accelFilteringFactor);
                accels[1] = event.values[1] * accelFilteringFactor + accels[1] * (1.0f - accelFilteringFactor);
                accels[2] = event.values[2] * accelFilteringFactor + accels[2] * (1.0f - accelFilteringFactor);
                break;

            default:
                return;
        




        if(mags != null && accels != null && isReady) 
            isReady = false;

            SensorManager.getRotationMatrix(rot, inclination, accels, mags);

            boolean isLandscape = getIsLandscape();
            if(isLandscape) 
                outR = rot;
             else 
                // Remap the coordinates to work in portrait mode.
                SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
            

            SensorManager.getOrientation(outR, values);

            double x180pi = 180.0 / Math.PI;
            float azimuth = (float)(values[0] * x180pi);
            float pitch = (float)(values[1] * x180pi);
            float roll = (float)(values[2] * x180pi);

            // In landscape mode swap pitch and roll and invert the pitch.
            if(isLandscape) 
                float tmp = pitch;
                pitch = -roll;
                roll = -tmp;
                azimuth = 180 - azimuth;
             else 
                pitch = -pitch - 90;
                azimuth = 90 - azimuth;
            

            onOrientationChanged(azimuth,pitch,roll);
        
    




    private float[] mags = new float[3];
    private float[] accels = new float[3];
    private boolean isReady;

    private float[] rot = new float[9];
    private float[] outR = new float[9];
    private float[] inclination = new float[9];
    private float[] values = new float[3];



    /**
    Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West
    Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis.
    Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis.
    */
    public abstract void onOrientationChanged(float azimuth, float pitch, float roll);

我试图弄清楚如何添加陀螺仪数据,但我做得不对。 http://developer.android.com/reference/android/hardware/SensorEvent.html 的谷歌文档显示了一些从陀螺仪数据中获取增量矩阵的代码。这个想法似乎是我将加速计和磁传感器的滤波器调低,以使它们真正稳定。这将跟踪长期方向。

然后,我会保留来自陀螺仪的最新 N delta 矩阵的历史记录。每次我得到一个新的时,我都会放弃最旧的一个,然后将它们全部相乘以获得最终矩阵,然后将其与加速度计和磁传感器返回的稳定矩阵相乘。

这似乎不起作用。或者,至少,我的实现不起作用。结果远比加速度计更加紧张。增加陀螺仪历史记录的大小实际上会增加抖动,这让我认为我没有从陀螺仪计算正确的值。

public abstract class SensorsListener3
    implements
        SensorEventListener

    /** The lower this is, the greater the preference which is given to previous values. (slows change) */
    private static final float kFilteringFactor = 0.001f;
    private static final float magKFilteringFactor = 0.001f;


    public abstract boolean getIsLandscape();

    @Override
    public void onSensorChanged(SensorEvent event) 
        Sensor sensor = event.sensor;
        int type = sensor.getType();

        switch (type) 
            case Sensor.TYPE_MAGNETIC_FIELD:
                mags[0] = event.values[0] * magKFilteringFactor + mags[0] * (1.0f - magKFilteringFactor);
                mags[1] = event.values[1] * magKFilteringFactor + mags[1] * (1.0f - magKFilteringFactor);
                mags[2] = event.values[2] * magKFilteringFactor + mags[2] * (1.0f - magKFilteringFactor);

                isReady = true;
                break;
            case Sensor.TYPE_ACCELEROMETER:
                accels[0] = event.values[0] * kFilteringFactor + accels[0] * (1.0f - kFilteringFactor);
                accels[1] = event.values[1] * kFilteringFactor + accels[1] * (1.0f - kFilteringFactor);
                accels[2] = event.values[2] * kFilteringFactor + accels[2] * (1.0f - kFilteringFactor);
                break;

            case Sensor.TYPE_GYROSCOPE:
                gyroscopeSensorChanged(event);
                break;

            default:
                return;
        




        if(mags != null && accels != null && isReady) 
            isReady = false;

            SensorManager.getRotationMatrix(rot, inclination, accels, mags);

            boolean isLandscape = getIsLandscape();
            if(isLandscape) 
                outR = rot;
             else 
                // Remap the coordinates to work in portrait mode.
                SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
            

            if(gyroUpdateTime!=0) 
                matrixHistory.mult(matrixTmp,matrixResult);
                outR = matrixResult;
            

            SensorManager.getOrientation(outR, values);

            double x180pi = 180.0 / Math.PI;
            float azimuth = (float)(values[0] * x180pi);
            float pitch = (float)(values[1] * x180pi);
            float roll = (float)(values[2] * x180pi);

            // In landscape mode swap pitch and roll and invert the pitch.
            if(isLandscape) 
                float tmp = pitch;
                pitch = -roll;
                roll = -tmp;
                azimuth = 180 - azimuth;
             else 
                pitch = -pitch - 90;
                azimuth = 90 - azimuth;
            

            onOrientationChanged(azimuth,pitch,roll);
        
    



    private void gyroscopeSensorChanged(SensorEvent event) 
        // This timestep's delta rotation to be multiplied by the current rotation
        // after computing it from the gyro sample data.
        if(gyroUpdateTime != 0) 
            final float dT = (event.timestamp - gyroUpdateTime) * NS2S;
            // Axis of the rotation sample, not normalized yet.
            float axisX = event.values[0];
            float axisY = event.values[1];
            float axisZ = event.values[2];

            // Calculate the angular speed of the sample
            float omegaMagnitude = (float)Math.sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ);

            // Normalize the rotation vector if it's big enough to get the axis
            if(omegaMagnitude > EPSILON) 
                axisX /= omegaMagnitude;
                axisY /= omegaMagnitude;
                axisZ /= 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 * dT / 2.0f;
            float sinThetaOverTwo = (float)Math.sin(thetaOverTwo);
            float cosThetaOverTwo = (float)Math.cos(thetaOverTwo);
            deltaRotationVector[0] = sinThetaOverTwo * axisX;
            deltaRotationVector[1] = sinThetaOverTwo * axisY;
            deltaRotationVector[2] = sinThetaOverTwo * axisZ;
            deltaRotationVector[3] = cosThetaOverTwo;
        
        gyroUpdateTime = event.timestamp;
        SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);
        // User code should concatenate the delta rotation we computed with the current rotation
        // in order to get the updated rotation.
        // rotationCurrent = rotationCurrent * deltaRotationMatrix;
        matrixHistory.add(deltaRotationMatrix);
    



    private float[] mags = new float[3];
    private float[] accels = new float[3];
    private boolean isReady;

    private float[] rot = new float[9];
    private float[] outR = new float[9];
    private float[] inclination = new float[9];
    private float[] values = new float[3];

    // gyroscope stuff
    private long gyroUpdateTime = 0;
    private static final float NS2S = 1.0f / 1000000000.0f;
    private float[] deltaRotationMatrix = new float[9];
    private final float[] deltaRotationVector = new float[4];
//TODO: I have no idea how small this value should be.
    private static final float EPSILON = 0.000001f;
    private float[] matrixMult = new float[9];
    private MatrixHistory matrixHistory = new MatrixHistory(100);
    private float[] matrixTmp = new float[9];
    private float[] matrixResult = new float[9];


    /**
    Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West 
    Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis. 
    Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis.
    */
    public abstract void onOrientationChanged(float azimuth, float pitch, float roll);



public class MatrixHistory

    public MatrixHistory(int size) 
        vals = new float[size][];
    

    public void add(float[] val) 
        synchronized(vals) 
            vals[ix] = val;
            ix = (ix + 1) % vals.length;
            if(ix==0)
                full = true;
        
    

    public void mult(float[] tmp, float[] output) 
        synchronized(vals) 
            if(full) 
                for(int i=0; i<vals.length; ++i) 
                    if(i==0) 
                        System.arraycopy(vals[i],0,output,0,vals[i].length);
                     else 
                        MathUtils.multiplyMatrix3x3(output,vals[i],tmp);
                        System.arraycopy(tmp,0,output,0,tmp.length);
                    
                
             else 
                if(ix==0)
                    return;
                for(int i=0; i<ix; ++i) 
                    if(i==0) 
                        System.arraycopy(vals[i],0,output,0,vals[i].length);
                     else 
                        MathUtils.multiplyMatrix3x3(output,vals[i],tmp);
                        System.arraycopy(tmp,0,output,0,tmp.length);
                    
                
            
        
    


    private int ix = 0;
    private boolean full = false;
    private float[][] vals;

第二个代码块包含我对添加陀螺仪的第一块代码所做的更改。

具体来说,accel 的过滤因子变小了(使值更稳定)。 MatrixHistory 类跟踪在 gyroscopeSensorChanged 方法中计算的最后 100 个陀螺仪 deltaRotationMatrix 值。

我在这个网站上看到了很多关于这个主题的问题。他们帮助我达到了这一点,但我不知道下一步该做什么。我真希望 Sensor Fusion 的人刚刚在某处发布了一些代码。很明显,他把这一切都放在了一起。

【问题讨论】:

根据《Professional Android Sensor Programming》一书,InvenSense 的 Sensor Fusion 算法是专有的,因此很难在公共访问中找到源代码。该库包含在系统级别的大多数现代设备中,因此 SENSOR.TYPE_ROTATION 已经提供了有关基于陀螺仪的短时校正的测量。我认为关于此事的最详尽的公共资源是this。我不确定它是否是一个好的替代品。 还有几篇关于使用卡尔曼滤波器进行传感器融合的学术论文。它们通常不包含源代码,但应该包含您需要的技术和数学细节。 scholar.google.com 为什么要对磁值进行低通滤波? 【参考方案1】:

好吧,即使您知道卡尔曼滤波器是什么,也要为您 +1。如果你愿意,我会编辑这篇文章并给你我几年前写的代码来做你想做的事情。

但首先,我会告诉你为什么不需要它。

Android 传感器堆栈的现代实现使用 Sensor Fusion,正如 Stan 上面提到的。这只是意味着所有可用的数据——加速度、磁力、陀螺仪——都被收集在一个算法中,然后所有的输出都以 Android 传感器的形式被读出。

编辑:我刚刚偶然发现了这个关于该主题的精彩 Google 技术讲座:Sensor Fusion on Android Devices: A Revolution in Motion Processing。如果您对该主题感兴趣,值得花 45 分钟观看。

本质上,Sensor Fusion 是一个黑匣子。我查看了 Android 实现的源代码,它是一个用 C++ 编写的大卡尔曼滤波器。那里有一些非常好的代码,比我写过的任何过滤器都要复杂得多,而且可能比你写的更复杂。请记住,这些人这样做是为了谋生。

我还知道至少有一家芯片组制造商拥有自己的传感器融合实施方案。然后设备制造商根据自己的标准在 Android 和供应商实现之间进行选择。

最后,正如 Stan 上面提到的,Invensense 在芯片级有自己的传感器融合实现。

无论如何,归根结底,您设备中的内置传感器融合可能优于您或我拼凑的任何东西。所以你真正想要做的是访问它。

在 Android 中,既有物理传感器也有虚拟传感器。虚拟传感器是从可用物理传感器合成的传感器。最著名的示例是 TYPE_ORIENTATION,它采用加速度计和磁力计并创建滚动/俯仰/航向输出。 (顺便说一句,你不应该使用这个传感器;它有太多的限制。)

但重要的是,较新版本的 Android 包含这两个新的虚拟传感器:

TYPE_GRAVITY 是已过滤掉运动效果的加速度计输入 TYPE_LINEAR_ACCELERATION 是过滤掉重力分量的加速度计。

这两个虚拟传感器是通过加速度计输入和陀螺仪输入的组合合成的。

另一个值得注意的传感器是 TYPE_ROTATION_VECTOR,它是由加速度计、磁力计和陀螺仪合成的四元数。它代表了设备的完整 3 维方向,已滤除线性加速度的影响。

但是,对于大多数人来说,四元数有点抽象,而且由于您可能无论如何都在使用 3-d 转换,因此您最好的方法是通过 SensorManager.getRotationMatrix() 结合 TYPE_GRAVITY 和 TYPE_MAGNETIC_FIELD。

还有一点:如果您使用的是运行旧版 Android 的设备,则需要检测到您没有收到 TYPE_GRAVITY 事件并改用 TYPE_ACCELEROMETER。从理论上讲,这将是一个使用您自己的卡尔曼滤波器的地方,但如果您的设备没有内置传感器融合,它可能也没有陀螺仪。

无论如何,这里有一些示例代码来展示我是如何做到的。

  // Requires 1.5 or above

  class Foo extends Activity implements SensorEventListener 

    SensorManager sensorManager;
    float[] gData = new float[3];           // Gravity or accelerometer
    float[] mData = new float[3];           // Magnetometer
    float[] orientation = new float[3];
    float[] Rmat = new float[9];
    float[] R2 = new float[9];
    float[] Imat = new float[9];
    boolean haveGrav = false;
    boolean haveAccel = false;
    boolean haveMag = false;

    onCreate() 
        // Get the sensor manager from system services
        sensorManager =
          (SensorManager)getSystemService(Context.SENSOR_SERVICE);
    

    onResume() 
        super.onResume();
        // Register our listeners
        Sensor gsensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
        Sensor asensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        Sensor msensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        sensorManager.registerListener(this, gsensor, SensorManager.SENSOR_DELAY_GAME);
        sensorManager.registerListener(this, asensor, SensorManager.SENSOR_DELAY_GAME);
        sensorManager.registerListener(this, msensor, SensorManager.SENSOR_DELAY_GAME);
    

    public void onSensorChanged(SensorEvent event) 
        float[] data;
        switch( event.sensor.getType() ) 
          case Sensor.TYPE_GRAVITY:
            gData[0] = event.values[0];
            gData[1] = event.values[1];
            gData[2] = event.values[2];
            haveGrav = true;
            break;
          case Sensor.TYPE_ACCELEROMETER:
            if (haveGrav) break;    // don't need it, we have better
            gData[0] = event.values[0];
            gData[1] = event.values[1];
            gData[2] = event.values[2];
            haveAccel = true;
            break;
          case Sensor.TYPE_MAGNETIC_FIELD:
            mData[0] = event.values[0];
            mData[1] = event.values[1];
            mData[2] = event.values[2];
            haveMag = true;
            break;
          default:
            return;
        

        if ((haveGrav || haveAccel) && haveMag) 
            SensorManager.getRotationMatrix(Rmat, Imat, gData, mData);
            SensorManager.remapCoordinateSystem(Rmat,
                    SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X, R2);
            // Orientation isn't as useful as a rotation matrix, but
            // we'll show it here anyway.
            SensorManager.getOrientation(R2, orientation);
            float incl = SensorManager.getInclination(Imat);
            Log.d(TAG, "mh: " + (int)(orientation[0]*DEG));
            Log.d(TAG, "pitch: " + (int)(orientation[1]*DEG));
            Log.d(TAG, "roll: " + (int)(orientation[2]*DEG));
            Log.d(TAG, "yaw: " + (int)(orientation[0]*DEG));
            Log.d(TAG, "inclination: " + (int)(incl*DEG));
        
      
    

嗯;如果您碰巧有一个四元数库,那么接收 TYPE_ROTATION_VECTOR 并将其转换为数组可能更简单。

【讨论】:

是否有机会在本机端访问这些数据?大多数东西都没有通过sensor.h中的NDK暴露 你有没有让这一切正常工作,我的仍然很紧张,我有一个低通滤波器 你好,爱德华,这是一个很好的解释,我已经尝试了一段时间。你能告诉我为什么需要使用SensorManager.remapCoordinateSystem吗?谢谢。 Android 上的传感器系统不知道也不关心您的设备是如何旋转的。每个设备都有自己的坐标系,+X 向右,+Y 向上,+Z 向你。如果设备旋转,例如横向的手机,您需要将传感器坐标转换为对您握持设备的方式有意义的值。这究竟意味着什么取决于您的应用程序。 remapcoordinateSystem() 是一个在这里很有用的辅助函数。我还有一些笔记efalk.org/Docs/Android/sensors_0.html#remapCoordinateSystem Here's a sensor fusion library 这可能很有趣。【参考方案2】:

关于在哪里可以找到完整代码的问题,这是 Android 果冻豆的默认实现:https://android.googlesource.com/platform/frameworks/base/+/jb-release/services/sensorservice/ 首先检查 fusion.cpp/h。 它使用修改后的罗德里格斯参数(接近欧拉角)而不是四元数。除了方向之外,卡尔曼滤波器还估计陀螺漂移。对于测量更新,它使用磁力计,并且有点不正确地使用加速度(比力)。

要使用该代码,您应该是一个向导或了解 INS 和 KF 的基础知识。许多参数必须经过微调才能使过滤器正常工作。正如爱德华所说,这些人这样做是为了生活。

至少在 google 的 Galaxy nexus 中,这个默认实现未被使用,并被 Invense 的专有系统覆盖。

【讨论】:

以上是关于使用 Android 陀螺仪代替加速度计。我发现很多零碎的东西,但没有完整的代码的主要内容,如果未能解决你的问题,请参考以下文章

仅使用陀螺仪和加速度计的正确方法可以在 ANDROID 上的任何轴上获得可靠的当前角度

从陀螺仪android传感器获取旋转度

使用 BLE 从智能手表接收加速度计和陀螺仪信号

使用陀螺仪和加速度计获取方向

使用加速度计和陀螺仪的 6DOF

使用加速度计、陀螺仪和指南针计算设备在 3D 世界中的运动