数学基础
可以通过运算
(
⋅
)
∧
{\left( \cdot \right)^ \wedge }
(⋅)∧将三维向量
ω
∈
R
3
\omega \in {\mathbb{R}^{3}}
ω∈R3转换为对角线为零的反对称矩阵
ω
∧
∈
R
3
×
3
\ {\omega ^ \wedge } \in {\mathbb{R}^{3 \times 3}}
ω∧∈R3×3:
ω
∧
=
[
ω
1
ω
2
ω
3
]
∧
=
[
0
−
ω
3
ω
2
ω
3
0
−
ω
1
−
ω
2
ω
1
0
]
(1.1)
{\omega ^ \wedge } = {\left[ \begin{array}{l}{\omega _1}\\{\omega _2}\\{\omega _3}\end{array} \right]^ \wedge } = \left[ {\begin{array}{ccccccccccccccc}0&{ - {\omega _3}}&{{\omega _2}}\\{{\omega _3}}&0&{ - {\omega _1}}\\{ - {\omega _2}}&{{\omega _1}}&0\end{array}} \right] \tag{1.1}
ω∧=
ω1ω2ω3
∧=
0ω3−ω2−ω30ω1ω2−ω10
(1.1)
同理可以使用
(
⋅
)
∨
{\left( \cdot \right)^ \vee }
(⋅)∨运算将一个对角线为零的反对称矩阵转换为三维向量。
运算
(
⋅
)
∧
{\left( \cdot \right)^ \wedge }
(⋅)∧满足:
a
∧
b
=
−
b
∧
a
,
∀
a
,
b
∈
R
3
{a^ \wedge }b = - {b^ \wedge }a,{\rm{ }}\forall a, b \in {\mathbb{R}^{3}}
a∧b=−b∧a,∀a,b∈R3
对于旋转向量
ϕ
∈
s
o
(
3
)
\phi \in \mathfrak{s}\mathfrak{o}\left( 3 \right)
ϕ∈so(3),可以通过指数映射
exp
(
⋅
)
\exp \left( {\cdot } \right)
exp(⋅)将其转换为旋转矩阵
R
∈
S
O
(
3
)
R \in SO\left( 3 \right)
R∈SO(3):
R
=
exp
(
ϕ
∧
)
=
I
+
sin
(
∥
ϕ
∥
)
∥
ϕ
∥
ϕ
∧
+
1
−
cos
(
∥
ϕ
∥
)
∥
ϕ
∥
2
(
ϕ
)
2
(1.2)
R = \exp \left( {{\phi ^ \wedge }} \right) = I + \frac{{\sin \left( {\left\| \phi \right\|} \right)}}{{\left\| \phi \right\|}}{\phi ^ \wedge } + \frac{{1 - \cos \left( {\left\| \phi \right\|} \right)}}{{{{\left\| \phi \right\|}^2}}}{\left( \phi \right)^2}\tag{1.2}
R=exp(ϕ∧)=I+∥ϕ∥sin(∥ϕ∥)ϕ∧+∥ϕ∥21−cos(∥ϕ∥)(ϕ)2(1.2)
同时,式
(
1.2
)
(1.2)
(1.2)可以进行一阶近似:
R
=
exp
(
ϕ
∧
)
≈
I
+
ϕ
∧
(1.3)
R = \exp \left( {{\phi ^ \wedge }} \right) \approx I + {\phi ^ \wedge }\tag{1.3}
R=exp(ϕ∧)≈I+ϕ∧(1.3)
同理,旋转矩阵
R
R
R可以通过对数映射
log
(
⋅
)
\log \left( {\cdot } \right)
log(⋅)将其转换为旋转向量。
通过BCH公式,可以将李代数上的加法近似为李群上的乘法:
exp
(
ϕ
+
δ
ϕ
)
≈
exp
(
ϕ
)
exp
(
J
r
(
ϕ
)
δ
ϕ
)
(1.4)
\exp \left( {\phi + \delta \phi } \right) \approx \exp \left( \phi \right)\exp \left( {{J_r}\left( \phi \right)\delta \phi } \right)\tag{1.4}
exp(ϕ+δϕ)≈exp(ϕ)exp(Jr(ϕ)δϕ)(1.4)
J
r
(
ϕ
)
=
I
−
1
−
cos
(
∥
ϕ
∥
)
∥
ϕ
∥
2
ϕ
∧
+
∥
ϕ
∥
−
sin
(
∥
ϕ
∥
)
∥
ϕ
3
∥
(
ϕ
∧
)
2
(1.5)
{J_r}\left( \phi \right) = I - \frac{{1 - \cos \left( {\left\| \phi \right\|} \right)}}{{{{\left\| \phi \right\|}^2}}}{\phi ^ \wedge } + \frac{{\left\| \phi \right\| - \sin \left( {\left\| \phi \right\|} \right)}}{{\left\| {{\phi ^3}} \right\|}}{\left( {{\phi ^ \wedge }} \right)^2}\tag{1.5}
Jr(ϕ)=I−∥ϕ∥21−cos(∥ϕ∥)ϕ∧+∥ϕ3∥∥ϕ∥−sin(∥ϕ∥)(ϕ∧)2(1.5)
也可以将李群上的乘法近似为李代数上的加法:
log
(
exp
(
ϕ
)
exp
(
δ
ϕ
)
)
∨
≈
ϕ
+
J
r
−
1
(
ϕ
)
δ
ϕ
(1.6)
\log {\left( {\exp \left( \phi \right)\exp \left( {\delta \phi } \right)} \right)^ \vee } \approx \phi + J_r^{ - 1}\left( \phi \right)\delta \phi\tag{1.6}
log(exp(ϕ)exp(δϕ))∨≈ϕ+Jr−1(ϕ)δϕ(1.6)
J
r
−
1
(
ϕ
)
=
I
+
1
2
ϕ
∧
+
(
1
∥
ϕ
∥
2
+
1
+
cos
(
∥
ϕ
∥
)
2
∥
ϕ
∥
sin
(
∥
ϕ
∥
)
)
(
ϕ
∧
)
2
(1.7)
J_r^{ - 1}\left( \phi \right) = I + \frac{1}{2}{\phi ^ \wedge } + \left( {\frac{1}{{{{\left\| \phi \right\|}^2}}} + \frac{{1 + \cos \left( {\left\| \phi \right\|} \right)}}{{2\left\| \phi \right\|\sin \left( {\left\| \phi \right\|} \right)}}} \right){\left( {{\phi ^ \wedge }} \right)^2}\tag{1.7}
Jr−1(ϕ)=I+21ϕ∧+(∥ϕ∥21+2∥ϕ∥sin(∥ϕ∥)1+cos(∥ϕ∥))(ϕ∧)2(1.7)
李群的转置性质:
exp
(
ϕ
∧
)
R
=
R
exp
(
R
T
ϕ
∧
)
(1.8)
\exp \left( {{\phi ^ \wedge }} \right)R = { R} \exp \left( {{R^T}{\phi ^ \wedge }} \right)\tag{1.8}
exp(ϕ∧)R=Rexp(RTϕ∧)(1.8)
四元数
单位四元数表达三维空间的旋转,由一个实部
s
∈
R
s \in \mathbb{R}
s∈R和一个虚部
v
∈
R
3
×
1
v \in {\mathbb{R}^{3 \times 1}}
v∈R3×1组成:
q
=
[
w
x
y
z
]
=
[
s
v
]
q = \left[ \begin{array}{l}w\\x\\y\\z\end{array} \right] = \left[ \begin{array}{l}s\\v\end{array} \right]
q=
wxyz
=[sv]
四元数的乘法运算
⊗
\otimes
⊗定义如下:
q
a
⊗
q
b
=
[
s
a
s
b
−
v
a
T
v
b
s
a
v
b
+
s
b
v
a
+
v
a
×
v
b
]
{q_a} \otimes {q_b} = \left[ \begin{array}{l}{s_a}{s_b} - v_a^T{v_b}\\{s_a}{v_b} + {s_b}{v_a} + {v_a} \times {v_b}\end{array} \right]
qa⊗qb=[sasb−vaTvbsavb+sbva+va×vb]推导过程中经常将四元数乘法转换为矩阵乘法:
q
a
⊗
q
b
=
[
q
a
]
L
q
b
=
q
a
[
q
b
]
R
{q_a} \otimes {q_b} = {\left[ {{q_a}} \right]_L}{q_b} = {q_a}{\left[ {{q_b}} \right]_R}
qa⊗qb=[qa]Lqb=qa[qb]R其中:
[
q
a
]
L
=
[
w
−
x
−
y
−
z
x
w
−
z
y
y
z
w
−
x
z
−
y
x
w
]
,
[
q
b
]
R
=
[
w
−
x
−
y
−
z
x
w
z
−
y
y
−
z
w
x
z
y
−
x
w
]
{\left[ {{q_a}} \right]_L} = \left[ {\begin{array}{ccccccccccccccc}w&{ - x}&{ - y}&{ - z}\\x&w&{ - z}&y\\y&z&w&{ - x}\\z&{ - y}&x&w\end{array}} \right],{\left[ {{q_b}} \right]_R} = \left[ {\begin{array}{ccccccccccccccc}w&{ - x}&{ - y}&{ - z}\\x&w&z&{ - y}\\y&{ - z}&w&x\\z&y&{ - x}&w\end{array}} \right]
[qa]L=
wxyz−xwz−y−y−zwx−zy−xw
,[qb]R=
wxyz−xw−zy−yzw−x−z−yxw
同时,四元数和旋转向量可以相互转换:
q
=
[
cos
θ
2
n
⃗
sin
θ
2
]
q = \left[ \begin{array}{l}\cos \frac{\theta }{2}\\\vec n\sin \frac{\theta }{2}\end{array} \right]
q=[cos2θnsin2θ]当
θ
\theta
θ为小量时,有以下近似:
q
=
[
1
θ
n
⃗
2
]
q = \left[ \begin{array}{c}1\\\frac{{\theta \vec n}}{2}\end{array} \right]
q=[12θn]
通常使用右乘扰动模型对李群求导,左乘和右乘等价,只不过扰动模型不能混用。李群可以使用旋转矩阵也可以使用四元数。
∂
(
R
p
)
∂
ϕ
=
lim
δ
ϕ
→
0
exp
(
δ
ϕ
∧
)
exp
(
ϕ
∧
)
p
−
exp
(
ϕ
∧
)
p
δ
ϕ
=
lim
δ
ϕ
→
0
(
I
+
δ
ϕ
∧
)
exp
(
ϕ
∧
)
p
−
exp
(
ϕ
∧
)
p
δ
ϕ
=
lim
δ
ϕ
→
0
δ
ϕ
∧
exp
(
ϕ
∧
)
p
δ
ϕ
=
lim
δ
ϕ
→
0
−
(
exp
(
ϕ
∧
)
p
)
∧
δ
ϕ
δ
ϕ
=
−
(
exp
(
ϕ
∧
)
p
)
∧
\begin{array}{l}\frac{{\partial \left( {Rp} \right)}}{{\partial \phi }} = \lim_{\delta \phi \to 0} \frac{{\exp \left( {\delta {\phi ^ \wedge }} \right)\exp \left( {{\phi ^ \wedge }} \right)p - \exp \left( {{\phi ^ \wedge }} \right)p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{\left( {I + \delta {\phi ^ \wedge }} \right)\exp \left( {{\phi ^ \wedge }} \right)p - \exp \left( {{\phi ^ \wedge }} \right)p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{\delta {\phi ^ \wedge }\exp \left( {{\phi ^ \wedge }} \right)p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{ - {{\left( {\exp \left( {{\phi ^ \wedge }} \right)p} \right)}^ \wedge }\delta \phi }}{{\delta \phi }}\\ = - {\left( {\exp \left( {{\phi ^ \wedge }} \right)p} \right)^ \wedge }\end{array}
∂ϕ∂(Rp)=limδϕ→0δϕexp(δϕ∧)exp(ϕ∧)p−exp(ϕ∧)p=limδϕ→0δϕ(I+δϕ∧)exp(ϕ∧)p−exp(ϕ∧)p=limδϕ→0δϕδϕ∧exp(ϕ∧)p=limδϕ→0δϕ−(exp(ϕ∧)p)∧δϕ=−(exp(ϕ∧)p)∧
∂ ( R p ) ∂ ϕ = lim δ ϕ → 0 exp ( ϕ ∧ ) exp ( δ ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 exp ( ϕ ∧ ) ( I + δ ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 exp ( ϕ ∧ ) δ ϕ ∧ p δ ϕ = lim δ ϕ → 0 − exp ( ϕ ∧ ) p ∧ δ ϕ δ ϕ = − exp ( ϕ ∧ ) p ∧ \begin{array}{l}\frac{{\partial \left( {Rp} \right)}}{{\partial \phi }} = \lim_{\delta \phi \to 0} \frac{{\exp \left( {{\phi ^ \wedge }} \right)\exp \left( {\delta {\phi ^ \wedge }} \right)p - \exp \left( {{\phi ^ \wedge }} \right)p}}{{\delta \phi }}\\{\rm{ = }} \lim_{\delta \phi \to 0} \frac{{\exp \left( {{\phi ^ \wedge }} \right)\left( {I + \delta {\phi ^ \wedge }} \right)p - \exp \left( {{\phi ^ \wedge }} \right)p}}{{\delta \phi }}\\{\rm{ = }} \lim_{\delta \phi \to 0} \frac{{\exp \left( {{\phi ^ \wedge }} \right)\delta {\phi ^ \wedge }p}}{{\delta \phi }}\\{\rm{ = }} \lim_{\delta \phi \to 0} \frac{{ - \exp \left( {{\phi ^ \wedge }} \right){p^ \wedge }\delta \phi }}{{\delta \phi }}\\ = - \exp \left( {{\phi ^ \wedge }} \right){p^ \wedge }\end{array} ∂ϕ∂(Rp)=limδϕ→0δϕexp(ϕ∧)exp(δϕ∧)p−exp(ϕ∧)p=limδϕ→0δϕexp(ϕ∧)(I+δϕ∧)p−exp(ϕ∧)p=limδϕ→0δϕexp(ϕ∧)δϕ∧p=limδϕ→0δϕ−exp(ϕ∧)p∧δϕ=−exp(ϕ∧)p∧ ∂ log ( R 1 R 2 ) ∂ ϕ 2 = lim δ ϕ 2 → 0 log ( R 1 R 2 exp ( δ ϕ 2 ∧ ) ) − log ( R 1 R 2 ) δ ϕ 2 = lim δ ϕ 2 → 0 log ( R 1 R 2 ) + J r − 1 ( log ( R 1 R 2 ) ) δ ϕ 2 − log ( R 1 R 2 ) δ ϕ 2 = J r − 1 ( log ( R 1 R 2 ) ) \begin{array}{l}\frac{{\partial \log \left( {{R_1}{R_2}} \right)}}{{\partial {\phi _2}}} = \lim_{\delta {\phi _2} \to 0} \frac{{\log \left( {{R_1}{R_2}\exp \left( {\delta \phi _2^ \wedge } \right)} \right) - \log \left( {{R_1}{R_2}} \right)}}{{\delta {\phi _2}}}\\ = \lim_{\delta {\phi _2} \to 0} \frac{{\log \left( {{R_1}{R_2}} \right) + J_r^{ - 1}\left( {\log \left( {{R_1}{R_2}} \right)} \right)\delta {\phi _2} - \log \left( {{R_1}{R_2}} \right)}}{{\delta {\phi _2}}}\\ = J_r^{ - 1}\left( {\log \left( {{R_1}{R_2}} \right)} \right)\end{array} ∂ϕ2∂log(R1R2)=limδϕ2→0δϕ2log(R1R2exp(δϕ2∧))−log(R1R2)=limδϕ2→0δϕ2log(R1R2)+Jr−1(log(R1R2))δϕ2−log(R1R2)=Jr−1(log(R1R2)) ∂ ( R − 1 p ) ∂ ϕ = lim lim δ ϕ → 0 ( R exp ( δ ϕ ∧ ) ) − 1 p − R − 1 p δ ϕ = lim δ ϕ → 0 exp ( − δ ϕ ∧ ) R − 1 p − R − 1 p δ ϕ = lim δ ϕ → 0 ( I − δ ϕ ∧ ) R − 1 p − R − 1 p δ ϕ = lim δ ϕ → 0 − δ ϕ ∧ R − 1 p δ ϕ = lim δ ϕ → 0 ( R − 1 p ) ∧ δ ϕ δ ϕ = ( R − 1 p ) ∧ \begin{array}{l}\frac{{\partial \left( {{R^{ - 1}}p} \right)}}{{\partial \phi }} = {\lim }\lim_{\delta \phi \to 0} \frac{{{{\left( {R\exp \left( {\delta {\phi ^ \wedge }} \right)} \right)}^{ - 1}}p - {R^{ - 1}}p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{\exp \left( { - \delta {\phi ^ \wedge }} \right){R^{ - 1}}p - {R^{ - 1}}p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{\left( {I - \delta {\phi ^ \wedge }} \right){R^{ - 1}}p - {R^{ - 1}}p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{ - \delta {\phi ^ \wedge }{R^{ - 1}}p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{{{\left( {{R^{ - 1}}p} \right)}^ \wedge }\delta \phi }}{{\delta \phi }}\\ = {\left( {{R^{ - 1}}p} \right)^ \wedge }\end{array} ∂ϕ∂(R−1p)=limlimδϕ→0δϕ(Rexp(δϕ∧))−1p−R−1p=limδϕ→0δϕexp(−δϕ∧)R−1p−R−1p=limδϕ→0δϕ(I−δϕ∧)R−1p−R−1p=limδϕ→0δϕ−δϕ∧R−1p=limδϕ→0δϕ(R−1p)∧δϕ=(R−1p)∧ ∂ log ( R 1 R 2 − 1 ) ∂ ϕ 2 = lim δ ϕ 2 → 0 log ( R 1 exp ( − δ ϕ 2 ∧ ) R 2 − 1 ) − log ( R 1 R 2 ) δ ϕ 2 = lim δ ϕ 2 → 0 log ( R 1 R 2 − 1 exp ( − R 2 δ ϕ 2 ∧ ) ) − log ( R 1 R 2 ) δ ϕ 2 = lim δ ϕ 2 → 0 − J r − 1 ( log ( R 1 R 2 − 1 ) ) R 2 δ ϕ 2 δ ϕ 2 = − J r − 1 ( log ( R 1 R 2 − 1 ) ) R 2 \begin{array}{l}\frac{{\partial \log \left( {{R_1}R_2^{ - 1}} \right)}}{{\partial {\phi _2}}} = {\lim }_{\delta {\phi _2} \to 0} \frac{{\log \left( {{R_1}\exp \left( { - \delta \phi _2^ \wedge } \right)R_2^{ - 1}} \right) - \log \left( {{R_1}{R_2}} \right)}}{{\delta {\phi _2}}}\\ = {\lim }_{\delta {\phi _2} \to 0} \frac{{\log \left( {{R_1}R_2^{ - 1}\exp \left( { - {R_2}\delta \phi _2^ \wedge } \right)} \right) - \log \left( {{R_1}{R_2}} \right)}}{{\delta {\phi _2}}}\\ = {\lim }_{\delta {\phi _2} \to 0} \frac{{ - J_r^{ - 1}\left( {\log \left( {{R_1}R_2^{ - 1}} \right)} \right){R_2}\delta {\phi _2}}}{{\delta {\phi _2}}}\\ = - J_r^{ - 1}\left( {\log \left( {{R_1}R_2^{ - 1}} \right)} \right){R_2}\end{array} ∂ϕ2∂log(R1R2−1)=limδϕ2→0δϕ2log(R1exp(−δϕ2∧)R2−1)−log(R1R2)=limδϕ2→0δϕ2log(R1R2−1exp(−R2δϕ2∧))−log(R1R2)=limδϕ2→0δϕ2−Jr−1(log(R1R2−1))R2δϕ2=−Jr−1(log(R1R2−1))R2李群更新可以使用旋转矩阵也可以使用四元数,两种方法是等价的,注意四元数需要归一化: R ← R exp ( ϕ ∧ ) q ← q ⊗ [ 1 1 2 ϕ ] \begin{array}{l}R \leftarrow {{\rm R}} \exp \left( {{\phi ^ \wedge }} \right)\\q \leftarrow q \otimes \left[ \begin{array}{c}1\\\frac{1}{2}\phi \end{array} \right]\end{array} R←Rexp(ϕ∧)q←q⊗[121ϕ]
IMU
IMU的误差可以分为确定性误差和随机误差。
确定性误差主要包括比例因子误差、零偏误差、非正交误差,同时误差受到温度的影响,可以通过标定方法确定误差的大小,例如六面法标定法,温度标定法等。
随机误差通常包括零偏随机游走和高斯白噪声。高斯白噪声假设IMU数据在连续时间上受到一个均值为0,方差为
σ
2
{\sigma ^2}
σ2,各时刻之间相互独立的高斯过程,对于离散的IMU数据,方差之间存在以下转化关系:
σ
d
2
=
σ
2
Δ
t
\sigma _d^2 = \frac{{{\sigma ^2}}}{{\Delta t}}
σd2=Δtσ2