设备运动相对于世界坐标系 - 乘以反姿态矩阵的逆矩阵

Question

如何正确使用 CMAttitude:multiplyByInverseOfAttitude？

假设有一台 iOS5 设备平放在桌子上，在启动 CMMotionManager 后：

CMMotionManager *motionManager = [[CMMotionManager alloc]init];
[motionManager startDeviceMotionUpdatesUsingReferenceFrame:
    CMAttitudeReferenceFrameXTrueNorthZVertical];

稍后，将获取CMDeviceMotion对象：

CMDeviceMotion *deviceMotion = [motionManager deviceMotion];

我期望[deviceMotion attitude]反映设备相对于真北的旋转。

通过观察，[deviceMotion userAcceleration]报告设备参考系中的加速度。也就是说，将设备从一侧移动到另一侧（保持在桌子上平放）会在x轴上注册加速度。将设备旋转90°（仍然保持平放状态），并将其从一侧移动到另一侧，仍然会报告x轴加速度。

如何正确地转换[deviceMotion userAcceleration]以获取南北/东西方向的加速度而不是左右/前后方向？

CMAttitude multiplyByInverseOfAttitude似乎是不必要的，因为已经指定了参考系，并且从文档中无法确定如何将姿态应用于CMAcceleration。

- Zaq

Answer 1

如果CMDeviceMotion具有以参考帧坐标表示的userAcceleration存取器，则不会出现这个问题。因此，我使用了一个类别来添加所需的方法：

在CMDeviceMotion+TransformToReferenceFrame.h中：

#import <CoreMotion/CoreMotion.h>

@interface CMDeviceMotion (TransformToReferenceFrame)
-(CMAcceleration)userAccelerationInReferenceFrame;
@end

这段代码位于CMDeviceMotion+TransformToReferenceFrame.m文件中：

#import "CMDeviceMotion+TransformToReferenceFrame.h"

@implementation CMDeviceMotion (TransformToReferenceFrame)

-(CMAcceleration)userAccelerationInReferenceFrame
{
    CMAcceleration acc = [self userAcceleration];
    CMRotationMatrix rot = [self attitude].rotationMatrix;

    CMAcceleration accRef;
    accRef.x = acc.x*rot.m11 + acc.y*rot.m12 + acc.z*rot.m13;
    accRef.y = acc.x*rot.m21 + acc.y*rot.m22 + acc.z*rot.m23;
    accRef.z = acc.x*rot.m31 + acc.y*rot.m32 + acc.z*rot.m33;

    return accRef;
}

@end

并且在 Swift 3 中

extension CMDeviceMotion {

    var userAccelerationInReferenceFrame: CMAcceleration {
        let acc = self.userAcceleration
        let rot = self.attitude.rotationMatrix

        var accRef = CMAcceleration()
        accRef.x = acc.x*rot.m11 + acc.y*rot.m12 + acc.z*rot.m13;
        accRef.y = acc.x*rot.m21 + acc.y*rot.m22 + acc.z*rot.m23;
        accRef.z = acc.x*rot.m31 + acc.y*rot.m32 + acc.z*rot.m33;

        return accRef;
    }
}

现在，之前使用的 [deviceMotion userAcceleration] 的代码可以改用 [deviceMotion userAccelerationInReferenceFrame]。

- Zaq

---

你的答案是不正确的。为了将一个向量从参考系A移动到参考系B，当给定从B到A的旋转矩阵时，应该将向量乘以逆旋转矩阵。以下是如何计算逆矩阵的方法：https://dev59.com/E3NA5IYBdhLWcg3wYMx-#984054 - Vitaly Stakhov

@Vitaly，感谢您的反馈。代码已经可以运行并且正常工作了，你试过了吗？你提供的链接给出了一个求任意矩阵逆的方法，但是“旋转矩阵的逆就是它的转置”（http://mathpages.com/home/kmath593/kmath593.htm）。虽然微妙，但代码在计算中对旋转矩阵进行了转置。 - Zaq

@Zaq，我收回我的话，你是对的。我看了一下我的旋转矩阵反转代码，它是一个任意矩阵反转代码，并且它产生了转置矩阵。显然，旋转矩阵的逆矩阵是其转置矩阵，因为有两个事实：a）行列式始终为1，因为向量在旋转时不会缩放；b）旋转矩阵是正交矩阵。 - Vitaly Stakhov

谢谢。终于找到了。 - zirinisp

