旋转矩阵转欧拉角
在我的应用场景中有一个角度始终为0,添加这个约束后就不用考虑欧拉角奇异性问题。借此机会自己推导了一下公式,梳理一下欧拉角和旋转矩阵之间的变换关系。
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 axis
Mat 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 axis
Mat 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 axis
Mat 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 matrix
Mat 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; // If
float 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);
}