李群李代数知识整理

最新推荐文章于 2025-03-23 15:21:01 发布

rain_promise

最新推荐文章于 2025-03-23 15:21:01 发布

阅读量7.2k

点赞数 3

分类专栏：视觉slam 文章标签：李群李代数位姿变换扰动模型 SLAM

视觉slam 专栏收录该内容

2 篇文章

订阅专栏

反反复复看过很多次，总是有些地方不太明白，希望通过整理，再有一些新的认识。

参考：

李群李代数转换关系

视觉SLAM14讲第4讲 https://zhuanlan.zhihu.com/p/33156814

https://blog.csdn.net/heyijia0327/article/details/50446140

群

群（group）是一种集合加上一种运算的代数结构。把集合记为 $\mathbf{A}$ ，运算记为 $\cdot$ , 群就可以记为 $\mathbf{G}=(\mathbf{A},\ \cdot)$ 。而这个运算必须满足以下条件：

封闭性： $\forall a_1,a_2 \in \mathbf{A},\ a_1 \cdot a_2 \in \mathbf{A}$
结合律： $\forall a_1,a_2,a_3 \in \mathbf{A},\ (a_1 \cdot a_2) \cdot a_3 = a_1\cdot (a_2 \cdot a_3)$
幺元： $\exists a_0 \in \mathbf{A},\ s.t. \ \forall a \in \mathbf{A}, a_0 \cdot a = a \cdot a_0$
逆： $\forall a \in \mathbf{A},\exists a^{-1} \in A, \ s.t.\ a \cdot a^{-1}=a_0$

李群

李群就是具有连续（光滑）性质的群。

三维旋转矩阵构成了特殊正交群 SO(3)，变换矩阵构成了特殊欧式群 SE(3)。

李代数

李代数描述了李群的局部性质。

李代数的必要性：

考虑问题：假设某个时刻我们预测机器人的位姿为 $\mathbf{T}$ （待定值）, 观测到了一个惯性坐标系下的点 $\mathbf{p}$ 而产生了一个观测数据 $\mathbf{z}$ ，

机器人的位姿估计即为寻找一个最优的 $\mathbf{T}$ 使得整体的误差最小化：

$\mathop{\min}_{T} J(\mathbf{T})=\sum\limits^N_{i=1}||\mathbf{z}_i-\mathbf{T}\mathbf{p}_i||^2_2$

如上最小二乘问题，一般利用初始值+梯度进行自变量更新，计算导数时需要用到加法，但是李群对加法不具备封闭性，遂引入李代数。

李代数的引出

考虑任意的旋转矩阵 $\mathbf{R}$ ，有

$\mathbf{R}\mathbf{R}^T=\mathbf{I}$

因为机器人在不断运动，所以 $\mathbf{R}$ 也在随时间不断变换，引入时间t 就得到 $\mathbf{R}(t)$ ，上式也变成

$\mathbf{R}(t)\mathbf{R}(t)^T=\mathbf{I}$

等式两边一起对时间求导可得：

$\dot{\mathbf{R}}(t)\mathbf{R}(t)^T+\mathbf{R}(t)\dot{\mathbf{R}}(t)^T=0$

亦即

$\dot{\mathbf{R}}(t)\mathbf{R}(t)^T=-\mathbf{R}(t)\dot{\mathbf{R}}(t)^T=-(\dot{\mathbf{R}}(t)\mathbf{R}(t)^T)^T$

一个矩阵等于其自身转置后取负号，可知 $\dot{\mathbf{R}}(t)\mathbf{R}(t)^T$ 是一个反对称矩阵。我们之前就介绍过反对称矩阵和^ 符号，可知对应地，对于任意的反对称矩阵，可以找到一个与之对应的向量，这个关系可以用 $^\vee$ 来表示：

$\mathbf{a}^\wedge=\mathbf{A}= \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{bmatrix} , \ \ \mathbf{A}^\vee=\mathbf{a}$

