STM32与MPU6050实现欧拉角姿态检测实战

发布时间：2026/7/19 3:52:16

1. 项目概述

在嵌入式系统开发中，姿态检测是一个常见且重要的功能需求。STM32作为广泛使用的微控制器，配合MPU6050六轴传感器实现欧拉角输出，是许多运动控制、无人机、机器人等项目的基础。这个组合之所以流行，是因为它提供了成本效益高且性能稳定的解决方案。

MPU6050集成了三轴陀螺仪和三轴加速度计，能够测量物体在空间中的角速度和线性加速度。通过适当的算法处理，这些原始数据可以转换为更直观的欧拉角（俯仰角Pitch、横滚角Roll和偏航角Yaw），用于描述物体的三维姿态。

2. 硬件准备与连接

2.1 所需材料清单

STM32开发板（如STM32F103C8T6最小系统板）
MPU6050六轴传感器模块
杜邦线若干
USB转TTL模块（用于串口通信）
可选：0.96寸OLED显示屏（用于本地数据显示）

2.2 硬件连接示意图

MPU6050与STM32通常通过I2C接口连接，具体引脚对应关系如下：

MPU6050引脚	STM32引脚	功能说明
VCC	3.3V	电源正极
GND	GND	电源地
SCL	PB6	I2C时钟线
SDA	PB7	I2C数据线
INT	PA0	中断信号

注意：不同STM32型号的I2C引脚可能不同，需查阅具体芯片手册确认。如果使用其他GPIO模拟I2C，则可以自由选择引脚。

3. 软件环境搭建

3.1 开发工具准备

Keil MDK：STM32的主要开发环境
STM32CubeMX：用于初始化代码生成
串口调试助手：如Putty或SecureCRT
MPU6050官方库：DMP驱动库

3.2 工程配置步骤

使用STM32CubeMX生成基础工程：
- 选择正确的STM32型号
- 启用I2C1外设
- 配置USART1用于调试输出
- 设置系统时钟（推荐72MHz）

添加必要的驱动文件：

/* 核心文件清单 */ mpu6050.c // MPU6050底层驱动 i2c_hal.c // I2C硬件抽象层 inv_mpu.c // DMP驱动核心 inv_mpu_dmp_motion_driver.c // DMP运动驱动

4. MPU6050驱动实现

4.1 初始化流程详解

完整的MPU6050初始化包含以下关键步骤：

