stm32mpu6050融合

在STM32微控制器上实现MPU6050（6轴IMU，含加速度计和陀螺仪）的传感器数据融合，通常需要结合滤波算法（如互补滤波或卡尔曼滤波）来优化姿态角（俯仰角、横滚角、偏航角）的精度。以下是实现步骤和关键代码示例：

1. 硬件连接

MPU6050 -> STM32：
- VCC → 3.3V
- GND → GND
- SCL → I2C时钟引脚（如PB6）
- SDA → I2C数据引脚（如PB7）
- INT → 可选中断引脚（用于数据就绪中断）

2. 软件实现步骤

(1) 初始化I2C和MPU6050

#include "stm32f1xx_hal.h"
#include "mpu6050.h"  // 需自行封装或使用第三方库

// 初始化I2C
void I2C_Init() {
  hi2c1.Instance = I2C1;
  hi2c1.Init.ClockSpeed = 400000;  // 400kHz
  hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
  hi2c1.Init.OwnAddress1 = 0;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  HAL_I2C_Init(&hi2c1);
}

// 初始化MPU6050
void MPU6050_Init() {
  uint8_t check = MPU6050_Read_Byte(MPU6050_RA_WHO_AM_I);
  if (check == 0x68) {  // 确认设备ID正确
    // 配置加速度计量程 ±2g
    MPU6050_Write_Byte(MPU6050_RA_ACCEL_CONFIG, 0x00);
    // 配置陀螺仪量程 ±250dps
    MPU6050_Write_Byte(MPU6050_RA_GYRO_CONFIG, 0x00);
    // 设置采样率分频（1kHz/(1+7)=125Hz）
    MPU6050_Write_Byte(MPU6050_RA_SMPLRT_DIV, 0x07);
    // 关闭休眠模式
    MPU6050_Write_Byte(MPU6050_RA_PWR_MGMT_1, 0x01);
  }
}

(2) 读取原始数据

typedef struct {
  int16_t ax, ay, az;  // 加速度计原始数据
  int16_t gx, gy, gz;  // 陀螺仪原始数据
} MPU6050_Data;

void MPU6050_Read_Raw(MPU6050_Data *data) {
  uint8_t buffer[14];
  // 从0x3B寄存器开始读取14字节数据
  HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, MPU6050_RA_ACCEL_XOUT_H, 1, buffer, 14, 100);

  // 解析数据
  data->ax = (buffer[0] << 8) | buffer[1];
  data->ay = (buffer[2] << 8) | buffer[3];
  data->az = (buffer[4] << 8) | buffer[5];
  data->gx = (buffer[8] << 8) | buffer[9];
  data->gy = (buffer[10] << 8) | buffer[11];
  data->gz = (buffer[12] << 8) | buffer[13];
}

(3) 数据校准

加速度计校准：静止时读取多组数据，计算零点偏移。
陀螺仪校准：静止时读取陀螺仪数据，计算零偏（需减去偏移）。

// 示例：陀螺仪零偏校准
int16_t gx_offset, gy_offset, gz_offset;
void Calibrate_Gyro() {
  int32_t sum_gx = 0, sum_gy = 0, sum_gz = 0;
  for (int i=0; i<1000; i++) {
    MPU6050_Data data;
    MPU6050_Read_Raw(&data);
    sum_gx += data.gx;
    sum_gy += data.gy;
    sum_gz += data.gz;
    HAL_Delay(2);
  }
  gx_offset = sum_gx / 1000;
  gy_offset = sum_gy / 1000;
  gz_offset = sum_gz / 1000;
}

(4) 数据融合（互补滤波）

// 计算俯仰角(pitch)和横滚角(roll)
float dt = 0.01;  // 采样时间间隔（例如100Hz）
float alpha = 0.98; // 互补滤波系数

void Complementary_Filter(float *pitch, float *roll) {
  MPU6050_Data data;
  MPU6050_Read_Raw(&data);

  // 去除零偏
  float gx = (data.gx - gx_offset) / 131.0;  // ±250dps灵敏度，转换为dps
  float gy = (data.gy - gy_offset) / 131.0;
  float gz = (data.gz - gz_offset) / 131.0;

  // 加速度计计算姿态角（弧度）
  float acc_pitch = atan2(data.ay, data.az) * 180 / PI;
  float acc_roll = atan2(data.ax, data.az) * 180 / PI;

  // 陀螺仪积分
  static float gyro_pitch = 0, gyro_roll = 0;
  gyro_pitch += gy * dt;  // 注意陀螺仪轴与角度的对应关系
  gyro_roll += gx * dt;

  // 互补滤波融合
  *pitch = alpha * (*pitch + gy * dt) + (1 - alpha) * acc_pitch;
  *roll = alpha * (*roll + gx * dt) + (1 - alpha) * acc_roll;
}

(5) 卡尔曼滤波（高阶优化）

若需要更高精度，可改用卡尔曼滤波。需定义状态变量和协方差矩阵，代码较复杂，以下为简化示例：

// 定义卡尔曼结构体
typedef struct {
  float Q_angle;   // 过程噪声协方差
  float Q_gyro;    // 陀螺仪噪声协方差
  float R_accel;   // 加速度计测量噪声
  float angle;     // 估计角度
  float bias;      // 陀螺仪零偏
  float P[2][2];   // 协方差矩阵
} KalmanFilter;

// 卡尔曼滤波更新
float Kalman_Update(KalmanFilter *kf, float acc_angle, float gyro_rate, float dt) {
  // 预测步骤
  kf->angle += (gyro_rate - kf->bias) * dt;
  kf->P[0][0] += dt * (dt*kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle);
  kf->P[0][1] -= dt * kf->P[1][1];
  kf->P[1][0] -= dt * kf->P[1][1];
  kf->P[1][1] += kf->Q_gyro * dt;

  // 更新步骤
  float y = acc_angle - kf->angle;
  float S = kf->P[0][0] + kf->R_accel;
  float K[2] = {kf->P[0][0]/S, kf->P[1][0]/S};
  kf->angle += K[0] * y;
  kf->bias += K[1] * y;

  // 更新协方差矩阵
  float P00_temp = kf->P[0][0];
  kf->P[0][0] -= K[0] * P00_temp;
  kf->P[0][1] -= K[0] * kf->P[0][1];
  kf->P[1][0] -= K[1] * P00_temp;
  kf->P[1][1] -= K[1] * kf->P[0][1];

  return kf->angle;
}