四旋翼DIY实战：用STM32和ICM20602实现Mahony姿态解算（附完整代码）

四旋翼DIY实战：用STM32和ICM20602实现Mahony姿态解算

1. 项目背景与硬件选型

四旋翼飞行器的核心在于稳定控制，而姿态解算是实现这一目标的基础。ICM20602作为一款六轴IMU传感器，集成了三轴加速度计和三轴陀螺仪，配合STM32系列微控制器，能够构建高性价比的姿态解算系统。

硬件选型考量因素：

• STM32F4系列：Cortex-M4内核，带FPU浮点运算单元，适合实时数据处理
• ICM20602优势：
  • 相比MPU6050更低的噪声密度（±2000dps时仅0.01dps/√Hz）
  • 支持最高32kHz的采样率
  • 内置16位ADC和数字温度传感器

提示：实际采购时注意区分ICM20602的封装版本，LGA封装更适合手工焊接，而QFN封装需要热风枪操作。

2. 硬件连接与SPI配置

2.1 引脚连接方案

// SPI初始化代码示例 void SPI1_Init(void) { GPIO_InitTypeDef GPIO_InitStruct; SPI_InitTypeDef SPI_InitStruct; // 时钟使能 RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); // GPIO配置 GPIO_InitStruct.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; GPIO_Init(GPIOA, &GPIO_InitStruct); // 片选引脚配置 GPIO_InitStruct.GPIO_Pin = GPIO_Pin_4; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; GPIO_Init(GPIOA, &GPIO_InitStruct); // SPI参数配置 SPI_InitStruct.SPI_Direction = SPI_Direction_2Lines_FullDuplex; SPI_InitStruct.SPI_Mode = SPI_Mode_Master; SPI_InitStruct.SPI_DataSize = SPI_DataSize_8b; SPI_InitStruct.SPI_CPOL = SPI_CPOL_High; SPI_InitStruct.SPI_CPHA = SPI_CPHA_2Edge; SPI_InitStruct.SPI_NSS = SPI_NSS_Soft; SPI_InitStruct.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16; SPI_InitStruct.SPI_FirstBit = SPI_FirstBit_MSB; SPI_Init(SPI1, &SPI_InitStruct); SPI_Cmd(SPI1, ENABLE); }

2.2 传感器初始化关键步骤

1. 复位设备（写入PWR_MGMT_1寄存器0x80）
2. 等待复位完成（读取WHO_AM_I寄存器确认值为0x12）
3. 设置时钟源（PWR_MGMT_1寄存器配置为0x01）
4. 配置陀螺仪量程（GYRO_CONFIG寄存器，推荐±2000dps）
5. 配置加速度计量程（ACCEL_CONFIG寄存器，推荐±8g）
6. 设置采样率（SMPLRT_DIV寄存器，根据需求计算）

3. Mahony算法实现细节

3.1 算法核心思想

Mahony算法是一种基于互补滤波的姿态解算方法，通过PI控制器融合陀螺仪和加速度计数据：

• 陀螺仪数据：提供短期高精度角度变化
• 加速度计数据：提供长期稳定的姿态参考
• PI补偿：修正陀螺仪的漂移误差

// Mahony算法参数定义 #define SAMPLE_FREQ 500.0f // 采样频率(Hz) #define Kp 2.0f // 比例系数 #define Ki 0.005f // 积分系数 float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 四元数初始化 float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // 误差积分

3.2 数据预处理

传感器原始数据需要转换为物理量：

