1 关于欧拉角

参考文章：

欧拉角与旋转矩阵的转换关系
三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换
欧拉角与旋转矩阵之间的转化公式及原理
无人机飞控算法-姿态估计-欧拉角-旋转矩阵-四元数

需要注意的是，由于欧拉角和旋转矩阵之间的转换关系跟很多因素有关，如各轴旋转角定义、欧拉角顺规、手性定义、参考坐标系等，因此讨论之前需要先明确定义。

本文讨论的内容约束如下：右手系、ZYX顺规（Z-Yaw，X-Roll，Y-Pitch）、内在旋转、主动旋转，也即参考文章【4】中的NED_Z-Y-X_Euler_Angles。

2 转换公式推导

2.1 由欧拉角构造旋转矩阵

欧拉角构造旋转矩阵直接将三个基础旋转矩阵按顺序相乘即可：
R （ Z 1 Y 2 X 3 ） = [ c 1 − s 1 0 s 1 c 1 0 0 0 1 ] [ c 2 0 s 2 0 1 0 − s 2 0 c 2 ] [ 1 0 0 0 c 3 − s 3 0 s 3 c 3 ] = [ c 1 c 2 c 1 s 2 s 3 − c 3 s 1 s 1 s 3 + c 1 c 3 s 2 c 2 s 1 c 1 c 3 + s 1 s 2 s 3 c 3 s 1 s 2 − c 1 s 3 − s 2 c 2 s 3 c 2 c 3 ] R（Z_1Y_2X_3） =\begin{bmatrix} c_1 & -s_1 & 0 \\ s_1 & c_1 & 0\\ 0 & 0 &1 \end{bmatrix} \quad \begin{bmatrix} c_2 & 0 & s_2 \\ 0 &1 &0\\ -s_2 & 0 & c_2\end{bmatrix} \quad \begin{bmatrix}1 &0 &0\\ 0 &c_3 & -s_3 \\ 0 & s_3 & c_3\end{bmatrix} \quad\\ =\begin{bmatrix}c_1c_2 &c_1s_2s_3-c_3s_1 &s_1s_3+c_1c_3s_2 \\ c_2s_1 &c_1c_3+s_1s_2s_3 & c_3s_1s_2-c_1s_3 \\ -s_2 & c_2s_3 & c_2c_3\end{bmatrix} \quad R（Z1Y2X3）=⎣⎡c1s10−s1c10001⎦⎤⎣⎡c20−s2010s20c2⎦⎤⎣⎡1000c3s30−s3c3⎦⎤=⎣⎡c1c2c2s1−s2c1s2s3−c3s1c1c3+s1s2s3c2s3s1s3+c1c3s2c3s1s2−c1s3c2c3⎦⎤

2.2 由旋转矩阵推算欧拉角

2.2.1 一般情况

从上面构造出的旋转矩阵可以很容易地推算出欧拉角：
Y轴俯仰角(pitch):
p = a r c s i n ( − R 3 , 1 ) p=arcsin(-R_{3,1}) p=arcsin(−R3,1)
X轴滚转角(roll):
r = a t a n 2 ( R 3 , 2 ， R 3 , 3 ) r=atan2(R_{3,2}，R_{3,3}) r=atan2(R3,2，R3,3)
Z轴偏航角(yaw):
y = a t a n 2 ( R 2 , 1 ， R 1 , 1 ) y=atan2(R_{2,1}，R_{1,1}) y=atan2(R2,1，R1,1)
其中

