九轴四元数计算和欧拉角换算

[l1813779684]

最近做惯导的一个项目，需要通过9轴或6轴数据计算欧拉角。
项目实现过程中，采用IMU算法和ARHS进行测试，测试过程中发现很多问题。
九轴数据是通过串口传送到计算机进行处理，通过解析串口协议可以获取到加速度、陀螺仪、磁场九轴的数据，发现以下几个问题：
1.初始数据的加速度和陀螺仪6轴的数据都比较大，磁场3轴的数据则是一个时间一个值，硬件工程师说是零点漂移问题，我也不太懂，不知道是不是这个原因？
2.对原数据进行分析得到四元数，可以发现相邻两个四元数对应位置符号相反
3.我怀疑是他说的零点漂移问题，就将6轴数据相邻两个做差值，作为新的6轴数据，但是计算出的角度变化令人费解，实际角度为90度，而测量出的欧拉角却只有40-60度；
4.角度与实际不相符合，我怀疑是参数设置的原因，我们这边串口是每秒发送10帧数据。于是我就开始测试采样周期，最后在俯仰角找到一个合适的值，但是翻滚角和偏航角的计算角度与实际角度又不相吻合，我瞬间头大了。

不知道是硬件问题还是算法问题？各位有没有相关的经验，期待高手解决！

[l1813779684]

