MPU6050数据分析

1.关于6050 陀螺仪和加速度计 的角速度和角度计算。 A.陀螺仪角度计算，很多帖子中都提到了用的是积分，但是我这里还是重新讲下。 angle_n = angle_n-1 + (Gyro-C_Gyro)*R_Gyro; angle_n 当前角度值，它的单位是度（°） angle_n-1 上一次计算出的角度值 Gyro 陀螺仪敏感轴偏转值，也就是当前敏感轴读数 C_Gyro 陀螺仪零点偏移值，这个值的测量方法是：将陀螺仪敏感轴水平放置静止时的读数，我的零点偏移值是水平、垂直、倒置，分别取1024次，作平均值得出的，读数是-177.8865041，但是最后在程序实践中，调整到-99.90。或许还有别的办法，自己看着办吧。 R_Gyro 是陀螺仪比例。飞思卡尔的参考中提到这个值是可以计算出来的，下面我会提供下载，大家自己去看看怎么算的，但是在其论坛和调试手册中都提到，这个比例值还是实验法测量出来的比较准确。 B.加速度仪 角度计算。 加速度仪的角度计算有很多方法，论坛里就有2中。但是都用到了三角函数，数学没学好，照抄了也不行。参考了飞思卡尔的计算方法后大概是这样的。 Angle_Z = (az-C_Z)*R_Z; angle_z 加速度计敏感轴Z轴产生倾角计算出的角度，单位度（°） az 是加速度仪 Z轴读数 C_Z Z轴零点偏移量 测量方法和陀螺仪的一样。 R_Z 加速度计Z轴比例 C.反复试验，MPU6050加速度计Z轴对应的是陀螺仪的X轴。 3.代码部分 ARDUINO 代码复制打印  1. #include "Wire.h" 2. #include "I2Cdev.h" 3. #include "MPU6050.h" 4. MPU6050 accelgyro; 5. int16_t ax, ay, az; 6. int16_t gx, gy, gz; 7.   8. float C_Z = -1343.91;//Z轴零点偏移量 9. float C_Gyro = -99.90;//陀螺仪零点偏移量 10. float Z_Min = -17873.76;//最小极值 11. float Z_Max = 15186.91;//最大极值 12. float T_Z = 3;//Z轴角度补偿时间常数 13. float R_Z = 180/(Z_Max - Z_Min);//Z轴比例 14. float R_Gyro = 0.081;//陀螺仪比例 15. unsigned long T_Now =0;//系统当前时间 16. unsigned long T_Last;//上次时间 17. float Angle_G,Angle_AG,Angle_GG; 18. int i; 19. float GYRO; 20.   21. void setup() { 22.   Wire.begin(); 23.   Serial.begin(38400); 24.   // initialize device 25.   Serial.println("Initializing I2C devices..."); 26.   accelgyro.initialize(); 27.   // verify connection 28.   Serial.println("Testing device connections..."); 29.   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); 30. } 31.   32. void loop() { 33.   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取6050数据 34.   if (i>20) 35.   { 36.     GYRO = GYRO/20; 37.     float Angle_Z = (az-C_Z)*R_Z;//加速度计 角度计算 （读取值-偏移量）*比例  单位：° 38.     Angle_G = -(GYRO-C_Gyro)*R_Gyro;//陀螺仪采样 （采样值-偏移量）*比例 单位：°/s 39.     Angle_AG = Angle_AG + (((Angle_Z-Angle_AG)*1/T_Z)+Angle_G)*0.005;//滤波 40.     Angle_GG = Angle_GG + Angle_G*0.005;//陀螺仪对X轴积分 得出角度。 41.     Serial.print(Angle_Z); 42.     Serial.print(","); 43.     Serial.print(Angle_GG); 44.     Serial.print(","); 45.     Serial.print(Angle_AG); 46.     Serial.print("\n"); 47.     i=0; 48.   } 49.   GYRO = GYRO + gx; 50.   i++; 51. } 1. #include "Wire.h" 2.  3. #include "I2Cdev.h" 4.  5. #include "MPU6050.h" 6.  7. MPU6050 accelgyro; 8.  9. int16_t ax, ay, az; 10.  11. int16_t gx, gy, gz; 12.  13.   14.  15. float C_Z = -1343.91;//Z轴零点偏移量 16.  17. float C_Gyro = -99.90;//陀螺仪零点偏移量 18.  19. float Z_Min = -17873.76;//最小极值 20.  21. float Z_Max = 15186.91;//最大极值 22.  23. float T_Z = 3;//Z轴角度补偿时间常数 24.  25. float R_Z = 180/(Z_Max - Z_Min);//Z轴比例 26.  27. float R_Gyro = 0.081;//陀螺仪比例 28.  29. unsigned long T_Now =0;//系统当前时间 30.  31. unsigned long T_Last;//上次时间 32.  33. float Angle_G,Angle_AG,Angle_GG; 34.  35. int i; 36.  37. float GYRO; 38.  39.   40.  41. void setup() { 42.  43.   Wire.begin(); 44.  45.   Serial.begin(38400); 46.  47.   // initialize device 48.  49.   Serial.println("Initializing I2C devices..."); 50.  51.   accelgyro.initialize(); 52.  53.   // verify connection 54.  55.   Serial.println("Testing device connections..."); 56.  57.   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); 58.  59. } 60.  61.   62.  63. void loop() { 64.  65.   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取6050数据 66.  67.   if (i>20) 68.  69.   { 70.  71.     GYRO = GYRO/20; 72.  73.     float Angle_Z = (az-C_Z)*R_Z;//加速度计 角度计算 （读取值-偏移量）*比例  单位：° 74.  75.     Angle_G = -(GYRO-C_Gyro)*R_Gyro;//陀螺仪采样 （采样值-偏移量）*比例 单位：°/s 76.  77.     Angle_AG = Angle_AG + (((Angle_Z-Angle_AG)*1/T_Z)+Angle_G)*0.005;//滤波 78.  79.     Angle_GG = Angle_GG + Angle_G*0.005;//陀螺仪对X轴积分 得出角度。 80.  81.     Serial.print(Angle_Z); 82.  83.     Serial.print(","); 84.  85.     Serial.print(Angle_GG); 86.  87.     Serial.print(","); 88.  89.     Serial.print(Angle_AG); 90.  91.     Serial.print("\n"); 92.  93.     i=0; 94.  95.   } 96.  97.   GYRO = GYRO + gx; 98.  99.   i++; 100.  101. } 102.  代码loop段中，我使用了一个if循环，采集20次陀螺仪读数，进行平均。这样似乎影响了数据采集，但是在波形中灵敏度似乎不受影响。 知道办法笨了点，希望有高手给出简化方法。 滤波中用到的0.005是参考中给出的。本来准备用系统时间积分的，但是看滤出的波有模有样的就没改了。 参考中给出了大部分的算法思想，人笨没办法，就折腾出个这么个代码。 4.如何绘制波形图 A.绘制波形图一般用excel，吧串口调试工具读出是数据导入excel，选择全部数据，单击图表向导就可以了，记住，在arduino输出数据的时候不要用“逗号”分割，打印一个table也就是把我上面的代码 ARDUINO 代码复制打印  1. Serial.print(Angle_Z); 2.     Serial.print(","); 3.     Serial.print(Angle_GG); 4.     Serial.print(","); 5.     Serial.print(Angle_AG); 6.     Serial.print("\n"); 1. Serial.print(Angle_Z); 2.  3.     Serial.print(","); 4.  5.     Serial.print(Angle_GG); 6.  7.     Serial.print(","); 8.  9.     Serial.print(Angle_AG); 10.  11.     Serial.print("\n"); 12.  换成 ARDUINO 代码复制打印  1. Serial.print(Angle_Z); 2.     Serial.print("\t"); 3.     Serial.print(Angle_GG); 4.     Serial.print("\t"); 5.     Serial.print(Angle_AG); 6.     Serial.print("\n"); 1. Serial.print(Angle_Z); 2.  3.     Serial.print("\t"); 4.  5.     Serial.print(Angle_GG); 6.  7.     Serial.print("\t"); 8.  9.     Serial.print(Angle_AG); 10.  11.     Serial.print("\n"); 12.  在串口调试工具得到的数据就可以粘贴到excel，直接得到一个N行3列的表格。至于多少行，看你要采样多少个数据。 B.许多串口调试工具都提供绘制波形的。 黑马给出的 http://www.geek-workshop.com/thread-676-1-1.html 还有一个是 SerialChart 也是论坛里谁找的，忘了是谁了。 我用的是SerialChart。 具体参数如下  1. [_setup_] 2.  3. port=COM4 4.  5. baudrate=38400 6.  7.   8. width=1200 9.  10. height=600 11.  12. background_color = white 13.  14.   15. grid_h_origin = 200 16.  17. grid_h_step = 10 18.  19. grid_h_color = #EEE 20.  21. grid_h_origin_color = #CCC 22.  23.   24. grid_v_origin = 200 25.  26. grid_v_step = 10 27.  28. grid_v_color = #EEE 29.  30. grid_v_origin_color = transparent 31.  32.   33. [_default_] 34.  35. min=-100 36.  37. max=100 38.  39.   40.   41. [Angle_Z] 42.  43. color = red 44.  45.   46. [Angle_GG] 47.  48. color = orange 49.  50.   51. [Angle_AG] 52.  53. color = black 1. [_setup_] 2.  3. port=COM4 4.  5. baudrate=38400 6.  7.   8.  9. width=1200 10.  11. height=600 12.  13. background_color = white 14.  15.   16.  17. grid_h_origin = 200 18.  19. grid_h_step = 10 20.  21. grid_h_color = #EEE 22.  23. grid_h_origin_color = #CCC 24.  25.   26.  27. grid_v_origin = 200 28.  29. grid_v_step = 10 30.  31. grid_v_color = #EEE 32.  33. grid_v_origin_color = transparent 34.  35.   36.  37. [_default_] 38.  39. min=-100 40.  41. max=100 42.  43.   44.  45.   46.  47. [Angle_Z] 48.  49. color = red 50.  51.   52.  53. [Angle_GG] 54.  55. color = orange 56.  57.   58.  59. [Angle_AG] 60.  61. color = black 62.  好多人都看那个帖子了，但是不会用的居多，我也是瞎折腾，出了波形。 解释一下代码 port=COM4 //端口 baudrate=38400//波特率 width=1200//波形图底面宽 height=600//波形图底面高 background_color = white//波形图底面颜色 grid_h_origin = 200 //这个貌似是中间哪个参考线的高度 grid_h_step = 10 grid_h_color = #EEE grid_h_origin_color = #CCC grid_v_origin = 200 grid_v_step = 10 grid_v_color = #EEE grid_v_origin_color = transparent [_default_]//可视区域大小 min=-100 max=100 [Angle_Z]//这个格式是[名字]，名字可以随便取，但是读数是串口发送第一行，以逗号分隔的第一个，后面的以此类推。 color = red//紧跟名字下面的是对于该名字指向的数据绘制波形的颜色。 [Angle_GG] color = orange [Angle_AG] color = black 记住，串口发送数据的时候要以逗号分隔。如下： ARDUINO 代码复制打印  1. Serial.print(Angle_Z); 2.     Serial.print(","); 3.     Serial.print(Angle_GG); 4.     Serial.print(","); 5.     Serial.print(Angle_AG); 6.     Serial.print("\n"); 1. Serial.print(Angle_Z); 2.  3.     Serial.print(","); 4.  5.     Serial.print(Angle_GG); 6.  7.     Serial.print(","); 8.  9.     Serial.print(Angle_AG); 10.  11.     Serial.print("\n"); 12.  这样，SerialChart，就可以认识你发送过来的数据了。 5.在调试的时候发现，总是因为调整一个参数而不断的下载程序。不知道论坛里有没有懂VC++接口编程的一起讨论下制作一个串口调试工具，我懂点编程语言，但是不会VC++，接口之类的。如果有人提供那就更好了。 具体实现内容如下： A.串口接收单片机发送的 陀螺仪、加速度计等传感器原始数据。 B.软件提供相应的公式参数输入框，方便调整参数，比如计算零点值偏移量，对角度积分等等参数。 C.图形化原始数据和公式后数据。 顺便大家帮我看下有什么问题，这样的波形是否能在平衡车中使用。