欧拉角与旋转矩阵的互相转换

绕坐标轴旋转

刚体绕X，Y，Z轴旋转θ角的公式

欧拉角的定义方式有两种，第一种是三个旋转均是相对于原始坐标系进行的(以后我们称这种欧拉角为世界系欧拉角)，第二种是三个旋转均是以前一次旋转得到的新坐标系为基础继续旋转(以后我们称这种欧拉角为刚体系欧拉角)。

刚体系欧拉角

eg:如下描述的就是ZYX欧拉角，旋转过程如下图所示：

旋转矩阵为：

其余12种欧拉角坐标系的定义为：

世界系欧拉角

世界系欧拉角也是三个角度，只不过对应的三次变换都是绕着世界坐标系旋转。RPY是roll（滚转），pitch（俯仰），yaw（偏航）的合写，分别代表了绕世界系 x，y，z三个轴的旋转。

其旋转矩阵为：

注： 1.三次绕固定轴旋转的最终姿态和以相反顺序三次绕运动坐标轴旋转的最终姿态相同；

所以R0=R1，可以说所有欧拉角只有12组（原本24组）。

12种固定角坐标系的定义由下式给出：

代码实现

旋转矩阵转欧拉角

std::vector<int> _NEXT_AXIS = { 1, 2, 0, 1 };
double _EPS4 = std::numeric_limits<double>::epsilon() * 4.0;

std::map<std::string, std::tuple<int, int, int, int>> _AXES2TUPLE = {
    {"sxyz", std::make_tuple(0, 0, 0, 0)}, {"sxyx", std::make_tuple(0, 0, 1, 0)},
    {"sxzy", std::make_tuple(0, 1, 0, 0)}, {"sxzx", std::make_tuple(0, 1, 1, 0)},
    {"syzx", std::make_tuple(1, 0, 0, 0)}, {"syzy", std::make_tuple(1, 0, 1, 0)},
    {"syxz", std::make_tuple(1, 1, 0, 0)}, {"syxy", std::make_tuple(1, 1, 1, 0)},
    {"szxy", std::make_tuple(2, 0, 0, 0)}, {"szxz", std::make_tuple(2, 0, 1, 0)},
    {"szyx", std::make_tuple(2, 1, 0, 0)}, {"szyz", std::make_tuple(2, 1, 1, 0)},
    {"rzyx", std::make_tuple(0, 0, 0, 1)}, {"rxyx", std::make_tuple(0, 0, 1, 1)},
    {"ryzx", std::make_tuple(0, 1, 0, 1)}, {"rxzx", std::make_tuple(0, 1, 1, 1)},
    {"rxzy", std::make_tuple(1, 0, 0, 1)}, {"ryzy", std::make_tuple(1, 0, 1, 1)},
    {"rzxy", std::make_tuple(1, 1, 0, 1)}, {"ryxy", std::make_tuple(1, 1, 1, 1)},
    {"ryxz", std::make_tuple(2, 0, 0, 1)}, {"rzxz", std::make_tuple(2, 0, 1, 1)},
    {"rxyz", std::make_tuple(2, 1, 0, 1)}, {"rzyz", std::make_tuple(2, 1, 1, 1)}
};

std::tuple<double, double, double> mat2euler(const Eigen::Matrix3d& mat, const std::string& axes) 
{
    char firstaxis = std::get<0>(_AXES2TUPLE[axes]);
    char parity = std::get<1>(_AXES2TUPLE[axes]);
    char repetition = std::get<2>(_AXES2TUPLE[axes]);
    char frame = std::get<3>(_AXES2TUPLE[axes]);

    int i = firstaxis;
    int j = _NEXT_AXIS[i + parity];
    int k = _NEXT_AXIS[i - parity + 1];

    Eigen::Matrix3d M = mat;
    if (repetition) {
        double sy = std::sqrt(M(i, j) * M(i, j) + M(i, k) * M(i, k));
        double ax, ay, az;
        if (sy > _EPS4) {
            ax = std::atan2(M(i, j), M(i, k));
            ay = std::atan2(sy, M(i, i));
            az = std::atan2(M(j, i), -M(k, i));
        }
        else {
            ax = std::atan2(-M(j, k), M(j, j));
            ay = std::atan2(sy, M(i, i));
            az = 0.0;
        }
        if (parity) {
            ax = -ax;
            ay = -ay;
            az = -az;
        }
        if (frame) {
            std::swap(ax, az);
        }
        return std::make_tuple(ax, ay, az);
    }
    else {
        double cy = std::sqrt(M(i, i) * M(i, i) + M(j, i) * M(j, i));
        double ax, ay, az;
        if (cy > _EPS4) {
            ax = std::atan2(M(k, j), M(k, k));
            ay = std::atan2(-M(k, i), cy);
            az = std::atan2(M(j, i), M(i, i));
        }
        else {
            ax = std::atan2(-M(j, k), M(j, j));
            ay = std::atan2(-M(k, i), cy);
            az = 0.0;
        }
        if (parity) {
            ax = -ax;
            ay = -ay;
            az = -az;
        }
        if (frame) {
            std::swap(ax, az);
        }
        return std::make_tuple(ax, ay, az);
    }
}

欧拉角转旋转矩阵

Matrix3d eulerToRotationMatrix(double ai, double aj, double ak, const std::string& axes)
{
    char firstaxis = std::get<0>(_AXES2TUPLE[axes]);
    char parity = std::get<1>(_AXES2TUPLE[axes]);
    char repetition = std::get<2>(_AXES2TUPLE[axes]);
    char frame = std::get<3>(_AXES2TUPLE[axes]);

    int i = firstaxis;
    int j = _NEXT_AXIS[i + parity];
    int k = _NEXT_AXIS[i - parity + 1];

    if (frame)
    {
        std::swap(ai, ak);
    }
    if (parity)
    {
        ai = -ai;
        aj = -aj;
        ak = -ak;
    }

    double si = sin(ai), sj = sin(aj), sk = sin(ak);
    double ci = cos(ai), cj = cos(aj), ck = cos(ak);
    double cc = ci * ck, cs = ci * sk;
    double sc = si * ck, ss = si * sk;

    Matrix3d M = Matrix3d::Identity();
    if (repetition)
    {
        M(i, i) = cj;
        M(i, j) = sj * si;
        M(i, k) = sj * ci;
        M(j, i) = sj * sk;
        M(j, j) = -cj * ss + cc;
        M(j, k) = -cj * cs - sc;
        M(k, i) = -sj * ck;
        M(k, j) = cj * sc + cs;
        M(k, k) = cj * cc - ss;
    }
    else
    {
        M(i, i) = cj * ck;
        M(i, j) = sj * sc - cs;
        M(i, k) = sj * cc + ss;
        M(j, i) = cj * sk;
        M(j, j) = sj * ss + cc;
        M(j, k) = sj * cs - sc;
        M(k, i) = -sj;
        M(k, j) = cj * si;
        M(k, k) = cj * ci;
    }
    return M;
}

参考

机器人学——姿态描述方法（欧拉角，固定角，D-H法，绕定轴旋转）
机器人正运动学—姿态描述之欧拉角