转载

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

前言

在SLAM中，除了表达3D旋转与位移之外，我们还要对它们进行估计，因为SLAM整个过程就是在不断地估计机器人的位姿与地图。为了做这件事，需要对变换矩阵进行插值、求导、迭代等操作。例如，在经典ICP问题中，给定了两组3D点，我们要计算它们之间的变换矩阵。假设第一组的3D点为$/mathbf{P}=/{ /mathbf{p}_i | i = [1,2, /ldots, N] /}$，第二组3D点为$/mathbf{Q}=/{ /mathbf{q}_i | i = [1,2, /ldots, N] /}$，那我们实际要做的事情是求一个欧氏变换$/mathbf{T}$，使得$/mathbf{T}$满足：

/[/begin{equation}

/forall i, /quad /mathbf{q}_i = /mathbf{T} /mathbf{p}_i

/end{equation}/]

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

/[/begin{equation}

/mathop {/min }/limits_{/mathbf{T}} u/left( {/mathbf{T}} /right) = /sum/limits_{i = 1}^N {{{/left/| {{/mathbf{q}_i} - /mathbf{T} {/mathbf{p}_i}} /right/|}^2}}

/end{equation}/]

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

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

李代数基础

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

群

群（Group）是一种集合加上一种运算的代数结构，记作$(A,/cdot)$。其中$A$代表集合，$/cdot$是定义在该集合上的二元运算。那么，如果这个运算满足以下几个条件，则称$G=(A, /cdot)$为群。

封闭性 : $ /quad /forall a_1, a_2, /quad a_1 /cdot a_2 /in A$
结合律 : $ /quad /forall a_1, a_2, a_3, /quad (a_1 /cdot a_2) /cdot a_3 = a_1 /cdot ( a_2 /cdot a_3) $
幺元 : $ /quad /exists a_0 /in A, /quad s.t. /quad /forall a /in A, /quad a_0 /cdot a = a /cdot a_0 = a $
逆 : $ /quad /forall a /in A, /quad /exists a^{-1} /in A, /quad s.t. /quad a /cdot a^{-1} = a_0 $

读者可以记作“封结幺逆”（谐音凤姐咬你），并可以把一些常见的群放进去验证。例如整数的加法（幺元为0），去掉0后的有理数的乘法（幺元为1）。对于矩阵，可以找到一些常见的矩阵群，例如：

一般线性群$GL(n)$ 指$n /times n$的可逆矩阵，它们对矩阵乘法成群。
特殊正交群$SO(n)$ 也就是所谓的旋转矩阵群，其中$SO(2)$ 和$SO(3)$最为常见。正式的记法是：

/[/begin{equation}

SO(n) = /{ /mathbf{R} /in /mathbb{R}^{n /times n} | /mathbf{R R}^T = /mathbf{I}, det(/mathbf{R})=1 /}

/end{equation}/]

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

/[/begin{equation}

SE(3) = /left/{ /mathbf{T} = /left[ {/begin{array}{*{20}{c}}

/mathbf{R} & /mathbf{t} //

{{/mathbf{0}^T}} & 1

/end{array}} /right]

/in /mathbb{R}^{4 /times 4} | /mathbf{R} /in SO(3), /mathbf{t} /in /mathbb{R}^3/right/}

/end{equation}/]

群结构保证了在群上的运算具有良好的性质，而群论则研究群的各种结构和性质，但我们在此不多加介绍。感兴趣的读者可以参考任意一本近世代数教材。

李群是指具有连续性质的群。并且，一般连续群上的运算还是无限可微，乃至解析的（解析比无限可微更强，它还要求任意点邻域的泰勒展开都收敛）。这个问题在20世纪初被称为希尔伯特第五问题，并已得到了解决。而李群，则指实数空间上的连续群。常见的李群包括上边提到的$GL(n), SO(n), SE(n)$，以及其他的如酉群$U(n)$，辛群$Sp(2n)$等等。

三维旋转群$SO(3)$

三维旋转群$SO(3)$是特殊正交群$SO(n)$在$n=3$时的特例，它们可以用来描述三维空间的旋转，其元素都是$3 /times3$ 的正交且行列式为$+1$的矩阵。假设有这样一个矩阵$/mathbf{R}$，满足$/mathbf{R} /mathbf{R}^T＝/mathbf{I}$。现在，考虑它随时间发生变化，即从$/mathbf{R}$ 变成了$/mathbf{R}(t)$，仍有$/mathbf{R}(t) /mathbf{R}(t) ^T = /mathbf{I}$。在等式两边对时间求导，得到：

/[/begin{equation}

/mathbf{/dot{R}} (t) /mathbf{R} {(t)^T} + /mathbf{R} (t) /mathbf{/dot{R}} {(t)^T} = 0

/end{equation}/]

