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

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

在使用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. 总结

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