首页 > 资讯 > 电子杂谈

mpu6050_mpu6050中文数据手册_mpu6050数据处理

字号: T | T
2014-11-29  来源:mpu6050   作者:mpu6050  浏览次数: 8519
[摘要]mpu6050中文数据手册 点击地址直接下载:http://download.csdn.net/detail/family5love/5118008mpu6050数据处理:1.STM32如何处理

mpu6050中文数据手册  点击地址直接下载:http://download.csdn.net/detail/family5love/5118008

mpu6050

mpu6050数据处理:

1.STM32如何处理陀螺仪MPU6050送出的数据?

 四轴飞行器卡在了飞行姿势调整上。。。不知怎样计算陀螺仪数据,原先还以为像安卓的API一样直接会输出角度数值。。。 

这是模块静止水平放置时输出的数据:我使用的MPU6050模块:收到的数据跳动很厉害,谷歌搜索了两天了,到现在还是不知道怎样把这些数据转换成角度、加速度和温度数值。求方法,另外说明本人不会微积分。。。附上我的程序源码: 
答案:加速度计和陀螺仪的融合算法你可以在坛子搜索两轮平衡车。里面有讲解融合算法
MPU6050数据处理实验一:
MPU6050模块买了都好几个月了,出来前些日子做一下简单的数据获取,再没做过新的实验,一直在看资料,从自横小车的姿态矫正到四轴飞行器的姿态矫正等给类帖子看了不少,昨天晚上终于动手做了一次实验,这次实验主要是想试一下用陀螺仪来输出传感器的三轴角度(相对于地平面)。
下面是实验的代码:
  1. #include <I2Cdev.h>
  2. #include <MPU6050.h>
  3. #include <Wire.h>//添加必须的库文件
  4. //====一下三个定义了陀螺仪的偏差===========
  5. #define Gx_offset -3.06
  6. #define Gy_offset 1.01
  7. #define Gz_offset -0.88
  8. //====================
  9. MPU6050 accelgyro;
  10.  
  11. int16_t ax,ay,az;
  12. int16_t gx,gy,gz;//存储原始数据
  13. float aax,aay,aaz,ggx,ggy,ggz;//存储量化后的数据
  14. float Ax,Ay,Az;//单位 g(9.8m/s^2)
  15. float Gx,Gy,Gz;//单位 °/s
  16.  
  17. float Angel_accX,Angel_accY,Angel_accZ;//存储加速度计算出的角度
  18.  
  19. long LastTime,NowTime,TimeSpan;//用来对角速度积分的
  20.  
  21. #define LED_PIN 13
  22.  
  23. bool blinkState=false;
  24.  
  25. void setup()//MPU6050的设置都采用了默认值,请参看库文件
  26. {
  27.   Wire.begin();
  28.  
  29.   Serial.begin(9600);
  30.  
  31.   Serial.println("Initializing I2C device.....");
  32.   accelgyro.initialize();
  33.  
  34.   Serial.println("Testing device connections...");
  35.   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful":"MPU6050 connection failure");
  36.  
  37.   pinMode(LED_PIN,OUTPUT);
  38. }
  39.  
  40. void loop()
  41. {
  42.   accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);//获取三个轴的加速度和角速度
  43. //  Serial.print(ax/16384.00);
  44. //  Serial.print(",");
  45. //  Serial.print(ay/16384.00);
  46. //  Serial.print(",");
  47. //  Serial.print(az/16384.00);
  48. //  Serial.print(",");
  49. //  Serial.print(gx/131.00);
  50. //  Serial.print(",");
  51. //  Serial.print(gy/131.00);
  52. //  Serial.print(",");
  53. //  Serial.println(gz/131.00);
  54. //======一下三行是对加速度进行量化,得出单位为g的加速度值
  55.    Ax=ax/16384.00;
  56.    Ay=ay/16384.00;
  57.    Az=az/16384.00;
  58.    //==========以下三行是用加速度计算三个轴和水平面坐标系之间的夹角
  59. //请参考:[url]http://www.geek-workshop.com/forum.php?mod=viewthread&tid=2328&page=1#pid27876[/url]
  60. //个人觉得原帖中case0算的不对,应该是z/sqrt(x*x+y*y),估计是江南楼主写错了
  61.    Angel_accX=atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14;
  62.    Angel_accY=atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14;
  63.    Angel_accZ=atan(Az/sqrt(Ax*Ax+Ay*Ay))*180/3.14;
  64.    //==========以下三行是对角速度做量化==========
  65.    ggx=gx/131.00;
  66.    ggy=gy/131.00;
  67.    ggz=gz/131.00;
  68.  
  69.   //===============以下是对角度进行积分处理================
  70.   NowTime=millis();//获取当前程序运行的毫秒数
  71.   TimeSpan=NowTime-LastTime;//积分时间这样算不是很严谨
  72. //下面三行就是通过对角速度积分实现各个轴的角度测量,当然假设各轴的起始角度都是0
  73.   Gx=Gx+(ggx-Gx_offset)*TimeSpan/1000;
  74.   Gy=Gy+(ggy-Gy_offset)*TimeSpan/1000;
  75.   Gz=Gz+(ggz-Gz_offset)*TimeSpan/1000;
  76.  
  77.   LastTime=NowTime;
  78.  
  79.   //==============================
  80.   Serial.print(Angel_accX);
  81.   Serial.print(",");
  82.   Serial.print(Angel_accY);
  83.   Serial.print(",");
  84.   Serial.print(Angel_accZ);
  85.   Serial.print(",");
  86.   Serial.print(Gx);
  87.   Serial.print(",");
  88.   Serial.print(Gy);
  89.   Serial.print(",");
  90.   Serial.println(Gz);
  91. //  Serial.print(ggx-Gx_offset);
  92. //  Serial.print(",");
  93. //  Serial.print(ggy-Gy_offset);
  94. //  Serial.print(",");
  95. //  Serial.println(ggz-Gz_offset);
  96.  
  97.   blinkState=!blinkState;
  98.   digitalWrite(LED_PIN,blinkState);
  99.  
  100.   delay(100);//这个用来控制采样速度
  101. }



