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

1 介绍

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

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 address
float 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
  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 da