摘要

本文档讨论了从旋转矩阵中找所有可能欧拉角的简单技术。在计算机图形学、视觉，机器人和动力学中，有时候欧拉角的确定是必须的一步。然而，它的解可能不是那么的显而易见。
paper: Computing Euler angles from a rotation matrix
author：Gregory G. Slabaugh

Rotation matrices

我们从绕三个主轴旋转的标准定义开始。
绕x轴旋转$\psi$ψ弧度可定义为
$R_x(\psi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \psi & -sin \psi \\ 0 & \sin \psi & \cos \psi \end{bmatrix}$Rx​(ψ)=⎣⎡​100​0cosψsinψ​0−sinψcosψ​⎦⎤​
与此类似，绕y轴旋转$\theta$θ弧度可定义为
$R_y(\theta) = \begin{bmatrix} \cos \theta & 0 & sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end{bmatrix}$Ry​(θ)=⎣⎡​cosθ0−sinθ​010​sinθ0cosθ​⎦⎤​
最后，绕z轴旋转$\phi$ϕ弧度可定义为
$R_z(\phi) = \begin{bmatrix} \cos \phi & -\sin \phi & 0 \\ \sin \phi & \cos \phi & 0 \\ 0 & 0 & 1 \end{bmatrix}$Rz​(ϕ)=⎣⎡​cosϕsinϕ0​−sinϕcosϕ0​001​⎦⎤​
角$\psi, \theta, \phi$ψ,θ,ϕ是欧拉角。

Generalized rotation matrices

旋转矩阵一般可以写成
$R = \begin{bmatrix} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{bmatrix}$R=⎣⎡​R11​R21​R31​​R12​R22​R32​​R13​R23​R33​​⎦⎤​
可以将此矩阵考虑为三个旋转序列，每个旋转轴绕一个主轴旋转。由于矩阵乘法不满足交换律，因此绕轴旋转的顺序会影响结果。对于此分析，我们将首先绕x轴旋转，然后绕y轴旋转，最后绕z轴旋转。 这样的旋转序列可以表示为矩阵乘积，
$\begin{alignedat}{2} R = R_z(\phi)R_y(\theta)R_x(\psi) \\ &=\begin{bmatrix} \cos \theta \cos \phi & \sin \psi \sin \theta \cos \phi-\cos \psi \sin \phi & \cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi \\ \cos \theta \sin \phi & \sin \psi \sin \theta \sin \phi+\cos \psi \cos \phi & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi \\ -\sin \theta & \sin \psi \cos \theta & \cos \psi \cos \theta \end{bmatrix} \end{alignedat}$R=Rz​(ϕ)Ry​(θ)Rx​(ψ)​=⎣⎡​cosθcosϕcosθsinϕ−sinθ​sinψsinθcosϕ−cosψsinϕsinψsinθsinϕ+cosψcosϕsinψcosθ​cosψsinθcosϕ+sinψsinϕcosψsinθsinϕ−sinψcosϕcosψcosθ​⎦⎤​​
给定一个旋转矩阵$R$R,我们可以通过将$R$R每个元素与矩阵乘积$R_z(\phi)R_y(\theta)R_x(\phi)$Rz​(ϕ)Ry​(θ)Rx​(ϕ)对应元素相等计算欧拉角$\psi, \theta, \phi$ψ,θ,ϕ。这将得到九个方程，可用于找到欧拉角。

Finding two possible angles for $\theta$θ

由$R_{31}$R31​，我们得到
$R_{31} = - \sin \theta$R31​=−sinθ
对等式求反得到
$\theta = - \mathrm{asin}(R_{31}) \tag{1}$θ=−asin(R31​)(1)
然而，对等式(1)解释的时候应该注意到$\sin (\pi - \theta)=\sin \theta$sin(π−θ)=sinθ,因此有两个不同的$\theta$θ都满足等式(1)（备注：除了$\theta= \pm \pi /2$θ=±π/2(即$R_{31}=\pm1$R31​=±1)）。因此$\theta$θ有两个有效的解。
$\begin{aligned} \theta_1 &= - \mathrm{asin}(R_{31}) \\ \theta_2 &= \pi - \theta_1 = \pi + \mathrm{asin}(R_{31}) \end{aligned}$θ1​θ2​​=−asin(R31​)=π−θ1​=π+asin(R31​)​
接下来的报告中，我们将单独处理$R_{31}= \pm 1$R31​=±1的情况。因此，通过利用$R_{31}$R31​元素的值，我们可以确定$\theta$θ的两个不同的值。

Find the corresponding angles of $\psi$ψ

为了找到$\psi$ψ的值，我们观察到
$\frac{R_{32}}{R_{33}} = \tan(\psi)$R33​R32​​=tan(ψ)
我们使用该等式解$\psi$ψ,有
$\psi = \mathrm{atan2}(R_{32}, R_{33}) \tag{2}$ψ=atan2(R32​,R33​)(2)
其中$\mathrm{atan2}(y, x)$atan2(y,x)表示变量$x, y$x,y的反正切，它类似于计算y / x的反正切，不同之处在于，两个自变量的符号都用于确定结果的象限，该象限在[-π，π]范围内。函数$\mathrm{atan2}$atan2在很多程序语言中都有实现。
在解释等式（2）时我们注意到，当$\cos \theta \gt 0$cosθ>0时， $\psi = \mathrm{atan(R_{32}, R_{33})}$ψ=atan(R32​,R33​)，然而，当$\cos \psi \lt 0$cosψ<0时， $\psi = \mathrm{atan2}(-R_{32}, -R_{33})$ψ=atan2(−R32​,−R33​)，一个简单的解决方法是使用等式
$\psi = \mathrm{atan2}\left(\frac{R_{32}}{\cos \theta}, \frac{R_{33}}{\cos \theta} \right) \tag{3}$ψ=atan2(cosθR32​​,cosθR33​​)(3)
计算$\psi$ψ。
等式3对于所有的情况都适用，除了当$\cos \theta = 0$cosθ=0时，随后将处理这种特殊情况。对于每一个$\theta$θ值，我们利用等式（3）计算一个对应的$\psi$ψ值，得到
$\psi_1 = \mathrm{atan2} \left( \frac{R_{32}}{\cos \theta_1}, \frac{R_{33}}{\cos \theta_1} \right) \tag{4}$ψ1​=atan2(cosθ1​R32​​,cosθ1​R33​​)(4)
$\psi_2 = \mathrm{atan2} \left( \frac{R_{32}}{\cos \theta_2}, \frac{R_{33}}{\cos \theta_2} \right) \tag{5}$ψ2​=atan2(cosθ2​R32​​,cosθ2​R33​​)(5)

Finding the corresponding angles of $\phi$ϕ

类似的分析也适用于寻找$\phi$ϕ,观察到
$\frac{R_{21}}{R_{11}} = \tan \phi$R11​R21​​=tanϕ
解$\phi$ϕ使用等式
$\phi = \mathrm{atan2}\left( \frac{R_{21}}{\cos \theta}, \frac{R_{11}}{\cos \theta}\right) \tag{6}$ϕ=atan2(cosθR21​​,cosθR11​​)(6)
同样，等式6对于所有的情况都适用，除了当$\cos \theta = 0$cosθ=0时，随后将处理这种特殊情况。对于每一个$\theta$θ值，我们利用等式（6）计算一个对应的$\phi$ϕ值，得到
$\phi_1 = \mathrm{atan2} \left( \frac{R_{21}}{\cos \theta_1}, \frac{R_{11}}{\cos \theta_1} \right) \tag{7}$ϕ1​=atan2(cosθ1​R21​​,cosθ1​R11​​)(7)
$\phi_2 = \mathrm{atan2} \left( \frac{R_{21}}{\cos \theta_2}, \frac{R_{11}}{\cos \theta_2} \right) \tag{8}$ϕ2​=atan2(cosθ2​R21​​,cosθ2​R11​​)(8)

Two solutions if $\cos \theta \ne 0$cosθ​=0

对于$\cos \theta \ne 0$cosθ​=0的情况，我们现在有两个三维(triplets)欧拉角来重现（reproduce）旋转矩阵，即
$(\psi_1, \theta_1, \phi_1) \\ (\psi_2, \theta_2, \phi_2)$(ψ1​,θ1​,ϕ1​)(ψ2​,θ2​,ϕ2​)
这两个解都是有效的。

What if $\cos \theta=0$cosθ=0?

如果旋转矩阵的$R_{33}$R33​元素为1或-1（分别对应$\theta = - \pi/2$θ=−π/2或$\theta = \pi/2$θ=π/2且$\cos \theta = 0$cosθ=0），则上述技术无效。当我们试图使用上面的技术解$\psi, \phi$ψ,ϕ时，将会出现问题，因为$R_{11}, R_{21}, R_{32}, R_{33}$R11​,R21​,R32​,R33​都等于0，等式（3）和（6）变成
$\psi = \mathrm{atan2} \left(0, 0 \right) \\ \phi = \mathrm{atan2} \left(0, 0 \right)$ψ=atan2(0,0)ϕ=atan2(0,0)
在这种情况下，$R_{11}, R_{21}, R_{32}, R_{33}$R11​,R21​,R32​,R33​不限制$\psi$ψ和$\phi$ϕ的值。 因此，我们必须使用旋转矩阵的不同元素来计算$\psi$ψ和$\phi$ϕ的值。

• $\theta = \pi/2$θ=π/2情况：当$\theta = \pi/2$θ=π/2时有
  $R_{12} = \sin \psi \sin \theta \cos \phi-\cos \psi \sin \phi = \sin \psi \cos \phi-\cos \psi \sin \phi =\sin(\psi - \phi) \\ R_{13}=\cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi = \cos \psi \cos \phi+\sin \psi \sin \phi=\cos (\psi - \phi) \\ R_{22} = \sin \psi \sin \theta \sin \phi+\cos \psi \cos \phi= \sin \psi \sin \phi+\cos \psi \cos \phi = \cos(\psi - \phi) = R_{13} \\ R_{23} = \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi = \cos \psi \sin \phi-\sin \psi \cos \phi = - \sin (\psi - \phi) = -R_{12}$R12​=sinψsinθcosϕ−cosψsinϕ=sinψcosϕ−cosψsinϕ=sin(ψ−ϕ)R13​=cosψsinθcosϕ+sinψsinϕ=cosψcosϕ+sinψsinϕ=cos(ψ−ϕ)R22​=sinψsinθsinϕ+cosψcosϕ=sinψsinϕ+cosψcosϕ=cos(ψ−ϕ)=R13​R23​=cosψsinθsinϕ−sinψcosϕ=cosψsinϕ−sinψcosϕ=−sin(ψ−ϕ)=−R12​
  满足这些等式的任何$\phi, \psi$ϕ,ψ都是有效的解，利用等式$R_{12}, R_{13}$R12​,R13​，我们发现
  $(\psi - \phi) = \mathrm{atan2}(R_{12}, R_{13}) \\ \psi = \phi + \mathrm{atan2}(R_{12}, R_{13})$(ψ−ϕ)=atan2(R12​,R13​)ψ=ϕ+atan2(R12​,R13​)
• $\theta =- \pi/2$θ=−π/2情况：类似的， 有
  $R_{12} = \sin \psi \sin \theta \cos \phi-\cos \psi \sin \phi = -\sin \psi \cos \phi-\cos \psi \sin \phi =-\sin(\psi + \phi) \\ R_{13}=\cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi = -\cos \psi \cos \phi+\sin \psi \sin \phi=-\cos (\psi +\phi) \\ R_{22} = \sin \psi \sin \theta \sin \phi+\cos \psi \cos \phi= -\sin \psi \sin \phi+\cos \psi \cos \phi = \cos(\psi + \phi) = -R_{13} \\ R_{23} = \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi = - \cos \psi \sin \phi-\sin \psi \cos \phi = - -\sin (\psi + \phi) = R_{12}$R12​=sinψsinθcosϕ−cosψsinϕ=−sinψcosϕ−cosψsinϕ=−sin(ψ+ϕ)R13​=cosψsinθcosϕ+sinψsinϕ=−cosψcosϕ+sinψsinϕ=−cos(ψ+ϕ)R22​=sinψsinθsinϕ+cosψcosϕ=−sinψsinϕ+cosψcosϕ=cos(ψ+ϕ)=−R13​R23​=cosψsinθsinϕ−sinψcosϕ=−cosψsinϕ−sinψcosϕ=−−sin(ψ+ϕ)=R12​
  同样，有
  $(\psi + \phi) = \mathrm{atan2}(-R_{12}, -R_{13}) \\ \psi = -\phi + \mathrm{atan2}(-R_{12}, -R_{13})$(ψ+ϕ)=atan2(−R12​,−R13​)ψ=−ϕ+atan2(−R12​,−R13​)

• 无论哪种情况: 在$\theta = \pi/2$θ=π/2和$\theta = -\pi/2$θ=−π/2的情况下，我们都发现$\psi$ψ和$\phi$ϕ是关联的。 这种现象称为万向节锁(Gimbal lock)。尽管在这种情况下，有无数个问题的解，但在实践中，人们常常有兴趣寻找一个解。 对于此任务，如上所述方便地设置$\phi=0$ϕ=0并计算$\psi$ψ。

Pseudo-code

现在，我们通过图1中的伪代码实现来总结该方法。代码非常简单。

More than one solution

有趣的是，围绕三个主轴旋转的序列始终不止一种，他们都能得到物体相同的方向。 如本报告所示，在$\cos \theta \ne 0$cosθ​=0的非退化(non-degenerate)的情况下，有两个解。 对于$\cos \theta = 0$cosθ=0的退化情况，存在无限数量的解。

例如，考虑一本书放在您面前的桌子上。 将x轴定义为右侧，将y轴定义为远离您，将z轴定义为向上。 绕y轴旋转$\pi$π弧度将使书本旋转，使得后盖现在朝上。 实现相同方向的另一种方法是绕$x$x轴旋转书本$\pi$π弧度，然后绕z轴旋转$\pi$π弧度。 因此，存在不止一种方式来实现期望的旋转。

reference

[1]. Gregory G. Slabaugh, Computing Euler angles from a rotation matrix