void MPU6050_Init(void) { // 1. 硬件复位 MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x80); HAL_Delay(100); // 2. 唤醒设备 MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x00); // 3. 设置陀螺仪量程(±2000dps) MPU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x18); // 4. 设置加速度计量程(±4g) MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG, 0x08); // 5. 配置数字低通滤波器(DLPF) MPU6050_WriteReg(MPU6050_RA_CONFIG, 0x03); // 6. 设置采样率(1kHz/(1+SMPLRT_DIV)) MPU6050_WriteReg(MPU6050_RA_SMPLRT_DIV, 0x07); // 7. 禁用所有中断 MPU6050_WriteReg(MPU6050_RA_INT_ENABLE, 0x00); }

4.2 DMP固件加载

MPU6050的Digital Motion Processor(DMP)是其核心优势，可以硬件解算姿态：

uint8_t MPU6050_DMP_Init(void) { // 1. 重置DMP MPU6050_WriteReg(MPU6050_RA_USER_CTRL, 0x04); HAL_Delay(50); // 2. 加载DMP固件 if(!mpu_load_firmware(DMP_CODE_SIZE, dmp_memory)) { return 0; // 加载失败 } // 3. 设置DMP输出速率(200Hz) mpu_set_dmp_state(1); dmp_set_fifo_rate(200); // 4. 启用6轴四元数输出 dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL); return 1; // 初始化成功 }

5. 欧拉角解算实现

5.1 数据读取与处理

通过DMP获取四元数后转换为欧拉角的完整流程：

void Get_Euler_Angles(float *pitch, float *roll, float *yaw) { float q0=1.0f, q1=0.0f, q2=0.0f, q3=0.0f; unsigned long sensor_timestamp; short gyro[3], accel[3], sensors; unsigned char more; long quat[4]; // 从FIFO读取四元数 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); // 转换为浮点四元数 q0 = quat[0] / 1073741824.0f; // Q30格式转换 q1 = quat[1] / 1073741824.0f; q2 = quat[2] / 1073741824.0f; q3 = quat[3] / 1073741824.0f; // 计算欧拉角(弧度) *pitch = asin(2*(q0*q2 - q1*q3)); *roll = atan2(2*(q0*q1 + q2*q3), 1-2*(q1*q1 + q2*q2)); *yaw = atan2(2*(q0*q3 + q1*q2), 1-2*(q2*q2 + q3*q3)); // 转换为角度制 *pitch *= 180/M_PI; *roll *= 180/M_PI; *yaw *= 180/M_PI; }

5.2 卡尔曼滤波优化

原始数据存在噪声，建议添加卡尔曼滤波：

typedef struct { float Q_angle; // 过程噪声协方差 float Q_bias; // 过程噪声协方差 float R_measure; // 测量噪声协方差 float angle; // 计算得到的最优角度 float bias; // 陀螺仪偏置 float P[2][2]; // 误差协方差矩阵 } Kalman_t; float Kalman_update(Kalman_t *kalman, float newAngle, float newRate, float dt) { // 预测阶段 kalman->angle += dt * (newRate - kalman->bias); kalman->P[0][0] += dt * (dt*kalman->P[1][1] - kalman->P[0][1] - kalman->P[1][0] + kalman->Q_angle); kalman->P[0][1] -= dt * kalman->P[1][1]; kalman->P[1][0] -= dt * kalman->P[1][1]; kalman->P[1][1] += kalman->Q_bias * dt; // 更新阶段 float S = kalman->P[0][0] + kalman->R_measure; float K[2]; K[0] = kalman->P[0][0] / S; K[1] = kalman->P[1][0] / S; float y = newAngle - kalman->angle; kalman->angle += K[0] * y; kalman->bias += K[1] * y; float P00_temp = kalman->P[0][0]; float P01_temp = kalman->P[0][1]; kalman->P[0][0] -= K[0] * P00_temp; kalman->P[0][1] -= K[0] * P01_temp; kalman->P[1][0] -= K[1] * P00_temp; kalman->P[1][1] -= K[1] * P01_temp; return kalman->angle; }

6. 系统集成与调试

6.1 主程序架构

典型的主循环结构应包含以下功能：

int main(void) { // 硬件初始化 HAL_Init(); SystemClock_Config(); MX_I2C1_Init(); MX_USART1_UART_Init(); // MPU6050初始化 MPU6050_Init(); while(!MPU6050_DMP_Init()) { printf("DMP Init Failed!\r\n"); HAL_Delay(500); } // 卡尔曼滤波器初始化 Kalman_t kalman_pitch = {0.001f, 0.003f, 0.03f, 0, 0, {{0,0},{0,0}}}; while(1) { float pitch, roll, yaw; Get_Euler_Angles(&pitch, &roll, &yaw); // 应用卡尔曼滤波 static float last_time = 0; float dt = (HAL_GetTick() - last_time) / 1000.0f; last_time = HAL_GetTick(); pitch = Kalman_update(&kalman_pitch, pitch, MPU6050_Get_Gyro_Y()/131.0f, dt); // 输出结果 printf("Pitch:%.2f Roll:%.2f Yaw:%.2f\r\n", pitch, roll, yaw); HAL_Delay(10); } }

6.2 常见问题排查

问题1：I2C通信失败

现象：读取WHO_AM_I寄存器返回值不正确
解决方案：

检查硬件连接，确认SDA/SCL线已正确上拉（通常4.7kΩ电阻）
用逻辑分析仪抓取I2C波形，确认时序符合规范
降低I2C时钟频率（尝试100kHz）

问题2：DMP初始化失败

现象：mpu_load_firmware返回错误
解决方案：

确保MPU6050供电稳定（3.3V电压波动应小于±5%）
检查I2C通信质量，DMP加载需要稳定的数据传输
尝试官方提供的不同版本DMP固件

问题3：欧拉角漂移

现象：静止时角度缓慢变化
解决方案：

进行陀螺仪零偏校准（静止状态下采集1000个样本取平均）
提高卡尔曼滤波器的Q_angle参数
定期进行加速度计校准（六面校准法）

7. 进阶应用与优化

7.1 传感器融合算法

对于更高精度的需求，可以融合磁力计数据实现9轴姿态解算：

void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // 归一化加速度计和磁力计数据 float norm = sqrt(ax*ax + ay*ay + az*az); ax /= norm; ay /= norm; az /= norm; norm = sqrt(mx*mx + my*my + mz*mz); mx /= norm; my /= norm; mz /= norm; // 计算参考方向 float hx = 2.0f*(mx*(0.5f - q2*q2 - q3*q3) + my*(q1*q2 - q0*q3) + mz*(q1*q3 + q0*q2)); float hy = 2.0f*(mx*(q1*q2 + q0*q3) + my*(0.5f - q1*q1 - q3*q3) + mz*(q2*q3 - q0*q1)); float bx = sqrt(hx*hx + hy*hy); float bz = 2.0f*(mx*(q1*q3 - q0*q2) + my*(q2*q3 + q0*q1) + mz*(0.5f - q1*q1 - q2*q2)); // 计算误差 float vx = 2.0f*(q2*q3 + q0*q1); float vy = 2.0f*(q0*q2 - q1*q3); float vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; float wx = 2.0f*bx*(0.5f - q2*q2 - q3*q3) + 2.0f*bz*(q1*q3 - q0*q2); float wy = 2.0f*bx*(q1*q2 - q0*q3) + 2.0f*bz*(q0*q1 + q2*q3); float wz = 2.0f*bx*(q0*q2 + q1*q3) + 2.0f*bz*(0.5f - q1*q1 - q2*q2); // 反馈控制 float ex = (ay*vz - az*vy) + (my*wz - mz*wy); float ey = (az*vx - ax*vz) + (mz*wx - mx*wz); float ez = (ax*vy - ay*vx) + (mx*wy - my*wx); integralFBx += Ki * ex * dt; integralFBy += Ki * ey * dt; integralFBz += Ki * ez * dt; // 调整陀螺仪读数 gx += Kp*ex + integralFBx; gy += Kp*ey + integralFBy; gz += Kp*ez + integralFBz; // 四元数积分 q0 += (-q1*gx - q2*gy - q3*gz)*0.5f*dt; q1 += (q0*gx + q2*gz - q3*gy)*0.5f*dt; q2 += (q0*gy - q1*gz + q3*gx)*0.5f*dt; q3 += (q0*gz + q1*gy - q2*gx)*0.5f*dt; // 归一化四元数 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm; }

7.2 性能优化技巧

浮点运算加速：
- 启用STM32的FPU单元（在CubeMX中设置）
- 使用arm_math库中的DSP指令

数据采样优化：

// 使用DMA进行I2C数据传输 HAL_I2C_Mem_Read_DMA(&hi2c1, MPU6050_ADDR, MPU6050_RA_ACCEL_XOUT_H, 1, (uint8_t*)accel_data, 14); // 配置DMA循环模式实现连续采样 hdma_i2c1_rx.Init.Mode = DMA_CIRCULAR;

低功耗设计：

// 配置MPU6050进入低功耗模式 MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x20); // 周期唤醒模式 MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_2, 0x07); // 仅加速度计工作 // 配置STM32进入STOP模式 HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI);

8. 实际应用案例

8.1 四轴飞行器姿态控制

在四轴飞行器应用中，欧拉角作为PID控制的输入：

void Quadcopter_Control(void) { float pitch, roll, yaw; Get_Euler_Angles(&pitch, &roll, &yaw); // PID控制器计算 static float pitch_err_sum = 0, last_pitch_err = 0; float pitch_err = target_pitch - pitch; pitch_err_sum += pitch_err; float pitch_output = Kp*pitch_err + Ki*pitch_err_sum + Kd*(pitch_err - last_pitch_err); last_pitch_err = pitch_err; // 电机输出 Motor1_Set(pitch_output); Motor2_Set(-pitch_output); }

8.2 虚拟现实手柄

作为VR设备输入时，需要优化数据传输延迟：

void VR_Transmit_Data(void) { float q[4]; Get_Quaternion(q); // 获取四元数 // 使用BLE传输优化后的数据 uint8_t tx_data[16]; memcpy(tx_data, (uint8_t*)q, 16); HAL_UART_Transmit_DMA(&huart1, tx_data, 16); // 运动预测补偿传输延迟 float gyro[3]; MPU6050_Get_Gyro(gyro); for(int i=0; i<4; i++) { q[i] += gyro[i%3] * PREDICT_TIME * 0.5f; } }

9. 测试与校准

9.1 六面校准法

加速度计校准流程：

将设备水平放置（Z轴向下），保持静止
采集1000个样本，记录Z轴平均值AZ1
翻转设备（Z轴向上），记录AZ2

计算比例因子和偏移：

float A_scale = (AZ1 - AZ2)/(2*9.8f); float A_offset = (AZ1 + AZ2)/2;

9.2 陀螺仪零偏校准

void Gyro_Calibrate(void) { float sum[3] = {0}; for(int i=0; i<1000; i++) { short gyro[3]; MPU6050_Get_Gyro(gyro); sum[0] += gyro[0]; sum[1] += gyro[1]; sum[2] += gyro[2]; HAL_Delay(5); } gyro_offset_x = sum[0]/1000; gyro_offset_y = sum[1]/1000; gyro_offset_z = sum[2]/1000; }