SLAM中的李群和李代数

群

定义

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

• 封闭性
• 结合律
• 幺元
• 逆

李群的定义

李群是指具有光滑连续性质的群

特殊正交群

特殊正交群SO(n) 也就是所谓的旋转矩阵群，其中SO(2)和SO(3)最为常见。正式的记法是：

\[SO(n)=\{R∈\R_{n×n}|RR^{T}=I,det(R)=1\} \]

假设有这样一个矩阵\(R\)，满足\(RR^T＝I\)。
现在，考虑它随时间发生变化，即从\(R\) 变成了\(R(t)\)，仍有\(R(t)R(t)^T=I\)。

\[R˙(t)R(t)^T+R(t)R˙(t)^T=0 \]

则：

\[R˙(t)R(t)^T=−(R˙(t)R(t)^T)^T \]

可以看出\(R˙(t)R(t)^T\)是一个反对称矩阵,则我们可以找到一个三维向量\(ϕ(t)∈R^3\)与之对应。于是有：

\[R˙(t)R(t)^T=ϕ(t)^∧ \]

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

\[R˙(t)=ϕ(t)^∧R(t)= \left[ \begin{matrix} 0 & ϕ3 & −ϕ2 \\ −ϕ3 & 0 & ϕ1 \\ ϕ2 & −ϕ1 & 0 \end{matrix} \right] R(t)\]

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

\[R(t)=exp(ϕ(t)^∧)R(t0) \]

由此我们可以引出两个概念。

• 求ϕ的方法以及它的结构？——ϕ是对应到SO(3)上的李代数so(3)
• exp(ϕ)如何计算？——李群与李代数间的指数/对数映射

即：李代数表达的正切空间，具有和对应李群相同的自由度。指数映射能把正切空间中任意向量正好映射到原李群。

特殊欧式群

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

\[SE(3)=\{T= \left[ \begin{matrix} R^T & t \\ 0^T & 1 \end{matrix} \right] ∈R_{4×4}|R∈SO(3),t∈R^3\} \]

李代数

对于SO(3)和SE(3)，李代数可定义于李群的正切空间上，描述了李群中元素局部性质。

定义

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

• 封闭性
• 双线性
• 自反性
• 雅可比等价性

三维旋转群与对应的李代数

\(SO(3)\) 对应的李代数是定义在\(R^3\)上的向量，我们记作\(ϕ\)(注意这是个向量)。根据前面的推导，每个\(ϕ\)都可以生成一个反对称矩阵：

\[Φ=ϕ^∧= \left[ \begin{matrix} 0 & ϕ3 & −ϕ2 \\ −ϕ3 & 0 & ϕ1 \\ ϕ2 & −ϕ1 & 0 \end{matrix} \right] ∈R^{3×3} \]

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

\[[ϕ1,ϕ2]=Φ1Φ2−Φ2Φ1 \]

所以三维旋转群与对应的李代数\(\frak{so(3)}\)：

\[\frak{so(3)} \rm=\{Φ=ϕ^∧∈R^{3×3}|ϕ∈R^3\} \]

至此，我们已清楚\(\frak{so(3)}\)的结构。它们是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。

三维欧氏群与对应的李代数

\[SE(3) = \left\{ \mathbf{T} = \left[ {\begin{matrix} R & {t} \\ {{{0}^T}} & 1 \end{matrix}} \right] ∈ {R}^{4 \times 4} | {R} \in SO(3), \mathbf{t} \in \mathbb{R}^3 \right\} \]

每个变换矩阵有六个自由度，故对应的李代数位于\(\mathbb{R}^6\),但是∧不再对应到一个反对称关系，而是：

\[\mathbf{\xi}^\wedge = { \left[ \begin{array}{l} \mathbf{\rho} \\ \mathbf{\phi} \end{array} \right]^ \wedge } = \left[ {\begin{array}{*{20}{c}} {{\mathbf{\phi} ^ \wedge }}&\mathbf{\rho} \\ {{\mathbf{0}^T}}&0 \end{array}} \right] = \mathbf{\Xi} \]

\(\mathbf{\xi}\)的前三维为旋转向量，后三维为平移向量，其定义也十分的直观。该李代数对应于微分方程：

