基于icm42688的rpy轴计算代码

最新推荐文章于 2026-06-21 17:52:10 发布

原创最新推荐文章于 2026-06-21 17:52:10 发布 · 413 阅读

1 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

标签

#stm32

开发板推荐：天空星STM32F407VET6开发板

超高性价比 STM32主控 | 超高主频 | 一板兼容百芯 | 比赛神器 | 沉金彩色丝印

点击查看

使用icm42688 6轴imu，融合加速度计和陀螺仪数据，输出roll，pitch，yaw轴角度。

IDE：arduino，主控：esp32。

imu i2c地址：0x68，波特率115200。

1.mahony

#include <Wire.h>


#define ICM42688_ADDR      0x68
#define WHO_AM_I           0x75
#define EXPECTED_WHO_AM_I  0x47
#define REG_BANK_SEL       0x76
#define DEVICE_CONFIG      0x11
#define ACCEL_CONFIG0      0x50
#define GYRO_CONFIG0       0x4F
#define PWR_MGMT0          0x4E
#define ACCEL_DATA_X1      0x1F   // 加速度计数据起始地址
#define GYRO_DATA_X1       0x25   // 陀螺仪数据起始地址


#define GYRO_SENS          (1000.0f / 32768.0f)   // ±1000°/s -> 度/秒
#define ACC_SENS           (4.0f / 32768.0f)       // ±4g -> g

// Mahony 滤波器参数 (可根据实际调整)
#define Kp 2.0f          // 比例增益
#define Ki 0.001f        // 积分增益
#define sampleFreq 100.0f // 预期采样频率 100Hz (与传感器配置一致)


volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 四元数姿态
volatile float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;   // 误差积分

// 时间测量
unsigned long lastUpdate = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000);

  // 检查传感器
  uint8_t who;
  readRegs(ICM42688_ADDR, WHO_AM_I, 1, &who);
  if (who != EXPECTED_WHO_AM_I) {
    Serial.print("传感器检测失败! WHO_AM_I = 0x");
    Serial.println(who, HEX);
    while (1);
  }
  Serial.println("ICM42688 初始化成功");


  writeReg(ICM42688_ADDR, REG_BANK_SEL, 0);      // 切换到 Bank 0
  writeReg(ICM42688_ADDR, DEVICE_CONFIG, 0x01);  // 软复位
  delay(100);
  writeReg(ICM42688_ADDR, ACCEL_CONFIG0, 0x48);  // ±4g, 100Hz
  writeReg(ICM42688_ADDR, GYRO_CONFIG0, 0x28);   // ±1000dps, 100Hz
  writeReg(ICM42688_ADDR, PWR_MGMT0, 0x0f);      // 开启陀螺仪和加速度计
  delay(100);

  lastUpdate = micros(); // 初始化时间戳
}

void loop() {
  // 读取原始数据
  uint8_t buffer[12];
  readRegs(ICM42688_ADDR, ACCEL_DATA_X1, 12, buffer);

  // 解析加速度计 (大端格式)
  int16_t ax_raw = (int16_t)(buffer[0] << 8 | buffer[1]);
  int16_t ay_raw = (int16_t)(buffer[2] << 8 | buffer[3]);
  int16_t az_raw = (int16_t)(buffer[4] << 8 | buffer[5]);

  // 解析陀螺仪
  int16_t gx_raw = (int16_t)(buffer[6] << 8 | buffer[7]);
  int16_t gy_raw = (int16_t)(buffer[8] << 8 | buffer[9]);
  int16_t gz_raw = (int16_t)(buffer[10] << 8 | buffer[11]);

  // 转换为物理单位
  float ax = ax_raw * ACC_SENS;        // g
  float ay = ay_raw * ACC_SENS;
  float az = az_raw * ACC_SENS;
  float gx = gx_raw * GYRO_SENS;       // °/s
  float gy = gy_raw * GYRO_SENS;
  float gz = gz_raw * GYRO_SENS;

  // 陀螺仪转换为弧度/秒 (Mahony 算法要求)
  gx *= DEG_TO_RAD;
  gy *= DEG_TO_RAD;
  gz *= DEG_TO_RAD;

  // 计算时间间隔 dt (秒)
  unsigned long now = micros();
  float dt = (now - lastUpdate) / 1000000.0f;
  lastUpdate = now;

  // 调用 Mahony 更新 (仅六轴，使用加速度计修正俯仰/横滚)
  MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt);

  // 从四元数计算欧拉角 (单位：度)
  float roll, pitch, yaw;
  quaternionToEuler(q0, q1, q2, q3, &roll, &pitch, &yaw);

  // 打印结果
  Serial.print("Roll: "); Serial.print(roll);
  Serial.print("\tPitch: "); Serial.print(pitch);
  Serial.print("\tYaw: "); Serial.println(yaw);

  delay(10); // 略小于采样周期，保证实时性
}

