[四轴技术交流] MPU6050+HMC5883L IMU融合姿态,YAW为何还是飘??

[复制链接]
6098|11
 楼主 | 2016-1-15 18:31 | 显示全部楼层 |阅读模式
无论陀螺仪还是加速度,电子罗盘都校准过,但是最后YAW偏航角还是会飘,慢的几秒一度,快的一秒一度..
检查过好多次,没发现哪里有问题..

  1. //IMU四元素
  2. void Calc_Quaternions(_Triaxial_f acc, _Triaxial_f gyro, _Triaxial_f mag)
  3. {
  4.         float ax = acc.x,ay = acc.y,az = acc.z;
  5.         float gx = gyro.x,gy = gyro.y,gz = gyro.z;
  6.         float mx = mag.x,my = mag.y,mz = mag.z;
  7. //        float mx = 0,my = 0,mz = 0;

  8.         float norm;
  9.         float hx, hy, hz, bx, bz;               
  10.         float vx, vy, vz, wx, wy, wz;
  11.         float ex, ey, ez;

  12.         float q0q0 = q0*q0;
  13.         float q0q1 = q0*q1;
  14.         float q0q2 = q0*q2;
  15.         float q0q3 = q0*q3;
  16.         float q1q1 = q1*q1;
  17.         float q1q2 = q1*q2;
  18.         float q1q3 = q1*q3;
  19.         float q2q2 = q2*q2;   
  20.         float q2q3 = q2*q3;
  21.         float q3q3 = q3*q3;  
  22.        
  23.         float deltaT;

  24.         if(ax*ay*az==0)
  25.         return;

  26.         deltaT = getDeltaT(GetSysTime_us());
  27.        
  28.         //ÖØÁ¦¼ÓËٶȹéÒ»»¯
  29.         norm = invSqrt(ax*ax + ay*ay + az*az);      
  30.         ax = ax * norm;
  31.         ay = ay * norm;
  32.         az = az * norm;
  33.         //´ÅÁ¦¼Æ¹éÒ»»¯
  34.         norm = invSqrt(mx*mx* + my*my + mz*mz);         
  35.         mx = mx * norm;
  36.         my = my * norm;
  37.         mz = mz * norm;

  38.         hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
  39.         hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
  40.         hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);
  41.      
  42.         bx = sqrt((hx*hx) + (hy*hy));               
  43.         bz = hz;     
  44.        
  45.         //ÌáÈ¡ËÄÔªÊýµÄµÈЧÓàÏÒ¾ØÕóÖеÄÖØÁ¦·ÖÁ¿
  46.         vx = 2*(q1q3 - q0q2);                                                                                               
  47.         vy = 2*(q0q1 + q2q3);
  48.         vz = q0q0 - q1q1 - q2q2 + q3q3 ;

  49.         wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
  50.         wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
  51.         wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
  52.        
  53.         //ÏòÁ¿²æ»ýµÃ³ö×Ë̬Îó²î
  54.         ex = (ay*vz - az*vy) + (my*wz - mz*wy);
  55.         ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
  56.         ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

  57.         if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
  58.         {
  59.                 exInt = exInt + ex * Ki * halfT*deltaT;
  60.                 eyInt = eyInt + ey * Ki * halfT*deltaT;       
  61.                 ezInt = ezInt + ez * Ki * halfT*deltaT;
  62.                
  63.                 //»¥²¹Â˲¨£¬×Ë̬Îó²î²¹³¥µ½½ÇËÙ¶ÈÉÏ£¬ÐÞÕý½ÇËٶȻý·ÖƯÒÆ
  64.                 gx = gx + Kp*ex + exInt;                                                                                          
  65.                 gy = gy + Kp*ey + eyInt;
  66.                 gz = gz + Kp*ez + ezInt;       
  67.         }                                                                  

  68.         //Ò»½×Áú¸ñ¿âËþ·¨¸üÐÂËÄÔªÊý
  69.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT*deltaT;
  70.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT*deltaT;
  71.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT*deltaT;
  72.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT*deltaT;

  73.         //ËÄÔªÊý¹éÒ»»¯
  74.         norm = invSqrt(q0q0 + q1q1 + q2q2 + q3q3);
  75.         q0 = q0 * norm;
  76.         q1 = q1 * norm;
  77.         q2 = q2 * norm;
  78.         q3 = q3 * norm;

  79.         //ËÄÔªÊýתŷÀ­½Ç
  80.         Angle.x = atan2f(2.0f * q2q3 + 2.0f * q0q1, -2.0f * q1q1 - 2.0f * q2q2 + 1) * RAD_TO_DEG;// ·­¹ö
  81.         Angle.y = -safe_asin(-2.0f * q1q3 + 2.0f * q0q2) * RAD_TO_DEG; // ¸©Ñö
  82.         Angle.z = -atan2f(2.0f*q1q2 + 2.0f*q0q3, -2.0f *q2q2 - 2.0f*q3q3 + 1) * RAD_TO_DEG;//Æ«º½               
  83. }
复制代码
| 2016-1-18 08:54 | 显示全部楼层
不好说是什么问题,我之前用6050没有用磁力计刚开始yaw有点飘,后面就趋于稳定,估计和代码有点关系。你是用32还是用其他来读取的呢?
| 2016-2-20 12:06 | 显示全部楼层
同样遇到这样的问题,用的MPU9150,没有使用DMP。原始数据融合的Pitch和Roll特别的稳定,就Yaw会一直漂移,情况和你的一样。可能是代码哪里没有搞清楚,还在看,可以一起交流。
| 2016-2-26 16:43 | 显示全部楼层
orphanping 发表于 2016-2-20 12:06
同样遇到这样的问题,用的MPU9150,没有使用DMP。原始数据融合的Pitch和Roll特别的稳定,就Yaw会一直漂移, ...