设这个对应的向量为 $\phi(t)\in R^3$ ，就有

$\dot{\mathbf{R}}(t)\mathbf{R}(t)^T=\phi(t)^\wedge$

等式两边同时右乘 $\mathbf{R}(t)$ 则有

$\dot{\mathbf{R}}(t)=\phi(t)^\wedge\mathbf{R}(t)$

从式(11)（上式）可以看到，每对旋转矩阵求一次导数，只需要左乘一个 $\phi(t)^\wedge$ 就可以了。假设t0 = 0 并设此时 $\mathbf{R}(0)=\mathbf{I}$ ，对式(10) 进行一阶泰勒展开就有：

$\mathbf{R}(t) \approx \mathbf{R}(t_0)+\dot{\mathbf{R}}(t_0)(t-t_0)=\mathbf{I}+\phi(t_0)^\wedge \cdot (t)$

NOTE： $\phi$ 反应了 $\mathbf{R}$ 的导数的性质，故称它在SO(3) 原点附近的正切空间（Tangent Space）上。

同时，在t0 附近，设 $\phi$ 也是一个常数，即 $\phi(t_0)=\phi_0$ ，根据式(11) 就有

$\dot{\mathbf{R}}(t)=\phi(t_0)^\wedge\mathbf{R}(t)=\phi_0^\wedge\mathbf{R}(t)$

显然，上式是一个关于 $\mathbf{R}$ 的微分方程，而且知道初始值 $\mathbf{R}(0)=\mathbf{I}$ ，解为

$\mathbf{R}(t)=\exp (\phi_0^\wedge t)$

由于之前的假设，所以这个式子只在t 附近有效。

但可以确定，当某个时刻的 $\mathbf{R}$ 已知时，存在一个向量 $\phi$ ，二者满足这个矩阵指数关系 。而这个 $\phi$ ，就是对应到SO(3) 上的李代数so(3)。

李代数so(3)

李代数so(3) 对应的向量 $\phi$ ，是定义在R3 上的向量，每个向量对应到一个反对称矩阵；（李括号为三维向量的外积运算）；可以用来表达旋转矩阵的导数，和SO(3) 的关系由指数映射给定： $\mathbf{R}=\exp (\phi^\wedge)$ .

两个向量 $\phi_1, \phi_2$ 的李括号为：

$[\phi_1, \phi_2]=(\mathbf{\Phi_1}\mathbf{\Phi_2}-\mathbf{\Phi_2}\mathbf{\Phi_1})^\vee$

李代数se(3)

对于SE(3)，它也有对应的李代数se(3)。和so(3) 类似，se(3) 在R6 空间中：

$se(3)=\begin{Bmatrix} \mathbf{\xi}=\begin{bmatrix} \mathbf{\rho} \\ \mathbf{\phi} \end{bmatrix} \in R^6,\mathbf{\rho} \in R^3, \mathbf{\phi} \in so(3), \mathbf{\xi}^\wedge=\begin{bmatrix}\phi^\wedge & \mathbf{\rho} \\ \mathbf{0}^T & 0 \end{bmatrix} \in R^{4 \times 4} \end{Bmatrix}$

把每个se(3) 元素记做 $\xi$ ，它是一个六维向量：前三维为平移，记作 $\rho$ ；后三维是旋转，记作 $\phi$ ，实质上是so(3) 元素。

此外，在se(3) 中， $^\wedge$ 符号的含义被拓展了：这里它将一个六维向量转换为四维矩阵，但这里不再表示反对称矩阵。

李代数se(3) 的李括号：

$[\xi_1,\ \xi_2]=(\xi_1^\wedge \xi_2^\wedge - \xi_2^\wedge\xi_1^\wedge)^\vee$

指数和对数映射

从李代数到李群，通过指数映射；反之，从李群到李代数，则通过对数映射。

