欧拉角和旋转矩阵之间的转换
在使用Eigen时,经常会遇到旋转矩阵,旋转向量,四元数,欧拉角之间的两两相互转换。这里最常见、最容易出错的是欧拉角和旋转矩阵之间的相互转换。下面就欧拉角和旋转矩阵之间的转换进行详细分析。欧拉角和旋转矩阵有一个弊端:会有万向节死锁的问题。
图1 旋转顺序Z-Y-X,正方向内旋
1. 欧拉角
(1)欧拉角的叫法:
欧拉角的叫法不固定,跟坐标轴的定义强相关。
在图1中,假设 X 是车头, Y是车左方, Z是车上方,那么绕X轴旋转得到的是 r o l l ,绕 Y 轴旋转得到的是 p i t c h ,绕 Z 轴得到的是 y a w 。
在图1中,假设 Y是车头, X是车右方, Z是车上方,那么绕X轴旋转得到的是 p i t c h ,绕 Y Y Y 轴旋转得到的是 r o l l ,绕 Z 轴得到的是 y a w。
(2)欧拉角正负:
如果是右手系,旋转轴正方向面对观察者时,逆时针方向的旋转是正、顺时针方向的旋转是负。
亦可这样描述:使用右手的大拇指指向旋转轴正方向,其他4个手指在握拳过程中的指向便是正方向。
如图1中的三次旋转都是正向旋转。
(3)欧拉角的范围:
这个要具体问题具体对待。
假如是车体坐标系( x -前, y-左, z-上),那么 r o l l 和 p i t c h 应该定义在(-90°,+90°), y a w 应该定义在(-180°,+180°)。
假如是飞机坐标系,那么 r o l l 、 p i t c h 和 y a w 都应该定义在(-180°,+180°)。
Eigen中的默认范围 r o l l 、 p i t c h 和 y a w 都是(-180°,+180°)。
(4)明确旋转顺序和旋转轴:
对于x,y,z三个轴的不同旋转顺序一共有( x − y − z , y − z − x , z − x − y , x − z − y , z − y − x , y − x − z )六种组合,在旋转相同的角度的情况下不同的旋转顺序得到的姿态是不一样的。
比如,先绕 x 轴旋转 α,再绕 y 轴旋转 β;先绕 y轴旋转 β ,再绕 x 轴旋转 α 。这两种顺序得到的姿态是不一样的。
(5)内旋和外旋:
每次旋转是绕固定轴(一个固定参考系,比如世界坐标系)旋转,称为外旋。
每次旋转是绕自身旋转之后的轴旋转,称为内旋。
下图说明了内旋和外旋的区别。
2. 旋转矩阵
假设绕 X 、 Y 、 Z三个轴旋转的角度分别为 α 、 β 、 γ ,则三次旋转的旋转矩阵计算方法如下:
3. 欧拉角和旋转矩阵之间的转换程序示例
下面程序测试欧拉角和旋转矩阵之间的转换:(pythons)
#欧拉角到旋转矩阵的转换
def euler_angle_to_rotate_matrix(eu, t):
theta = eu
#Calculate rotation about x axis
R_x = np.array([
[1, 0, 0],
[0, math.cos(theta[0]), -math.sin(theta[0])],
[0, math.sin(theta[0]), math.cos(theta[0])]
])
#Calculate rotation about y axis
R_y = np.array([
[math.cos(theta[1]), 0, math.sin(theta[1])],
[0, 1, 0],
[-math.sin(theta[1]), 0, math.cos(theta[1])]
])
#Calculate rotation about z axis
R_z = np.array([
[math.cos(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), math.cos(theta[2]), 0],
[0, 0, 1]])
R = np.matmul(R_x, np.matmul(R_y, R_z))
t = t.reshape([-1,1])
R = np.concatenate([R,t], axis=-1)
R = np.concatenate([R, np.array([0,0,0,1]).reshape([1,-1])], axis=0)
return R
#rotate_matrix = euler_angle_to_rotate_matrix([0,0,math.pi/2],np.array([0,0,0]))[:3,:3]
rotate_matrix = euler_angle_to_rotate_matrix([0,0,0],np.array([0,0,0]))[:3,:3]
print(rotate_matrix)
4. 总结
欧拉角和旋转矩阵之间的转换要注意的细节很多,在使用时我们一定要明确欧拉角的旋转顺序、内旋还是外旋、正向旋转还是反向旋转以及欧拉角范围。另外,文章中如果有错误的地方,还请大佬不要喷。