李群和李代数

引言

旋转矩阵内的各个元素取值收到内在约束，即旋转矩阵为正交矩阵，且行列式为1。在采用优化方法进行SLAM位姿估计时，这种内在约束让优化求解变的非常困难。因此这里需要引入李群、李代数的概念，通过李群-李代数的转换可以去除这种内在约束，简化优化求解过程。

群

群（group）是一种集合加上一种运算的代数结构。我们把集合记作A，运算记作 $\cdot$ ，那么群可以记作 $G=\left(A,\cdot \right )$ 。群要求这个运算满足以下几个条件：

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

李群是指具有连续（光滑）性质的群，也就是说李群是群，并且该群是连续光滑的。旋转矩阵R和转移矩阵T满足群的定义，所以旋转矩阵R所构成的群称为特殊正交群 $SO(3)$ ，转移矩阵T所构成的群称为特殊欧式群 $SE(3)$ 。值得注意的是， $SO(3)$ 和 $SE(3)$ 在加法运算上不封闭，即 $R_1 + R_2 \notin SO(3)$ , $T_1 + T_2 \notin SE(3)$ ，而在乘法运算上封闭，即 $R_1 R_2 \in SO(3)$ , $T_1 T_2 \in SE(3)$ 。

由于刚体能在空间中连续运动，也就是说描述其运动的旋转矩阵R和转移矩阵T也具有连续（光滑）的性质，因此 $SO(3)$ 和 $SE(3)$ 也是李群。那么李群 $SO(3)$ 和 $SE(3)$ 的数学描述如式（1）和式（2）所示。

$SO(3) = \left \{ R \in {\mathbb{R}}^{3 \times 3} | RR^T = I, det(R) = 1 \right \}$ (1)

$SE(3)=\left \{ T=\begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \in {\mathbb{R}}^{4 \times 4} | R \in SO(3), t \in {\mathbb{R}}^3 \right \}$ (2)

李代数

李代数与李群相对应，李代数由一个集合 $\mathbb{V}$ 、一个数域 $\mathbb{F}$ 和一个二元运算 $[,]$ 组成，这里的二元运算[,]也成为李括号。如果它们满足以下几条性质，则称 $(\mathbb{V}, \mathbb{F}, [,])$ 为一个李代数，记作 $g$ 。

封闭性 $\forall X, Y \in \mathbb{V}, [X,Y] \in \mathbb{V}.$
双线性 $\forall X,Y,X \in \mathbb{V}, a,b \in \mathbb{F}$ ，有 $[aX + bY, Z] = a[X, Z] + b[Y,Z], \quad [Z, aX + bY] = a[Z,X] + b[Z,Y].$
自反性 $\forall X \in \mathbb{V}, [X,X]=0$ .
雅可比等价 $\forall X, Y, Z \in \mathbb{V}, [X, [Y, Z]] + [Z, [X, Y]] + [Y, [Z, X]] = 0.$

机器人中最常用的李群是 $SO(3)$ 和 $SE(3)$ ，其对应的李代数分别为 $so(3)$ 和 $se(3)$ ，这里的记法是李群用大学字母，对应的李代数用相应的小写字母。这里直接给出李代数 $so(3)$ 和 $se(3)$ 的数学描述，如式（3）和式（4）所示。

$so(3) = \left \{ \phi \in {\mathbb{R}}^3, \Phi = {\phi}^{\hat{}} \in {\mathbb{R}}^{3 \times 3} \right \}$ (3)

其中 ${\phi}^{\hat{}} = \begin{bmatrix} 0 & -\phi_3 & \phi_2\\ \phi_3 & 0 & -\phi_1\\ -\phi_2 & \phi_1 & 0 \end{bmatrix}$ ，李括号形式为： $\forall {\phi}_1^{\hat{}}, {\phi}_2^{\hat{}} \in so(3), [{\phi}_1^{\hat{}}, {\phi}_2^{\hat{}}] = {\phi}_1^{\hat{}}{\phi}_2^{\hat{}}-{\phi}_2^{\hat{}}{\phi}_1^{\hat{}} = ({\phi}_1^{\hat{}} {\phi}_2)^{\hat{}} \in so(3)$

