本帖最后由 葱拌豆腐 于 2013-3-16 09:12 编辑
MPU6050模块买了都好几个月了，出来前些日子做一下简单的数据获取，再没做过新的实验，一直在看资料，从自横小车的姿态矫正到四轴飞行器的姿态矫正等给类帖子看了不少，昨天晚上终于动手做了一次实验，这次实验主要是想试一下用陀螺仪来输出传感器的三轴角度（相对于地平面）。
下面是实验的代码：
[pre lang="arduino" line="1" file="MPU6050TestCode"]
#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/for ... amp;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);//这个用来控制采样速度
}
[/code]
角速度传感器一定要矫正偏移，我采用的方法就是先读取若干数值，然后将数值导入到excel中，取平均值，该平均值就是传感器当前状态下的偏移，在后面的角度积分中一定要先用原始角速度值减掉对应的偏移，下面两个曲线就是进行偏移校正前后的效果：
下面的图片是进行了偏移量校正，可以发现三个轴的角速度基本都在0坐标轴附近上下波动：
下图是对角速度进行积分输出的三个轴的角度变化曲线，没做滤波处理，基本上能反应出传感器的姿态。
下图是对比角速度和加速度计算出的姿态之间的关系，图中柠檬绿是陀螺仪的x轴，蓝色是加速度的y轴，红色是加速度的z轴，第一段曲线实际上是一个yz平面绕x轴旋转的一个动作，大家可以想象一下，所以陀螺仪的yz输出都是零，而加速的yz是有输出的。
下一步的实验是将加速度和陀螺仪的角度进行融合，同时研究一下低通滤波和高通滤波在加速度传感器中的使用。
=================下面是SerialChart的代码============================
- [_setup_]
- 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
- [_default_]
- min=-1
- max=1
- [ax]
- color=gold
- min=-90
- max=90
- [ay]
- color=blue
- min=-90
- max=90
- [az]
- color=red
- min=-90
- max=90
- [gx]
- color=lime
- min=-90
- max=90
- [gy]
- color=magenta
- min=-90
- max=90
- [gz]
- color=cyan
- min=-90
- max=90
|
[复制链接]
发表于 2013-3-15 14:18:34
楼主
