SLAM中的李群和李代数

群

定义

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

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

李群的定义

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

特殊正交群

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

SO(n)={R∈\Rn×n|RRT=I,det(R)=1}" role="presentation">SO(n)={R∈\Rn×n|RRT=I,det(R)=1}$$SO(n)=\{R\in {\text{R}}_{n\times n}|R{R}^T=I,det(R)=1\}$$

假设有这样一个矩阵R" role="presentation">R$R$，满足RRT＝I" role="presentation">RRT＝I$R{R}^T＝I$。
现在，考虑它随时间发生变化，即从R" role="presentation">R$R$ 变成了R(t)" role="presentation">R(t)$R(t)$，仍有R(t)R(t)T=I" role="presentation">R(t)R(t)T=I$R(t)R(t{)}^T=I$。

R˙(t)R(t)T+R(t)R˙(t)T=0" role="presentation">R˙(t)R(t)T+R(t)R˙(t)T=0$$R\dot{}(t)R(t{)}^T+R(t)R\dot{}(t{)}^T=0$$

则：

R˙(t)R(t)T=−(R˙(t)R(t)T)T" role="presentation">R˙(t)R(t)T=−(R˙(t)R(t)T)T$$R\dot{}(t)R(t{)}^T=-(R\dot{}(t)R(t{)}^T{)}^T$$

可以看出R˙(t)R(t)T" role="presentation">R˙(t)R(t)T$R\dot{}(t)R(t{)}^T$是一个反对称矩阵,则我们可以找到一个三维向量ϕ(t)∈R3" role="presentation">ϕ(t)∈R3$\varphi (t)\in R^3$与之对应。于是有：

R˙(t)R(t)T=ϕ(t)∧" role="presentation">R˙(t)R(t)T=ϕ(t)∧$$R\dot{}(t)R(t{)}^T=\varphi (t{)}^{\wedge }$$

左右各右乘R(t)" role="presentation">R(t)$R(t)$，由于R" role="presentation">R$R$为正交阵，有：

R˙(t)=ϕ(t)∧R(t)=[0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10]R(t)" role="presentation">R˙(t)=ϕ(t)∧R(t)=⎡⎣⎢0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10⎤⎦⎥R(t)$$R\dot{}(t)=\varphi (t{)}^{\wedge }R(t)=\left[\begin{array}{ccc}0& \varphi 3& -\varphi 2\\ -\varphi 3& 0& \varphi 1\\ \varphi 2& -\varphi 1& 0\end{array}\right]R(t)$$

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

R(t)=exp(ϕ(t)∧)R(t0)" role="presentation">R(t)=exp(ϕ(t)∧)R(t0)$$R(t)=exp(\varphi (t{)}^{\wedge })R(t0)$$

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

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

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

特殊欧式群

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

SE(3)={T=[RTt0T1]∈R4×4|R∈SO(3),t∈R3}" role="presentation">SE(3)={T=[RT0Tt1]∈R4×4|R∈SO(3),t∈R3}$$SE(3)=\{T=\left[\begin{array}{cc}{R}^T& t\\ 0^T& 1\end{array}\right]\in R_{4\times 4}|R\in SO(3),t\in R^3\}$$

李代数

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

定义

李代数由一个集合V，一个数域F和一个二元运算[](其中二元运算被称为李括号)组成。如果它们满足以下几条性质，称(V,F,[]) 为一个李代数，记作g" role="presentation" style="position: relative;">g$g$

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

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

SO(3)" role="presentation" style="position: relative;">SO(3)$SO(3)$ 对应的李代数是定义在R3" role="presentation" style="position: relative;">R3$R^3$上的向量，我们记作ϕ" role="presentation" style="position: relative;">ϕ$\varphi$(注意这是个向量)。根据前面的推导，每个ϕ" role="presentation">ϕ$\varphi$都可以生成一个反对称矩阵：

