【学习心得】MPU6050数据处理实验一
本帖最后由 葱拌豆腐 于 2013-3-16 09:12 编辑MPU6050模块买了都好几个月了，出来前些日子做一下简单的数据获取，再没做过新的实验，一直在看资料，从自横小车的姿态矫正到四轴飞行器的姿态矫正等给类帖子看了不少，昨天晚上终于动手做了一次实验，这次实验主要是想试一下用陀螺仪来输出传感器的三轴角度（相对于地平面）。
下面是实验的代码：
#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>//添加必须的库文件
//====一下三个定义了陀螺仪的偏差===========
#define Gx_offset -3.06
#define Gy_offset 1.01
#define Gz_offset -0.88
//====================
MPU6050 accelgyro;
int16_t ax,ay,az;
int16_t gx,gy,gz;//存储原始数据
float aax,aay,aaz,ggx,ggy,ggz;//存储量化后的数据
float Ax,Ay,Az;//单位 g(9.8m/s^2)
float Gx,Gy,Gz;//单位 °/s
float Angel_accX,Angel_accY,Angel_accZ;//存储加速度计算出的角度
long LastTime,NowTime,TimeSpan;//用来对角速度积分的
#define LED_PIN 13
bool blinkState=false;
void setup()//MPU6050的设置都采用了默认值，请参看库文件
{
Wire.begin();
Serial.begin(9600);
Serial.println("Initializing I2C device.....");
accelgyro.initialize();
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful":"MPU6050 connection failure");
pinMode(LED_PIN,OUTPUT);
}
void loop()
{
accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);//获取三个轴的加速度和角速度
//Serial.print(ax/16384.00);
//Serial.print(",");
//Serial.print(ay/16384.00);
//Serial.print(",");
//Serial.print(az/16384.00);
//Serial.print(",");
//Serial.print(gx/131.00);
//Serial.print(",");
//Serial.print(gy/131.00);
//Serial.print(",");
//Serial.println(gz/131.00);
//======一下三行是对加速度进行量化，得出单位为g的加速度值
Ax=ax/16384.00;
Ay=ay/16384.00;
Az=az/16384.00;
//==========以下三行是用加速度计算三个轴和水平面坐标系之间的夹角
//请参考：http://www.geek-workshop.com/forum.php?mod=viewthread&tid=2328&page=1#pid27876
//个人觉得原帖中case0算的不对，应该是z/sqrt（x*x+y*y），估计是江南楼主写错了
Angel_accX=atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14;
Angel_accY=atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14;
Angel_accZ=atan(Az/sqrt(Ax*Ax+Ay*Ay))*180/3.14;
//==========以下三行是对角速度做量化==========
ggx=gx/131.00;
ggy=gy/131.00;
ggz=gz/131.00;
//===============以下是对角度进行积分处理================
NowTime=millis();//获取当前程序运行的毫秒数
TimeSpan=NowTime-LastTime;//积分时间这样算不是很严谨
//下面三行就是通过对角速度积分实现各个轴的角度测量，当然假设各轴的起始角度都是0
Gx=Gx+(ggx-Gx_offset)*TimeSpan/1000;
Gy=Gy+(ggy-Gy_offset)*TimeSpan/1000;
Gz=Gz+(ggz-Gz_offset)*TimeSpan/1000;
LastTime=NowTime;
//==============================
Serial.print(Angel_accX);
Serial.print(",");
Serial.print(Angel_accY);
Serial.print(",");
Serial.print(Angel_accZ);
Serial.print(",");
Serial.print(Gx);
Serial.print(",");
Serial.print(Gy);
Serial.print(",");
Serial.println(Gz);
//Serial.print(ggx-Gx_offset);
//Serial.print(",");
//Serial.print(ggy-Gy_offset);
//Serial.print(",");
//Serial.println(ggz-Gz_offset);
blinkState=!blinkState;
digitalWrite(LED_PIN,blinkState);
delay(100);//这个用来控制采样速度
}
角速度传感器一定要矫正偏移，我采用的方法就是先读取若干数值，然后将数值导入到excel中，取平均值，该平均值就是传感器当前状态下的偏移，在后面的角度积分中一定要先用原始角速度值减掉对应的偏移，下面两个曲线就是进行偏移校正前后的效果：
下面的图片是进行了偏移量校正，可以发现三个轴的角速度基本都在0坐标轴附近上下波动：
下图是对角速度进行积分输出的三个轴的角度变化曲线，没做滤波处理，基本上能反应出传感器的姿态。
下图是对比角速度和加速度计算出的姿态之间的关系，图中柠檬绿是陀螺仪的x轴，蓝色是加速度的y轴，红色是加速度的z轴，第一段曲线实际上是一个yz平面绕x轴旋转的一个动作，大家可以想象一下，所以陀螺仪的yz输出都是零，而加速的yz是有输出的。
下一步的实验是将加速度和陀螺仪的角度进行融合，同时研究一下低通滤波和高通滤波在加速度传感器中的使用。
=================下面是SerialChart的代码============================
port=COM3
baudrate=9600
width=1000
height=200
background_color = black
grid_h_origin = 100
grid_h_step = 50
grid_h_color = #EEE
grid_h_origin_color = #CCC
grid_v_origin = 0
grid_v_step = 200
grid_v_color = #EEE
grid_v_origin_color = transparent
min=-1
max=1
color=gold
min=-90
max=90
color=blue
min=-90
max=90
color=red
min=-90
max=90
color=lime
min=-90
max=90
color=magenta
min=-90
max=90
color=cyan
min=-90
max=90 楼主能不能介绍下怎么用输出的数据做的图？ 酸枣树 发表于 2013-3-15 22:38 static/image/common/back.gif
楼主能不能介绍下怎么用输出的数据做的图？
简单的话可以用串口监控，然后把串口监控的数据复制到excel中处理，这种方式不能实时观察趋势。另外一种是用serialChart来生成数据，具体的使用可以参考论坛里的帖子。我在后面会补上我的代码。 查到了，谢谢！ 期待更新。 这样处理角速度漂移太大，传感器不动，角度一直上升 楼主，我已经用serialChart来生成数据了，但就是没曲线输出，这是怎么回事？ 楼主 累计误差有点大啊 Z轴的 传感器放着不动曲线一直上升有没有什么好的修正方法？ 花名未闻 发表于 2013-4-4 14:03 static/image/common/back.gif
楼主 累计误差有点大啊 Z轴的 传感器放着不动曲线一直上升有没有什么好的修正方法？
你是对z轴的角速度进行积分了吧？如果需要对角速度进行积分，那么必须先将获取到的角速度减去偏移量才可以，否则肯定会出现你说的情况。 葱拌豆腐 发表于 2013-4-6 00:06 static/image/common/back.gif
你是对z轴的角速度进行积分了吧？如果需要对角速度进行积分，那么必须先将获取到的角速度减去偏移量才可以 ...
忽略这个问题了 感谢 楼主，有没有MPU6000的库~{:soso_e154:} 绝对的好帖啊，我给你跪下了 楼主，我觉得你的程序里计算z轴角度的地方弄错了，原帖是正确的，原帖计算的是物体z轴与绝对z轴的角度，而你计算的是物体z轴与水平面的角度。。 对了楼主，我也在青岛额 楼主
Gx=Gx+(ggx-Gx_offset)*TimeSpan/1000;
Gy=Gy+(ggy-Gy_offset)*TimeSpan/1000;
Gz=Gz+(ggz-Gz_offset)*TimeSpan/1000;
这里为啥要除以1000？