// ===================== Mahony 算法实现 =====================
void MahonyAHRSupdateIMU(float gx, float gy, float gz,
                         float ax, float ay, float az, float dt) {
  float norm;
  float vx, vy, vz;
  float ex, ey, ez;

  // 加速度计归一化
  norm = sqrt(ax * ax + ay * ay + az * az);
  if (norm == 0.0f) return; // 避免除零
  ax /= norm;
  ay /= norm;
  az /= norm;

  // 根据当前四元数估计重力方向
  vx = 2.0f * (q1 * q3 - q0 * q2);
  vy = 2.0f * (q0 * q1 + q2 * q3);
  vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

  // 计算误差：测量值与估计值的叉积
  ex = ay * vz - az * vy;
  ey = az * vx - ax * vz;
  ez = ax * vy - ay * vx;

  // 积分误差
  exInt += ex * Ki * dt;
  eyInt += ey * Ki * dt;
  ezInt += ez * Ki * dt;

  // 修正陀螺仪：比例 + 积分
  gx += Kp * ex + exInt;
  gy += Kp * ey + eyInt;
  gz += Kp * ez + ezInt;

  // 用修正后的角速度更新四元数 (一阶龙格-库塔法)
  float q0_dot = (-q1 * gx - q2 * gy - q3 * gz) * 0.5f;
  float q1_dot = (q0 * gx + q2 * gz - q3 * gy) * 0.5f;
  float q2_dot = (q0 * gy - q1 * gz + q3 * gx) * 0.5f;
  float q3_dot = (q0 * gz + q1 * gy - q2 * gx) * 0.5f;

  q0 += q0_dot * dt;
  q1 += q1_dot * dt;
  q2 += q2_dot * dt;
  q3 += q3_dot * dt;

  // 四元数归一化
  norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 /= norm;
  q1 /= norm;
  q2 /= norm;
  q3 /= norm;
}

// 四元数转欧拉角 (Z-Y-X 旋转顺序，即偏航-俯仰-横滚)
void quaternionToEuler(float q0, float q1, float q2, float q3,
                       float* roll, float* pitch, float* yaw) {
  // 计算俯仰角 (pitch) 并限制在 -90° 到 +90° 之间
  float sinp = 2.0f * (q0 * q2 - q3 * q1);
  if (fabs(sinp) >= 1.0f)
    *pitch = copysign(90.0f, sinp); // 处理奇点
  else
    *pitch = asin(sinp) * RAD_TO_DEG;

  // 计算横滚角 (roll)
  float sinr_cosp = 2.0f * (q0 * q1 + q2 * q3);
  float cosr_cosp = 1.0f - 2.0f * (q1 * q1 + q2 * q2);
  *roll = atan2(sinr_cosp, cosr_cosp) * RAD_TO_DEG;

  // 计算偏航角 (yaw)
  float siny_cosp = 2.0f * (q0 * q3 + q1 * q2);
  float cosy_cosp = 1.0f - 2.0f * (q2 * q2 + q3 * q3);
  *yaw = atan2(siny_cosp, cosy_cosp) * RAD_TO_DEG;
}

// ===================== I²C 读写函数 =====================
void writeReg(uint8_t addr, uint8_t reg, uint8_t data) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}

void readRegs(uint8_t addr, uint8_t reg, uint8_t count, uint8_t* dest) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(addr, count);
  while (count--) *dest++ = Wire.read();
}

2.卡尔曼滤波

#include <Wire.h>


