第 4 章 李群和李代数

  旋转向量/矩阵、变换矩阵、欧拉角、四元数实现了对位姿的表示。在 SLAM 问题中，需要对位姿进行估计和优化，因此通常构建优化问题进行求解。

  在这过程涉及到求导问题，如果使用旋转矩阵，其本身带有约束（正交且行列式为 1），将之作为优化变量会带来额外约束，从而使得优化求解变难。因此引出李群、李代数构建无约束优化问题，简化求解。

4.1 李群

  群： 一种集合$ (A) $加上一种运算$ (.) $的代数结构：

$$
G = (A; ·)
\tag{4-1}
$$

  群满足以下条件：

名称	内容
封闭性	$ \forall a_{1}, a_{2} \in A, \quad a_{1} \cdot a_{2} \in A $
结合律	$ \forall a_{1}, a_{2}, a_{3} \in A, \quad\left(a_{1} \cdot a_{2}\right) \cdot a_{3}=a_{1} \cdot\left(a_{2} \cdot a_{3}\right) $
幺元	$ \exists a_{0} \in A, \quad \text {s.t. } \quad \forall a \in A, \quad a_{0} \cdot a=a \cdot a_{0}=a $
逆	$ \forall a \in A, \quad \exists a^{-1} \in A, \quad \text { s.t. } \quad a \cdot a^{-1}=a_{0} $

  矩阵中常见的群（$ SO(n) $、$ SE(n) $对乘法封闭，对加法不封闭）：

名称	内容
一般线性群$ GL(n) $	$ n×n $的可逆矩阵，它们对矩阵乘法成群
特殊正交群$ SO(n) $	也就是所谓的旋转矩阵群，其中$ SO(2) $和$ SO(3) $最为常见。
特殊欧式群$ SE(n) $	也就是前面提到的n维欧氏变换，如$ SE(2) $和$ SE(3) $。

  特殊正交群、特殊欧式群对乘法封闭，对加法不封闭。

  李群： 具有连续性质的群。如整数群没有连续性质；特殊正交群、特殊欧式群是连续的，因为机器人/相机在空间中的运动时连续的，因此它们是李群。

4.2 李代数

4.2.1 李代数的引出

• 推导：

  （1）旋转函数代表机器人/相机的旋转，随着时间连续变化，即为时间的函数。旋转矩阵是正交矩阵，有：

$$
\boldsymbol{R}(t) \boldsymbol{R}(t)^{T}=\boldsymbol{I} \tag{4-2}
$$

  （2）等式两边对时间求导，可得出是一个反对称矩阵（2-3），又：定义向量对应的反对称矩阵，反对称矩阵对应的向量（2-4）。

$$
\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}+\boldsymbol{R}(t) \dot{\boldsymbol{R}}(t)^{T}=0 \tag{4-3}
$$

$$
\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=-\left(\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}\right)^{T} \tag{4-4}
$$

$$
\boldsymbol{a}^{\wedge}=\boldsymbol{A}=\left[ \begin{array}{ccc}{0} & {-a_{3}} & {a_{2}} \\ {a_{3}} & {0} & {-a_{1}} \\ {-a_{2}} & {a_{1}} & {0}\end{array}\right], \quad \boldsymbol{A}^{\vee}=\boldsymbol{a} \tag{4-5}
$$

  （3）因此可找到一个三维向量 $ \phi(t) \in \mathbb{R}^{3} $ 与 $ \dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T} $ 对应（2-5）。等式两边右乘 $ \boldsymbol{R}(t) $ ，可得到每对旋转矩阵求一次导数，只需左乘一个 $ \phi(t)^{\wedge} $ 矩阵即可（2-6）。

$$
\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=\boldsymbol{\phi}(t)^{\wedge} \tag{4-6}
$$

$$
\dot{\boldsymbol{R}}(t)=\boldsymbol{\phi}(t)^{\wedge} \boldsymbol{R}(t)=\left[ \begin{array}{ccc}{0} & {-\phi_{3}} & {\phi_{2}} \\ {\phi_{3}} & {0} & {-\phi_{1}} \\ {-\phi_{2}} & {\phi_{1}} & {0}\end{array}\right] \boldsymbol{R}(t) \tag{4-7}
$$

  （4）令 $ t_{0}=0 $，且 $ \phi\left(t_{0}\right)=\phi_{0} $，则有（2-7）；上式为微分方程，且有初值 $\boldsymbol{R}(0)=\boldsymbol{I}$，解之得（4-9）。

