如何通过罗盘读数和陀螺仪读数获得手机的方位角？

Question

我希望通过以下方法获取我手机的当前方向：

1. 首先通过getRotationMatrix() 和getOrientation() 获取初始方向（方位角）。
2. 添加陀螺仪读数随时间的积分以获取当前方向。

手机方向：

手机的 x-y 平面与地平面平行固定。即，处于“边走边发短信”的方向。

“getOrientation()”返回：

Android API 让我可以轻松地从getOrientation() 获取方向，即方位角、俯仰角、滚动角。

请注意此方法总是返回其值在以下范围内：[0, -PI]和[o, PI].

我的问题：

由于陀螺仪读数的集成，用dR表示，可能相当大，所以当我做CurrentOrientation += dR时，CurrentOrientation可能会超过[0, -PI]和[o, PI] 范围。

需要进行哪些操作才能始终在[0, -PI] 和[o, PI] 范围内获取当前方向？

我在 Python 中尝试过以下方法，但我非常怀疑它的正确性。

rotation = scipy.integrate.trapz(gyroSeries, timeSeries) # integration
if (headingDirection - rotation) < -np.pi:
    headingDirection += 2 * np.pi
elif (headingDirection - rotation) > np.pi:
    headingDirection -= 2 * np.pi
# Complementary Filter
headingDirection = ALPHA * (headingDirection - rotation) + (1 - ALPHA) * np.mean(azimuth[np.array(stepNo.tolist()) == i])
if headingDirection < -np.pi:
    headingDirection += 2 * np.pi
elif headingDirection > np.pi:
    headingDirection -= 2 * np.pi

---

备注

这不是那么简单，因为它涉及以下麻烦制造者：

1. 方向传感器读数从0 到-PI，然后直接跳转 到+PI，然后通过+PI/2 逐渐回到0。
2. 陀螺仪读数的集成也带来了一些麻烦。我应该将dR 添加到方向还是减去dR。

在给出确认答案之前，请先参考 Android 文档。

估计的答案没有帮助。

Answer 1

方向传感器实际上是从真实的磁力计和加速度计获得读数。

我想这可能是混乱的根源。这在文档中在哪里说明？更重要的是，文档是否明确指出陀螺仪读数被忽略？据我所知，该视频中描述的方法已实现：

Sensor Fusion on Android Devices: A Revolution in Motion Processing

此方法使用陀螺仪并整合它们的读数。这几乎使问题的其余部分变得毫无意义。不过我会尽量回答。

---

方向传感器已经为您整合了陀螺仪读数，这就是您获取方向的方式。我不明白你为什么要自己做。

你没有正确整合陀螺仪读数，它比CurrentOrientation += dR 更复杂（这是不正确的）。如果您需要整合陀螺仪读数（我不明白为什么，SensorManager 已经为您完成了），请阅读 Direction Cosine Matrix IMU: Theory 如何正确完成（方程式 17）。

不要尝试与欧拉角积分（又名方位角、俯仰角、滚动角），不会有任何好的结果。

请在计算中使用四元数或旋转矩阵，而不是欧拉角。如果您使用旋转矩阵，您可以随时将它们转换为欧拉角，请参阅

Computing Euler angles from a rotation matrix by Gregory G. Slabaugh

（四元数也是如此。）（在非退化的情况下）有两种表示旋转的方法，即，你会得到两个欧拉角。选择您需要的范围内的那个。（如果是gimbal lock，欧拉角有无限多个，请参见上面的 PDF）。只需保证在将旋转矩阵转换为欧拉角后，您不会在计算中再次开始使用欧拉角。

不清楚你在用互补过滤器做什么。你可以根据Direction Cosine Matrix IMU: Theory 手稿实现一个非常好的传感器融合，这基本上是一个教程。这样做并非易事，但我认为您不会找到比这份手稿更好、更易理解的教程。

当我根据这份手稿实现传感器融合时，我不得不自己发现的一件事是，所谓的integral windup 可能会发生。我通过绑定TotalCorrection（第27页）来处理它。如果你实现了这个传感器融合，你就会明白我在说什么。

---

---

更新：我在这里回答您在接受答案后在 cmets 中发布的问题。

我认为指南针通过重力和磁场为我提供当前方向，对吧？指南针用陀螺仪吗？

是的，如果手机或多或少静止至少半秒钟，您可以仅使用重力和指南针来获得良好的方向估计。操作方法如下：Can anyone tell me whether gravity sensor is as a tilt sensor to improve heading accuracy?

不，指南针中没有使用陀螺仪。

能否请您解释一下为什么我所做的集成是错误的？我知道如果我的手机的音调向上，欧拉角就会失败。但是我的集成还有其他问题吗？

有两件不相关的事情：(i) 积分应该以不同的方式进行，(ii) 由于万向节锁，欧拉角很麻烦。我再说一遍，这两个是无关的。

