文章目录
- 摘要
- 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