角速度传感器一定要矫正偏移,我采用的方法就是先读取若干数值,然后将数值导入到excel中,取平均值,该平均值就是传感器当前状态下的偏移,在后面的角度积分中一定要先用原始角速度值减掉对应的偏移,下面两个曲线就是进行偏移校正前后的效果:
未进行偏移量校正 
下面的图片是进行了偏移量校正,可以发现三个轴的角速度基本都在0坐标轴附近上下波动:
进行偏移量校正后 

下图是对角速度进行积分输出的三个轴的角度变化曲线,没做滤波处理,基本上能反应出传感器的姿态。
三个轴的角度变化 

下图是对比角速度和加速度计算出的姿态之间的关系,图中柠檬绿是陀螺仪的x轴,蓝色是加速度的y轴,红色是加速度的z轴,第一段曲线实际上是一个yz平面绕x轴旋转的一个动作,大家可以想象一下,所以陀螺仪的yz输出都是零,而加速的yz是有输出的。
6轴姿态对比 

下一步的实验是将加速度和陀螺仪的角度进行融合,同时研究一下低通滤波和高通滤波在加速度传感器中的使用。


=================下面是SerialChart的代码============================
ARDUINO 
 
  1. [_setup_]
  2. port=COM3   
  3. baudrate=9600
  4.  
  5. width=1000
  6. height=200
  7. background_color = black
  8.  
  9. grid_h_origin = 100
  10. grid_h_step = 50
  11. grid_h_color = #EEE
  12. grid_h_origin_color = #CCC
  13.  
  14. grid_v_origin = 0
  15. grid_v_step = 200
  16. grid_v_color = #EEE
  17. grid_v_origin_color = transparent
  18.  
  19. [_default_]
  20. min=-1
  21. max=1
  22.  
  23. [ax]
  24. color=gold
  25. min=-90
  26. max=90
  27.  
  28. [ay]
  29. color=blue
  30. min=-90
  31. max=90
  32. [az]
  33. color=red
  34. min=-90
  35. max=90
  36.  
  37. [gx]
  38. color=lime
  39. min=-90
  40. max=90
  41.  
  42. [gy]
  43. color=magenta
  44. min=-90
  45. max=90
  46. [gz]
  47. color=cyan
  48. min=-90
  49. max=90