至于集成：这是一个简单的示例，您可以实际查看您的集成出了什么问题。设 x 和 y 为房间内水平面的轴。把手机拿在手里。将手机绕 x 轴（房间）旋转 45 度，然后绕 y 轴（房间）旋转 45 度。然后，从头开始重复这些步骤，但现在先绕 y 轴旋转，然后绕 x 轴旋转。手机最终以完全不同的方向结束。如果您按照CurrentOrientation += dR 进行集成，您将看不到任何区别！如果您想正确进行积分，请阅读上面链接的方向余弦矩阵 IMU：理论手稿。

至于欧拉角：它们破坏了应用程序的稳定性，我不使用它们在 3D 中进行任意旋转就足够了。

我还是不明白你为什么要自己做，为什么不想使用平台提供的方向估计。很有可能，你不能做得比这更好。

Answer 2

我认为你应该避免使用过时的“方向传感器”，并使用已经实现了专门的 Invensense 融合算法的 getRotationVector、getRotationMatrix 等传感器融合方法，这些方法已经使用了陀螺仪数据。

如果您想要一种称为平衡滤波器的简单传感器融合算法 （参考http://www.filedump.net/dumped/filter1285099462.pdf）可以使用。方法如

http://postimg.org/image/9cu9dwn8z/

这会集成陀螺仪来获取角度，然后高通过滤结果以去除 漂移，并将其添加到平滑的加速度计和指南针结果中。集成的、高通滤波的陀螺仪数据和加速度计/罗盘数据以这样的方式相加，即两部分相加 为一，以便输出是以有意义的单位进行的准确估计。 对于平衡滤波器，可以调整时间常数以调整响应。时间越短 常数，响应越好，但允许通过的加速度噪声越多。

要了解其工作原理，假设您将最新的陀螺仪数据点（以 rad/s 为单位）存储在陀螺仪中，即 来自加速度计的最新角度测量值存储在 angle_acc 中，dtis 从 到现在为止的最后一个陀螺仪数据。然后你的新角度将使用

计算

角度 = b * (角度 + 陀螺*dt) + (1 - b) *(angle_acc);

例如，您可以从尝试 b = 0.98 开始。您可能还想使用快速陀螺仪测量时间 dt，这样在进行下一次测量之前，陀螺仪的漂移不会超过几度。平衡滤波器有用且易于实现，但不是理想的传感器融合方法。 Invensense 的方法涉及一些巧妙的算法，可能还有某种形式的卡尔曼滤波器。

来源：专业 Android 传感器编程，Adam Stroud。

Answer 3

如果由于磁干扰导致方位角值不准确，据我所知，您无法采取任何措施来消除它。如果 TYPE_GRAVITY 不可用，要获得稳定的方位角读数，您需要过滤加速度计值。如果 TYPE_GRAVITY 不可用，那么我很确定该设备没有陀螺仪，因此您可以使用的唯一滤波器是低通滤波器。以下代码是使用 TYPE_GRAVITY 和 TYPE_MAGNETIC_FIELD 实现的稳定指南针。

public class Compass  implements SensorEventListener
{
    public static final float TWENTY_FIVE_DEGREE_IN_RADIAN = 0.436332313f;
    public static final float ONE_FIFTY_FIVE_DEGREE_IN_RADIAN = 2.7052603f;

    private SensorManager mSensorManager;
    private float[] mGravity;
    private float[] mMagnetic;
    // If the device is flat mOrientation[0] = azimuth, mOrientation[1] = pitch
    // and mOrientation[2] = roll, otherwise mOrientation[0] is equal to Float.NAN
    private float[] mOrientation = new float[3];
    private LinkedList<Float> mCompassHist = new LinkedList<Float>();
    private float[] mCompassHistSum = new float[]{0.0f, 0.0f};
    private int mHistoryMaxLength;

    public Compass(Context context)
    {
         mSensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
         // Adjust the history length to fit your need, the faster the sensor rate
         // the larger value is needed for stable result.
         mHistoryMaxLength = 20;
    }

    public void registerListener(int sensorRate)
    {
        Sensor magneticSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        if (magneticSensor != null)
        {
            mSensorManager.registerListener(this, magneticSensor, sensorRate);
        }
        Sensor gravitySensor = mSensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
        if (gravitySensor != null)
        {
            mSensorManager.registerListener(this, gravitySensor, sensorRate);
        }
    }

    public void unregisterListener()
    {
         mSensorManager.unregisterListener(this);
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy)
    {

    }

    @Override
    public void onSensorChanged(SensorEvent event)
    {
        if (event.sensor.getType() == Sensor.TYPE_GRAVITY)
        {
            mGravity = event.values.clone();
        }
        else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
        {
            mMagnetic = event.values.clone();
        }
        if (!(mGravity == null || mMagnetic == null))
        {
            mOrientation = getOrientation();
        } 
    }

