群的定义

设有一个集合$G$，集合中的元素为$g$，元素之间存在乘法运算$\cdot$，称$G$是一个群如果其满足以下条件：

• 封闭性 $\forall g_1, g_2\in G,\ \ g_1\cdot g_2\in G$
• 结合律 $g_1\cdot(g_2\cdot g_3)=(g_1\cdot g_2)\cdot g_3$
• 存在幺元 $\forall g\in G,\ \ e\cdot g=g\cdot e=g$
• 存在逆元 $\forall g\in G,\ \ \exists g^{-1}\in G,\ \ g\cdot g^{-1}=g^{-1}\cdot g=e$

SLAM中常用的群

只考虑实数域。所有$n\times n$的正交矩阵$R$构成了一个群，称为正交群，记为$O(n)$，在这些正交矩阵中，行列式为正1的矩阵本身也构成一个群，称为特殊正交群，记为$SO(n)$，也就是说$SO(n)$是$O(n)$的一个子群（准确的说是正规子群）。

在SLAM以及很多机器人技术中，最常用到的就是$SO(3)$，也就是所有三维旋转矩阵构成的群，可以用来表示机器人或者飞行器的姿态。

当需要描述一个刚体变换（旋转加平移）时，光靠一个旋转矩阵是不够的，当我们使用齐次坐标表示空间中的点时，对该点的刚体变换可用下面的$4\times 4$矩阵表示

$$T= \begin{pmatrix} R & t \\ 0 & 1 \end{pmatrix}$$

其中$R\in \mathbb{R}^{3\times 3}$是旋转矩阵，$t\in \mathbb{R}^3$是平移矢量。容易验证，矩阵$T$也是满足构成群的四个条件的，其中逆元为

$$T^{-1}= \begin{pmatrix} R^T & -R^Tt \\ 0 & 1 \end{pmatrix}$$

幺元就是单位阵，因此所有刚体变换矩阵也构成一个群，称为特殊欧式群，记为$SE(3)$。当然也存在$SE(n)$，看做是任意维度下的刚体变换。如果不要求$T$里面的$R$必须是一个旋转矩阵，甚至不要求是正交矩阵，只要可逆就行，那么我们就得到了更一般的仿射群，在这就不深入讨论了。

为了方便之后介绍不变卡尔曼滤波，这里对$SE(3)$做一个扩展，我们发现型如

$$\chi= \begin{pmatrix} R & t & p_1 & p_2 & ... & p_k \\ 0 & 1 & 0 & 0 & ... & 0 \\ 0 & 0 & 1 & 0 & ... & 0 \\ 0 & 0 & 0 & 1 & ... & 0 \\ \vdots \\ 0 & 0 & 0 & 0 & ... & 1 \end{pmatrix}$$

的矩阵，大小为$(4+k)\times(4+k)$，$p_k\in \mathbb{R}^3$，也满足群定义，逆元为

$$\chi^{-1}= \begin{pmatrix} R^T & -R^Tt & -R^Tp_1 & -R^Tp_2 & ... & -R^Tp_k \\ 0 & 1 & 0 & 0 & ... & 0 \\ 0 & 0 & 1 & 0 & ... & 0 \\ 0 & 0 & 0 & 1 & ... & 0 \\ \vdots \\ 0 & 0 & 0 & 0 & ... & 1 \end{pmatrix}$$

幺元也是单位阵，我们将这种矩阵构成的群记为$SE_{k+1}(3)$。

切空间与李代数

$SO(3)$和$SE(3)$这类群，还有一个特点是平滑连续，即他们是一个平滑的流形，这类群称为李群。在流形上微分可以得到流形上某一点的切空间（类比于对曲线微分得到切线）。其中在幺元处的切空间最重要（与后面的指数映射有关）。下面分别来求一下$SO(3)$，$SE(3)$，以及$SE_k(3)$在幺元处的切空间。

$SO(3)$的切空间

具体写出$SO(3)$的定义为

$$SO(3)=\{R\in\mathbb{R}^{3\times 3}|R^TR=I, det(R)=1\}$$

现在假设$R$是一个随时间变化的量$R(t)$，则$R(t)^TR(t)=I$两边对时间求导，并令$\partial R/\partial t=\dot{R}$，有

