DS-学习笔记02--MPU6050数据分析与滤波

BlackCat

论坛关于6050的帖子看了个遍也没整明白哪些东西可以用，哪些数据是干什么的。
沉寂了2天，尽可能的在网上收集资料，但是关于6050的太少了
在本论坛问题区提的问题有人看，没人回答，估计是看的人不会，会的人不会去问题区，要么就是大家对6050不熟悉或者是没用过吧。也没人来问津下我们初学者。

让我想起卜学亮一首歌：搞不懂就问人，搞得懂就答人，没有人懂还可以问神！

其实整到现在有的地方我还是不太明白，将就先贡献出来，给大家参考。
我参考的是飞思卡尔官方给出的设计参考。

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轴。不知道是不是我的有问题，还是就这么设计的？

2.关于滤波，实验了一天，大概滤出个这么样个波形。


红色：加速度计算出的角度
橙色：陀螺仪积分出的角度
黑色：滤波后的角度

比较大的浮动是我将mpu6050 旋转大约45°的波形。后面是没放稳产生的前后晃动。

3.代码部分

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. }
    

复制代码

代码loop段中，我使用了一个if循环，采集20次陀螺仪读数，进行平均。这样似乎影响了数据采集，但是在波形中灵敏度似乎不受影响。
知道办法笨了点，希望有高手给出简化方法。

滤波中用到的0.005是参考中给出的。本来准备用系统时间积分的，但是看滤出的波有模有样的就没改了。
参考中给出了大部分的算法思想，人笨没办法，就折腾出个这么个代码。

4.如何绘制波形图
A.绘制波形图一般用excel，吧串口调试工具读出是数据导入excel，选择全部数据，单击图表向导就可以了，记住，在arduino输出数据的时候不要用“逗号”分割，打印一个table也就是把我上面的代码

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.     Serial.print("\t");
   
3.     Serial.print(Angle_GG);
   
4.     Serial.print("\t");
   
5.     Serial.print(Angle_AG);
   
6.     Serial.print("\n");

复制代码

在串口调试工具得到的数据就可以粘贴到excel，直接得到一个N行3列的表格。至于多少行，看你要采样多少个数据。

B.许多串口调试工具都提供绘制波形的。
黑马给出的 http://www.geek-workshop.com/thread-676-1-1.html
还有一个是 SerialChart 也是论坛里谁找的，忘了是谁了。
我用的是SerialChart。
具体参数如下
[pre lang="SerialChart" line="1"][_setup_]
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
[/code]
好多人都看那个帖子了，但是不会用的居多，我也是瞎折腾，出了波形。
解释一下代码
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

记住，串口发送数据的时候要以逗号分隔。如下：

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");

复制代码

这样，SerialChart，就可以认识你发送过来的数据了。

5.在调试的时候发现，总是因为调整一个参数而不断的下载程序。不知道论坛里有没有懂VC++接口编程的一起讨论下制作一个串口调试工具，我懂点编程语言，但是不会VC++，接口之类的。如果有人提供那就更好了。
具体实现内容如下：

A.串口接收单片机发送的 陀螺仪、加速度计等传感器原始数据。
B.软件提供相应的公式参数输入框，方便调整参数，比如计算零点值偏移量，对角度积分等等参数。
C.图形化原始数据和公式后数据。

顺便大家帮我看下有什么问题，这样的波形是否能在平衡车中使用。
有什么疑问的可以提出，我明白的都可以解答，不懂的地方就.....

弘毅

非常好的分享~~~其实回答的人少，主要是这个芯片出来时间短，用的人确实不多。这个也属于高级应用了，哈哈，大部分还是得自问自答。。。。

debian123

嗯 很有用

wing

其实我也不明白,先MARK下来

飞翔的红猪

很强的技术贴

zhanggang1971

感谢楼主分享，很有收获！

Randy

感谢你的精彩分享，开始我也对精度转换的问题烦恼了一段时间，我写了那个学习笔记算是给大家会操作使用，至于重要部分还没写出来！支持！

yuyu

计算方法应该是有问题的,因为加速度不是只与陀螺仪一个轴同步,与陀螺仪的三个轴都相关的.

BlackCat

yuyu 发表于 2012-7-8 17:15
计算方法应该是有问题的,因为加速度不是只与陀螺仪一个轴同步,与陀螺仪的三个轴都相关的.

貌似我应该把名字改一下，这个数据分析是为以后平衡车考虑的，如果只需要平衡，单轴加速度和单轴陀螺仪的计算已经可以了啊，如果+转向，需要加一个轴。
这样的话，另一个轴应该是自转，数据只是漂移量吧？

Farid163

mark 学习下………………

BlackCat

补充一下为什么加速度Z轴对应的角速度轴上x轴的问题

因为加速度z轴向前，x轴为下底边，y轴 向上，如图。

此时加速度Z轴为加速度灵敏轴。
角速度x轴为角速度灵敏轴。

拾瑞

不太明白楼主的X轴的陀螺修正为什么要用Z轴的加速度?

往前滑

很有用啊感谢！

BlackCat

拾瑞 发表于 2012-7-11 13:33
不太明白楼主的X轴的陀螺修正为什么要用Z轴的加速度?

我最近也在研究这个问题，如上面我的图，6050 Z轴在水平方向有一个向下的重力，所以可以计算出Z轴对于水平坐标（红色那条线）的偏移角度。
对于6050的陀螺仪，围绕X轴方向转动，才能得到参考方向Z方向的角速度。

这个参考方向上Z方向。也就是Z上向前的，比如直立车的正方向。

我手上只有6050 没有其他的加速度计和角速度计，不知道其他的传感器是怎样工作的。想来应该差不多。

BlackCat

其实6050的文档中，已经详细说明了这个关系，只是我们没有仔细看而已。