视觉SLAM中的数学基础 第四篇 李群与李代数(2)

前言

  理解李群与李代数,是理解许多SLAM中关键问题的基础。本讲我们继续介绍李群李代数的相关知识,重点放在李群李代数的微积分上,这对解决姿态估计问题具有重要意义。


 回顾

  为了描述三维空间里的运动,我们使用3$\times $3的旋转矩阵$\mathbf{R}$来描述一个刚体的旋转,并且,用4$\times$4的变换矩阵来描述六自由度的旋转+平移。这两种矩阵在传统的欧氏空间$\mathbb{R}^{3 \times 3}$和$\mathbb{R}^{4 \times 4}$中,不存在加法运算,只有乘法运算,故无法构成线性空间,只能构成群。也就是我们说的李群$SO(3)$和$SE(3)$:

\[\begin{equation}
SO(3) = \{ \mathbf{R} \in \mathbb{R}^{3 \times 3} | \mathbf{R R}^T = \mathbf{I}, det(\mathbf{R})=1 \}
\end{equation}\]

\[\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}\]

   在李群中,我们使用矩阵来表达一个旋转和平移,这存在冗余的自由度。三维空间的旋转只有三自由度,旋转+平移有六自由度。因此,我们希望寻找一个没有冗余自由度(但是相应的存在奇异性)的表示,也就是李代数$\mathfrak{so}(3)$和$\mathfrak{se}(3)$:

\[\begin{equation}
\mathfrak{so}(3) = \{ \mathbf{\phi} \in \mathbb{R}^{3} \}
\end{equation}\]

\[\begin{equation}
\mathfrak{se}(3) = \{ \mathbf{\xi } \in \mathbb{R}^{6} \}
\end{equation}\]

  怎么将李代数和李群元素对应起来呢?我们说,从李代数到李群,通过指数映射;反之,从李群到李代数,则通过对数映射

  $\mathfrak{so}(3)$的一个元素$\mathbf{\phi}=\mathbf{a} \theta $的指数映射为:

\[\begin{equation} exp\left( {{\phi ^ \wedge }} \right) = \cos \theta {\bf{I}} + (1 - \cos \theta ){\bf{a}}{{\bf{a}}^T} + \sin \theta {{\bf{a}}^ \wedge } \end{equation}\]

  其中$\wedge$算符把一个三维向量变换为它对应的反对称矩阵(3$\times $3)。

  该式和Rodrigues(罗德里格斯)公式是一致的。说明李代数$\mathfrak{so}(3)$里的向量,其方向指向旋转轴,而模长表示转过的角度。因此,平时用的rpy欧拉角,实际上是李代数之一。另一点需要说明的是,指数映射是一个满射而非单射。由于旋转具有周期性,多转360度和不转,表示的是同一个旋转。如果我们把旋转限制在正负180度内,那么李群到李代数就是一一对应的。

  反之,对数映射$\ln {\left( \mathbf{R} \right)^ \vee }$将一个旋转矩阵转换为一个李代数。但由于对数的泰勒展开不是很优雅,通常直接按照下式计算李代数向量的大小和方向:

\[ \begin{equation} \begin{array}{l}
\theta = {\cos ^{ - 1}}\frac{{tr\left( \mathbf{R} \right) - 1}}{2} + 2k\pi \\
\mathbf{Ra = a}
\end{array} \end{equation} \]

  根据该结果得到的$\mathbf{\phi} = \theta \mathbf{a}$就是对应的李代数了。到这为止的内容都在上一讲中介绍过,本讲我们就接着往下说。


 $SE(3)$中的指数映射和对数映射

  $\mathfrak{se}(3)$里的向量经常写成$\mathbf{\xi}$。它是一个六维向量,通常前三维为平移,后三维为旋转(也就是$\mathfrak{so}(3)$里的元素)。请注意,有些地方的定义是倒过来的,即前三维为旋转,后三维为平移,在使用的时候一定要小心!例如g2o的李代数和Sohpus的李代数定义就不一样。

   记$\mathbf{\xi}=[\mathbf{\rho}, \mathbf{\phi}]^T$,它们各为三维向量,一共组成了六维,构成一个李代数。我们的记法里$\rho$为平移,$\phi$为旋转,与$SO(3)$中一致。

  与$SO(3)$类似,$\mathfrak{se}(3)$到$SE(3)$的指数映射为:

