0. 简介

这几个月，博主已经从SLAM算法的使用向着算法的数学推导进行了记录和分享，之前也分享了李群李代数关注核心一文，从现象中解释了李群和李代数表达的含义。但是这还不够，所以这次作者作为SLAM本质剖析的番外，来介绍李群李代数的微分和导数。

1. 旋转点求导

李群或者李代数上叠加微小量的情况呢？传统的求导过程中，我们常见的做法是对自变量添加一个微小值来进行：

$f'(x) = \lim_{\Delta x\rightarrow0}\frac{f(x+\Delta x)}{\Delta x}$

但是这种形式对于旋转矩阵 $SO(3)$ 我们不能这么做，因为李群对加法不封闭，因此两个旋转矩阵相加不一定是旋转矩阵，但是利用李代数，根据下面两个方向的 BCH 近似不难看出我们有两种思路进行求导，分别是：

• 用李代数（旋转向量）来表示姿态，然后利用李代数加法叠加微小量并对该微小量进行求导

  • 李代数求导：在李群对应的李代数的局部坐标上，即：（ $\boldsymbol{\phi}, \boldsymbol{\phi}^\wedge\in \mathfrak{so}(3)$ ） 上添加扰动，即： $\boldsymbol{\phi} \leftarrow \boldsymbol{\phi} + \delta\boldsymbol{\phi}$ ，由于李代数本身对应旋转向量，因此对旋转向量添加扰动相当于同时改变旋转轴和旋转角度。

• 用李群（旋转矩阵）表示姿态，然后左/右乘上一个扰动，然后对该扰动求导，即左扰动模型和右扰动模型

  • 旋转矩阵右扰动求导：由于旋转矩阵没有加法，因此要对旋转矩阵本身添加扰动，需要先通过指数映射将李代数转化为李群，然后根据李群的运算来添加扰动，即： $\boldsymbol{R} \leftarrow \boldsymbol{R}\exp{(\boldsymbol{\phi}^\wedge)}$ ，由于是旋转矩阵右乘扰动，因此相当于是在局部坐标系下对旋转矩阵进行更新
  • 旋转矩阵左扰动求导：和右扰动同理，我们也可以将扰动添加在旋转矩阵左侧，即： $\boldsymbol{R} \leftarrow \exp{(\boldsymbol{\phi}^\wedge)}\boldsymbol{R}$ ，由于是旋转矩阵左乘扰动，因此相当于在全局坐标系下对旋转矩阵进行更新

李代数这样的形式我们可以理解，Ceres也是通过这样的形式进行来实现李代数的累加。但是李群就需要根据BCH来进行计算了。

2 $\boldsymbol{J}_{\boldsymbol{R}_0}$ 是什么

为了明白在李群李代数公式中各个成员的含义，我们给出例子来解释不同的变量。假设我们对空间一个点 $p$ 使用旋转矩阵 ${R_0}$ 进行旋转得到 ${R_0} p$ ：

该式子在实际计算时可以施加微小扰动 ${\phi}$ ，通过最小化扰动来对误差进行线性化，并近似转换为，从而求出在 $R_0$ 到 $R$ 的情况下 $p$ 的变化。此刻默认 $\boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p})$ 是已知的。

$\boldsymbol{e}(\boldsymbol{R}, \boldsymbol{p}) = \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{J}_{\boldsymbol{R}_0}\delta\boldsymbol{\phi}$

得到基于 ${R_0}$ 的偏导 --------也就是 $\boldsymbol{J}_{\boldsymbol{R}_0}$ 是函数在 $\boldsymbol{R}_0$ 的雅可比矩阵

3 $\oplus$ 、 $\wedge$ 和 $_{\times}$ 的含义

对于李群而言，由于其没有向量空间上的加法操作，因此为了引入导数的概念，这里用一个映射将局部坐标 $\xi$ 映射到李群元素 $a$ 在李群空间附近的邻域上，用来作为李群上的 “加法” 操作，所以常常会使用 $\oplus$ 来表示李群的加法，如下所示：

