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

问题描述

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

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.

我的问题是:当我拿着传感器的 z 轴水平或向下(即滚动或俯仰角超过 90 度)时 - 来自滤波器的四元数变得非常不稳定并随机翻转 180 度.更准确地说，q0 和 q3 不断变化符号，导致旋转 180 度翻转.

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.

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

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

我没有在示例视频中看到过这样的内容.

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

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

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.

我必须将 beta 参数设置得非常大(~100)，因为较小的值会在突然翻转时产生非常不稳定的结果.我确实觉得这很奇怪，因为通常测试版约为 0.5-0.05.

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.

滤波器更新频率为 1KHz.

Filter update frequency is 1KHz.

有人可以帮我吗?

推荐答案

这里是说madgwick过滤器的代码有错误！

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屋！