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

taotao_2016 2017-03-24

展开全文

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

前言

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

\begin{matrix} (1) & \forall i, q_{i} = T p_{i} \end{matrix}

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

\begin{matrix} (2) & min_{T} u (T) = \sum_{i = 1}^{N} {∥ q_{i} - T p_{i} ∥}^{2} \end{matrix}

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

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

李代数基础

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

群

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

封闭性: $\forall a_{1}, a_{2}, a_{1} \cdot a_{2} \in A$
结合律: $\forall a_{1}, a_{2}, a_{3}, (a_{1} \cdot a_{2}) \cdot a_{3} = a_{1} \cdot (a_{2} \cdot a_{3})$
幺元: $\exists a_{0} \in A, s . t . \forall a \in A, a_{0} \cdot a = a \cdot a_{0} = a$
逆: $\forall a \in A, \exists a^{- 1} \in A, s . t . a \cdot a^{- 1} = a_{0}$

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

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

\begin{matrix} (3) & S O (n) = {R \in R^{n \times n} | {R R}^{T} = I, d e t (R) = 1} \end{matrix}

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

\begin{matrix} (4) & S E (3) = {T = [\begin{matrix} R & t \\ 0^{T} & 1 \end{matrix}] \in R^{4 \times 4} | R \in S O (3), t \in R^{3}} \end{matrix}

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

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

三维旋转群 $S O (3)$

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

\begin{matrix} (5) & \dot{R} (t) R (t)^{T} + R (t) \dot{R} (t)^{T} = 0 \end{matrix}

　　于是：

\begin{matrix} (6) & \dot{R} (t) R (t)^{T} = - {(\dot{R} (t) R (t)^{T})}^{T} \end{matrix}

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

\begin{matrix} (7) & a^{\land} = A = [\begin{matrix} 0 & - a_{3} & a_{2} \\ a_{3} & 0 & - a_{1} \\ - a_{2} & a_{1} & 0 \end{matrix}] \end{matrix}

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

\begin{matrix} (8) & A^{\lor} = a \end{matrix}

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

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

\begin{matrix} (9) & \dot{R} (t) R (t)^{T} = ϕ (t)^{\land} \end{matrix}

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

\begin{matrix} (10) & \dot{R} (t) = ϕ (t)^{\land} R (t) = [\begin{matrix} 0 & - ϕ_{3} & ϕ_{2} \\ ϕ_{3} & 0 & - ϕ_{1} \\ - ϕ_{2} & ϕ_{1} & 0 \end{matrix}] R (t) \end{matrix}

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

\begin{matrix} (11) & R (t) = \exp (ϕ (t)^{\land}) R (t_{0}) \end{matrix}

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

什么是李代数

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

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

封闭性 $\forall X, Y \in V, [X Y] \in V$
双线性 $\forall X, Y, Z \in V, a, b \in F,$ 有
$[a X + b Y, Z] = a [X Z] + b [Y Z] [Z, a X + b Y] = a [Z X] + b [Z Y]$
自反性 $\forall X \in V, [X X] = 0$
雅可比等价 $\forall X, Y, Z \in V, [X, [Y Z]] + [Z, [Y X]] + [Y, [Z X]]$

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

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

\begin{matrix} (12) & Φ = ϕ^{\land} = [\begin{matrix} 0 & - ϕ_{3} & ϕ_{2} \\ ϕ_{3} & 0 & - ϕ_{1} \\ - ϕ_{2} & ϕ_{1} & 0 \end{matrix}] \in R^{3 \times 3} \end{matrix}

　　在此定义下，两个向量 $ϕ_{1}, ϕ_{2}$ 的李括号为：

\begin{matrix} (13) & [ϕ_{1}, ϕ_{2}] = Φ_{1} Φ_{2} - Φ_{2} Φ_{1} \end{matrix}

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

\begin{matrix} (14) & s o (3) = {Φ = ϕ^{\land} \in R^{3 \times 3} | ϕ \in R^{3}} \end{matrix}

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

\begin{matrix} (15) & ϕ ϕ^{T} = ϕ^{\land} ϕ^{\land} + ∥ ϕ ∥^{2} I_{3 \times 3} \end{matrix}

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

\begin{matrix} (16) & ϕ ϕ^{T} = ϕ^{\land} ϕ^{\land} + I 1 \end{matrix}

　　以及

\begin{matrix} (17) & ϕ^{\land} ϕ^{\land} ϕ^{\land} = - ϕ^{\land} \end{matrix}

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

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

指数映射

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

\begin{matrix} (18) & \exp (A) = \sum_{n = 0}^{\infty} \frac{1}{n!} A^{n} \end{matrix}

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

\begin{matrix} (19) & \exp (ϕ^{\land}) = \sum_{n = 0}^{\infty} \frac{1}{n!} (ϕ^{\land})^{n} \end{matrix}

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

\begin{aligned} \exp (ϕ^{\land}) & = \exp (θ a^{\land}) = \sum_{n = 0}^{\infty} \frac{1}{n!} {(θ a^{\land})}^{n} \\ = I + θ a^{\land} + \frac{1}{2!} θ^{2} a^{\land} a^{\land} + \frac{1}{3!} θ^{3} a^{\land} a^{\land} a^{\land} + \frac{1}{4!} θ^{4} {(a^{\land})}^{4} + . . . \\ = a a^{T} - a^{\land} a^{\land} + θ a^{\land} + \frac{1}{2!} θ a^{\land} a^{\land} - \frac{1}{3!} θ^{3} a^{\land} + \frac{1}{4!} θ^{4} {(a^{\land})}^{4} + . . . \\ = a a^{T} + (θ - \frac{1}{3!} θ^{3} + \frac{1}{5!} θ^{5} - . . .) a^{\land} - (1 - \frac{1}{2!} θ^{2} + \frac{1}{4!} θ^{4} - . . .) a^{\land} a^{\land} \\ = a^{\land} a^{\land} + I + \sin θ a^{\land} - \cos θ a^{\land} a^{\land} \\ = (1 - \cos θ) a^{\land} a^{\land} + I + \sin θ a^{\land} \\ = \cos θ I + (1 - \cos θ) a a^{T} + \sin θ a^{\land} \end{aligned}

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

\begin{matrix} (20) & \exp (θ a) = \cos θ I + (1 - \cos θ) a a^{T} + \sin θ a^{\land} \end{matrix}

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

　　反之，如果定义对数映射，我们也能把 $S O (3)$ 中的元素对应到 $s o (3)$ 中：

\begin{matrix} (21) & ϕ = \ln {(R)}^{\lor} = {(\sum_{n = 0}^{\infty} \frac{{(- 1)}^{n}}{n + 1} {(R - I)}^{n + 1})}^{\lor} \end{matrix}

　　其中 $^{\lor}$ 表示从反对称矩阵到向量的对应关系，为 $^{\land}$ 的逆运算。

　　读者可能会问，指数映射性质如何呢？它是一个双射吗？很遗憾，它只是一个满射。每个 $S O (3)$ 中的元素，都可以找到 $s o (3)$ 中至少一个与之对应；但是可能存在多个 $s o (3)$ 中的元素，对应到同一个 $S O (3)$ 元素上。至少对于旋转角 $θ$ ，我们知道它具有周期性。