包子浅谈之解姿态角

大包子

已于 2025-01-25 14:49:21 修改

阅读量1.4k

点赞数 27

文章标签：嵌入式硬件

于 2024-12-08 22:41:07 首次发布

本文链接：https://blog.csdn.net/2302_79999839/article/details/144318745

版权

本文仅代表个人的理解展开叙述，欢迎各位大佬来到评论区交流

IMU（惯性测量单元）在很多嵌入式应用中都发挥着重要作用，特别是在需要精确测量物体姿态（即物体在三维空间中的方向）的场景下，IMU发挥着至关重要的作用。MPU6050作为一款常用的传感器，它集成了加速度计、陀螺仪，并提供高精度的运动数据。本文将通过具体代码和算法步骤，详细探讨如何通过MPU6050获取姿态角，并基于原始数据实现姿态角的解算。

一、解姿态角的常用方法

旋转表示是三维图形学、机器人学、航空航天等领域的重要内容，三种常用的旋转表示法——欧拉角、轴角和四元数，在数学表达、应用和计算效率等方面都有显著差异。我们将更详细地探讨这些旋转表示的定义、优缺点以及应用场景。

1. 欧拉角（Euler Angles）

欧拉角表示方法通过将物体的旋转分解成绕三轴的旋转来描述姿态。这三轴一般选择为X轴、Y轴和Z轴，每个轴的旋转用一个角度来表示，通常这三个角度分别称为：滚转角（roll）、俯仰角（pitch）和偏航角（yaw）。它们分别代表物体围绕不同轴的旋转。

常见的欧拉角顺序有：

ZYX顺序：先绕Z轴旋转，再绕Y轴旋转，最后绕X轴旋转（这个顺序常用于飞行器姿态描述）。
XYZ顺序：先绕X轴旋转，再绕Y轴旋转，最后绕Z轴旋转。

优点：

直观易懂：旋转角度与物体的物理姿态有直接关系，因此可以非常直观地理解。例如，飞行器的俯仰、滚转和偏航角就是典型的欧拉角应用。
简单易用：描述物体的旋转只需要三个角度，因此非常简洁和易用，尤其适合实时系统中快速描述物体姿态。
转换和插值简便：与其他表示方法相比，欧拉角的转换与插值较为直接，常见的转化公式比较简单。

缺点：

万向节锁（Gimbal Lock）：欧拉角表示的一个显著缺点是可能出现万向节锁。当其中两个旋转轴对齐时，旋转自由度降低，导致某些旋转无法表示，系统会陷入奇异状态。比如，当俯仰角（pitch）接近 ±90° 时，飞行器的偏航角（yaw）和滚转角（roll）无法独立控制。
旋转顺序问题：欧拉角的结果与旋转顺序密切相关，改变旋转顺序可能会导致不同的旋转结果。这会使得在不同系统或应用中处理旋转时，产生混乱或不一致的结果。
插值复杂：由于万向节锁和旋转顺序的影响，欧拉角的插值方法并不直观，尤其是在旋转过渡时，容易产生不自然的旋转轨迹。

2. 轴角（Axis-Angle）

轴角表示法使用一个单位向量和一个旋转角度来描述旋转。旋转是绕某个固定轴进行的，旋转角度表示旋转的幅度。具体来说，轴角表示由以下两部分组成：

旋转轴：一个单位向量，表示旋转的轴线。
旋转角度：一个角度 θ，表示围绕旋转轴的旋转角度。

优点：

避免万向节锁：轴角表示法没有万向节锁的问题，可以表示任意的三维旋转。
简洁高效：旋转轴和角度的两个参数足以完全描述物体的旋转，表示更为简洁。
无旋转顺序问题：与欧拉角不同，轴角不受旋转顺序的影响，因此避免了顺序问题，旋转操作更为稳定。

缺点：

不直观：轴角的直观性较差。尽管它提供了精确的旋转控制，但理解旋转轴和角度之间的关系对普通用户来说相对困难。
计算复杂度：将轴角转换为其他表示方法（如四元数或旋转矩阵）可能需要额外的计算，尤其是在多次旋转组合时，计算复杂度上升。

3. 四元数（Quaternions）

四元数是由四个分量组成的扩展复数，表示为： $q = w + xi + yj + zk$ 其中 $w, x, y, z$ 是四元数的分量，满足单位四元数的条件 $w^2 + x^2 + y^2 + z^2 = 1$ 。四元数本质上是一个四维向量，可以非常高效地表示三维空间中的旋转。

优点：

避免万向节锁：四元数能够表示任意的三维旋转，并且不会受到万向节锁的影响。
高效的旋转运算：四元数的旋转计算非常高效，尤其适用于连续的旋转运算，计算量小且插值精度高（例如SLERP插值法）。
数值稳定性：四元数能有效避免数值误差积累，尤其是在多个旋转的连续运算中，保持了高精度和数值稳定性。

缺点：

不直观：四元数本质上是数学抽象，缺乏物理直观性。理解四元数如何表示旋转需要一定的数学基础。
初始化和转换复杂：从欧拉角、旋转矩阵等其他旋转表示转换为四元数时，需进行较为复杂的数学操作。

综上所述，我们最终得到的结果如下表所示：

二、解姿态角的步骤

1.流程图

2.代码详例

注：本文章解算运用的是四元数解算

陀螺仪的初始化

// 函数声明
void MPU6050_Init(void);
void I2C_WriteByte(uint8_t addr, uint8_t reg, uint8_t data);

void MPU6050_Init(void) {
    // 唤醒MPU6050
    I2C_WriteByte(MPU6050_ADDR, MPU6050_PWR_MGMT_1, 0x00);
    
    // 设置采样率（8kHz / (1 + SMPLRT_DIV)）
    I2C_WriteByte(MPU6050_ADDR, MPU6050_SMPLRT_DIV, 0x07);  // 设置为1kHz采样率

    // 设置低通滤波器
    I2C_WriteByte(MPU6050_ADDR, MPU6050_CONFIG, 0x06); // 低通滤波器设置为5Hz

    // 配置陀螺仪，±500°/s
    I2C_WriteByte(MPU6050_ADDR, MPU6050_GYRO_CONFIG, 0x08);

    // 配置加速度计，±4g
    I2C_WriteByte(MPU6050_ADDR, MPU6050_ACCEL_CONFIG, 0x08);
}

void I2C_WriteByte(uint8_t addr, uint8_t reg, uint8_t data) {
    // 启动I2C通信
    I2C_Start();
    
    // 发送设备地址（写）
    I2C_SendByte(addr);
    I2C_WaitAck();
    
    // 发送寄存器地址
    I2C_SendByte(reg);
    I2C_WaitAck();
    
    // 发送数据
    I2C_SendByte(data);
    I2C_WaitAck();
    
    // 停止I2C通信
    I2C_Stop();
}

零漂校准

零漂校准的核心作用：

消除传感器固有的偏移量；
提高测量精度，增强信号可靠性；
减少姿态计算的累积误差；
准确反映传感器的实际工作状态。

void MPU6050_Calibrate(int16_t *accel_offset, int16_t *gyro_offset) {
    // 累加器用于存储加速度计和陀螺仪的总和
    int32_t accel_x_sum = 0, accel_y_sum = 0, accel_z_sum = 0;
    int32_t gyro_x_sum = 0, gyro_y_sum = 0, gyro_z_sum = 0;

    // 用于存储单次读取的加速度和陀螺仪原始数据
    int16_t accel_data[3], gyro_data[3];

    // 采样的次数，校准结果的准确性与样本数量相关
    uint16_t sample_count = 1000;

    // 多次采样加速度计和陀螺仪的数据，进行累加
    for (uint16_t i = 0; i < sample_count; i++) {
        // 读取当前加速度计和陀螺仪的原始数据
        // 此函数需要实现，读取加速度x/y/z和陀螺仪x/y/z
        MPU6050_ReadRawData(accel_data, gyro_data);

        // 累加加速度计的三个轴数据
        accel_x_sum += accel_data[0];
        accel_y_sum += accel_data[1];
        accel_z_sum += accel_data[2];

        // 累加陀螺仪的三个轴数据
        gyro_x_sum += gyro_data[0];
        gyro_y_sum += gyro_data[1];
        gyro_z_sum += gyro_data[2];
    }

    // 计算加速度计的零偏
    // 将加速度计x/y/z的总和除以样本数，得到平均值
    accel_offset[0] = accel_x_sum / sample_count; // x轴零偏
    accel_offset[1] = accel_y_sum / sample_count; // y轴零偏
    accel_offset[2] = accel_z_sum / sample_count - 16384; // z轴零偏，减去1g重力加速度

    // 计算陀螺仪的零偏
    // 将陀螺仪x/y/z的总和除以样本数，得到平均值
    gyro_offset[0] = gyro_x_sum / sample_count;  // x轴零偏
    gyro_offset[1] = gyro_y_sum / sample_count;  // y轴零偏
    gyro_offset[2] = gyro_z_sum / sample_count;  // z轴零偏
}

