|
|
1 介紹
自己在研究一個(gè)題目，就是如何把MPU6050輸出的加速度和角速度換算成角度。所以我想在市面上找一個(gè)MPU6050，可以自己輸出的角度的，這樣我就能做一個(gè)對(duì)比了。同時(shí)，能夠把IIC引腳留出來(lái)的方便我自己開(kāi)發(fā)MPU6050芯片。我在淘寶上找到了一款JY61模塊。它內(nèi)置的是MPU6050芯片。串口直接輸出的很簡(jiǎn)單。給大家看下圖片：
JY61
2 MPU6050的工作過(guò)程
MPU6050 IMU在單芯片上集成了一個(gè)3軸加速度計(jì)和一個(gè)3軸陀螺儀。陀螺儀沿X、Y和Z軸測(cè)量角位置隨時(shí)間的旋轉(zhuǎn)速度或變化率。它使用MEMS技術(shù)和科里奧利效應(yīng)進(jìn)行測(cè)量。陀螺儀的輸出以每秒度數(shù)為單位，因此為了獲得角度位置，我們只需要對(duì)角速度進(jìn)行積分。另一方面，MPU6050加速度計(jì)測(cè)量加速度的方式與ADXL345加速度傳感器相同。簡(jiǎn)而言之，它可以測(cè)量沿3個(gè)軸的重力加速度，并使用一些三角學(xué)數(shù)學(xué)，我們可以計(jì)算傳感器定位的角度。因此，如果我們?nèi)诤匣蚪M合加速度計(jì)和陀螺儀數(shù)據(jù)，我們可以獲得有關(guān)傳感器方向的非常準(zhǔn)確的信息。MPU6050 IMU也稱為六軸運(yùn)動(dòng)跟蹤設(shè)備或6 DoF（六自由度）設(shè)備，因?yàn)樗?個(gè)輸出，即3個(gè)加速度計(jì)輸出和3個(gè)陀螺儀輸出。
3 Arduino和MPU6050的連接方法
我們來(lái)看看如何使用Arduino連接和讀取MPU6050傳感器的數(shù)據(jù)。我們使用I2C協(xié)議與Arduino進(jìn)行通信，因此只需要兩條線進(jìn)行連接，另外還有兩條線用于供電。
4 MPU6050的Arduino代碼
以下是用于從MPU6050傳感器讀取數(shù)據(jù)的Arduino代碼。這個(gè)代碼可以直接復(fù)制使用的。在代碼下方，您可以找到它的詳細(xì)說(shuō)明。
*
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
/*
// 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庫(kù)，并定義一些存儲(chǔ)數(shù)據(jù)所需的變量。
在setup()函數(shù)部分，我們需要初始化wire庫(kù)并通過(guò)電源管理寄存器復(fù)位傳感器。 為此，我們需要查看傳感器的數(shù)據(jù)手冊(cè)，從中我們可以看到寄存器地址。
MPU6050電源管理寄存器0x6B
此外，如果需要，我們可以使用配置寄存器為加速度計(jì)和陀螺儀選擇滿量程范圍。 對(duì)于這個(gè)例子，我們將使用加速度計(jì)的默認(rèn)±2g范圍和陀螺儀的250度/秒范圍。
在loop()函數(shù)部分，我們首先讀取加速度計(jì)的數(shù)據(jù)。 每個(gè)軸的數(shù)據(jù)存儲(chǔ)在兩個(gè)字節(jié)或寄存器中，我們可以從傳感器的數(shù)據(jù)手冊(cè)中看到這些寄存器的地址。
MPU6050 imu加速度計(jì)數(shù)據(jù)寄存器
為了全部讀取它們，我們從第一個(gè)寄存器開(kāi)始，然后使用requiestFrom()函數(shù)，我們請(qǐng)求讀取X、Y和Z軸的所有6個(gè)寄存器。 然后我們從每個(gè)寄存器讀取數(shù)據(jù)，并且由于輸出是二進(jìn)制補(bǔ)碼，我們將它們相應(yīng)地組合以獲得正確的值。
[backcolor=inherit !important]
為了獲得-1g到+ 1g的輸出值，適合計(jì)算角度，我們將輸出除以先前選擇的靈敏度。
mpu6050加速度計(jì)靈敏度滿量程范圍
最后，使用這兩個(gè)公式，我們從加速度計(jì)數(shù)據(jù)計(jì)算滾轉(zhuǎn)角和俯仰角。
[backcolor=inherit !important]
接下來(lái)，使用相同的方法我們得到陀螺儀數(shù)據(jù)。
我們讀取了六個(gè)陀螺儀寄存器，適當(dāng)?shù)亟M合它們的數(shù)據(jù)并將其除以先前選擇的靈敏度，以便以每秒的度數(shù)獲得輸出。
[backcolor=inherit !important]
在這里你可以注意到我用一些小的計(jì)算誤差值來(lái)校正輸出值，我將在接下來(lái)解釋它們是如何得到它們的。 因此，當(dāng)輸出以度/秒為單位時(shí)，現(xiàn)在我們需要將它們與時(shí)間相乘以得到度數(shù)。 使用millis()函數(shù)在每次讀取迭代之前捕獲時(shí)間值。
[backcolor=inherit !important]
最后，我們使用互補(bǔ)濾波器融合加速度計(jì)和陀螺儀數(shù)據(jù)。在這里，我們采用96％的陀螺儀數(shù)據(jù)，因?yàn)樗浅?zhǔn)確，不會(huì)受到外力的影響。陀螺儀的缺點(diǎn)是存在漂移，或者隨著時(shí)間的推移在輸出中引入誤差。因此，從長(zhǎng)遠(yuǎn)來(lái)看，我們使用來(lái)自加速度計(jì)的數(shù)據(jù)，本例為4％，足以消除陀螺儀的漂移誤差。
[backcolor=inherit !important]
但是，由于我們無(wú)法從加速度計(jì)數(shù)據(jù)計(jì)算偏航，我們無(wú)法在其上實(shí)現(xiàn)互補(bǔ)濾波器。
在我們看一下結(jié)果之前，讓我快速解釋一下如何獲得糾錯(cuò)值。為了計(jì)算這些錯(cuò)誤，我們可以在傳感器處于平坦靜止位置時(shí)調(diào)用calculate_IMU_error()自定義函數(shù)。在這里，我們?yōu)樗休敵鲎隽?00個(gè)讀數(shù)，我們將它們相加并將它們除以200。由于我們將傳感器保持在平坦靜止位置，因此預(yù)期輸出值應(yīng)為0。因此，通過(guò)此計(jì)算，我們可以得到傳感器的平均誤差。
[backcolor=inherit !important]
輸出結(jié)果
我們只需在串行監(jiān)視器上打印這些值，一旦我們知道它們，我們就可以在前面所示的代碼中實(shí)現(xiàn)它們，用于滾動(dòng)和俯仰計(jì)算，以及3個(gè)陀螺儀輸出。
最后，使用Serial.print函數(shù)，我們可以在串行監(jiān)視器上打印Roll、Pitch和Yaw值，看看傳感器是否正常工作。
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
