如何把MPU6050输出的加速度和角速度换算成角度

1 介绍

自己在研究一个题目,就是如何把MPU6050输出的加速度和角速度换算成角度。所以我想在市面上找一个MPU6050,可以自己输出的角度的,这样我就能做一个对比了。同时,能够把IIC引脚留出来的方便我自己开发MPU6050芯片。我在淘宝上找到了一款JY61模块。它内置的是MPU6050芯片。串口直接输出的很简单。给大家看下图片:

JY61

2 MPU6050的工作过程

MPU6050 IMU在单芯片上集成了一个3轴加速度计和一个3轴陀螺仪。陀螺仪沿X、Y和Z轴测量角位置随时间的旋转速度或变化率。它使用MEMS技术和科里奥利效应进行测量。陀螺仪的输出以每秒度数为单位,因此为了获得角度位置,我们只需要对角速度进行积分。另一方面,MPU6050加速度计测量加速度的方式与ADXL345加速度传感器相同。简而言之,它可以测量沿3个轴的重力加速度,并使用一些三角学数学,我们可以计算传感器定位的角度。因此,如果我们融合或组合加速度计和陀螺仪数据,我们可以获得有关传感器方向的非常准确的信息。MPU6050 IMU也称为六轴运动跟踪设备或6 DoF(六自由度)设备,因为它有6个输出,即3个加速度计输出和3个陀螺仪输出。

3 Arduino和MPU6050的连接方法

我们来看看如何使用Arduino连接和读取MPU6050传感器的数据。我们使用I2C协议与Arduino进行通信,因此只需要两条线进行连接,另外还有两条线用于供电。

4 MPU6050的Arduino代码

以下是用于从MPU6050传感器读取数据的Arduino代码。这个代码可以直接复制使用的。在代码下方,您可以找到它的详细说明。

/*

  Arduino and MPU6050 Accelerometer and Gyroscope Sensor Tutorial

*/#include <Wire.h>const int MPU = 0x68; // MPU6050 I2C addressfloat AccX, AccY, AccZ;float GyroX, GyroY, GyroZ;float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;float roll, pitch, yaw;float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;float elapsedTime, currentTime, previousTime;int c = 0;void setup() {  Serial.begin(19200);  Wire.begin();                      // Initialize comunication  Wire.beginTransmission(MPU);      // Start communication with MPU6050 // MPU=0x68  Wire.write(0x6B);                  // Talk to the register 6B  Wire.write(0x00);                  // Make reset - place a 0 into the 6B register  Wire.endTransmission(true);        //end the transmission  /*

  // Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)

  Wire.beginTransmission(MPU);

  Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)

  Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)

  Wire.endTransmission(true);

  // Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)

  Wire.beginTransmission(MPU);

  Wire.write(0x1B);                  // Talk to the GYRO_CONFIG register (1B hex)

  Wire.write(0x10);                  // Set the register bits as 00010000 (1000deg/s full scale)

  Wire.endTransmission(true);

  delay(20);

  */  // Call this function if you need to get the IMU error values for your module  calculate_IMU_error();  delay(20);}void loop() {  // === Read acceleromter data === //  Wire.beginTransmission(MPU);  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)  Wire.endTransmission(false);  Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers  //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value  // Calculating Roll and Pitch from the accelerometer data  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)  // === Read gyroscope data === //  previousTime = currentTime;        // Previous time is stored before the actual time read  currentTime = millis();            // Current time actual time read  elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds  Wire.beginTransmission(MPU);  Wire.write(0x43); // Gyro data first register address 0x43  Wire.endTransmission(false);  Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;  // Correct the outputs with the calculated error values  GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)  GyroY = GyroY - 2; // GyroErrorY ~(2)  GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)  // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees  gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg  gyroAngleY = gyroAngleY + GyroY * elapsedTime;  yaw =  yaw + GyroZ * elapsedTime;  // Complementary filter - combine acceleromter and gyro angle values  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

  // Print the values on the serial monitor  Serial.print(roll);  Serial.print("/");  Serial.print(pitch);  Serial.print("/");  Serial.println(yaw);}void calculate_IMU_error() {  // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.  // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values  // Read accelerometer values 200 times  while (c < 200) {    Wire.beginTransmission(MPU);    Wire.write(0x3B);    Wire.endTransmission(false);    Wire.requestFrom(MPU, 6, true);    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;    // Sum all readings    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));    c++;  }  //Divide the sum by 200 to get the error value  AccErrorX = AccErrorX / 200;  AccErrorY = AccErrorY / 200;  c = 0;  // Read gyro values 200 times  while (c < 200) {    Wire.beginTransmission(MPU);    Wire.write(0x43);    Wire.endTransmission(false);    Wire.requestFrom(MPU, 6, true);    GyroX = Wire.read() << 8 | Wire.read();    GyroY = Wire.read() << 8 | Wire.read();    GyroZ = Wire.read() << 8 | Wire.read();    // Sum all readings    GyroErrorX = GyroErrorX + (GyroX / 131.0);    GyroErrorY = GyroErrorY + (GyroY / 131.0);    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);    c++;  }  //Divide the sum by 200 to get the error value  GyroErrorX = GyroErrorX / 200;  GyroErrorY = GyroErrorY / 200;  GyroErrorZ = GyroErrorZ / 200;  // Print the error values on the Serial Monitor  Serial.print("AccErrorX: ");  Serial.println(AccErrorX);  Serial.print("AccErrorY: ");  Serial.println(AccErrorY);  Serial.print("GyroErrorX: ");  Serial.println(GyroErrorX);  Serial.print("GyroErrorY: ");  Serial.println(GyroErrorY);  Serial.print("GyroErrorZ: ");  Serial.println(GyroErrorZ);}复制代码