于是：

/[/begin{equation}

/mathbf{/dot{R}} (t) /mathbf{R} {(t)^T} = - /left( /mathbf{/dot{R}} (t) /mathbf{R} {(t)^T} /right)^T

/end{equation}/]

可以看出$/mathbf{/dot{R}} (t) /mathbf{R} {(t)^T}$是一个反对称矩阵。注意到对于任意一个$3 /times $的矩阵，我们记它为$/mathbf{A}$。由于$/mathbf{A}^T=-/mathbf{A}$，所以它主对角线元素必为$0$，而非对角线元素则只有三个自由度。我们可以把它对应到一个向量$/mathbf{a}=[a_1, a_2, a_3]^T$中去：

/[/begin{equation}

{/mathbf{a}^ /wedge } = /mathbf{A} = /left[ {/begin{array}{*{20}{c}}

0&{ - {a_3}}&{{a_2}}//

{{a_3}}&0&{ - {a_1}}//

{ - {a_2}}&{{a_1}}&0

/end{array}} /right]

/end{equation}/]

其中$^{/wedge}$符号表示由向量转换为矩阵，反之我们也可以用符号$^{/vee}$定义由矩阵转换为向量的方式：

/[/begin{equation}

{ /mathbf{A}^ /vee } = /mathbf{a}

/end{equation}/]

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

现在，由于$/mathbf{/dot{R}} (t) /mathbf{R} {(t)^T}$是一个反对称矩阵，我们可以找到一个三维向量$/mathbf{/phi} (t) /in /mathbb{R}^3$与之对应。于是有：

/[/begin{equation}

/mathbf{ /dot{R} } (t) /mathbf{R}(t)^T = /mathbf{/phi} (t) ^ {/wedge}

/end{equation}/]

左右各右乘$/mathbf{R}(t)$，由于$/mathbf{R}$为正交阵，有：

/[/begin{equation}

/mathbf{ /dot{R} } (t) = /mathbf{/phi} (t)^{/wedge} /mathbf{R}(t) =

/left[ {/begin{array}{*{20}{c}}

0&{ - {/phi _3}}&{{/phi _2}}//

{{/phi _3}}&0&{ - {/phi _1}}//

{ - {/phi _2}}&{{/phi _1}}&0

/end{array}} /right] /mathbf{R} (t)

/end{equation}/]

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

/[/begin{equation}

/label{eq:so3ode}

/mathbf{R}(t) = /exp /left( /mathbf{/phi} (t) /right) /mathbf{R}(t_0)

/end{equation}/]

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

什么是李代数

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

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

封闭性 $/forall /mathbf{X}, /mathbf{Y} /in /mathbb{V}, [/mathbf{X} /mathbf{Y}] /in /mathbb{V}$
双线性 $/forall /mathbf{X,Y,Z} /in /mathbb{V}, a,b /in /mathbb{F}, $ 有 $$ [a/mathbf{X}+b/mathbf{Y}, /mathbf{Z}] = a[/mathbf{X}/mathbf{Z}] + b [ /mathbf{Y} /mathbf{Z} ] /quad [/mathbf{Z}, a /mathbf{X}+b/mathbf{Y}] = a [/mathbf{Z} /mathbf{X} ]+ b [/mathbf{ZY}] $$
自反性 $/forall /mathbf{X} /in /mathbb{V}, [/mathbf{X} /mathbf{X}] = /mathbf{0}$
雅可比等价 $/forall /mathbf{X,Y,Z} /in /mathbb{V}, [/mathbf{X}, [/mathbf{YZ}] ] + [/mathbf{Z}, [/mathbf{YX}] ] + [/mathbf{Y}, [/mathbf{ZX}]] $

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

三维旋转群与对应的李代数

$SO(3)$对应的李代数是定义在$/mathbb{R}^3$上的向量，我们记作$/mathbf{/phi}$（注意这是个向量，虽然希腊字母的粗体不明显）。根据前面的推导，每个$/mathbf{/phi}$都可以生成一个反对称矩阵：

/[/begin{equation}

/label{eq:phi}

/mathbf{/Phi} = /mathbf{/phi}^{/wedge} = /left[ {/begin{array}{*{20}{c}}

0&{ - {/phi _3}}&{{/phi _2}}//

{{/phi _3}}&0&{ - {/phi _1}}//

{ - {/phi _2}}&{{/phi _1}}&0

/end{array}} /right] /in /mathbb{R}^{3 /times 3}

/end{equation}/]

在此定义下，两个向量$/mathbf{/phi}_1, /mathbf{/phi}_2$的李括号为：

