旋转(Rotation)矩阵转欧拉角(euler）
文章目录
- 摘要
- Rotation matrices
- Generalized rotation matrices
- Finding two possible angles for θ \theta θ
- Find the corresponding angles of ψ \psi ψ
- Finding the corresponding angles of ϕ \phi ϕ
- Two solutions if cos θ ≠ 0 \cos \theta \ne 0 cosθ=0
- What if cos θ = 0 \cos \theta=0 cosθ=0?
- Pseudo-code
- More than one solution
- reference
摘要
本文档讨论了从旋转矩阵中找所有可能欧拉角的简单技术。在计算机图形学、视觉，机器人和动力学中，有时候欧拉角的确定是必须的一步。然而，它的解可能不是那么的显而易见。
paper: Computing Euler angles from a rotation matrix
author：Gregory G. Slabaugh
Rotation matrices
我们从绕三个主轴旋转的标准定义开始。
绕x轴旋转 ψ \psi ψ弧度可定义为
R x ( ψ ) = [ 1 0 0 0 cos ψ − s i n ψ 0 sin ψ cos ψ ] R_x(\psi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \psi & -sin \psi \\ 0 & \sin \psi & \cos \psi \end{bmatrix} Rx(ψ)=⎣⎡1000cosψsinψ0−sinψcosψ⎦⎤
与此类似，绕y轴旋转 θ \theta θ弧度可定义为
R y ( θ ) = [ cos θ 0 s i n θ 0 1 0 − sin θ 0 cos θ ] R_y(\theta) = \begin{bmatrix} \cos \theta & 0 & sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end{bmatrix} Ry(θ)=⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤
最后，绕z轴旋转 ϕ \phi ϕ弧度可定义为
R z ( ϕ ) = [ cos ϕ − sin ϕ 0 sin ϕ cos ϕ 0 0 0 1 ] 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ϕ0001⎦⎤
角 ψ , θ , ϕ \psi, \theta, \phi ψ,θ,ϕ是欧拉角。
Generalized rotation matrices
旋转矩阵一般可以写成
R = [ R 11 R 12 R 13 R 21 R 22 R 23 R 31 R 32 R 33 ] R = \begin{bmatrix} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{bmatrix} R=⎣⎡R11R21R31R12R22R32R13R23R33⎦⎤
可以将此矩阵考虑为三个旋转序列，每个旋转轴绕一个主轴旋转。由于矩阵乘法不满足交换律，因此绕轴旋转的顺序会影响结果。对于此分析，我们将首先绕x轴旋转，然后绕y轴旋转，最后绕z轴旋转。 这样的旋转序列可以表示为矩阵乘积，
R = R z ( ϕ ) R y ( θ ) R x ( ψ ) = [ cos θ cos ϕ sin ψ sin θ cos ϕ − cos ψ sin ϕ cos ψ sin θ cos ϕ + sin ψ sin ϕ cos θ sin ϕ sin ψ sin θ sin ϕ + cos ψ cos ϕ cos ψ sin θ sin ϕ − sin ψ cos ϕ − sin θ sin ψ cos θ cos ψ cos θ ] \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 R每个元素与矩阵乘积 R z ( ϕ ) R y ( θ ) R x ( ϕ ) R_z(\phi)R_y(\theta)R_x(\phi) Rz(ϕ)Ry(θ)Rx(ϕ)对应元素相等计算欧拉角 ψ , θ , ϕ \psi, \theta, \phi ψ,θ,ϕ。这将得到九个方程，可用于找到欧拉角。
Finding two possible angles for θ \theta θ
由 R 31 R_{31} R31，我们得到
R 31 = − sin θ R_{31} = - \sin \theta R31=−sinθ
对等式求反得到
θ = − a s i n ( R 31 ) (1) \theta = - \mathrm{asin}(R_{31}) \tag{1} θ=−asin(R31)(1)
然而，对等式(1)解释的时候应该注意到 sin ( π − θ ) = sin θ \sin (\pi - \theta)=\sin \theta sin(π−θ)=sinθ,因此有两个不同的 θ \theta θ都满足等式(1)（备注：除了 θ = ± π / 2 \theta= \pm \pi /2 θ=±π/2(即 R 31 = ± 1 R_{31}=\pm1 R31=±1)）。因此 θ \theta θ有两个有效的解。
θ 1 = − a s i n ( R 31 ) θ 2 = π − θ 1 = π + a s i n ( R 31 ) \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 = ± 1 R_{31}= \pm 1 R31=±1的情况。因此，通过利用 R 31 R_{31} R31元素的值，我们可以确定 θ \theta θ的两个不同的值。
Find the corresponding angles of ψ \psi ψ
为了找到 ψ \psi ψ的值，我们观察到
R 32 R 33 = tan ( ψ ) \frac{R_{32}}{R_{33}} = \tan(\psi) R33R32=tan(ψ)
我们使用该等式解 ψ \psi ψ,有
ψ = a t a n 2 ( R 32 , R 33 ) (2) \psi = \mathrm{atan2}(R_{32}, R_{33}) \tag{2} ψ=atan2(R32,R33)(2)
其中 a t a n 2 ( y , x ) \mathrm{atan2}(y, x) atan2(y,x)表示变量 x , y x, y x,y的反正切，它类似于计算y / x的反正切，不同之处在于，两个自变量的符号都用于确定结果的象限，该象限在[-π，π]范围内。函数 a t a n 2 \mathrm{atan2} atan2在很多程序语言中都有实现。
在解释等式（2）时我们注意到，当 cos θ > 0 \cos \theta \gt 0 cosθ>0时， ψ = a t a n ( R 32 , R 33 ) \psi = \mathrm{atan(R_{32}, R_{33})} ψ=atan(R32,R33)，然而，当 cos ψ < 0 \cos \psi \lt 0 cosψ<0时， ψ = a t a n 2 ( − R 32 , − R 33 ) \psi = \mathrm{atan2}(-R_{32}, -R_{33}) ψ=atan2(−R32,−R33)，一个简单的解决方法是使用等式
ψ = a t a n 2 ( R 32 cos θ , R 33 cos θ ) (3) \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 θ = 0 \cos \theta = 0 cosθ=0时，随后将处理这种特殊情况。对于每一个 θ \theta θ值，我们利用等式（3）计算一个对应的 ψ \psi ψ值，得到
ψ 1 = a t a n 2 ( R 32 cos θ 1 , R 33 cos θ 1 ) (4) \psi_1 = \mathrm{atan2} \left( \frac{R_{32}}{\cos \theta_1}, \frac{R_{33}}{\cos \theta_1} \right) \tag{4} ψ1=atan2(cosθ1R32,cosθ1R33)(4)
ψ 2 = a t a n 2 ( R 32 cos θ 2 , R 33 cos θ 2 ) (5) \psi_2 = \mathrm{atan2} \left( \frac{R_{32}}{\cos \theta_2}, \frac{R_{33}}{\cos \theta_2} \right) \tag{5} ψ2=atan2(cosθ2R32,cosθ2R33)(5)
Finding the corresponding angles of ϕ \phi ϕ
类似的分析也适用于寻找 ϕ \phi ϕ,观察到
R 21 R 11 = tan ϕ \frac{R_{21}}{R_{11}} = \tan \phi R11R21=tanϕ
解 ϕ \phi ϕ使用等式
ϕ = a t a n 2 ( R 21 cos θ , R 11 cos θ ) (6) \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 θ = 0 \cos \theta = 0 cosθ=0时，随后将处理这种特殊情况。对于每一个 θ \theta θ值，我们利用等式（6）计算一个对应的 ϕ \phi ϕ值，得到
ϕ 1 = a t a n 2 ( R 21 cos θ 1 , R 11 cos θ 1 ) (7) \phi_1 = \mathrm{atan2} \left( \frac{R_{21}}{\cos \theta_1}, \frac{R_{11}}{\cos \theta_1} \right) \tag{7} ϕ1=atan2(cosθ1R21,cosθ1R11)(7)
ϕ 2 = a t a n 2 ( R 21 cos θ 2 , R 11 cos θ 2 ) (8) \phi_2 = \mathrm{atan2} \left( \frac{R_{21}}{\cos \theta_2}, \frac{R_{11}}{\cos \theta_2} \right) \tag{8} ϕ2=atan2(cosθ2R21,cosθ2R11)(8)
Two solutions if cos θ ≠ 0 \cos \theta \ne 0 cosθ=0
对于 cos θ ≠ 0 \cos \theta \ne 0 cosθ=0的情况，我们现在有两个三维(triplets)欧拉角来重现（reproduce）旋转矩阵，即
( ψ 1 , θ 1 , ϕ 1 ) ( ψ 2 , θ 2 , ϕ 2 ) (\psi_1, \theta_1, \phi_1) \\ (\psi_2, \theta_2, \phi_2) (ψ1,θ1,ϕ1)(ψ2,θ2,ϕ2)
这两个解都是有效的。
What if cos θ = 0 \cos \theta=0 cosθ=0?
如果旋转矩阵的 R 33 R_{33} R33元素为1或-1（分别对应 θ = − π / 2 \theta = - \pi/2 θ=−π/2或 θ = π / 2 \theta = \pi/2 θ=π/2且 cos θ = 0 \cos \theta = 0 cosθ=0），则上述技术无效。当我们试图使用上面的技术解 ψ , ϕ \psi, \phi ψ,ϕ时，将会出现问题，因为 R 11 , R 21 , R 32 , R 33 R_{11}, R_{21}, R_{32}, R_{33} R11,R21,R32,R33都等于0，等式（3）和（6）变成
ψ = a t a n 2 ( 0 , 0 ) ϕ = a t a n 2 ( 0 , 0 ) \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 R_{11}, R_{21}, R_{32}, R_{33} R11,R21,R32,R33不限制 ψ \psi ψ和 ϕ \phi ϕ的值。 因此，我们必须使用旋转矩阵的不同元素来计算 ψ \psi ψ和 ϕ \phi ϕ的值。
- θ = π / 2 \theta = \pi/2 θ=π/2情况：当 θ = π / 2 \theta = \pi/2 θ=π/2时有
R 12 = sin ψ sin θ cos ϕ − cos ψ sin ϕ = sin ψ cos ϕ − cos ψ sin ϕ = sin ( ψ − ϕ ) R 13 = cos ψ sin θ cos ϕ + sin ψ sin ϕ = cos ψ cos ϕ + sin ψ sin ϕ = cos ( ψ − ϕ ) R 22 = sin ψ sin θ sin ϕ + cos ψ cos ϕ = sin ψ sin ϕ + cos ψ cos ϕ = cos ( ψ − ϕ ) = R 13 R 23 = cos ψ sin θ sin ϕ − sin ψ cos ϕ = cos ψ sin ϕ − sin ψ cos ϕ = − sin ( ψ − ϕ ) = − R 12 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(ψ−ϕ)=R13R23=cosψsinθsinϕ−sinψcosϕ=cosψsinϕ−sinψcosϕ=−sin(ψ−ϕ)=−R12
满足这些等式的任何 ϕ , ψ \phi, \psi ϕ,ψ都是有效的解，利用等式 R 12 , R 13 R_{12}, R_{13} R12,R13，我们发现
( ψ − ϕ ) = a t a n 2 ( R 12 , R 13 ) ψ = ϕ + a t a n 2 ( R 12 , R 13 ) (\psi - \phi) = \mathrm{atan2}(R_{12}, R_{13}) \\ \psi = \phi + \mathrm{atan2}(R_{12}, R_{13}) (ψ−ϕ)=atan2(R12,R13)ψ=ϕ+atan2(R12,R13) - θ = − π / 2 \theta =- \pi/2 θ=−π/2情况：类似的， 有
R 12 = sin ψ sin θ cos ϕ − cos ψ sin ϕ = − sin ψ cos ϕ − cos ψ sin ϕ = − sin ( ψ + ϕ ) R 13 = cos ψ sin θ cos ϕ + sin ψ sin ϕ = − cos ψ cos ϕ + sin ψ sin ϕ = − cos ( ψ + ϕ ) R 22 = sin ψ sin θ sin ϕ + cos ψ cos ϕ = − sin ψ sin ϕ + cos ψ cos ϕ = cos ( ψ + ϕ ) = − R 13 R 23 = cos ψ sin θ sin ϕ − sin ψ cos ϕ = − cos ψ sin ϕ − sin ψ cos ϕ = − − sin ( ψ + ϕ ) = R 12 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(ψ+ϕ)=−R13R23=cosψsinθsinϕ−sinψcosϕ=−cosψsinϕ−sinψcosϕ=−−sin(ψ+ϕ)=R12
同样，有
( ψ + ϕ ) = a t a n 2 ( − R 12 , − R 13 ) ψ = − ϕ + a t a n 2 ( − R 12 , − R 13 ) (\psi + \phi) = \mathrm{atan2}(-R_{12}, -R_{13}) \\ \psi = -\phi + \mathrm{atan2}(-R_{12}, -R_{13}) (ψ+ϕ)=atan2(−R12,−R13)ψ=−ϕ+atan2(−R12,−R13)
- 无论哪种情况: 在 θ = π / 2 \theta = \pi/2 θ=π/2和 θ = − π / 2 \theta = -\pi/2 θ=−π/2的情况下，我们都发现 ψ \psi ψ和 ϕ \phi ϕ是关联的。 这种现象称为万向节锁(Gimbal lock)。尽管在这种情况下，有无数个问题的解，但在实践中，人们常常有兴趣寻找一个解。 对于此任务，如上所述方便地设置 ϕ = 0 \phi=0 ϕ=0并计算 ψ \psi ψ。
Pseudo-code
现在，我们通过图1中的伪代码实现来总结该方法。代码非常简单。
More than one solution
有趣的是，围绕三个主轴旋转的序列始终不止一种，他们都能得到物体相同的方向。 如本报告所示，在 cos θ ≠ 0 \cos \theta \ne 0 cosθ=0的非退化(non-degenerate)的情况下，有两个解。 对于 cos θ = 0 \cos \theta = 0 cosθ=0的退化情况，存在无限数量的解。
例如，考虑一本书放在您面前的桌子上。 将x轴定义为右侧，将y轴定义为远离您，将z轴定义为向上。 绕y轴旋转 π \pi π弧度将使书本旋转，使得后盖现在朝上。 实现相同方向的另一种方法是绕 x x x轴旋转书本 π \pi π弧度，然后绕z轴旋转 π \pi π弧度。 因此，存在不止一种方式来实现期望的旋转。
reference
[1]. Gregory G. Slabaugh, Computing Euler angles from a rotation matrix