基于C++ Eigen库旋转矩阵转静态RPY欧拉角的问题排查

问题根源 #

你的核心问题有两点：

• 旋转矩阵的生成顺序与你期望的sxyz（Rx*Ry*Rz）不符；
• 对EigeneulerAngles函数的旋转顺序逻辑理解偏差。

Eigen的eulerAngles(a,b,c)函数返回的欧拉角ea满足：

旋转矩阵 = AngleAxisd(ea[0], 轴a) * AngleAxisd(ea[1], 轴b) * AngleAxisd(ea[2], 轴c)

这里的执行顺序是从右到左：先绕轴c旋转ea[2]，再绕轴b旋转ea[1]，最后绕轴a旋转ea[0]，正好对应你需要的**sxyz（固定轴X-Y-Z）**旋转顺序（即Rx(ax)*Ry(ay)*Rz(az)）。

但你构造Quaternion时用了Az * Ay * Ax，Quaternion乘法的执行顺序是右边的变换先执行，这对应的旋转矩阵是Rz(az)*Ry(ay)*Rx(ax)（先绕X转ax，再绕Y转ay，最后绕Z转az），和你期望的sxyz顺序完全相反，因此提取的欧拉角无法还原原始矩阵。

修正后的代码 #

#include <iostream>
#include <eigen3/Eigen/Dense>

int main() {
    // 初始化欧拉角：sxyz顺序，即先绕Z转az，再绕Y转ay，最后绕X转ax
    double ax = M_PI / 2;
    double ay = M_PI / 2;
    double az = 0;

    // 构造Quaternion，顺序为Ax * Ay * Az（对应Rx*Ry*Rz的旋转矩阵）
    Eigen::Quaterniond q = Eigen::AngleAxisd(ax, Eigen::Vector3d::UnitX()) *
                Eigen::AngleAxisd(ay, Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(az, Eigen::Vector3d::UnitZ());
    
    Eigen::Matrix3d mat = q.toRotationMatrix();

    // 打印原始旋转矩阵
    std::cout << "原始旋转矩阵：" << std::endl;
    std::cout << mat << std::endl;
    std::cout << "----" << std::endl;

    // 提取sxyz顺序的欧拉角（对应eulerAngles(0,1,2)，0=X,1=Y,2=Z）
    Eigen::Vector3d ea = mat.eulerAngles(0, 1, 2);

    // 提取的欧拉角
    std::cout << "提取的欧拉角（ax, ay, az）：" << std::endl;
    std::cout << ea << std::endl;
    std::cout << "----" << std::endl;
    
    // 用提取的欧拉角重新生成旋转矩阵
    Eigen::Quaterniond q2 = Eigen::AngleAxisd(ea(0), Eigen::Vector3d::UnitX()) *
                Eigen::AngleAxisd(ea(1), Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(ea(2), Eigen::Vector3d::UnitZ());
    
    Eigen::Matrix3d mat2 = q2.toRotationMatrix();

    // 打印重新生成的旋转矩阵
    std::cout << "还原的旋转矩阵：" << std::endl;
    std::cout << mat2 << std::endl;
    std::cout << "----" << std::endl;

    // 验证两个矩阵是否相等（考虑浮点误差）
    std::cout << "矩阵是否相等（浮点误差范围内）：" << std::endl;
    std::cout << (mat.isApprox(mat2) ? "是" : "否") << std::endl;
}

关键说明 #

1. 旋转顺序修正：将Quaternion的构造顺序改为Ax * Ay * Az，确保生成的旋转矩阵符合sxyz（Rx*Ry*Rz）的要求；
2. 万向锁处理：当俯仰角ay接近±π/2时，会触发万向锁，此时欧拉角的表示会有歧义，但Eigen返回的结果依然能生成等价的旋转矩阵（浮点误差范围内）；
3. 浮点误差验证：使用isApprox方法验证矩阵是否相等，避免直接比较浮点数。

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