1. 项目概述与核心需求
去年冬天在实验室调试四轴飞行器时,我发现市面上现成的飞控模块要么太贵,要么性能达不到要求。于是决定基于STM32F103自己开发一套多传感器融合的姿态解算系统。这个系统的核心目标是通过九轴传感器数据(加速度计+陀螺仪+磁力计)实时计算飞行器的俯仰角、滚转角和航向角,同时结合气压计数据实现高度测量。
选择STM32F103作为主控是因为它性价比高,72MHz主频足够处理传感器数据,且具备硬件浮点运算单元。传感器方面,MPU6050负责六轴数据(三轴加速度+三轴陀螺仪),HMC5883L提供三轴磁力数据,BMP085气压计用于高度测量。整个系统的难点在于如何将这些传感器的数据有效融合,输出稳定的姿态信息。
2. 硬件选型与传感器配置
2.1 主控芯片:STM32F103C8T6
这款芯片被称为"蓝色药丸",性价比极高。72MHz Cortex-M3内核,64KB Flash,20KB RAM,完全能满足我们的需求。特别要注意的是:
- 必须开启硬件浮点单元(FPU),否则姿态解算的浮点运算会非常慢
- I2C接口建议使用PB6/PB7引脚,因为这两个引脚有内部上拉电阻
- 时钟配置要确保系统时钟为72MHz,APB1总线不超过36MHz
2.2 传感器配置
2.2.1 MPU6050六轴传感器
这是系统的核心传感器,需要注意以下配置参数:
c复制// MPU6050初始化配置
void mpu6050_init(void) {
// 唤醒设备
I2C_Write_Byte(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_1, 0x00);
// 设置陀螺仪量程 ±2000°/s
I2C_Write_Byte(MPU6050_ADDR, MPU6050_RA_GYRO_CONFIG, 0x18);
// 设置加速度计量程 ±8g
I2C_Write_Byte(MPU6050_ADDR, MPU6050_RA_ACCEL_CONFIG, 0x10);
// 设置低通滤波器带宽 42Hz
I2C_Write_Byte(MPU6050_ADDR, MPU6050_RA_CONFIG, 0x03);
// 设置采样率 1kHz/(1+4)=200Hz
I2C_Write_Byte(MPU6050_ADDR, MPU6050_RA_SMPLRT_DIV, 0x04);
}
2.2.2 HMC5883L磁力计
磁力计的配置有几个关键点容易出错:
c复制void hmc5883l_init(void) {
// 设置为连续测量模式
I2C_Write_Byte(HMC5883L_ADDR, HMC5883L_RA_MODE, 0x00);
// 设置数据输出速率75Hz
I2C_Write_Byte(HMC5883L_ADDR, HMC5883L_RA_CONFIG_A, 0x18);
// 设置量程±1.3Ga
I2C_Write_Byte(HMC5883L_ADDR, HMC5883L_RA_CONFIG_B, 0x20);
}
特别注意磁力计的安装方向必须与MPU6050保持一致,否则需要进行坐标系转换。
2.2.3 BMP085气压计
气压计主要用于高度测量,初始化时需要注意:
c复制void bmp085_init(void) {
// 读取校准参数
ac1 = I2C_Read_16(BMP085_ADDR, 0xAA);
ac2 = I2C_Read_16(BMP085_ADDR, 0xAC);
// ...其他校准参数读取
// 设置超采样参数为标准模式
oss = 0;
}
3. 传感器数据融合算法
3.1 DMP解算与九轴融合
MPU6050内置的DMP(数字运动处理器)可以解算姿态,但默认只使用六轴数据。要实现九轴融合,需要修改DMP配置:
c复制void dmp_init_with_mag(void) {
mpu_device_init();
// 启用加速度计和陀螺仪
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(100);
dmp_load_motion_driver_firmware();
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
// 启用九轴融合
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL |
DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL);
// 设置磁力计参数
dmp_enable_9x_sensor_fusion();
dmp_set_fifo_rate(100);
mpu_set_dmp_state(1);
}
3.2 卡尔曼滤波实现
对于资源有限的STM32F103,全量九轴卡尔曼滤波计算量太大。我采用了分离轴处理的简化方案:
c复制typedef struct {
float angle; // 角度估计值
float bias; // 陀螺仪零偏
float P[2][2]; // 误差协方差矩阵
float Q_angle; // 过程噪声协方差
float Q_bias; // 过程噪声协方差
float R_measure; // 测量噪声协方差
} Kalman_t;
float Kalman_update(Kalman_t *kf, float newAngle, float newRate, float dt) {
// 预测步骤
float rate = newRate - kf->bias;
kf->angle += dt * rate;
// 更新协方差矩阵
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_bias * dt;
// 计算卡尔曼增益
float S = kf->P[0][0] + kf->R_measure;
float K[2];
K[0] = kf->P[0][0] / S;
K[1] = kf->P[1][0] / S;
// 更新估计
float y = newAngle - kf->angle;
kf->angle += K[0] * y;
kf->bias += K[1] * y;
// 更新协方差
float P00_temp = kf->P[0][0];
float P01_temp = kf->P[0][1];
kf->P[0][0] -= K[0] * P00_temp;
kf->P[0][1] -= K[0] * P01_temp;
kf->P[1][0] -= K[1] * P00_temp;
kf->P[1][1] -= K[1] * P01_temp;
return kf->angle;
}
参数调优是关键,经过多次实验,我确定了以下参数组合效果最佳:
- Q_angle = 0.001
- Q_bias = 0.003
- R_measure = 0.03
4. 系统实现与优化
4.1 主程序架构
系统采用定时中断+主循环的架构:
c复制int main(void) {
// 硬件初始化
HAL_Init();
SystemClock_Config();
MX_I2C1_Init();
MX_USART1_UART_Init();
// 传感器初始化
mpu6050_init();
hmc5883l_init();
bmp085_init();
// 滤波器初始化
Kalman_t pitch_kalman = {0};
Kalman_t roll_kalman = {0};
Kalman_t yaw_kalman = {0};
// 定时器配置,100Hz更新频率
HAL_TIM_Base_Start_IT(&htim2);
while (1) {
// 等待中断标志
if (data_ready_flag) {
data_ready_flag = 0;
// 读取传感器数据
mpu6050_read_data(&accel, &gyro);
hmc5883l_read_data(&mag);
bmp085_read_data(&temp, &pressure);
// 计算时间间隔
uint32_t now = HAL_GetTick();
float dt = (now - last_time) / 1000.0f;
last_time = now;
// 计算原始角度
float pitch_acc = atan2f(accel.y, sqrtf(accel.x*accel.x + accel.z*accel.z));
float roll_acc = atan2f(-accel.x, accel.z);
// 磁力计航向角计算
float mag_x = mag.x * cosf(pitch_kalman.angle) + mag.z * sinf(pitch_kalman.angle);
float mag_y = mag.x * sinf(roll_kalman.angle) * sinf(pitch_kalman.angle)
+ mag.y * cosf(roll_kalman.angle)
- mag.z * sinf(roll_kalman.angle) * cosf(pitch_kalman.angle);
float yaw_mag = atan2f(-mag_y, mag_x);
// 卡尔曼滤波更新
pitch_kalman.angle = Kalman_update(&pitch_kalman, pitch_acc, gyro.x, dt);
roll_kalman.angle = Kalman_update(&roll_kalman, roll_acc, gyro.y, dt);
yaw_kalman.angle = Kalman_update(&yaw_kalman, yaw_mag, gyro.z, dt);
// 高度计算
float altitude = calculate_altitude(pressure, sea_level_pressure);
// 数据输出
printf("Pitch:%.2f,Roll:%.2f,Yaw:%.2f,Alt:%.2f\n",
pitch_kalman.angle, roll_kalman.angle, yaw_kalman.angle, altitude);
}
}
}
4.2 性能优化技巧
-
浮点运算优化:
- 启用STM32的硬件FPU
- 将常用三角函数值预先计算并存储
- 使用快速平方根近似算法
-
I2C通信优化:
- 将多次单字节读取改为批量读取
- 合理设置I2C时钟频率(实测400kHz在STM32F103上不稳定,建议使用100kHz)
- 添加超时重试机制
-
内存优化:
- 将大型数组定义为const类型存放在Flash中
- 使用位域结构体节省内存
- 合理使用DMA减少CPU负载
5. 实际测试与问题排查
5.1 常见问题及解决方案
- 航向角漂移问题:
- 现象:静止状态下航向角缓慢变化
- 原因:磁力计受周围磁场干扰
- 解决:实施硬铁和软铁补偿校准
c复制void compass_calibrate(void) {
// 采集多个方向的磁力计数据
for(int i=0; i<500; i++) {
hmc5883l_read_data(&mag);
// 更新最大最小值
if(mag.x < mag_min.x) mag_min.x = mag.x;
if(mag.x > mag_max.x) mag_max.x = mag.x;
// 同样处理y和z轴
HAL_Delay(10);
}
// 计算补偿参数
mag_offset.x = (mag_max.x + mag_min.x)/2;
mag_offset.y = (mag_max.y + mag_min.y)/2;
mag_offset.z = (mag_max.z + mag_min.z)/2;
mag_scale.x = (mag_max.x - mag_min.x)/2;
mag_scale.y = (mag_max.y - mag_min.y)/2;
mag_scale.z = (mag_max.z - mag_min.z)/2;
float avg_scale = (mag_scale.x + mag_scale.y + mag_scale.z)/3;
mag_scale.x = avg_scale/mag_scale.x;
mag_scale.y = avg_scale/mag_scale.y;
mag_scale.z = avg_scale/mag_scale.z;
}
- 高度测量漂移问题:
- 现象:高度测量值缓慢变化
- 原因:气压受温度影响
- 解决:增加温度补偿,采用滑动平均滤波
c复制float pressure_filter(float new_pressure) {
static float pressure_buffer[10] = {0};
static uint8_t index = 0;
pressure_buffer[index] = new_pressure;
index = (index + 1) % 10;
float sum = 0;
for(int i=0; i<10; i++) {
sum += pressure_buffer[i];
}
return sum / 10;
}
5.2 系统性能测试
经过优化后,系统性能指标如下:
| 指标 | 数值 |
|---|---|
| 更新频率 | 100Hz |
| 角度分辨率 | 0.1° |
| 静态漂移 | <1°/min |
| 动态响应时间 | <50ms |
| 高度分辨率 | 0.1m |
| 处理器负载 | 65% |
6. 扩展功能实现
6.1 数据记录与回放
添加SD卡模块实现飞行数据记录:
c复制void log_data(float pitch, float roll, float yaw, float alt) {
static FIL file;
static bool first_open = true;
if(first_open) {
f_open(&file, "flight.log", FA_WRITE | FA_CREATE_ALWAYS);
first_open = false;
f_printf(&file, "Time,Pitch,Roll,Yaw,Altitude\n");
}
uint32_t time = HAL_GetTick();
f_printf(&file, "%lu,%.2f,%.2f,%.2f,%.2f\n", time, pitch, roll, yaw, alt);
// 每隔一定时间同步到磁盘
static uint32_t last_sync = 0;
if(time - last_sync > 1000) {
f_sync(&file);
last_sync = time;
}
}
6.2 无线数据传输
添加HC-05蓝牙模块实现实时数据监控:
c复制void send_telemetry(float pitch, float roll, float yaw, float alt) {
char buffer[128];
snprintf(buffer, sizeof(buffer),
"P:%.1f,R:%.1f,Y:%.1f,A:%.1f\n",
pitch, roll, yaw, alt);
[HAL](https://taotoken.net/?utm_source=hardware)_UART_Transmit(&huart1, (uint8_t*)buffer, strlen(buffer), 100);
}
7. 项目总结与改进方向
经过两个月的开发和调试,这套基于STM32F103的多传感器融合姿态解算系统已经能够稳定工作。实测表明,在静态环境下角度漂移小于1°/分钟,动态响应时间在50ms以内,完全满足小型无人机的控制需求。
几个关键的经验教训:
- 传感器校准至关重要,特别是磁力计的硬铁和软铁补偿
- 对于资源有限的MCU,算法复杂度需要仔细权衡
- 时间同步是数据融合的关键,必须精确测量数据采集间隔
未来的改进方向包括:
- 添加GPS模块实现位置估计
- 实现基于扩展卡尔曼滤波的完整状态估计
- 开发地面站软件进行三维姿态可视化