视觉SLAM中的数学基础第三篇李群与李代数

转自：http://www.cnblogs.com/gaoxiang12/p/5137454.html

致敬原作者，请移步原作者博文详细浏览

前言

　　在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]}，那我们实际要做的事情是求一个欧氏变换 T T，使得 T T满足：

∀i,qi=Tpi(1) (1)∀i,qi=Tpi

　　注意这里使用了齐次坐标表示。通常，这许多个匹配过的点是通过特征匹配得到的，构成了一个超定方程。而由于噪声的存在，这个方程往往是无解的。因此我们转而计算一个最小二乘：

minTu(T)=∑i=1N∥qi−Tpi∥2(2) (2)minT⁡u(T)=∑i=1N∥qi−Tpi∥2

　　这时问题就来了：如果用迭代方式求解这个优化时（尽管可以不用迭代方式来求），如何求目标函数 u u相对于 T T的导数呢？首先， T T只有6 个自由度，最好能够在一个六维空间表达它，那么 u(T) u(T)相对于这个六维空间的导数（雅可比矩阵）是一个 6×6 6×6的矩阵。其次， T T对于乘法是封闭的，但对加法不封闭，即任意两个变换矩阵相加后并不是一个变换矩阵，这主要是因为旋转矩阵对加法是不封闭的。

　　出于这两个原因，我们希望有更好的数学工具帮助我们做这些事，而李群与李代数理论正好提供了这样的工具。李群与李代数广泛地用于机器人与计算机视觉领域，并在机器人动力学推导上占据重要地位。不过，由于SLAM不涉及过多的动力学推导。我们重点介绍它在SLAM中相关的几个重要的结果，而略去许多数学性质的证明。特别地，重点介绍 SO(3) SO(3)和 SE(3) SE(3)这两个李群与对应的李代数。

李代数基础

　　首先，我们来讨论较为简单的三维旋转群。为了说明它的结构，首先介绍群的概念。

群

　　群（Group）是一种集合加上一种运算的代数结构，记作 (A,⋅) (A,⋅)。其中 A A代表集合， ⋅ ⋅是定义在该集合上的二元运算。那么，如果这个运算满足以下几个条件，则称 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×n n×n的可逆矩阵，它们对矩阵乘法成群。
特殊正交群 SO(n) SO(n) 也就是所谓的旋转矩阵群，其中 SO(2) SO(2) 和 SO(3) SO(3)最为常见。正式的记法是：

SO(n)={R∈Rn×n|RRT=I,det(R)=1}(3) (3)SO(n)={R∈Rn×n|RRT=I,det(R)=1}

特殊欧氏群 SE(n) SE(n) 也就是前面提到的 n n维欧氏变换，如 SE(2) SE(2)和 SE(3) SE(3)。这里给出 SE(3) SE(3)的记法：

SE(3)={T=[R0Tt1]∈R4×4|R∈SO(3),t∈R3}(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=3 n=3时的特例，它们可以用来描述三维空间的旋转，其元素都是 3×3 3×3 的正交且行列式为 +1 +1的矩阵。假设有这样一个矩阵 R R，满足 RRT＝I RRT＝I。现在，考虑它随时间发生变化，即从 R R 变成了 R(t) R(t)，仍有 R(t)R(t)T=I R(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)T R˙(t)R(t)T是一个反对称矩阵。注意到对于任意一个 3×3 3×3的反对称矩阵，我们记它为 A A。由于 AT=−A AT=−A，所以它主对角线元素必为 0 0，而非对角线元素则只有三个自由度。我们可以把它对应到一个向量 a=[a1,a2,a3]T a=[a1,a2,a3]T中去：

a∧=A=⎡⎣⎢0a3−a2−a30a1a2−a10⎤⎦⎥(7) (7)a∧=A=[0−a3a2a30−a1−a2a10]

　　其中 ∧ ∧符号表示由向量转换为矩阵，反之我们也可以用符号 ∨ ∨定义由矩阵转换为向量的方式：

A∨=a(8) (8)A∨=a

　　注意到这样定义的好处之一，是它与叉积的兼容性。我们可以直接把矩阵与任意向量的乘积 Ab Ab写成 a×b a×b。读者可以自行验证这个兼容性。除此之外，这样定义的向量还有一些较好的性质，后文会提到。

　　现在，由于 R˙(t)R(t)T R˙(t)R(t)T是一个反对称矩阵，我们可以找到一个三维向量 ϕ(t)∈R3 ϕ(t)∈R3与之对应。于是有：

R˙(t)R(t)T=ϕ(t)∧(9) (9)R˙(t)R(t)T=ϕ(t)∧

　　左右各右乘 R(t) R(t)，由于 R R为正交阵，有：

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)

