Question

我坚持使用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)) );

有人有任何想法吗？

Answer 1

我有一些不同的计算，我最近基于我之前做过的ruby实现实现了。我手边没有细节，我可以尝试找到它们并稍后添加它们，但我认为你的问题可能是使用atan2作为音高。我认为您的计算基于您链接到的数据表中的以下公式。

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

doc说的那些。

由于等式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);

Answer 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);

Answer 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)

倾斜补偿指南针

3 个答案: