STM32F4平台MPU6050四元数计算异常问题求助

我关注MPU6050已有一段时间，目前能够读取陀螺仪数据，但需要帮助将其转换为四元数。在Arduino上用jrowberg的库很容易实现，但现在我用的是STM32F4平台。

我查阅MPU6050数据手册后编写了代码，定义如下：

typedef struct {
    float Temperature;

    int16_t Gyro_X_RAW;
    int16_t Gyro_Y_RAW;
    int16_t Gyro_Z_RAW;
    float Gx;
    float Gy;
    float Gz;
} MPU6050_t;

typedef struct {
    float w;
    float x; 
    float y; 
    float z; 
} Quaternion;

Quaternion q = {1.0, 0.0, 0.0, 0.0}; 
MPU6050_t MPU6050;

读取陀螺仪数据的代码：

void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
    uint8_t Rec_Data[6];

    // 从GYRO_XOUT_H寄存器开始读取6字节数据
    HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, 100);

    DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
    DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
    DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);

    /*** 将原始值转换为dps（°/s）
         根据FS_SEL设置的满量程值进行除法
         我配置的FS_SEL = 0，所以除以131.0
         更多细节查看GYRO_CONFIG寄存器 ****/

    DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
    DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
    DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;
}

主循环代码：

while (1)
{
    MPU6050_Read_Temp(&hi2c1, &MPU6050);
    MPU6050_Read_Gyro(&hi2c1, &MPU6050);
    float gyroX = MPU6050.Gx;
    float gyroY = MPU6050.Gy;
    float gyroZ = MPU6050.Gz;
    calculateQuaternion(&q, gyroX, gyroY, gyroZ, dt);

    char buffer[50];    
    uint8_t size = sprintf(buffer,"quat\t%.2f\t%.2f\t%.2f\t%.2f\r\n", q.w, q.x, q.y, q.z);      
    HAL_StatusTypeDef status = HAL_UART_Transmit(&huart3, (uint8_t*)buffer, size, 1000);
    HAL_Delay(1000);
}

四元数计算函数：

void calculateQuaternion(Quaternion *q, float gyroX, float gyroY, float gyroZ, float dt) {

    float dThetaX = gyroX * dt;
    float dThetaY = gyroY * dt;
    float dThetaZ = gyroZ * dt;

    float dq0 = 1.0;
    float dq1 = dThetaX * 0.5;
    float dq2 = dThetaY * 0.5;
    float dq3 = dThetaZ * 0.5;

    q->w += dq0 * q->w - dq1 * q->x - dq2 * q->y - dq3 * q->z;
    q->x += dq0 * q->x + dq1 * q->w + dq2 * q->z - dq3 * q->y;
    q->y += dq0 * q->y - dq1 * q->z + dq2 * q->w + dq3 * q->x;
    q->z += dq0 * q->z + dq1 * q->y - dq2 * q->x + dq3 * q->w;

    float norm = sqrt(q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);
    q->w /= norm;
    q->x /= norm;
    q->y /= norm;
    q->z /= norm;
}

调试时发现，即使传感器静止、陀螺仪数据无明显变化，四元数仍持续变动。

问题修正 #

你的四元数更新逻辑存在两个核心错误：

1. 角速度单位不匹配：陀螺仪输出的是dps（度/秒），但四元数更新需要的是弧度/秒，必须先进行单位转换。
2. 微分四元数计算错误：错误地将dq0设为1.0，正确的四元数更新应基于导数公式，而非错误的小角度近似扩展。

修正后的calculateQuaternion函数：

#include <math.h>

void calculateQuaternion(Quaternion *q, float gyroX, float gyroY, float gyroZ, float dt) {
    // 将dps转换为rad/s
    float gx = gyroX * M_PI / 180.0f;
    float gy = gyroY * M_PI / 180.0f;
    float gz = gyroZ * M_PI / 180.0f;

    // 预计算四元数的半值，简化导数计算
    float half_w = q->w * 0.5f;
    float half_x = q->x * 0.5f;
    float half_y = q->y * 0.5f;
    float half_z = q->z * 0.5f;

    // 计算四元数的变化量（欧拉积分实现）
    float dqw = (-half_x * gx) - (half_y * gy) - (half_z * gz);
    float dqx = (half_w * gx) + (half_y * gz) - (half_z * gy);
    float dqy = (half_w * gy) + (half_z * gx) - (half_x * gz);
    float dqz = (half_w * gz) + (half_x * gy) - (half_y * gx);

    // 更新四元数
    q->w += dqw * dt;
    q->x += dqx * dt;
    q->y += dqy * dt;
    q->z += dqz * dt;

    // 归一化，防止数值漂移
    float norm = sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z);
    if (norm > 0.0f) {
        float inv_norm = 1.0f / norm;
        q->w *= inv_norm;
        q->x *= inv_norm;
        q->y *= inv_norm;
        q->z *= inv_norm;
    }
}

另外注意：主循环中dt应精确匹配实际采样间隔，若使用HAL_Delay(1000)，dt设为1.0f；更优方案是用定时器计算实际时间差，避免固定延迟带来的误差。

STM32F4平台推荐库/示例 #

• MPU6050_STM32_HAL：专为STM32 HAL库设计的MPU6050驱动，内置Madgwick/Mahony姿态滤波，直接支持四元数输出。
• MadgwickAHRS/MahonyAHRS：经典开源姿态滤波算法，可单独集成，建议结合加速度计数据校准陀螺仪漂移（仅用陀螺仪长期会有累积误差）。
• STM32CubeMX官方示例：通过CubeMX配置I2C接口后，可参考官方传感器融合示例代码，部分工程包含完整的四元数计算逻辑。

内容的提问来源于stack exchange，提问作者f.cerkesli