　　可以看到，每对旋转矩阵求一次导数，只需左乘一个 ϕ ϕ矩阵即可。由于 ϕ ϕ反映了 R R的导数性质，故称它在 SO(3) SO(3)的正切空间(tangent space)上。同时，将上式类比于一个关于 R R的微分方程，可得：

R(t)=exp(ϕ(t)∧)R(t0)(11) (11)R(t)=exp⁡(ϕ(t)∧)R(t0)

　　由此我们可以引出两个概念。（1）求 ϕ ϕ的方法以及它的结构？—— ϕ ϕ是对应到 SO(3) SO(3)上的李代数 so(3) so(3)；（2） exp(ϕ) exp⁡(ϕ)如何计算？——李群与李代数间的指数/对数映射。下面我们一一加以介绍。

什么是李代数

　　对于 SO(3) SO(3)和 SE(3) SE(3)，李代数可定义于李群的正切空间上，描述了李群中元素局部性质，分别把它们记作小写的 so(3) so(3)和 se(3) se(3)。首先，给出通用的李代数的定义。

　　李代数由一个集合 V V，一个数域 F F和一个二元运算 [] []组成。如果它们满足以下几条性质，称 (V,F,[]) (V,F,[]) 为一个李代数，记作 g g。

封闭性 ∀X,Y∈V,[XY]∈V ∀X,Y∈V,[XY]∈V
双线性 ∀X,Y,Z∈V,a,b∈F, ∀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∈V,[XX]=0 ∀X∈V,[XX]=0
雅可比等价 ∀X,Y,Z∈V,[X,[YZ]]+[Z,[YX]]+[Y,[ZX]] ∀X,Y,Z∈V,[X,[YZ]]+[Z,[YX]]+[Y,[ZX]]

　　从表面上来看，李代数所需要的性质还是挺多的。其中二元运算被称为李括号。相比于群中的较为简单的二元运算，李括号表达了两个集合元素的差异。它不要求结合律，而满足反对称性，以及元素和自己做李括号之后为零的性质。作为类比，三维向量 R3 R3 上定义的叉积 × ×是一种李括号，因此 g=(R3,R,×) g=(R3,R,×)构成了一个李代数。读者可以尝试将叉积的性质代入到上面四条性质中。

三维旋转群与对应的李代数
　　 SO(3) SO(3)对应的李代数是定义在 R3 R3上的向量，我们记作 ϕ ϕ（注意这是个向量，虽然希腊字母的粗体不明显）。根据前面的推导，每个 ϕ ϕ都可以生成一个反对称矩阵：

Φ=ϕ∧=⎡⎣⎢0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎤⎦⎥∈R3×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

　　读者可以去验证该定义下的李括号满足上面的几条性质。由于 ϕ ϕ 与反对称矩阵关系很紧密，在不引起歧义的情况下，就说 so(3) so(3)的元素是3维向量或者3维反对称矩阵，不加区别：

so(3)={Φ=ϕ∧∈R3×3|ϕ∈R3}(14) (14)so(3)={Φ=ϕ∧∈R3×3|ϕ∈R3}

