在自动驾驶中，我们常常会面对这线性代数位姿表示和坐标系变换问题。之前作者陆陆续续写了一些
R
T
exp
(
p
∧
)
R
=
exp
(
(
R
T
p
)
∧
)
⇒
exp
(
p
∧
)
R
=
R
exp
(
(
R
T
p
)
∧
)
RTexp(p∧)R=exp((RTp)∧)⇒exp(p∧)R=Rexp((RTp)∧)
一个标量和一个矢量来表示四元数就会如下图所示，这样会便于我们计算四元数之间的累乘变换
q
=
[
q
0
q
1
q
2
q
3
]
T
=
[
w
x
y
z
]
T
=
[
s
v
]
T
q=[q0q1q2q3]T=[wxyz]T=[sv]T
乘法运算中，向量的计算方法比较容易记忆，相乘后实部为两实部相乘减去两虚部正交，虚部为两个四元数和实部和虚部交叉相乘的和加上虚部的叉积。连续旋转两个角度可以用这两个旋转对应的四元数的乘积来表示，即
q
(
t
1
+
t
2
)
=
q
(
t
1
)
q
(
t
2
)
\boldsymbol{q}(t_1+t_2) = \boldsymbol{q}(t_1)\boldsymbol{q}(t_2)
q(t1+t2)=q(t1)q(t2)
q
a
⊗
q
b
=
q
(
a
)
q
(
b
)
=
w
a
w
b
−
x
a
x
b
−
y
a
y
b
−
z
a
z
b
+
(
w
a
x
b
+
x
a
w
b
+
y
a
z
b
−
z
a
y
b
)
i
+
(
w
a
y
b
−
x
a
z
b
+
y
a
w
b
+
z
a
x
b
)
j
+
(
w
a
z
b
+
x
a
y
b
−
y
a
x
b
+
z
a
w
b
)
k
=
[
s
a
s
b
−
v
a
T
v
b
T
s
a
v
b
+
s
b
v
a
+
v
a
×
v
b
]
qa⊗qb=q(a)q(b)= wawb−xaxb−yayb−zazb+(waxb+xawb+yazb−zayb)i+(wayb−xazb+yawb+zaxb)j+(wazb+xayb−yaxb+zawb)k=[sasb−vTavTbsavb+sbva+va×vb]
我们说四元数是除了旋转矩阵以外的另一种对旋转表达方式，并且它不具备奇异性，可以表达任意三维旋转，因此有必要学习一下它对时间的求导方式。
首先来看四元数和角轴的转换关系，假设某个旋转运动的旋转轴为单位向量 u \boldsymbol{u} u， 绕该轴旋转的角度为 θ \theta θ 那这个旋转的对应的四元数可表示为：
q
=
[
cos
θ
2
u
sin
θ
2
]
\boldsymbol{q} = [cosθ2usinθ2]
对时间求导：当旋转一段微小时间时，旋转经过角度可以视为趋于 0 0 0 ，因此上述四元数为：
Δ
q
=
[
cos
θ
2
u
sin
θ
2
]
≈
[
1
u
δ
θ
2
]
=
[
1
1
2
δ
θ
]
Δq=[cosθ2usinθ2]≈[1uδθ2]=[112δθ]
其中， δ θ \delta\boldsymbol{\theta} δθ 的方向为旋转轴，模长为（这段时间内非常微小的）旋转角度。
下面对时间求导：
q
˙
=
lim
Δ
t
→
0
q
(
t
+
Δ
t
)
−
q
(
t
)
Δ
t
=
lim
Δ
t
→
0
q
(
t
)
⊗
q
(
Δ
t
)
−
q
(
t
)
Δ
t
=
lim
Δ
t
→
0
q
⊗
Δ
q
−
q
Δ
t
=
lim
Δ
t
→
0
q
⊗
(
[
1
1
2
δ
θ
]
−
[
1
0
]
)
Δ
t
=
q
⊗
lim
Δ
t
→
0
(
[
0
1
2
δ
θ
]
)
Δ
t
=
q
⊗
[
0
1
2
ω
]
˙q=limΔt→0q(t+Δt)−q(t)Δt=limΔt→0q(t)⊗q(Δt)−q(t)Δt=limΔt→0q⊗Δq−qΔt=limΔt→0q⊗([112δθ]−[10])Δt=q⊗limΔt→0([012δθ])Δt=q⊗[012ω]
上式中， ω = lim Δ t → 0 δ θ Δ t \boldsymbol{\omega} = \lim_{\Delta t \rightarrow 0}\frac{\delta\boldsymbol{\theta}}{\Delta{t}} ω=limΔt→0Δtδθ 是求导时间的角速度。
从旋转向量到旋转矩阵基本就是通过罗德里格斯公式表明：
R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ \boldsymbol{R} = \boldsymbol{\mathrm{\cos{\theta}}I + \mathrm{(1-\cos{\theta})}nn^\mathrm{T} + \mathrm{\sin{\theta}}n^\wedge} R=cosθI+(1−cosθ)nnT+sinθn∧
从旋转矩阵到旋转向量的过程可以利用矩阵迹（trace）的性质进行推导：
t
r
(
R
)
=
cos
θ
t
r
(
I
)
+
(
1
−
cos
θ
)
t
r
(
n
n
)
T
+
sin
θ
n
∧
=
3
cos
θ
+
(
1
−
cos
θ
)
=
1
+
2
cos
θ
⇒
θ
=
arccos
t
r
(
R
)
−
1
2
tr(R)=cosθ tr(I)+(1−cosθ) tr(nn)T+sinθ n∧=3cosθ+(1−cosθ)=1+2cosθ⇒θ=arccostr(R)−12
李群（ S O ( 3 ) SO(3) SO(3)）对应的李代数 ( ϕ 1 , ϕ 2 ) (\boldsymbol{\phi}_1, \boldsymbol{\phi}_2) (ϕ1,ϕ2)的指数映射时。并且两个李代数其中有一个为小量时，二次项以上的小量都可以忽略。此时， BCH 公式有以下线性形式：
ln
(
exp
(
ϕ
1
∧
)
exp
(
ϕ
2
∧
)
)
∨
≈
{
J
l
(
ϕ
2
)
−
1
ϕ
1
+
ϕ
2
当
ϕ
1
为
小
量
J
r
(
ϕ
1
)
−
1
ϕ
2
+
ϕ
1
当
ϕ
2
为
小
量
\ln{(\exp(\boldsymbol{\phi}_1^\wedge)\exp{(\boldsymbol{\phi}_2^\wedge))}}^\vee \approx{Jl(ϕ2)−1ϕ1+ϕ2当ϕ1为小量Jr(ϕ1)−1ϕ2+ϕ1当ϕ2为小量
当我们对一个旋转
R
\boldsymbol{R}
R左乘或者右乘一个扰动
Δ
R
\Delta\boldsymbol{R}
ΔR时，BCH 近似过程可以表示为：
exp
(
Δ
ϕ
∧
)
exp
(
ϕ
∧
)
)
=
exp
(
(
ϕ
+
J
l
−
1
(
ϕ
)
Δ
ϕ
)
∧
)
exp
(
Δ
ϕ
∧
)
exp
(
ϕ
∧
)
)
=
exp
(
(
ϕ
+
J
r
−
1
(
ϕ
)
Δ
ϕ
)
∧
)
exp(Δϕ∧)exp(ϕ∧))=exp((ϕ+J−1l(ϕ)Δϕ)∧)exp(Δϕ∧)exp(ϕ∧))=exp((ϕ+J−1r(ϕ)Δϕ)∧)
而反过来，当我们对一个李代数 ϕ \boldsymbol{\phi} ϕ 加上一个微小扰动 Δ ϕ \Delta \boldsymbol{\phi} Δϕ 时，通过 BCH 可以近似为：
exp
(
(
ϕ
+
Δ
ϕ
)
∧
)
=
exp
(
J
l
Δ
ϕ
)
∧
exp
(
ϕ
∧
)
=
exp
(
ϕ
∧
)
exp
(
J
r
Δ
ϕ
)
∧
exp((ϕ+Δϕ)∧)=exp(JlΔϕ)∧exp(ϕ∧)=exp(ϕ∧)exp(JrΔϕ)∧
对于李群而言，由于其没有向量空间上的加法操作，因此为了引入导数的概念，这里用一个映射将局部坐标 ξ \xi ξ 映射到李群元素 a a a 在李群空间附近的邻域上，用来作为李群上的 “加法” 操作，所以常常会使用 ⊕ \oplus ⊕来表示李群的加法，如下所示：
a ⊕ ξ ≜ a exp ( ξ ^ ) a\oplus \xi \triangleq a \exp{(\hat{\xi})} a⊕ξ≜aexp(ξ^)
式中， ξ ∈ R n \xi\in\mathbb{R}^n ξ∈Rn 是 a a a 系下的局部坐标，这个也是我们所说的极小值右乘的做法，以李群 SO(3) 为例，局部坐标可以表示为 ξ = ω t \xi = \omega t ξ=ωt，其几何意义为以 a a a 作为参考系下的一个角度扰动；
ξ ^ \hat{\xi} ξ^ 为 ξ \xi ξ 的对应李代数，当然也可以像上面表示为 ξ ∧ \xi^{\wedge} ξ∧；
exp ξ \exp{\xi} expξ 为李代数到李群的指数映射；
ξ ^ = [ ω t ] × \hat{\xi} = [\omega t]_{\times} ξ^=[ωt]× 为李代数，李代数可以转化为反对称矩阵。通过 × _{\times} ×来表示角度扰动（旋转轴+旋转角度）的旋转向量。