代码描述:首先我们需要包含用于I2C通信的Wire.h库,并定义一些存储数据所需的变量。

在setup()函数部分,我们需要初始化wire库并通过电源管理寄存器复位传感器。 为此,我们需要查看传感器的数据手册,从中我们可以看到寄存器地址。

MPU6050电源管理寄存器0x6B

此外,如果需要,我们可以使用配置寄存器为加速度计和陀螺仪选择满量程范围。 对于这个例子,我们将使用加速度计的默认±2g范围和陀螺仪的250度/秒范围。

// Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)  Wire.beginTransmission(MPU);  Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)  Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)  Wire.endTransmission(true);  // Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)  Wire.beginTransmission(MPU);  Wire.write(0x1B);                  // Talk to the GYRO_CONFIG register (1B hex)  Wire.write(0x10);                  // Set the register bits as 00010000 (1000deg/s full scale)  Wire.endTransmission(true);  */复制代码

在loop()函数部分,我们首先读取加速度计的数据。 每个轴的数据存储在两个字节或寄存器中,我们可以从传感器的数据手册中看到这些寄存器的地址。

MPU6050 imu加速度计数据寄存器

为了全部读取它们,我们从第一个寄存器开始,然后使用requiestFrom()函数,我们请求读取X、Y和Z轴的所有6个寄存器。 然后我们从每个寄存器读取数据,并且由于输出是二进制补码,我们将它们相应地组合以获得正确的值。

// === Read acceleromter data === //  Wire.beginTransmission(MPU);  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)  Wire.endTransmission(false);  Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers  //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value 16384.0就是灵敏度如下图  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value

为了获得-1g到+ 1g的输出值,适合计算角度,我们将输出除以先前选择的灵敏度。

mpu6050加速度计灵敏度满量程范围

最后,使用这两个公式,我们从加速度计数据计算滚转角和俯仰角。

  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)

接下来,使用相同的方法我们得到陀螺仪数据。

我们读取了六个陀螺仪寄存器,适当地组合它们的数据并将其除以先前选择的灵敏度,以便以每秒的度数获得输出。

// === Read gyroscope data === //  previousTime = currentTime;        // Previous time is stored before the actual time read  currentTime = millis();            // Current time actual time read  elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds  Wire.beginTransmission(MPU);  Wire.write(0x43); // Gyro data first register address 0x43  Wire.endTransmission(false);  Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

在这里你可以注意到我用一些小的计算误差值来校正输出值,我将在接下来解释它们是如何得到它们的。 因此,当输出以度/秒为单位时,现在我们需要将它们与时间相乘以得到度数。 使用millis()函数在每次读取迭代之前捕获时间值。

// Correct the outputs with the calculated error values  GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)  GyroY = GyroY - 2; // GyroErrorY ~(2)  GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)  // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees  gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg  gyroAngleY = gyroAngleY + GyroY * elapsedTime;  yaw =  yaw + GyroZ * elapsedTime;

最后,我们使用互补滤波器融合加速度计和陀螺仪数据。在这里,我们采用96%的陀螺仪数据,因为它非常准确,不会受到外力的影响。陀螺仪的缺点是存在漂移,或者随着时间的推移在输出中引入误差。因此,从长远来看,我们使用来自加速度计的数据,本例为4%,足以消除陀螺仪的漂移误差。

// Complementary filter - combine acceleromter and gyro angle values  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

但是,由于我们无法从加速度计数据计算偏航,我们无法在其上实现互补滤波器。

在我们看一下结果之前,让我快速解释一下如何获得纠错值。为了计算这些错误,我们可以在传感器处于平坦静止位置时调用calculate_IMU_error()自定义函数。在这里,我们为所有输出做了200个读数,我们将它们相加并将它们除以200。由于我们将传感器保持在平坦静止位置,因此预期输出值应为0。因此,通过此计算,我们可以得到传感器的平均误差。

