java - 如何根据加速度估计角度以及根据 3 维角度估计加速度？

Question

我在很多论坛上看到了几个与此相关的问题，但没有一个问题能够让我足够清楚或足够精确地满足我的需求。或者我只是不理解它们，这是另一种可能性。

好吧，考虑设备(不是 Android 设备，这里不需要实时处理)采取的几个加速度计测量(3 维)(m/s^2)。我正在从文本文件中读取这些值，并且需要将它们转换为度数。

我尝试的第一个代码是:

   private static final double G = 9.81;
   private static final double RAD_TO_DEG = 180.0 / Math.PI;
    
   private double[] calculateAngles(double acX, double acY, double acZ) {
        
       double accelX = acX * G;
       double accelY = acY * G;
       double accelZ = acZ * G;

       double angleX = Math.atan2(accelX, Math.sqrt(accelY * accelY + accelZ * accelZ)) * RAD_TO_DEG;
       double angleY = Math.atan2(accelY, Math.sqrt(accelX * accelX + accelZ * accelZ)) * RAD_TO_DEG;
       double angleZ = Math.atan2(Math.sqrt(accelX * accelX + accelY * accelY), accelZ) * RAD_TO_DEG;
        
       return new double[] { angleX, angleY, angleZ };
   }

这段代码“似乎”给了我一个很好的结果。至少，可信。我将加速度计放在固定位置，注意到随机测量给出的 3 个角度(更新:我说的是偏航、俯仰和滚动)，转动加速度计90°(大约)，其中一个角度将其值增加了或多或少 90°。

尽管如此，我尝试的逆运算根本不起作用:

private static final double RAD_TO_DEG = 180.0 / Math.PI;
private static double[] calculateAccelerations(double angleX, double angleY, double angleZ) {

        // Convertir les angles en radians
        double radianAngleX = angleX * DEG_TO_RAD;
        double radianAngleY = angleY * DEG_TO_RAD;
        double radianAngleZ = angleZ * DEG_TO_RAD;

        double accelX = Math.tan(radianAngleX) / Math.sqrt(1.0 + Math.tan(radianAngleX) * Math.tan(radianAngleX) + Math.tan(radianAngleY) * Math.tan(radianAngleY));
        double accelY = Math.tan(radianAngleY) / Math.sqrt(1.0 + Math.tan(radianAngleX) * Math.tan(radianAngleX) + Math.tan(radianAngleY) * Math.tan(radianAngleY));
        double accelZ = Math.tan(radianAngleZ) * Math.sqrt(1.0 + Math.tan(radianAngleX) * Math.tan(radianAngleX) + Math.tan(radianAngleY) * Math.tan(radianAngleY));

        return new double[]{accelX/G, accelY/G, accelZ/G};
    }

如果我获取初始加速度计值，估计偏航、俯仰和滚动，将它们转换为加速度...怎么说...发明这些值会更准确。第一个问题是……为什么？我在这里看不到我的错误。

在谷歌上搜索替代方案，卡尔曼滤波器是我能找到的最相关的结果。我在 common-math3 (Apache) 中找到了一个实现，但我不明白它是如何工作的，也不明白它是否真的适用于此。

我需要读取这些加速度计值，并估计加速度计以不同方式定位时的这些值。为此，我需要能够“估计”加速度计的位置，添加需要添加的角度以实际改变其方向，然后返回加速度计值。

最好是 Java 建议，但欢迎任何帮助，包括不可知的解释!

Answer 1

首先，您想要测量什么角度？根据您在 calculateAngles 中的计算，您似乎正在计算的是:

angleX

“x”加速度计测量值与地板之间的角度。 (也就是说，当“x”测量垂直指向上方时，您得到 90°，当“x”测量指向地平线时，您得到 0°，当“x”测量指向地板时，您得到 -90 °)

angleY

“y”加速度计测量值与地面之间的角度。 (也就是说，当“y”测量笔直向上时，您会得到 90° 等，与“x”相同)

angleZ

“z”加速度计测量值与直线之间的角度。 (也就是说，当“z”测量垂直指向上方时，您得到 0°，当“z”测量指向地平线时，您得到 90°，当“z”测量指向地板时，您得到 180° )

鉴于您的后续评论，我将假设这些实际上是您希望测量并从那里继续的角度。

现在，由于您在该方法中使用的函数 (atan2)，将所有三个加速度测量值乘以同一个常数不会产生任何效果；因此，在calculateAngles中可以省略* G;步骤。

首先，我将该函数更改为:

   private double[] calculateAngles(double acX, double acY, double acZ) {
       double angleX = Math.atan2(acX, Math.sqrt(acY * acY + acZ * acZ)) * RAD_TO_DEG;
       double angleY = Math.atan2(acY, Math.sqrt(acX * acX + acZ * acZ)) * RAD_TO_DEG;
       double angleZ = Math.atan2(Math.sqrt(acX * acX + acY * acY), acZ) * RAD_TO_DEG;
        
       return new double[] { angleX, angleY, angleZ };
   }

现在，为了反转计算，我们需要利用这样一个事实:完成后，三个加速度的平方和应等于 G 平方。另外，我们想利用这样的事实来反转

 angle = Math.atan2(y, x);

您通常不应使用 tan 函数，而应使用:

 x = Math.cos(angle);
 y = Math.sin(angle);

 // then multiple x and y by some scaling factor

所以:

private static double[] calculateAccelerations(double angleX, double angleY, double angleZ) {

    // Convertir les angles en radians
    double radianAngleX = angleX * DEG_TO_RAD;
    double radianAngleY = angleY * DEG_TO_RAD;
    double radianAngleZ = angleZ * DEG_TO_RAD;

    double acX = Math.sin(radianAngleX);
    double acY = Math.sin(radianAngleY);
    double acZ = Math.cos(radianAngleZ);

    // use this if you need the sum of the acceleration squared to be G squared
    //double scaling = G / Math.sqrt(acX*acX + acY*acY + acZ*acZ);
    // EDIT Cheloute: with scaling = 1, that works perfectly for my use case
    double scaling = 1;

    return new double[]{acX*scaling, acY*scaling, acZ*scaling};
}

请注意，尽管 calculateAccelerations 函数会返回任意三个输入角度的值，但这并不意味着任何三个角度组合实际上都是有效的；例如，根据您设置测量的方式，angleY 仅限于 -abs(abs(angleX)-90) 和 abs( abs(angleX)-90)，并且给定 angleX 和 angleY，我很确定可以确定 angleZ 或至少将其范围缩小到只有两种可能性。

但是，如果您输入来自 calculateAngles 的 calculateAccelerations 角度，您应该会得到原始加速度，或者足够接近。