　　反对称矩阵有一些重要的性质，重点包括以下两条：

ϕϕT=ϕ∧ϕ∧+∥ϕ∥2I3×3(15) (15)ϕϕT=ϕ∧ϕ∧+∥ϕ∥2I3×3

　　当 ϕ ϕ为单位向量时，进而有：

ϕϕT=ϕ∧ϕ∧+I1(16) (16)ϕϕT=ϕ∧ϕ∧+I1

　　以及

ϕ∧ϕ∧ϕ∧=−ϕ∧(17) (17)ϕ∧ϕ∧ϕ∧=−ϕ∧

　　这两条性质读者也可以自行验证，我们在指数映射中会用到。

　　至此，我们已清楚了 so(3) so(3)的结构。它们是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。现在来考虑 exp(ϕ∧) exp⁡(ϕ∧)是如何计算的，为此我们引入指数映射。

指数映射

　　首先，回忆任意矩阵的指数映射。它可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵。

exp(A)=∑n=0∞1n!An(18) (18)exp⁡(A)=∑n=0∞1n!An

　　同样地，对 so(3) so(3)中任意一元素 ϕ ϕ，我们亦可按此方式定义它的指数映射：

exp(ϕ∧)=∑n=0∞1n!(ϕ∧)n(19) (19)exp⁡(ϕ∧)=∑n=0∞1n!(ϕ∧)n

　　现在我们来仔细看看它的含义。由于 ϕ ϕ是三维向量，我们可以定义它的模长和它的方向，分别记作 θ θ和 a a（注意这里记号是有含义的，此时 a 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⁡(ϕ∧)=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∧

　　回忆前一节内容，它和罗德里格斯公式（参观本系列第一篇）如出一辄。这表明， so(3) so(3)实际上就是由所谓的旋转向量组成的空间。特别地，当转轴取一定顺序时，李代数 so(3) so(3)还会变为对应的欧拉角。通过罗德里格斯公式或者指数映射，我们把 R3 R3 中的一个向量对应到了一个位于 SO(3) SO(3)中的3D旋转。

　　反之，如果定义对数映射，我们也能把 SO(3) SO(3)中的元素对应到 so(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)中的元素，都可以找到 so(3) so(3)中至少一个与之对应；但是可能存在多个 so(3) so(3)中的元素，对应到同一个 SO(3) SO(3)元素上。至少对于旋转角 θ θ，我们知道它具有周期性。

　　 SO(3) SO(3)与 so(3) so(3)的结论似乎在我们意料之中。它和我们前面讲的旋转向量与旋转矩阵很相似，而指数映射即是罗德里格斯公式。旋转向量可以视为旋转矩阵的导数，指导如何在旋转矩阵中进行微积分运算。

三维欧氏群与对应的李代数

　　下面我们来介绍三维欧氏群 SE(3) SE(3)以及对应的李代数 se(3) se(3)。有了前面的基础，我们可以直接介绍它们的结构及运算了。 SE(3) SE(3)的结构已经在前面介绍群的时候给出：

SE(3)={T=[R0Tt1]∈R4×4|R∈SO(3),t∈R3}(22) (22)SE(3)={T=[Rt0T1]∈R4×4|R∈SO(3),t∈R3}

　　每个变换矩阵有六个四由度，故对应的李代数位于 R6 R6中：

se(3)={Ξ=ξ∧∈R4×4|ξ∈R6}(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)

　　那么 se(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]

　　左上角的 Φ Φ是我们熟知的 so(3) so(3)中的元素，前文已经介绍过了。而右上角的 J J则可整理为（设 ϕ=θa ϕ=θa）：

J=sinθθI+(1−sinθθ)aaT+1−cosθθa∧(29) (29)J=sin⁡θθI+(1−sin⁡θθ)aaT+1−cos⁡θθa∧

　　因此我们就得到了 se(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.