$se(3) = \left \{ \xi = \begin{bmatrix} \rho\\ \phi \end{bmatrix} \in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3, \phi \in so(3), \xi^{\hat{}} = \begin{bmatrix} \phi^{\hat{}} & \rho\\ 0^T & 0 \end{bmatrix} \in {\mathbb{R}}^{4 \times 4} \right \}$ (4)

其中李括号形式为： $\forall \xi_1^{\hat{}}, \xi_2^{\hat{}} \in se(3), [\xi_1^{\hat{}}, \xi_2^{\hat{}}] = \xi_1^{\hat{}} \xi_2^{\hat{}} - \xi_2^{\hat{}} \xi_1^{\hat{}} = (\xi_1^{\hat{}} \xi_2)^{\hat{}} \in se(3)$

李代数描述了李群的局部性质，每个李群都有相对应的李代数。李代数对应李群的切（tangent）空间，它描述了李群局部的导数。

指数与对数映射

李代数通过指数映射转换成李群，李群通过对数映射转换成李代数。下面直接给出李群 $SO(3)$ 和 $SE(3)$ 及其对应的李代数 $so(3)$ 和 $se(3)$ 的互相转换。 $SO(3)$ 和 $so(3)$

相互转换如式（5）和式（6）所示

$R = exp(\phi \hat{} \ ) = (cos\theta) I + (sin\theta)a \hat{} + (1-cos\theta) a a^T$ (5)

其中： $\theta = \left \| \phi \right \|, a = \frac {\phi}{\left \| \phi \right \|}, \phi = \theta a$

$\phi = (lnR) \ \check{} = \left \| \phi \right \| \frac {\phi}{\left \| \phi \right \|} = \theta a$ (6)

其中：

求a，解特征方程 $Ra = a$ , $a$ 为矩阵R在特征值 $\lambda$ 为1时的特征向量；
求 $\theta$ ， $\theta = arccos(\frac{tr(R)-1}{2})$

$SE(3)$ 和 $se(3)$ 相互转换如式（7）和式（8）所示

$T = exp(\xi \hat{} \ ) = exp \left ( \begin{bmatrix} \rho\\ \phi \end{bmatrix} ^ {\hat{}} \ \right ) = \begin{bmatrix} R & t\\ 0^T & 1 \end{bmatrix}$ (7)

其中：

$R = exp(\phi \hat{} \ );$
$t = J \rho$ ，左雅可比 $J = \frac {sin\theta}{\theta} I + \left ( 1 - \frac {sin\theta}{\theta} \right ) a a^T + \frac {1-cos\theta}{\theta} a \hat{}, \theta = \left \| \phi \right \|, a = \frac {\phi}{\left \| \phi \right \|}$ 。

$\xi = \left ( lnT \right ) \check{} = \begin{bmatrix} \rho \\ \phi \end{bmatrix}$ (8)

其中： $\phi = \left ( lnR \right ) \check{}, \quad \rho = J^{-1}t$

BCH公式

使用李代数的一大动机是进行优化，而在优化过程中导数是非常必要的信息，我们经常需要知道在李群中两个矩阵相乘，相应的在李代数上会发生什么改变。BCH（Baker-Campbell-Hausdorff）公式给出了答案。BCH公式如式（9)所示

$ln(exp(\phi_1^ {\hat{}}) exp(\phi_2^{\hat{}})) \check{} \approx \left\{\begin{matrix} J_l(\phi_2)^{-1} \phi_1 + \phi_2 \quad \phi_1 \ is \ small\\ J_r(\phi_1)^{-1} \phi_2 + \phi_1 \quad \phi_2 \ is \ small \end{matrix}\right.$ (9)

其中左雅可比矩阵为： $J_l = \frac{sin\theta}{\theta}I + \left ( 1 - \frac{sin\theta}{\theta} \right ) a a^T + \frac{1 - cos\theta}{\theta} a \hat{}$

它的逆为： $J_l^{-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 \hat{}$

右乘雅可比矩阵仅需要对自变量取负号即可：

$J_r(\phi) = J_l(-\phi)$ (10)

为了方便理解，我们重新描述BCH近似的意义。假定对某个旋转R，对应的李代数为 $\phi$ ，我们给它左乘一个微小旋转，记作 $\Delta R$ ，对应的李代数为 $\Delta \phi$ 。那么，在李群上，得到的结果就是 $\Delta R \cdot R$ ，而在李代数上，根据BCH近似，为 $J_l^{-1}(\phi) \Delta \phi + \phi$ 。合并起来，可以简单的写成：

$exp(\Delta \phi \hat{}) exp(\phi \hat{}) = exp \left ( (J_l^{-1}(\phi) \Delta \phi + \phi) \right )$ (11)

反之，如果我们在李代数上进行加法，让一个 $\phi$ 加上 $\Delta \phi$ ，那么可以近似为李群上带左右雅可比的乘法：

$exp\left ( (\phi + \Delta \phi) \hat{} \right ) = exp\left ( (J_l \Delta \phi) \hat{} \right ) exp(\phi \hat{}) = exp(\phi \hat{}) exp\left ( (J_r \Delta \phi) \hat{} \right )$ (12)

同样的，对于 $SE(3)$ ，也有类似的BCH近似：

$exp(\Delta \xi \hat{}) exp(\xi \hat{}) \approx exp\left ( (J_l^{-1} \Delta \xi + \xi) \hat{} \right )$ (13)

李代数求导

首先考虑 $SO(3)$ 上的求导，某个空间点p，经过旋转矩阵R（对应的李代数是 $\phi$ ）的变换后的位姿为 $Rp$ 。求目标位姿关于旋转矩阵R的导数。这里采用左扰动模型，通过给旋转矩阵R左乘一个微小扰动 $\Delta R$ ，对应的李代数是 $\varphi$ 。然后对 $\varphi$ 求导，即

$\frac{\partial (Rp)}{\partial \varphi} = \lim_{\varphi \to 0} \frac{exp(\varphi \hat{}) exp(\phi \hat{}) p - exp(\phi \hat{}) p}{\varphi}$

$\approx \lim_{\varphi \to 0} \frac{(I + \varphi \hat{}) exp(\phi \hat{}) p - exp(\phi \hat{}) p}{\varphi}$

$= \lim_{\varphi \to 0} \frac{\varphi \hat{} exp(\phi \hat{}) p}{\varphi}$

$=\lim_{\varphi \to 0} \frac{\varphi \hat{} R p}{\varphi}$

$=\lim_{\varphi \to 0} \frac{-(Rp) \hat{} \varphi}{\varphi}$

$=-(Rp) \hat{}$

考虑 $SE(3)$ 上的求导。假设某空间点p经过一次变换T（对应的李代数为 $\xi$ ），得到Tp，现在给T左乘一个微小扰动 $\Delta T = exp(\delta \xi \hat{})$ ，设扰动项的李代数为 $\delta \xi = \left [ \delta \rho, \delta \phi \right ] ^ T$ ，那么：

$\frac {\partial (Tp)}{\partial \delta \xi} = \lim_{\delta \xi \to 0} \frac{exp(\delta \xi \hat{}) exp(\xi \hat{}) p- exp(\xi \hat{}) p}{\delta \xi}$

$\approx \lim_{\delta \xi \to 0} \frac{(I + \delta \xi \hat{}) exp(\xi \hat{}) p - exp(\xi \hat{}) p}{\delta \xi}$

$= \lim_{\delta \xi \to 0} \frac {\delta \xi \hat{} exp(\xi \hat{}) p}{\delta \xi}$

$= \lim_{\delta \xi \to 0} \frac {\begin{bmatrix} \delta \phi \hat{} & \delta \rho\\ 0^T & 0 \end{bmatrix} \begin{bmatrix} Rp + t\\ 1 \end{bmatrix}}{\delta \xi}$

$=\lim_{\delta \xi \to 0} \frac{\begin{bmatrix} \delta \phi \hat{} (Rp + t) + \delta \rho\\ 0^T \end{bmatrix}}{\left [ \delta \rho, \delta \phi \right ]^T}$

$=\begin{bmatrix} I & -(Rp+t) \hat{}\\ 0^T & 0^T \end{bmatrix}$

引言

群

李代数

指数与对数映射

BCH公式

李代数求导

猜你喜欢