$$
\dot{\boldsymbol{R}}(t)=\boldsymbol{\phi}\left(t_{0}\right)^{\wedge} \boldsymbol{R}(t)=\boldsymbol{\phi}_{0}^{\wedge} \boldsymbol{R}(t) \tag{4-8}
$$

$$
\boldsymbol{R}(t)=\exp \left(\phi_{0}^{\wedge} t\right) \tag{4-9}
$$

• 结论：

  （1）$ \phi $：给定某时刻的$ \boldsymbol{R} $，可求得一个$ \phi $，描述了$ \boldsymbol{R} $在局部的导数关系。其中$ \phi $即是李代数。（4.2.2 节）

  （2）$ \exp \left(\right) $：矩阵指数。描述了李群和李代数之间的指数映射关系。（4.2.3 节）

4.2.2 定义

• 李代数和李括号：

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

名称	内容
封闭性	$ \forall \boldsymbol{X}, \boldsymbol{Y} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{Y}] \in \mathbb{V} $
双线性	$ \forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V}, a, b \in \mathbb{F}，[a \boldsymbol{X}+b \boldsymbol{Y}, \boldsymbol{Z}]=a[\boldsymbol{X}, \boldsymbol{Z}]+b[\boldsymbol{Y}, \boldsymbol{Z}], \quad[\boldsymbol{Z}, a \boldsymbol{X}+b \boldsymbol{Y}]=a[\boldsymbol{Z}, \boldsymbol{X}]+b[\boldsymbol{Z}, \boldsymbol{Y}] $
自反性	$ \forall \boldsymbol{X} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{X}]=\mathbf{0} $
雅可比矩阵	$ \forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V}，[\boldsymbol{X},[\boldsymbol{Y}, \boldsymbol{Z}]]+[\boldsymbol{Z},[\boldsymbol{Y}, \boldsymbol{X}]]+[\boldsymbol{Y},[\boldsymbol{Z}, \boldsymbol{X}]]=0 $

  二元运算被称为李括号。

  相比于群中的较为简单的二元运算，李括号表达了两个元素的差异。它不要求结合律，而要求元素和自己做李括号之后为零的性质。

• 李代数 so(3)：

  $ \mathfrak{s o}(3) $是一个由三维向量组成的集合，每个向量对应一个反对称矩阵。根据 2.1 节推导可知，它可用于旋转矩阵的求导，李代数和李括号定义如下：
$$

\mathfrak{s o}(3)=\left{\phi \in \mathbb{R}^{3}, \mathbf{\Phi}=\phi^{\wedge}=\left[\begin{array}{ccc}{0} & {-\phi_{3}} & {\phi_{2}} \\ {\phi_{3}} & {0} & {-\phi_{1}} \\ {-\phi_{2}} & {\phi_{1}} & {0}\end{array}\right] \in \mathbb{R}^{3 \times 3}\right}

$$

$$

\left[\phi_{1}, \phi_{2}\right]=\left(\mathbf{\Phi}_{1} \mathbf{\Phi}_{2}-\mathbf{\Phi}_{2} \mathbf{\Phi}_{1}\right)^{\vee}

$$

• 李代数 se(3)：

  $ \mathfrak{s e}(3) $每个元素是一个六维向量记作$ \boldsymbol{\xi} $：前三维是平移记作$ \boldsymbol{\rho} $（这里的平移左乘一个系数矩阵才是变换矩阵中的真实平移$ \boldsymbol{t} $，见 2.2.2 节），后三位是旋转记作$ \phi $（实质是$ \mathfrak{s o}(3) $的元素）。

  此处符号$ \boldsymbol{^{\wedge}} $和$ \boldsymbol{^{\vee}} $不代表反对称，而是被扩展成“六位向量”与“四维矩阵”的相互转换关系（为了保持和$ \mathfrak{s o}(3) $形式一致）。

  李代数和李括号定义如下：
$$

