从旋转矩阵计算欧拉角
从旋转矩阵中找到所有可能的欧拉角的简单方法,在计算机图形学、视觉学、机器人学和运动学中,欧拉角的确定有时是必要的一步。然而,解决方案可能是明显的,也可能不是。
旋转矩阵
我们从三个主要轴的旋转的标准定义开始。
绕x轴的弧度旋转
ψ
\psi
ψ 被定义为:
R
x
(
ψ
)
=
[
1
0
0
0
cos
ψ
−
sin
ψ
0
sin
ψ
cos
ψ
]
R_{x}(\psi)=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \psi & -\sin \psi \\ 0 & \sin \psi & \cos \psi \end{array}\right]
R x ( ψ ) = ⎣ ⎡ 1 0 0 0 cos ψ sin ψ 0 − sin ψ cos ψ ⎦ ⎤ 类似地,绕y轴旋转的弧度定义为
R
x
(
ψ
)
=
[
c
o
s
θ
0
s
i
n
θ
0
1
0
−
s
i
n
θ
0
c
o
s
θ
]
R_{x}(\psi)=\left[\begin{array}{ccc} cos\theta & 0 & sin\theta \\ 0 & 1 & 0 \\ -sin\theta & 0 & cos\theta \end{array}\right]
R x ( ψ ) = ⎣ ⎡ c o s θ 0 − s i n θ 0 1 0 s i n θ 0 c o s θ ⎦ ⎤ 最后,定义了沿z轴旋转的弧度为
R
z
(
ϕ
)
=
[
cos
ϕ
−
sin
ϕ
0
sin
ϕ
cos
ϕ
0
0
0
1
]
R_{z}(\phi)=\left[\begin{array}{ccc} \cos \phi & -\sin \phi & 0 \\ \sin \phi & \cos \phi & 0 \\ 0 & 0 & 1 \end{array}\right]
R z ( ϕ ) = ⎣ ⎡ cos ϕ sin ϕ 0 − sin ϕ cos ϕ 0 0 0 1 ⎦ ⎤ 这三个角
ψ
,
θ
,
ϕ
\psi,\theta, \phi
ψ , θ , ϕ 是欧拉角
广义旋转矩阵
一般的旋转矩阵可以有这样的形式
R
z
(
ϕ
)
=
[
R
11
R
12
R
13
R
21
R
22
R
23
R
31
R
32
R
33
]
R_{z}(\phi)=\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]
R z ( ϕ ) = ⎣ ⎡ R 1 1 R 2 1 R 3 1 R 1 2 R 2 2 R 3 2 R 1 3 R 2 3 R 3 3 ⎦ ⎤ 这个矩阵可以被认为是一个三旋转的序列,每个主轴各一个。因为矩阵乘法不能交换,旋转的轴的顺序将影响结果。在这个分析中,我们先绕
x
x
x 轴旋转,然后绕
y
y
y 轴旋转,最后绕
z
z
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{aligned} R &=R_{z}(\phi) R_{y}(\theta) R_{x}(\psi) \\ &=\left[\begin{array}{ccc} \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{array}\right] \end{aligned}
R = R z ( ϕ ) R y ( θ ) R x ( ψ ) = ⎣ ⎡ 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(\psi)
R z ( ϕ ) , R y θ , R x ( ψ ) 中的相应元素等价,可以计算出欧拉角
ψ
,
θ
,
ϕ
\psi,\theta, \phi
ψ , θ , ϕ 。九个方程,可以用来找到欧拉角。
求出两个可能的角度
θ
\theta
θ
从
R
31
R_{31}
R 3 1 开始,我们发现
R
31
=
−
s
i
n
θ
R_{31}=-sin\theta
R 3 1 = − s i n θ 这个方程可以倒过来表示
θ
=
−
s
i
n
−
1
(
R
31
)
\theta=-sin^{-1}(R_{31})
θ = − s i n − 1 ( R 3 1 ) 然而,在解释这个等式时必须谨慎。由于$sin(\pi-\theta)=sin(\theta),实际上有两个不同的值(对于
R
31
≠
±
1
R_{31} \not=\pm1
R 3 1 = ± 1 )满足方程,因此,这两个值
θ
1
=
−
s
i
n
−
1
(
R
31
)
\theta{1}=-sin^{-1}(R_{31})
θ 1 = − s i n − 1 ( R 3 1 )
θ
2
=
π
−
θ
1
=
π
+
s
i
n
−
1
(
R
31
)
\theta_{2}=\pi-\theta_1=\pi+sin^{-1}(R_{31})
θ 2 = π − θ 1 = π + s i n − 1 ( R 3 1 ) 都是有效的解。我们将在后面处理
R
31
=
±
1
R_{31} =\pm1
R 3 1 = ± 1 的特殊情况,因此使用旋转矩阵的R_{31}元素,我们能够确定两个可能的值。
找到
ψ
\psi
ψ 对应的角度
想要找到
ψ
\psi
ψ 的值,我们观察这一点
R
32
R
33
=
t
a
n
(
ψ
)
\frac{R_{32}}{R_{33}}=tan(\psi)
R 3 3 R 3 2 = t a n ( ψ ) 我们用这个方程来解出
ψ
=
a
t
a
n
2
(
R
32
,
R
33
)
\psi=atan2(R_{32},R_{33})
ψ = a t a n 2 ( R 3 2 , R 3 3 ) 其中
a
t
a
n
2
(
y
,
x
)
atan2(y, x)
a t a n 2 ( y , x ) 是两个变量
x
x
x 和
y
y
y 的
a
r
c
t
a
n
arctan
a r c t a n ,类似于计算
y
x
\frac{y}{x}
x y 的
a
r
c
t
a
n
arctan
a r c t a n ,只是用两个参数的符号来确定结果的象限,其范围为
[
−
π
,
π
]
[-\pi,\pi]
[ − π , π ] ,函数
a
t
a
n
2
atan2
a t a n 2 在许多编程语言中都可用。
在解释方程
2
2
2 时必须小心,如果
c
o
s
(
θ
)
>
0
cos(\theta)>0
c o s ( θ ) > 0 ,那么
ψ
=
a
t
a
n
2
(
R
32
,
R
33
)
\psi=atan2(R_{32},R_{33})
ψ = a t a n 2 ( R 3 2 , R 3 3 ) 。然而,当
c
o
s
(
θ
)
<
0
cos(\theta)<0
c o s ( θ ) < 0 ,
ψ
=
a
t
a
n
2
(
−
R
32
,
−
R
33
)
\psi=atan2(-R_{32},-R_{33})
ψ = a t a n 2 ( − R 3 2 , − R 3 3 ) 。处理这个问题的一个简单方法是使用这个方程
ψ
=
a
t
a
n
2
(
R
32
c
o
s
θ
,
R
33
c
o
s
θ
)
\psi=atan2(\frac{R_{32}}{cos\theta},\frac{R_{33}}{cos\theta})
ψ = a t a n 2 ( c o s θ R 3 2 , c o s θ R 3 3 ) 去计算
ψ
\psi
ψ 。
方程
3
3
3 对除
c
o
s
θ
=
0
cos\theta = 0
c o s θ = 0 之外的所有情况都有效。
ψ
=
a
t
a
n
2
(
R
32
c
o
s
θ
1
,
R
33
c
o
s
θ
1
)
\psi=atan2(\frac{R_{32}}{cos\theta_{1}},\frac{R_{33}}{cos\theta_{1}})
ψ = a t a n 2 ( c o s θ 1 R 3 2 , c o s θ 1 R 3 3 )
ψ
=
a
t
a
n
2
(
R
32
c
o
s
θ
2
,
R
33
c
o
s
θ
2
)
\psi=atan2(\frac{R_{32}}{cos\theta_{2}},\frac{R_{33}}{cos\theta_{2}})
ψ = a t a n 2 ( c o s θ 2 R 3 2 , c o s θ 2 R 3 3 )
求出
ϕ
\phi
ϕ 对应的角度
类似的分析也适用于寻找。我们观察到
R
21
R
11
=
t
a
n
ϕ
\frac{R_{21}}{R_{11}}=tan\phi
R 1 1 R 2 1 = t a n ϕ 我们用这个方程解出了
ϕ
\phi
ϕ
ψ
=
a
t
a
n
2
(
R
21
c
o
s
θ
1
,
R
11
c
o
s
θ
1
)
\psi=atan2(\frac{R_{21}}{cos\theta_{1}},\frac{R_{11}}{cos\theta_{1}})
ψ = a t a n 2 ( c o s θ 1 R 2 1 , c o s θ 1 R 1 1 )
ψ
=
a
t
a
n
2
(
R
21
c
o
s
θ
2
,
R
11
c
o
s
θ
2
)
\psi=atan2(\frac{R_{21}}{cos\theta_{2}},\frac{R_{11}}{cos\theta_{2}})
ψ = a t a n 2 ( c o s θ 2 R 2 1 , c o s θ 2 R 1 1 )
c
o
s
θ
≠
0
cos\theta\not=0
c o s θ = 0 时的两个解
对于
c
o
s
θ
≠
0
cos\theta\not=0
c o s θ = 0 的情况,我们现在有两个三个一组的欧拉角再现了旋转矩阵,为
ψ
1
,
θ
1
,
ϕ
1
\psi_1,\theta_1,\phi_1
ψ 1 , θ 1 , ϕ 1
ψ
2
,
θ
2
,
ϕ
2
\psi_2,\theta_2,\phi_2
ψ 2 , θ 2 , ϕ 2 这两个解都是有效的。
扫描二维码关注公众号,回复:
11586304 查看本文章
如果
c
o
s
θ
=
0
cos\theta=0
c o s θ = 0 呢?
如果旋转矩阵的
R
31
R_{31}
R 3 1 元素为
1
1
1 或
−
1
−1
− 1 ,对应的
θ
=
−
π
2
\theta=-\frac{\pi}{2}
θ = − 2 π or
θ
=
π
2
\theta=\frac{\pi}{2}
θ = 2 π ,
c
o
s
θ
=
0
cos\theta=0
c o s θ = 0 上述就不起作用。当我们尝试使用上述技术来解决可能的值
ψ
,
ϕ
\psi,\phi
ψ , ϕ 问题发生了,因为
R
11
,
R
21
,
R
32
和
R
33
R_{11},R_{21},R_{32}和R_{33}
R 1 1 , R 2 1 , R 3 2 和 R 3 3 可能值为
0
0
0 ,因此
ψ
,
ϕ
\psi,\phi
ψ , ϕ 变为
ψ
=
a
t
a
n
2
(
0
0
,
0
0
)
\psi=atan2(\frac{0}{0},\frac{0}{0})
ψ = a t a n 2 ( 0 0 , 0 0 )
ϕ
=
a
t
a
n
2
(
0
0
,
0
0
)
\phi=atan2(\frac{0}{0},\frac{0}{0})
ϕ = a t a n 2 ( 0 0 , 0 0 ) 这种情况下,
R
11
、
R
21
、
R
32
R11、R21、R32
R 1 1 、 R 2 1 、 R 3 2 和
R
33
R33
R 3 3 没有约束
ψ
,
ϕ
\psi,\phi
ψ , ϕ 这些值。因此,我们必须利用旋转矩阵的不同元素来计算
ψ
,
ϕ
\psi,\phi
ψ , ϕ 。
θ
=
π
2
\theta=\frac{\pi}{2}
θ = 2 π :
R
12
=
sin
ψ
cos
ϕ
−
cos
ψ
sin
ϕ
=
sin
(
ψ
−
ϕ
)
R
13
=
cos
ψ
cos
ϕ
+
sin
ψ
sin
ϕ
=
cos
(
ψ
−
ϕ
)
R
22
=
sin
ψ
sin
ϕ
+
cos
ψ
cos
ϕ
=
cos
(
ψ
−
ϕ
)
=
R
13
R
23
=
cos
ψ
sin
ϕ
−
sin
ψ
cos
ϕ
=
−
sin
(
ψ
−
ϕ
)
=
−
R
12
\begin{array}{l} R_{12}=\sin \psi \cos \phi-\cos \psi \sin \phi=\sin (\psi-\phi) \\ R_{13}=\cos \psi \cos \phi+\sin \psi \sin \phi=\cos (\psi-\phi) \\ R_{22}=\sin \psi \sin \phi+\cos \psi \cos \phi=\cos (\psi-\phi)=R_{13} \\ R_{23}=\cos \psi \sin \phi-\sin \psi \cos \phi=-\sin (\psi-\phi)=-R_{12} \end{array}
R 1 2 = sin ψ cos ϕ − cos ψ sin ϕ = sin ( ψ − ϕ ) R 1 3 = cos ψ cos ϕ + sin ψ sin ϕ = cos ( ψ − ϕ ) R 2 2 = sin ψ sin ϕ + cos ψ cos ϕ = cos ( ψ − ϕ ) = R 1 3 R 2 3 = cos ψ sin ϕ − sin ψ cos ϕ = − sin ( ψ − ϕ ) = − R 1 2 任意的
ψ
\psi
ψ 和
ϕ
\phi
ϕ 都满足方程。我们会发现
(
ψ
−
ϕ
)
=
a
t
a
n
2
(
R
12
,
R
13
)
(\psi-\phi)=atan2(R_{12},R_{13})
( ψ − ϕ ) = a t a n 2 ( R 1 2 , R 1 3 )
ψ
=
ϕ
+
a
t
a
n
2
(
R
12
,
R
13
)
\psi=\phi+atan2(R_{12},R_{13})
ψ = ϕ + a t a n 2 ( R 1 2 , R 1 3 )
θ
=
−
π
2
\theta=-\frac{\pi}{2}
θ = − 2 π :
R
12
=
−
sin
ψ
cos
ϕ
−
cos
ψ
sin
ϕ
=
−
sin
(
ψ
+
ϕ
)
R
13
=
−
cos
ψ
cos
ϕ
+
sin
ψ
sin
ϕ
=
−
cos
(
ψ
+
ϕ
)
R
22
=
−
sin
ψ
sin
ϕ
+
cos
ψ
cos
ϕ
=
cos
(
ψ
+
ϕ
)
=
−
R
13
R
23
=
−
cos
ψ
sin
ϕ
−
sin
ψ
cos
ϕ
=
−
sin
(
ψ
+
ϕ
)
=
R
12
\begin{array}{l} R_{12}=-\sin \psi \cos \phi-\cos \psi \sin \phi=-\sin (\psi+\phi) \\ R_{13}=-\cos \psi \cos \phi+\sin \psi \sin \phi=-\cos (\psi+\phi) \\ R_{22}=-\sin \psi \sin \phi+\cos \psi \cos \phi=\cos (\psi+\phi)=-R_{13} \\ R_{23}=-\cos \psi \sin \phi-\sin \psi \cos \phi=-\sin (\psi+\phi)=R_{12} \end{array}
R 1 2 = − sin ψ cos ϕ − cos ψ sin ϕ = − sin ( ψ + ϕ ) R 1 3 = − cos ψ cos ϕ + sin ψ sin ϕ = − cos ( ψ + ϕ ) R 2 2 = − sin ψ sin ϕ + cos ψ cos ϕ = cos ( ψ + ϕ ) = − R 1 3 R 2 3 = − cos ψ sin ϕ − sin ψ cos ϕ = − sin ( ψ + ϕ ) = R 1 2 同样
(
ψ
+
ϕ
)
=
a
t
a
n
2
(
−
R
12
,
−
R
13
)
(\psi+\phi)=atan2(-R_{12},-R_{13})
( ψ + ϕ ) = a t a n 2 ( − R 1 2 , − R 1 3 )
ψ
=
−
ϕ
+
a
t
a
n
2
(
−
R
12
,
−
R
13
)
\psi=-\phi+atan2(-R_{12},-R_{13})
ψ = − ϕ + a t a n 2 ( − R 1 2 , − R 1 3 )
伪代码:
if
(
R
31
≠
±
1
)
θ
1
=
−
asin
(
R
31
)
θ
2
=
π
−
θ
1
ψ
1
=
atan
2
(
R
32
cos
θ
1
,
R
33
cos
θ
1
)
ψ
2
=
atan
2
(
R
32
cos
θ
2
,
R
33
cos
θ
2
)
ϕ
1
=
atan
2
(
R
21
cos
θ
1
,
R
11
cos
θ
1
)
ϕ
2
=
atan
2
(
R
21
cos
θ
2
,
R
11
cos
θ
2
)
else
ϕ
=
anything; can set to
0
if
(
R
31
=
−
1
)
θ
=
π
/
2
ψ
=
ϕ
+
atan
2
(
R
12
,
R
13
)
else
θ
=
−
π
/
2
ψ
=
−
ϕ
+
atan
2
(
−
R
12
,
−
R
13
)
end if
end if
\begin{array}{l} \text { if }\left(R_{31} \neq\pm 1\right) \\ \theta_{1}=-\operatorname{asin}\left(R_{31}\right) \\ \theta_{2}=\pi-\theta_{1} \\ \psi_{1}=\operatorname{atan} 2\left(\frac{R_{32}}{\cos \theta_{1}}, \frac{R_{33}}{\cos \theta_{1}}\right) \\ \psi_{2}=\operatorname{atan} 2\left(\frac{R_{32}}{\cos \theta_{2}}, \frac{R_{33}}{\cos \theta_{2}}\right) \\ \phi_{1}=\operatorname{atan} 2\left(\frac{R_{21}}{\cos \theta_{1}}, \frac{R_{11}}{\cos \theta_{1}}\right) \\ \phi_{2}=\operatorname{atan} 2\left(\frac{R_{21}}{\cos \theta_{2}}, \frac{R_{11}}{\cos \theta_{2}}\right) \\ \text { else } \\ \begin{array}{c} \phi=\text { anything; can set to } 0 \\ \text { if }\left(R_{31}=-1\right) \\ \theta=\pi / 2 \\ \psi=\phi+\operatorname{atan} 2\left(R_{12}, R_{13}\right) \\ \text { else } \\ \quad \quad \theta=-\pi / 2 \\ \quad \psi=-\phi+\operatorname{atan} 2\left(-R_{12},-R_{13}\right) \\ \text { end if } \end{array} \\ \text { end if } \end{array}
if ( R 3 1 = ± 1 ) θ 1 = − a s i n ( R 3 1 ) θ 2 = π − θ 1 ψ 1 = a t a n 2 ( cos θ 1 R 3 2 , cos θ 1 R 3 3 ) ψ 2 = a t a n 2 ( cos θ 2 R 3 2 , cos θ 2 R 3 3 ) ϕ 1 = a t a n 2 ( cos θ 1 R 2 1 , cos θ 1 R 1 1 ) ϕ 2 = a t a n 2 ( cos θ 2 R 2 1 , cos θ 2 R 1 1 ) else ϕ = anything; can set to 0 if ( R 3 1 = − 1 ) θ = π / 2 ψ = ϕ + a t a n 2 ( R 1 2 , R 1 3 ) else θ = − π / 2 ψ = − ϕ + a t a n 2 ( − R 1 2 , − R 1 3 ) end if end if
实际例子
下面提供了一个例子,演示了从一个旋转矩阵中计算出
ψ
,
θ
,
ϕ
\psi,\theta, \phi
ψ , θ , ϕ 。 假设要求我们找出产生这个矩阵的欧拉角
R
z
(
ϕ
)
=
[
.
5
−
.
1464
.
8536
.
5
.
8536
−
.
1464
−
.
7071
.
5
.
5
]
R_{z}(\phi)=\left[\begin{array}{ccc} .5&-.1464&.8536 \\ .5&.8536&-.1464\\ -.7071&.5&.5 \end{array}\right]
R z ( ϕ ) = ⎣ ⎡ . 5 . 5 − . 7 0 7 1 − . 1 4 6 4 . 8 5 3 6 . 5 . 8 5 3 6 − . 1 4 6 4 . 5 ⎦ ⎤ 首先,求出可能
θ
\theta
θ 的取值
θ
1
=
−
s
i
n
(
−
.
7071
)
=
π
4
\theta_1=-sin(-.7071)=\frac{\pi}{4}
θ 1 = − s i n ( − . 7 0 7 1 ) = 4 π
θ
2
=
π
−
θ
1
=
3
π
4
\theta_2=\pi-\theta_1=\frac{3\pi}{4}
θ 2 = π − θ 1 = 4 3 π 然后,我们找到相应的数值
ψ
1
=
atan
2
(
.
5
cos
(
π
/
4
)
,
.
5
cos
(
π
/
4
)
)
=
π
4
ψ
2
=
atan
2
(
.
5
cos
(
3
π
/
4
)
,
.
5
cos
(
3
π
/
4
)
)
=
−
3
π
4
\begin{array}{l} \psi_{1}=\operatorname{atan} 2\left(\frac{.5}{\cos (\pi / 4)}, \frac{.5}{\cos (\pi / 4)}\right)=\frac{\pi}{4} \\ \psi_{2}=\operatorname{atan} 2\left(\frac{.5}{\cos (3 \pi / 4)}, \frac{.5}{\cos (3 \pi / 4)}\right)=-\frac{3 \pi}{4} \end{array}
ψ 1 = a t a n 2 ( cos ( π / 4 ) . 5 , cos ( π / 4 ) . 5 ) = 4 π ψ 2 = a t a n 2 ( cos ( 3 π / 4 ) . 5 , cos ( 3 π / 4 ) . 5 ) = − 4 3 π
ϕ
1
=
atan
2
(
.
5
cos
(
π
/
4
)
,
.
5
cos
(
π
/
4
)
)
=
π
4
ϕ
2
=
atan
2
(
.
5
cos
(
3
π
/
4
)
,
.
5
cos
(
3
π
/
4
)
)
=
−
3
π
4
\begin{array}{l} \phi_{1}=\operatorname{atan} 2\left(\frac{.5}{\cos (\pi / 4)}, \frac{.5}{\cos (\pi / 4)}\right)=\frac{\pi}{4} \\ \phi_{2}=\operatorname{atan} 2\left(\frac{.5}{\cos (3 \pi / 4)}, \frac{.5}{\cos (3 \pi / 4)}\right)=-\frac{3 \pi}{4} \end{array}
ϕ 1 = a t a n 2 ( cos ( π / 4 ) . 5 , cos ( π / 4 ) . 5 ) = 4 π ϕ 2 = a t a n 2 ( cos ( 3 π / 4 ) . 5 , cos ( 3 π / 4 ) . 5 ) = − 4 3 π 因此,解为
(
π
4
,
π
4
,
π
4
)
,
(
−
3
π
4
,
3
π
4
,
−
3
π
4
)
(\frac{\pi}{4},\frac{\pi}{4},\frac{\pi}{4}),(-\frac{3\pi}{4},\frac{3\pi}{4},-\frac{3\pi}{4})
( 4 π , 4 π , 4 π ) , ( − 4 3 π , 4 3 π , − 4 3 π )