\[\mathbf{\dot{T}}(t) = \mathbf{\xi}^\wedge(t) \mathbf{T}(t) \]

因此，

\[\begin{equation} \mathbf{T}(t) = \exp ( \mathbf{\xi}(t)^\wedge ) \mathbf{T}(t) \end{equation} \]

指数映射和对数映射

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

\[exp(A)=\sum_{n=0}^{\infty}{\frac{1}{n!}A^n} \]

• \(\frak{so(3)}\)中的指数映射
  因此，对\(\frak{so(3)}\)中任意一元素\(ϕ\)，我们亦可按此方式定义它的指数映射：

\[exp(ϕ^∧)=\sum_{n=0}^{\infty}{\frac{1}{n!}(ϕ^∧)^n} \]

由于\(ϕ\)是三维向量，我们可以定义它的模长和它的方向，分别记作\(θ\)和\(a\)(注意这里记号是有含义的，此时\(a\)是一个单位长度的向量)，则我们可以得到:

\[exp(θa)=cosθI+(1−cosθ)aa^T+sinθa^∧ \]

它和罗德里格斯公式(参观本系列第一篇)如出一辄。这表明，\(\frak{so(3)}\)实际上就是由所谓的旋转向量组成的空间(注意，SO(3)是旋转矩阵群)。通过罗德里格斯公式或者指数映射，我们把R3 中的一个向量对应到了一个位于SO(3)中的3D旋转。说明李代数\(\frak{so(3)}\)里的向量，其方向指向旋转轴，而模长表示转过的角度。因此，平时用的rpy欧拉角，实际上是李代数之一。
对应的，如果定义对数映射，我们也能把SO(3)中的元素对应到\(\frak{so(3)}\)中：

\[ϕ=ln(R)^∨=(\sum_{n=0}^{\infty}\frac{(−1)^n}{n+1}(R−I)^{n+1})^∨ \]

其中∨表示从反对称矩阵到向量的对应关系，为∧的逆运算。\(SO(3)\)与\(\frak{so(3)}\)旋转向量与旋转矩阵很相似，而指数映射即是罗德里格斯公式。旋转向量可以视为旋转矩阵的导数，指导如何在旋转矩阵中进行微积分运算。

• \(\frak{se(3)}\)中的指数映射

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

左上角的\(Φ\)是我们熟知的\(so(3)\)中的元素，前文已经介绍过了。而右上角的\(J\)则可整理为(设\(ϕ=θa\))：

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

BCH近似

我们想要知道当李群发生乘法时，李代数上是不是发生加法，即:

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

是否成立？他们并不成立，BCH给出了答案：

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

这里给出BCH的近似：

\[\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)上，我们可以求它关于某一个变量的一阶近似：

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

在 \(ϕ1\) 较小时，使用第一个式；在在$ ϕ2 $较小时，使用第二个式。这里的 \(J_l\) 和 $J_r $也称为左/右雅可比——从而李代数就分成了左右两种模型:

\[{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) \]

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

因此，

\[exp(\Delta \phi^\wedge)exp(\phi^\wedge)={\Delta R \cdot R} =exp( \phi + J_l^{ - 1}\left( \phi \right)\Delta \phi)^\wedge \]

我们将\(J_l^{ - 1}\left( \phi \right)\Delta \phi\)看作\(\Delta \phi\)，则有$$\exp {\left( {\phi + \Delta \phi } \right)^ \wedge } = \exp {\left( {{J_l}\Delta \phi } \right)^ \wedge } \cdot \exp {\left( \phi \right)^ \wedge }$$因此，当我们在李代数上进行加法，则可以近似为李群上带左右雅可比的乘法。
相当于左乘一个 \(J_lϕ\) 对应的旋转阵。类似地，由于左右对称，有：

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

在BCH线性近似的意义下，李群与李代数微小量之间只差一个雅可比矩阵。当区别左右乘时，这两个雅可比自变量差一个符号。

微分模型和扰动模型

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

为了解决这个问题，有两种方式：

• 一是把增量定义在李代数上，对应于微分模型；
• 二是把增量直接乘在R上，对应于扰动模型。

定义在李代数上的增量

在这个意义下，导数的含义为:

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

定义在李群上的乘增量

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

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

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