so(3) : $\varphi =a\Theta$

so(3) —— SO(3):

$\exp{\phi^\wedge}=\exp(\theta \mathbf{a})=\sum\limits_{n=0}^\infty \frac{1}{n!}(\theta \mathbf{a}^\wedge)^n \\ =\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{a}\mathbf{a}^T+\sin\theta\mathbf{a}^T$

SO(3) —— so(3):

$\Theta =\arccos \frac{tr(R)-1}{2}$

Ra=a

BCH近似

对于两次连续的旋转可以用矩阵乘法来描述，但用李代数如何表示呢？我们自然是希望李群中两个矩阵相乘对应李代数上对应元素的相加，这样我们才能够利用加法来计算导数和进行更新。但很遗憾，这是不成立的。

李代数指数映射的乘积，见BCH公式：

$\ln(\exp(\mathbf{A})\exp(\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{12}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\ldots$

BCH 的线性近似：

$\ln(\exp(\phi^\wedge_1)\exp(\phi^\wedge_2))^\vee \approx \begin{cases} \mathbf{J}_l(\phi_2)^{-1}\phi_1 + \phi2,\ 当\phi_1 为小量,\\ \mathbf{J}_r(\phi_1)^{-1}\phi_2+\phi_1,\ 当\phi_2 为小量,\\ \end{cases}$

根据BCH 近似：

$\exp(\Delta\phi^\wedge)\exp(\Delta\phi^\wedge)=\exp((\phi+\mathbf{J}_l(\phi)^{-1}\Delta\phi)^\wedge)$

$\exp((\phi+\Delta\phi)^\wedge)=\exp((\mathbf{J}_l\Delta\phi)^\wedge)\exp(\phi^\wedge)=\exp(\phi^\wedge)\exp((\mathbf{J}_r\Delta\phi)^\wedge)$

扰动模型

利用李代数求导，有两种思路：

用李代数来表示位姿，根据李代数加法来对李代数进行求导；
利用李群来左乘或者右乘微小扰动，在对这个扰动的李代数进行求导。

一般使用第二种，扰动模型：

先对旋转矩阵 $\mathbf{R}$ 进行一次扰动 $\Delta\mathbf{R}$ ，然后再求导。

这里以左乘扰动模型为例。设左扰动的李代数为 $\varphi$ ，直接对 $\varphi$ （而不是 $\mathbf{R}$ ）进行求导：

$\frac{\partial(\mathbf{R}\mathbf{p})}{\partial\varphi}=\lim_{\varphi\to 0}\frac{\exp(\varphi^\wedge)\exp(\phi^\wedge)\mathbf{p}-\exp(\phi^\wedge)\mathbf{p}}{\varphi}=-(\mathbf{R}\mathbf{p})^\wedge$

SE(3)上的扰动模型：

假设某空间点 $\mathbf{p}$ 经过了一次变换 $\mathbf{T}$ ，对应的李代数为 $\xi$ ，得到了 $\mathbf{T}\mathbf{p}$ 。给 $\mathbf{T}$ 左乘一个微小扰动 $\Delta\mathbf{T}$ ，扰动项的李代数为 $\delta\xi=[\delta\rho,\delta\phi]^T$ 。则有：

$\frac{\partial(\mathbf{T}\mathbf{p})}{\partial\delta\xi}=\lim_{\delta\xi\to0}\frac{\exp(\delta\xi^\wedge)\exp(\xi^\wedge)\mathbf{p}-\exp(\xi^\wedge)\mathbf{p}}{\delta\xi}= \begin{bmatrix} \mathbf{I}_{3\times3} & -(\mathbf{R}\mathbf{p}+\mathbf{t})^\wedge_{3\times3} \\ \mathbf{0}^T_{1\times3} & \mathbf{0}^T_{1\times3}\\ \end{bmatrix}=(\mathbf{T}\mathbf{p})^\bigodot$