void ConvertRawData(IMU_Data *imu) { // 加速度计转换 (±8g量程) imu->accel_x = (float)imu->raw_accel_x / 4096.0f; imu->accel_y = (float)imu->raw_accel_y / 4096.0f; imu->accel_z = (float)imu->raw_accel_z / 4096.0f; // 陀螺仪转换 (±2000dps量程) imu->gyro_x = (float)imu->raw_gyro_x / 16.4f * (PI / 180.0f); imu->gyro_y = (float)imu->raw_gyro_y / 16.4f * (PI / 180.0f); imu->gyro_z = (float)imu->raw_gyro_z / 16.4f * (PI / 180.0f); }

3.3 算法实现代码

void MahonyAHRSupdate(IMU_Data *imu) { float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; float qa, qb, qc; // 加速度计数据归一化 recipNorm = invSqrt(imu->accel_x * imu->accel_x + imu->accel_y * imu->accel_y + imu->accel_z * imu->accel_z); float ax = imu->accel_x * recipNorm; float ay = imu->accel_y * recipNorm; float az = imu->accel_z * recipNorm; // 计算重力向量在机体坐标系中的理论方向 halfvx = q1 * q3 - q0 * q2; halfvy = q0 * q1 + q2 * q3; halfvz = q0 * q0 - 0.5f + q3 * q3; // 计算误差向量 halfex = (ay * halfvz - az * halfvy); halfey = (az * halfvx - ax * halfvz); halfez = (ax * halfvy - ay * halfvx); // 积分误差 integralFBx += Ki * halfex * (1.0f / SAMPLE_FREQ); integralFBy += Ki * halfey * (1.0f / SAMPLE_FREQ); integralFBz += Ki * halfez * (1.0f / SAMPLE_FREQ); // 补偿陀螺仪偏差 imu->gyro_x += Kp * halfex + integralFBx; imu->gyro_y += Kp * halfey + integralFBy; imu->gyro_z += Kp * halfez + integralFBz; // 四元数微分方程 qa = q0; qb = q1; qc = q2; q0 += (-qb * imu->gyro_x - qc * imu->gyro_y - q3 * imu->gyro_z) * (0.5f / SAMPLE_FREQ); q1 += (qa * imu->gyro_x + qc * imu->gyro_z - q3 * imu->gyro_y) * (0.5f / SAMPLE_FREQ); q2 += (qa * imu->gyro_y - qb * imu->gyro_z + q3 * imu->gyro_x) * (0.5f / SAMPLE_FREQ); q3 += (qa * imu->gyro_z + qb * imu->gyro_y - qc * imu->gyro_x) * (0.5f / SAMPLE_FREQ); // 四元数归一化 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; }

4. 系统集成与调试技巧

4.1 定时器中断配置

姿态解算需要稳定的采样周期，推荐使用定时器中断：

void TIM3_Init(uint16_t arr, uint16_t psc) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; NVIC_InitTypeDef NVIC_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); TIM_TimeBaseStructure.TIM_Period = arr; TIM_TimeBaseStructure.TIM_Prescaler = psc; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); TIM_Cmd(TIM3, ENABLE); } // 中断服务例程 void TIM3_IRQHandler(void) { if(TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) { IMU_Data imu; ICM20602_ReadData(&imu); // 读取传感器数据 MahonyAHRSupdate(&imu); // 更新姿态 TIM_ClearITPendingBit(TIM3, TIM_IT_Update); } }

4.2 参数调优经验

Kp和Ki参数调试方法：

1. 初始设置Ki=0，逐渐增大Kp直到系统开始振荡
2. 将Kp设为振荡临界值的50%
3. 逐渐增加Ki直到静态误差被消除
4. 实际飞行测试微调

常见问题排查：

• 数据跳动大：检查电源稳定性，增加硬件滤波电容
• 姿态漂移：重新校准传感器，检查安装是否牢固
• 响应迟钝：适当提高Kp值，但注意不要引起振荡

4.3 欧拉角转换

void QuaternionToEuler(float q0, float q1, float q2, float q3, float *roll, float *pitch, float *yaw) { *roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)); *pitch = asinf(2.0f * (q0 * q2 - q3 * q1)); *yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)); // 转换为角度制 *roll *= 180.0f / PI; *pitch *= 180.0f / PI; *yaw *= 180.0f / PI; }

5. 性能优化与扩展

5.1 计算效率提升

快速平方根倒数算法：

float invSqrt(float x) { float halfx = 0.5f * x; float y = x; long i = *(long*)&y; i = 0x5f3759df - (i >> 1); y = *(float*)&i; y = y * (1.5f - (halfx * y * y)); return y; }

使用STM32硬件FPU：

• 确保在工程设置中启用FPU
• 使用__attribute__((optimize("O3")))优化关键函数

5.2 传感器校准方法

六面法校准步骤：

1. 将设备水平放置（Z轴朝上），静止采集100组数据
2. 旋转180度再次采集
3. 对X/Y/Z轴重复上述过程
4. 计算各轴的偏移量和比例因子

typedef struct { float accel_offset[3]; float gyro_offset[3]; float accel_scale[3]; } IMU_Calibration; void CalibrateIMU(IMU_Calibration *cal) { // 实际实现应包含多位置数据采集和最小二乘法计算 // ... }

5.3 扩展应用方向

• 磁力计融合：添加HMC5883L等磁力计实现全姿态解算
• 高度估计：结合气压计数据实现三维定位
• 运动追踪：扩展为惯性导航系统
• 无线传输：通过NRF24L01实现实时姿态监控

6. 完整工程框架

推荐的项目文件结构：

/Drivers /STM32F4xx_HAL_Driver /CMSIS /Inc icm20602.h mahony.h imu_calibration.h /Src main.c icm20602.c mahony.c imu_calibration.c

main.c关键流程：

int main(void) { HAL_Init(); SystemClock_Config(); // 外设初始化 SPI1_Init(); TIM3_Init(8399, 0); // 100Hz中断 // IMU初始化 ICM20602_Init(); IMU_Calibration cal; LoadCalibrationData(&cal); // 从Flash加载校准数据 while(1) { // 主循环处理其他任务 DisplayEulerAngles(); // 显示姿态角 HAL_Delay(100); } }

实际部署中发现，将Mahony算法放在500Hz的中断中运行时，STM32F407能够保持约70%的CPU利用率，为其他控制任务留出了充足的计算余量。对于更复杂的应用场景，可以考虑使用RTOS进行任务调度。