欧拉角和旋转矩阵之间的转换

在使用Eigen时，经常会遇到旋转矩阵，旋转向量，四元数，欧拉角之间的两两相互转换。这里最常见、最容易出错的是欧拉角和旋转矩阵之间的相互转换。下面就欧拉角和旋转矩阵之间的转换进行详细分析。

图1 旋转顺序Z-Y-X，正方向内旋

1. 欧拉角

（1）欧拉角的叫法：

欧拉角的叫法不固定，跟坐标轴的定义强相关。
在图1中，假设XXX是车头，YYY是车左方，ZZZ是车上方，那么绕X轴旋转得到的是 rollrollroll，绕 YYY 轴旋转得到的是pitchpitchpitch，绕ZZZ轴得到的是yawyawyaw。
在图1中，假设YYY是车头，XXX是车右方，ZZZ是车上方，那么绕X轴旋转得到的是 pitchpitchpitch，绕 YYY 轴旋转得到的是 rollrollroll，绕ZZZ轴得到的是yawyawyaw。

（2）欧拉角正负：

如果是右手系，旋转轴正方向面对观察者时，逆时针方向的旋转是正、顺时针方向的旋转是负。
亦可这样描述：使用右手的大拇指指向旋转轴正方向，其他4个手指在握拳过程中的指向便是正方向。
如图1中的三次旋转都是正向旋转。

（3）欧拉角的范围：

这个要具体问题具体对待。
假如是车体坐标系（xxx-前，yyy-左，zzz-上），那么 rollrollroll 和 pitchpitchpitch 应该定义在（-90°，+90°），yawyawyaw 应该定义在（-180°，+180°）。
假如是飞机坐标系，那么 rollrollroll、pitchpitchpitch 和 yawyawyaw 都应该定义在（-180°，+180°）。
Eigen中的默认范围 rollrollroll、pitchpitchpitch 和 yawyawyaw都是（-180°，+180°）。

（4）明确旋转顺序和旋转轴：

对于x,y,z三个轴的不同旋转顺序一共有(x−y−z,y−z−x,z−x−y,x−z−y,z−y−x,y−x−zx-y-z, y-z-x,z-x-y,x-z-y, z-y-x,y-x-zx−y−z,y−z−x,z−x−y,x−z−y,z−y−x,y−x−z)六种组合，在旋转相同的角度的情况下不同的旋转顺序得到的姿态是不一样的。
比如，先绕xxx轴旋转α\alphaα，再绕yyy轴旋转β\betaβ；先绕yyy轴旋转β\betaβ，再绕xxx轴旋转α\alphaα。这两种顺序得到的姿态是不一样的。

（5）内旋和外旋：

每次旋转是绕固定轴（一个固定参考系，比如世界坐标系）旋转，称为外旋。
每次旋转是绕自身旋转之后的轴旋转，称为内旋。
下图说明了内旋和外旋的区别。

2. 旋转矩阵

假设绕X、Y、ZX、Y、ZX、Y、Z三个轴旋转的角度分别为 α、β、γ\alpha、\beta、\gammaα、β、γ，则三次旋转的旋转矩阵计算方法如下：

$R_{x}(\alpha)=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \alpha & -\sin \alpha \\ 0 & \sin \alpha & \cos \alpha \end{array}\right]$

$R_{y}(\beta)=\left[\begin{array}{ccc} \cos \beta & 0 & \sin \beta \\ 0 & 1 & 0 \\ -\sin \beta & 0 & \cos \beta \end{array}\right]$

$R_{z}(\gamma)=\left[\begin{array}{ccc} \cos \gamma & -\sin \gamma & 0 \\ \sin \gamma & \cos \gamma & \\ 0 & 0 & 1 \end{array}\right]$

按照内旋方式，Z-Y-X旋转顺序（指先绕自身轴Z，再绕自身轴Y，最后绕自身轴X），可得旋转矩阵（内旋是右乘）：

$R1 = R_{z}(\gamma)* R_{y}(\beta)* R_{x}(\alpha )$

