1. 项目背景与目标
姿态检测是嵌入式系统开发中的常见需求,广泛应用于无人机、平衡车、机器人导航等领域。MPU6050作为一款集成了三轴加速度计和三轴陀螺仪的6轴运动传感器,因其高性价比和易用性成为开发者的首选方案。然而,直接使用MPU6050的原始数据会面临两个主要问题:陀螺仪存在积分漂移,加速度计则容易受到高频噪声干扰。
本项目基于TI的MSPM0G3507单片机,通过软件模拟I2C协议读取MPU6050数据,并采用卡尔曼滤波算法进行数据融合,最终实现俯仰角(Pitch)、横滚角(Roll)和偏航角(Yaw)的稳定检测。相比硬件I2C,软件模拟方案具有更好的移植性和灵活性,而卡尔曼滤波则能有效解决传感器噪声和漂移问题。
2. 硬件设计与连接
2.1 MSPM0G3507单片机简介
MSPM0G3507是TI推出的低成本、低功耗ARM Cortex-M0+内核单片机,主要特性包括:
- 48MHz主频,64KB Flash,8KB SRAM
- 丰富的外设接口:UART、SPI、I2C等
- 超低功耗设计,运行模式电流仅80μA/MHz
- 工作电压范围1.62V至3.6V
2.2 MPU6050传感器特性
MPU6050的主要技术参数:
- 三轴加速度计:量程±2g/±4g/±8g/±16g可选
- 三轴陀螺仪:量程±250°/s至±2000°/s可调
- 16位ADC分辨率
- I2C接口通信,最高400kHz
- 内置温度传感器
- 工作电压2.375V-3.46V
2.3 硬件连接方案
由于MSPM0G3507的硬件I2C引脚可能被其他外设占用,我们采用软件模拟I2C的方案,使用GPIO引脚连接MPU6050:
| MPU6050引脚 | MSPM0连接 | 配置说明 |
|---|---|---|
| VCC | 3.3V | 电源输入 |
| GND | GND | 地线 |
| SCL | PB9 | GPIO模拟SCL |
| SDA | PB8 | GPIO模拟SDA |
| AD0 | GND | I2C地址选择 |
注意:MPU6050的AD0引脚接地表示I2C地址为0x68(7位地址),左移一位后的写地址为0xD0,读地址为0xD1。
3. 软件I2C驱动实现
3.1 I2C协议基础
I2C总线协议要点:
- 两线制:SCL(时钟)和SDA(数据)
- 主从架构,支持多主多从
- 数据传输速率:标准模式100kHz,快速模式400kHz
- 通信流程:起始条件→地址帧→数据帧→停止条件
3.2 GPIO模拟I2C实现
软件I2C的核心是通过GPIO模拟时序,关键函数包括:
3.2.1 引脚方向控制
c复制void SDA_OUT(void) {
DL_GPIO_enableOutput(mpu6050_PORT, mpu6050_B8_PIN);
}
void SDA_IN(void) {
DL_GPIO_disableOutput(mpu6050_PORT, mpu6050_B8_PIN);
}
3.2.2 起始和停止条件
c复制void i2c_start(void) {
SDA_OUT();
i2c_sda_write(1);
i2c_scl_write(1);
i2c_sda_write(0); // 起始条件:SCL高时SDA由高变低
i2c_scl_write(0);
}
void i2c_stop(void) {
SDA_OUT();
i2c_sda_write(0);
i2c_scl_write(1);
i2c_sda_write(1); // 停止条件:SCL高时SDA由低变高
SDA_IN();
}
3.2.3 字节发送与接收
c复制void i2c_sendbyte(uint8_t byte) {
SDA_OUT();
for(uint8_t i=0; i<8; i++) {
i2c_sda_write(byte & (0x80 >> i));
i2c_scl_write(1);
i2c_scl_write(0);
}
}
uint8_t i2c_receivebyte(void) {
uint8_t byte = 0x00;
SDA_IN();
for(uint8_t i=0; i<8; i++) {
i2c_scl_write(1);
if(i2c_sda_read()) byte |= (0x80 >> i);
i2c_scl_write(0);
}
return byte;
}
3.3 MPU6050驱动封装
基于软件I2C实现MPU6050的读写函数:
c复制void mpu_write_reg(uint8_t reg, uint8_t data) {
i2c_start();
i2c_sendbyte(MPU6050_ADDR); // 从机地址+写
i2c_receiveack();
i2c_sendbyte(reg); // 寄存器地址
i2c_receiveack();
i2c_sendbyte(data); // 写入数据
i2c_receiveack();
i2c_stop();
}
uint8_t mpu_read_reg(uint8_t reg) {
uint8_t data;
i2c_start();
i2c_sendbyte(MPU6050_ADDR); // 从机地址+写
i2c_receiveack();
i2c_sendbyte(reg); // 寄存器地址
i2c_receiveack();
i2c_start();
i2c_sendbyte(MPU6050_ADDR|0x01); // 从机地址+读
i2c_receiveack();
data = i2c_receivebyte();
i2c_sendack(1); // 非应答
i2c_stop();
return data;
}
4. MPU6050初始化与数据读取
4.1 传感器初始化配置
c复制void mpu_init(void) {
i2c_init();
// 解除睡眠模式,选择陀螺仪X轴作为时钟源
mpu_write_reg(MPU6050_PWR_MGMT_1, 0x01);
// 所有轴都不进入待机模式
mpu_write_reg(MPU6050_PWR_MGMT_2, 0x00);
// 设置采样率分频器
mpu_write_reg(MPU6050_SMPLRT_DIV, 0x09); // 采样率=1kHz/(1+9)=100Hz
// 配置数字低通滤波器
mpu_write_reg(MPU6050_CONFIG, 0x06); // 带宽44Hz,延迟4.9ms
// 配置陀螺仪量程±2000°/s
mpu_write_reg(MPU6050_GYRO_CONFIG, 0x18);
// 配置加速度计量程±8g
mpu_write_reg(MPU6050_ACCEL_CONFIG, 0x10);
}
4.2 原始数据读取与处理
MPU6050的传感器数据存储在连续的寄存器中,读取时需要先读取高字节再读低字节:
c复制void mpu_get_data(int16_t *acc, int16_t *gyro) {
uint8_t buf[14];
// 读取所有传感器数据(14个寄存器)
i2c_start();
i2c_sendbyte(MPU6050_ADDR);
i2c_receiveack();
i2c_sendbyte(MPU6050_ACCEL_XOUT_H);
i2c_receiveack();
i2c_start();
i2c_sendbyte(MPU6050_ADDR|0x01);
i2c_receiveack();
for(uint8_t i=0; i<13; i++) {
buf[i] = i2c_receivebyte();
i2c_sendack(0); // 应答
}
buf[13] = i2c_receivebyte();
i2c_sendack(1); // 非应答
i2c_stop();
// 组合高低字节
acc[0] = (buf[0]<<8)|buf[1]; // AccX
acc[1] = (buf[2]<<8)|buf[3]; // AccY
acc[2] = (buf[4]<<8)|buf[5]; // AccZ
gyro[0] = (buf[8]<<8)|buf[9]; // GyroX
gyro[1] = (buf[10]<<8)|buf[11];// GyroY
gyro[2] = (buf[12]<<8)|buf[13];// GyroZ
}
5. 卡尔曼滤波算法实现
5.1 卡尔曼滤波原理
卡尔曼滤波是一种递归的状态估计算法,通过"预测-更新"两个阶段实现对系统状态的最优估计:
-
预测阶段:
- 状态预测:x̂ₖ⁻ = Fx̂ₖ₋₁ + Buₖ
- 协方差预测:Pₖ⁻ = FPₖ₋₁Fᵀ + Q
-
更新阶段:
- 卡尔曼增益:Kₖ = Pₖ⁻Hᵀ(HPₖ⁻Hᵀ + R)⁻¹
- 状态更新:x̂ₖ = x̂ₖ⁻ + Kₖ(zₖ - Hx̂ₖ⁻)
- 协方差更新:Pₖ = (I - KₖH)Pₖ⁻
在姿态解算中:
- 状态量:角度、角速度偏差
- 观测量:加速度计计算的角度
- 过程噪声Q:陀螺仪的噪声特性
- 观测噪声R:加速度计的噪声特性
5.2 一维卡尔曼滤波实现
针对单个轴的角度滤波实现:
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 *k, float new_angle, float new_rate, float dt) {
// 预测阶段
k->angle += dt * (new_rate - k->bias);
k->P[0][0] += dt * (dt*k->P[1][1] - k->P[0][1] - k->P[1][0] + k->Q_angle);
k->P[0][1] -= dt * k->P[1][1];
k->P[1][0] -= dt * k->P[1][1];
k->P[1][1] += k->Q_bias * dt;
// 更新阶段
float S = k->P[0][0] + k->R_measure;
float K[2];
K[0] = k->P[0][0] / S;
K[1] = k->P[1][0] / S;
float y = new_angle - k->angle;
k->angle += K[0] * y;
k->bias += K[1] * y;
float P00_temp = k->P[0][0];
float P01_temp = k->P[0][1];
k->P[0][0] -= K[0] * P00_temp;
k->P[0][1] -= K[0] * P01_temp;
k->P[1][0] -= K[1] * P00_temp;
k->P[1][1] -= K[1] * P01_temp;
return k->angle;
}
5.3 姿态解算实现
结合加速度计和陀螺仪数据进行姿态解算:
c复制void get_attitude(float *roll, float *pitch, float *yaw) {
static float roll_kalman, pitch_kalman;
static Kalman_t kalman_roll = {
.Q_angle = 0.001f,
.Q_bias = 0.003f,
.R_measure = 0.5f
};
static Kalman_t kalman_pitch = {
.Q_angle = 0.001f,
.Q_bias = 0.003f,
.R_measure = 0.5f
};
// 获取原始数据
int16_t acc[3], gyro[3];
mpu_get_data(acc, gyro);
// 转换为物理量
float acc_x = acc[0] / 4096.0f; // ±8g量程,灵敏度4096LSB/g
float acc_y = acc[1] / 4096.0f;
float acc_z = acc[2] / 4096.0f;
float gyro_x = gyro[0] / 16.4f; // ±2000°/s量程,灵敏度16.4LSB/(°/s)
float gyro_y = gyro[1] / 16.4f;
float gyro_z = gyro[2] / 16.4f;
// 加速度计计算角度
float roll_acc = atan2(acc_y, acc_z) * 180.0f / PI;
float pitch_acc = -atan2(acc_x, sqrt(acc_y*acc_y + acc_z*acc_z)) * 180.0f / PI;
// 卡尔曼滤波
*roll = kalman_update(&kalman_roll, roll_acc, gyro_x, 0.01f);
*pitch = kalman_update(&kalman_pitch, pitch_acc, gyro_y, 0.01f);
*yaw += gyro_z * 0.01f; // 偏航角仅用陀螺仪积分
}
6. 系统集成与优化
6.1 零偏校准
传感器上电后需要进行零偏校准:
c复制void mpu_calibrate() {
float acc_offset[3] = {0};
float gyro_offset[3] = {0};
int16_t acc[3], gyro[3];
// 采集100次数据求平均
for(int i=0; i<100; i++) {
mpu_get_data(acc, gyro);
acc_offset[0] += acc[0]/4096.0f;
acc_offset[1] += acc[1]/4096.0f;
acc_offset[2] += acc[2]/4096.0f - 1.0f; // Z轴应有1g重力
gyro_offset[0] += gyro[0]/16.4f;
gyro_offset[1] += gyro[1]/16.4f;
gyro_offset[2] += gyro[2]/16.4f;
delay_ms(10);
}
// 保存校准值
for(int i=0; i<3; i++) {
acc_offset[i] /= 100.0f;
gyro_offset[i] /= 100.0f;
}
// 后续读取数据时减去偏移量
// acc_x = (acc[0]/4096.0f) - acc_offset[0];
// ...
}
6.2 主程序流程
c复制int main(void) {
// 硬件初始化
board_init();
i2c_init();
mpu_init();
oled_init();
// 传感器校准
mpu_calibrate();
// 主循环
while(1) {
float roll, pitch, yaw;
get_attitude(&roll, &pitch, &yaw);
// OLED显示
oled_show_float(0, 0, roll, 1);
oled_show_float(0, 2, pitch, 1);
oled_show_float(0, 4, yaw, 1);
// 串口输出
printf("Roll:%.1f Pitch:%.1f Yaw:%.1f\n", roll, pitch, yaw);
delay_ms(10); // 10ms周期
}
}
7. 实际应用中的问题与解决
7.1 常见问题排查
-
I2C通信失败
- 检查硬件连接是否正确,SCL/SDA线是否接反
- 用逻辑分析仪抓取I2C波形,确认时序符合规范
- 检查MPU6050的地址是否正确(AD0引脚电平)
-
数据跳动严重
- 确保传感器安装稳固,避免机械振动
- 调整卡尔曼滤波参数Q和R
- 检查电源是否稳定,必要时增加滤波电容
-
角度漂移
- 重新进行零偏校准
- 检查陀螺仪量程是否合适,过大量程会降低分辨率
- 增加加速度计的权重(减小R_measure)
7.2 参数调优建议
卡尔曼滤波性能很大程度上取决于Q和R参数的设置:
- Q_angle:角度过程噪声,值越小表示对陀螺仪信任度越高
- Q_bias:零偏过程噪声,影响陀螺仪零偏估计的速度
- R_measure:测量噪声,值越大表示对加速度计信任度越低
典型调参步骤:
- 先设置R_measure=0.5,Q_angle=0.001,Q_bias=0.003
- 观察响应速度和稳定性
- 如需更平滑的输出,增大R_measure或减小Q_angle
- 如需更快响应,减小R_measure或增大Q_angle
7.3 性能优化技巧
- 定点数运算:在资源受限的单片机上,可将浮点运算转换为定点数运算提高效率
- 采样率匹配:根据应用需求选择合适采样率,平衡性能和资源占用
- 传感器融合:可结合磁力计数据提高偏航角精度
- 动态调参:根据运动状态动态调整卡尔曼参数,静止时更信任加速度计
8. 项目扩展与改进
8.1 硬件改进方向
- 改用硬件I2C:在引脚允许的情况下,使用硬件I2C可提高通信可靠性
- 增加磁力计:添加HMC5883L等磁力计可实现9轴姿态解算
- 多传感器冗余:使用多个IMU提高系统可靠性
8.2 算法改进方向
- 互补滤波:作为卡尔曼滤波的替代方案,计算量更小
- Mahony滤波:一种轻量级的姿态估计算法
- DCM矩阵:方向余弦矩阵法,适合全姿态解算
- 四元数:避免欧拉角的万向节死锁问题
8.3 应用场景扩展
- 无人机飞控:作为姿态反馈核心
- 平衡车控制:检测车身倾斜角度
- 虚拟现实:头部运动追踪
- 运动分析:运动员动作捕捉和分析
通过本项目的实践,我们实现了基于MSPM0和MPU6050的高精度姿态检测系统。软件I2C方案具有良好的可移植性,卡尔曼滤波有效融合了加速度计和陀螺仪的数据优势。实际应用中可根据具体需求调整参数和算法,获得最佳性能。