    private void getOrientation()
    {
        float[] rotMatrix = new float[9];
        if (SensorManager.getRotationMatrix(rotMatrix, null, 
            mGravity, mMagnetic))
        {
            float inclination = (float) Math.acos(rotMatrix[8]);
            // device is flat
            if (inclination < TWENTY_FIVE_DEGREE_IN_RADIAN 
                || inclination > ONE_FIFTY_FIVE_DEGREE_IN_RADIAN)
            {
                float[] orientation = sensorManager.getOrientation(rotMatrix, mOrientation);
                mCompassHist.add(orientation[0]);
                mOrientation[0] = averageAngle();
            }
            else
            {
                mOrientation[0] = Float.NAN;
                clearCompassHist();
            }
        }
    }

    private void clearCompassHist()
    {
        mCompassHistSum[0] = 0;
        mCompassHistSum[1] = 0;
        mCompassHist.clear();
    }

    public float averageAngle()
    {
        int totalTerms = mCompassHist.size();
        if (totalTerms > mHistoryMaxLength)
        {
            float firstTerm = mCompassHist.removeFirst();
            mCompassHistSum[0] -= Math.sin(firstTerm);
            mCompassHistSum[1] -= Math.cos(firstTerm);
            totalTerms -= 1;
        }
        float lastTerm = mCompassHist.getLast();
        mCompassHistSum[0] += Math.sin(lastTerm);
        mCompassHistSum[1] += Math.cos(lastTerm);
        float angle = (float) Math.atan2(mCompassHistSum[0] / totalTerms, mCompassHistSum[1] / totalTerms);

        return angle;
    }
}

在你的活动中实例化一个 Compass 对象，比如在 onCreate 中，在 onResume 中 registerListener 和在 onPause 中 unregisterListener

private Compass mCompass;

@Override
protected void onCreate(Bundle savedInstanceState)
{
    super.onCreate(savedInstanceState);

    mCompass = new Compass(this);
}

@Override
protected void onPause()
{
    super.onPause();

    mCompass.unregisterListener();
}

@Override
protected void onResume()
{
    super.onResume();

    mCompass.registerListener(SensorManager.SENSOR_DELAY_NORMAL);
}

Answer 4

最好让android的方向检测实现来处理。现在，是的，您得到的值是从 -PI 到 PI，您可以将它们转换为度数 (0-360)。一些相关部分：

保存要处理的数据：

@Override
public void onSensorChanged(SensorEvent sensorEvent) {
    switch (sensorEvent.sensor.getType()) {
        case Sensor.TYPE_ACCELEROMETER:
            mAccValues[0] = sensorEvent.values[0];
            mAccValues[1] = sensorEvent.values[1];
            mAccValues[2] = sensorEvent.values[2];
            break;
        case Sensor.TYPE_MAGNETIC_FIELD:
            mMagValues[0] = sensorEvent.values[0];
            mMagValues[1] = sensorEvent.values[1];
            mMagValues[2] = sensorEvent.values[2];
            break;
    }

}

计算横滚、俯仰和偏航（方位角）。mR 和 mI 是用来保存旋转和倾角矩阵的，mO 是一个临时数组。数组mResults 的值以度为单位，最后：

    private void updateData() {
    SensorManager.getRotationMatrix(mR, mI, mAccValues, mMagValues);

    /**
     * arg 2: what world(according to app) axis , device's x axis aligns with
     * arg 3: what world(according to app) axis , device's y axis aligns with
     * world x = app's x = app's east
     * world y = app's y = app's north
     * device x = device's left side = device's east
     * device y = device's top side  = device's north
     */

    switch (mDispRotation) {
        case Surface.ROTATION_90:
            SensorManager.remapCoordinateSystem(mR, SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X, mR2);
            break;
        case Surface.ROTATION_270:
            SensorManager.remapCoordinateSystem(mR, SensorManager.AXIS_MINUS_Y, SensorManager.AXIS_X, mR2);
            break;
        case Surface.ROTATION_180:
            SensorManager.remapCoordinateSystem(mR, SensorManager.AXIS_MINUS_X, SensorManager.AXIS_MINUS_Y, mR2);
            break;
        case Surface.ROTATION_0:
        default:
            mR2 = mR;
    }

    SensorManager.getOrientation(mR2, mO);


    //--upside down when abs roll > 90--
    if (Math.abs(mO[2]) > PI_BY_TWO) {
        //--fix, azimuth always to true north, even when device upside down, realistic --
        mO[0] = -mO[0];

        //--fix, roll never upside down, even when device upside down, unrealistic --
        //mO[2] = mO[2] > 0 ? PI - mO[2] : - (PI - Math.abs(mO[2]));

        //--fix, pitch comes from opposite , when device goes upside down, realistic --
        mO[1] = -mO[1];
    }

    CircleUtils.convertRadToDegrees(mO, mOut);
    CircleUtils.normalize(mOut);

    //--write--
    mResults[0] = mOut[0];
    mResults[1] = mOut[1];
    mResults[2] = mOut[2];
}