按照外旋方式，X-Y-Z旋转顺序（指先绕固定轴X，再绕固定轴Y，最后绕固定轴Z），可得旋转矩阵（外旋是左乘）：

$R2 = R_{z}(\gamma)* R_{y}(\beta)* R_{x}(\alpha )$

故R1=R2，具体不在此证明，记住即可。这个结论说明ZYX顺序的内旋等价于XYZ顺序的外旋。

slam十四讲中提到的常用旋转顺序是Z-Y-X，对应rpy，指的就是内旋（绕自身轴）Z-Y-X顺序。而欧拉角转换成旋转矩阵（相对于世界坐标系的旋转矩阵）通常是按外旋方式（绕固定轴），即X-Y-Z顺序，所以旋转矩阵为：

$R = R2 = R_{z}(\gamma)* R_{y}(\beta)* R_{x}(\alpha )$

3. 欧拉角和旋转矩阵之间的转换程序示例

下面程序分别使用Eigen和自定义函数测试欧拉角和旋转矩阵之间的转换：

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>using namespace std;Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta);
bool isRotationMatirx(Eigen::Matrix3d R);
Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R);const double ARC_TO_DEG = 57.29577951308238;
const double DEG_TO_ARC = 0.0174532925199433;int main()
{// 设定车体欧拉角(角度)，绕固定轴double roll_deg = 0.5;      // 绕X轴double pitch_deg = 0.8;     // 绕Y轴double yaw_deg = 108.5;     // 绕Z轴// 转化为弧度double roll_arc = roll_deg * DEG_TO_ARC;    // 绕X轴double pitch_arc = pitch_deg * DEG_TO_ARC;  // 绕Y轴double yaw_arc = yaw_deg * DEG_TO_ARC;      // 绕Z轴cout << endl;cout << "roll_arc = " << roll_arc << endl;cout << "pitch_arc = " << pitch_arc << endl;cout << "yaw_arc = " << yaw_arc << endl;// 初始化欧拉角（rpy）,对应绕x轴，绕y轴，绕z轴的旋转角度Eigen::Vector3d euler_angle(roll_arc, pitch_arc, yaw_arc);// 使用Eigen库将欧拉角转换为旋转矩阵Eigen::Matrix3d rotation_matrix1, rotation_matrix2;rotation_matrix1 = Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitX());cout << "\nrotation matrix1 =\n" << rotation_matrix1 << endl << endl;// 使用自定义函数将欧拉角转换为旋转矩阵rotation_matrix2 = eulerAnglesToRotationMatrix(euler_angle);cout << "rotation matrix2 =\n" << rotation_matrix2 << endl << endl;// 使用Eigen将旋转矩阵转换为欧拉角Eigen::Vector3d eulerAngle1 = rotation_matrix1.eulerAngles(2,1,0); // ZYX顺序，yaw,pitch,rollcout << "roll_1 pitch_1 yaw_1 = " << eulerAngle1[2] << " " << eulerAngle1[1] << " " << eulerAngle1[0] << endl << endl;// 使用自定义函数将旋转矩阵转换为欧拉角Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(rotation_matrix1); // roll,pitch,yawcout << "roll_2 pitch_2 yaw_2 = " << eulerAngle2[0] << " " << eulerAngle2[1] << " " << eulerAngle2[2] << endl << endl;return 0;
}Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta)
{Eigen::Matrix3d R_x;    // 计算旋转矩阵的X分量R_x <<1,              0,               0,0,  cos(theta[0]),  -sin(theta[0]),0,  sin(theta[0]),   cos(theta[0]);Eigen::Matrix3d R_y;    // 计算旋转矩阵的Y分量R_y <<cos(theta[1]),   0, sin(theta[1]),0,   1,             0,-sin(theta[1]),  0, cos(theta[1]);Eigen::Matrix3d R_z;    // 计算旋转矩阵的Z分量R_z <<cos(theta[2]), -sin(theta[2]), 0,sin(theta[2]),  cos(theta[2]), 0,0,              0,             1;Eigen::Matrix3d R = R_z * R_y * R_x;return R;
}bool isRotationMatirx(Eigen::Matrix3d R)
{double err=1e-6;Eigen::Matrix3d shouldIdenity;shouldIdenity=R*R.transpose();Eigen::Matrix3d I=Eigen::Matrix3d::Identity();return (shouldIdenity - I).norm() < err;
}Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R)
{assert(isRotationMatirx(R));double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));bool singular = sy < 1e-6;double x, y, z;if (!singular){x = atan2( R(2,1), R(2,2));y = atan2(-R(2,0), sy);z = atan2( R(1,0), R(0,0));}else{x = atan2(-R(1,2), R(1,1));y = atan2(-R(2,0), sy);z = 0;}return {x, y, z};
}

