IMU姿态补偿：坐标系不匹配的四元数转换问题

你的核心问题是混淆了姿态旋转和坐标系轴映射——前者是调整传感器的姿态角度，后者是改变我们描述姿态的坐标系轴定义，单纯用旋转四元数无法解决轴互换的本质问题，需要通过坐标变换矩阵来实现。

问题本质 #

IMU输出的四元数是基于自身坐标系的姿态描述，当目标坐标系的轴定义（比如Roll/Yaw轴互换）与IMU不同时，需要建立两个坐标系之间的轴映射关系，再通过矩阵变换将IMU的姿态转换到目标坐标系下，而不是对姿态做旋转。

解决方案步骤 #

1. 明确轴对应关系 #

先精确列出两个坐标系的轴定义，比如：

• IMU坐标系（I）：X=Roll（右）、Y=Pitch（前）、Z=Yaw（上）
• 目标坐标系（T）：X=Yaw（前）、Y=Pitch（右）、Z=Roll（上）

根据对应关系，构造从IMU坐标系到目标坐标系的正交变换矩阵M——矩阵的每一列是目标坐标系的轴在IMU坐标系中的分量。比如上面的例子，M为：

[0  0  1]  // T的X轴对应I的Z轴
[0  1  0]  // T的Y轴对应I的Y轴
[1  0  0]  // T的Z轴对应I的X轴

如果某轴方向相反（比如IMU的Z向上，目标的Z向下），对应列的分量取负即可。注意：如果是右手系到右手系的转换，矩阵行列式必须为1；如果涉及左手系，需要单独处理反射部分。

2. 实现矩阵与四元数的转换 #

四元数本质是旋转的紧凑表示，我们需要先将IMU四元数转成旋转矩阵，完成坐标变换后再转回四元数。

辅助函数实现

// 四元数转3x3旋转矩阵
void quaternion_to_matrix(const Quaternion* q, float mat[3][3]) {
    float w = q->w, x = q->x, y = q->y, z = q->z;
    float xx = x*x, yy = y*y, zz = z*z;
    float xy = x*y, xz = x*z, yz = y*z;
    float wx = w*x, wy = w*y, wz = w*z;

    mat[0][0] = 1 - 2*(yy + zz);
    mat[0][1] = 2*(xy - wz);
    mat[0][2] = 2*(xz + wy);

    mat[1][0] = 2*(xy + wz);
    mat[1][1] = 1 - 2*(xx + zz);
    mat[1][2] = 2*(yz - wx);

    mat[2][0] = 2*(xz - wy);
    mat[2][1] = 2*(yz + wx);
    mat[2][2] = 1 - 2*(xx + yy);
}

// 3x3旋转矩阵转四元数
void matrix_to_quaternion(const float mat[3][3], Quaternion* q) {
    float trace = mat[0][0] + mat[1][1] + mat[2][2];
    float w, x, y, z;

    if (trace > 0.0f) {
        float s = sqrt(trace + 1.0f);
        w = s * 0.5f;
        s = 0.5f / s;
        x = (mat[2][1] - mat[1][2]) * s;
        y = (mat[0][2] - mat[2][0]) * s;
        z = (mat[1][0] - mat[0][1]) * s;
    } else {
        int i = 0;
        if (mat[1][1] > mat[0][0]) i = 1;
        if (mat[2][2] > mat[i][i]) i = 2;

        int j = (i + 1) % 3;
        int k = (i + 2) % 3;

        float s = sqrt(mat[i][i] - mat[j][j] - mat[k][k] + 1.0f);
        float* q_arr[] = {&x, &y, &z};
        *q_arr[i] = s * 0.5f;
        s = 0.5f / s;
        w = (mat[k][j] - mat[j][k]) * s;
        *q_arr[j] = (mat[j][i] + mat[i][j]) * s;
        *q_arr[k] = (mat[k][i] + mat[i][k]) * s;
    }

    // 归一化消除浮点误差
    float norm = sqrt(w*w + x*x + y*y + z*z);
    q->w /= norm;
    q->x /= norm;
    q->y /= norm;
    q->z /= norm;
}

// 3x3矩阵乘法
void matrix_multiply(const float a[3][3], const float b[3][3], float result[3][3]) {
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            result[i][j] = 0.0f;
            for (int k = 0; k < 3; k++) {
                result[i][j] += a[i][k] * b[k][j];
            }
        }
    }
}

3. 实现四元数转换函数 #

根据你定义的变换矩阵M，编写转换函数：

// 定义你的坐标变换矩阵（根据实际轴对应关系调整）
float M[3][3] = {
    {0.0f, 0.0f, 1.0f},
    {0.0f, 1.0f, 0.0f},
    {1.0f, 0.0f, 0.0f}
};

Quaternion convert_imu_quaternion(const Quaternion* imu_q) {
    float R_imu[3][3];
    quaternion_to_matrix(imu_q, R_imu);

    // 计算目标坐标系下的旋转矩阵：R_target = M * R_imu
    float R_target[3][3];
    matrix_multiply(M, R_imu, R_target);

    Quaternion target_q;
    matrix_to_quaternion(R_target, &target_q);

    return target_q;
}

4. 验证与调整 #

• 初始状态测试：将IMU水平静止，转换后的四元数应对应目标坐标系的初始姿态（比如欧拉角Roll/Pitch/Yaw均为0，符合你的预期）。
• 轴映射测试：旋转IMU的Roll轴，目标坐标系的Yaw轴应同步旋转；旋转IMU的Yaw轴，目标坐标系的Roll轴应同步旋转，以此验证轴映射是否正确。

为什么你的原有方法失效 #

你之前使用的conjugate(m_initialRotation) * imu_quaternion * m_initialRotation是姿态的参考系转换——将IMU姿态从世界系转换到m_initialRotation定义的参考系下，而非改变坐标系的轴定义。这种方法只能调整姿态角度，无法解决轴互换的本质问题，因此在组合旋转时会出现局限性。

内容的提问来源于stack exchange，提问作者Anselme