#define ICM42688_ADDR       0x68
#define WHO_AM_I            0x75
#define EXPECTED_WHO_AM_I   0x47
#define REG_BANK_SEL        0x76
#define DEVICE_CONFIG       0x11
#define ACCEL_CONFIG0       0x50
#define GYRO_CONFIG0        0x4F
#define PWR_MGMT0           0x4E
#define ACCEL_DATA_X1       0x1F
#define GYRO_DATA_X1        0x25

// 灵敏度系数
#define GYRO_SENS           (1000.0f / 32768.0f)   // ±1000°/s -> °/s
#define ACC_SENS            (4.0f / 32768.0f)      // ±4g -> g

// 卡尔曼滤波器结构体
typedef struct {
  float Q;        // 过程噪声协方差（角度随机游走）
  float R;        // 观测噪声协方差（加速度计角度噪声）
  float x;        // 状态：角度（度）
  float P;        // 估计误差协方差
} Kalman1D;

// 初始化卡尔曼滤波器
void kalmanInit(Kalman1D *k, float Q, float R, float initAngle) {
  k->Q = Q;
  k->R = R;
  k->x = initAngle;
  k->P = 1.0f;    // 初始估计误差较大
}

// 卡尔曼滤波更新：输入角速度(°/s)、加速度计角度(°)、采样时间dt(s)，输出滤波后角度(°)
float kalmanUpdate(Kalman1D *k, float gyroRate, float accAngle, float dt) {
  // 预测
  float x_pred = k->x + gyroRate * dt;
  float P_pred = k->P + k->Q;

  // 更新
  float K = P_pred / (P_pred + k->R);   // 卡尔曼增益
  k->x = x_pred + K * (accAngle - x_pred);
  k->P = (1 - K) * P_pred;

  return k->x;
}

// 全局变量
Kalman1D kalmanRoll, kalmanPitch;
float yaw = 0.0f;           // 偏航角由陀螺仪积分得到（会漂移）
unsigned long lastTime = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000);

  // 检查传感器
  uint8_t who;
  readRegs(ICM42688_ADDR, WHO_AM_I, 1, &who);
  if (who != EXPECTED_WHO_AM_I) {
    Serial.print("传感器检测失败! WHO_AM_I = 0x");
    Serial.println(who, HEX);
    while (1);
  }

  // 初始化 ICM42688
  writeReg(ICM42688_ADDR, REG_BANK_SEL, 0);
  writeReg(ICM42688_ADDR, DEVICE_CONFIG, 0x01);
  delay(100);
  writeReg(ICM42688_ADDR, ACCEL_CONFIG0, 0x48);   // ±4g, 100Hz ODR
  writeReg(ICM42688_ADDR, GYRO_CONFIG0, 0x28);    // ±1000dps, 100Hz ODR
  writeReg(ICM42688_ADDR, PWR_MGMT0, 0x0f);       // 开启陀螺仪和加速度计
  delay(100);

  // 静止时读取加速度计，计算初始角度用于卡尔曼初始化
  float ax, ay, az;
  readAccel(&ax, &ay, &az);
  float initRoll  = atan2(ay, az) * RAD_TO_DEG;
  float initPitch = atan2(-ax, sqrt(ay*ay + az*az)) * RAD_TO_DEG;

  // 初始化两个卡尔曼滤波器
  // Q = 0.1, R = 10.0 是经验值，可根据实际噪声调整
  kalmanInit(&kalmanRoll,  0.1f, 10.0f, initRoll);
  kalmanInit(&kalmanPitch, 0.1f, 10.0f, initPitch);

  lastTime = micros();
}