/[/begin{equation}

[/mathbf{/phi}_1, /mathbf{/phi}_2] = /mathbf{ /Phi }_1 /mathbf{ /Phi }_2 - /mathbf{ /Phi }_2 /mathbf{ /Phi }_1

/end{equation}/]

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

/[/begin{equation}

/mathfrak{so}(3) = /left/{ /Phi = /mathbf{/phi^/wedge} /in /mathbb{R}^{3 /times 3} | /mathbf{/phi} /in /mathbb{R}^3 /right/}

/end{equation}/]

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

/[/begin{equation}

/mathbf{/phi} /mathbf{/phi}^T = /mathbf{/phi}^{/wedge} /mathbf{/phi}^{/wedge} + /| /mathbf{/phi} /|^2 /mathbf{I}_{3 /times 3}

/end{equation}/]

当$/mathbf{/phi}$为单位向量时，进而有：

/[/begin{equation}

/mathbf{/phi} /mathbf{/phi}^T = /mathbf{/phi}^{/wedge} /mathbf{/phi}^{/wedge} + /mathbf{I}1

/end{equation}/]

以及

/[/begin{equation}

/mathbf{/phi}^{/wedge} /mathbf{/phi}^{/wedge} /mathbf{/phi}^{/wedge} = - /mathbf{/phi}^{/wedge}

/end{equation}/]

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

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

指数映射

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

/[/begin{equation}

/exp(/mathbf{A}) = /sum/limits_{n = 0}^/infty {/frac{1}{{n!}}{ /mathbf{A}^n}}

/end{equation}/]

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

/[/begin{equation}

/exp(/mathbf{/phi}^/wedge) = /sum/limits_{n = 0}^/infty {/frac{1}{{n!}}{ (/mathbf{/phi}^{/wedge})^n}}

/end{equation}/]

现在我们来仔细看看它的含义。由于$/mathbf{/phi}$是三维向量，我们可以定义它的模长和它的方向，分别记作$/theta$和$/mathbf{a}$（注意这里记号是有含义的，此时$/mathbf{a}$是一个单位长度的向量），那么按照上式，可以推出如下公式，注意中间使用了上面讲到了两个反对称矩阵的性质：

/[/begin{align*}

/exp /left( {{/mathbf{/phi} ^ /wedge }} /right) &= /exp /left( {/theta {/mathbf{a}^ /wedge }} /right) = /sum/limits_{n = 0}^/infty {/frac{1}{{n!}}{{/left( {/theta {/mathbf{a}^ /wedge }} /right)}^n}} //

&= /mathbf{I} + /theta {/mathbf{a}^ /wedge } + /frac{1}{{2!}}{/theta ^2}{/mathbf{a}^ /wedge }{/mathbf{a}^ /wedge } + /frac{1}{{3!}}{/theta ^3}{/mathbf{a}^ /wedge }{/mathbf{a}^ /wedge }{/mathbf{a}^ /wedge } + /frac{1}{{4!}}{/theta ^4}{/left( {{/mathbf{a}^ /wedge }} /right)^4} + ...//

&= /mathbf{a} {/mathbf{a}^T} - {/mathbf{a}^ /wedge }{/mathbf{a}^ /wedge } + /theta {/mathbf{a}^ /wedge } + /frac{1}{{2!}}/theta {/mathbf{a}^ /wedge }{/mathbf{a}^ /wedge } - /frac{1}{{3!}}{/theta ^3}{/mathbf{a}^ /wedge } - /frac{1}{{4!}}{/theta ^4}{/left( {{/mathbf{a}^ /wedge }} /right)^4} + ...//

&= /mathbf{a}{/mathbf{a}^T} + /left( {/theta - /frac{1}{{3!}}{/theta ^3} + /frac{1}{{5!}}{/theta ^5} - ...} /right){/mathbf{a}^ /wedge } - /left( {1 - /frac{1}{{2!}}{/theta ^2} + /frac{1}{{4!}}{/theta ^4} - ...} /right){/mathbf{a}^ /wedge }{/mathbf{a}^ /wedge }//

&= {/mathbf{a}^ /wedge }{/mathbf{a}^ /wedge } + /mathbf{I} + /sin /theta {/mathbf{a}^ /wedge } - /cos /theta {/mathbf{a}^ /wedge }{/mathbf{a}^ /wedge }//

&= (1 - /cos /theta ){/mathbf{a}^ /wedge }{/mathbf{a}^ /wedge } + I + /sin /theta {/mathbf{a}^ /wedge }//

&= /cos /theta /mathbf{I} + (1 - /cos /theta )/mathbf{a}{/mathbf{a}^T} + /sin /theta {/mathbf{a}^ /wedge } //

/end{align*}/]

最后我们得到了一个似曾相识的式子：

