九轴姿态：MPU-9250九轴姿态解算全攻略

目录

1. 硬件与 DMP 简介

MPU-9250 特性

DMP 作用

2. 软件整体架构

3. 代码实现（STM32 HAL 库）

3.1 I²C 底层驱动

3.2 MPU-9250 与 DMP 初始化

3.3 读取 DMP 姿态数据

3.4 主函数与串口输出

4. DMP 库移植说明

5. 关键优化与注意事项

6. 进阶应用

MPU-9250 9 轴传感器的融合解算代码，包含DMP（Digital Motion Processor）库的移植和姿态角（Pitch/Roll/Yaw）输出，适用于 STM32/CH32 平台，使用 I²C 通信。

1. 硬件与 DMP 简介

MPU-9250 特性

• 内部集成：
  • 3 轴加速度计
  • 3 轴陀螺仪
  • 3 轴磁力计（AK8963）
• 内置DMP（数字运动处理器），可直接输出四元数、欧拉角等融合数据
• 通信接口：I²C / SPI

DMP 作用

• 减轻 MCU 负担，在传感器内部完成：
  • 加速度计 + 陀螺仪 + 磁力计 数据融合
  • 姿态解算（四元数、欧拉角）
  • 自动校准与误差补偿
• 输出频率最高可达 200Hz

2. 软件整体架构

┌─────────────────┐ │ 硬件层：I2C驱动 │ ← 与MPU-9250、AK8963通信 └────────┬────────┘ │ ┌────────▼────────┐ │ 驱动层：MPU-9250初始化 │ ← 配置采样率、量程、DMP └────────┬────────┘ │ ┌────────▼────────┐ │ DMP层：固件加载与启动 │ ← 官方DMP库移植 └────────┬────────┘ │ ┌────────▼────────┐ │ 数据层：姿态数据读取 │ ← 四元数 / 欧拉角 └────────┬────────┘ │ ┌────────▼────────┐ │ 应用层：串口输出 │ ← Pitch/Roll/Yaw └─────────────────┘

3. 代码实现（STM32 HAL 库）

这里使用 STM32F103 + HAL 库，I²C1 通信，串口 1 输出。

3.1 I²C 底层驱动

#include "stm32f1xx_hal.h" extern I2C_HandleTypeDef hi2c1; #define MPU9250_I2C &hi2c1 // I2C写寄存器 void I2C_WriteReg(uint8_t addr, uint8_t reg, uint8_t data) { HAL_I2C_Mem_Write(MPU9250_I2C, addr<<1, reg, I2C_MEMADD_SIZE_8BIT, &data, 1, HAL_MAX_DELAY); } // I2C读寄存器 uint8_t I2C_ReadReg(uint8_t addr, uint8_t reg) { uint8_t data; HAL_I2C_Mem_Read(MPU9250_I2C, addr<<1, reg, I2C_MEMADD_SIZE_8BIT, &data, 1, HAL_MAX_DELAY); return data; } // 多字节读取 void I2C_ReadRegs(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) { HAL_I2C_Mem_Read(MPU9250_I2C, addr<<1, reg, I2C_MEMADD_SIZE_8BIT, buf, len, HAL_MAX_DELAY); }

3.2 MPU-9250 与 DMP 初始化

#include "mpu9250.h" #include "inv_mpu.h" #include "inv_mpu_dmp_motion_driver.h" #define MPU9250_ADDR 0x68 // AD0接地 uint8_t mpu_init(void) { int result; // 初始化MPU9250 if (mpu_init(NULL) != 0) return 1; // 配置陀螺仪和加速度计 mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); mpu_set_sample_rate(100); // 100Hz // 加载DMP固件 if (dmp_load_motion_driver_firmware() != 0) return 2; // 启用DMP dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO); dmp_set_fifo_rate(100); // 与采样率一致 // 启动DMP if (mpu_set_dmp_state(1) != 0) return 3; return 0; }

3.3 读取 DMP 姿态数据

#include <math.h> #define RAD_TO_DEG 57.29578f typedef struct { float pitch; float roll; float yaw; } EulerAngles; EulerAngles euler; void read_dmp_data(void) { long sensor_timestamp; short gyro[3], accel[3], sensors; unsigned char more; long quat[4]; // 读取DMP数据 if (dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more)) return; // 转换四元数到欧拉角 float q0 = quat[0] / 16384.0f; float q1 = quat[1] / 16384.0f; float q2 = quat[2] / 16384.0f; float q3 = quat[3] / 16384.0f; euler.pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * RAD_TO_DEG; euler.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * RAD_TO_DEG; euler.yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * RAD_TO_DEG; }

3.4 主函数与串口输出

#include "stdio.h" extern UART_HandleTypeDef huart1; int fputc(int ch, FILE *f) { HAL_UART_Transmit(&huart1, (uint8_t*)&ch, 1, HAL_MAX_DELAY); return ch; } int main(void) { HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); MX_I2C1_Init(); MX_USART1_UART_Init(); if (mpu_init() != 0) { printf("MPU9250 init failed!\r\n"); while (1); } while (1) { read_dmp_data(); printf("Pitch: %.1f\tRoll: %.1f\tYaw: %.1f\r\n", euler.pitch, euler.roll, euler.yaw); HAL_Delay(10); // 100Hz输出 } }

4. DMP 库移植说明

1. 获取 DMP 库

   • 从 InvenSense 官方 GitHub 或 ST 提供的 MPU-9250 驱动包获取：
     • inv_mpu.h
     • inv_mpu_dmp_motion_driver.h
     • 及其.c文件
   • 这些文件实现了 DMP 固件加载、FIFO 读取、四元数计算等功能。

2. 适配 I²C

   • 在inv_mpu.h中修改 I²C 读写函数，指向你实现的I2C_WriteReg/I2C_ReadRegs。

3. 配置编译选项

   • 添加宏MPU9250以启用磁力计支持。
   • 在编译器中添加浮点支持（-mfpu=fpv4-sp-d16 -mfloat-abi=hard针对 Cortex-M4）。

5. 关键优化与注意事项

1. 磁力计校准
   • DMP 内置磁力计校准，但首次使用建议在无磁干扰环境下做一次八位置校准。
2. 数据滤波
   • 对输出的欧拉角可加滑动平均滤波，减少抖动。
3. 动态性能
   • DMP 融合在动态环境下比纯加速度计 + 磁力计稳定，但高速运动仍需结合外部算法（如扩展卡尔曼滤波）。
4. 电源与布线
   • MPU-9250 对电源噪声敏感，需加去耦电容，磁力计远离电机 / 强磁场。

6. 进阶应用

• 无人机姿态控制：将欧拉角输入 PID 控制器，控制电机转速。
• VR/AR 头戴设备：高刷新率姿态输出，实时显示头部方向。
• 机器人平衡：结合陀螺仪和加速度计，实现自平衡。