void loop() {
  // 读取原始数据（加速度 + 陀螺仪）
  uint8_t buffer[12];
  readRegs(ICM42688_ADDR, ACCEL_DATA_X1, 12, buffer);

  // 解析加速度计
  int16_t ax_raw = (int16_t)(buffer[0] << 8 | buffer[1]);
  int16_t ay_raw = (int16_t)(buffer[2] << 8 | buffer[3]);
  int16_t az_raw = (int16_t)(buffer[4] << 8 | buffer[5]);
  float ax = ax_raw * ACC_SENS;
  float ay = ay_raw * ACC_SENS;
  float az = az_raw * ACC_SENS;

  // 解析陀螺仪
  int16_t gx_raw = (int16_t)(buffer[6] << 8 | buffer[7]);
  int16_t gy_raw = (int16_t)(buffer[8] << 8 | buffer[9]);
  int16_t gz_raw = (int16_t)(buffer[10] << 8 | buffer[11]);
  float gx = gx_raw * GYRO_SENS;   // °/s
  float gy = gy_raw * GYRO_SENS;
  float gz = gz_raw * GYRO_SENS;

  // 计算时间间隔 dt (秒)
  unsigned long now = micros();
  float dt = (now - lastTime) / 1000000.0f;
  lastTime = now;
  if (dt <= 0 || dt > 0.1) dt = 0.01; // 防错

  // 由加速度计计算横滚角和俯仰角（单位：度）
  float accRoll  = atan2(ay, az) * RAD_TO_DEG;
  float accPitch = atan2(-ax, sqrt(ay*ay + az*az)) * RAD_TO_DEG;

  // 卡尔曼滤波融合
  float roll  = kalmanUpdate(&kalmanRoll,  gx, accRoll,  dt);
  float pitch = kalmanUpdate(&kalmanPitch, gy, accPitch, dt);

  // 偏航角：直接积分陀螺仪 Z 轴（无修正，会漂移）
  yaw += gz * dt;

  // 输出结果
  Serial.print("Roll: "); Serial.print(roll);
  Serial.print("\tPitch: "); Serial.print(pitch);
  Serial.print("\tYaw: "); Serial.println(yaw);

  delay(10); // 略小于采样周期
}

// ========== I²C 读写函数 ==========
void writeReg(uint8_t addr, uint8_t reg, uint8_t data) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}

void readRegs(uint8_t addr, uint8_t reg, uint8_t count, uint8_t* dest) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(addr, count);
  while (count--) *dest++ = Wire.read();
}

// 单独读取加速度计（用于初始化）
void readAccel(float* ax, float* ay, float* az) {
  uint8_t buffer[6];
  readRegs(ICM42688_ADDR, ACCEL_DATA_X1, 6, buffer);
  int16_t ax_raw = (int16_t)(buffer[0] << 8 | buffer[1]);
  int16_t ay_raw = (int16_t)(buffer[2] << 8 | buffer[3]);
  int16_t az_raw = (int16_t)(buffer[4] << 8 | buffer[5]);
  *ax = ax_raw * ACC_SENS;
  *ay = ay_raw * ACC_SENS;
  *az = az_raw * ACC_SENS;
}

3.一阶互补滤波

#include <Wire.h>


#define ICM42688_ADDR       0x68
#define WHO_AM_I            0x75
#define EXPECTED_WHO_AM_I   0x47
#define REG_BANK_SEL        0x76
#define DEVICE_CONFIG       0x11
#define ACCEL_CONFIG0       0x50
#define GYRO_CONFIG0        0x4F
#define PWR_MGMT0           0x4E
#define ACCEL_DATA_X1       0x1F
#define GYRO_DATA_X1        0x25

// 灵敏度系数
#define GYRO_SENS           (1000.0f / 32768.0f)   // ±1000°/s -> °/s
#define ACC_SENS            (4.0f / 32768.0f)      // ±4g -> g

// 互补滤波系数 (0 < K < 1)
// K 越大越信任加速度计，响应快但噪声大；K 越小越信任陀螺仪积分，平滑但有滞后
#define COMPLEMENTARY_K     0.1f    // 典型值 0.05~0.2

// 全局变量
float roll = 0.0f, pitch = 0.0f, yaw = 0.0f;  // 融合后的角度（度）
unsigned long lastTime = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000);

  // 检查传感器
  uint8_t who;
  readRegs(ICM42688_ADDR, WHO_AM_I, 1, &who);
  if (who != EXPECTED_WHO_AM_I) {
    Serial.print("传感器检测失败! WHO_AM_I = 0x");
    Serial.println(who, HEX);
    while (1);
  }

  // 初始化 ICM42688
  writeReg(ICM42688_ADDR, REG_BANK_SEL, 0);
  writeReg(ICM42688_ADDR, DEVICE_CONFIG, 0x01);
  delay(100);
  writeReg(ICM42688_ADDR, ACCEL_CONFIG0, 0x48);   // ±4g, 100Hz ODR
  writeReg(ICM42688_ADDR, GYRO_CONFIG0, 0x28);    // ±1000dps, 100Hz ODR
  writeReg(ICM42688_ADDR, PWR_MGMT0, 0x0f);       // 开启陀螺仪和加速度计
  delay(100);

  // 静止时读取加速度计，初始化融合角度
  float ax, ay, az;
  readAccel(&ax, &ay, &az);
  roll  = atan2(ay, az) * RAD_TO_DEG;
  pitch = atan2(-ax, sqrt(ay*ay + az*az)) * RAD_TO_DEG;
  yaw   = 0.0f;   // 偏航角初值为0

  lastTime = micros();
}

