在SLAM后端非线性优化中,李群和李代数是一个绕不开的玩意儿。我们需要借助李代数来表达旋转或者位姿(平移加旋转),进行求导操作。那么,这一篇博客让我们来扒一扒李群和李代数是什么东西。在此之前,你可能有一连串疑问:
问:群是什么?
答:群是一种代数结构。通俗点说,群就是元素集合加上代数运算,使得集合中任意两个元素经过运算后形成的第三个元素仍然在这个集合里面。
问:群需要满足什么性质?
答:任何群都需要满足群公理。一共有四个:封闭性(closure),结合性(associtivity), 单位元(identity),逆元(invertibility)。
问:李群是什么?
答:除了需要满足四个群公理外,李群还需要满足其他性质,例如不可交换。顺便说一下,不可交换的群也称为非阿贝尔群。除此而外,李群是具有微分流形结构的群,群集合中的元素是矩阵。
有了这样一个基本概念后,我们来介绍两个常用李群:特殊正交群$SO(3)$和特殊欧氏群$SE(3)$,以及分别对应的李代数$\mathfrak{so}(3)$和$\mathfrak{se}(3)$。表达旋转时,我们可以用$SO(3)$或者$\mathfrak{so}(3)$;表达位姿时,我们可以用$SE(3)$或者$\mathfrak{se}(3)$。
另外,澄清一下:博客中所有文章,如果不加特殊说明,所有矢量均表示列矢量:记号$\mathbb{R}^n$表示实数域上的$n$维列矢量。
$SO(3)$与$SE(3)$
特殊正交群用来表达旋转,简单的说就是一系列旋转矩阵的集合:$$SO(3) = \{ \mathbf{R} \in \mathbb{R}^{3 \times 3} | \mathbf{R} \mathbf{R}^T = \mathbf{I}, \det(\mathbf{R}) = 1 \}$$ 正交性条件$\mathbf{R} \mathbf{R}^T = \mathbf{I}$在具有9个分量的矩阵上施加了6个约束,使得矩阵自由度变为3。另外,$$\det(\mathbf{R} \mathbf{R}^T) = \det(\mathbf{I}) = 1 \Rightarrow \det(\mathbf{R}) = \pm 1 $$ 当$\det(\mathbf{R}) = -1$时,我们称之为非正常转动(improper rotation),或者旋转反射 (rotary reflection)。选择 $\det(\mathbf{R}) = +1$保证了我们得到的是一个正常的转动 (proper rotation)。
特殊欧氏群用来表达位姿,包括平移和旋转:$$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} \Bigg| \mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^3 \right\} $$
接着,我们来看看$SO(3)$和$SE(3)$需要满足四个群公理,即:
- 封闭性:$\mathbf{R}_1, \mathbf{R}_2 \in SO(3) \Rightarrow \mathbf{R}_1\mathbf{R}_2 \in SO(3)$ and $\mathbf{T}_1, \mathbf{T}_2 \in SE(3) \Rightarrow \mathbf{T}_1\mathbf{T}_2 \in SE(3)$
- 结合性:$\mathbf{R}_1(\mathbf{R}_2\mathbf{R}_3) = (\mathbf{R}_1\mathbf{R}_2)\mathbf{R}_3 = \mathbf{R}_1\mathbf{R}_2\mathbf{R}_3$ and $\mathbf{T}_1(\mathbf{T}_2\mathbf{T}_3) = (\mathbf{T}_1\mathbf{T}_2)\mathbf{T}_3 = \mathbf{T}_1\mathbf{T}_2\mathbf{T}_3$
- 单位元:$\mathbf{R}, \mathbf{I} \in SO(3) \Rightarrow \mathbf{R}\mathbf{I} = \mathbf{I}\mathbf{R} = \mathbf{R}$ and $\mathbf{T}, \mathbf{I} \in SE(3) \Rightarrow \mathbf{T}\mathbf{I} = \mathbf{I}\mathbf{T} = \mathbf{T}$
- 逆元:$\mathbf{R} \in SO(3) \Rightarrow \mathbf{R}^{-1} \in SO(3) $ and $\mathbf{T} \in SE(3) \Rightarrow \mathbf{T}^{-1} \in SE(3)$
我们以封闭性为例,来证一证。先来看$SO(3)$的封闭性:$$(\mathbf{R}_1\mathbf{R}_2)(\mathbf{R}_1\mathbf{R}_2)^T = \mathbf{R}_1 \mathbf{R}_2\mathbf{R}_{2}^{T} \mathbf{R}_{1}^{T} = \mathbf{R}_1 \mathbf{R}_{1}^{T} = \mathbf{I} \qquad \det(\mathbf{R}_1\mathbf{R}_2) =\det(\mathbf{R}_1) \det(\mathbf{R}_2) = 1$$ 所以,$\mathbf{R}_1\mathbf{R}_2 \in SO(3)$,再来看看$SE(3)$的封闭性:
$$\mathbf{T}_1\mathbf{T}_2 = \left [ \begin{array} {*{20}{c}} \mathbf{R}_1 & \mathbf{t}_1 \\ \mathbf{0}^T & 1 \end{array} \right] \left [ \begin{array} {*{20}{c}} \mathbf{R}_2 & \mathbf{t}_2 \\ \mathbf{0}^T & 1 \end{array} \right] = \left [ \begin{array} {*{20}{c}} \mathbf{R}_1 \mathbf{R}_2 & \mathbf{R}_1\mathbf{t}_2 + \mathbf{t}_1 \\ \mathbf{0}^T & 1 \end{array} \right] \in SE(3)$$ 其中,$\mathbf{R}_1\mathbf{R}_2 \in SO(3)$ 且 $\mathbf{R}_1\mathbf{t}_2 + \mathbf{t}_1 \in \mathbb{R}^3$
需要说明的是,以上都是在说$SO(3)$和$SE(3)$对乘法封闭,然而两者对加法并不封闭:$$\mathbf{R}_1, \mathbf{R}_2 \in SO(3) \nRightarrow \mathbf{R}_1 + \mathbf{R}_2 \in SO(3) \qquad and \qquad \mathbf{T}_1, \mathbf{T}_2 \in SE(3) \nRightarrow \mathbf{T}_1 + \mathbf{T}_2 \in SE(3)$$ 正因为对加法的不封闭,后面我们会引入李代数利用扰动法进行导数计算。
其他三个公理有兴趣大家可以自己试着证明一下。不过不证也没关系,因为这在实际工程应用中,这些问题比较trivial,证明的过程纯粹是为了锻炼自己的思维能力。
$\mathfrak{so}(3)$与$\mathfrak{se}(3)$
不论$SO(3)$还是$SE(3)$,都伴随着一个相应的李代数。李代数是定义在某个数域$\mathbb{F}$上的集合$\mathbb{V}$,同时定义了一个称为李括号$[\cdot , \cdot]$的双目运算符。我们把蓝色字体部分称为李代数的三个要素。同李群类似,对所有的$\mathbf{X}, \mathbf{Y}, \mathbf{Z} \in \mathbb{V}$ 和 $a, b \in \mathbb{F}$,李代数也必须满足四个性质:
- 封闭性:$[\mathbf{X}, \mathbf{Y}] \in \mathbb{V}$
- 双线性:$[a\mathbf{X} + b\mathbf{Y}, \mathbf{Z}] = a[\mathbf{X}, \mathbf{Z}] + b[\mathbf{Y}, \mathbf{Z}] \qquad and \qquad [ \mathbf{Z}, a\mathbf{X} + b\mathbf{Y}] = a[\mathbf{Z}, \mathbf{X}] + b[\mathbf{Z}, \mathbf{Y}]$
- 自反性:$[\mathbf{X}, \mathbf{X}] = \mathbf{0}$
- 雅可比恒等:$[\mathbf{X}, [\mathbf{Y}, \mathbf{Z}]] + [\mathbf{Y}, [\mathbf{Z}, \mathbf{X}]] + [\mathbf{Z}, [\mathbf{X}, \mathbf{Y}]] = \mathbf{0}$
李代数所表达的矢量空间是对应李群在单位阵附近的切空间。下面我们分别来介绍$\mathfrak{so}(3)$与$\mathfrak{se}(3)$。
- $\mathfrak{so}(3)$
$SO(3)$对应的李代数$\mathfrak{so}(3)$是定义在$\mathbb{R}^3$上的向量,记作$\mathbf{\boldsymbol{\phi}}$。来看看$\mathfrak{so}(3)$的三个要素:- 集合:$\mathfrak{so}(3) = \{ \mathbf{\Phi} = \boldsymbol{\phi}^{\wedge} \in \mathbb{R}^{3 \times 3} | \boldsymbol{\phi} \in \mathbb{R}^3\} $
- 数域:实数域$\mathbb{R}$
- 李括号:$[\mathbf{\Phi}_1, \mathbf{\Phi}_2] = \mathbf{\Phi}_1\mathbf{\Phi}_2 - \mathbf{\Phi}_2\mathbf{\Phi}_1 \in \mathbb{R}^{3 \times 3}$
其中,$$\mathbf{\Phi} = \boldsymbol{\phi}^{\wedge} = \left [ \begin{array} {*{20}{c}} 0 & -\boldsymbol{\phi}_3 & \boldsymbol{\phi}_2 \\ \boldsymbol{\phi}_3 & 0 & -\boldsymbol{\phi}_1 \\ -\boldsymbol{\phi}_2 & \boldsymbol{\phi}_1 & 0\end{array} \right] \in \mathbb{R}^{3 \times 3}, \quad \boldsymbol{\phi} \in \mathbb{R}^3$$
另外,我们定义运算$[\cdot]^{\vee}$为$[\cdot]^{\wedge}$的逆运算:$$\mathbf{\Phi} = \boldsymbol{\phi}^{\wedge} \Leftrightarrow \boldsymbol{\phi} = \mathbf{\Phi}^{\vee}$$
作为比较trivial的内容,我们来验证一下这样定义的李括号,是否满足上面的四个性质。这里只验证前两个,留两个大家自行验证。
首先,封闭性:$$[\mathbf{\Phi}_1, \mathbf{\Phi}_2] = (\mathbf{\Phi}_1\mathbf{\Phi}_2 - \mathbf{\Phi}_2\mathbf{\Phi}_1) = (\boldsymbol{\phi}_1^{\wedge}\boldsymbol{\phi}_2^{\wedge} - \boldsymbol{\phi}_2^{\wedge}\boldsymbol{\phi}_1^{\wedge}) = (\underbrace{\boldsymbol{\phi}_1^{\wedge}\boldsymbol{\phi}_2}_{\in \mathbb{R}^3})^{\wedge} \in \mathfrak{so}(3) $$ 其次,自反性:$$[\mathbf{\Phi}, \mathbf{\Phi}] = \mathbf{\Phi}\mathbf{\Phi} - \mathbf{\Phi}\mathbf{\Phi} = \mathbf{0}$$
- $\mathfrak{se}(3)$
$SE(3)$对应的李代数$\mathfrak{se}(3)$是定义在$\mathbb{R}^6$上的向量,记作$\boldsymbol{\xi}$。来看看$\mathfrak{se}(3)$的三个要素:
- 集合:$\mathfrak{se}(3) = \{ \mathbf{\Xi} = \boldsymbol{\xi}^{\wedge} \in \mathbb{R}^{4 \times 4} | \boldsymbol{\xi} \in \mathbb{R}^6\} $
- 数域:实数域$\mathbb{R}$
- 李括号:$[\mathbf{\Xi}_1, \mathbf{\Xi}_2] = \mathbf{\Xi}_1\mathbf{\Xi}_2 - \mathbf{\Xi}_2\mathbf{\Xi}_1 \in \mathbb{R}^{4 \times 4}$
其中,$$\mathbf{\Xi} = \boldsymbol{\xi}^{\wedge} = \left [ \begin{array} {*{20}{c}} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{array} \right] ^{\wedge} = \left [ \begin{array} {*{20}{c}} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T & 0 \end{array} \right] \in \mathbb{R}^{4 \times 4}, \quad \boldsymbol{\rho}, \boldsymbol{\phi} \in \mathbb{R}^3$$
上式中,$\boldsymbol{\rho}$表示$\mathfrak{se}(3)$上的平移,它和$SE(3)$上的平移$\mathbf{t}$还有一些不同,后面会说明。
类似地,$$\mathbf{\Xi} = \boldsymbol{\xi}^{\wedge} \Leftrightarrow \boldsymbol{\xi} = \mathbf{\Xi}^{\vee}$$
同样,我们来验证一下封闭性:$$[\mathbf{\Xi}_1, \mathbf{\Xi}_2] = (\mathbf{\Xi}_1\mathbf{\Xi}_2 - \mathbf{\Xi}_2\mathbf{\Xi}_1) = (\boldsymbol{\xi}_1^{\wedge}\boldsymbol{\xi}_2^{\wedge} - \boldsymbol{\xi}_2^{\wedge}\boldsymbol{\xi}_1^{\wedge}) = (\underbrace{\boldsymbol{\xi}_1^{\curlywedge}\boldsymbol{\xi}_2}_{\in \mathbb{R}^6})^{\wedge} \in \mathfrak{se}(3)$$ 注意到运算$[\cdot]^{\curlywedge}$,它表示:$$\boldsymbol{\xi}^{\curlywedge} = \left [ \begin{array} {*{20}{c}} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{array} \right] ^{\curlywedge} = \left [ \begin{array} {*{20}{c}} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho}^{\wedge} \\ \mathbf{0} & \boldsymbol{\phi}^{\wedge} \end{array} \right] \in \mathbb{R}^{6 \times 6}, \quad \boldsymbol{\rho}, \boldsymbol{\phi} \in \mathbb{R}^3 $$
可以看到:旋转如果用李代数$\boldsymbol{\phi}$来表示,依然是3个自由度,这和旋转矩阵$\mathbf{R}$自由度一致;而位姿用李代数$\boldsymbol{\xi}$来表示,也仍然是6个自由度,这和用姿态矩阵$\mathbf{T}$表达也一致,因为其中旋转3个自由度,平移也有3个自由度。那么,我们的问题来了:李群和对应李代数究竟是什么关系?另外,还有一个问题就是用李代数$\boldsymbol{\phi}$表达旋转,或者$\boldsymbol{\xi}$表达位姿有何意义?因为毕竟它们看起来并不直观。不急,下一节指数映射会为我们揭开这些问题的答案!
指数映射
先直接搬结论:李代数经过指数映射会得到对应李群,反过来,李群经过对数映射得到对应李代数 (这个结论可以通过正交性条件对时间求导,然后求解一个常微分方程得出,这里不加证明)。
这里引入方阵的指数映射:$$\exp(\mathbf{A}) = \mathbf{I} + \mathbf{A} + \frac{1}{2!} \mathbf{A}^2 + ... = \sum_{0}^{\infty}{\frac{1}{n!}\mathbf{A}^n} \in \mathbb{R}^{n \times n}, \quad \mathbf{A} \in \mathbb{R}^{n \times n}$$
对于旋转而言,$SO(3)$中的元素和$\mathfrak{so}(3)$中的元素是通过指数映射联系的:$$ \mathbf{R} = \exp(\boldsymbol{\phi}^{\wedge}) = \sum_{0}^{\infty}{\frac{1}{n!} (\boldsymbol{\phi}^{\wedge})^n}$$
同时, 我们记$\boldsymbol{\phi} = \phi \mathbf{a}$,其中,$\phi = \| \boldsymbol{\phi} \| $表示$\boldsymbol{\phi}$的模长,而$\mathbf{a} = \frac{\boldsymbol{\phi}}{\phi}$表示与$\boldsymbol{\phi}$方向一致的单位向量。这样,指数映射就可表达为:
\begin{equation*}
\begin{split}
\exp(\boldsymbol{\phi}^{\wedge}) &= \exp((\phi \mathbf{a})^{\wedge}) = \exp(\phi \mathbf{a}^{\wedge}) \\
&= \underbrace{\mathbf{I}}_{\mathbf{a} \mathbf{a}^T - \mathbf{a}^{\wedge}\mathbf{a}^{\wedge}} + \phi \mathbf{a}^{\wedge} + \frac{1}{2!}\phi^2\mathbf{a}^{\wedge}\mathbf{a}^{\wedge} + \frac{1}{3!}\phi^3\underbrace{\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}}_{-\mathbf{a}^{\wedge}} + \frac{1}{4!}\phi^4\underbrace{\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}}_{-\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}} + ... \\
& = \mathbf{a} \mathbf{a}^T + \underbrace{(\phi - \frac{1}{3!}\phi^3+ \frac{1}{5!}\phi^5 - ...)}_{\sin \phi}\mathbf{a}^{\wedge} - \underbrace{(1-\frac{1}{2!}\phi^2 + \frac{1}{4!}\phi^4 - ...)}_{\cos \phi} \underbrace{\mathbf{a}^{\wedge} \mathbf{a}^{\wedge}}_{-\mathbf{I} + \mathbf{a} \mathbf{a}^T} \\
&= \cos \phi \mathbf{I} + (1 - \cos \phi) \mathbf{a} \mathbf{a}^T + \sin \phi \mathbf{a}^{\wedge}
\end{split}
\end{equation*}
也就是说:$$\mathbf{R} = \cos \phi \mathbf{I} + (1 - \cos \phi) \mathbf{a} \mathbf{a}^T + \sin \phi \mathbf{a}^{\wedge}$$ 最后这个式子,跟旋转向量转成旋转矩阵的罗德里格斯公式一模一样。 因此,在数学意义上,我们可以说: 李代数$\boldsymbol{\phi}$表征的就是一个旋转向量,$\boldsymbol{\phi}$的模长表达了旋转的角度,$\boldsymbol{\phi}$的方向表达了旋转轴的方向。
反过来,当给定一个旋转矩阵$\mathbf{R}$,我们怎么转成李代数$\boldsymbol{\phi}$形式?这个问题也等价于,如何把一个旋转矩阵转成对应的旋转向量形式?你会说,很简单对数映射嘛!$$\boldsymbol{\phi} = \ln(\mathbf{R})^{\vee}$$ 但从计算角度来说,这不太方便。我们有更简单的求解方式:
首先,来说角度$\phi$,我们把罗德里格斯公式两边同时取迹$\text{tr}(\cdot)$: $$\text{tr}(\mathbf{R}) = \cos \phi \underbrace{\text{tr}(\mathbf{I})}_{=3} +(1 - \cos \phi) \underbrace{\text{tr}(\mathbf{a} \mathbf{a}^T)}_{=1} + \sin \phi \underbrace{\text{tr}(\mathbf{a}^{\wedge})}_{=0} = 3 \cos \phi + (1 - \cos \phi) = 1 + 2 \cos \phi$$ 因此,$$\phi = \arccos(\frac{\text{tr}(\mathbf{R}) - 1}{2})$$ 需要说明的是,旋转角度由于具有周期性(旋转$2\pi$和没有旋转是一回事),$SO(3)$到$\mathfrak{so}(3)$的映射不是唯一的,但是我们利用了$\arccos(\cdot)$来限制角度的取值范围,这种意义下,$SO(3)$和$\mathfrak{so}(3)$之间的映射可以保证是单射。
其次,来求解旋转轴$\mathbf{a}$,如果我们把旋转轴拿来绕着自身旋转,那么它的结果也不会变。因此:$$\mathbf{R} \mathbf{a} = \mathbf{a}$$ 代数意义就是说,旋转轴是旋转矩阵特征值为1所对应的特征向量。那么,这个问题只需要做特征分解就可以搞定了。
同样地,我们通过指数映射也可以把$\mathfrak{se}(3)$映射到$SE(3)$:$$\mathbf{T} = \exp(\boldsymbol{\xi}^{\wedge}) = \sum_{0}^{\infty}{\frac{1}{n!}(\boldsymbol{\xi}^{\wedge})^n}$$ 反过来,从$SE(3)$到$\mathfrak{se}(3)$的对数映射为:$$\boldsymbol{\xi} = \ln(\mathbf{T})^{\vee}$$
现在我们来导出指数映射的简洁结果:
\begin{equation*}
\begin{split}
\exp(\boldsymbol{\xi}^{\wedge}) &= \sum_{0}^{\infty}{\frac{1}{n!}(\boldsymbol{\xi}^{\wedge})^n} \\
&= \sum_{0}^{\infty}\frac{1}{n!}\left( \left [ \begin{array} {*{20}{c}} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{array} \right] ^ {\wedge}\right) ^{n} \\
&= \sum_{0}^{\infty}\frac{1}{n!}\left [\begin{array} {*{20}{c}} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T & 0 \end{array} \right]^{n} \\
&=\left [ \begin{array} {*{20}{c}} \sum_{0}^{\infty}\frac{1}{n!}(\boldsymbol{\phi}^{\wedge})^n & \sum_{0}^{\infty}\frac{1}{(n+1)!}(\boldsymbol{\phi}^{\wedge})^n\boldsymbol{\rho} \\ \mathbf{0}^{T} & 1 \end{array} \right] \\
&= \left [ \begin{array} {*{20}{c}} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^{T} & 1\end{array} \right] \in SE(3)
\end{split}
\end{equation*}
其中,$\mathbf{t} = \mathbf{J} \boldsymbol{\rho} \in \mathbb{R}^3, \quad \mathbf{J} = \sum_{0}^{\infty}\frac{1}{(n+1)!}(\boldsymbol{\phi}^{\wedge})^n$。利用反对称矩阵乘积的性质,像之前导出旋转指数映射的过程一样化简就可以得到($\boldsymbol{\phi} = \phi \mathbf{a}$):$$\mathbf{J} = \frac{\sin \phi}{\phi} \mathbf{I} + \left(1 - \frac{\sin \phi}{\phi} \right) \mathbf{a} \mathbf{a}^T + \frac{1-\cos \phi}{\phi} \mathbf{a}^{\wedge} \in \mathbb{R}^{3 \times 3}$$ 现在,我们清楚了$\mathfrak{se}(3)$上的平移$\boldsymbol{\rho}$和$SE(3)$上的平移$\mathbf{t}$的联系,两者并非一回事。反过来,我们也可以知道:$\boldsymbol{\rho} = \mathbf{J}^{-1} \mathbf{t}$,其中,$\mathbf{J}^{-1}$也有解析的表达式:$$ \mathbf{J}^{-1} = \frac{\phi}{2}\cot\frac{\phi}{2}\mathbf{I} + \left(1- \frac{\phi}{2}\cot\frac{\phi}{2} \right) \mathbf{a} \mathbf{a}^T -\frac{\phi}{2}\mathbf{a}^{\wedge}$$
反过来,当给定一个位姿矩阵$\mathbf{T}$,我们怎么转成李代数$\boldsymbol{\xi}$形式?答案当然还是对数映射,但是仍然计算不太方便。
一种比较简便的方式是:通过$\mathbf{R}$先计算$\boldsymbol{\phi}$,然后通过$\boldsymbol{\phi}$计算$\mathbf{J}$,再通过$\mathbf{t} = \mathbf{J} \boldsymbol{\rho}$去求解线性方程组,从而算出$\boldsymbol{\rho}$得到$\boldsymbol{\xi}$。
几个恒等式
现在给出一些非常有用的恒等式,在利用李代数进行导数计算的时候,我们经常会用到,请务必掌握。
等式一:$$\mathbf{R} \mathbf{p}^{\wedge}\mathbf{R}^T = (\mathbf{R}\mathbf{p})^{\wedge}, \quad \mathbf{R} \in SO(3), \mathbf{p} \in \mathbb{R}^3$$