2

我在这个特定问题上浪费了 2 天（和晚上），并且在倾斜补偿我的磁力计输出方面完全失败。
我尝试了我在谷歌和开源示例中可以阅读的所有内容，没有任何东西可以指导我对两者（滚动和俯仰）进行适当的倾斜补偿。.

设置：我使用校准值而不是统一值。
我有一个融合的重力矢量，它工作正常且准确。
传感器是 9dof BMX055 ( http://www.mouser.com/ds/2/621/BST-BMX055-DS000-01-274824.pdf ) 每个轴上的磁力计最小值/最大值为 +- 512 (小差异，但所有轴都归零）。硬件是 Sam3X8e (Cortex M3)，全部用 C 语言编写。
浮点和三角函数可以很快完成，所以没问题。BMX055 芯片对齐，使引脚 20、19、18 指向前面。
数据表第 159-161 页显示了方向。当我上升前，音高上升。
当我抬起左侧时，滚动上升。

示例值：
指向我的算法在水平调平时调用 305 度的方向

Pitch: 0  , Roll 0   : MAG cal:  x 132   y  93  z   -364  
Pitch: +24, Roll 0   : MAG cal:  x -109   y  93  z   -397  
Pitch: +46, Roll 0   : MAG cal:  x -303   y  89  z   -351  
Pitch: 0  , Roll -44 : MAG cal:  x 151   y  352  z   -235  
Pitch: 0  , Roll +36 : MAG cal:  x 130   y  -140  z   -328  
Pitch: 78 , Roll -2  : MAG cal:  x -503   y  93  z   -199  
Pitch: 7  , Roll -53 : MAG cal:  x 135   y  424  z   -180

对齐应该始终在 305 度左右（尽我所能），也许 +- 5 度。

公式：（到处都用同一个）

uint16_t compass_tilt_compensation(float roll_radians, float pitch_radians,float mag_y, float mag_x, float mag_z) 
{

  float tilt_compensated_heading;
  float MAG_X;
  float MAG_Y;
  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;
  int16_t heading;
  //pitch_radians =roll_radians;
  //roll_radians *= -1;
  //mag_x *= -1;
  //roll_radians=0.0f;
  //pitch_radians=0;v
  //pitch_radians *=-1;

  cos_roll = cos(roll_radians);
  sin_roll = sin(roll_radians);
  cos_pitch = cos(pitch_radians);
  sin_pitch = sin(pitch_radians);


#if 0
  MAG_X = mag_x*cos_pitch+mag_y*sin_roll*sin_pitch+mag_z*cos_roll*sin_pitch;
  MAG_Y = mag_y*cos_roll-mag_z*sin_roll;
  tilt_compensated_heading = atan2f(-MAG_Y,MAG_X); 
#else
    MAG_X = mag_x * cos_pitch + mag_z * sin_pitch;
    MAG_Y = mag_x * sin_roll * sin_pitch + mag_y * cos_roll - mag_z * sin_roll * cos_pitch;
    tilt_compensated_heading = atan2f(-MAG_Y,MAG_X);
    //tilt_compensated_heading = atan2f(-mag_y,mag_x); // works fine when leveled
#endif

//convert to degree from 0-3599
    heading = tilt_compensated_heading * RAD_TO_DEG * 10;
     if ( heading < 0 ) 
     {
         heading += 3600;
     }

  return heading;
}

我尝试了各种组合，我尝试只修复一个轴并将一个轴始终保持为 0，在任何输入上交换 X/Y 俯仰/滚动，*-1。

结果总是完全错误的。有时（取决于我尝试通过试验/错误来反转或不反转值的位置）这些值似乎几乎是线性的。有时一个轴在正区域得到补偿。
然而，滚动和俯仰总是会导致“随机”跳跃和航向变化。
数学从来不是我最喜欢的，现在我后悔了。

我个人的猜测是这个公式在principe是正确的，mag在principe中工作（毕竟我在练级时获得了适当的学位）但我不知何故喂了一些错误的东西。（如Y和X需要互换，z*-1，pitch需要*-1）

在这个主题上擅长的人可以看看并指导我如何获得正确的标题吗？
今晚能睡几个小时而不必再梦见一个功能失调的算法会很棒:)

