【问题标题】:Android: Algorithms for SensorManager.getRotationMatrix and SensorManager.getOrientation()Android:SensorManager.getRotationMatrix 和 SensorManager.getOrientation() 的算法
【发布时间】:2015-11-29 02:58:48
【问题描述】:

在 Android 中,要从欧拉角(例如俯仰角、滚动角、方位角)中获取方向,需要执行以下操作:

  1. SensorManager.getRotationMatrix(float[] R, float[] I, float[] 重力, float[] 地磁);
  2. SensorManager.getOrientation(float[] R, float[] 方向);

在第一个中,我意识到它使用了一种 TRIAD 算法;旋转矩阵(R[])由重力、地磁X重力、重力X(地磁X重力)组成---X是叉积。
见下面的代码:

    float Ax = gravity[0];
    float Ay = gravity[1];
    float Az = gravity[2];
    final float Ex = geomagnetic[0];
    final float Ey = geomagnetic[1];
    final float Ez = geomagnetic[2];
    float Hx = Ey*Az - Ez*Ay;
    float Hy = Ez*Ax - Ex*Az;
    float Hz = Ex*Ay - Ey*Ax;
    final float normH = (float)Math.sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
    if (normH < 0.1f) {
        // device is close to free fall (or in space?), or close to
        // magnetic north pole. Typical values are  > 100.
        return false;
    }
    final float invH = 1.0f / normH;
    Hx *= invH;
    Hy *= invH;
    Hz *= invH;
    final float invA = 1.0f / (float)Math.sqrt(Ax*Ax + Ay*Ay + Az*Az);
    Ax *= invA;
    Ay *= invA;
    Az *= invA;
    final float Mx = Ay*Hz - Az*Hy;
    final float My = Az*Hx - Ax*Hz;
    final float Mz = Ax*Hy - Ay*Hx;
    if (R != null) {
        if (R.length == 9) {
            R[0] = Hx;     R[1] = Hy;     R[2] = Hz;
            R[3] = Mx;     R[4] = My;     R[5] = Mz;
            R[6] = Ax;     R[7] = Ay;     R[8] = Az;
        } else if (R.length == 16) {
            R[0]  = Hx;    R[1]  = Hy;    R[2]  = Hz;   R[3]  = 0;
            R[4]  = Mx;    R[5]  = My;    R[6]  = Mz;   R[7]  = 0;
            R[8]  = Ax;    R[9]  = Ay;    R[10] = Az;   R[11] = 0;
            R[12] = 0;     R[13] = 0;     R[14] = 0;    R[15] = 1;
        }
    }

但是,我无法理解 SensorManager.getOrientation()。

 azimuth = (float)Math.atan2(R[1], R[4]);
 pitch = (float)Math.asin(-R[7]);
 roll = (float)Math.atan2(-R[6], R[8]);

获得欧拉角的确切算法是什么?