\[ \begin{equation} \begin{array}{lll}
\exp \left( {{\xi ^ \wedge }} \right) &=& \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {{\xi ^ \wedge }} \right)}^n}} \\
&=& \left[ {\begin{array}{*{20}{c}}
{\exp \left( {{\phi ^ \wedge }} \right)}&{ \mathbf{J\rho} }\\
{{0^T}}&1
\end{array}} \right]
\end{array} \end{equation} \]

   请注意该式左上即 $SO(3)$ 的指数映射,右上较特殊一些,是平移量 $\mathbf{\rho}$ 的线性表示。其系数$\mathbf{J}_{3\times 3}$的计算方式如下:

\[\begin{array}{lll}
\mathbf{J} &=& \sum\limits_{n = 0}^\infty {\frac{1}{{\left( {n + 1} \right)!}}{{\left( {{ \mathbf{\phi} ^ \wedge }} \right)}^n}} \\
&=& \frac{{\sin \theta }}{\theta }I + \left( {1 - \frac{{\sin \theta }}{\theta }} \right) \mathbf{a}{\mathbf{a}^T} + \frac{{1 - \cos \theta }}{\theta }{\mathbf{a}^ \wedge }
\end{array}\]

   它与Rodrigues相似但有差别,实际上是对$\phi$求了一次导数,所以这个阵也就记成了$\mathbf{J}$,意为一个雅可比阵。它在后续许多地方会用到,所以现在单独提一下它。

  这个矩阵是可逆的,它的逆形式如下:

\[\begin{equation} {\mathbf{J}^{ - 1}} = \frac{\theta }{2}\cot \frac{\theta }{2}I + \left( {1 - \frac{\theta }{2}\cot \frac{\theta }{2}} \right)a{a^T} - \frac{\theta }{2}{a^ \wedge } \end{equation}\]

  嗯,现在我们说清了$SE(3)$上的指数映射。那么它的对数映射怎么计算呢?也就是一个$SE(3)$中的矩阵怎么找到对应的向量呢?我们并不需要实际地计算一个矩阵对数,而只需要计算: 

  对于

