如何将加速度计和陀螺仪数据融合在一起

步骤1：MPU-6050模块

MPU-6050模块具有8个引脚，

INT：中断数字输出引脚。

AD0： I2C从地址LSB引脚。这是设备7位从机地址中的第0位。如果连接到VCC，则将其读为逻辑1，并且从机地址更改。

XCL：辅助串行时钟引脚。此引脚用于将其他具有I2C接口功能的传感器SCL引脚连接到MPU-6050。

XDA：辅助串行数据引脚。该引脚用于将其他启用I2C接口的传感器SDA引脚连接到MPU-6050。

SCL：串行时钟引脚。将此引脚连接到微控制器的SCL引脚。 SDA：串行数据引脚。将此引脚连接到微控制器的SDA引脚。

GND：接地引脚。将此引脚接地。

VCC：电源引脚。将此引脚连接到+ 5V DC电源。 MPU-6050模块的从站地址（当AD0 = 0时，即未连接到Vcc）为

从站写地址（SLA + W）： 0xD0

从站读取地址（SLA + R）： 0xD1

步骤2：计算

MPU6050模块的陀螺仪和加速度计传感器数据由2的补码形式的16位原始数据组成。

MPU6050模块的温度传感器数据由16位数据组成（不是2的补码形式）。

现在假设我们选择了

-加速度计满量程范围+/- 2g，灵敏度比例因子为16，384 LSB（Count）/g。

-陀螺仪的满量程范围为+/- 250°/s，灵敏度比例因子为131 LSB（计数）/°/s。然后，

要获取传感器原始数据，我们首先需要对加速度计和陀螺仪的传感器数据进行2的补码运算。在获得传感器原始数据之后，我们可以通过将传感器原始数据除以它们的灵敏度比例因子来计算加速度和角速度，方法如下：

以g（g力）计的加速度计值

ul》

沿X轴的加速度=（加速度计X轴原始数据/16384）g。

沿Y轴的加速度=（加速度计Y轴原始数据/16384）g。

沿Z轴的加速度=（加速度计Z轴原始数据/16384）g。

陀螺仪值，以°/s（每秒度）为单位

沿X轴的角速度=（陀螺仪X轴原始数据/131）°/s。

沿Y轴的角速度=（陀螺仪Y轴原始数据/131）°/s。

沿Z轴的角速度=（陀螺仪Z轴原始数据/131）°/s。

以°/c为单位的温度值（每度摄氏度）

以摄氏度为单位的温度=（（温度传感器数据）/340 + 36.53）°/c。

例如

假设在2‘补码之后得到加速度计X轴原始值= +15454

然后Ax = +15454/16384 = 0.94 g。

更多

所以我们知道我们在+/- 2G和+/- 250deg/s的灵敏度下运行，但是我们的值如何与这些加速度/角度相对应。

这两者都是直线图，我们可以从中得出结果，对于1G，我们将读取16384，对于1deg/sec，我们将读取131.07（尽管由于二进制，0.07将被忽略），这些值只是通过绘制直线图得出的在32767时2G在-32768时为-2G，在250/-250时为-2G。

所以现在我们知道了灵敏度值（16384和131.07），我们只需要减去这些值的偏移量即可。然后

这些对于X和Y值都适用，但是由于Z记录为1G而不是0，因此在除以灵敏度之前，我们需要减去1G（16384）。

步骤3：MPU6050-Atmega328p连接

。..

连接如下：-

MPU6050 《======》 Arduino Nano。

VCC 《====== 》 5v输出引脚

GND 《======》接地引脚

SDA 《======》 A4引脚//串行数据

SCL 《======》 A5引脚//串行时钟

俯仰和横滚计算：

滚动是围绕旋转x轴和螺距是沿y轴的旋转。

结果以弧度为单位。 （乘以180并除以pi即可转换为度）

步骤4：代码和说明

#include

const int MPU=0x68; //I2C address of the MPU-6050

int16_t AcX，AcY，AcZ，Tmp，GyX，GyY，GyZ; //16-bit integers

int AcXcal，AcYcal，AcZcal，GyXcal，GyYcal，GyZcal，tcal; //calibration variables

double t，tx，tf，pitch，roll;

void setup（）

{

Wire.begin（）; //initiate wire library and I2C

Wire.beginTransmission（MPU）; //begin transmission to I2C slave device

Wire.write（0x6B）; // PWR_MGMT_1 register

Wire.write（0）; // set to zero （wakes up the MPU-6050）

Wire.endTransmission（true）; //ends transmission to I2C slave device

Serial.begin（9600）; //serial communication at 9600 bauds

}