昨天晚上回去试了一下“P00=(1-Kg*H)*P10*(1-Kg*H)+Kg*R*Kg;”的效果,没有发现任何不同,Kg在第四个循环后就已经收敛到了0.42。
====================================================
自从接触自衡小车开始就发现卡尔曼是个必须过的坎儿,翻遍了坛子里的和卡尔曼有关的帖子,中文的外文的资料看了一大堆,还是没有真正搞明白卡尔曼的原理,没到这个时候心里那个后悔啊,当初在学校为啥不好好学习呢,早知今日要做自衡小车,当初真应该好好学习一下。好在最近照葫芦画瓢的写了一个简单的融合算法,先贴出来,自己做实验时,感觉效果很一般,没有预期中的牛逼,还在进一步研究,下面先贴代码。

  1. #include <I2Cdev.h>
  2. #include <MPU6050.h>
  3. #include <Wire.h>
  4.  
  5. float ax,az,gy;//get acceleration_x,acceleration_z,rotation_y
  6. long OffsetGy,OffsetSum;
  7. float AngleAcc,AngleRotation,AngleMerge;//angel computed by acceleration and rotation
  8. long LastTime,NowTime,TimeSpan,SampleTime;//
  9. //===========kalman filter parameter define============
  10. float A,B,H,Q,R,P00,P10,X00,X10,Z,U,Kg;
  11.  
  12.  
  13.  
  14. MPU6050 mpu;
  15.  
  16.  
  17. void setup()
  18. {
  19.   //============initial kalman parameter======
  20.   A=1.0;
  21.   H=1.0;
  22.   Q=0.003;
  23.   R=0.01;
  24.   P00=5.0;
  25.   P10=0.0;
  26.   X00=0.0;
  27.   //===============end initial===========
  28.   Wire.begin();
  29.   Serial.begin(9600);
  30.   Serial.println("start");
  31.   mpu.initialize();
  32.   Serial.println(mpu.getRotationY());
  33.   Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  34.   //compute the gyroscope offset automatic
  35.   for(int i=0;i<200;i++)
  36.   {
  37.     Serial.println(mpu.getRotationY());
  38.     OffsetSum=OffsetSum+mpu.getRotationY();
  39.     delay(50);
  40.    }
  41.  
  42.   OffsetGy=OffsetSum/200;
  43.   pid_SP=0.0;
  44.   Kp=10.0;
  45.   Kd=2.0;
  46.   Ki=0.0;
  47. }
  48.  
  49. void loop()
  50. {
  51.  
  52.   NowTime=millis();
  53.   TimeSpan=NowTime-LastTime;
  54.   LastTime=NowTime;
  55.   //=================kalman filter==========================
  56.   ax=mpu.getAccelerationX()/16384.00;
  57.   az=mpu.getAccelerationZ()/16384.00;
  58.   AngleAcc=(-1.0)*atan2(ax,az)*180/PI;
  59.   gy=(mpu.getRotationY()-OffsetGy)/131.00;
  60.   AngleRotation=AngleRotation+gy*TimeSpan/1000.00;
  61.   //=============kalman calculate============
  62.   B=TimeSpan/1000.0;
  63.   U=gy;
  64.   Z=AngleAcc;
  65. //一下按照卡尔曼滤波的五个公式写的,融合角度作为状态,陀螺仪的角速度作为控制量,加速度的角度作为观测值
  66. //所以A是1,B是采样周期
  67.   X10=A*X00+B*U; //=======formula 1
  68.   P10=A*P00*(A)+Q;//=============formula 2
  69.   Kg=P10*(H)/(H*P10*(H)+R);//=======formula 3
  70.  
  71.   X00=X10+Kg*(Z-H*X10);//========formula 4
  72.  
  73.   P00=(1-Kg*H)*P10;//=============formula 5
  74.   //P00=(1-Kg*H)*P10*(1-Kg*H)+Kg*R*Kg;//======根据维基百科的说法,当Kg不是最优时采用这个公式
  75.   AngleMerge=X00;
  76.  
  77.   Serial.print(AngleRotation);
  78.   Serial.print(",");
  79.   Serial.print(AngleMerge);
  80.   Serial.print(",");
  81.   Serial.print(AngleAcc);
  82.   Serial.print(",");
  83.   Serial.print(0.8*AngleRotation+0.2*AngleAcc);
  84.   Serial.print(",");
  85.   Serial.print(P00);
  86.   Serial.print(",");
  87.   Serial.println(Kg);
  88.   delay(100);
  89.  
  90. }



监视Kg的变化,发现几个周期之内就不再变化了,滤波效果图如下:
滤波效果对比 

2013-4-9 10:55 上传
滤波效果对比

对卡尔曼滤波的研究还在继续,后面会继续更新本文

 

  • 打印
  • 关闭