式中， $\xi\in\mathbb{R}^n$ 是 $a$ 系下的局部坐标，这个也是我们所说的极小值右乘的做法，以李群 SO(3) 为例，局部坐标可以表示为 $\xi = \omega t$ ，其几何意义为以 $a$ 作为参考系下的一个角度扰动；

$\hat{\xi}$ 为 $\xi$ 的对应李代数，当然也可以像上面表示为 $\xi^{\wedge}$ ；

$\exp{\xi}$ 为李代数到李群的指数映射；

$\hat{\xi} = [\omega t]_{\times}$ 为李代数，李代数可以转化为反对称矩阵。通过 $_{\times}$ 来表示角度扰动（旋转轴+旋转角度）的旋转向量。

4 由BCH得到的左扰动和右扰动基础公式

首先将旋转矩阵（李群 $SO(3)$ ）转换为旋转向量（李代数 $\boldsymbol{\mathfrak{so}}(3)$ ），并对旋转向量求导：

最后我们得到以下结果：

因为 $\boldsymbol{J}_l$ 一般乘上的是极小值，所以在一般情况是可以省略的。这就可以根据类似的推导得到左扰动公式：

不难看出来，利用左扰动模型计算的导数比使用李代数直接求导省去了一个 $\boldsymbol{J}_l(\boldsymbol{\phi}_0)$ 的计算，因此更为实用，同时理论上精度也会更高（因为在计算该矩阵时需要近似）。

下面是右扰动公式：

相比于左扰动的模型中计算 $\boldsymbol{Rp}$ 的反对称矩阵，右扰动模型计算的是 $\boldsymbol{p}$ 的反对称矩阵，因此有细微的区别，使用时注意区分。

5 连乘李群的求导

除了旋转点以外，我们还经常需要对两个旋转叠加的结果 $\boldsymbol{R}_1\boldsymbol{R}_2$ 对其中一个旋转进行求导，这也是我们在SLAM中最常用的公式，经常是一个李群乘以一个极小的李群来作为状态量的的累加，从而推算出当前机器人的位姿。

求导形式取决于我们对函数定义方式，通常来说函数需要将变量映射到向量空间，因此这里我们将旋转通过对数映射转化为对应李代数的坐标（3维向量），并对该函数进行线性化从而求解雅可比矩阵，即：

首先，对 $\boldsymbol{R}_2$ 求导，此时固定 $\boldsymbol{R}_1$ ：

• 右扰动模型

如下所示：

• 左扰动模型

过程大同小异

进一步再对 $\boldsymbol{R}_1$ 求导，此时固定 $\boldsymbol{R}_2$ ， $\boldsymbol{R}_1$ 为极小值。

• 右扰动模型

过程如下：

• 左扰动模型

过程如下：

可以发现，无论是对 $\boldsymbol{R}_1$ 还是 $\boldsymbol{R}_2$ 使用左扰动或者右扰动求导，我们总是想办法使用 BCH 或者 伴随性质将 $\boldsymbol{R}_1\boldsymbol{R}_2$ 凑到一起与第二项相减，将扰动单独分离出来获得结果，过程大同小异。

根据极小值可以推导得到下面的简式：

6. 参考链接

https://hermit.blog.csdn.net/article/details/121034589

https://zhuanlan.zhihu.com/p/356409543

https://xiaotaoguo.com/p/gtsam-math-derivatives/

https://www.bilibili.com/read/cv13649489

https://xiaotaoguo.com/p/lee-algebra-derivative-examples/

https://medium.com/@ckwang19/slam%E5%AD%B8%E7%BF%92%E4%B9%8B%E8%B7%AF-%E4%BA%94-%E6%9D%8E%E7%BE%A4%E6%9D%8E%E4%BB%A3%E6%95%B8-d8706c78fbd9