Φ=ϕ∧=[0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10]∈R3×3" role="presentation">Φ=ϕ∧=⎡⎣⎢0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10⎤⎦⎥∈R3×3$$\Phi ={\varphi }^{\wedge }=\left[\begin{array}{ccc}0& \varphi 3& -\varphi 2\\ -\varphi 3& 0& \varphi 1\\ \varphi 2& -\varphi 1& 0\end{array}\right]\in R^{3\times 3}$$

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

[ϕ1,ϕ2]=Φ1Φ2−Φ2Φ1" role="presentation">[ϕ1,ϕ2]=Φ1Φ2−Φ2Φ1$$[\varphi 1,\varphi 2]=\Phi 1\Phi 2-\Phi 2\Phi 1$$

所以三维旋转群与对应的李代数so(3)" role="presentation">so(3)$\mathfrak{s}\mathfrak{o}\mathfrak{(}\mathfrak{3}\mathfrak{)}$：

so(3)={Φ=ϕ∧∈R3×3|ϕ∈R3}" role="presentation">so(3)={Φ=ϕ∧∈R3×3|ϕ∈R3}$$\mathfrak{s}\mathfrak{o}\mathfrak{(}\mathfrak{3}\mathfrak{)}=\{\Phi ={\varphi }^{\wedge }\in {\mathrm{R}}^{3\times 3}|\varphi \in {\mathrm{R}}^3\}$$

至此，我们已清楚so(3)" role="presentation" style="position: relative;">so(3)$\mathfrak{s}\mathfrak{o}\mathfrak{(}\mathfrak{3}\mathfrak{)}$的结构。它们是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。

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

SE(3)={T=[Rt0T1]∈R4×4|R∈SO(3),t∈R3}" role="presentation">SE(3)={T=[R0Tt1]∈R4×4|R∈SO(3),t∈R3}$$SE(3)=\left\{\mathbf{T}=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in R^{4\times 4}|R\in SO(3),\mathbf{t}\in {\mathbb{R}}^3\right\}$$

每个变换矩阵有六个自由度，故对应的李代数位于R6" role="presentation" style="position: relative;">R6${\mathbb{R}}^6$,但是∧不再对应到一个反对称关系，而是：

ξ∧=[ρϕ]∧=[ϕ∧ρ0T0]=Ξ" role="presentation">ξ∧=[ρϕ]∧=[ϕ∧0Tρ0]=Ξ$${\xi }^{\wedge }={\left[\begin{array}{l}\rho \\ \varphi \end{array}\right]}^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ {\mathbf{0}}^T& 0\end{array}\right]=\mathbf{\Xi }$$

ξ" role="presentation" style="position: relative;">ξ$\xi$的前三维为旋转向量，后三维为平移向量，其定义也十分的直观。该李代数对应于微分方程：

T˙(t)=ξ∧(t)T(t)" role="presentation">T˙(t)=ξ∧(t)T(t)$$\dot{\mathbf{T}}(t)={\xi }^{\wedge }(t)\mathbf{T}(t)$$

因此，

T(t)=exp⁡(ξ(t)∧)T(t)" role="presentation">T(t)=exp(ξ(t)∧)T(t)$$\mathbf{T}(t)=\exp (\xi (t{)}^{\wedge })\mathbf{T}(t)$$

指数映射和对数映射

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

exp(A)=∑n=0∞1n!An" role="presentation">exp(A)=∑n=0∞1n!An$$exp(A)=\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}{A}^n$$

• so(3)" role="presentation" style="position: relative;">so(3)$\mathfrak{s}\mathfrak{o}\mathfrak{(}\mathfrak{3}\mathfrak{)}$中的指数映射
  因此，对so(3)" role="presentation" style="position: relative;">so(3)$\mathfrak{s}\mathfrak{o}\mathfrak{(}\mathfrak{3}\mathfrak{)}$中任意一元素ϕ" role="presentation" style="position: relative;">ϕ$\varphi$，我们亦可按此方式定义它的指数映射：