你们的pitch和roll在板子晃动的时候不会抖吗?比如pitch角从0变到-20,我的会往下跳很多再恢复,能不能一起交流下啊
  1. -3
  2. -3
  3. -3
  4. -3
  5. -3
  6. -3
  7. -3
  8. -3
  9. -3
  10. -3
  11. -3
  12. -3
  13. -3
  14. -3
  15. -3
  16. -3
  17. -3
  18. -3
  19. -3
  20. -3
  21. -3
  22. -3
  23. -3
  24. -3
  25. -3
  26. -3
  27. -3
  28. -3
  29. -2.95
  30. -2.96
  31. -2.96
  32. -2.96
  33. -2.97
  34. -2.97
  35. -2.97
  36. -2.98
  37. -2.98
  38. -2.98
  39. -2.98
  40. -2.98
  41. -2.98
  42. -2.55
  43. -2.44
  44. -2.49
  45. -2.53
  46. -2.58
  47. -2.5
  48. -2.5
  49. -2.54
  50. -2.61
  51. -2.74
  52. -2.58
  53. -2.29
  54. -1.85
  55. -57.9
  56. -51.96
  57. -74.21
  58. 86.2
  59. -29.03
  60. 67.96
  61. 14.94
  62. -9.64
  63. -25.64
  64. -24.77
  65. -20.87
  66. -20.8
  67. -24.79
  68. -28.19
  69. -30.21
  70. -20.47
  71. -19.58
  72. 86.16
  73. 50.4
  74. 28.31
  75. -7.21
  76. -41.62
  77. -52.05
  78. -55.52
  79. -58.09
  80. -60.04
  81. -58.5
  82. -55.28
  83. -52.74
  84. -50.52
  85. -48.64
  86. -46.97
  87. -47.71
  88. -46.06
  89. -44.47
  90. -42.41
  91. -40.52
  92. -38.79
  93. -37.75
  94. -36.75
  95. -35.38
  96. -34.47
  97. -33.61
  98. -32.51
  99. -31.73
  100. -30.96
  101. -30.07
  102. -29.36
  103. -26.32
  104. -23.51
  105. -23.27
  106. -23.07
  107. -22.82
  108. -22.63
  109. -22.39
  110. -22.25
  111. -19.83
  112. -17.55
  113. -15.43
  114. -15.79
  115. -16.15
  116. -16.47
  117. -16.71
  118. -16.98
  119. -17.16
  120. -17.31
  121. -17.41
  122. -17.51
  123. -17.63
  124. -17.8
  125. -17.96
  126. -18.11
  127. -18.15
  128. -18.33
  129. -18.34
  130. -18.47
  131. -18.58
  132. -18.68
  133. -18.72
  134. -18.72
  135. -18.86
  136. -16.7
  137. -17
  138. -17.13
  139. -17.29
  140. -17.41
  141. -17.65
  142. -17.83
  143. -18.04
  144. -18.23
  145. -18.3
  146. -18.47
  147. -18.63
  148. -18.72
  149. -16.57
  150. -16.83
  151. -17.12
  152. -17.3
  153. -17.45
  154. -17.69
  155. -17.82
  156. -18.03
  157. -18.22
  158. -18.33
  159. -18.4
  160. -18.44
  161. -18.6
  162. -18.75
  163. -18.77
  164. -18.83
  165. -18.94
  166. -19.06
  167. -19.17
  168. -16.99
  169. -14.93
  170. -15.27
  171. -15.67
  172. -16.05
  173. -16.4
  174. -16.66
  175. -16.89
  176. -17.08
  177. -17.35
  178. -15.32
  179. -15.72
  180. -16.1
  181. -16.4
  182. -16.68
  183. -16.93
  184. -17.15
  185. -17.33
  186. -17.58
  187. -17.72
  188. -17.99
  189. -18.19
  190. -18.37
  191. -18.55
  192. -18.7
  193. -18.84
  194. -16.71
  195. -17
  196. -17.28
  197. -17.47
  198. -17.7
复制代码
| 2016-3-4 10:48 | 显示全部楼层
huxiaoyingying 发表于 2016-2-26 16:43
你们的pitch和roll在板子晃动的时候不会抖吗?比如pitch角从0变到-20,我的会往下跳很多再恢复,能不能一 ...

Pitch和Roll倒是很稳定的,只是Yaw会一直漂移。
| 2016-3-4 14:36 | 显示全部楼层
orphanping 发表于 2016-3-4 10:48
Pitch和Roll倒是很稳定的,只是Yaw会一直漂移。

我的今天调好了,yaw也不会飘了,我之前的问题在于陀螺仪数据没有做单位换算,四元数参加运算的陀螺仪数据需要是弧度/s,之前一直用的度/s! Yaw会飘的原因是你的gz一直在抖吧!

评论

旭羽宏镞 2018-6-21 15:20 回复TA
怎么改啊,楼上会吗?你不是解决了吗? 
| 2016-7-19 10:53 | 显示全部楼层
楼主问题解决了么?我也遇到了同样的问题
| 2018-6-21 15:20 | 显示全部楼层
是啊,楼主解决了吗?
| 2018-6-30 22:23 | 显示全部楼层
MPU6050+HMC5883L IMU融合姿态,这个不是都是校正吗?
| 2018-6-30 22:23 | 显示全部楼层
以前做的姿态,没有使用HMC5883L也是飘,没有办法。                  
扫描二维码,随时随地手机跟帖
您需要登录后才可以回帖 登录 | 注册

本版积分规则

快速回复

您需要登录后才可以回帖
登录 | 注册
高级模式
我要创建版块 申请成为版主

论坛热帖

关闭

热门推荐上一条 /6 下一条

分享 快速回复 返回顶部 返回列表