https://www.cnblogs.com/gaoxiang12/p/5137454.html
视觉SLAM中的数学基础 第三篇 李群与李代数
前言
在SLAM中,除了表达3D旋转与位移之外,我们还要对它们进行估计,因为SLAM整个过程就是在不断地估计机器人的位姿与地图。为了做这件事,需要对变换矩阵进行插值、求导、迭代等操作。例如,在经典ICP问题中,给定了两组3D点,我们要计算它们之间的变换矩阵。假设第一组的3D点为P={pi|i=[1,2,…,N]}P={pi|i=[1,2,…,N]},第二组3D点为Q={qi|i=[1,2,…,N]}Q={qi|i=[1,2,…,N]},那我们实际要做的事情是求一个欧氏变换TT,使得TT满足:
∀i,qi=Tpi(1)(1)∀i,qi=Tpi
注意这里使用了齐次坐标表示。通常,这许多个匹配过的点是通过特征匹配得到的,构成了一个超定方程。而由于噪声的存在,这个方程往往是无解的。因此我们转而计算一个最小二乘:
minTu(T)=∑i=1N‖‖qi−Tpi‖‖2(2)(2)minTu(T)=∑i=1N‖qi−Tpi‖2
这时问题就来了:如果用迭代方式求解这个优化时(尽管可以不用迭代方式来求),如何求目标函数uu相对于TT的导数呢?首先,TT只有6 个自由度,最好能够在一个六维空间表达它,那么u(T)u(T)相对于这个六维空间的导数(雅可比矩阵)是一个6×66×6的矩阵。其次,TT对于乘法是封闭的,但对加法不封闭,即任意两个变换矩阵相加后并不是一个变换矩阵,这主要是因为旋转矩阵对加法是不封闭的。
出于这两个原因,我们希望有更好的数学工具帮助我们做这些事,而李群与李代数理论正好提供了这样的工具。李群与李代数广泛地用于机器人与计算机视觉领域,并在机器人动力学推导上占据重要地位。不过,由于SLAM不涉及过多的动力学推导。我们重点介绍它在SLAM中相关的几个重要的结果,而略去许多数学性质的证明。特别地,重点介绍SO(3)SO(3)和SE(3)SE(3)这两个李群与对应的李代数。
李代数基础
首先,我们来讨论较为简单的三维旋转群。为了说明它的结构,首先介绍群的概念。
群
群(Group)是一种集合加上一种运算的代数结构,记作(A,⋅)(A,⋅)。其中AA代表集合,⋅⋅是定义在该集合上的二元运算。那么,如果这个运算满足以下几个条件,则称G=(A,⋅)G=(A,⋅)为群。
- 封闭性: ∀a1,a2,a1⋅a2∈A∀a1,a2,a1⋅a2∈A
- 结合律: ∀a1,a2,a3,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)∀a1,a2,a3,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)
- 幺元: ∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=a∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=a
- 逆: ∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0
读者可以记作“封结幺逆”(谐音凤姐咬你),并可以把一些常见的群放进去验证。例如整数的加法(幺元为0),去掉0后的有理数的乘法(幺元为1)。对于矩阵,可以找到一些常见的矩阵群,例如:
- 一般线性群GL(n)GL(n) 指n×nn×n的可逆矩阵,它们对矩阵乘法成群。
- 特殊正交群SO(n)SO(n) 也就是所谓的旋转矩阵群,其中SO(2)SO(2) 和SO(3)SO(3)最为常见。正式的记法是:
SO(n)={R∈ℝn×n|RRT=I,det(R)=1}(3)(3)SO(n)={R∈Rn×n|RRT=I,det(R)=1}
- 特殊欧氏群SE(n)SE(n) 也就是前面提到的nn维欧氏变换,如SE(2)SE(2)和SE(3)SE(3)。这里给出SE(3)SE(3)的记法:
SE(3)={T=[R0Tt1]∈ℝ4×4|R∈SO(3),t∈ℝ3}(4)(4)SE(3)={T=[Rt0T1]∈R4×4|R∈SO(3),t∈R3}
群结构保证了在群上的运算具有良好的性质,而群论则研究群的各种结构和性质,但我们在此不多加介绍。感兴趣的读者可以参考任意一本近世代数教材。
李群是指具有连续性质的群。并且,一般连续群上的运算还是无限可微,乃至解析的(解析比无限可微更强,它还要求任意点邻域的泰勒展开都收敛)。这个问题在20世纪初被称为希尔伯特第五问题,并已得到了解决。而李群,则指实数空间上的连续群。常见的李群包括上边提到的GL(n),SO(n),SE(n)GL(n),SO(n),SE(n),以及其他的如酉群U(n)U(n),辛群Sp(2n)Sp(2n)等等。
三维旋转群SO(3)SO(3)
三维旋转群SO(3)SO(3)是特殊正交群SO(n)SO(n)在n=3n=3时的特例,它们可以用来描述三维空间的旋转,其元素都是3×33×3 的正交且行列式为+1+1的矩阵。假设有这样一个矩阵RR,满足RRT=IRRT=I。现在,考虑它随时间发生变化,即从RR 变成了R(t)R(t),仍有R(t)R(t)T=IR(t)R(t)T=I。在等式两边对时间求导,得到:
R˙(t)R(t)T+R(t)R˙(t)T=0(5)(5)R˙(t)R(t)T+R(t)R˙(t)T=0
于是:
R˙(t)R(t)T=−(R˙(t)R(t)T)T(6)(6)R˙(t)R(t)T=−(R˙(t)R(t)T)T
可以看出R˙(t)R(t)TR˙(t)R(t)T是一个反对称矩阵。注意到对于任意一个3×33×3的反对称矩阵,我们记它为AA。由于AT=−AAT=−A,所以它主对角线元素必为00,而非对角线元素则只有三个自由度。我们可以把它对应到一个向量a=[a1,a2,a3]Ta=[a1,a2,a3]T中去:
a∧=A=⎡⎣⎢⎢0a3−a2−a30a1a2−a10⎤⎦⎥⎥(7)(7)a∧=A=[0−a3a2a30−a1−a2a10]
其中∧∧符号表示由向量转换为矩阵,反之我们也可以用符号∨∨定义由矩阵转换为向量的方式:
A∨=a(8)(8)A∨=a
注意到这样定义的好处之一,是它与叉积的兼容性。我们可以直接把矩阵与任意向量的乘积AbAb写成 a×ba×b。读者可以自行验证这个兼容性。除此之外,这样定义的向量还有一些较好的性质,后文会提到。
现在,由于R˙(t)R(t)TR˙(t)R(t)T是一个反对称矩阵,我们可以找到一个三维向量ϕ(t)∈ℝ3ϕ(t)∈R3与之对应。于是有:
R˙(t)R(t)T=ϕ(t)∧(9)(9)R˙(t)R(t)T=ϕ(t)∧
左右各右乘R(t)R(t),由于RR为正交阵,有:
R˙(t)=ϕ(t)∧R(t)=⎡⎣⎢⎢0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎤⎦⎥⎥R(t)(10)(10)R˙(t)=ϕ(t)∧R(t)=[0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10]R(t)
可以看到,每对旋转矩阵求一次导数,只需左乘一个ϕϕ矩阵即可。由于ϕϕ反映了RR的导数性质,故称它在SO(3)SO(3)的正切空间(tangent space)上。同时,将上式类比于一个关于RR的微分方程,可得:
R(t)=exp(ϕ(t)∧)R(t0)(11)(11)R(t)=exp(ϕ(t)∧)R(t0)
由此我们可以引出两个概念。(1)求ϕϕ的方法以及它的结构?——ϕϕ是对应到SO(3)SO(3)上的李代数??(3)so(3);(2)exp(ϕ)exp(ϕ)如何计算?——李群与李代数间的指数/对数映射。下面我们一一加以介绍。
什么是李代数
对于SO(3)SO(3)和SE(3)SE(3),李代数可定义于李群的正切空间上,描述了李群中元素局部性质,分别把它们记作小写的??(3)so(3)和??(3)se(3)。首先,给出通用的李代数的定义。
李代数由一个集合?V,一个数域?F和一个二元运算[][]组成。如果它们满足以下几条性质,称(?,?,[])(V,F,[]) 为一个李代数,记作?g。
- 封闭性 ∀X,Y∈?,[XY]∈?∀X,Y∈V,[XY]∈V
- 双线性 ∀X,Y,Z∈?,a,b∈?,∀X,Y,Z∈V,a,b∈F, 有
[aX+bY,Z]=a[XZ]+b[YZ][Z,aX+bY]=a[ZX]+b[ZY][aX+bY,Z]=a[XZ]+b[YZ][Z,aX+bY]=a[ZX]+b[ZY]
- 自反性 ∀X∈?,[XX]=0∀X∈V,[XX]=0
- 雅可比等价 ∀X,Y,Z∈?,[X,[YZ]]+[Z,[YX]]+[Y,[ZX]]∀X,Y,Z∈V,[X,[YZ]]+[Z,[YX]]+[Y,[ZX]]
从表面上来看,李代数所需要的性质还是挺多的。其中二元运算被称为李括号。相比于群中的较为简单的二元运算,李括号表达了两个集合元素的差异。它不要求结合律,而满足反对称性,以及元素和自己做李括号之后为零的性质。作为类比,三维向量ℝ3R3 上定义的叉积××是一种李括号,因此?=(ℝ3,ℝ,×)g=(R3,R,×)构成了一个李代数。读者可以尝试将叉积的性质代入到上面四条性质中。
三维旋转群与对应的李代数
SO(3)SO(3)对应的李代数是定义在ℝ3R3上的向量,我们记作ϕϕ(注意这是个向量,虽然希腊字母的粗体不明显)。根据前面的推导,每个ϕϕ都可以生成一个反对称矩阵:
Φ=ϕ∧=⎡⎣⎢⎢0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎤⎦⎥⎥∈ℝ3×3(12)(12)Φ=ϕ∧=[0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10]∈R3×3
在此定义下,两个向量ϕ1,ϕ2ϕ1,ϕ2的李括号为:
[ϕ1,ϕ2]=Φ1Φ2−Φ2Φ1(13)(13)[ϕ1,ϕ2]=Φ1Φ2−Φ2Φ1
读者可以去验证该定义下的李括号满足上面的几条性质。由于ϕϕ 与反对称矩阵关系很紧密,在不引起歧义的情况下,就说??(3)so(3)的元素是3维向量或者3维反对称矩阵,不加区别:
??(3)={Φ=ϕ∧∈ℝ3×3|ϕ∈ℝ3}(14)(14)so(3)={Φ=ϕ∧∈R3×3|ϕ∈R3}
反对称矩阵有一些重要的性质,重点包括以下两条:
ϕϕT=ϕ∧ϕ∧+‖ϕ‖2I3×3(15)(15)ϕϕT=ϕ∧ϕ∧+‖ϕ‖2I3×3
当ϕϕ为单位向量时,进而有:
ϕϕT=ϕ∧ϕ∧+I1(16)(16)ϕϕT=ϕ∧ϕ∧+I1
以及
ϕ∧ϕ∧ϕ∧=−ϕ∧(17)(17)ϕ∧ϕ∧ϕ∧=−ϕ∧
这两条性质读者也可以自行验证,我们在指数映射中会用到。
至此,我们已清楚了??(3)so(3)的结构。它们是一个由三维向量组成的集合,每个向量对应到一个反对称矩阵,可以表达旋转矩阵的导数。现在来考虑exp(ϕ∧)exp(ϕ∧)是如何计算的,为此我们引入指数映射。
指数映射
首先,回忆任意矩阵的指数映射。它可以写成一个泰勒展开,但是只有在收敛的情况下才会有结果,其结果仍是一个矩阵。
exp(A)=∑n=0∞1n!An(18)(18)exp(A)=∑n=0∞1n!An
同样地,对??(3)so(3)中任意一元素ϕϕ,我们亦可按此方式定义它的指数映射:
exp(ϕ∧)=∑n=0∞1n!(ϕ∧)n(19)(19)exp(ϕ∧)=∑n=0∞1n!(ϕ∧)n
现在我们来仔细看看它的含义。由于ϕϕ是三维向量,我们可以定义它的模长和它的方向,分别记作θθ和aa(注意这里记号是有含义的,此时aa是一个单位长度的向量),那么按照上式,可以推出如下公式,注意中间使用了上面讲到了两个反对称矩阵的性质:
exp(ϕ∧)=exp(θa∧)=∑n=0∞1n!(θa∧)n=I+θa∧+12!θ2a∧a∧+13!θ3a∧a∧a∧+14!θ4(a∧)4+...=aaT−a∧a∧+θa∧+12!θa∧a∧−13!θ3a∧+14!θ4(a∧)4+...=aaT+(θ−13!θ3+15!θ5−...)a∧−(1−12!θ2+14!θ4−...)a∧a∧=a∧a∧+I+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧exp(ϕ∧)=exp(θa∧)=∑n=0∞1n!(θa∧)n=I+θa∧+12!θ2a∧a∧+13!θ3a∧a∧a∧+14!θ4(a∧)4+...=aaT−a∧a∧+θa∧+12!θa∧a∧−13!θ3a∧+14!θ4(a∧)4+...=aaT+(θ−13!θ3+15!θ5−...)a∧−(1−12!θ2+14!θ4−...)a∧a∧=a∧a∧+I+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧
最后我们得到了一个似曾相识的式子:
exp(θa)=cosθI+(1−cosθ)aaT+sinθa∧(20)(20)exp(θa)=cosθI+(1−cosθ)aaT+sinθa∧
回忆前一节内容,它和罗德里格斯公式(参观本系列第一篇)如出一辄。这表明,??(3)so(3)实际上就是由所谓的旋转向量组成的空间。特别地,当转轴取一定顺序时,李代数??(3)so(3)还会变为对应的欧拉角。通过罗德里格斯公式或者指数映射,我们把ℝ3R3中的一个向量对应到了一个位于SO(3)SO(3)中的3D旋转。
反之,如果定义对数映射,我们也能把SO(3)SO(3)中的元素对应到??(3)so(3)中:
ϕ=ln(R)∨=(∑n=0∞(−1)nn+1(R−I)n+1)∨(21)(21)ϕ=ln(R)∨=(∑n=0∞(−1)nn+1(R−I)n+1)∨
其中∨∨表示从反对称矩阵到向量的对应关系,为∧∧的逆运算。
读者可能会问,指数映射性质如何呢?它是一个双射吗?很遗憾,它只是一个满射。每个SO(3)SO(3)中的元素,都可以找到??(3)so(3)中至少一个与之对应;但是可能存在多个??(3)so(3)中的元素,对应到同一个SO(3)SO(3)元素上。至少对于旋转角θθ,我们知道它具有周期性。
SO(3)SO(3)与??(3)so(3)的结论似乎在我们意料之中。它和我们前面讲的旋转向量与旋转矩阵很相似,而指数映射即是罗德里格斯公式。旋转向量可以视为旋转矩阵的导数,指导如何在旋转矩阵中进行微积分运算。
三维欧氏群与对应的李代数
下面我们来介绍三维欧氏群SE(3)SE(3)以及对应的李代数??(3)se(3)。有了前面的基础,我们可以直接介绍它们的结构及运算了。SE(3)SE(3)的结构已经在前面介绍群的时候给出:
SE(3)={T=[R0Tt1]∈ℝ4×4|R∈SO(3),t∈ℝ3}(22)(22)SE(3)={T=[Rt0T1]∈R4×4|R∈SO(3),t∈R3}
每个变换矩阵有六个四由度,故对应的李代数位于ℝ6R6中:
??(3)={Ξ=ξ∧∈ℝ4×4|ξ∈ℝ6}(23)(23)se(3)={Ξ=ξ∧∈R4×4|ξ∈R6}
但是∧∧不再对应到一个反对称关系,而是:
ξ∧=[ρϕ]∧=[ϕ∧0Tρ0]=Ξ(24)(24)ξ∧=[ρϕ]∧=[ϕ∧ρ0T0]=Ξ
可以看到,ξξ 的前三维为旋转向量,后三维为平移向量,其定义也十分的直观。该李代数对应于微分方程:
T˙(t)=ξ∧(t)T(t)(25)(25)T˙(t)=ξ∧(t)T(t)
因此
T(t)=exp(ξ(t)∧)T(t)(26)(26)T(t)=exp(ξ(t)∧)T(t)
那么??(3)se(3)上的指数映射如何呢?略加推导可得:
exp(ξ∧)=⎡⎣⎢⎢⎢∑n=0∞1n!(ϕ∧)n0T∑n=0∞1(n+1)!(ϕ∧)nρ1⎤⎦⎥⎥⎥=[Φ0TJρ1](27)(28)(27)exp(ξ∧)=[∑n=0∞1n!(ϕ∧)n∑n=0∞1(n+1)!(ϕ∧)nρ0T1](28)=[ΦJρ0T1]
左上角的ΦΦ是我们熟知的??(3)so(3)中的元素,前文已经介绍过了。而右上角的JJ则可整理为(设ϕ=θaϕ=θa):
J=sinθθI+(1−sinθθ)aaT+1−cosθθa∧(29)(29)J=sinθθI+(1−sinθθ)aaT+1−cosθθa∧
因此我们就得到了??(3)se(3)的指数映射的关系。 其对数映射亦可类比推得。
小结
最后,我们对之前介绍的李群李代数进行一个简单的小结。概而言之,李群有以下两个重要用处:
- 李代数表达的正切空间,具有和对应李群相同的自由度。
- 指数映射能把正切空间中任意向量正好映射到原李群。
下篇中,我们将教大家用Eigen和Sophus库处理变换矩阵与李代数。敬请期待。
参考资料
[1]. Yi Ma, An Invitation to 3D Vision. 2001.
[2]. Timothy D. Barfoot, State Estimation for Robotics: A Matrix-Lie-Group Approach, 2015.