R
1
R
2
∈
S
O
(
3
)
R_1R_2 \in SO(3)
R1?R2?∈SO(3)
T
1
T
2
∈
S
E
(
3
)
T_1T_2 \in SE(3)
T1?T2?∈SE(3)
设一个三维向量
?
(
t
)
∈
R
3
\phi(t)\in \mathbb{R^3}
?(t)∈R3，有
?
(
t
)
∧
=
R
˙
(
t
)
R
(
t
)
T
\phi(t)^{\land} = \dot{R}(t)R(t)^T
?(t)∧=R˙(t)R(t)T
注意，
∧
\land
∧为矩阵化符号，得到一关于
?
\phi
?的反对称矩阵，因此该等式要求
R
˙
(
t
)
R
(
t
)
T
\dot{R}(t)R(t)^T
R˙(t)R(t)T为反对称矩阵。
三维向量
?
(
t
)
\phi(t)
?(t)有什么意义呢？我们要联系一下旋转向量的性质：
R
R
T
=
I
RR^T = I
RRT=I
R
(
t
)
R
(
t
)
T
=
I
R(t)R(t)^T = I
R(t)R(t)T=I
两边对时间求导，可以得到如下形式
R
˙
(
t
)
R
(
t
)
T
+
R
(
t
)
R
˙
(
t
)
T
=
0
\dot{R}(t)R(t)^T + R(t)\dot{R}(t)^T = 0
R˙(t)R(t)T+R(t)R˙(t)T=0
R
˙
(
t
)
R
(
t
)
T
=
?
R
(
t
)
R
˙
(
t
)
T
\dot{R}(t)R(t)^T= - R(t)\dot{R}(t)^T
R˙(t)R(t)T=?R(t)R˙(t)T
R
˙
(
t
)
R
(
t
)
T
=
?
(
R
˙
(
t
)
R
(
t
)
T
)
T
\dot{R}(t)R(t)^T= - (\dot{R}(t)R(t)^T)^T
R˙(t)R(t)T=?(R˙(t)R(t)T)T
得证
R
˙
(
t
)
R
(
t
)
T
\dot{R}(t)R(t)^T
R˙(t)R(t)T为反对称矩阵。
?
(
t
)
∧
=
R
˙
(
t
)
R
(
t
)
T
\phi(t)^{\land} = \dot{R}(t)R(t)^T
?(t)∧=R˙(t)R(t)T
两边同时右乘
R
(
t
)
R(t)
R(t)，有
?
(
t
)
∧
R
(
t
)
=
R
˙
(
t
)
R
(
t
)
T
R
(
t
)
\phi(t)^{\land}R(t) = \dot{R}(t)R(t)^TR(t)
?(t)∧R(t)=R˙(t)R(t)TR(t)
由于
R
(
t
)
T
R
(
t
)
=
I
R(t)^TR(t)=I
R(t)TR(t)=I
得到
?
(
t
)
∧
R
(
t
)
=
R
˙
(
t
)
\phi(t)^{\land}R(t) = \dot{R}(t)
?(t)∧R(t)=R˙(t)
即对
?
(
t
)
∧
\phi(t)^{\land}
?(t)∧通过右乘
R
(
t
)
R(t)
R(t)，或者对
R
(
t
)
R(t)
R(t)左乘
?
(
t
)
∧
\phi(t)^{\land}
?(t)∧，我们可以实现对旋转矩阵的求导
R
˙
(
t
)
=
?
(
t
)
∧
R
(
t
)
\dot{R}(t) =\phi(t)^{\land}R(t)
R˙(t)=?(t)∧R(t)
设
t
=
0
t=0
t=0时有
R
(
0
)
=
I
R(0)=I
R(0)=I，
t
0
=
0
t_0=0
t0?=0进行一阶泰勒展开有如下形式
R
(
t
)
≈
R
(
t
0
)
+
R
˙
(
t
0
)
(
t
?
t
0
)
=
I
+
?
(
t
0
)
∧
(
t
)
R(t) \approx R(t_0) + \dot{R}(t_0)(t-t_0) = I + \phi(t_0)^{\land}(t)
R(t)≈R(t0?)+R˙(t0?)(t?t0?)=I+?(t0?)∧(t)
又有
R
˙
(
t
)
=
?
(
t
0
)
∧
R
(
t
)
\dot{R}(t)=\phi(t_0)^{\land}R(t)
R˙(t)=?(t0?)∧R(t)
解该微分方程，可以得到四元数与旋转矩阵的关系如下所示
R
(
t
)
=
e
x
p
(
?
0
∧
t
)
R(t) = exp(\phi_0^{\land}t)
R(t)=exp(?0∧?t)
对于三维向量
?
\phi
?，我们令
?
=
θ
a
\phi=\theta a
?=θa，
θ
\theta
θ为模长，
a
a
a为单位方向向量，有以下性质：
a
∧
a
∧
=
a
a
T
?
I
a^{\land}a^{\land} = aa^T-I
a∧a∧=aaT?I
a
∧
a
∧
a
∧
=
?
a
∧
a^{\land}a^{\land}a^{\land} =-a^{\land}
a∧a∧a∧=?a∧
e
x
p
(
?
∧
)
=
e
x
p
(
θ
a
∧
)
=
c
o
s
θ
I
+
(
1
?
c
o
s
θ
)
a
a
T
+
s
i
n
θ
a
∧
exp(\phi^{\land})=exp(\theta a^{\land}) = cos\theta I + (1-cos\theta)aa^T + sin\theta a^{\land}
exp(?∧)=exp(θa∧)=cosθI+(1?cosθ)aaT+sinθa∧
反回来，旋转向量的对数映射有
?
=
l
n
(
R
)
∨
=
(
∑
n
=
0
(
?
1
)
n
n
+
1
(
R
?
I
)
n
+
1
)
∨
\phi = ln(R)^{\vee} = (\sum_{n=0}\frac{(-1)^n}{n+1}(R-I)^{n+1})^{\vee}
?=ln(R)∨=(n=0∑?n+1(?1)n?(R?I)n+1)∨
e
x
p
(
?
∧
)
=
[
R
J
ρ
0
T
1
]
=
T
exp(\epsilon^{\land})=\begin{bmatrix} R & J\rho \\ 0^T & 1 \end{bmatrix} = T
exp(?∧)=[R0T?Jρ1?]=T
其中，
J
=
s
i
n
θ
θ
I
+
(
1
?
s
i
n
θ
θ
)
a
a
T
+
1
?
c
o
s
θ
θ
a
∧
J = \frac{sin\theta}{\theta}I + (1- \frac{sin\theta}{\theta})aa^T + \frac{1-cos\theta}{\theta}a^{\land}
J=θsinθ?I+(1?θsinθ?)aaT+θ1?cosθ?a∧
?
1
\phi_1
?1?为小量，有
l
n
(
e
x
p
(
?
1
∧
)
e
x
p
(
?
2
∧
)
)
∨
≈
J
l
(
?
2
)
?
1
?
1
+
?
2
ln(exp(\phi_1^{\land})exp(\phi_2^{\land}))^{\vee} \approx J_l(\phi_2)^{-1}\phi_1 + \phi_2
ln(exp(?1∧?)exp(?2∧?))∨≈Jl?(?2?)?1?1?+?2?
?
2
\phi_2
?2?为小量，有
l
n
(
e
x
p
(
?
1
∧
)
e
x
p
(
?
2
∧
)
)
∨
≈
J
r
(
?
1
)
?
1
?
2
+
?
1
ln(exp(\phi_1^{\land})exp(\phi_2^{\land}))^{\vee} \approx J_r(\phi_1)^{-1}\phi_2 + \phi_1
ln(exp(?1∧?)exp(?2∧?))∨≈Jr?(?1?)?1?2?+?1?
J l = s i n θ θ I + ( 1 ? s i n θ θ ) a a T + 1 ? c o s θ θ a ∧ J_l = \frac{sin\theta}{\theta}I + (1- \frac{sin\theta}{\theta})aa^T + \frac{1-cos\theta}{\theta}a^{\land} Jl?=θsinθ?I+(1?θsinθ?)aaT+θ1?cosθ?a∧
J
l
?
1
=
θ
2
c
o
t
θ
2
I
+
(
1
?
θ
2
c
o
t
θ
2
)
a
a
T
?
θ
2
a
∧
J_l ^{-1} = \frac{\theta}{2}cot \frac{\theta}{2}I + (1- \frac{\theta}{2}cot \frac{\theta}{2})aa^T - \frac{\theta}{2}a^{\land}
Jl?1?=2θ?cot2θ?I+(1?2θ?cot2θ?)aaT?2θ?a∧
J
r
(
?
)
=
J
l
(
?
?
)
J_r(\phi) = J_l(-\phi)
Jr?(?)=Jl?(??)
l
n
(
R
e
x
p
(
?
∧
)
)
∨
=
l
n
(
R
)
∨
+
J
r
?
1
?
ln(Rexp(\phi^{\land}))^{\vee}=ln(R)^{\vee}+J_r^{-1}\phi
ln(Rexp(?∧))∨=ln(R)∨+Jr?1??
SO(3)伴随性质：
R
T
e
x
p
(
?
∧
)
R
=
e
x
p
(
(
R
T
?
)
∧
)
R^Texp(\phi^{\land})R = exp((R^T\phi)^{\land})
RTexp(?∧)R=exp((RT?)∧)
a
∧
b
=
?
b
∧
a
a^{\land} b = -b^{\land}a
a∧b=?b∧a