MPU6050 是一个集成了三轴陀螺仪和三轴加速度计的惯性测量单元（IMU），常用来测量角度和角速度。
计算 MPU6050 的姿态角度（如俯仰角 Pitch、横滚角 Roll）主要有两种常用方法：
1. 利用加速度计计算角度（静态）
加速度计测量的是重力分量，根据加速度在各轴上的分量计算俯仰角和横滚角：
- 俯仰角 Pitch：
Pitch=arctan(axay2+az2)×180π
- 横滚角 Roll：
Roll=arctan(ayax2+az2)×180π
其中 ax,ay,az 是加速度计在三个轴上的读数（单位一般是 g）。
2. 利用陀螺仪积分角度（动态）
陀螺仪给出角速度 ωx,ωy,ωz，通过积分角速度计算角度：θ(t)=θ(t−1)+ω×Δt
但是陀螺仪有漂移，积分误差会累积。
3. 结合加速度计和陀螺仪 — 卡尔曼滤波或互补滤波
为克服加速度计受加速度影响大，陀螺仪漂移问题，通常采用滤波算法：
- 互补滤波（简单实现）：
Angle=α×(Angle+ω×Δt)+(1−α)×AccAngle
α 一般取 0.95 左右。
4. 示例代码（Arduino 风格 C++）
#include <Wire.h>
#include <MPU6050.h>
MPU6050 mpu;
float gyroAngleX, gyroAngleY, gyroAngleZ;
float accAngleX, accAngleY;
float pitch, roll;
unsigned long timer;
void setup() {
Serial.begin(115200);
Wire.begin();
mpu.initialize();
timer = millis();
}
void loop() {
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// 加速度计角度
accAngleX = atan2(ay, az) * 180 / PI;
accAngleY = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / PI;
// 计算陀螺仪角度增量
float dt = (millis() - timer) / 1000.0;
timer = millis();
gyroAngleX += gx * dt / 131.0; // 陀螺仪灵敏度调整（假设±250度/秒）
gyroAngleY += gy * dt / 131.0;
// 互补滤波
pitch = 0.98 * (pitch + gyroAngleX * dt) + 0.02 * accAngleX;
roll = 0.98 * (roll + gyroAngleY * dt) + 0.02 * accAngleY;
Serial.print("Pitch: "); Serial.print(pitch);
Serial.print(" Roll: "); Serial.println(roll);
delay(10);
}
5. 总结
- 加速度计角度适合静态测量，噪声较大；
- 陀螺仪积分角度适合短时间动态，但会漂移；
- 互补滤波结合两者优点，实用性好；
- 如果要求更高精度，考虑卡尔曼滤波或使用 MPU6050 内部 DMP（数字运动处理器）；
下面给你一个 Python 版的 MPU6050 角度计算示例，包含加速度计、陀螺仪读数解析，结合互补滤波计算俯仰角 Pitch 和横滚角 Roll。
Python 示例（使用 smbus2 读取 MPU6050）
import smbus2
import time
import math
# MPU6050寄存器地址
MPU6050_ADDR = 0x68
PWR_MGMT_1 = 0x6B
ACCEL_XOUT_H = 0x3B
GYRO_XOUT_H = 0x43
bus = smbus2.SMBus(1) # 树莓派I2C bus编号通常是1
# 初始化MPU6050
bus.write_byte_data(MPU6050_ADDR, PWR_MGMT_1, 0)
def read_word(reg):
high = bus.read_byte_data(MPU6050_ADDR, reg)
low = bus.read_byte_data(MPU6050_ADDR, reg + 1)
val = (high << 8) + low
if val >= 0x8000:
val = -((65535 - val) + 1)
return val
def get_accel_data():
ax = read_word(ACCEL_XOUT_H)
ay = read_word(ACCEL_XOUT_H + 2)
az = read_word(ACCEL_XOUT_H + 4)
return ax, ay, az
def get_gyro_data():
gx = read_word(GYRO_XOUT_H)
gy = read_word(GYRO_XOUT_H + 2)
gz = read_word(GYRO_XOUT_H + 4)
return gx, gy, gz
# 互补滤波参数
alpha = 0.98
dt = 0.01 # 10ms采样间隔
pitch = 0.0
roll = 0.0
try:
while True:
ax, ay, az = get_accel_data()
gx, gy, gz = get_gyro_data()
# 加速度计转为g值
ax /= 16384.0
ay /= 16384.0
az /= 16384.0
# 陀螺仪转为角速度 (deg/s)，灵敏度为±250度/s，16.4 LSB/deg/s
gx /= 131.0
gy /= 131.0
gz /= 131.0
# 计算加速度计角度 (单位：度)
acc_pitch = math.degrees(math.atan2(ay, math.sqrt(ax*ax + az*az)))
acc_roll = math.degrees(math.atan2(-ax, az))
# 互补滤波计算角度
pitch = alpha * (pitch + gx * dt) + (1 - alpha) * acc_pitch
roll = alpha * (roll + gy * dt) + (1 - alpha) * acc_roll
print(f"Pitch: {pitch:.2f} Roll: {roll:.2f}")
time.sleep(dt)
except KeyboardInterrupt:
print("结束")
说明：
- 读取加速度计和陀螺仪原始数据，并换算成实际单位；
- 采用简单互补滤波公式融合两种传感器数据；
- 采样间隔
dt根据实际硬件设定（这里是10毫秒）； - 运行环境需要有 I2C 接口和 MPU6050 硬件支持，如树莓派。
如果你想要：
- C++ / Arduino 版本代码；
- Java 版本算法实现；
- 或更复杂的卡尔曼滤波方案；
告诉我，我可以帮你继续提供。
发表回复