姿态角解算与 MPU6050 传感器的应用 (基于 STM32 HAL 库)

最新推荐文章于 2025-02-28 19:38:57 发布

超级工男

最新推荐文章于 2025-02-28 19:38:57 发布

阅读量830

点赞数 18

文章标签： stm32 嵌入式硬件单片机无人机

本文链接：https://blog.youkuaiyun.com/zsbsdsz/article/details/145605687

版权

姿态角解算与 MPU6050 传感器的应用 (基于 STM32 HAL 库)

引言

在嵌入式系统中，姿态角（如俯仰角、横滚角和偏航角）是描述物体在三维空间中旋转状态的重要参数。无论是在无人机飞行控制、机器人导航，还是智能手机的运动识别中，姿态角的实时获取与精确解算都具有重要意义。本文将详细讲解如何基于 STM32 HAL 库和 MPU6050 传感器实现姿态角的解算，帮助你快速理解和实现这一技术。

背景知识

MPU6050 传感器

MPU6050 是一款广泛使用的六轴传感器，集成了三轴加速度计和三轴陀螺仪。它能够提供关于物体在空间中的加速度和角速度信息，这些数据可以用来计算物体的姿态角。STM32 通过 I2C 接口与 MPU6050 进行通信，读取加速度计和陀螺仪的原始数据，并根据这些数据进行姿态解算。

姿态角的定义

姿态角通常包括：

俯仰角（Pitch）：描述物体绕横轴旋转的角度。
横滚角（Roll）：描述物体绕纵轴旋转的角度。
偏航角（Yaw）：描述物体绕垂直轴旋转的角度。

我们通过加速度计来测量物体在静止或缓慢运动时的重力方向，通过陀螺仪来计算物体的旋转角速度。结合这两者的数据，可以推算出物体的实时姿态角。

基本原理

加速度计的姿态解算

原理：
加速度计测量的是物体在三个轴上的加速度（包含重力加速度）。当物体静止时，加速度计的输出本质上是重力加速度在各轴上的分量。通过三角函数可以计算姿态角。

公式推导：
假设传感器水平放置时：

Z轴检测重力加速度（约9.81m/s²）
X/Y轴加速度为0

当传感器倾斜时，重力分量会分配到不同轴上：

俯仰角（Pitch）: 绕X轴旋转的角度

在这里插入图片描述

横滚角（Roll）：绕Y轴旋转的角度

在这里插入图片描述
特点：

优点：静态下精度高（直接依赖重力分量）
缺点：动态运动时误差大（无法区分重力加速度和运动加速度）

陀螺仪的姿态解算

原理：
陀螺仪测量的是绕各轴的角速度（单位：°/s）。通过积分角速度数据，可以计算姿态角的变化量。

公式推导：
角速度积分公式：
在这里插入图片描述
其中，ω为角速度，_Δt为采样时间。

特点：

优点：动态响应快，短期精度高
缺点：存在积分漂移（长时间累积误差）

数据融合：互补滤波

为什么需要融合？

加速度计：低频信号可靠（适合静态或缓慢运动）
陀螺仪：高频信号可靠（适合快速运动）
互补滤波：将两者优势结合，通过加权平均消除误差

融合公式

在这里插入图片描述

α：滤波系数（通常取0.95~0.98）
θ _accel：加速度计计算的角度
θ_prev +ω⋅_Δt：陀螺仪积分角度

物理意义：

陀螺仪主导短期变化（抑制加速度计的高频噪声）
加速度计修正长期漂移（抑制陀螺仪的低频漂移）

代码实现关键步骤

传感器初始化

// MPU6050初始化
void MPU6050_Init(void)
 {
    MPU6050_Write(0x6B, 0x80);  // 复位设备
    HAL_Delay(100);
    MPU6050_Write(0x6B, 0x00);  // 唤醒
    MPU6050_Write(0x1B, 0x18);  // 陀螺仪±2000dps
    MPU6050_Write(0x1C, 0x00);  // 加速度±2g
    MPU6050_Write(0x1A, 0x05);  // 低通滤波器5Hz
}

设置MPU6050的量程（例如加速度计±2g，陀螺仪±2000dps）
配置低通滤波器（减少高频噪声）

数据读取与单位转换

// 读取原始数据（16位有符号数）
int16_t accel_raw[3], gyro_raw[3];
MPU6050_ReadRaw(accel_raw, gyro_raw);

// 转换为实际物理量（根据量程的灵敏度）
float accel[3] = 
{
    accel_raw[0] / 16384.0f,  // ±2g时灵敏度为16384 LSB/g
    accel_raw[1] / 16384.0f,
    accel_raw[2] / 16384.0f
};

float gyro[3] = 
{
    gyro_raw[0] / 16.4f,      // ±2000dps时灵敏度为16.4 LSB/(°/s)
    gyro_raw[1] / 16.4f,
    gyro_raw[2] / 16.4f
};

加速度计计算姿态角

void CalcAccelAngles(float *accel, float *pitch, float *roll) 
{
    *pitch = atan2f(accel[1], sqrtf(accel[0]*accel[0] + accel[2]*accel[2])) * 57.2958f;
    *roll  = atan2f(-accel[0], sqrtf(accel[1]*accel[1] + accel[2]*accel[2])) * 57.2958f;
}

互补滤波融合

// 计算时间间隔（单位：秒）
float dt = (HAL_GetTick() - prev_tick) * 0.001f;
prev_tick = HAL_GetTick();

// 陀螺仪积分
angle_pitch += gyro[0] * dt;  // 绕X轴的角速度积分得到Pitch
angle_roll  += gyro[1] * dt;  // 绕Y轴的角速度积分得到Roll

// 加速度计修正
angle_pitch = 0.96f * angle_pitch + 0.04f * accel_pitch;
angle_roll  = 0.96f * angle_roll  + 0.04f * accel_roll;