采集6050数据



#define MPU6050_ADDR 0xD0  // MPU6050的I2C地址（写模式）
#define MPU6050_ACCEL_XOUT_H 0x3B  // 加速度计X轴高字节寄存器地址
#define MPU6050_GYRO_XOUT_H 0x43   // 陀螺仪X轴高字节寄存器地址

// 函数声明
void MPU6050_ReadRawData(int16_t *accel_data, int16_t *gyro_data);
int16_t MPU6050_ReadWord(uint8_t reg);

void MPU6050_ReadRawData(int16_t *accel_data, int16_t *gyro_data) {
    // 读取加速度计的3个轴数据
    accel_data[0] = MPU6050_ReadWord(MPU6050_ACCEL_XOUT_H);     // X轴加速度
    accel_data[1] = MPU6050_ReadWord(MPU6050_ACCEL_XOUT_H + 2); // Y轴加速度
    accel_data[2] = MPU6050_ReadWord(MPU6050_ACCEL_XOUT_H + 4); // Z轴加速度

    // 读取陀螺仪的3个轴数据
    gyro_data[0] = MPU6050_ReadWord(MPU6050_GYRO_XOUT_H);       // X轴角速度
    gyro_data[1] = MPU6050_ReadWord(MPU6050_GYRO_XOUT_H + 2);   // Y轴角速度
    gyro_data[2] = MPU6050_ReadWord(MPU6050_GYRO_XOUT_H + 4);   // Z轴角速度
}

int16_t MPU6050_ReadWord(uint8_t reg) {
    uint8_t high, low;

    // 开始通信并发送寄存器地址
    I2C_Start();
    I2C_SendByte(MPU6050_ADDR);    // 发送MPU6050地址（写模式）
    I2C_WaitAck();
    I2C_SendByte(reg);             // 发送目标寄存器地址
    I2C_WaitAck();

    // 再次开始通信并切换到读模式
    I2C_Start();
    I2C_SendByte(MPU6050_ADDR | 0x01); // 发送MPU6050地址（读模式）
    I2C_WaitAck();

    // 读取高字节数据
    high = I2C_ReceiveByte();
    I2C_SendAck(); // 发送应答信号

    // 读取低字节数据
    low = I2C_ReceiveByte();
    I2C_SendNack(); // 最后一字节发送非应答信号

    I2C_Stop(); // 停止通信

    // 合成16位数据，高字节在前，低字节在后
    return (int16_t)((high << 8) | low);
}

低通滤波

低通滤波在信号处理中用于减少噪声或平滑数据输出。

一般只对加速度计进行滤波，陀螺仪不滤波

// 定义滤波器系数（范围 0 < alpha <= 1）
#define ALPHA 0.1f // 滤波强度，0.1 为平滑效果较强，数据响应较慢

// 一阶低通滤波函数
float LowPassFilter(float current_input, float previous_output) {
    return ALPHA * current_input + (1 - ALPHA) * previous_output;
}

// 示例：应用到MPU6050数据
void ApplyLowPassFilter(int16_t *raw_data, float *filtered_data) {
    for (int i = 0; i < 3; i++) {
        // 对x、y、z轴分别进行低通滤波
        filtered_data[i] = LowPassFilter((float)raw_data[i], filtered_data[i]);
    }
}

互补滤波

四元数互补滤波原理

加速度计更新方向向量：
- 根据加速度计的三轴数据 Ax,Ay,Az，计算出当前设备的方向向量。
- 方向向量用于矫正四元数的漂移。
陀螺仪更新旋转速率：
- 根据陀螺仪的三轴角速度 Gx,Gy,Gz，通过积分计算四元数变化。
互补滤波融合：
- 结合加速度计的长时间稳定性和陀螺仪的短时间精确性，使用加权系数进行融合：

- α是滤波系数，通常取 0.98。

#define GYRO_SENSITIVITY 131.0      // 陀螺仪灵敏度（±250°/s 对应 131）
#define ACCEL_SENSITIVITY 16384.0   // 加速度计灵敏度（±2g 对应 16384）
#define ALPHA 0.98                  // 互补滤波系数