更新：此处的倾斜补偿适用于指向 305 度航向时的负滚动。这里也用到了：http ://www.emcu.it/MEMS/Compass/MEMS_Compass_A_RTM1v3.pdf

4

2 回答 2

2

3天的工作，终于找到了我遇到的问题，倾斜补偿正在起作用！
我在各种论坛上读到很多人都有这样的问题，所以这就是我所做的：

我一步一步地解释我做了什么以及我为什么这样做。也许有类似问题的人会以这种方式找到解决方案。

  1. 使用这些值可能会有所帮助（可能只是需要颠倒俯仰或滚动，必须交换 X 或 Y）。
    但是，如果您的问题不仅仅是一个小问题，那么有很多值并且组合的数量太高。

  2. 发布的公式工作正常（在活动的#if 分支中的那个。
    如果你有一个磁力计，并且 atan2(-y,x) 在水平平整时给你一个正确的航向，那么这个公式也适用于你。

  3. 我所做的是从 I2C 二进制逻辑（和数据表）开始彻底检查所有传感器和矢量。
    在这种特殊情况下（BMX055）很重要，数据表方向页是错误的！
    关于轴 (x,y) 的方向以及旋转为正或负时存在多个错误。有时适用右手法则，有时则不适用，并且图纸具有误导性（错误）。博世在记录这个芯片（以及之前的芯片）方面做得不好。他们似乎不希望人们正确理解它，他们多次写了一个带有优化定点算法和高级校准的融合 API，但它不向公众开放。

我需要做的是建立一个合适的身体参考系统。确定 X 在哪里，然后让它在向相同方向（正/负）俯仰/滚动时正确更改 X 轴。对俯仰、滚动和重力/磁场执行此操作。

一旦他们一起玩得很好，我就重新开始了。
航向公式仍然无法正常工作，但现在我第一次信任了攻击者。
我添加了一个矢量旋转矩阵函数，并使用 roll 和 pitch 以及 yaw=0 将磁矢量旋转回来。惊喜：磁矢量被倾斜补偿。

现在我知道它可以做到:)

我回到原来的公式，用 Y 代替 X（因为我已经交换了它们以匹配身体参考系统（陀螺仪/磁力的 X 和 Y）。
现在倾斜补偿适用于俯仰，但不适用于滚动。
所以我反转了 roll_radians和突然间，它得到了完美的倾斜补偿。

我现在有两个解决方案。一种是旋转矩阵解决方案，另一种是每个人都在使用的标准解决方案。我会看看其中一个是否表现更好，如果结果值得，可能会在此处提供最后一次更新。

于 2014-07-09T02:39:59.367 回答
0

首先，验证它确实是软件问题。你的方程式似乎是正确的。以防万一，生成一个填充有测试数据的表以传递给您的过程，并将函数的输出与预期值进行比较（如果代码正确）。

您正在使用磁力计，这些是非常敏感的设备。我要检查的第一件事是传感器附近是否有任何大型金属结构。如果您可以使用示波器，请探测芯片的电源轨，看看进入它的电源是否稳定。添加一个 1uF 去耦电容可以解决电源问题。我还建议在采样频率大于 100Hz 时获取图表，并查看跳跃是否是周期性的。如果信号是 50Hz 频率的周期性信号，假设传感器没有移动，则表明来自主电源的干扰。对数据执行 FFT 分析不会有什么坏处。我们的实验室地板下运行的电源线也引起了类似的问题。如果传感器一直随机跳动，芯片可能已经死了。您在处理它时是否使用了适当的 ESD 保护？

希望这可以帮助。

于 2014-07-08T20:03:03.290 回答