$$\dot{R}^TR+R^T\dot{R}=0$$

由于要求幺元处的切空间，令上式中的$R=I$，得

$$\dot{R}^T+\dot{R}=0$$

即$\dot{R}$是一个斜对称矩阵，有如下形式

$$\dot{R}= \begin{pmatrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{pmatrix}$$

这就是$SO(3)$幺元处的切空间所具有的形式。以上斜对称矩阵实际只有3个维度，我们定义一个映射

$$\begin{bmatrix} x \\ y \\ z \end{bmatrix}_\times= \begin{pmatrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{pmatrix}$$

则矢量$\xi_R=(x,y,z)^T$就是我们常说的轴角。矢量的方向代表旋转的轴，矢量的模长代表要旋转的角度。

$SE(3)$与$SE_{k+1}(3)$的切空间

有了$SO(3)$的切空间后，$SE(3)$与$SE_{k+1}(3)$的切空间就很好求了。幺元处的切空间实际上是幺元在流形上发生微小摄动产生的增量，比如对$SO(3)$而言，幺元$I$在流形上的微小摄动产生$I+\Lambda(\xi_R)$，那么平移量$t$本身属于向量空间，微小的摄动仍然是一个三维矢量。因此$SE(3)$切空间的形式就是

$$\dot{T}= \begin{pmatrix} [\xi_R]_\times & \xi_t \\ 0 & 0 \end{pmatrix}$$

其中$\xi_R,\xi_t \in \mathbb{R}^3$。

而$SE_{k+1}(3)$的切空间自然就是

$$\dot{\chi}= \begin{pmatrix} [\xi_R]_\times & \xi_t &\xi_{p_1} & ... & \xi_{p_k} \\ 0 & 0 & 0 & ... & 0 \\ \vdots \\ 0 & 0 & 0 & ... & 0 \end{pmatrix}$$

上面的表达是很冗余的，我们也可以像$SO(3)$那样定义一个映射

$$\Lambda \begin{pmatrix} \xi_R\\ \xi_t\\ \xi_{p_1}\\ \vdots\\ \xi_{p_k} \end{pmatrix} = \begin{pmatrix} [\xi_R]_\times & \xi_t &\xi_{p_1} & ... & \xi_{p_k} \\ 0 & 0 & 0 & ... & 0 \\ \vdots \\ 0 & 0 & 0 & ... & 0 \end{pmatrix}$$

我们可以发现，所有这些群的切空间都是向量空间，即满足矢量的线性运算（对加法和数乘封闭），实际上，它们还对另外一种运算封闭，被称为李括号，定义为$[X, Y]=XY-YX$，其中$X,Y$是群的切空间的矩阵形式。并可以验证有如下关系成立

$$\begin{aligned} \\ [X,Y]+[Y,X]&=0 \\ [X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]]&=0 \end{aligned}$$

如果将李括号也视为一种乘法（和矩阵一样不满足交换律），那么切空间就同时对加法运算和乘法运算封闭，因此构成了一个环（Ring）！所以，这样的切空间也被称为李代数。$SO(3),SE(3),SE_{k+1}(3)$的李代数分别用$so(3),se(3),se_{k+1}(3)$表示。

指数映射

先对指数函数做泰勒展开，有

$$e^x=1+x+\frac{1}{2!}x^2+...+\frac{1}{n!}x^n+...$$

将其中的$x$替换为某个矩阵$A$，可得到

$$e^A=I+A+\frac{1}{2!}A^2+...+\frac{1}{n!}A^n+...$$

可证明上式对任意$n\times n$实矩阵都是收敛的。

上式构成了指数映射的基础。当矩阵$A$属于某个李群的李代数时，比如$A\in so(3)$，指数映射将其映射到对应的李群上，即$e^A\in SO(3)$。还记得李代数是李群在幺元处的切空间，也就是说，通过指数映射，整个李群完全可以通过幺元处的切空间得到！（实际上，指数映射一般只能将李代数映射到李群的一部分上，但对于$SO(n), SE(n),SE_{k+1}(n)$而言，能够映射到全部，即任何一个属于这些群的元素，一定能在对应李代数中找到一个对应元素）。下面分别给出$SO(n), SE(n),SE_{k+1}(n)$指数映射的解析形式。

$so(3)$到$SO(3)$

为了能够利用李代数的矢量表示（即轴角），我们一般定义其指数映射为

$$\text{Exp}(\bf{\xi_R})=\exp([\bf{\xi_R}]_\times)$$

设$\bf{\xi_R}=\theta \bf{u}$，其中$\theta$是一个标量，代表旋转的角度，$\bf{u}$是单位矢量，表示旋转的轴。则利用指数函数的泰勒展开式，有

$$\text{Exp}(\bf{\xi_R})=I+\theta[\bf{u}]_\times+\frac{1}{2!}\theta^2[\bf{u}]_\times^2+...+\frac{1}{n!}\theta^n[\bf{u}]_\times^n+...$$

注意到

$$\begin{aligned} \\ [\bf{u}]_\times^2&=\bf{u}\bf{u}^T-I \\ [\bf{u}]_\times^3&=-[\bf{u}]_\times \end{aligned}$$

有

$$\begin{aligned} \\ [\bf{u}]_\times^4&=-[\bf{u}]_\times^2 \\ [\bf{u}]_\times^5&=[\bf{u}]_\times \\ [\bf{u}]_\times^6&=[\bf{u}]_\times^2 \\ [\bf{u}]_\times^7&=-[\bf{u}]_\times \\ ... \end{aligned}$$

于是我们对展开式分离奇偶项，可得

$$\text{Exp}(\bf{\xi_R})=I+(\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5-...)[\bf{u}]_\times+(\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+...)[\bf{u}]_\times^2$$

又注意到

$$\begin{aligned} \sin\theta&=\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5-... \\ \cos\theta&=1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4-... \end{aligned}$$

最终可得

$$\text{Exp}(\bf{\xi_R})=I+\sin\theta[\bf{u}]_\times+(1-\cos\theta)[\bf{u}]_\times^2$$

这就是众所周知的罗德里格斯变换。

$se(3)$到$SE(3)$

设$\xi=(\xi_R^T, \xi_t^T)^T$。同样定义

$$\text{Exp}(\xi)=\exp(\Lambda(\xi))$$

推导方式与处理$SO(3)$时完全一样，首先计算泰勒级数，可以得到

$$\text{Exp}(\xi)= \begin{pmatrix} \sum\frac{1}{n!}\theta^n[\bf{u}]_\times^n & (\sum\frac{1}{n!}\theta^{n-1}[\bf{u}]_\times^{n-1})\xi_t \\ 0 & 1 \end{pmatrix}$$

矩阵中左上角元素就是$so(3)$的指数映射，而另外一项，同样利用三角函数极数的对照关系，可以得到

$$\text{Exp}(\xi)= \begin{pmatrix} R & V\xi_t \\ 0 & 1 \end{pmatrix}$$

其中

$$\begin{aligned} R&=I+\sin\theta[\bf{u}]_\times+(1-\cos\theta)[\bf{u}]_\times^2 \\ V&=I+\frac{1-\cos\theta}{\theta}[\bf{u}]_\times+\frac{\theta-\sin\theta}{\theta}[\bf{u}]_\times^2 \end{aligned}$$

$se_{k+1}(3)$到$SE_{k+1}(3)$

简单计算即可发现，$se_{k+1}(3)$的指数映射就是$se(3)$的简单扩展，这里直接给出结果。设$\xi=(\xi_R^T, \xi_t^T, \xi_{p_1}^T, ..., \xi_{p_k}^T)^T$，则

$$\text{Exp}(\xi)= \begin{pmatrix} R & V\xi_t &V\xi_{p_1} & ... & V\xi_{p_k} \\ 0 & 1 & 0 & ... & 0 \\ 0 & 0 & 1 & ... & 0 \\ \vdots \\ 0 & 0 & 0 & ... & 1 \end{pmatrix}$$

其中

$$\begin{aligned} R&=I+\sin\theta[\bf{u}]_\times+(1-\cos\theta)[\bf{u}]_\times^2 \\ V&=I+\frac{1-\cos\theta}{\theta}[\bf{u}]_\times+\frac{\theta-\sin\theta}{\theta}[\bf{u}]_\times^2 \end{aligned}$$

一些指数映射的其他性质

当两个矩阵满足交换律时，即$AB=BA$时，有

$$e^Ae^B=e^{A+B}$$

指数映射的行列式与矩阵的迹还有如下一个美妙关系

$$det(e^A)=e^{tr(A)}$$

群伴随

这里省去很多关于伴随的导出过程，直接给出其定义。

$SO(3)$的伴随

设$R\in SO(3), \xi \in so(3)$，则在$R$处的伴随记为$Ad_R$，定义为

$$R\cdot \text{Exp}(\xi) = \text{Exp}(Ad_R\cdot \xi)\cdot R$$

下面给出$Ad_R$的具体形式。由上式可得

$$\begin{aligned} \text{Exp}(Ad_R\cdot\xi)&=R\cdot\text{Exp}(\xi)\cdot R^{-1} \\ &=R(\sum_n^{\infty}{\frac{1}{n!}[\xi]_\times^n})R^{-1} \\ &=\sum_n^{\infty}{\frac{1}{n!}(R[\xi]_\times R^{-1})^n} \\ &=\exp(R[\xi]_\times R^{-1}) \\ &=\exp([R\xi]_\times) \\ &=\text{Exp}(R\xi) \end{aligned}$$

所以有

$$Ad_R=R$$

关于$R[\xi]_\times R^{-1} = [R\xi]_\times$的证明可见叉乘速查手册。

$SE(3)$与$SE_{k+1}(3)$的伴随

推导方式与$SO(3)$类似，设$T\in SE(3)$，即

$$T= \begin{pmatrix} R & t \\ 0 & 1 \end{pmatrix}$$

又设$\xi=(\xi_R^T, \xi_t^T)^T \in se(3)$，有

$$\begin{aligned} \text{Exp}(Ad_T\cdot\xi)&=\exp(T\Lambda(\xi) T^{-1}) \\ &=\exp \begin{pmatrix} R[\xi_R]_\times R^{-1} & -R[\xi_R]_\times R^{-1}t+R\xi_t \\ 0 & 0 \end{pmatrix} \\ &=\exp \begin{pmatrix} [R\xi_R]_\times & -[R\xi_R]_\times t+R\xi_t \\ 0 & 0 \end{pmatrix} \\ &=\exp \begin{pmatrix} [R\xi_R]_\times & [t]_\times R\xi_R+R\xi_t \\ 0 & 0 \end{pmatrix} \\ &=\exp\Bigg( \Lambda \bigg[ \begin{pmatrix} R & 0 \\ [t]_\times R & R \end{pmatrix} \begin{pmatrix} \xi_R \\ \xi_t \end{pmatrix} \bigg] \Bigg) \\ &=\text{Exp}\Bigg( \begin{pmatrix} R & 0 \\ [t]_\times R & R \end{pmatrix} \begin{pmatrix} \xi_R \\ \xi_t \end{pmatrix} \Bigg) \end{aligned}$$

因此有

$$Ad_T= \begin{pmatrix} R & 0 \\ [t]_\times R & R \end{pmatrix}$$

简单计算可以发现$SE_{k+1}(3)$的伴随也是$SE(3)$的扩展，设$\chi \in SE_{k+1}(3)$，有

$$Ad_\chi = \begin{pmatrix} R & 0 & 0 & 0 & ... & 0\\ [t]_\times R & R & 0 & 0 &... & 0\\ [p_1]_\times R & 0 & R & 0 &... & 0 \\ [p_2]_\times R & 0 & 0 & R &... & 0 \\ \vdots \\ [p_k]_\times R & 0 & 0 & 0 &... & R \end{pmatrix}$$

References

1. Lie Groups for 2D and 3D Transformations
2. Quaternion kinematics for the error-state Kalman filter
3. Naive Lie Theory by John Stillwell
4. The Invariant Extended Kalman Filter as a Stable Observer
5. An EKF-SLAM algorithm with consistency property

---

• Previous
  叉乘速查手册
• Next
  不变扩展卡尔曼滤波（二）：原理与推导