程序输出如下：

4. 总结

欧拉角和旋转矩阵之间的转换要注意的细节很多，在使用时我们一定要明确欧拉角的旋转顺序、内旋还是外旋、正向旋转还是反向旋转以及欧拉角范围。另外，文章中如果有错误的地方，还请大佬不要喷，在评论区指出来，我核实后会修改的，谢谢。

【自动驾驶】欧拉角和旋转矩阵之间的转换相关推荐

四元素、欧拉角及旋转矩阵之间的转换
版权声明:本文为博主原创文章,未经博主允许不得转载. https://blog.csdn.net/zhuoyueljl/article/details/70789472 查看全文 http://www ...
【四足机器人】学习笔记欧拉角与旋转矩阵之间的转化关系
[四足机器人]学习笔记欧拉角与旋转矩阵之间的转化一.基础旋转矩阵二.组合旋转矩阵在研究四足机器人站立姿态的时候,需要将通过IMU模块采集到的欧拉角数据信息转变为旋转矩阵R,关于如何将获取的欧拉 ...
rpy角与旋转矩阵之间的转换(附完整代码)
文章目录一. rpy角转换为旋转矩阵二. 旋转矩阵转换为rpy角三. 小结根据绕轴旋转的次序不同,易知姿态的rpy(roll, pitch, yaw)表示总共有12种,分别为:XYZ, X ...
ROS中四元数、欧拉角、旋转矩阵等格式转换
未完- ROS接收到odometry格式消息: nav_msgs::Odometry pos_msg 具有: pos_msg.pose.pose.orientation.x; // xyzw pos_ ...
欧拉角与旋转矩阵之间的转化公式及原理
1.背景说明首先,说明几点需要注意的问题: a.左手坐标系与右手坐标系:既然是在坐标系中进行变换,首先就要了解坐标系的类别.同样的数据在左手坐标系和右手坐标系会有不同的呈现结果.所以,所有涉及到坐标 ...
旋转矩阵与欧拉角之间的转换
简介: 对于欧拉角与旋转矩阵之间的转换公式和程序实现进行了测试.也显示了这其中的转换关系的复杂性,来自于欧拉角的方向.范围.转换顺序.这在实际应用中需要特别的关注. 关键词: 欧拉角,旋转矩阵 #m ...
计算机视觉：相机成像原理：世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换(转载)
版权声明:本文为博主原创文章,未经博主允许不得转载. https://blog.csdn.net/chentravelling/article/details/53558096 0.前言最近整理了& ...
欧拉角和旋转矩阵的学习心得
1)欧拉角正负: 如果是右手系,旋转轴正方向面对观察者时,逆时针方向的旋转是正.顺时针方向的旋转是负. 即使用右手的大拇指指向旋转轴正方向,其他4个手指在握拳过程中的指向便是正方向. 在使用时,要注意 ...
【自动驾驶】26.【很清晰】旋转矩阵，欧拉角，四元数，旋转向量和齐次变换矩阵
原文链接:https://blog.csdn.net/varyshare/article/details/91899049 前言这几个名词都是用来描述一个物体的位置和姿态的.它们之间可以相互转化,而 ...

【自动驾驶】欧拉角和旋转矩阵之间的转换