void loop（）

{

Wire.beginTransmission（MPU）; //begin transmission to I2C slave device

Wire.write（0x3B）; // starting with register 0x3B （ACCEL_XOUT_H）

Wire.endTransmission（false）; //restarts transmission to I2C slave device

Wire.requestFrom（MPU，14，true）; //request 14 registers in total

//Acceleration data correction

AcXcal = -950;

AcYcal = -300;

AcZcal = 0;

//Temperature correction

tcal = -1600;

//Gyro correction

GyXcal = 480;

GyYcal = 170;

GyZcal = 210;

//read accelerometer data

AcX=Wire.read（）《《8|Wire.read（）; // 0x3B （ACCEL_XOUT_H） 0x3C （ACCEL_XOUT_L）

AcY=Wire.read（）《《8|Wire.read（）; // 0x3D （ACCEL_YOUT_H） 0x3E （ACCEL_YOUT_L）

AcZ=Wire.read（）《《8|Wire.read（）; // 0x3F （ACCEL_ZOUT_H） 0x40 （ACCEL_ZOUT_L）

//read temperature data

Tmp=Wire.read（）《《8|Wire.read（）; // 0x41 （TEMP_OUT_H） 0x42 （TEMP_OUT_L）

//read gyroscope data

GyX=Wire.read（）《《8|Wire.read（）; // 0x43 （GYRO_XOUT_H） 0x44 （GYRO_XOUT_L）

GyY=Wire.read（）《《8|Wire.read（）; // 0x45 （GYRO_YOUT_H） 0x46 （GYRO_YOUT_L）

GyZ=Wire.read（）《《8|Wire.read（）; // 0x47 （GYRO_ZOUT_H） 0x48 （GYRO_ZOUT_L）

//temperature calculation

tx = Tmp + tcal;

t = tx/340 + 36.53; //equation for temperature in degrees C from datasheet

tf = （t * 9/5） + 32; //fahrenheit

//get pitch/roll

getAngle（AcX，AcY，AcZ）;

//printing values to serial port

Serial.print（“Angle： ”）;

Serial.print（“Pitch = ”）; Serial.print（pitch）;

Serial.print（“ Roll = ”）; Serial.println（roll）;

Serial.print（“Accelerometer： ”）;

Serial.print（“X = ”）; Serial.print（AcX + AcXcal）;

Serial.print（“ Y = ”）; Serial.print（AcY + AcYcal）;

Serial.print（“ Z = ”）; Serial.println（AcZ + AcZcal）;

Serial.print（“Temperature in celsius = ”）; Serial.print（t）;

Serial.print（“ fahrenheit = ”）; Serial.println（tf）;

Serial.print（“Gyroscope： ”）;

Serial.print（“X = ”）; Serial.print（GyX + GyXcal）;

Serial.print（“ Y = ”）; Serial.print（GyY + GyYcal）;

Serial.print（“ Z = ”）; Serial.println（GyZ + GyZcal）;

delay（1000）;

}

//function to convert accelerometer values into pitch and roll

void getAngle（int Ax，int Ay，int Az）

{

double x = Ax;

double y = Ay;

double z = Az; pitch = atan（x/sqrt（（y*y） + （z*z）））; //pitch calculation

roll = atan（y/sqrt（（x*x） + （z*z）））; //roll calculation

//converting radians into degrees

pitch = pitch * （180.0/3.14）;

roll = roll * （180.0/3.14） ;

}

-----------------------------------------------------------------------------------------------

Results：-

--------------------------------------------------------------------------------------

Angle： Pitch = 88.89 Roll = -0.47

Accelerometer： X = 15974 Y = -440 Z = 312

Temperature in celsius = 29.38 fahrenheit = 84.88

Gyroscope： X = -111 Y = 341 Z = 211

Angle： Pitch = 89.41 Roll = -0.27

Accelerometer： X = 16102 Y = -380 Z = 172

Temperature in celsius = 29.42 fahrenheit = 84.96

Gyroscope： X = -115 Y = 373 Z = 228

Angle： Pitch = 89.28 Roll = -0.34

Accelerometer： X = 16058 Y = -400 Z = 204

Temperature in celsius = 29.42 fahrenheit = 84.96

Gyroscope： X = -98 Y = 354 Z = 224