\mathfrak{s e}(3)=\left{\boldsymbol{\xi}=\left[\begin{array}{c}{\boldsymbol{\rho}} \\ {\boldsymbol{\phi}}\end{array}\right] \in \mathbb{R}^{6}, \boldsymbol{\rho} \in \mathbb{R}^{3}, \boldsymbol{\phi} \in \mathfrak{s o}(3), \boldsymbol{\xi}^{\wedge}=\left[\begin{array}{cc}{\boldsymbol{\phi}^{\wedge}} & {\boldsymbol{\rho}} \\ {\mathbf{0}^{T}} & {0}\end{array}\right] \in \mathbb{R}^{4 \times 4}\right}

$$

$$

\left[\boldsymbol{\xi}_{1}, \boldsymbol{\xi}_{2}\right]=\left(\boldsymbol{\xi}_{1}^{\wedge} \boldsymbol{\xi}_{2}^{\wedge}-\boldsymbol{\xi}_{2}^{\wedge} \boldsymbol{\xi}_{1}^{\wedge}\right)^{\vee}

$$

4.2.3 李群和李代数关系

4.2.3.1 SO(3)上的指数映射

  $ SO(3) $与$ \mathfrak{s o}(3) $的关系由指数映射给定：
$$

\boldsymbol{R}=\exp \left(\boldsymbol{\phi}^{\wedge}\right)

$$

• 推导：

  （1）将指数映射写成泰勒展开式：
$$

\exp \left(\phi^{\wedge}\right)=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\phi^{\wedge}\right)^{n}

$$
  （2）因为$ \phi $是三维向量，将之表示为模长（$ \theta $）和方向（$ \boldsymbol{a} $单位方向向量）的乘积，且具有以下性质：
$$

\begin{cases}
\phi=\theta \boldsymbol{a}\\
\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}=\boldsymbol{a} \boldsymbol{a}^{T}-\boldsymbol{I}\\
\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}=-\boldsymbol{a}^{\wedge}
\end{cases}

$$
  （3）将上述表示带入指数映射：
$$

\begin{aligned} \exp \left(\phi^{\wedge}\right) &=\exp \left(\theta \boldsymbol{a}^{\wedge}\right)=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\theta \boldsymbol{a}^{\wedge}\right)^{n} \\ &=\boldsymbol{I}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{4}+\ldots \\ &=\boldsymbol{a} \boldsymbol{a}^{T}-\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}-\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge}-\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{2}+\ldots \\ &=\boldsymbol{a} \boldsymbol{a}^{T}+\left(\theta-\frac{1}{3 !} \theta^{3}+\frac{1}{5 !} \theta^{5}-\ldots\right) \boldsymbol{a}^{\wedge}-\left(1-\frac{1}{2 !} \theta^{2}+\frac{1}{4 !} \theta^{4}-\ldots\right) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge}-\cos \theta \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=(1-\cos \theta) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge}\\ &=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge} \end{aligned}

$$

• 结论：

  上式同罗德里格斯公式，即旋转向量与旋转矩阵的转换公式。因此如下结论：

  （1）$ \mathfrak{s o}(3) $到$ SO(3) $的指数映射：由罗德里格斯公式给出（2.6.2 节），$ \mathfrak{s o}(3) $实际上就是旋转向量组成的空间。

  （2）$ SO(3) $到$ \mathfrak{s o}(3) $到的映射：由如下对数映射给出。但实际中常用 2.6.2 节所述方法，根据旋转矩阵计算旋转向量。
$$

\phi=\ln (\boldsymbol{R})^{\vee}=\left(\sum_{n=0}^{\infty} \frac{(-1)^{n}}{n+1}(\boldsymbol{R}-\boldsymbol{I})^{n+1}\right)^{\vee}

$$

4.2.3.2 SE(3)上的指数映射

  $ SE(3) $与$ \mathfrak{s e}(3) $的相互转换，可通过下述映射进行计算：
$$

\begin{aligned} \exp \left(\boldsymbol{\xi}^{\wedge}\right) &=\left[\begin{array}{ccc}{\sum_{n=0}^{\infty} \frac{1}{n !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n}} & {\sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} \boldsymbol{\rho}} \\ {\mathbf{0}^{T}} & {1}\end{array}\right] \\ & \triangleq\left[\begin{array}{cc}{\boldsymbol{R}} & {\boldsymbol{J} \rho} \\ {\mathbf{0}^{T}} & {1}\end{array}\right]=\boldsymbol{T} \end{aligned}

$$

$$

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

$$