@xmkevinchen，感谢您的关注。代码确实实现了您所建议的功能。正如VitalyStakhov所意识到的那样，该代码在计算中转置了旋转矩阵。 - Zaq

Answer 2

根据苹果文档，`CMAttitude` 是指相对于给定参考系的物体方向。而 `userAcceleration` 或 `gravity` 是设备框架的值。所以为了获取参考框架的值，我们应该像 @Batti 所说的那样：

1. 每次更新时间获取姿态旋转矩阵。
2. 计算逆矩阵。
3. 将逆矩阵乘以 UserAcceleration 向量。

以下是 Swift 版本:

import CoreMotion
import GLKit

extension CMDeviceMotion {

    func userAccelerationInReferenceFrame() -> CMAcceleration {

        let origin = userAcceleration
        let rotation = attitude.rotationMatrix
        let matrix = rotation.inverse()

        var result = CMAcceleration()
        result.x = origin.x * matrix.m11 + origin.y * matrix.m12 + origin.z * matrix.m13;
        result.y = origin.x * matrix.m21 + origin.y * matrix.m22 + origin.z * matrix.m23;
        result.z = origin.x * matrix.m31 + origin.y * matrix.m32 + origin.z * matrix.m33;

        return result
    }

    func gravityInReferenceFrame() -> CMAcceleration {

        let origin = self.gravity
        let rotation = attitude.rotationMatrix
        let matrix = rotation.inverse()

        var result = CMAcceleration()
        result.x = origin.x * matrix.m11 + origin.y * matrix.m12 + origin.z * matrix.m13;
        result.y = origin.x * matrix.m21 + origin.y * matrix.m22 + origin.z * matrix.m23;
        result.z = origin.x * matrix.m31 + origin.y * matrix.m32 + origin.z * matrix.m33;

        return result
    }
}

extension CMRotationMatrix {

    func inverse() -> CMRotationMatrix {

        let matrix = GLKMatrix3Make(Float(m11), Float(m12), Float(m13), Float(m21), Float(m22), Float(m23), Float(m31), Float(m32), Float(m33))
        let invert = GLKMatrix3Invert(matrix, nil)

        return CMRotationMatrix(m11: Double(invert.m00), m12: Double(invert.m01), m13: Double(invert.m02),
                            m21: Double(invert.m10), m22: Double(invert.m11), m23: Double(invert.m12),
                            m31: Double(invert.m20), m32: Double(invert.m21), m33: Double(invert.m22))

    }

}

希望对您有些帮助。

- xmkevinchen

Answer 3

我阅读了上述论文后，尝试实现了一种解决方案。

步骤如下：

• 每次更新时获取姿态旋转矩阵。
• 计算逆矩阵。
• 将逆矩阵乘以UserAcceleration向量。

结果向量将是该向量的投影。

-x表示北方，+x表示南方

-y表示东方，+y表示西方。

我的代码还不完美，我正在努力改进中。

- Batti

---

我也可以为俯仰和翻滚做这个吗？ - nr5

你能分享一下代码吗？或者解释一下上面的步骤吗？我只知道将想要创建的姿态存储为参考，计算当前姿态的反向。 - nr5

Answer 4

参考框架与姿态值相关，查看偏航角的姿态值；如果您不使用参考框架，在启动应用程序时，此值始终为零，而如果您使用参考框架CMAttitudeReferenceFrameXTrueNorthZVertical，则偏航值表示x轴和真北之间的角度。 有了这些信息，您可以确定手机在地球坐标系中的姿态，因此可以确定加速度计轴相对于基准点的位置。

- Batti

---

谢谢@batti，我理解几何学，问题是关于编码的。API似乎不支持简单的应用变换的方法。在CMAttitude中，欧拉角、四元数表示和旋转矩阵都很容易访问，但没有明显的方法将它们应用到CMAcceleration上。更糟糕的是，CMAcceleration是只读的，所以我不能自己转换它并在发送之前更新对象。 - Zaq

问题在于加速度计的轴是固定在设备上的，因此加速度计可以测量设备框架中的加速度。如果传感器不那么嘈杂，您应该只需找到加速度的水平面，即与重力矢量垂直的平面，查看该平面上的矢量并计算矢量与北方之间的角度，以了解加速度在基本坐标系中的方向。你的应用程序是做什么的？ - Batti

这应该会有所帮助，第3.A节[链接]（http://ieeexplore.ieee.org/xpl/freeabs_all.jsp?arnumber=5767590） - Batti