void loop() {
  // 读取原始数据（加速度 + 陀螺仪）
  uint8_t buffer[12];
  readRegs(ICM42688_ADDR, ACCEL_DATA_X1, 12, buffer);

  // 解析加速度计
  int16_t ax_raw = (int16_t)(buffer[0] << 8 | buffer[1]);
  int16_t ay_raw = (int16_t)(buffer[2] << 8 | buffer[3]);
  int16_t az_raw = (int16_t)(buffer[4] << 8 | buffer[5]);
  float ax = ax_raw * ACC_SENS;
  float ay = ay_raw * ACC_SENS;
  float az = az_raw * ACC_SENS;

  // 解析陀螺仪
  int16_t gx_raw = (int16_t)(buffer[6] << 8 | buffer[7]);
  int16_t gy_raw = (int16_t)(buffer[8] << 8 | buffer[9]);
  int16_t gz_raw = (int16_t)(buffer[10] << 8 | buffer[11]);
  float gx = gx_raw * GYRO_SENS;   // °/s
  float gy = gy_raw * GYRO_SENS;
  float gz = gz_raw * GYRO_SENS;

  // 计算时间间隔 dt (秒)
  unsigned long now = micros();
  float dt = (now - lastTime) / 1000000.0f;
  lastTime = now;
  if (dt <= 0 || dt > 0.1) dt = 0.01; // 防错

  // 由加速度计计算横滚角和俯仰角（单位：度）
  float accRoll  = atan2(ay, az) * RAD_TO_DEG;
  float accPitch = atan2(-ax, sqrt(ay*ay + az*az)) * RAD_TO_DEG;

  // --- 一阶互补滤波融合 ---
  // Roll 和 Pitch 分别融合：新角度 = K * 加速度计角度 + (1-K) * (旧角度 + 陀螺仪角速度积分)
  roll  = COMPLEMENTARY_K * accRoll  + (1.0f - COMPLEMENTARY_K) * (roll  + gx * dt);
  pitch = COMPLEMENTARY_K * accPitch + (1.0f - COMPLEMENTARY_K) * (pitch + gy * dt);

  // 偏航角：直接积分陀螺仪 Z 轴（无修正，会随时间漂移）
  yaw += gz * dt;

  // 输出结果
  Serial.print("Roll: "); Serial.print(roll);
  Serial.print("\tPitch: "); Serial.print(pitch);
  Serial.print("\tYaw: "); Serial.println(yaw);

  delay(10); // 略小于采样周期
}

// ========== I²C 读写函数 ==========
void writeReg(uint8_t addr, uint8_t reg, uint8_t data) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}

void readRegs(uint8_t addr, uint8_t reg, uint8_t count, uint8_t* dest) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(addr, count);
  while (count--) *dest++ = Wire.read();
}

// 单独读取加速度计（用于初始化）


// 注意：上面的readAccel函数有索引错误，需要修正。下面的版本是正确的：
void readAccel(float* ax, float* ay, float* az) {
  uint8_t buffer[6];
  readRegs(ICM42688_ADDR, ACCEL_DATA_X1, 6, buffer);
  int16_t ax_raw = (int16_t)(buffer[0] << 8 | buffer[1]);
  int16_t ay_raw = (int16_t)(buffer[2] << 8 | buffer[3]);
  int16_t az_raw = (int16_t)(buffer[4] << 8 | buffer[5]);
  *ax = ax_raw * ACC_SENS;
  *ay = ay_raw * ACC_SENS;
  *az = az_raw * ACC_SENS;
}

开发板推荐：天空星STM32F407VET6开发板

超高性价比 STM32主控 | 超高主频 | 一板兼容百芯 | 比赛神器 | 沉金彩色丝印

点击查看