当俯仰角 p = ± 9 0 。 p=\pm 90^。 p=±90。时 c 2 = 0 , s 2 = ± 1 c_2=0,s_2=\pm 1 c2=0,s2=±1，即 R 1 , 1 = 0 ， R 2 , 1 = 0 , R 3 , 2 = 0 , R 3 , 3 = 0 R_{1,1}=0，R_{2,1}=0,R_{3,2}=0,R_{3,3}=0 R1,1=0，R2,1=0,R3,2=0,R3,3=0，旋转矩阵退化成如下形式：
R = [ 0 ± c 1 s 3 − c 3 s 1 s 1 s 3 ± c 1 c 3 0 c 1 c 3 ± s 1 s 3 ± c 3 s 1 − c 1 s 3 ± 1 0 0 ] R =\begin{bmatrix}0 &\pm c_1s_3-c_3s_1 &s_1s_3\pm c_1c_3 \\ 0 &c_1c_3\pm s_1s_3 & \pm c_3s_1-c_1s_3 \\\pm 1 & 0 & 0\end{bmatrix} \quad R=⎣⎡00±1±c1s3−c3s1c1c3±s1s30s1s3±c1c3±c3s1−c1s30⎦⎤
此时 r r r 和 y y y 的计算公式中 a r c t a n 2 ( ) arctan2() arctan2() 就失效了，即出现了Gimbal Lock。
要处理这种情况，根据情况给 r r r 或 y y y 其中一个赋值，然后再根据退化后的旋转矩阵计算另一个角度即可。

2.2.2 约束滚转角

在我的应用场景中，滚转角 r r r 始终为0，因此欧拉角退化为两个自由度，此时， s 3 = 0 , c 3 = 1 s_3=0,c_3=1 s3=0,c3=1，旋转矩阵退化为：
R = [ c 1 c 2 − s 1 c 1 s 2 c 2 s 1 c 1 s 1 s 2 − s 2 0 c 2 ] R =\begin{bmatrix}c_1c_2 &-s_1 &c_1s_2 \\ c_2s_1 &c_1 & s_1s_2 \\ -s_2 & 0 & c_2\end{bmatrix} \quad R=⎣⎡c1c2c2s1−s2−s1c10c1s2s1s2c2⎦⎤

X轴滚转角(roll):
r = 0 r=0 r=0
Y轴俯仰角(pitch):
p = a t a n 2 ( − R 3 , 1 ， R 3 , 3 ) p=atan2(-R_{3,1}，R_{3,3}) p=atan2(−R3,1，R3,3)
Z轴偏航角(yaw):
y = a t a n 2 ( − R 1 , 2 ， R 2 , 2 ) y=atan2(-R_{1,2}，R_{2,2}) y=atan2(−R1,2，R2,2)

3 转换代码（C++)

参考文章：Rotation Matrix To Euler Angles或旋转矩阵与欧拉角互转

3.1 欧拉角–>旋转矩阵

// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{// Calculate rotation about x axisMat R_x = (Mat_<double>(3,3) <<1,       0,              0,0,       cos(theta[0]),   -sin(theta[0]),0,       sin(theta[0]),   cos(theta[0]));// Calculate rotation about y axisMat R_y = (Mat_<double>(3,3) <<cos(theta[1]),    0,      sin(theta[1]),0,               1,      0,-sin(theta[1]),   0,      cos(theta[1]));// Calculate rotation about z axisMat R_z = (Mat_<double>(3,3) <<cos(theta[2]),    -sin(theta[2]),      0,sin(theta[2]),    cos(theta[2]),       0,0,               0,                  1);// Combined rotation matrixMat R = R_z * R_y * R_x;return R;
}

3.2 旋转矩阵–>欧拉角

3.2.1 一般情况

// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{Mat Rt;transpose(R, Rt);Mat shouldBeIdentity = Rt * R;Mat I = Mat::eye(3,3, shouldBeIdentity.type());return  norm(I, shouldBeIdentity) < 1e-6;
}// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{assert(isRotationMatrix(R));float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );bool singular = sy < 1e-6; // Iffloat x, y, z;if (!singular){x = atan2(R.at<double>(2,1) , R.at<double>(2,2));y = atan2(-R.at<double>(2,0), sy);z = atan2(R.at<double>(1,0), R.at<double>(0,0));}else//由于欧拉角存在奇异性，当pitch为90°时会出现解不稳定的情况因此需要进行特殊处理{x = atan2(-R.at<double>(1,2), R.at<double>(1,1));y = atan2(-R.at<double>(2,0), sy);z = 0;}return Vec3f(x, y, z);}