Angle： Pitch = 88.83 Roll = -0.54

Accelerometer： X = 15978 Y = -460 Z = 320

Temperature in celsius = 29.33 fahrenheit = 84.79

Gyroscope： X = -124 Y = 376 Z = 207

Angle： Pitch = 89.21 Roll = -0.31

Accelerometer： X = 15978 Y = -392 Z = 228

Temperature in celsius = 29.42 fahrenheit = 84.96

Gyroscope： X = -121 Y = 364 Z = 189

Angle： Pitch = 89.00 Roll = -0.56

Accelerometer： X = 15890 Y = -464 Z = 260

Temperature in celsius = 29.38 fahrenheit = 84.88

Gyroscope： X = -111 Y = 361 Z = 221

Angle： Pitch = 88.67 Roll = -0.65

Accelerometer： X = 16018 Y = -492 Z = 360

Temperature in celsius = 29.38 fahrenheit = 84.88

Gyroscope： X = -130 Y = 340 Z = 216

Angle： Pitch = 88.53 Roll = -0.43

Accelerometer： X = 16110 Y = -428 Z = 432

Temperature in celsius = 29.42 fahrenheit = 84.96

Gyroscope： X = -92 Y = 380 Z = 217

Angle： Pitch = 88.85 Roll = -0.60

Accelerometer： X = 15930 Y = -476 Z = 304

Temperature in celsius = 29.47 fahrenheit = 85.05

Gyroscope： X = -102 Y = 374 Z = 219

Angle： Pitch = 88.87 Roll = -0.24

Accelerometer： X = 16222 Y = -372 Z = 344

Temperature in celsius = 29.52 fahrenheit = 85.13

Gyroscope： X = -96 Y = 351 Z = 226

Angle： Pitch = 89.05 Roll = -0.26

Accelerometer： X = 15970 Y = -376 Z = 284

Temperature in celsius = 29.38 fahrenheit = 84.88

Gyroscope： X = -105 Y = 367 Z = 201

Angle： Pitch = 89.13 Roll = -0.62

Accelerometer： X = 16034 Y = -484 Z = 200

Temperature in celsius = 29.52 fahrenheit = 85.13

Gyroscope： X = -110 Y = 391 Z = 207

Angle： Pitch = 88.98 Roll = -0.51

Accelerometer： X = 16178 Y = -452 Z = 280

Temperature in celsius = 29.47 fahrenheit = 85.05

Gyroscope： X = -117 Y = 379 Z = 221

Angle： Pitch = 89.27 Roll = -0.43

Accelerometer： X = 16066 Y = -428 Z = 192

Temperature in celsius = 29.42 fahrenheit = 84.96

Gyroscope： X = -101 Y = 359 Z = 208

Angle： Pitch = 89.31 Roll = -0.19

Accelerometer： X = 16150 Y = -356 Z = 212

Temperature in celsius = 29.52 fahrenheit = 85.13

Gyroscope： X = -115 Y = 361 Z = 189

Angle： Pitch = 88.76 Roll = -0.51

Accelerometer： X = 16026 Y = -452 Z = 348

Temperature in celsius = 29.42 fahrenheit = 84.96

Gyroscope： X = -139 Y = 368 Z = 192

Angle： Pitch = 88.57 Roll = -0.69

Accelerometer： X = 16086 Y = -504 Z = 388

Temperature in celsius = 29.33 fahrenheit = 84.79

Gyroscope： X = -118 Y = 352 Z = 214

步骤5：了解倾斜角

加速度计

地球的重力是恒定的加速度，力始终指向降到地球中心。

当加速度计与重力平行时，测得的加速度将为1G，当加速度计与重力垂直时，加速度将为0G。

倾斜角可以使用以下公式从测量的加速度中计算出：

θ= sin-1（测量的加速度/重力加速度）

陀螺仪

陀螺仪（又称速率传感器）用于测量角速度（ω）。

为了获得机器人的倾斜角，我们需要将陀螺仪的数据如下式所示：

ω=dθ/dt，

θ=∫ωdt

Gyro和加速度传感器Fusio n

在研究了陀螺仪和加速度计的特性之后，我们知道它们各有优缺点。根据加速度计数据计算出的倾斜角具有较慢的响应时间，而根据陀螺仪数据计算出的积分倾斜角会在一段时间内发生漂移。换句话说，可以说加速度计数据对长期有用，而陀螺仪数据对短期有用。

责任编辑：wv