MPU6050+HMC5883L IMU融合姿态,YAW为何还是飘??
无论陀螺仪还是加速度,电子罗盘都校准过,但是最后YAW偏航角还是会飘,慢的几秒一度,快的一秒一度..检查过好多次,没发现哪里有问题..
//IMU四元素
void Calc_Quaternions(_Triaxial_f acc, _Triaxial_f gyro, _Triaxial_f mag)
{
float ax = acc.x,ay = acc.y,az = acc.z;
float gx = gyro.x,gy = gyro.y,gz = gyro.z;
float mx = mag.x,my = mag.y,mz = mag.z;
// float mx = 0,my = 0,mz = 0;
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
float deltaT;
if(ax*ay*az==0)
return;
deltaT = getDeltaT(GetSysTime_us());
//ÖØÁ¦¼ÓËٶȹéÒ»»¯
norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
//´ÅÁ¦¼Æ¹éÒ»»¯
norm = invSqrt(mx*mx* + my*my + mz*mz);
mx = mx * norm;
my = my * norm;
mz = mz * norm;
hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
//ÌáÈ¡ËÄÔªÊýµÄµÈЧÓàÏÒ¾ØÕóÖеÄÖØÁ¦·ÖÁ¿
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
//ÏòÁ¿²æ»ýµÃ³ö×Ë̬Îó²î
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
{
exInt = exInt + ex * Ki * halfT*deltaT;
eyInt = eyInt + ey * Ki * halfT*deltaT;
ezInt = ezInt + ez * Ki * halfT*deltaT;
//»¥²¹Â˲¨£¬×Ë̬Îó²î²¹³¥µ½½ÇËÙ¶ÈÉÏ£¬ÐÞÕý½ÇËٶȻý·ÖƯÒÆ
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
}
//Ò»½×Áú¸ñ¿âËþ·¨¸üÐÂËÄÔªÊý
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT*deltaT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT*deltaT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT*deltaT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT*deltaT;
//ËÄÔªÊý¹éÒ»»¯
norm = invSqrt(q0q0 + q1q1 + q2q2 + q3q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
//ËÄÔªÊýתŷÀ½Ç
Angle.x = atan2f(2.0f * q2q3 + 2.0f * q0q1, -2.0f * q1q1 - 2.0f * q2q2 + 1) * RAD_TO_DEG;// ·¹ö
Angle.y = -safe_asin(-2.0f * q1q3 + 2.0f * q0q2) * RAD_TO_DEG; // ¸©Ñö
Angle.z = -atan2f(2.0f*q1q2 + 2.0f*q0q3, -2.0f *q2q2 - 2.0f*q3q3 + 1) * RAD_TO_DEG;//Æ«º½
} 不好说是什么问题，我之前用6050没有用磁力计刚开始yaw有点飘，后面就趋于稳定，估计和代码有点关系。你是用32还是用其他来读取的呢？ 同样遇到这样的问题，用的MPU9150，没有使用DMP。原始数据融合的Pitch和Roll特别的稳定，就Yaw会一直漂移，情况和你的一样。可能是代码哪里没有搞清楚，还在看，可以一起交流。 orphanping 发表于 2016-2-20 12:06
同样遇到这样的问题，用的MPU9150，没有使用DMP。原始数据融合的Pitch和Roll特别的稳定，就Yaw会一直漂移， ...
你们的pitch和roll在板子晃动的时候不会抖吗？比如pitch角从0变到-20，我的会往下跳很多再恢复，能不能一起交流下啊-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-3
-2.95
-2.96
-2.96
-2.96
-2.97
-2.97
-2.97
-2.98
-2.98
-2.98
-2.98
-2.98
-2.98
-2.55
-2.44
-2.49
-2.53
-2.58
-2.5
-2.5
-2.54
-2.61
-2.74
-2.58
-2.29
-1.85
-57.9
-51.96
-74.21
86.2
-29.03
67.96
14.94
-9.64
-25.64
-24.77
-20.87
-20.8
-24.79
-28.19
-30.21
-20.47
-19.58
86.16
50.4
28.31
-7.21
-41.62
-52.05
-55.52
-58.09
-60.04
-58.5
-55.28
-52.74
-50.52
-48.64
-46.97
-47.71
-46.06
-44.47
-42.41
-40.52
-38.79
-37.75
-36.75
-35.38
-34.47
-33.61
-32.51
-31.73
-30.96
-30.07
-29.36
-26.32
-23.51
-23.27
-23.07
-22.82
-22.63
-22.39
-22.25
-19.83
-17.55
-15.43
-15.79
-16.15
-16.47
-16.71
-16.98
-17.16
-17.31
-17.41
-17.51
-17.63
-17.8
-17.96
-18.11
-18.15
-18.33
-18.34
-18.47
-18.58
-18.68
-18.72
-18.72
-18.86
-16.7
-17
-17.13
-17.29
-17.41
-17.65
-17.83
-18.04
-18.23
-18.3
-18.47
-18.63
-18.72
-16.57
-16.83
-17.12
-17.3
-17.45
-17.69
-17.82
-18.03
-18.22
-18.33
-18.4
-18.44
-18.6
-18.75
-18.77
-18.83
-18.94
-19.06
-19.17
-16.99
-14.93
-15.27
-15.67
-16.05
-16.4
-16.66
-16.89
-17.08
-17.35
-15.32
-15.72
-16.1
-16.4
-16.68
-16.93
-17.15
-17.33
-17.58
-17.72
-17.99
-18.19
-18.37
-18.55
-18.7
-18.84
-16.71
-17
-17.28
-17.47
-17.7
huxiaoyingying 发表于 2016-2-26 16:43
你们的pitch和roll在板子晃动的时候不会抖吗？比如pitch角从0变到-20，我的会往下跳很多再恢复，能不能一 ...
Pitch和Roll倒是很稳定的，只是Yaw会一直漂移。 orphanping 发表于 2016-3-4 10:48
Pitch和Roll倒是很稳定的，只是Yaw会一直漂移。
我的今天调好了，yaw也不会飘了，我之前的问题在于陀螺仪数据没有做单位换算，四元数参加运算的陀螺仪数据需要是弧度/s，之前一直用的度/s! Yaw会飘的原因是你的gz一直在抖吧！ 楼主问题解决了么？我也遇到了同样的问题{:sad:}{:sad:}{:sad:} 是啊，楼主解决了吗？ MPU6050+HMC5883L IMU融合姿态，这个不是都是校正吗？ 以前做的姿态，没有使用HMC5883L也是飘，没有办法。 yaw需要做初始对准 tiandihuanyu 发表于 2018-8-1 10:32
yaw需要做初始对准
怎么校准 各个轴的定义，以及方向是否都一致？ 后来怎么解决了，我也遇到这个问题了，这个问题的正解是什么 这个问题的正解是什么
页:
[1]
2