3.2.2 约束滚转自由度

// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{Mat Rt;transpose(R, Rt);Mat shouldBeIdentity = Rt * R;Mat I = Mat::eye(3,3, shouldBeIdentity.type());return  norm(I, shouldBeIdentity) < 1e-6;
}Vec3f rotationMatrixToEulerAngles_RollEquel0(Mat &R)
{assert(isRotationMatrix(R));double x,y,z;x = 0;y = atan2(-R.at<double>(2,0), R.at<double>(2,2));z = atan2(-R.at<double>(0,1), R.at<double>(1,1));return Vec3f(x, y, z);
}

旋转矩阵转欧拉角（二自由度约束）相关推荐

【ANSYS命令流】加载与求解技术（二）：关键点、线、面及其他自由度约束相关命令
目录关键点自由度约束举个栗子对线施加自由度约束举个栗子对面施加自由度约束约束转换命令自由度约束命令冲突关键点自由度约束文中涉及的其他命令请参照上一篇博客节点自由度约束在关键点.线. ...
旋转矩阵、欧拉角、万向锁详解
旋转矩阵.欧拉角.万向锁详解一:物体的姿态描述物体的姿态可由固定于此物体的坐标系描述,为了规定空间某刚体B的姿态,设置一直角坐标系{B}与此刚体固接.用坐标系{B}的三个单位主矢量{Xb,Yb,Z ...
旋转矩阵、欧拉角、四元数、轴/角之间的转换
在机器人学中,表示旋转的有四种方式.不同的人可能习惯于用不同的方法,现将四种方式之间的转换整理出来如下. 旋转矩阵旋转矩阵R表示坐标系`O-x'y'z'`中的向量坐标变换为同一向量在坐标系`O-xy ...
旋转矩阵与欧拉角的相互转换及代码
这篇博客将会分享旋转矩阵和欧拉角的相互转换. 三维旋转矩阵有三个自由度,旋转能够使用多种方法表示(旋转矩阵,欧拉角,四元数,轴角,李群与李代数),比如一个3x3的矩阵,比如四元数,甚至可以将旋转表示为 ...
旋转矩阵、欧拉角、四元数理论及其转换关系
旋转矩阵.欧拉角.四元数理论及其转换关系 author@jason_ql(lql0716) http://blog.csdn.net/lql0716 1.概述旋转矩阵.欧拉角.四元数主要用于表示坐标 ...
3D 中的方位与角位移(旋转矩阵、欧拉角、四元数)
文章目录一.3D 中的方位与角位移 1. 欧拉角 (Euler angles) 2. 四元数的相关知识 2.1 复数 2.2 欧拉旋转定理 2.3 三维空间旋转的拆分 3. 四元数 (Quatern ...
matlab 欧拉角方向余弦,旋转矩阵、欧拉角之间转换
学习过程中涉及欧拉角和旋转矩阵的转换,索性整理学习一下欧拉角四元数和旋转矩阵的概念以及matlab中的互相转换本文摘自各大课本,博客,自己学习整理使用,侵删 MATLAB矩阵乘法从左到右依次相乘用 ...
运用C#在VS2017的PictureBox控件中绘制简易二自由度机械臂，并且让机械臂实现画直线、圆、人物轮廓及写字的功能。
运用C#在VS2017的PictureBox控件中绘制简易二自由度机械臂,并且让机械臂实现画直线.圆.人物轮廓及写字的功能. 给大家看看效果吧演示写字视频在下: VID 首先放置了诸多控件在给控件 ...
旋转矩阵、欧拉角、轴角
1. 适用于RxRyRz顺序的旋转矩阵与欧拉角变换关系 1.1. 基本旋转矩阵 1.2. 欧拉角->旋转矩阵 1.3. 旋转矩阵->欧拉角 Eigen自带的转换函数会出现欧拉角跳变的现象, ...

旋转矩阵转欧拉角（二自由度约束）

旋转矩阵转欧拉角