倾斜补偿罗盘

Posted 2023-03-05

【中文标题】倾斜补偿罗盘

【英文标题】：Tilt compensated compass

【发布时间】：2018-03-13 11:19:44

【问题描述】：

我坚持使用 arduino 成功倾斜补偿我的 9DOF IMU。我知道这不是常见的语法问题，而是一种数学问题，但有人可以帮忙吗？

问题是当我以滚动和/或俯仰方式移动传感器同时保持相同的偏航方向时，偏航角不会保持应有的恒定。

float Ax; float Ay; float Az        // these are raw accelerometer readings
float Magx; float Magy; float Magz        // these are raw magnetometer readings
float RollAngle; float PitchAngle; float YawAngle;       // these are calculated angles from raw readings

RollAngle = atan2(Ay,Az);

PitchAngle = atan2((-Ax),((Ay*sin(RollAngle)) + Az*cos(RollAngle)));

YawAngle = atan2( (Magz*sin(RollAngle)-Magy*cos(RollAngle)) , ((Magx*cos(PitchAngle))+(Magy*sin(PitchAngle)*sin(RollAngle))+Magz*sin(PitchAngle)*cos(RollAngle)) );

有人有什么想法吗？

【问题讨论】：

您应该所有您的计算仅基于原始读数Ax, Ay和aZ。但是您使用计算值 Roll in Pitch 和 Yaw。

嗨，保罗。感谢您的回复。我不知道你打算如何做到这一点。滚动角和俯仰角必须从原始 Ax、Ay 和 Az 计算出来，然后在下面的等式中用作 phi 和 theta。还是我把它弄反了/错了？请参阅变量声明和 cmets 中的 EDIT

这是我用作指导的数学。 cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf

如果你确定数学没问题，那么我很抱歉。但我看到计算中可能存在循环性，其中顺序会影响结果。

【参考方案1】：

我有一些不同的计算，我最近根据我以前做过的一个 ruby​​ 实现来实现这些计算。我手头没有详细信息，我可以尝试找到它们并稍后添加它们，但我认为您的问题可能是使用 atan2 进行音高。 我认为您的计算基于您链接到的数据表中的以下方程式。

tan(φ) = (Gpy/Gpx)  // eq 13
tan(θ) = (-Gpx / (Gpy sin(φ) + Gpz cos(φ))  // eq 15

文档所说的地方。

由于方程 13、15 和 22 在 360°的倍数，限制解是标准约定 滚动、俯仰和偏航范围为 -180° 至 180°。进一步 对俯仰角施加约束以将其限制在 -90° 范围内 至 90°。这确保了指南针只存在一个唯一的解决方案， 任何手机方向的俯仰和滚动角度。等式 13 和 22 因此使用软件 ATAN2 函数计算（输出 角度范围 -180° 到 180°) 和公式 15 用软件计算 ATAN功能（输出角度范围-90°到90°）。

如果您有兴趣，这是我的计算。 （正如我所说，我没有手头的文档来说明我是如何想出这些的。

float xM = magCurrent[0] - magOffsets[0];
float yM = magCurrent[1] - magOffsets[1];
float zM = magCurrent[2] - magOffsets[2];
float heading = (atan2(yM, xM) * 180) / Pi;
if (heading < 0) 
  heading = 360 + heading;

logger.infoln("Compass Heading: %f", heading);
float g = 9.8;
float xG = (accelCurrent[0] - accelOffsets[0]) / g; 
float yG = (accelCurrent[1] - accelOffsets[1]) / g; 
float zG = (accelCurrent[2] - accelOffsets[2]) / g;
float pitch = atan2(-xG, sqrt(yG * yG + zG * zG));
float roll = atan2(yG, zG);
logger.infoln("pitch: %f", pitch * 180 / Pi);
logger.infoln("roll: %f", roll * 180 / Pi);
float xM2 = xM * cos(pitch) + zM * sin(pitch);
float yM2 = xM * sin(roll) * sin(pitch) + yM * cos(roll) - zM * sin(roll) * cos(pitch);
float compHeading = (atan2(yM2, xM2) * 180 / Pi);
if (compHeading < 0) 
  compHeading = 360 + compHeading;

logger.infoln("tilt compensated Compass Heading: %f", compHeading);

【讨论】：

嗨 nPn。感谢您的回复。我已经实现了您的代码，并且遇到了同样的问题。但我没有像你建议的那样限制欧拉角。我只是按原样获取您的代码并输入我的变量。言归正传，我看到您使用 ATAN2 进行所有角度计算？您是否在其他地方限制了变量？您上面的代码是否提供了完全倾斜补偿的航向？

我已经测试了你的代码并且得到同样错误的结果。我检查了角度是否都受到限制。老实说，我已经没有想法了。

【参考方案2】：

尝试对您的代码进行以下更改：

float pitch = atan2(-xG, zG);

float roll = atan2(yG, zG);

...

float xM2 = xM * cos(pitch) + zM * sin(pitch);

float yM2 = yM * cos(roll) - zM * sin(roll);

【参考方案3】：

有时加速度计和磁力计不在同一个坐标内，您应该首先定义一个变换坐标以将磁力计坐标映射到加速度计坐标。就像是： modeling and control mark W.spong 那么你应该考虑上面的变换坐标来计算倾斜变换： R = R1（坐标变换）*R2（倾斜变换） 但通常，您可以在倾斜转换中随机否定项：

double xhorizental = (-/+)x*cos(pitch) (-/+)y*sin(pitch)*sin(roll) 
                     (-/+)z*sin(pitch)*cos(roll);
double yhorizental = (-/+)y*cos(roll) (-/+)z*sin(roll);

heading = atan2(yhorizental,xhorizantal) (+PI)