【问题讨论】:

    标签: android rotation orientation accelerometer magnetometer


    【解决方案1】:

    我将给出getOrientation 的几何解释,并解释如何仅使用RotationMatrix 计算所有类型的旋转。如果您了解getOrientation 的作用,则无需使用它,如果您不了解,则使用它会给您带来各种麻烦。

    具体来说,它将回答许多有关在 Stack Exchange 上发布的方向的问题,例如

    • 在纵向模式下为平板电脑编写指南针。为什么调用getRotation后需要加90度才能得到正确答案。
    • 为什么在调用getOrientation之前必须调用RemapCoordinateSystem才能获取后置摄像头的方向(设备z轴负方向)。
    • 为什么在调用RemapCoordinateSystemgetOrientation 以正确获取设备z 轴方向后,当设备是平的时,结果不再有意义。
    • 如何计算与设备位置无关的任何设备轴的旋转,即平面或非平面。

    首先我需要更详细地解释RotationMatrix,在解释getOrientation之前它能做什么

    这里需要考虑 2 个坐标系。
    一种是世界坐标系,x 轴指向东方,y 轴指向北方,z 轴指向天空。
    另一个是设备坐标系,x 轴是手机的短边(平板电脑的长边),y 轴是手机的长边(平板电脑的短边),z 轴是正交向量指向你的屏幕。

    从数学上讲,我们的对象是 3 维实向量空间,我们正在为这个向量空间使用以下基。

    世界基W = {E, N, SKY} 其中
    E 是位于东方的单位向量
    N 是位于北方的单位向量
    SKY 是位于东方的单位向量在天空方向

    设备基础D = {X, Y, Z} 其中
    X 是手机短边方向的单位向量(平板电脑的长边)
    Y 是手机长边方向的单位向量(平板电脑的短边)
    Z 是一个单位向量,位于与指向您的屏幕正交的方向上

    对于那些忘记什么是基的人来说,这意味着3空间中的每个向量v都可以写成

    在世界基础
    v = a_1 E + a_2 N + a_3 SKY 其中 a_1, a_2, a_3 是实数
    通常写为 (a_1, a_2, a_3)_W 或仅写为 (a_1, a_2, a_3)。这 3 个元组称为 v 相对于 World 基的坐标,或者仅称为 v 的坐标,基隐式理解为 World 基

    在设备基础
    v = b_1 X + b_2 Y + b_3 Z 其中 b_1, b_2, b_3 是实数
    通常写为 (b_1, b_2, b_3)_W 或仅写为 (b_1, b_2, b_3)。这 3 个元组被称为 v 相对于设备基础的坐标,或者只是 v 的坐标,基础隐含地理解为设备基础。传感器返回的值以设备为基础,例如加速度计返回为 values[0],values[1] 和 values[2] 写入设备基础将是
    acc = values[0] X + 值[1] Y + 值[2] Z

    特别是
    X = 1 X + 0 Y + 0 Z
    Y = 0 X + 1 Y + 0 Z
    Z = 0 X + 0 Y + 1 Z

    XYZ相对于设备基准的坐标为(1,0,0),(分别为 0, 1, 0) 和 (0, 0, 1)。稍后您将在getOrientation 中看到如何使用这些向量来解释计算。

    请注意,世界基础是固定的,但设备基础会随着手机位置的变化而变化。即单位向量X,Y,Z随着设备位置的变化而变化。它们以相同的方式定义,但是当位置变化时它们是不同的向量。您仍然将它们写为 X、Y、Z,但它们是不同的 X、Y、Z
    因此,如果手机静止不动,则作用在手机上的唯一力是重力,因此加速度计矢量理论上是位于世界天空轴上的矢量,即在世界基础上,坐标为 (0, 0, g)。在设备基础上,对于某些 a_1、a_2 和 a_3,它是 (a_1, a_2, a_3)。如果设备静止在不同的方向,加速度计仍然与世界基础中的 (0, 0, g) 相同,但现在它在设备基础中为 (b_1, b_2, b_3),其中至少一个 a 与b的。

    getRotationMatrix 的参数包括gravitygeomagneticgravity 假设是真实的 gravity 即它的坐标在世界基础上是 (0, 0, k) 并且地磁假设位于世界 N-Sky 平面上,它的坐标是 (0, a, b ) 在世界基础上。

    有了这些假设,让我们看看Rotation Matrix 是如何在getRotationMatrix 中计算的。它在这种方法中尝试的是获得一个矩阵 M 使得给定任何具有坐标 (a_1, a_2, a_3) 的向量在设备基础上的乘积 M (a_1, a_2 , a_3)_T (transpose) 在世界基础上给出坐标 (b_1, b_2, b_3)。从数学上讲getRotationMatrix计算基矩阵的变化。

    gravity g 的传入参数应该是从onSensorChanged 获得的值,对于TYPE_GRAVITYTYPE_ACCELEROMETER 的低通滤波器和对于geomagnetic m 应该是来自TYPE_MAGNETIC_FIELD 的值

    g = a_1 X + a_2 Y + a_3 Z
    m强> = b_1 X + b_2 Y + b_3 Z

    不失一般性假设 gm 已经归一化,这是 gm 的范数> 等于 1。 我们现在要做的是在设备基础中写入世界基础{E, N, SKY}

    由于 g 被假定为重力,即世界基础上的 (0, 0, 1),这正是矢量 SKY

    天空 = g = a_1 X + a_2 Y + a_3 Z
    SKY在Device基础上的坐标是(a_1, a_2, a_3)

    现在由于 m 被假定在 World N-SKY 平面中,所以 mg 的叉积是一个正交的单位向量到世界 N-SKY 平面并指向右侧,因此这是 E。

    E = m x g = b_1 X + b_2 Y + b_3 Z

    E相对于Device基础的坐标是(b_1, b_2, b_3) b是m的叉积和g

    最后SKYE的叉积是一个与E-SKY平面正交的向量,为N

    N = 天空 x E = c_1 X + c_2 Y + c_3 Z

    我们有

    E = b_1 X + b_2 Y + b_3 Z
    N强> = c_1 X + c_2 Y + c_3 Z
    天空 = a_1 X + a_2 Y + a_3 Z

    因此,给定世界基础中的任何坐标,我们可以找到设备基础中的坐标。例如 (1, 2, 3) 是一个向量 v

    v = E + 2 N + 3 天空

    写入设备坐标将通过代入和乘出

    v = b_1 X + b_2 Y + b_3 Z + 2 (c_1 X + c_2 Y + c_3 Z) + 3 (a_1 X + a_2 Y + a_3 Z)
    v = (b_1 + 2 c_1 + 3 a_1) X + (b_2 + 2 c_2 + 3 a_2) Y + (b_3 + 2 c_3 + 3 a_3) Z

    但是我们真正感兴趣的是相反的,即给定设备基础中的任何坐标,找到世界基础中的坐标。好吧,对于那些不记得线性代数的人来说,在上面我们有 3 个未知数中的 3 个方程,因此我们应该能够解决 XYY 的方程strong>Z 表示 ENSKY

    从数学上讲,将上面的a,b和c放在列中得到的矩阵是基矩阵从世界基到设备基的变化,因此我们需要找到它的逆矩阵。但是这个矩阵是一个正交矩阵,因此它的逆矩阵就是它的转置。

    用代码编写

    a[0]  a[1]  a[2]
    a[3]  a[4]  a[5]
    a[6]  a[7]  a[8]
    

    给定设备基础中的任何坐标(a_1,a_2,a_3),我们可以通过将上面的矩阵与(a_1,a_2, a_3)。

    特别是

    a[0]  a[1]  a[2]      1       a[0]
    a[3]  a[4]  a[5]  x   0   =   a[3]
    a[6]  a[7]  a[8]      0       a[6]
    

    因此X在World基础上的坐标为(a[0], a[3], a[6])

    a[0]  a[1]  a[2]      0       a[1]
    a[3]  a[4]  a[5]  x   1   =   a[4]
    a[6]  a[7]  a[8]      0       a[7]
    

    因此Y在World基础上的坐标为(a[1], a[4], a[7])

    a[0]  a[1]  a[2]      0       a[2]
    a[3]  a[4]  a[5]  x   0   =   a[5]
    a[6]  a[7]  a[8]      1       a[8]
    

    因此Z在World基础上的坐标为(a[2], a[5], a[8])

    现在让我们看看getOrientation 做了什么。

    它的代码是

    azimuth = (float)Math.atan2(R[1], R[4]);
    pitch = (float)Math.asin(-R[7]);
    roll = (float)Math.atan2(-R[6], R[8]);
    

    文件说

    values[0]:方位角,绕-Z轴旋转,即相反 Z轴方向。
    values[1]:俯仰,绕 -X 轴旋转, 即X轴的相反方向。
    values[2]:滚动,旋转 Y 轴。

    从字面上看上面的文件,不考虑设备的位置是错误的。仅当设备是扁平的时,该文档才适用于 azimuth

    在我们继续之前,让我注意所有计算都必须使用相对于同一基础的坐标来完成。例如,如果您想找到 2 个向量之间的角度,则这 2 个向量的坐标必须相对于同一个基。

    现在,当您计算旋转时,您会隐含地理解旋转是与固定位置的偏差。也就是说,如果答案是 90 度,那么它与什么是 90 度?在azimuth 的情况下,它与磁北成 90 度,即您计算与 N 向量的偏差。如果你不能拼出这个隐含的固定位置,你就会遇到很多麻烦。例如,音高的固定向量是什么?卷?设备方向(纵向-横向)?

    让我们看看每个计算,看看它真正计算的是什么。

    对于azimuth官方计算是

    azimuth = (float)Math.atan2(R[1], R[4]); 
    

    让我们回到Y在设备坐标中的写法。如前所述,Y 的坐标在 Device 基中为 (0, 1, 0),Y 在 World 基中的坐标为 (a[1], a [4],一个[7])

    Y投影到世界XY平面的世界坐标系为(a[1], a[4])。这个投影向量和N向量的夹角是(画出投影向量的图和N如果不明白的话)

    Math.atan2(R[1], R[4]);
    

    这正是azimuth 的计算结果。

    因此getOrientation 计算设备 y 轴投影到世界 XY 平面和世界北轴之间的角度。因此,如果设备垂直放置,则此计算没有意义,因为投影的 y 坐标始终为 0。从几何上讲,如果您指向天空,则计算正北方向是没有意义的。此外,在这种情况下,围绕 -Z 轴旋转意味着旋转远离肖像,因此文档不正确。

    对于 Pitch 的官方计算是

    pitch = (float)Math.asin(-R[7]);
    

    Y向量投影到World N-SKY平面的World基坐标为(a[4],a[7]),该投影向量与Y向量在世界EN平面上的投影与Z向量在YZ平面上的投影与重力向量的夹角相同(再次绘制自己拍的)

    Math.asin(-R[7]) 
    

    因此,pitch 是设备 y 轴投影到 World N-SKY 平面与 Y 投影到世界 E-N 平面之间的角度

    同样,Roll 是设备 x 轴投影到 World X-SKY 平面与 X 投影到世界 E-N 平面之间的夹角。

    例如我之前指出的。

    • 在纵向模式下为平板电脑编写指南针。为什么调用getRotation 后需要加上90 度才能得到正确答案。

    在这种情况下,需要计算 x 轴投影到世界 XY 平面之间的角度,因此需要添加 90 度,因为 x 轴和 y 轴之间的角度始终为 90 度。可以取而代之的是获得X 向量在世界基础中的坐标,即 (a[0], a[3], a[6]),然后投影到世界 XY 平面以获得 (a[0], a [3]) 并进行 Math.atan2(R[0], R[3]) 的计算。

    • 为什么在调用getOrientation之前必须调用RemapCoordinateSystem才能获取后置摄像头的方向(设备z轴负方向)。

    在这种情况下,您要计算 z 轴的方向,而不是 getOrientation 计算的 y 轴。因此,您必须调用remapCoordinateSystem,它将 Z 轴映射到 Y 轴。在几何上,您所做的是将 z 轴旋转到 y 轴,现在 y 轴变为 -z 轴。因此,您需要否定旋转矩阵中的 y 列以返回原始定义坐标系。您可以只获取 -Z 的世界坐标,即 (-a[2], -a[5], -a[8]),然后投影到世界 XY 平面得到 (-a[2], -a[5]) 并进行计算。

    注意:我在 2011 年首次使用 Sensor 的类,在了解 getRotationMatrixgetOrientation 的作用后,我创建了一个库来仅使用旋转矩阵进行计算。我不认为remapCoordinateSystemgetOrientation 是必需的,但是直到我写下这个答案我才意识到它们不好用,因为它会给出正确的azymuth,但会给出错误的pitch 值。 remapCoordinateSystem 所做的就是让getOrientation 中的azymuth 的计算按照你要计算的方向正确。但是,pitch 值不正确,您必须为其添加 -90 度以获得正确的后置摄像头方向值以及其他情况下的值。

    如果我要写这个类,我会创建

    float getPitch(float[] rotMatrix)   
    float getRoll(float[] rotMatrix) 
    float getOrientation(float[] rotMatrix, int axis_you_want_to_calculate) 
    

    那么就不会有混淆,所有的答案都是正确的。

    • 为什么在调用RemapCoordinateSystem 然后getOrientation 以正确获取设备z 轴的方向后,当设备平坦时,结果不再有意义。

    您正在尝试计算 -z 轴正北的方向,现在它指向天空,因此计算不再有意义。

    • 如何计算与设备位置无关的任何设备轴的旋转,即平面或非平面。

    明天我将在Get Euler Yaw angle of an Android Device 发布关于绕 z 轴旋转的情况的答案。

    注意:如果你只是想写一个指南针应用并且设备总是平坦的,那么 TYPE_ORIENTATION 还是不错的。

    注意 2:我的绘画很糟糕,因此无法发布任何图片来说明。我什至无法使用 Gimp 画一条直线(我按照说明按住 shift 键,但我的线参差不齐)。如果有人擅长绘画并愿意提供帮助,请发表评论,我将指导我想到的图片插图以及放置它们的位置。

    【讨论】:

      【解决方案2】:

      让我试着解释一下: getRotationMatrix 根据重力和磁矢量组成旋转矩阵。

      我们这里的主要目标是构造NED frame

      我们假设重力指向地球的中心,而磁铁指向北极。但在实际情况下,这些向量是不垂直的,这就是为什么我们首先计算与 E 和 A 正交且属于切平面的向量 H。 H 是叉积 (E x A),与 E 和 A 正交。

      float Hx = Ey*Az - Ez*Ay;
      float Hy = Ez*Ax - Ex*Az;
      float Hz = Ex*Ay - Ey*Ax;
      final float normH = (float)Math.sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
      

      归一化加速度和 H 向量(因为这些向量将构成 ENU 坐标系的基础)

      final float invH = 1.0f / normH;
          Hx *= invH;
          Hy *= invH;
          Hz *= invH;
      final float invA = 1.0f / (float)Math.sqrt(Ax*Ax + Ay*Ay + Az*Az);
          Ax *= invA;
          Ay *= invA;
          Az *= invA;
      

      找到最后一个基向量 (M) 作为 H 和 A 的叉积:

      double Mx = Ay * Hz - Az * Hy;
      double My = Az * Hx - Ax * Hz;
      double Mz = Ax * Hy - Ay * Hx;
      

      任意向量(a)在body frame中的坐标通过NED坐标表示为a = Ra' R - 变换矩阵矩阵,其列是旧基中新基向量的坐标

      但是NED框架中的坐标是 计算为 a' = T^(-1) * a。对于正交变换矩阵逆等于转置矩阵。因此我们有:

      R[0] = Hx;     R[1] = Hy;     R[2] = Hz;
      R[3] = Mx;     R[4] = My;     R[5] = Mz;
      R[6] = Ax;     R[7] = Ay;     R[8] = Az;
      

      一旦我们有了旋转矩阵,我们就可以将其转换为欧拉角表示。转换公式取决于您使用的convention。 你的公式

      azimuth = (float)Math.atan2(R[1], R[4]);
      pitch = (float)Math.asin(-R[7]);
      roll = (float)Math.atan2(-R[6], R[8]);
      

      对于具有约定 Y-X-Z 的 Tiat Bryan 角度是正确的。为了更好地理解从旋转矩阵到欧拉角的转换,我建议学习 Gregory G. Slabaugh - "Computing Euler angles from a rotation matrix"

      的一篇文章

      【讨论】:

        猜你喜欢
        • 1970-01-01
        • 1970-01-01
        • 1970-01-01
        • 2013-10-05
        • 1970-01-01
        • 2020-12-31
        • 2012-12-18
        相关资源
        最近更新 更多