exp(ϕ∧)=∑n=0∞1n!(ϕ∧)n" role="presentation">exp(ϕ∧)=∑n=0∞1n!(ϕ∧)n$$exp({\varphi }^{\wedge })=\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}({\varphi }^{\wedge }{)}^n$$

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

exp(θa)=cosθI+(1−cosθ)aaT+sinθa∧" role="presentation">exp(θa)=cosθI+(1−cosθ)aaT+sinθa∧$$exp(\theta a)=cos\theta I+(1-cos\theta )a{a}^T+sin\theta a^{\wedge }$$

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

ϕ=ln(R)∨=(∑n=0∞(−1)nn+1(R−I)n+1)∨" role="presentation">ϕ=ln(R)∨=(∑n=0∞(−1)nn+1(R−I)n+1)∨$$\varphi =ln(R{)}^{\vee }=(\sum _{n=0}^{\mathrm{\infty }}\frac{(-1{)}^n}{n+1}(R-I{)}^{n+1}{)}^{\vee }$$

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

• se(3)" role="presentation" style="position: relative;">se(3)$\mathfrak{s}\mathfrak{e}\mathfrak{(}\mathfrak{3}\mathfrak{)}$中的指数映射

exp⁡(ξ∧)=[∑n=0∞1n!(ϕ∧)n∑n=0∞1(n+1)!(ϕ∧)nρ0T1]=[ΦJρ0T1]" role="presentation">exp(ξ∧)=⎡⎣⎢∑n=0∞1n!(ϕ∧)n0T∑n=0∞1(n+1)!(ϕ∧)nρ1⎤⎦⎥=[Φ0TJρ1]$$\begin{array}{rl}\exp \left({\xi }^{\wedge }\right)& =\left[\begin{array}{cc}\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}{\left({\varphi }^{\wedge }\right)}^n& \sum _{n=0}^{\mathrm{\infty }}\frac{1}{\left(n+1\right)!}{\left({\varphi }^{\wedge }\right)}^n\rho \\ {\mathbf{0}}^T& 1\end{array}\right]\\ & =\left[\begin{array}{cc}\mathbf{\Phi }& \mathbf{J}\rho \\ {\mathbf{0}}^T& 1\end{array}\right]\end{array}$$

左上角的Φ" role="presentation" style="position: relative;">Φ$\Phi$是我们熟知的so(3)" role="presentation" style="position: relative;">so(3)$so(3)$中的元素，前文已经介绍过了。而右上角的J" role="presentation" style="position: relative;">J$J$则可整理为(设ϕ=θa" role="presentation" style="position: relative;">ϕ=θa$\varphi =\theta a$)：

J=sin⁡θθI+(1−sin⁡θθ)aaT+1−cos⁡θθa∧" role="presentation">J=sinθθI+(1−sinθθ)aaT+1−cosθθ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⁡(ϕ1∧)exp⁡(ϕ2∧)=exp⁡((ϕ1+ϕ2)∧)" role="presentation">exp(ϕ∧1)exp(ϕ∧2)=exp((ϕ1+ϕ2)∧)$$\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)=\exp \left({\left({\varphi }_1+{\varphi }_2\right)}^{\wedge }\right)$$

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

ln⁡(exp⁡(A)exp⁡(B))=∑n=1∞(−1)n−1n∑ri+si>0,i∈[1,n](∑i=1n(ri+si))−1∏i=1nri!si![Ar1Bs1Ar2Bs2⋯ArnBsn]" role="presentation">ln(exp(A)exp(B))=∑n=1∞(−1)n−1n∑ri+si>0,i∈[1,n](∑i=1n(ri+si))−1∏i=1nri!si![Ar1Bs1Ar2Bs2⋯ArnBsn]$$\ln \left(\exp \left(A\right)\exp \left(B\right)\right)=\sum _{n=1}^{\mathrm{\infty }}\frac{{\left(-1\right)}^{n-1}}{n}\sum _{r_i+s_i>0,i\in \left[1,n\right]}^{}\frac{{\left(\sum _{i=1}^n\left(r_i+s_i\right)\right)}^{-1}}{\prod _{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⁡(exp⁡(A)exp⁡(B))=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]+⋯" role="presentation">ln(exp(A)exp(B))=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]+⋯$$\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$$