void calculate_IMU_error() {  // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.  // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values  // Read accelerometer values 200 times  while (c < 200) {    Wire.beginTransmission(MPU);    Wire.write(0x3B);    Wire.endTransmission(false);    Wire.requestFrom(MPU, 6, true);    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;    // Sum all readings    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));    c++;  }  //Divide the sum by 200 to get the error value  AccErrorX = AccErrorX / 200;  AccErrorY = AccErrorY / 200;  c = 0;  // Read gyro values 200 times  while (c < 200) {    Wire.beginTransmission(MPU);    Wire.write(0x43);    Wire.endTransmission(false);    Wire.requestFrom(MPU, 6, true);    GyroX = Wire.read() << 8 | Wire.read();    GyroY = Wire.read() << 8 | Wire.read();    GyroZ = Wire.read() << 8 | Wire.read();    // Sum all readings    GyroErrorX = GyroErrorX + (GyroX / 131.0);    GyroErrorY = GyroErrorY + (GyroY / 131.0);    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);    c++;  }  //Divide the sum by 200 to get the error value  GyroErrorX = GyroErrorX / 200;  GyroErrorY = GyroErrorY / 200;  GyroErrorZ = GyroErrorZ / 200;  // Print the error values on the Serial Monitor  Serial.print("AccErrorX: ");  Serial.println(AccErrorX);  Serial.print("AccErrorY: ");  Serial.println(AccErrorY);  Serial.print("GyroErrorX: ");  Serial.println(GyroErrorX);  Serial.print("GyroErrorY: ");  Serial.println(GyroErrorY);  Serial.print("GyroErrorZ: ");  Serial.println(GyroErrorZ);}

输出结果

我们只需在串行监视器上打印这些值,一旦我们知道它们,我们就可以在前面所示的代码中实现它们,用于滚动和俯仰计算,以及3个陀螺仪输出。

最后,使用Serial.print函数,我们可以在串行监视器上打印Roll、Pitch和Yaw值,看看传感器是否正常工作。

©著作权归作者所有,转载或内容合作请联系作者
  • 序言:七十年代末,一起剥皮案震惊了整个滨河市,随后出现的几起案子,更是在滨河造成了极大的恐慌,老刑警刘岩,带你破解...
    沈念sama阅读 214,837评论 6 496
  • 序言:滨河连续发生了三起死亡事件,死亡现场离奇诡异,居然都是意外死亡,警方通过查阅死者的电脑和手机,发现死者居然都...
    沈念sama阅读 91,551评论 3 389
  • 文/潘晓璐 我一进店门,熙熙楼的掌柜王于贵愁眉苦脸地迎上来,“玉大人,你说我怎么就摊上这事。” “怎么了?”我有些...
    开封第一讲书人阅读 160,417评论 0 350
  • 文/不坏的土叔 我叫张陵,是天一观的道长。 经常有香客问我,道长,这世上最难降的妖魔是什么? 我笑而不...
    开封第一讲书人阅读 57,448评论 1 288
  • 正文 为了忘掉前任,我火速办了婚礼,结果婚礼上,老公的妹妹穿的比我还像新娘。我一直安慰自己,他们只是感情好,可当我...
    茶点故事阅读 66,524评论 6 386
  • 文/花漫 我一把揭开白布。 她就那样静静地躺着,像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上,一...
    开封第一讲书人阅读 50,554评论 1 293
  • 那天,我揣着相机与录音,去河边找鬼。 笑死,一个胖子当着我的面吹牛,可吹牛的内容都是我干的。 我是一名探鬼主播,决...
    沈念sama阅读 39,569评论 3 414
  • 文/苍兰香墨 我猛地睁开眼,长吁一口气:“原来是场噩梦啊……” “哼!你这毒妇竟也来了?” 一声冷哼从身侧响起,我...
    开封第一讲书人阅读 38,316评论 0 270
  • 序言:老挝万荣一对情侣失踪,失踪者是张志新(化名)和其女友刘颖,没想到半个月后,有当地人在树林里发现了一具尸体,经...
    沈念sama阅读 44,766评论 1 307
  • 正文 独居荒郊野岭守林人离奇死亡,尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 37,077评论 2 330
  • 正文 我和宋清朗相恋三年,在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 39,240评论 1 343
  • 序言:一个原本活蹦乱跳的男人离奇死亡,死状恐怖,灵堂内的尸体忽然破棺而出,到底是诈尸还是另有隐情,我是刑警宁泽,带...
    沈念sama阅读 34,912评论 5 338
  • 正文 年R本政府宣布,位于F岛的核电站,受9级特大地震影响,放射性物质发生泄漏。R本人自食恶果不足惜,却给世界环境...
    茶点故事阅读 40,560评论 3 322
  • 文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹,春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 31,176评论 0 21
  • 文/苍兰香墨 我抬头看了看天上的太阳。三九已至,却和暖如春,着一层夹袄步出监牢的瞬间,已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 32,425评论 1 268
  • 我被黑心中介骗来泰国打工, 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留,地道东北人。 一个月前我还...
    沈念sama阅读 47,114评论 2 366
  • 正文 我出身青楼,却偏偏与公主长得像,于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子,可洞房花烛夜当晚...
    茶点故事阅读 44,114评论 2 352

推荐阅读更多精彩内容