基于加速度计和陀螺仪的四元数角计算

Question

我一直在使用6 6dof LSM6DS0 IMU单元(带有加速度计和陀螺仪)。我试着计算三个轴的旋转角度。我试过可能的方法，但没有得到预期的结果。

尝试过的方法：

(i)互补滤波方法--我能够使用链接角坐标法中提供的公式得到角度。

但问题是，角度一点也不一致，而且经常漂移。此外，当IMU围绕一个轴旋转时，计算出的另一个轴上的角度会摆动过大。

(2)基于四元数的角度计算:有大量资源声称四元数方法很好地计算了角，但没有一种有明确的解释。我使用这种方法来更新从IMU单元获取的每个值的四元数。但是链接力解释了如何从四元数计算角度。

为了将四元数转换为欧拉角，我使用了glm数学库，并尝试了wiki链接中指定的公式。使用这种方法，因为在音高计算中，只返回-90到+90度，我无法像他们在上述链接中所做的那样，在3D中旋转对象。

有人试过四元数转换角度吗？？我需要计算出在0到360度或-180到+180度范围内的所有三轴的角度。

任何帮助都是值得感激的。提前谢谢。

Answer 1

http://davidegironi.blogspot.com.ar/2013/02/avr-atmega-mpu6050-gyroscope-and.html#.VzjDKfnhCUk

  Sq = [q0, q1, q2, q3]
  //sensor quaternion can be translated to aerospace sequence of yaw/pitch/roll angles
  yaw = atan2(2*q1*q2 - 2*q0*q3, 2*(q0^2) + 2*(q1^2) - 1)
  pitch = -asin(2*q1*q3 + 2*q0*q2)
  roll = atan2(2*q2*q3 - 2*q0*q1, 2*(q0^2) + 2*(q3^2) - 1)