旋转向量_旋转矩阵_四元数_欧拉角
0.引言
经常查，总结一下。推导看原论文。
1.旋转矩阵与旋转向量
1.1.旋转向量转旋转矩阵(指数映射）
旋转向量(角速度乘以
Δ
t
\Delta t
Δ
t
：
v
=
ω
Δ
t
=
ϕ
u
\mathbf{v}=\boldsymbol{\omega} \Delta t=\phi \mathbf{u}
v
=
ω
Δ
t
=
ϕ
u
旋转向量转旋转矩阵，指数映射：
Exp
(
v
)
≜
exp
(
[
v
]
×
)
\operatorname{Exp}(\mathbf{v}) \triangleq \exp \left([\mathbf{v}]_{\times}\right)
E
x
p
(
v
)
≜
exp
(
[
v
]
×
)
即是：
R
=
e
ϕ
[
u
]
×
=
I
+
ϕ
[
u
]
×
+
1
2
ϕ
2
[
u
]
×
2
+
1
3
!
ϕ
3
[
u
]
×
3
+
1
4
!
ϕ
4
[
u
]
×
4
+
…
\mathbf{R}=e^{\phi[\mathbf{u}]_{\times}}=\mathbf{I}+\phi[\mathbf{u}]_{\times}+\frac{1}{2} \phi^{2}[\mathbf{u}]_{\times}^{2}+\frac{1}{3 !} \phi^{3}[\mathbf{u}]_{\times}^{3}+\frac{1}{4 !} \phi^{4}[\mathbf{u}]_{\times}^{4}+\ldots
R
=
e
ϕ
[
u
]
×
=
I
+
ϕ
[
u
]
×
+
2
1
ϕ
2
[
u
]
×
2
+
3
!
1
ϕ
3
[
u
]
×
3
+
4
!
1
ϕ
4
[
u
]
×
4
+
…
整理得（罗德里格斯公式）：
R
=
I
cos
ϕ
+
[
u
]
×
sin
ϕ
+
u
u
⊤
(
1
−
cos
ϕ
)
\mathbf{R}=\mathbf{I} \cos \phi+[\mathbf{u}]_{\times} \sin \phi+\mathbf{u u}^{\top}(1-\cos \phi)
R
=
I
cos
ϕ
+
[
u
]
×
sin
ϕ
+
u
u
⊤
(
1
−
cos
ϕ
)
1.2.旋转矩阵转旋转向量(对数映射)
log
:
S
O
(
3
)
→
s
o
(
3
)
;
R
↦
log
(
R
)
=
[
u
ϕ
]
×
\log : S O(3) \rightarrow \mathfrak{s o}(3) ; \mathbf{R} \mapsto \log (\mathbf{R})=[\mathbf{u} \phi]_{\times}
lo
g
:
S
O
(
3
)
→
s
o
(
3
)
;
R
↦
lo
g
(
R
)
=
[
u
ϕ
]
×
log
(
R
)
≜
(
log
(
R
)
)
∨
\log (\mathbf{R}) \triangleq(\log (\mathbf{R}))^{\vee}
lo
g
(
R
)
≜
(
lo
g
(
R
)
)
∨
ϕ
=
arccos
(
trace
(
R
)
−
1
2
)
\phi=\arccos \left(\frac{\operatorname{trace}(\mathbf{R})-1}{2}\right)
ϕ
=
arccos
(
2
t
r
a
c
e
(
R
)
−
1
)
u
=
(
R
−
R
⊤
)
∨
2
sin
ϕ
\mathbf{u}=\frac{\left(\mathbf{R}-\mathbf{R}^{\top}\right)^{\vee}}{2 \sin \phi}
u
=
2
sin
ϕ
(
R
−
R
⊤
)
∨
2.四元数与旋转向量
2.1.旋转向量转四元数(指数映射)
旋转向量(角速度乘以
Δ
t
\Delta t
Δ
t
：
v
=
ω
Δ
t
=
ϕ
u
\mathbf{v}=\boldsymbol{\omega} \Delta t=\phi \mathbf{u}
v
=
ω
Δ
t
=
ϕ
u
指数运算(为啥要除以2参考原论文推导)：
Exp
(
v
)
≜
exp
(
v
/
2
)
\operatorname{Exp}(\mathbf{v}) \triangleq \exp (\mathbf{v} / 2)
E
x
p
(
v
)
≜
exp
(
v
/
2
)
q
≜
Exp
(
ϕ
u
)
=
e
ϕ
u
/
2
=
cos
ϕ
2
+
u
sin
ϕ
2
=
[
cos
(
ϕ
/
2
)
u
sin
(
ϕ
/
2
)
]
\mathbf{q} \triangleq \operatorname{Exp}(\phi \mathbf{u})=e^{\phi \mathbf{u} / 2}=\cos \frac{\phi}{2}+\mathbf{u} \sin \frac{\phi}{2}=\left[\begin{array}{c} \cos (\phi / 2) \\ \mathbf{u} \sin (\phi / 2) \end{array}\right]
q
≜
E
x
p
(
ϕ
u
)
=
e
ϕ
u
/
2
=
cos
2
ϕ
+
u
sin
2
ϕ
=
[
cos
(
ϕ
/
2
)
u
sin
(
ϕ
/
2
)
]
即是：
q
=
[
cos
ϕ
2
,
u
x
sin
ϕ
2
,
u
y
sin
ϕ
2
,
u
z
sin
ϕ
2
]
T
q=\left[\cos \frac{\phi}{2}, u_{x} \sin \frac{\phi}{2}, u_{y} \sin \frac{\phi}{2}, u_{z} \sin \frac{\phi}{2}\right]^{T}
q
=
[
cos
2
ϕ
,
u
x
sin
2
ϕ
,
u
y
sin
2
ϕ
,
u
z
sin
2
ϕ
]
T
2.2.四元数转旋转向量(对数映射)
log
:
S
3
→
H
p
;
q
↦
log
(
q
)
=
u
θ
\log : S^{3} \rightarrow \mathbb{H}_{p} ; \mathbf{q} \mapsto \log (\mathbf{q})=\mathbf{u} \theta
lo
g
:
S
3
→
H
p
;
q
↦
lo
g
(
q
)
=
u
θ
log
(
q
)
≜
2
log
(
q
)
\log (\mathbf{q}) \triangleq 2 \log (\mathbf{q})
lo
g
(
q
)
≜
2
lo
g
(
q
)
ϕ
=
2
arctan
(
∥
q
v
∥
,
q
w
)
u
=
q
v
/
∥
q
v
∥
\begin{aligned} \phi &=2 \arctan \left(\left\|\mathbf{q}_{v}\right\|, q_{w}\right) \\ \mathbf{u} &=\mathbf{q}_{v} /\left\|\mathbf{q}_{v}\right\| \end{aligned}
ϕ
u
=
2
arctan
(
∥
q
v
∥
,
q
w
)
=
q
v
/
∥
q
v
∥
3.四元数与旋转矩阵
q
⊗
x
⊗
q
∗
=
R
x
\mathbf{q} \otimes \mathbf{x} \otimes \mathbf{q}^{*}=\mathbf{R} \mathbf{x}
q
⊗
x
⊗
q
∗
=
R
x
3.1.四元数转旋转矩阵
q
⊗
x
⊗
q
∗
=
[
q
∗
]
R
[
q
]
L
[
0
x
]
=
[
0
R
x
]
\mathbf{q} \otimes \mathbf{x} \otimes \mathbf{q}^{*}={\left[\mathbf{q}^{*}\right]_{R}[\mathbf{q}]_{L}}\left[\begin{array}{l} 0 \\ \mathbf{x} \end{array}\right]=\left[\begin{array}{c} 0 \\ \mathbf{R} \mathbf{x} \end{array}\right]
q
⊗
x
⊗
q
∗
=
[
q
∗
]
R
[
q
]
L
[
0
x
]
=
[
0
R
x
]
R
=
[
1
−
2
q
2
2
−
2
q
3
2
2
q
1
q
2
+
2
q
0
q
3
2
q
1
q
3
−
2
q
0
q
2
2
q
1
q
2
−
2
q
0
q
3
1
−
2
q
1
2
−
2
q
3
2
2
q
2
q
3
+
2
q
0
q
1
2
q
1
q
3
+
2
q
0
q
2
2
q
2
q
3
−
2
q
0
q
1
1
−
2
q
1
2
−
2
q
2
2
]
\boldsymbol{R}=\left[\begin{array}{ccc} 1-2 q_{2}^{2}-2 q_{3}^{2} & 2 q_{1} q_{2}+2 q_{0} q_{3} & 2 q_{1} q_{3}-2 q_{0} q_{2} \\ 2 q_{1} q_{2}-2 q_{0} q_{3} & 1-2 q_{1}^{2}-2 q_{3}^{2} & 2 q_{2} q_{3}+2 q_{0} q_{1} \\ 2 q_{1} q_{3}+2 q_{0} q_{2} & 2 q_{2} q_{3}-2 q_{0} q_{1} & 1-2 q_{1}^{2}-2 q_{2}^{2} \end{array}\right]
R
=
⎣
⎡
1
−
2
q
2
2
−
2
q
3
2
2
q
1
q
2
−
2
q
0
q
3
2
q
1
q
3
+
2
q
0
q
2
2
q
1
q
2
+
2
q
0
q
3
1
−
2
q
1
2
−
2
q
3
2
2
q
2
q
3
−
2
q
0
q
1
2
q
1
q
3
−
2
q
0
q
2
2
q
2
q
3
+
2
q
0
q
1
1
−
2
q
1
2
−
2
q
2
2
⎦
⎤
R
=
(
q
w
2
−
(
q
v
⊤
q
)
)
I
+
2
q
v
q
v
⊤
+
2
q
w
[
q
v
]
×
\mathbf{R}=\left(q_{w}^{2}-\left(\mathbf{q}_{v}^{\top} \mathbf{q}_{\mathbf{}}\right)\right) \mathbf{I}+2 \mathbf{q}_{v} \mathbf{q}_{v}^{\top}+2 q_{w}\left[\mathbf{q}_{v}\right]_{\times}
R
=
(
q
w
2
−
(
q
v
⊤
q
)
)
I
+
2
q
v
q
v
⊤
+
2
q
w
[
q
v
]
×
3.2.旋转矩阵转四元数
q
0
=
tr
(
R
)
+
1
2
,
q
1
=
m
23
−
m
32
4
q
0
,
q
2
=
m
31
−
m
13
4
q
0
,
q
3
=
m
12
−
m
21
4
q
0
q_{0}=\frac{\sqrt{\operatorname{tr}(R)+1}}{2}, q_{1}=\frac{m_{23}-m_{32}}{4 q_{0}}, q_{2}=\frac{m_{31}-m_{13}}{4 q_{0}}, q_{3}=\frac{m_{12}-m_{21}}{4 q_{0}}
q
0
=
2
t
r
(
R
)
+
1
,
q
1
=
4
q
0
m
2
3
−
m
3
2
,
q
2
=
4
q
0
m
3
1
−
m
1
3
,
q
3
=
4
q
0
m
1
2
−
m
2
1
4.欧拉角与旋转矩阵
4.1.欧拉角转旋转矩阵
某次旋转绕固定坐标轴X-Y-Z旋转(γ,β,α)或者说绕自身坐标轴Z-Y-X旋转(α,β,γ)，对应的旋转矩阵如下
R
=
R
Z
(
α
)
R
Y
(
β
)
R
X
(
γ
)
=
(
c
o
s
α
c
o
s
β
c
o
s
α
s
i
n
β
s
i
n
γ
−
s
i
n
α
c
o
s
γ
c
o
s
α
s
i
n
β
c
o
s
γ
+
s
i
n
α
s
i
n
γ
s
i
n
α
c
o
s
β
c
o
s
α
c
o
s
γ
+
s
i
n
α
s
i
n
β
s
i
n
γ
s
i
n
α
s
i
n
β
c
o
s
γ
−
s
i
n
γ
c
o
s
α
−
s
i
n
β
c
o
s
β
s
i
n
γ
c
o
s
β
c
o
s
γ
)
R = R_{Z}(\alpha)R_{Y}(\beta)R_{X}(\gamma)=\begin{pmatrix} cos\alpha cos\beta & cos\alpha sin\beta sin\gamma – sin\alpha cos\gamma & cos\alpha sin\beta cos\gamma + sin\alpha sin\gamma\\ sin\alpha cos\beta & cos\alpha cos\gamma + sin\alpha sin\beta sin\gamma & sin\alpha sin\beta cos\gamma – sin\gamma cos\alpha\\ -sin\beta & cos\beta sin\gamma & cos\beta cos\gamma \end{pmatrix}
R
=
R
Z
(
α
)
R
Y
(
β
)
R
X
(
γ
)
=
⎝
⎛
c
o
s
α
c
o
s
β
s
i
n
α
c
o
s
β
−
s
i
n
β
c
o
s
α
s
i
n
β
s
i
n
γ
−
s
i
n
α
c
o
s
γ
c
o
s
α
c
o
s
γ
+
s
i
n
α
s
i
n
β
s
i
n
γ
c
o
s
β
s
i
n
γ
c
o
s
α
s
i
n
β
c
o
s
γ
+
s
i
n
α
s
i
n
γ
s
i
n
α
s
i
n
β
c
o
s
γ
−
s
i
n
γ
c
o
s
α
c
o
s
β
c
o
s
γ
⎠
⎞
4.2.旋转矩阵转欧拉角
有旋转矩阵
R
=
(
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
)
R=\begin{pmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{pmatrix}
R
=
⎝
⎛
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
⎠
⎞
可求出各轴的旋转角为
θ
Z
=
a
t
a
n
2
(
r
21
,
r
11
)
θ
Y
=
a
t
a
n
2
(
−
r
31
,
r
31
2
+
r
33
2
)
θ
X
=
a
t
a
n
2
(
r
32
,
r
33
)
\begin{matrix} \theta_{Z}= atan2(r_{21},r_{11})\\ \theta_{Y}= atan2(-r_{31},\sqrt{r_{31}^{2}+r_{33}^{2}})\\ \theta_{X}= atan2(r_{32},r_{33}) \end{matrix}
θ
Z
=
a
t
a
n
2
(
r
2
1
,
r
1
1
)
θ
Y
=
a
t
a
n
2
(
−
r
3
1
,
r
3
1
2
+
r
3
3
2
)
θ
X
=
a
t
a
n
2
(
r
3
2
,
r
3
3
)
atan2为C++中函数，atan2(y,x)的做法：当 x 的绝对值比 y 的绝对值大时使用 atan(y/x)；反之使用 atan(x/y)。这样就保证了数值稳定性。需要注意的是，旋转顺序必须要是Z、Y、X。
5.欧拉角与四元数
5.1.欧拉角转四元素
将Z-Y-X欧拉角（或RPY角：绕固定坐标系的X-Y-Z依次旋转α,β,γ角）转换为四元数：
q
=
(
q
0
q
1
q
2
q
3
)
=
(
c
o
s
(
α
2
)
c
o
s
(
β
2
)
c
o
s
(
γ
2
)
+
s
i
n
(
α
2
)
s
i
n
(
β
2
)
s
i
n
(
γ
2
)
s
i
n
(
α
2
)
c
o
s
(
β
2
)
c
o
s
(
γ
2
)
−
c
o
s
(
α
2
)
s
i
n
(
β
2
)
s
i
n
(
γ
2
)
c
o
s
(
α
2
)
s
i
n
(
β
2
)
c
o
s
(
γ
2
)
+
s
i
n
(
α
2
)
c
o
s
(
β
2
)
s
i
n
(
γ
2
)
c
o
s
(
α
2
)
c
o
s
(
β
2
)
s
i
n
(
γ
2
)
−
s
i
n
(
α
2
)
s
i
n
(
β
2
)
c
o
s
(
γ
2
)
)
\textbf{q}=\begin{pmatrix} q_{0}\\ q_{1}\\ q_{2}\\ q_{3} \end{pmatrix}=\begin{pmatrix} cos(\frac{\alpha}{2})cos(\frac{\beta}{2})cos(\frac{\gamma}{2})+sin(\frac{\alpha}{2})sin(\frac{\beta}{2})sin(\frac{\gamma}{2})\\ sin(\frac{\alpha}{2})cos(\frac{\beta}{2})cos(\frac{\gamma}{2})-cos(\frac{\alpha}{2})sin(\frac{\beta}{2})sin(\frac{\gamma}{2})\\ cos(\frac{\alpha}{2})sin(\frac{\beta}{2})cos(\frac{\gamma}{2})+sin(\frac{\alpha}{2})cos(\frac{\beta}{2})sin(\frac{\gamma}{2})\\ cos(\frac{\alpha}{2})cos(\frac{\beta}{2})sin(\frac{\gamma}{2})-sin(\frac{\alpha}{2})sin(\frac{\beta}{2})cos(\frac{\gamma}{2}) \end{pmatrix}
q
=
⎝
⎜
⎜
⎛
q
0
q
1
q
2
q
3
⎠
⎟
⎟
⎞
=
⎝
⎜
⎜
⎛
c
o
s
(
2
α
)
c
o
s
(
2
β
)
c
o
s
(
2
γ
)
+
s
i
n
(
2
α
)
s
i
n
(
2
β
)
s
i
n
(
2
γ
)
s
i
n
(
2
α
)
c
o
s
(
2
β
)
c
o
s
(
2
γ
)
−
c
o
s
(
2
α
)
s
i
n
(
2
β
)
s
i
n
(
2
γ
)
c
o
s
(
2
α
)
s
i
n
(
2
β
)
c
o
s
(
2
γ
)
+
s
i
n
(
2
α
)
c
o
s
(
2
β
)
s
i
n
(
2
γ
)
c
o
s
(
2
α
)
c
o
s
(
2
β
)
s
i
n
(
2
γ
)
−
s
i
n
(
2
α
)
s
i
n
(
2
β
)
c
o
s
(
2
γ
)
⎠
⎟
⎟
⎞
5.2.四元数转欧拉角
由四元数
q
=
(
q
0
,
q
1
,
q
2
,
q
3
)
q=(q0,q1,q2,q3)
q
=
(
q
0
,
q
1
,
q
2
,
q
3
)
或
q
=
(
w
,
x
,
y
,
z
)
q=(w,x,y,z)
q
=
(
w
,
x
,
y
,
z
)
到欧拉角的转换为
α
=
a
r
c
t
a
n
(
2
(
q
0
q
1
+
q
2
q
3
)
1
−
2
(
q
1
2
+
q
2
2
)
)
β
=
a
r
c
s
i
n
(
2
(
q
0
q
2
−
q
1
q
3
)
)
γ
=
a
r
c
t
a
n
(
2
(
q
0
q
3
+
q
1
q
2
)
1
−
2
(
q
2
2
+
q
3
2
)
)
\alpha = arctan(\frac{2(q_{0}q_{1}+q_{2}q_{3})}{1-2(q_{1}^{2}+q_{2}^{2})})\\ \beta = arcsin(2(q_{0}q_{2}-q_{1}q_{3}))\\ \gamma = arctan(\frac{2(q_{0}q_{3}+q_{1}q_{2})}{1-2(q_{2}^{2}+q_{3}^{2})})
α
=
a
r
c
t
a
n
(
1
−
2
(
q
1
2
+
q
2
2
)
2
(
q
0
q
1
+
q
2
q
3
)
)
β
=
a
r
c
s
i
n
(
2
(
q
0
q
2
−
q
1
q
3
)
)
γ
=
a
r
c
t
a
n
(
1
−
2
(
q
2
2
+
q
3
2
)
2
(
q
0
q
3
+
q
1
q
2
)
)
由于arctan和arcsin的取值范围在−π/2 – π/2之间，只有180°，而绕某个轴旋转时范围是360°，因此要使用atan2函数代替arctan函数。
α
=
a
t
a
n
2
(
2
(
q
0
q
1
+
q
2
q
3
)
,
1
−
2
(
q
1
2
+
q
2
2
)
)
β
=
a
r
c
s
i
n
(
2
(
q
0
q
2
−
q
1
q
3
)
)
γ
=
a
t
a
n
2
(
2
(
q
0
q
3
+
q
1
q
2
)
,
1
−
2
(
q
2
2
+
q
3
2
)
)
\alpha = atan2(2(q_{0}q_{1}+q_{2}q_{3}),1-2(q_{1}^{2}+q_{2}^{2}))\\ \beta = arcsin(2(q_{0}q_{2}-q_{1}q_{3}))\\ \gamma = atan2(2(q_{0}q_{3}+q_{1}q_{2}),1-2(q_{2}^{2}+q_{3}^{2}))
α
=
a
t
a
n
2
(
2
(
q
0
q
1
+
q
2
q
3
)
,
1
−
2
(
q
1
2
+
q
2
2
)
)
β
=
a
r
c
s
i
n
(
2
(
q
0
q
2
−
q
1
q
3
)
)
γ
=
a
t
a
n
2
(
2
(
q
0
q
3
+
q
1
q
2
)
,
1
−
2
(
q
2
2
+
q
3
2
)
)
最后
这个网站
可以动态展示四元数与欧拉角的对应关系和变化情况，便于理解。