1 介紹
自己在研究一個題目，就是如何把MPU6050輸出的加速度和角速度換算成角度吴汪。所以我想在市面上找一個MPU6050蜈亩，可以自己輸出的角度的，這樣我就能做一個對比了修赞。同時婶恼，能夠把IIC引腳留出來的方便我自己開發(fā)MPU6050芯片桑阶。我在淘寶上找到了一款JY61模塊。它內(nèi)置的是MPU6050芯片勾邦。串口直接輸出的很簡單蚣录。給大家看下圖片：
JY61
2 MPU6050的工作過程
MPU6050 IMU在單芯片上集成了一個3軸加速度計和一個3軸陀螺儀。陀螺儀沿X眷篇、Y和Z軸測量角位置隨時間的旋轉(zhuǎn)速度或變化率萎河。它使用MEMS技術(shù)和科里奧利效應(yīng)進(jìn)行測量。陀螺儀的輸出以每秒度數(shù)為單位蕉饼，因此為了獲得角度位置虐杯，我們只需要對角速度進(jìn)行積分。另一方面昧港，MPU6050加速度計測量加速度的方式與ADXL345加速度傳感器相同擎椰。簡而言之，它可以測量沿3個軸的重力加速度创肥，并使用一些三角學(xué)數(shù)學(xué)达舒，我們可以計算傳感器定位的角度。因此叹侄，如果我們?nèi)诤匣蚪M合加速度計和陀螺儀數(shù)據(jù)休弃，我們可以獲得有關(guān)傳感器方向的非常準(zhǔn)確的信息。MPU6050 IMU也稱為六軸運(yùn)動跟蹤設(shè)備或6 DoF（六自由度）設(shè)備圈膏，因為它有6個輸出，即3個加速度計輸出和3個陀螺儀輸出篙骡。
3 Arduino和MPU6050的連接方法
我們來看看如何使用Arduino連接和讀取MPU6050傳感器的數(shù)據(jù)稽坤。我們使用I2C協(xié)議與Arduino進(jìn)行通信，因此只需要兩條線進(jìn)行連接糯俗，另外還有兩條線用于供電尿褪。
4 MPU6050的Arduino代碼
以下是用于從MPU6050傳感器讀取數(shù)據(jù)的Arduino代碼。這個代碼可以直接復(fù)制使用的得湘。在代碼下方杖玲，您可以找到它的詳細(xì)說明。
/*
? 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);}復(fù)制代碼
代碼描述：首先我們需要包含用于I2C通信的Wire.h庫淘正，并定義一些存儲數(shù)據(jù)所需的變量摆马。
在setup()函數(shù)部分，我們需要初始化wire庫并通過電源管理寄存器復(fù)位傳感器鸿吆。 為此囤采，我們需要查看傳感器的數(shù)據(jù)手冊，從中我們可以看到寄存器地址惩淳。
MPU6050電源管理寄存器0x6B
此外蕉毯，如果需要，我們可以使用配置寄存器為加速度計和陀螺儀選擇滿量程范圍。 對于這個例子代虾，我們將使用加速度計的默認(rèn)±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);? */復(fù)制代碼
在loop()函數(shù)部分，我們首先讀取加速度計的數(shù)據(jù)棉磨。 每個軸的數(shù)據(jù)存儲在兩個字節(jié)或寄存器中江掩，我們可以從傳感器的數(shù)據(jù)手冊中看到這些寄存器的地址。
MPU6050 imu加速度計數(shù)據(jù)寄存器
為了全部讀取它們含蓉，我們從第一個寄存器開始频敛，然后使用requiestFrom()函數(shù)，我們請求讀取X馅扣、Y和Z軸的所有6個寄存器斟赚。 然后我們從每個寄存器讀取數(shù)據(jù)，并且由于輸出是二進(jìn)制補(bǔ)碼差油，我們將它們相應(yīng)地組合以獲得正確的值拗军。
// === 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加速度計靈敏度滿量程范圍
最后，使用這兩個公式妆偏，我們從加速度計數(shù)據(jù)計算滾轉(zhuǎn)角和俯仰角刃鳄。
? 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)
接下來，使用相同的方法我們得到陀螺儀數(shù)據(jù)钱骂。
我們讀取了六個陀螺儀寄存器叔锐，適當(dāng)?shù)亟M合它們的數(shù)據(jù)并將其除以先前選擇的靈敏度，以便以每秒的度數(shù)獲得輸出见秽。
// === 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;
在這里你可以注意到我用一些小的計算誤差值來校正輸出值愉烙，我將在接下來解釋它們是如何得到它們的。 因此解取，當(dāng)輸出以度/秒為單位時步责，現(xiàn)在我們需要將它們與時間相乘以得到度數(shù)。 使用millis()函數(shù)在每次讀取迭代之前捕獲時間值禀苦。
// 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;
最后蔓肯，我們使用互補(bǔ)濾波器融合加速度計和陀螺儀數(shù)據(jù)。在這里伦忠，我們采用96％的陀螺儀數(shù)據(jù)省核，因為它非常準(zhǔn)確，不會受到外力的影響昆码。陀螺儀的缺點是存在漂移气忠，或者隨著時間的推移在輸出中引入誤差邻储。因此，從長遠(yuǎn)來看旧噪，我們使用來自加速度計的數(shù)據(jù)吨娜，本例為4％，足以消除陀螺儀的漂移誤差淘钟。
// Complementary filter - combine acceleromter and gyro angle values? roll = 0.96 * gyroAngleX + 0.04 * accAngleX;? pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
但是宦赠，由于我們無法從加速度計數(shù)據(jù)計算偏航，我們無法在其上實現(xiàn)互補(bǔ)濾波器米母。
在我們看一下結(jié)果之前勾扭，讓我快速解釋一下如何獲得糾錯值。為了計算這些錯誤铁瞒，我們可以在傳感器處于平坦靜止位置時調(diào)用calculate_IMU_error()自定義函數(shù)妙色。在這里，我們?yōu)樗休敵鲎隽?00個讀數(shù)慧耍，我們將它們相加并將它們除以200身辨。由于我們將傳感器保持在平坦靜止位置，因此預(yù)期輸出值應(yīng)為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);}
輸出結(jié)果
我們只需在串行監(jiān)視器上打印這些值定庵，一旦我們知道它們，我們就可以在前面所示的代碼中實現(xiàn)它們踪危，用于滾動和俯仰計算洗贰，以及3個陀螺儀輸出。
最后陨倡，使用Serial.print函數(shù)，我們可以在串行監(jiān)視器上打印Roll许布、Pitch和Yaw值兴革，看看傳感器是否正常工作。
