欧拉角与旋转矩阵之间的相互转化（推导和Python代码）

欧拉角与旋转矩阵之间的相互转化（推导和Python代码）

表示三维空间中的旋转可以有多种表示的方法（旋转矩阵，欧拉角，四元素，轴角，李群李代数）。欧拉角表示法，分别是指定了三个角度yaw，roll，pitch，分别是绕z轴，x轴，y轴的转动的角度。

1. 欧拉角 -> 旋转矩阵

欧拉角来合成旋转矩阵的过程需要指定顺规，我们常用的顺规是$ZYX$，即分别将旋转矩阵$R_z$，$R_y$和$R_x$相乘。

参考机器人学的知识，根据下图所示，

绕$z,y,x$轴旋转角度分别为$\alpha ,\beta ,\gamma$，我们可以获得绕$z,y,x$轴的旋转矩阵分别为：
$$\begin{gathered} R_z(\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}_x(\gamma )=\left[\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right] \end{gathered}$$
然后按照$ZYX$顺规合成旋转矩阵
$$\begin{array}{rl}{R}_{ab}(\alpha ,\beta ,\gamma )& =R_z(\alpha )R_y(\beta )R_x(\gamma )\\ & =\left[\begin{array}{ccc}{c}_{\alpha }{c}_{\beta }& -s_{\alpha }{c}_{\gamma }+c_{\alpha }{s}_{\beta }{s}_{\gamma }& s_{\alpha }{s}_{\gamma }+c_{\alpha }{s}_{\beta }{c}_{\gamma }\\ s_{\alpha }{c}_{\beta }& c_{\alpha }{c}_{\gamma }+s_{\alpha }{s}_{\beta }{s}_{\gamma }& -c_{\alpha }{s}_{\gamma }+s_{\alpha }{s}_{\beta }{c}_{\gamma }\\ -s_{\beta }& c_{\beta }{s}_{\gamma }& c_{\beta }{c}_{\gamma }\end{array}\right]\\ & =\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\end{array}$$
其中，$c_{\alpha }=\cos \alpha ,s_{\beta }=\sin \beta$，别的符号以此类推。

如果已知欧拉角求旋转矩阵，这个非常好求，我们直接求出$R_z(\alpha ),R_y(\beta ),R_x(\gamma )$然后矩阵相乘即可。

Python

import math
import numpy as np
def rpy2R(rpy): # [r,p,y] 单位rad
    rot_x = np.array([[1, 0, 0],
                    [0, math.cos(rpy[0]), -math.sin(rpy[0])],
                    [0, math.sin(rpy[0]), math.cos(rpy[0])]])
    rot_y = np.array([[math.cos(rpy[1]), 0, math.sin(rpy[1])],
                    [0, 1, 0],
                    [-math.sin(rpy[1]), 0, math.cos(rpy[1])]])
    rot_z = np.array([[math.cos(rpy[2]), -math.sin(rpy[2]), 0],
                    [math.sin(rpy[2]), math.cos(rpy[2]), 0],
                    [0, 0, 1]])
    R = np.dot(rot_z, np.dot(rot_y, rot_x))
    return R

alpha = 0.25 * np.pi
beta = 0.4 * np.pi
gamma = 0.3 * np.pi
rpy = [alpha, beta, gamma]
print('rpy:', str(rpy))
print('R:', str(rpy2R(rpy)))

结果如下：

rpy: [0.7853981633974483, 1.2566370614359172, 0.9424777960769379]
R: [[ 0.18163563 -0.1767767   0.96734611]
 [ 0.25        0.95968966  0.12843579]
 [-0.95105652  0.21850801  0.21850801]]

2. 旋转矩阵 -> 欧拉角

如果已知旋转矩阵求欧拉角，就是求解如下方程的过程
$$\left[\begin{array}{ccc}{c}_{\alpha }{c}_{\beta }& -s_{\alpha }{c}_{\gamma }+c_{\alpha }{s}_{\beta }{s}_{\gamma }& s_{\alpha }{s}_{\gamma }+c_{\alpha }{s}_{\beta }{c}_{\gamma }\\ s_{\alpha }{c}_{\beta }& c_{\alpha }{c}_{\gamma }+s_{\alpha }{s}_{\beta }{s}_{\gamma }& -c_{\alpha }{s}_{\gamma }+s_{\alpha }{s}_{\beta }{c}_{\gamma }\\ -s_{\beta }& c_{\beta }{s}_{\gamma }& c_{\beta }{c}_{\gamma }\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$
整理一下可以得到

$$\{\begin{array}{rl}& -\sin \beta =r_{31}\\ & {\cos }^2\beta {\sin }^2\gamma +{\cos }^2\beta {\cos }^2\gamma =r_{32}^2+r_{33}^2\end{array}$$

所以可以得到

$$\{\begin{array}{rl}& \alpha =\arctan (r_{21}/c_{\beta },r_{11}/c_{\beta })\\ & \beta =\arctan (-r_{31},\sqrt{r_{32}^2+r_{33}^2})\\ & \gamma =\arctan (r_{32}/c_{\beta },r_{33}/c_{\beta })\end{array}$$

当$\beta =\frac{\pi }{2}$的时候，$\cos \beta =0$，这时为奇异点（singular）不能解出欧拉角，这也是欧拉角表示法的一个弊端。

Python

import math
import numpy as np
# Checks if a matrix is a valid rotation matrix.
def isRotationMatrix(R) :
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype = R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 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 ).
def rotationMatrixToEulerAngles(R) :

    assert(isRotationMatrix(R))
    
    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
    
    singular = sy < 1e-6

    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0

    return np.array([x, y, z])

R = np.array([[ 0.18163563, -0.1767767,   0.96734611],
 [ 0.25,        0.95968966,  0.12843579],
 [-0.95105652,  0.21850801,  0.21850801]])

print('R:', str(R))
print('rpy:', str(rotationMatrixToEulerAngles(R)))

结果如下

R: [[ 0.18163563 -0.1767767   0.96734611]
 [ 0.25        0.95968966  0.12843579]
 [-0.95105652  0.21850801  0.21850801]]
rpy: [0.78539816 1.25663706 0.9424778 ]

可以看到结果和我们之前转换的结果保持一致。

Reference

[1]旋转矩阵与欧拉角的相互转换及代码