/[/begin{equation}

/exp( /theta /mathbf{a} ) = /cos /theta /mathbf{I} + (1 - /cos /theta )/mathbf{a}{/mathbf{a}^T} + /sin /theta {/mathbf{a}^ /wedge }

/end{equation}/]

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

反之，如果定义对数映射，我们也能把$SO(3)$中的元素对应到$/mathfrak{so}(3)$中：

/[/begin{equation}

/mathbf{/phi} = /ln {/left( /mathbf{R} /right)^ /vee } = {/left( {/sum/limits_{n = 0}^/infty {/frac{{{{/left( { - 1} /right)}^n}}}{{n + 1}}{{/left( { /mathbf{R} - /mathbf{I}} /right)}^{n + 1}}} } /right)^ /vee }

/end{equation}/]

其中$^/vee$表示从反对称矩阵到向量的对应关系，为$^/wedge$的逆运算。

读者可能会问，指数映射性质如何呢？它是一个双射吗？很遗憾，它只是一个满射。每个$/mathfrak{so}(3)$中的元素，都可以对应到$SO(3)$ 中的一个，但是可能存在多个$SO(3)$中的元素，对应到同一个$/mathfrak{so}(3)$里。至少对于旋转角$/theta$，我们知道它具有周期性。

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

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

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

/[/begin{equation}

SE(3) = /left/{ /mathbf{T} = /left[ {/begin{array}{*{20}{c}}

/mathbf{R} & /mathbf{t} //

{{/mathbf{0}^T}} & 1

/end{array}} /right]

/in /mathbb{R}^{4 /times 4} | /mathbf{R} /in SO(3), /mathbf{t} /in /mathbb{R}^3/right/}

/end{equation}/]

每个变换矩阵有六个四由度，故对应的李代数位于$/mathbb{R}^6$中：

/[/begin{equation}

/mathfrak{se}(3) = /left/{ /mathbf{ /Xi } = /mathbf{/xi}^/wedge /in /mathbb{R}^{4 /times 4} | /mathbf{/xi} /in /mathbb{R}^6 /right/}

/end{equation}/]

但是$^/wedge$不再对应到一个反对称关系，而是：

/[/begin{equation}

/mathbf{/xi}^/wedge = {/left[ /begin{array}{l}

/mathbf{/rho} //

/mathbf{/phi}

/end{array} /right]^ /wedge } = /left[ {/begin{array}{*{20}{c}}

{{/mathbf{/phi} ^ /wedge }}&/mathbf{/rho} //

{{/mathbf{0}^T}}&0

/end{array}} /right] = /mathbf{/Xi}

/end{equation}/]

可以看到，$/mathbf{/xi}$ 的前三维为旋转向量，后三维为平移向量，其定义也十分的直观。该李代数对应于微分方程：

/[/begin{equation}

/mathbf{/dot{T}}(t) = /mathbf{/xi}^/wedge(t) /mathbf{T}(t)

/end{equation}/]

因此

/[/begin{equation}

/mathbf{T}(t) = /exp ( /mathbf{/xi}(t)^/wedge ) /mathbf{T}(t)

/end{equation}/]

那么$/mathfrak{se}(3)$上的指数映射如何呢？略加推导可得：

/[/begin{align}

/exp /left( {{ /mathbf{/xi} ^ /wedge }} /right) &= /left[ {/begin{array}{*{20}{c}}

{/sum/limits_{n = 0}^/infty {/frac{1}{{n!}}{{/left( {{/mathbf{/phi} ^ /wedge }} /right)}^n}} }&{/sum/limits_{n = 0}^/infty {/frac{1}{{/left( {n + 1} /right)!}}{{/left( {{/mathbf{/phi} ^ /wedge }} /right)}^n} /mathbf{/rho} } }//

{{/mathbf{0}^T}}&1

/end{array}} /right] //

&= /left[ {/begin{array}{*{20}{c}}

/mathbf{/Phi} &{/mathbf{J/rho} } //

{{/mathbf{0}^T}}&1

/end{array}} /right]

/end{align}/]

左上角的$/mathbf{/Phi}$是我们熟知的$/mathfrak{so}(3)$中的元素，前文已经介绍过了。而右上角的$/mathbf{J}$则可整理为（设$/mathbf{/phi}=/theta/mathbf{a}$）：

/[/begin{equation}

/mathbf{J} = /frac{{/sin /theta }}{/theta } /mathbf{I} + /left( {1 - /frac{{/sin /theta }}{/theta }} /right) /mathbf{a} { /mathbf{a}^T} + /frac{{1 - /cos /theta }}{/theta }{ /mathbf{a}^ /wedge }

/end{equation}/]

因此我们就得到了$/mathfrak{se}(3)$的指数映射的关系。其对数映射亦可类比推得。