姿态解算代码详解

#include <Wire.h>
#include <math.h>

// MPU6050引脚定义
const int MPU6050_ADDR = 0x68; // I2C地址
const int SCL_PIN = 22;         // GPIO22
const int SDA_PIN = 21;         // GPIO21

// 校准参数
float accelBias[3] = {0, 0, 0};
float gyroBias[3] = {0, 0, 0};

// 姿态变量
float roll = 0, pitch = 0, yaw = 0;
unsigned long prevTime = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin(SDA_PIN, SCL_PIN, 400000); // 400kHz I2C
  
  // 初始化MPU6050
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B); // PWR_MGMT_1寄存器
  Wire.write(0);    // 解除休眠状态
  Wire.endTransmission(true);
  
  // 校准传感器（放置传感器水平静止2秒）
  calibrateMPU6050();
  Serial.println("MPU6050 Ready");
}

void loop() {
  // 读取原始数据
  int16_t accelX, accelY, accelZ, gyroX, gyroY, gyroZ;
  readMPU6050(accelX, accelY, accelZ, gyroX, gyroY, gyroZ);
  
  // 转换为实际物理量
  float accelData[3] = {
    (accelX / 16384.0) - accelBias[0], // ±2g范围
    (accelY / 16384.0) - accelBias[1],
    (accelZ / 16384.0) - accelBias[2]
  };
  
  float gyroData[3] = {
    (gyroX / 131.0) - gyroBias[0],     // ±250°/s范围
    (gyroY / 131.0) - gyroBias[1],
    (gyroZ / 131.0) - gyroBias[2]
  };
  
  // 计算时间间隔（单位：秒）
  unsigned long currentTime = micros();
  float deltaTime = (currentTime - prevTime) / 1000000.0;
  prevTime = currentTime;
  
  // 姿态解算（互补滤波）
  calculateAttitude(accelData, gyroData, deltaTime);
  
  // 串口输出
  Serial.print("Roll: "); Serial.print(roll);
  Serial.print("°\tPitch: "); Serial.print(pitch);
  Serial.print("°\tYaw: "); Serial.print(yaw); Serial.println("°");
  
  delay(50); // 控制输出频率
}

// 读取MPU6050原始数据
void readMPU6050(int16_t &ax, int16_t &ay, int16_t &az, int16_t &gx, int16_t &gy, int16_t &gz) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x3B); // 从ACCEL_XOUT_H开始读取
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 14, true);
  
  ax = Wire.read() << 8 | Wire.read();
  ay = Wire.read() << 8 | Wire.read();
  az = Wire.read() << 8 | Wire.read();
  Wire.read(); // 跳过温度数据
  Wire.read();
  gx = Wire.read() << 8 | Wire.read();
  gy = Wire.read() << 8 | Wire.read();
  gz = Wire.read() << 8 | Wire.read();
}

// 校准MPU6050
void calibrateMPU6050() {
  Serial.println("Calibrating MPU6050... Keep sensor stationary!");
  delay(2000);
  
  int32_t accelSum[3] = {0}, gyroSum[3] = {0};
  for (int i = 0; i < 200; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    readMPU6050(ax, ay, az, gx, gy, gz);
    accelSum[0] += ax; accelSum[1] += ay; accelSum[2] += az;
    gyroSum[0] += gx; gyroSum[1] += gy; gyroSum[2] += gz;
    delay(10);
  }
  
  accelBias[0] = accelSum[0] / 200 / 16384.0;
  accelBias[1] = accelSum[1] / 200 / 16384.0;
  accelBias[2] = (accelSum[2] / 200 - 16384) / 16384.0; // 减去1g重力
  
  gyroBias[0] = gyroSum[0] / 200 / 131.0;
  gyroBias[1] = gyroSum[1] / 200 / 131.0;
  gyroBias[2] = gyroSum[2] / 200 / 131.0;
  
  Serial.println("Calibration Complete");
}

// 互补滤波姿态解算
void calculateAttitude(float accel[3], float gyro[3], float dt) {
  // 加速度计计算姿态（单位：度）
  float accRoll = atan2(accel[1], accel[2]) * 180 / PI;
  float accPitch = atan2(-accel[0], sqrt(accel[1]*accel[1] + accel[2]*accel[2])) * 180 / PI;
  
  // 互补滤波系数（0.98依赖陀螺仪，0.02依赖加速度计）
  const float alpha = 0.98;
  
  // 融合数据
  roll = alpha * (roll + gyro[0] * dt) + (1 - alpha) * accRoll;
  pitch = alpha * (pitch + gyro[1] * dt) + (1 - alpha) * accPitch;
  yaw += gyro[2] * dt; // 陀螺仪直接积分
}

1. 代码结构解析

1.1 硬件配置部分

const int MPU6050_ADDR = 0x68;  // I2C器件地址
const int SCL_PIN = 22;         // I2C时钟引脚
const int SDA_PIN = 21;         // I2C数据引脚

• 采用硬件I2C接口，时钟频率设置为400kHz
• 标准MPU6050地址为0x68（AD0引脚接地时）

1.2 核心变量

float accelBias[3], gyroBias[3];  // 校准偏移量
float roll, pitch, yaw;           // 欧拉角
unsigned long prevTime;           // 时间戳

• 使用三轴校准参数补偿传感器误差
• 采用micros()高精度计时实现dt计算

1.3 初始化流程

void setup() {
  // 1. 串口初始化
  // 2. I2C总线启动
  // 3. 唤醒MPU6050
  // 4. 自动校准
}

• 通过PWR_MGMT_1寄存器(0x6B)解除休眠
• 校准过程需保持设备静止2秒

1.4 主循环逻辑

void loop() {
  // 1. 读取原始数据
  // 2. 单位转换和校准补偿
  // 3. 计算时间间隔dt
  // 4. 互补滤波姿态解算
  // 5. 串口输出结果
}

• 20Hz数据更新频率（delay(50)）
• 同时输出roll/pitch/yaw三轴角度

2. 关键算法说明

2.1 数据读取协议

void readMPU6050() {
  // 寄存器0x3B起始的14字节数据:
  // ACCEL_XOUT_H → ACCEL_ZOUT_L → TEMP_OUT_H → GYRO_XOUT_H → GYRO_ZOUT_L
}

• 采用连续读取模式提高效率
• 数据格式：大端序16位补码

2.2 校准算法

void calibrateMPU6050() {
  // 加速度计：扣除重力影响
  // 陀螺仪：计算零偏平均值
}

• 加速度计Z轴默认减去1g(16384 LSB)
• 采集200个样本进行统计分析

2.3 互补滤波实现

void calculateAttitude() {
  // 加速度计求瞬时角度
  // 陀螺仪积分得到趋势角度
  // 加权融合(α=0.98)
}

• 加速度计：atan2三角函数解算
• 陀螺仪：角度=角速度×时间积分
• 动态权重抑制各自缺点

3. 代码流程图

4. 关键参数说明

参数	计算公式	物理意义
加速度计灵敏度	16384 LSB/g (±2g)	每g重力对应的数字量
陀螺仪灵敏度	131 LSB/°/s (±250°/s)	每度/秒对应的数字量
互补滤波系数α	0.98	陀螺仪数据权重占比
校准采样数	200次	统计平均的样本数量