LSM9DS0上的Madgwick传感器融合 [英] Madgwick sensor fusion on LSM9DS0

问题描述

我正在尝试实施Madgwick传感器融合算法从这里在STM Cortex M3微控制器上的LSM9DS0传感器（加速度计，陀螺仪和磁力计）上。

所有传感器的原始数据似乎都很好。 p>

我的问题是：当我将传感器的z轴水平或向下放置时（即，横滚角或俯仰角大于90度）-滤镜中的四元数实际上变得不稳定且随机翻转180度。
更正确地说，q0和q3不断变化的符号，导致旋转180度翻转。

我尝试使用常量值而不是实际的传感器输出，

当z轴或多或少垂直时，四元数似乎是合理的。

在示例视频中看不到任何类似的东西。

我试图忽略磁力计数据并使用6DOF版本的过滤器，但这是一场灾难。四元数正飞来飞去，无法控制地旋转。

我必须将beta参数设置得很大（〜100），因为较小的值会导致突然翻转产生非常不稳定的结果。我确实发现这很奇怪，因为通常beta约为0.5-0.05。

过滤器更新频率为1KHz。

谁能帮我吗？

解决方案

这里据说madgwick代码有误

渐变体面步骤应如下所示：

  s0 = -_2q2 *（2 *（q1q3-q0q2）-ax）+ _2q1 *（2 *（q0q1 + q2q3）-ay）+ -__ 4bz * q2 *（_ 4bx *（0.5-q2q2-q3q3）+ _4bz *（q1q3 -q0q2）-mx）+（-_4bx * q3 + _4bz * q1）*（_ 4bx *（q1q2-q0q3）+ _4bz *（q0q1 + q2q3）-my）+ _4bx * q2 *（_ 4bx *（q0q2 + q1q3） + _4bz *（0.5-q1q1-q2q2）-mz）; 
 s1 = _2q3 *（2 *（q1q3-q0q2）-ax）+ _2q0 *（2 *（q0q1 + q2q3）-ay）+ -4 * q1 *（2 *（0.5-q1q1-q2q2）- az）+ _4bz * q3 *（_ 4bx *（0.5-q2q2-q3q3）+ _4bz *（q1q3-q0q2）-mx）+（_4bx * q2 + _4bz * q0）*（_ 4bx *（q1q2-q0q3）+ _4bz * （q0q1 + q2q3）-我的）+（_4bx * q3-_8bz * q1）*（_ 4bx *（q0q2 + q1q3）+ _4bz *（0.5-q1q1-q2q2）-mz）; 
 s2 = -_2q0 *（2 *（q1q3-q0q2）-ax）+ _2q3 *（2 *（q0q1 + q2q3）-ay）+（-4 * q2）*（2 *（0.5-q1q1- q2q2）-az）+（-_8bx * q2-_4bz * q0）*（_ 4bx *（0.5-q2q2-q3q3）+ _4bz *（q1q3-q0q2）-mx）+（_ 4bx * q1 + _4bz * q3）*（ _4bx *（q1q2-q0q3）+ _4bz *（q0q1 + q2q3）-my）+（_ 4bx * q0-_8bz * q2）*（_ 4bx *（q0q2 + q1q3）+ _4bz *（0.5-q1q1-q2q2）-m ; 
 s3 = _2q1 *（2 *（q1q3-q0q2）-ax）+ _2q2 *（2 *（q0q1 + q2q3）-ay）+（-__ 8bx * q3 + _4bz * q1）*（_ 4bx *（0.5 -q2q2-q3q3）+ _4bz *（q1q3-q0q2）-mx）+（-_ 4bx * q0 + _4bz * q2）*（_ 4bx *（q1q2-q0q3）+ _4bz *（q0q1 + q2q3）-my）+（_ 4bx * q1）*（_ 4bx *（q0q2 + q1q3）+ _4bz *（0.5-q1q1-q2q2）-mz）; 
  

，官方网站上的代码已过时，很快将被替换。

更正此结果产生了令人满意的结果。

我的另一个错误是未正确读取函数原型。我复制了稍有变化的代码版本，其中加速度计和陀螺仪的值互换了。

I'm trying to implement Madgwick sensor fusion algorithm from here on LSM9DS0 sensor (accelerometer, gyroscope and magnetometer) on STM Cortex M3 microcontroller.

Raw data from all sensors seems to be fine.

My problem is: when I hold sensor with it's z-axis horizontal or downwards (i.e. roll or pitch angle is more than 90 degrees) - quaternion from filter becomes really unstable and randomly flips 180 degrees. More correctly, q0 and q3 are constantly changing signs, resulting in 180 degree flip of rotation.

I tried using constant values instead of real sensor output and still got this behavior.

When z-axis is more or less vertical, quaternion seems plausible.

I haven't seen anything like this on example videos.

I tried to ignore magnetometer data and use 6DOF version of filter but it was a disaster; quaternion is just flying around and spinning uncontrollibly.

I have to set beta-parameter quite large (~100) because lesser values have given very shaky result with sudden flips. I do find this strange since usually beta is about 0.5-0.05.

Filter update frequency is 1KHz.

Can anyone please help me?

解决方案

Here is said that there is a mistake in code of madgwick filter!

Gradient decent step should look like this:

s0= -_2q2*(2*(q1q3 - q0q2) - ax)    +   _2q1*(2*(q0q1 + q2q3) - ay)   +  -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   +   (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)    +   _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s1= _2q3*(2*(q1q3 - q0q2) - ax) +   _2q0*(2*(q0q1 + q2q3) - ay) +   -4*q1*(2*(0.5 - q1q1 - q2q2) - az)    +   _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)   +   (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);             
s2= -_2q0*(2*(q1q3 - q0q2) - ax)    +     _2q3*(2*(q0q1 + q2q3) - ay)   +   (-4*q2)*(2*(0.5 - q1q1 - q2q2) - az) +   (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s3= _2q1*(2*(q1q3 - q0q2) - ax) +   _2q2*(2*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);

and that code on official site is outdated and soon will be replaced.

Correcting this produced a satisfying result.

My other mistake was not reading function prototype properly. I copied slightly changed version of the code, where accelerometer and gyroscope values were interchanged.

这篇关于LSM9DS0上的Madgwick传感器融合的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！