Question

我已经在这个特定问题上浪费了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页显示了方向。当我抬起前面时，音高上升当我抬起左侧时，滚动升起。

示例值：
指向一个方向，我的算法在水平水平时调用305deg

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。

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

我的个人猜测是公式是正确的，原则是在普林西比工作（毕竟我得到适当的度数）但我不知何故喂错了。（像Y和X需要交换，z * -1，音调需要* -1）

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

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

Answer 1

首先，确认它确实是一个软件问题。你的方程似乎是正确的。为了以防万一，生成一个填充了测试数据的表，以传递给您的过程，并在代码正确的情况下将函数的输出与期望值进行比较。

您使用的是磁力计，这些都是非常敏感的设备。我要检查的第一件事是传感器附近是否有任何大型金属结构。如果您可以访问示波器，请探测芯片的电源轨并查看进入其中的电源是否稳定。添加1uF去耦电容可以解决电源问题。我还建议在采样频率大于100Hz时获取图形并查看跳转是否是周期性的。如果信号是50Hz频率的周期性信号，假设传感器未被移动，则表示主电源受到干扰。对数据执行FFT分析不会有害。我们在实验室底层下面运行的电缆造成了类似的问题。如果传感器随机跳动，芯片可能已经死了。处理时是否使用了适当的ESD保护？

希望这有帮助。

Answer 2

3天的工作，最后我发现了我遇到的问题，倾斜补偿工作正常！我在不同的论坛上看过很多人都有这样的问题，所以这就是我所做的：

我一步一步解释我做了什么，为什么做了。也许有类似问题的人会找到解决方案。

使用这些值可能有所帮助（可能只需要翻转俯仰或滚动，必须交换X或Y）。
但是，如果您的问题不仅仅是一个小问题，那么有很多值，并且组合的数量太高。
发布的公式正常（活动#if分支中的那个。）如果你有一个磁力计，并且atan2（-y，x）在平整时给你一个合适的航向，那么这个公式也适用于你。
我所做的是从I2C二进制逻辑（和数据表）开始完全检查所有传感器和向量。
在此特殊情况下很重要（BMX055），数据表方向页面错误！
关于轴（x，y）的方向以及旋转为正或负时存在多个错误。有时右手规则适用，有时不适用，图纸误导（错误）。博世在记录这个芯片（以及之前的芯片）方面做得不好。他们似乎不希望人们正确地理解它，他们用优化的定点算术和高级校准多次写一个融合API，但它不适用于公众。

我需要做的是制作一个合适的身体参考系统。确定自己X的位置然后使其成为所有传感器在倾斜/滚动到相同方向（正/负）时正确地改变X轴。对俯仰，滚转和重力/磁场这样做。

一旦他们一起玩得很好，我就重新开始了标题公式仍然不正常，但现在我第一次信任vextors 我添加了一个矢量旋转矩阵函数，并使用滚动和俯仰以及yaw = 0旋转磁矢量。惊喜：磁矢量被倾斜补偿。

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

我回到原来的公式，用Y替换X （因为我交换了它们以匹配身体参考系统（陀螺/ mag的X和Y）。
现在倾斜补偿适用于俯仰，但不适用于侧倾所以我倒置roll_radians 突然它完全倾斜补偿了。

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

倾斜的磁力计输出 - 节距和滚动倾斜补偿 - 我迷路了

2 个答案: