群论
定义
群(Group)是一种集合加上一个二元运算。
设 G G G是一个非空集合, ⋅ \cdot ⋅是一个二元运算,若满足如下条件:
- 封闭性: ∀ a , b ∈ G , a ⋅ b ∈ G \forall a,b\in G, a\cdot b \in G ∀a,b∈G,a⋅b∈G
- 结合律: ∀ a , b , c ∈ G , ( a ⋅ b ) ⋅ c = a ⋅ ( b ⋅ c ) \forall a,b,c\in G, (a\cdot b)\cdot c=a\cdot(b\cdot c) ∀a,b,c∈G,(a⋅b)⋅c=a⋅(b⋅c)
- 单位元(幺元): ∃ e ∈ G , s . t . ∀ a ∈ G a ⋅ e = e ⋅ a = a \exists e\in G,\enspace s.t.\enspace \forall a\in G\enspace a\cdot e=e\cdot a=a ∃e∈G,s.t.∀a∈Ga⋅e=e⋅a=a
- 逆元: ∀ a ∈ G , ∃ a − 1 ∈ G , s . t . a ⋅ a − 1 = a − 1 ⋅ a = e \forall a\in G,\exist a^{-1}\in G,\enspace s.t.\enspace a\cdot a^{-1}=a^{-1}\cdot a=e ∀a∈G,∃a−1∈G,s.t.a⋅a−1=a−1⋅a=e
则称 G G G对 ⋅ \cdot ⋅构成一个群,记作 ( G , ⋅ ) (G,\cdot) (G,⋅)。
- 称 G G G上二元运算 ⋅ \cdot ⋅为“乘法”
- a ⋅ b a\cdot b a⋅b为 a a a与 b b b的积,简写 a b ab ab
- 若群 G G G中元素有限,则称其为有限群,反之则称其为无限群
- 有限群的元素个数称为有限群的阶
子群
对于群
(
G
,
⋅
)
(G,\cdot)
(G,⋅)存在
H
⊂
G
H\subset G
H⊂G且
H
≠
∅
H\neq \varnothing
H=∅,若
(
H
,
⋅
)
(H,\cdot)
(H,⋅)也是一个群,则称
H
H
H为
G
G
G的子群。子群的充要条件如下:
H
H
=
H
且
H
−
1
=
H
HH=H且H^{-1}=H
HH=H且H−1=H
李群
李群是一种具有连续(光滑)性质的群,它既是群也是流形。
刚体能够连续地在空间中运动,故而特殊正交群 S O ( n ) SO(n) SO(n)和特殊欧式群 S E ( n ) SE(n) SE(n)为李群。
李代数
定义
每个李群都有与之对应的李代数,李代数描述了李群单位元附近的正切空间性质。
李代数由一个集合 V V V,一个数域 F F F以及一个二元运算 [ , ] [,] [,]组成,记为 ( V , F , [ , ] ) (V,F,[,]) (V,F,[,])。其中二元运算 [ , ] [,] [,]被称为李括号,表示了两元素间的差异。
李代数满足如下条件:
-
封闭性: ∀ X , Y ∈ V , [ X , Y ] ∈ V \forall X,Y\in V,\enspace [X,Y]\in V ∀X,Y∈V,[X,Y]∈V
-
双线性: ∀ X , Y , Z ∈ V , a , b ∈ F \forall X,Y,Z\in V,\enspace a,b\in F ∀X,Y,Z∈V,a,b∈F有:
[ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] [aX+bY,Z]=a[X,Z]+b[Y,Z]\\ [Z,aX+bY]=a[Z,X]+b[Z,Y] [aX+bY,Z]=a[X,Z]+b[Y,Z][Z,aX+bY]=a[Z,X]+b[Z,Y] -
自反性: ∀ X ∈ V , [ X , X ] = 0 \forall X\in V,[X,X]=0 ∀X∈V,[X,X]=0
-
雅可比等价: ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Y , [ Z , X ] ] + [ Z , [ X , Y ] ] = 0 \forall X,Y,Z\in V,[X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]]=0 ∀X,Y,Z∈V,[X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]]=0
-
反对称性: ∀ X , Y ∈ V , [ X , Y ] = − [ Y , X ] \forall X,Y\in V, [X,Y]=-[Y,X] ∀X,Y∈V,[X,Y]=−[Y,X]
推导(SO(3)李代数)
对于任意旋转矩阵
R
R
R,满足
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
两侧对时间
t
t
t进行求导:
R
˙
(
t
)
R
(
t
)
T
+
R
(
t
)
R
˙
(
t
)
T
=
0
R
˙
(
t
)
R
(
t
)
T
=
−
R
(
t
)
R
˙
(
t
)
T
R
˙
(
t
)
R
(
t
)
T
=
−
(
R
˙
(
t
)
R
(
t
)
T
)
T
\begin{aligned} \dot R(t)R(t)^T&+R(t)\dot R(t)^T=0\\ \dot R(t)R(t)^T&=-R(t)\dot R(t)^T\\ \dot R(t)R(t)^T&=-\Bigl(\dot R(t)R(t)^T\Bigr)^T \end{aligned}
R˙(t)R(t)TR˙(t)R(t)TR˙(t)R(t)T+R(t)R˙(t)T=0=−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为反对称矩阵,记
R
˙
(
t
)
R
(
t
)
T
=
ϕ
(
t
)
∧
\dot R(t)R(t)^T=\phi(t)^\wedge
R˙(t)R(t)T=ϕ(t)∧,等式两侧右乘
R
(
t
)
R(t)
R(t):
R
˙
(
t
)
=
ϕ
(
t
)
∧
R
(
t
)
R
˙
(
t
)
=
[
0
−
ϕ
3
ϕ
2
ϕ
3
0
−
ϕ
1
−
ϕ
2
ϕ
1
0
]
R
(
t
)
\begin{aligned} \dot R(t)&=\phi(t)^\wedge R(t)\\ \dot R(t)&=\begin{bmatrix}0&-\phi_3&\phi_2\\\phi_3&0&-\phi_1\\-\phi_2&\phi_1&0\end{bmatrix}R(t) \end{aligned}
R˙(t)R˙(t)=ϕ(t)∧R(t)=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤R(t)
由上式可知,每对旋转矩阵求一次导数,只需对其左乘一个
ϕ
(
t
)
∧
\phi(t)^\wedge
ϕ(t)∧。
S
O
(
3
)
SO(3)
SO(3)的单位元为:
t
0
=
0
,
R
(
0
)
=
I
t_0=0,R(0)=I
t0=0,R(0)=I,将
R
(
t
)
R(t)
R(t)在单位元附近进行一阶泰勒展开:
R
(
t
)
≈
R
(
t
0
)
+
R
˙
(
t
0
)
(
t
−
t
0
)
=
I
+
ϕ
(
t
0
)
∧
R
(
t
0
)
(
t
−
t
0
)
=
I
+
ϕ
(
t
0
)
∧
t
\begin{aligned} R(t)&\approx R(t_0)+\dot R(t_0)(t-t_0)\\ &=I+\phi(t_0)^\wedge R(t_0)(t-t_0)\\ &=I+\phi(t_0)^\wedge t \end{aligned}
R(t)≈R(t0)+R˙(t0)(t−t0)=I+ϕ(t0)∧R(t0)(t−t0)=I+ϕ(t0)∧t
ϕ
\phi
ϕ反应了旋转矩阵
R
R
R的导数性质,称其在
S
O
(
3
)
SO(3)
SO(3)原点附近的正切空间上。
设
t
0
t_0
t0附近
ϕ
\phi
ϕ为常数不变:
ϕ
(
t
0
)
=
ϕ
0
\phi(t_0)=\phi_0
ϕ(t0)=ϕ0,则有:
R
˙
(
t
)
=
ϕ
0
∧
R
(
t
)
\dot R(t)=\phi_0^\wedge R(t)
R˙(t)=ϕ0∧R(t)
在初始条件
R
(
0
)
=
I
R(0)=I
R(0)=I下解微分方程,得到:
R
(
t
)
=
e
x
p
(
ϕ
0
∧
t
)
R(t)=exp(\phi_0^\wedge t)
R(t)=exp(ϕ0∧t)
上式称为李代数
s
o
(
3
)
so(3)
so(3)的指数映射关系。
李代数so(3)
向量空间: s o ( 3 ) = { Φ = ϕ ∧ ∈ ℜ 3 × 3 ∣ ϕ ∈ ℜ 3 } so(3)=\Bigl\{\Phi=\phi^\wedge\in\Re^{3\times3}\enspace\Big\vert \phi\in\Re^3\Bigr\} so(3)={Φ=ϕ∧∈ℜ3×3∣∣∣ϕ∈ℜ3}
数域: ℜ \Re ℜ
李括号:
[
ϕ
a
,
ϕ
b
]
=
Φ
a
Φ
b
−
Φ
b
Φ
a
[\phi_a,\phi_b]=\Phi_a\Phi_b-\Phi_b\Phi_a
[ϕa,ϕb]=ΦaΦb−ΦbΦa
[
ϕ
a
,
ϕ
b
]
=
Φ
a
Φ
b
−
Φ
b
Φ
a
=
ϕ
a
∧
ϕ
b
∧
−
ϕ
b
∧
ϕ
a
∧
=
(
ϕ
a
∧
ϕ
b
)
∧
∈
s
o
(
3
)
\begin{aligned} {[}\phi_a,\phi_b]&=\Phi_a\Phi_b-\Phi_b\Phi_a\\ &=\phi_a^\land\phi_b^\land-\phi_b^\land\phi_a^\land\\ &=\bigl(\phi_a^\land\phi_b\bigr)^\land\in so(3) \end{aligned}
[ϕa,ϕb]=ΦaΦb−ΦbΦa=ϕa∧ϕb∧−ϕb∧ϕa∧=(ϕa∧ϕb)∧∈so(3)
其中
Φ
=
ϕ
∧
\Phi=\phi^\land
Φ=ϕ∧,为反对称矩阵:
Φ
=
[
0
−
ϕ
3
ϕ
2
ϕ
3
0
−
ϕ
1
−
ϕ
2
ϕ
1
0
]
∈
ℜ
3
×
3
\Phi=\begin{bmatrix}0&-\phi_3&\phi_2\\\phi_3&0&-\phi_1\\-\phi_2&\phi_1&0\end{bmatrix}\in \Re^{3\times3}
Φ=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤∈ℜ3×3
视集合 ℜ 3 \Re^3 ℜ3和叉乘运算作为 s e ( 3 ) se(3) se(3)
李代数se(3)
向量空间: s e ( 3 ) = { Ξ = ξ ∧ ∈ ℜ 4 × 4 ∣ ξ ∈ ℜ 6 } se(3)=\Biggl\{\Xi=\xi^\land\in\Re^{4\times4} \Bigg\vert\xi\in\Re^{6}\Biggr\} se(3)={Ξ=ξ∧∈ℜ4×4∣∣∣∣∣ξ∈ℜ6}
数域: ℜ \Re ℜ
李括号
[
Ξ
1
,
Ξ
2
]
=
Ξ
1
Ξ
2
−
Ξ
2
Ξ
1
[\Xi_1,\Xi_2]=\Xi_1\Xi_2-\Xi_2\Xi_1
[Ξ1,Ξ2]=Ξ1Ξ2−Ξ2Ξ1
[
Ξ
a
,
Ξ
b
]
=
Ξ
a
Ξ
b
−
Ξ
a
Ξ
b
=
ξ
a
∧
ξ
b
∧
−
ξ
b
∧
ξ
a
∧
=
(
ξ
1
⋆
ξ
2
)
∧
∈
s
e
(
3
)
\begin{aligned} {[}\Xi_a,\Xi_b]&=\Xi_a\Xi_b-\Xi_a\Xi_b\\ &=\xi_a^\land\xi_b^\land-\xi_b^\land\xi_a^\land\\ &=\bigl(\xi_1^{\star}\xi_2\bigr)^\land\in se(3) \end{aligned}
[Ξa,Ξb]=ΞaΞb−ΞaΞb=ξa∧ξb∧−ξb∧ξa∧=(ξ1⋆ξ2)∧∈se(3)
其中,参量
ξ
\xi
ξ用于表示平移加旋转:
ξ
=
[
ρ
ϕ
]
,
ρ
∈
ℜ
3
,
ϕ
∈
s
o
(
3
)
\xi=\begin{bmatrix}\rho\\\phi\end{bmatrix},\rho\in\Re^3,\phi\in so(3)
ξ=[ρϕ],ρ∈ℜ3,ϕ∈so(3)
ξ
∧
\xi^\land
ξ∧非反对称矩阵,但仍保留记法:
ξ
∧
=
[
ϕ
∧
ρ
0
T
0
]
∈
ℜ
4
×
4
\xi^\land=\begin{bmatrix}\phi^\land&\rho\\0^T&0\end{bmatrix}\in\Re^{4\times4}
ξ∧=[ϕ∧0Tρ0]∈ℜ4×4
星计算
ξ
⋆
\xi^\star
ξ⋆定义为:
ξ
⋆
=
[
ρ
ϕ
]
⋆
=
[
ϕ
∧
ρ
∧
0
ϕ
∧
]
∈
ℜ
6
×
6
\xi^\star=\begin{bmatrix}\rho\\\phi\end{bmatrix}^\star=\begin{bmatrix}\phi^\land&\rho^\land\\0&\phi^\land\end{bmatrix}\in\Re^{6\times6}
ξ⋆=[ρϕ]⋆=[ϕ∧0ρ∧ϕ∧]∈ℜ6×6
视集合
ℜ
6
\Re^6
ℜ6和星计算为
s
e
(
3
)
se(3)
se(3)
指数映射与对数映射
指数映射反应了从李代数到李群的转换,任意矩阵的指数映射可写成一个泰勒展开,其结果仍是一个矩阵:
e
x
p
(
G
)
=
∑
n
=
0
∞
1
n
!
G
n
exp(G)=\sum_{n=0}^\infty\frac{1}{n!}G^n
exp(G)=n=0∑∞n!1Gn
对数映射则反应了从李群到李代数的转换:
ln
(
G
)
=
∑
n
=
0
∞
(
−
1
)
n
n
+
1
(
G
−
I
)
n
+
1
\ln(G)=\sum_{n=0}^\infty\frac{(-1)^{n}}{n+1}(G-I)^{n+1}
ln(G)=n=0∑∞n+1(−1)n(G−I)n+1
李群 S O ( 3 ) SO(3) SO(3)李代数 s o ( 3 ) so(3) so(3)
ϕ \phi ϕ是一个向量,定义其模长为 θ \theta θ方向为 a a a, ϕ = θ a \phi=\theta a ϕ=θa。其中, a a a为一个模长为1的单位向量: ∥ a ∥ = 1 \begin{Vmatrix}a\end{Vmatrix}=1 ∥∥a∥∥=1。
对于反对称矩阵
a
∧
a^\land
a∧,易得:
式
1
:
a
∧
a
∧
=
a
a
T
−
I
式
2
:
a
∧
a
∧
a
∧
=
a
∧
(
a
a
T
−
I
)
=
−
a
∧
\begin{aligned} 式1:\qquad&a^\land a^\land=aa^T-I\\ 式2:\qquad&a^\land a^\land a^\land=a^\land (aa^T-I)=-a^\land \end{aligned}
式1:式2:a∧a∧=aaT−Ia∧a∧a∧=a∧(aaT−I)=−a∧
指数映射
对指数映射进行Taylor展开并将上述等式代入:
e
x
p
(
ϕ
∧
)
=
e
x
p
(
θ
a
∧
)
=
∑
n
=
0
∞
1
n
!
(
θ
a
∧
)
n
1
:
=
I
+
θ
a
∧
+
1
2
!
θ
2
a
∧
a
∧
+
1
3
!
θ
3
a
∧
a
∧
a
∧
+
1
4
!
θ
4
(
a
∧
)
4
+
⋯
2
:
=
(
a
a
T
−
a
∧
a
∧
)
+
θ
a
∧
+
1
2
!
θ
2
a
∧
a
∧
−
1
3
!
θ
3
a
∧
−
1
4
!
θ
4
a
∧
a
∧
+
⋯
3
:
=
a
a
T
+
(
θ
−
1
3
!
θ
3
+
1
5
!
θ
5
−
⋯
)
a
∧
−
(
1
−
1
2
!
θ
2
+
1
4
!
θ
4
−
⋯
)
a
∧
a
∧
4
:
=
(
I
+
a
∧
a
∧
)
+
sin
θ
a
∧
−
cos
θ
a
∧
a
∧
5
:
=
(
1
−
cos
θ
)
a
∧
a
∧
+
I
+
sin
θ
a
∧
6
:
=
(
1
−
cos
θ
)
(
a
a
T
−
I
)
+
I
+
sin
θ
a
∧
7
:
=
cos
θ
I
+
(
1
−
cos
θ
)
a
a
T
+
sin
θ
a
∧
\begin{aligned} exp(\phi^\land)&=exp(\theta a^\land)=\sum^\infty_{n=0}\frac{1}{n!}(\theta a^\land)^n\\ 1:\qquad &=I+\theta a^\land + \frac{1}{2!}\theta^2a^\land a^\land +\frac{1}{3!}\theta^3 a^\land a^\land a^\land+\frac{1}{4!}\theta^4(a^\land)^4+\cdots\\ 2:\qquad &=(aa^T-a^\land a^\land)+\theta a^\land+\frac{1}{2!}\theta^2a^\land a^\land -\frac{1}{3!}\theta^3 a^\land-\frac{1}{4!}\theta^4a^\land a^\land+\cdots\\ 3:\qquad &=aa^T+\Bigl(\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5-\cdots\Bigr)a^\land-\Bigl(1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4-\cdots\Bigr)a^\land a^\land\\ 4:\qquad &=(I+a^\land a^\land) +\sin\theta a^\land -\cos\theta a^\land a^\land\\ 5:\qquad &=(1-\cos\theta)a^\land a^\land+I+\sin\theta a^\land\\ 6:\qquad &=(1-\cos\theta)(aa^T-I)+I+\sin\theta a^\land\\ 7:\qquad &=\cos\theta I+(1-\cos\theta)aa^T+\sin\theta a^\land \end{aligned}
exp(ϕ∧)1:2:3:4:5:6:7:=exp(θa∧)=n=0∑∞n!1(θa∧)n=I+θa∧+2!1θ2a∧a∧+3!1θ3a∧a∧a∧+4!1θ4(a∧)4+⋯=(aaT−a∧a∧)+θa∧+2!1θ2a∧a∧−3!1θ3a∧−4!1θ4a∧a∧+⋯=aaT+(θ−3!1θ3+5!1θ5−⋯)a∧−(1−2!1θ2+4!1θ4−⋯)a∧a∧=(I+a∧a∧)+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧=(1−cosθ)(aaT−I)+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧
上述计算过程的处理思路如下:
-
第一步:泰勒展开
-
第二步:将式1代入替换第一步中的 I I I;将式2代入替换 a ∧ a ∧ a ∧ a^\land a^\land a^\land a∧a∧a∧
-
第三步:对第二步进行整理
-
第四步:将式1代入替换第三步中的 a a T aa^T aaT;将三角函数的泰勒展开形式写回三角函数形式:
sin θ = θ − 1 3 ! θ 3 + 1 5 ! θ 5 − ⋯ cos θ = 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − ⋯ \begin{aligned} \sin\theta &=\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5-\cdots\\ \cos\theta &=1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4-\cdots \end{aligned} sinθcosθ=θ−3!1θ3+5!1θ5−⋯=1−2!1θ2+4!1θ4−⋯ -
第五步:对第四步进行整理
-
第六步:将式1代入替换第五步中 a ∧ a ∧ a^\land a^\land a∧a∧
-
第七步:对第六步进行整理
最终得到李代数
s
o
(
3
)
so(3)
so(3)到李群
S
O
(
3
)
SO(3)
SO(3)的指数映射:
e
x
p
(
ϕ
∧
)
=
e
x
p
(
θ
a
∧
)
=
cos
θ
I
+
(
1
−
cos
θ
)
a
a
T
+
sin
θ
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∧
该式被称为罗德里格斯公式,李代数
s
o
(
3
)
so(3)
so(3)的物理意义就是旋转向量。
对数映射
由
S
O
(
3
)
SO(3)
SO(3)到
s
o
(
3
)
so(3)
so(3)的对数映射如下:
ϕ
=
ln
(
R
)
∨
=
(
∑
n
=
0
∞
(
−
1
)
n
n
+
1
(
R
−
I
)
n
+
1
)
∨
\phi=\ln(R)^\lor=\Biggl(\sum_{n=0}^\infty\frac{(-1)^{n}}{n+1}(R-I)^{n+1}\Biggr)^\lor
ϕ=ln(R)∨=(n=0∑∞n+1(−1)n(R−I)n+1)∨
对数映射实现由旋转矩阵到旋转向量的转换,可使用刚体变换中计算方式,对指数映射两侧求迹从而简化计算:
θ
=
arccos
t
r
(
R
)
−
1
2
R
n
=
n
\theta=\arccos\frac{tr(R)-1}{2}\qquad\qquad\qquad\qquad Rn=n
θ=arccos2tr(R)−1Rn=n
注意点
指数映射是一个满射,不是一个单射。也即每个 S O ( 3 ) SO(3) SO(3)内的元素,都可以找到一个 s o ( 3 ) so(3) so(3)元素与之一一对应;但可能存在多个 s o ( 3 ) so(3) so(3)中的元素,对应到一个 S O ( 3 ) SO(3) SO(3)中元素。
若将旋转角度固定至 ± π \pm\pi ±π中,则可认为李群、李代数中元素一一对应。
李群 S E ( 3 ) SE(3) SE(3)李代数 s e ( 3 ) se(3) se(3)
指数映射
由李代数
s
e
(
3
)
se(3)
se(3)到李群
S
E
(
3
)
SE(3)
SE(3)的转换如下:
e
x
p
(
ξ
∧
)
=
[
∑
n
=
0
∞
1
n
!
(
ϕ
∧
)
n
∑
n
=
0
∞
1
(
n
+
1
)
!
(
ϕ
∧
)
n
ρ
0
T
1
]
≜
[
R
J
ρ
0
T
1
]
=
T
\begin{aligned} exp(\xi^\land)&=\begin{bmatrix}\sum^\infty_{n=0}\frac{1}{n!}(\phi^\land)^n&\sum_{n=0}^\infty\frac{1}{(n+1)!}(\phi^\land)^n\rho\\0^T&1\end{bmatrix}\\ &\triangleq \begin{bmatrix}R&J\rho\\0^T&1\end{bmatrix}\\ &=T \end{aligned}
exp(ξ∧)=[∑n=0∞n!1(ϕ∧)n0T∑n=0∞(n+1)!1(ϕ∧)nρ1]≜[R0TJρ1]=T
式中
J
J
J称为雅可比矩阵:
J
=
∑
n
=
0
∞
1
(
n
+
1
)
!
(
ϕ
∧
)
n
=
∑
n
=
0
∞
1
(
n
+
1
)
!
(
θ
a
∧
)
n
1
:
=
I
+
1
2
!
θ
a
∧
+
1
3
!
θ
2
(
a
∧
)
2
+
1
4
!
θ
3
(
a
∧
)
3
+
1
5
!
θ
4
(
a
∧
)
4
+
⋯
2
:
=
I
+
1
θ
(
1
2
!
θ
2
a
∧
+
1
3
!
θ
3
(
a
∧
)
2
+
1
4
!
θ
4
(
a
∧
)
3
+
1
5
!
θ
5
(
a
∧
)
4
+
⋯
)
3
:
=
I
+
1
θ
(
1
2
!
θ
2
a
∧
+
1
3
!
θ
3
(
a
∧
)
2
−
1
4
!
θ
4
a
∧
−
1
5
!
θ
5
(
a
∧
)
2
+
⋯
)
4
:
=
I
+
1
θ
(
1
2
!
θ
2
−
1
4
!
θ
4
+
⋯
)
a
∧
+
1
θ
(
1
3
!
θ
3
−
1
5
!
θ
5
+
⋯
)
(
a
∧
)
2
5
:
=
I
+
1
θ
(
1
−
cos
θ
)
a
∧
+
1
θ
(
θ
−
sin
θ
)
(
a
∧
)
2
6
:
=
I
+
1
θ
(
1
−
cos
θ
)
a
∧
+
1
θ
(
θ
−
sin
θ
)
(
a
a
T
−
I
)
7
:
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
∧
\begin{aligned} J=&\sum_{n=0}^\infty\frac{1}{(n+1)!}(\phi^\land)^n=\sum_{n=0}^\infty\frac{1}{(n+1)!}(\theta a^\land)^n\\ 1:\qquad =&I+\frac{1}{2!}\theta a^\land+\frac{1}{3!}\theta^2 (a^\land)^2+\frac{1}{4!}\theta^3 (a^\land)^3+\frac{1}{5!}\theta^4 (a^\land)^4+\cdots\\ 2:\qquad =&I+\frac{1}{\theta}\Bigl(\frac{1}{2!}\theta^2 a^\land+\frac{1}{3!}\theta^3 (a^\land)^2+\frac{1}{4!}\theta^4 (a^\land)^3+\frac{1}{5!}\theta^5 (a^\land)^4+\cdots\Bigr) \\ 3:\qquad =&I+\frac{1}{\theta}\Bigl(\frac{1}{2!}\theta^2 a^\land+\frac{1}{3!}\theta^3 (a^\land)^2-\frac{1}{4!}\theta^4 a^\land-\frac{1}{5!}\theta^5 (a^\land)^2+\cdots\Bigr)\\ 4:\qquad =& I+\frac{1}{\theta}\Bigl(\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+\cdots\Bigr)a^\land + \frac{1}{\theta}\Bigl(\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5+\cdots\Bigr)(a^\land)^2 \\ 5:\qquad =& I+\frac{1}{\theta}(1-\cos\theta)a^\land+\frac{1}{\theta}(\theta-\sin\theta)(a^\land)^2\\ 6:\qquad =& I+\frac{1}{\theta}(1-\cos\theta)a^\land+\frac{1}{\theta}(\theta-\sin\theta)(aa^T-I)\\ 7:\qquad =& \frac{\sin\theta}{\theta}I+\Bigl(1-\frac{\sin\theta}{\theta}\Bigr)aa^T+\frac{1-\cos\theta}{\theta}a^\land\\ \end{aligned}
J=1:=2:=3:=4:=5:=6:=7:=n=0∑∞(n+1)!1(ϕ∧)n=n=0∑∞(n+1)!1(θa∧)nI+2!1θa∧+3!1θ2(a∧)2+4!1θ3(a∧)3+5!1θ4(a∧)4+⋯I+θ1(2!1θ2a∧+3!1θ3(a∧)2+4!1θ4(a∧)3+5!1θ5(a∧)4+⋯)I+θ1(2!1θ2a∧+3!1θ3(a∧)2−4!1θ4a∧−5!1θ5(a∧)2+⋯)I+θ1(2!1θ2−4!1θ4+⋯)a∧+θ1(3!1θ3−5!1θ5+⋯)(a∧)2I+θ1(1−cosθ)a∧+θ1(θ−sinθ)(a∧)2I+θ1(1−cosθ)a∧+θ1(θ−sinθ)(aaT−I)θsinθI+(1−θsinθ)aaT+θ1−cosθa∧
上述计算过程的处理思路如下:
-
第一步:泰勒展开
-
第二步:除单位阵 I I I外其余项提出一个因子 1 θ \frac{1}{\theta} θ1
-
第三步:将式2代入代替 ( a ∧ ) 3 (a^\land)^3 (a∧)3
-
第四步:对第三步进行整理,将有共同项的写在一起
-
第五步:将三角函数的泰勒展开进行整理后代入上式:
cos θ = 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − ⋯ 1 − cos θ = 1 2 ! θ 2 − 1 4 ! θ 4 + ⋯ sin θ = θ − 1 3 ! θ 3 + 1 5 ! θ 5 − ⋯ θ − sin θ = 1 3 ! θ 3 − 1 5 ! θ 5 + ⋯ \begin{aligned} \cos\theta &=1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4-\cdots\\ 1-\cos\theta&=\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+\cdots\\ \\ \sin\theta &=\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5-\cdots\\ \theta-\sin\theta&=\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5+\cdots \end{aligned} cosθ1−cosθsinθθ−sinθ=1−2!1θ2+4!1θ4−⋯=2!1θ2−4!1θ4+⋯=θ−3!1θ3+5!1θ5−⋯=3!1θ3−5!1θ5+⋯ -
第六步:将式1代入代替 ( a ∧ ) 2 (a^\land)^2 (a∧)2
-
第七步:对第六步进行整理
由此得到雅可比矩阵的表达式:
J
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
∧
J=\frac{\sin\theta}{\theta}I+\Bigl(1-\frac{\sin\theta}{\theta}\Bigr)aa^T+\frac{1-\cos\theta}{\theta}a^\land
J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧
对数映射
从
S
E
(
3
)
SE(3)
SE(3)到
s
e
(
3
)
se(3)
se(3)的转换,如下:
ξ
=
ln
(
T
)
∨
\xi=\ln(T)^\lor
ξ=ln(T)∨
在进行实际转换时,使用对数映射较为复杂。一般用
T
T
T左上角旋转矩阵
R
R
R求解旋转向量;再由右侧平移向量计算平移部分:
t
=
J
ρ
t=J\rho
t=Jρ