\[\mathbf{T} = \left[ {\begin{array}{*{20}{c}}
\mathbf{R} & \mathbf{t}\\
{{0^T}}&1
\end{array}} \right]\]

  它对应的李代数 $\mathbf{\xi}=[\mathbf{\rho}, \mathbf{\phi}]^T$ 为:

  \[ \begin{equation} \phi  = \ln {\left( R \right)^ \vee },\rho  = {J^{ - 1}}t \end{equation} \]

   因为已经有了$SO(3)$上的计算方式,这里的式子就变得十分简单了。


 Baker-Campbell-Hausdorff公式

  小萝卜:师兄我还有个问题啊。

  师兄:嗯你说。

  小萝卜:李群和李代数的对应关系倒是不难啦。但实际用的时候,我经常在李群上做两个矩阵的乘法。例如,做$\mathbf{R_1} \cdot \mathbf{R_2}$时,它们对应的李代数发生了什么呢?是不是直接加起来啊

  对,这就是我们要讨论的问题,即:

  \[ \begin{equation} \exp \left( {\phi _1^ \wedge } \right)\exp \left( {\phi _2^ \wedge } \right) = \exp \left( {{{\left( {{\phi _1} + {\phi _2}} \right)}^ \wedge }} \right) \end{equation} \]

  成立吗?

  如果该式成立的话,李群和李代数的结构就很方便了。因为我们可以直接把李群的乘法变成李代数的加法,而不需要改变任何东西。直观上,两个指数函数相乘,也确实等于把幂加起来,再取指数。唯一不同的是,这里我们不是对一个数求指数,而是求一个反对称矩阵的指数

  但是成不成立呢?

  很遗憾,虽然我们非常希望该式成立,但是实质上它是不满足的。Baker-Campbell-Hausdorff公式(https://en.wikipedia.org/wiki/Baker%E2%80%93Campbell%E2%80%93Hausdorff_formula)给出了这个运算的答案:

\[\ln \left( {\exp \left( A \right)\exp \left( B \right)} \right) = \sum\limits_{n = 1}^\infty  {\frac{{{{\left( { - 1} \right)}^{n - 1}}}}{n}\sum\limits_{{r_i} + {s_i} > 0,i \in \left[ {1,n} \right]}^{} {\frac{{{{\left( {\sum\limits_{i = 1}^n {\left( {{r_i} + {s_i}} \right)} } \right)}^{ - 1}}}}{{\prod\limits_{i = 1}^n {{r_i}!{s_i}!} }}\left[ {{A^{{r_1}}}{B^{{s_1}}}{A^{{r_2}}}{B^{{s_2}}} \cdots {A^{{r_n}}}{B^{{s_n}}}} \right]} } \]

  那么这式子在说什么鬼呢?

  为了避免过于冗长的解释,我们就直接看它的近似式好了:

  \[\ln \left( {\exp \left( A \right)\exp \left( B \right)} \right) = A + B + \frac{1}{2}\left[ {A,B} \right] + \frac{1}{{12}}\left[ {A,\left[ {A,B} \right]} \right] - \frac{1}{{12}}\left[ {B,\left[ {A,B} \right]} \right] +  \cdots \]

  这里的方括号指的是李括号,也就是:

\[\left[ {A,B} \right] = AB - BA\]

  特别地,在$SO(3)$上,我们可以求它关于某一个变量的一阶近似:

\[ \begin{equation} \ln {\left( {{R_1}{R_2}} \right)^ \vee } \approx \left\{ \begin{array}{l}
{J_l}{\left( {{\phi _2}} \right)^{ - 1}}{\phi _1} + {\phi _2}\\
{J_r}{\left( {{\phi _1}} \right)^{ - 1}}{\phi _2} + {\phi _1}
\end{array} \right. \end{equation} \]

  对我们用处最大的就是这个近似式。在 $\phi_1$ 较小时,使用第一个式;在在 $\phi_2$ 较小时,使用第二个式。这里的 $J_l$ 和 $J_r$ 也称为左/右雅可比——从而李代数就分成了左右两种模型(因此,李代数程序库会声明它使用的是左乘模型还是右乘模型)。

  那么这俩雅可比是啥类?

  事实上,前面已经提到过它了:

\[ \begin{equation} {J_l} = J = \sum\limits_{n = 0}^\infty  {\frac{1}{{\left( {n + 1} \right)!}}{{\left( {{\phi ^ \wedge }} \right)}^n}} ,{J_r} = J\left( { - \phi } \right) \end{equation}\]

  在这个近似下,我们只保留了和小量相关的一阶信息,而舍弃了它的高阶项。因此,可以把这个过程类比于保留泰勒展开的一阶项而舍弃高阶项的过程,其中泰勒展开类比于BCH公式。


 BCH近似的直观意义

   以左乘形式($\phi_1$较小)为例。我们给一个旋转矩阵 $\mathbf{R}$ 左乘一个微小扰动 $\Delta R = \exp \left( {\Delta {\phi ^ \wedge }} \right)$,得到了$\Delta R \cdot R$。那么,对应到李代数上的变化量为:

  \[ \begin{equation} \ln {\left( {\Delta R \cdot R} \right)^ \vee } = \phi  + J_l^{ - 1}\left( \phi  \right)\Delta \phi \end{equation} \]

  反过来说,如果对某个固定的李代数$\phi$增加一个小量$\delta \phi$,对应到李代数上,为:

  \[ \begin{equation} \exp {\left( {\phi  + \delta \phi } \right)^ \wedge } = \exp {\left( {{J_l}\delta \phi } \right)^ \wedge } \cdot \exp {\left( \phi  \right)^ \wedge } \end{equation} \]

  相当于左乘一个 $J_l \phi$ 对应的旋转阵。类似地,由于左右对称,有:

  \[ \begin{equation} \exp {\left( {\phi  + \delta \phi } \right)^ \wedge } = \exp {\left( {{J_l} \delta \phi } \right)^ \wedge } \cdot \exp {\left( \phi  \right)^ \wedge } = \exp {\left( \phi  \right)^ \wedge }\exp {\left( {{J_r}\delta \phi } \right)^ \wedge } \end{equation} \]

  从这两个式子可以清楚地看到,BCH近似为我们指明了,李群与李代数微小量之间的关系——即,在BCH线性近似的意义下,它们只差一个雅可比矩阵。当区别左右乘时,这两个雅可比自变量差一个符号。


 应用:微分模型和扰动模型

  实际中,我们经常考虑这样一个问题(我们先讨论$SO(3)$上,再讨论$SE(3)$上):

  设一个空间点$P=[x,y,z]$,它经过一次旋转,变成$RP$。问$RP$随着$R$是如何变化的?

  由于$SO(3)$上面没有加法,我们无法使用传统导数的定义(因为 $R+\Delta R$ 不再是旋转矩阵):

  \[\frac{{d\left( {RP} \right)}}{{dR}} = \mathop {\lim }\limits_{\Delta R \to 0} \frac{{\left( {R + \Delta R} \right)P - RP}}{{\Delta R}}\]

  为了解决这个问题,有两种方式:一是把增量定义在李代数上,对应于微分模型;二是把增量直接乘在$R$上,对应于扰动模型。如果读者熟悉了前面的公式,那这件事情是很轻松的。

  1. 定义在李代数上的增量

  在这个意义下,导数的含义为(最后一步跳过了若干过程,实际需要分开对各分量计算导数):

\[\begin{array}{lll}
\frac{{d\left( {RP} \right)}}{{d\phi }} &=& \mathop {\lim }\limits_{\delta \phi \to 0} \frac{{\exp {{\left( {\phi + \delta \phi } \right)}^ \wedge }P - \exp {{\left( \phi \right)}^ \wedge }P}}{{\delta \phi }}\\
&=& \mathop {\lim }\limits_{\Delta \phi \to 0} \frac{{(\exp {{\left( {{J_l}\delta \phi } \right)}^ \wedge } - 1)exp{{\left( \phi \right)}^ \wedge }P}}{{\delta \phi }}\\
&=&  - {(RP)^ \wedge }{J_l}
\end{array}\]

  2. 定义在李群上的乘增量

   另一种方式是将小量乘在李群上,而非加在李代数上。这种方式定义的模型称为扰动模型。由于左右的差异,需要区别这个增量是左乘在旋转阵,还是右乘在旋转阵上的。如果使用左乘,那么导数的含义为:

\[\begin{equation} \begin{array}{l}
\frac{{d\left( {RP} \right)}}{{dR}} = \mathop {\lim }\limits_{\delta \phi \to 0} \frac{{\exp {{\left( {\delta \phi } \right)}^ \wedge } \cdot RP - RP}}{{\delta \phi }}\\
= \mathop {\lim }\limits_{\Delta \phi \to 0} \frac{{(\exp {{\left( {\delta \phi } \right)}^ \wedge } - 1)RP}}{{\delta \phi }}\\
= - {\left( {RP} \right)^ \wedge }
\end{array} \end{equation} \]

  可见,由于我们把小量乘在李群上(而非加在李代数上),结果中的导数将少一个雅可比矩阵。因此,扰动模型计算更为简洁,在实际上更加常用。


 $SE(3)$上扰动模型

  $SE(3)$上扰动模型为:考虑一个空间点$P$(必须为齐次坐标,否则维数不对)受到刚体变换 $T$ ,得到 $TP$ 。求 $TP$ 是如何随 $T$ 变化的。

  类似于$SO(3)$,我们给$T$左乘一个小量$\Delta T=exp(\delta \xi)^\wedge $,得到:

\[ \begin{equation}\begin{array}{l}
\frac{{d\left( {TP} \right)}}{{dT}} = \mathop {\lim }\limits_{\delta \xi \to 0} \frac{{\exp {{\left( {\delta \xi } \right)}^ \wedge }TP - TP}}{{\delta \xi }}\\
= {(TP)^ \odot }
\end{array}\end{equation} \]

  其中$^ \odot$算符把一个4$\times$4的矩阵变换成一个4$\times$6的矩阵,计算方式如下:

\[ \begin{equation} p = \left[ \begin{array}{l}
{x_{3 \times 1}}\\
{w_{1 \times 1}}
\end{array} \right], \quad {p^ \odot } = \left[ {\begin{array}{*{20}{c}}
{w{I_{3 \times 3}}}&{ - {x^ \wedge }_{3 \times 3}}\\
{{0^T}_{3 \times 1}}&{{0^T}_{3 \times 1}}
\end{array}} \right] \end{equation} \]

  至此,我们明确了一个被变换之后的点,是如何随变换矩阵而变化的。注意,我们省略了$SE(3)$上BCH近似雅可比的推导(比较复杂且扰动模型里没有该雅可比),如果读者感兴趣可参考[1]。

  小萝卜:师兄,知识我倒是知道了,您能讲讲$^ \odot$怎么念吗?

  师兄:诶?……你看它像一个石子掉进井里,就念"dong"吧。

  小萝卜:所以$SE(3)$对李代数的扰动的导数就是那个点自己的“dong”喽?

  师兄:你愿意怎么记就怎么记吧……我不管了。


 小结

  本节介绍了以下几个关于李群代数的知识:

  1.  $SE(3)$的结构,指数映射和对数映射;

  2.  BCH公式与近似公式;

  3.  李代数上的微分运算。

  其中,介绍微分是介绍李代数的主要目的所在。知道了如何对位姿求导,我们才能深入地讨论姿态估计,直接法VO等话题。下一讲我们将介绍direct VO和semi-direct VO的方法

 [1]. Barfoot, State Estimation for Robotics: A Matrix-Lie-Group Approach, 2016

posted @ 2016-06-12 20:08 半闲居士 阅读(...) 评论(...) 编辑 收藏