// 四元数存储 (q0, q1, q2, q3)
typedef struct {
    float q0;
    float q1;
    float q2;
    float q3;
} Quaternion;

// 初始化四元数
Quaternion quat = {1.0f, 0.0f, 0.0f, 0.0f};

// 函数声明
void ComplementaryFilter(int16_t *accel_data, int16_t *gyro_data, Quaternion *q, float dt);
void NormalizeQuaternion(Quaternion *q);

int main() {
    int16_t accel_raw[3], gyro_raw[3]; // 存储原始加速度计和陀螺仪数据
    float dt = 0.01f;                  // 假设采样时间间隔为 10ms

    while (1) {
        // 从 MPU6050 读取原始数据
        MPU6050_ReadRawData(accel_raw, gyro_raw);

        // 更新四元数
        ComplementaryFilter(accel_raw, gyro_raw, &quat, dt);

        // 打印结果
        printf("Quaternion: q0=%.2f, q1=%.2f, q2=%.2f, q3=%.2f\n", quat.q0, quat.q1, quat.q2, quat.q3);

        // 模拟延时
        DelayMs(10);
    }
}

// 互补滤波更新四元数
void ComplementaryFilter(int16_t *accel_data, int16_t *gyro_data, Quaternion *q, float dt) {
    // 加速度计数据转换为重力单位 (g)
    float accel_x = accel_data[0] / ACCEL_SENSITIVITY;
    float accel_y = accel_data[1] / ACCEL_SENSITIVITY;
    float accel_z = accel_data[2] / ACCEL_SENSITIVITY;

    // 陀螺仪数据转换为角速度 (°/s)
    float gyro_x = gyro_data[0] / GYRO_SENSITIVITY;
    float gyro_y = gyro_data[1] / GYRO_SENSITIVITY;
    float gyro_z = gyro_data[2] / GYRO_SENSITIVITY;

    // 步骤 1: 利用陀螺仪更新四元数
    float dq0 = -0.5f * (q->q1 * gyro_x + q->q2 * gyro_y + q->q3 * gyro_z);
    float dq1 =  0.5f * (q->q0 * gyro_x - q->q3 * gyro_y + q->q2 * gyro_z);
    float dq2 =  0.5f * (q->q3 * gyro_x + q->q0 * gyro_y - q->q1 * gyro_z);
    float dq3 = -0.5f * (q->q2 * gyro_x - q->q1 * gyro_y - q->q0 * gyro_z);

    // 更新四元数
    q->q0 += dq0 * dt;
    q->q1 += dq1 * dt;
    q->q2 += dq2 * dt;
    q->q3 += dq3 * dt;

    // 步骤 2: 利用加速度计校正四元数
    float norm = sqrt(accel_x * accel_x + accel_y * accel_y + accel_z * accel_z);
    accel_x /= norm;
    accel_y /= norm;
    accel_z /= norm;

    float accel_q0 = 0.0f;
    float accel_q1 = accel_x;
    float accel_q2 = accel_y;
    float accel_q3 = accel_z;

    // 互补滤波融合
    q->q0 = ALPHA * q->q0 + (1 - ALPHA) * accel_q0;
    q->q1 = ALPHA * q->q1 + (1 - ALPHA) * accel_q1;
    q->q2 = ALPHA * q->q2 + (1 - ALPHA) * accel_q2;
    q->q3 = ALPHA * q->q3 + (1 - ALPHA) * accel_q3;

    // 步骤 3: 归一化四元数
    NormalizeQuaternion(q);
}

// 归一化四元数
void NormalizeQuaternion(Quaternion *q) {
    float norm = sqrt(q->q0 * q->q0 + q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3);
    q->q0 /= norm;
    q->q1 /= norm;
    q->q2 /= norm;
    q->q3 /= norm;
}

计算角度

如果需要将四元数转换为欧拉角（Roll、Pitch、Yaw），可以使用以下公式：

float roll = atan2(2.0f * (q->q0 * q->q1 + q->q2 * q->q3), 1.0f - 2.0f * (q->q1 * q->q1 + q->q2 * q->q2)) * 180.0f / M_PI;
float pitch = asin(2.0f * (q->q0 * q->q2 - q->q3 * q->q1)) * 180.0f / M_PI;
float yaw = atan2(2.0f * (q->q0 * q->q3 + q->q1 * q->q2), 1.0f - 2.0f * (q->q2 * q->q2 + q->q3 * q->q3)) * 180.0f / M_PI;