算法如下： package com.dats.cn.algorithm; public class IMUCalculate { float q0=1,q1=0,q2=0,q3=0; float exInt=0,eyInt=0,ezInt=0; float gx=0,gy=0,gz=0,ax=0,ay=0,az=0; public float[] Quaternion=new float[4]; public float Beta; public float SamplePeriod; public float Yaw,Pitch,Rool; //偏航角，俯仰角，翻滚角 public IMUCalculate(float samplePeriod, float beta) { this.SamplePeriod = samplePeriod; this.Beta = beta; this.Quaternion = new float[] { 1f, 0f, 0f, 0f }; } public void IMUUpdate(float gx, float gy, float gz, float ax, float ay, float az){ float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3]; // short name local variable for readability float norm; float s1, s2, s3, s4; float qDot1, qDot2, qDot3, qDot4; // Auxiliary variables to avoid repeated arithmetic float _2q1 = 2f * q1; float _2q2 = 2f * q2; float _2q3 = 2f * q3; float _2q4 = 2f * q4; float _4q1 = 4f * q1; float _4q2 = 4f * q2; float _4q3 = 4f * q3; float _8q2 = 8f * q2; float _8q3 = 8f * q3; float q1q1 = q1 * q1; float q2q2 = q2 * q2; float q3q3 = q3 * q3; float q4q4 = q4 * q4; // Normalise accelerometer measurement norm = (float)Math.sqrt(ax * ax + ay * ay + az * az); if (norm == 0f) return; // handle NaN norm = 1 / norm; // use reciprocal for division ax *= norm; ay *= norm; az *= norm; // Gradient decent algorithm corrective step s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay; s2 = _4q2 * q4q4 - _2q4 * ax + 4f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az; s3 = 4f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az; s4 = 4f * q2q2 * q4 - _2q2 * ax + 4f * q3q3 * q4 - _2q3 * ay; norm = 1f / (float)Math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude s1 *= norm; s2 *= norm; s3 *= norm; s4 *= norm; // Compute rate of change of quaternion qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1; qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2; qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3; qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4; // Integrate to yield quaternion q1 += qDot1 * SamplePeriod; q2 += qDot2 * SamplePeriod; q3 += qDot3 * SamplePeriod; q4 += qDot4 * SamplePeriod; norm = 1f / (float)Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion Quaternion[0] = q1 * norm; Quaternion[1] = q2 * norm; Quaternion[2] = q3 * norm; Quaternion[3] = q4 * norm; Yaw=(float)Math.atan2(2*(Quaternion[0]*Quaternion[1]+Quaternion[2]*Quaternion[3]), 1-2*(Quaternion[1]*Quaternion[1]+Quaternion[2]*Quaternion[2]))*57.3f; Pitch=(float)Math.asin(2*(Quaternion[0]*Quaternion[2]-Quaternion[1]*Quaternion[3]))*57.3f; Rool=(float)Math.atan2(2*(Quaternion[0]*Quaternion[3]+Quaternion[1]*Quaternion[2]), 1-2*(Quaternion[2]*Quaternion[2]+Quaternion[3]*Quaternion[3]))*57.3f; //System.out.println(Quaternion[0]+" "+Quaternion[1]+" "+Quaternion[2]+" "+Quaternion[3]); } public void ARHSUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3]; // short name local variable for readability float norm; float hx, hy, _2bx, _2bz; float s1, s2, s3, s4; float qDot1, qDot2, qDot3, qDot4; // Auxiliary variables to avoid repeated arithmetic float _2q1mx; float _2q1my; float _2q1mz; float _2q2mx; float _4bx; float _4bz; float _2q1 = 2f * q1; float _2q2 = 2f * q2; float _2q3 = 2f * q3; float _2q4 = 2f * q4; float _2q1q3 = 2f * q1 * q3; float _2q3q4 = 2f * q3 * q4; float q1q1 = q1 * q1; float q1q2 = q1 * q2; float q1q3 = q1 * q3; float q1q4 = q1 * q4; float q2q2 = q2 * q2; float q2q3 = q2 * q3; float q2q4 = q2 * q4; float q3q3 = q3 * q3; float q3q4 = q3 * q4; float q4q4 = q4 * q4; // Normalise accelerometer measurement norm = (float)Math.sqrt(ax * ax + ay * ay + az * az); if (norm == 0f) return; // handle NaN norm = 1 / norm; // use reciprocal for division ax *= norm; ay *= norm; az *= norm; // Normalise magnetometer measurement norm = (float)Math.sqrt(mx * mx + my * my + mz * mz); if (norm == 0f) return; // handle NaN norm = 1 / norm; // use reciprocal for division mx *= norm; my *= norm; mz *= norm; // Reference direction of Earth's magnetic field _2q1mx = 2f * q1 * mx; _2q1my = 2f * q1 * my; _2q1mz = 2f * q1 * mz; _2q2mx = 2f * q2 * mx; hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; _2bx = (float)Math.sqrt(hx * hx + hy * hy); _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; _4bx = 2f * _2bx; _4bz = 2f * _2bz; // Gradient decent algorithm corrective step s1 = -_2q3 * (2f * q2q4 - _2q1q3 - ax) + _2q2 * (2f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s2 = _2q4 * (2f * q2q4 - _2q1q3 - ax) + _2q1 * (2f * q1q2 + _2q3q4 - ay) - 4f * q2 * (1 - 2f * q2q2 - 2f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s3 = -_2q1 * (2f * q2q4 - _2q1q3 - ax) + _2q4 * (2f * q1q2 + _2q3q4 - ay) - 4f * q3 * (1 - 2f * q2q2 - 2f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s4 = _2q2 * (2f * q2q4 - _2q1q3 - ax) + _2q3 * (2f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); norm = 1f / (float)Math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude s1 *= norm; s2 *= norm; s3 *= norm; s4 *= norm; // Compute rate of change of quaternion qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1; qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2; qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3; qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4; // Integrate to yield quaternion q1 += qDot1 * SamplePeriod; q2 += qDot2 * SamplePeriod; q3 += qDot3 * SamplePeriod; q4 += qDot4 * SamplePeriod; norm = 1f / (float)Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion Quaternion[0] = q1 * norm; Quaternion[1] = q2 * norm; Quaternion[2] = q3 * norm; Quaternion[3] = q4 * norm; Yaw=(float)Math.atan2(2*(Quaternion[0]*Quaternion[1]+Quaternion[2]*Quaternion[3]), 1-2*(Quaternion[1]*Quaternion[1]+Quaternion[2]*Quaternion[2]))*57.3f; Pitch=(float)Math.asin(2*(Quaternion[0]*Quaternion[2]-Quaternion[1]*Quaternion[3]))*57.3f; Rool=(float)Math.atan2(2*(Quaternion[0]*Quaternion[3]+Quaternion[1]*Quaternion[2]), 1-2*(Quaternion[2]*Quaternion[2]+Quaternion[3]*Quaternion[3]))*57.3f; System.out.println(Quaternion[0]+" "+Quaternion[1]+" "+Quaternion[2]+" "+Quaternion[3]); } }

[l1813779684]

算法参数设置如下： IMUCalculate imu=new IMUCalculate(1f /410f, 0.1f);

[l1813779684]

求高手解答！！！！！！！

[lanlangxt]

没有下文了，怎么

[lanlangxt]

没有下文了，好期待

[lanlangxt]

引用 2 楼 l1813779684 的回复:

算法参数设置如下： IMUCalculate imu=new IMUCalculate(1f /410f, 0.1f);

能分享一下吗，这个是什么意思 // Gradient decent algorithm corrective step s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay; s2 = _4q2 * q4q4 - _2q4 * ax + 4f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az; s3 = 4f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az; s4 = 4f * q2q2 * q4 - _2q2 * ax + 4f * q3q3 * q4 - _2q3 * ay; // Gradient decent algorithm corrective step s1 = -_2q3 * (2f * q2q4 - _2q1q3 - ax) + _2q2 * (2f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s2 = _2q4 * (2f * q2q4 - _2q1q3 - ax) + _2q1 * (2f * q1q2 + _2q3q4 - ay) - 4f * q2 * (1 - 2f * q2q2 - 2f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s3 = -_2q1 * (2f * q2q4 - _2q1q3 - ax) + _2q4 * (2f * q1q2 + _2q3q4 - ay) - 4f * q3 * (1 - 2f * q2q2 - 2f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s4 = _2q2 * (2f * q2q4 - _2q1q3 - ax) + _2q3 * (2f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);

[goinfog]

这个算法就是一个坟墓