其中，[A,B]=AB−BA" role="presentation" style="position: relative;">[A,B]=AB−BA$\left[A,B\right]=AB-BA$
特别地，在SO(3)上，我们可以求它关于某一个变量的一阶近似：

ln⁡(R1R2)∨≈{Jl(ϕ2)−1ϕ1+ϕ2Jr(ϕ1)−1ϕ2+ϕ1" role="presentation">ln(R1R2)∨≈{Jl(ϕ2)−1ϕ1+ϕ2Jr(ϕ1)−1ϕ2+ϕ1$$\ln {\left(R_1{R}_2\right)}^{\vee }\approx \{\begin{array}{l}{J}_l{\left({\varphi }_2\right)}^{-1}{\varphi }_1+{\varphi }_2\\ J_r{\left({\varphi }_1\right)}^{-1}{\varphi }_2+{\varphi }_1\end{array}$$

在 ϕ1" role="presentation" style="position: relative;">ϕ1$\varphi 1$ 较小时，使用第一个式；在在$ ϕ2 $较小时，使用第二个式。这里的 Jl" role="presentation" style="position: relative;">Jl$J_l$ 和 $J_r $也称为左/右雅可比——从而李代数就分成了左右两种模型:

Jl=J=∑n=0∞1(n+1)!(ϕ∧)n,Jr=J(−ϕ)" role="presentation">Jl=J=∑n=0∞1(n+1)!(ϕ∧)n,Jr=J(−ϕ)$$J_l=J=\sum _{n=0}^{\mathrm{\infty }}\frac{1}{\left(n+1\right)!}{\left({\varphi }^{\wedge }\right)}^n,J_r=J\left(-\varphi \right)$$

ln⁡(ΔR⋅R)∨=ϕ+Jl−1(ϕ)Δϕ" role="presentation">ln(ΔR⋅R)∨=ϕ+J−1l(ϕ)Δϕ$$\ln {\left(\mathrm{\Delta }R\cdot R\right)}^{\vee }=\varphi +J_l^{-1}\left(\varphi \right)\mathrm{\Delta }\varphi$$

因此，

exp(Δϕ∧)exp(ϕ∧)=ΔR⋅R=exp(ϕ+Jl−1(ϕ)Δϕ)∧" role="presentation">exp(Δϕ∧)exp(ϕ∧)=ΔR⋅R=exp(ϕ+J−1l(ϕ)Δϕ)∧$$exp(\mathrm{\Delta }{\varphi }^{\wedge })exp({\varphi }^{\wedge })=\mathrm{\Delta }R\cdot R=exp(\varphi +J_l^{-1}\left(\varphi \right)\mathrm{\Delta }\varphi {)}^{\wedge }$$

我们将Jl−1(ϕ)Δϕ" role="presentation" style="position: relative;">J−1l(ϕ)Δϕ$J_l^{-1}\left(\varphi \right)\mathrm{\Delta }\varphi$看作Δϕ" role="presentation" style="position: relative;">Δϕ$\mathrm{\Delta }\varphi$，则有

exp⁡(ϕ+Δϕ)∧=exp⁡(JlΔϕ)∧⋅exp⁡(ϕ)∧" role="presentation">exp(ϕ+Δϕ)∧=exp(JlΔϕ)∧⋅exp(ϕ)∧$$\exp {\left(\varphi +\mathrm{\Delta }\varphi \right)}^{\wedge }=\exp {\left(J_l\mathrm{\Delta }\varphi \right)}^{\wedge }\cdot \exp {\left(\varphi \right)}^{\wedge }$$

因此，当我们在李代数上进行加法，则可以近似为李群上带左右雅可比的乘法。
相当于左乘一个 Jlϕ" role="presentation" style="position: relative;">Jlϕ$J_l\varphi$ 对应的旋转阵。类似地，由于左右对称，有：

exp⁡(ϕ+δϕ)∧=exp⁡(Jlδϕ)∧⋅exp⁡(ϕ)∧=exp⁡(ϕ)∧exp⁡(Jrδϕ)∧" role="presentation">exp(ϕ+δϕ)∧=exp(Jlδϕ)∧⋅exp(ϕ)∧=exp(ϕ)∧exp(Jrδϕ)∧$$\exp {\left(\varphi +\delta \varphi \right)}^{\wedge }=\exp {\left(J_l\delta \varphi \right)}^{\wedge }\cdot \exp {\left(\varphi \right)}^{\wedge }=\exp {\left(\varphi \right)}^{\wedge }\exp {\left(J_r\delta \varphi \right)}^{\wedge }$$

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

微分模型和扰动模型

d(RP)dR=limΔR→0⁡(R+ΔR)P−RPΔR" role="presentation">d(RP)dR=limΔR→0(R+ΔR)P−RPΔR$$\frac{d\left(RP\right)}{dR}=\underset{\mathrm{\Delta }R\to 0}{lim}\frac{\left(R+\mathrm{\Delta }R\right)P-RP}{\mathrm{\Delta }R}$$

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

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

定义在李代数上的增量

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

d(RP)dϕ=limδϕ→0⁡exp⁡(ϕ+δϕ)∧P−exp⁡(ϕ)∧Pδϕ=limΔϕ→0⁡(exp⁡(Jlδϕ)∧−1)exp(ϕ)∧Pδϕ=−(RP)∧Jl" role="presentation">d(RP)dϕ===limδϕ→0exp(ϕ+δϕ)∧P−exp(ϕ)∧PδϕlimΔϕ→0(exp(Jlδϕ)∧−1)exp(ϕ)∧Pδϕ−(RP)∧Jl$$\begin{array}{lll}\frac{d\left(RP\right)}{d\varphi }& =& \underset{\delta \varphi \to 0}{lim}\frac{\exp {\left(\varphi +\delta \varphi \right)}^{\wedge }P-\exp {\left(\varphi \right)}^{\wedge }P}{\delta \varphi }\\ & =& \underset{\mathrm{\Delta }\varphi \to 0}{lim}\frac{(\exp {\left(J_l\delta \varphi \right)}^{\wedge }-1)exp{\left(\varphi \right)}^{\wedge }P}{\delta \varphi }\\ & =& -(RP{)}^{\wedge }{J}_l\end{array}$$

定义在李群上的乘增量

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

d(RP)dR=limδϕ→0⁡exp⁡(δϕ)∧⋅RP−RPδϕ=limΔϕ→0⁡(exp⁡(δϕ)∧−1)RPδϕ=−(RP)∧" role="presentation">d(RP)dR=limδϕ→0exp(δϕ)∧⋅RP−RPδϕ=limΔϕ→0(exp(δϕ)∧−1)RPδϕ=−(RP)∧$$\begin{array}{l}\frac{d\left(RP\right)}{dR}=\underset{\delta \varphi \to 0}{lim}\frac{\exp {\left(\delta \varphi \right)}^{\wedge }\cdot RP-RP}{\delta \varphi }\\ =\underset{\mathrm{\Delta }\varphi \to 0}{lim}\frac{(\exp {\left(\delta \varphi \right)}^{\wedge }-1)RP}{\delta \varphi }\\ =-{\left(RP\right)}^{\wedge }\end{array}$$

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