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来进行计算了。

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

$p$ ${R_0}$ ${R_0} p$ ：

\boldsymbol{J}_{R} = \frac{\partial \boldsymbol{e}(\boldsymbol{{R}}, \boldsymbol{p})}{\partial \boldsymbol{{R}}} = \frac{\partial \boldsymbol{{R}p}}{\partial \boldsymbol{R}}

${\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$ 的雅可比矩阵

$\oplus$ $\wedge$ $_{\times}$ 的含义

$\xi$ $a$ 在李群空间附近的邻域上 $\oplus$ 来表示李群的加法，如下所示：

a\oplus \xi \triangleq a \exp{(\hat{\xi})}

$\xi\in\mathbb{R}^n$ $a$ 极小值右乘的做法 $\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)$ ），并对旋转向量求导：

\begin{aligned} \boldsymbol{e}(\boldsymbol{R}, \boldsymbol{p}) &= \boldsymbol{Rp} = \exp{(\boldsymbol{\phi}^\wedge})\boldsymbol{p}\\ &= \exp{((\boldsymbol{\phi}_0 + \delta\boldsymbol{\phi})^\wedge})\boldsymbol{p}\\ &= \exp{(\boldsymbol{\phi}_0^\wedge + \delta\boldsymbol{\phi}^\wedge})\boldsymbol{p}\\ 将{J_l({\phi}_0)}简化为{J_l({\phi}_0)} &= (R_0R_{\delta})\boldsymbol{p}\\ \mathrm{BCH 近似}&\approx \exp{((\boldsymbol{J}_l\delta\boldsymbol{\phi})^\wedge})\exp{(\boldsymbol{\phi}_0^\wedge)}\boldsymbol{p} \\ \mathrm{泰勒展开并去除高阶项}&\approx (\boldsymbol{I} + (\boldsymbol{J}_l\delta\boldsymbol{\phi})^\wedge)\exp{(\boldsymbol{\phi}_0^\wedge)}\boldsymbol{p} \\ &= \exp{(\boldsymbol{\phi}_0^\wedge})\boldsymbol{p} + (\boldsymbol{J}_l\delta\boldsymbol{\phi})^\wedge\exp{(\boldsymbol{\phi}_0^\wedge)}\boldsymbol{p} \\ &= \boldsymbol{R}_0\boldsymbol{p} + (\boldsymbol{J}_l\delta\boldsymbol{\phi})^\wedge\boldsymbol{R}_0\boldsymbol{p} \\ &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + (\boldsymbol{J}_l\delta\boldsymbol{\phi})^\wedge\boldsymbol{R}_0\boldsymbol{p} \\ \mathrm{利用a^\wedge b = -b^\wedge a 性质}&= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) - (\boldsymbol{R}_0\boldsymbol{p})^\wedge\boldsymbol{J}_l\delta\boldsymbol{\phi} \\ 根据之前的的公式默认为 &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{J}_{\boldsymbol{R}_0}\delta\boldsymbol{\phi} \\ \Rightarrow \boldsymbol{J}_{\boldsymbol{R}_0} &= - (\boldsymbol{R}_0\boldsymbol{p})^\wedge\boldsymbol{J}_l \end{aligned}

最后我们得到以下结果：

\boldsymbol{J}_{\boldsymbol{R}_0} = - (\boldsymbol{R}_0\boldsymbol{p})^\wedge\boldsymbol{J}_l(\boldsymbol{\phi}_0)

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

\begin{aligned} \boldsymbol{e}(\boldsymbol{R}, \boldsymbol{p}) &= \boldsymbol{Rp}\\ &= \exp{(\boldsymbol{\psi})^\wedge}\boldsymbol{R}_0\boldsymbol{p}\\ \mathrm{泰勒展开并去除高阶项}&\approx (\boldsymbol{I} + \boldsymbol{\psi}^\wedge)\boldsymbol{R}_0\boldsymbol{p}\\ &= \boldsymbol{R}_0\boldsymbol{p} + \boldsymbol{\psi}^\wedge\boldsymbol{R}_0\boldsymbol{p}\\ &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{\psi}^\wedge\boldsymbol{R}_0\boldsymbol{p}\\ \mathrm{利用a^\wedge b = -b^\wedge a 性质}&= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) - (\boldsymbol{R}_0\boldsymbol{p})^\wedge\boldsymbol{\psi} \\ &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{J}_{\boldsymbol{R}_0}\boldsymbol{\psi} \\ \Rightarrow \boldsymbol{J}_{\boldsymbol{R}_0} &= - (\boldsymbol{R}_0\boldsymbol{p})^\wedge \end{aligned}

$\boldsymbol{J}_l(\boldsymbol{\phi}_0)$ 的计算，因此更为实用，同时理论上精度也会更高（因为在计算该矩阵时需要近似）。

下面是右扰动公式：

\begin{aligned} \boldsymbol{e}(\boldsymbol{R}, \boldsymbol{p}) &= \boldsymbol{Rp}\\ &= \boldsymbol{R}_0\exp{(\boldsymbol{\psi})^\wedge}\boldsymbol{p}\\ \mathrm{泰勒展开并去除高阶项}&\approx \boldsymbol{R}_0(\boldsymbol{I} + \boldsymbol{\psi}^\wedge)\boldsymbol{p}\\ &= \boldsymbol{R}_0\boldsymbol{p} + \boldsymbol{R}_0\boldsymbol{\psi}^\wedge\boldsymbol{p}\\ &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{R}_0\boldsymbol{\psi}^\wedge\boldsymbol{p}\\ \mathrm{利用a^\wedge b = -b^\wedge a 性质}&= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) - \boldsymbol{R}_0\boldsymbol{p}^\wedge\boldsymbol{\psi} \\ &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{J}_{\boldsymbol{R}_0}\boldsymbol{\psi} \\ \Rightarrow \boldsymbol{J}_{\boldsymbol{R}_0} &= - \boldsymbol{R}_0\boldsymbol{p}^\wedge\\ \end{aligned}

$\boldsymbol{Rp}$ $\boldsymbol{p}$ 的反对称矩阵，因此有细微的区别，使用时注意区分。

5 连乘李群的求导

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

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

\begin{aligned} \boldsymbol{e}(\boldsymbol{R}_1, \boldsymbol{R}_2) &= (\ln{(\boldsymbol{R}_1\boldsymbol{R}_2)})^\vee\\ &=\boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}^0_2)}\boldsymbol{\phi}_1 + \boldsymbol{J}^2_{(\boldsymbol{R}^0_1, \boldsymbol{R}_2^0)}\boldsymbol{\phi}_2\\ \boldsymbol{J}_{(\boldsymbol{R}_1^0, \boldsymbol{R}^2_0)} &= \begin{bmatrix} \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}^0_2)} & \boldsymbol{J}^2_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)} \end{bmatrix} \in \mathbb{R}^{3\times 6} \end{aligned}

$\boldsymbol{R}_2$ $\boldsymbol{R}_1$ ：

右扰动模型

如下所示：

\begin{aligned} \boldsymbol{e}(\boldsymbol{R}_1, \boldsymbol{R}_2) &= (\ln{(\boldsymbol{R}_1\boldsymbol{R}_2)})^\vee\\ &= (\ln{(\boldsymbol{R}_1^0\boldsymbol{R}_2^0\exp{(\boldsymbol{\phi}_2^\wedge})})^\vee\\ \mathrm{BCH 近似}&\approx (\ln{(\boldsymbol{R}_1^0\boldsymbol{R}_2^0)})^\vee + \boldsymbol{J}_r^{-1}\boldsymbol{\phi}_2\\ &= \boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}^2_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)}\boldsymbol{\phi}_2\\ \Rightarrow \boldsymbol{J}^2_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)} &= \boldsymbol{J}_r^{-1}= \boldsymbol{J}_r^{-1}(\boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)) \end{aligned}

左扰动模型

过程大同小异

\begin{aligned} \boldsymbol{e}(\boldsymbol{R}_1, \boldsymbol{R}_2) &= (\ln{(\boldsymbol{R}_1\boldsymbol{R}_2)})^\vee\\ &= (\ln{(\boldsymbol{R}_1^0\exp{(\boldsymbol{\phi}_2^\wedge})}\boldsymbol{R}_2^0)^\vee\\ \mathrm{SO(3) 的伴随性质}&= (\ln{(\boldsymbol{R}^0_1\boldsymbol{R}^0_2\exp{(\boldsymbol{R}_2^{0T}\boldsymbol{\phi}_2)^\wedge}})^\vee\\ \mathrm{BCH 近似}&\approx (\ln{(\boldsymbol{R}^0_1\boldsymbol{R}^0_2)})^\vee+\boldsymbol{J}^{-1}_r\boldsymbol{R}_{2}^{0T}\boldsymbol{\phi}_2\\ &= \boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}^2_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)}\boldsymbol{\phi}_2\\ \Rightarrow \boldsymbol{J}^2_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)} &= \boldsymbol{J}_r^{-1}\boldsymbol{R}_2^T = \boldsymbol{J}_r^{-1}(\boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0))\boldsymbol{R}_2^T &\approx 1 \end{aligned}

$\boldsymbol{R}_1$ $\boldsymbol{R}_2$ $\boldsymbol{R}_1$ 为极小值。

右扰动模型

过程如下：

\begin{aligned} \boldsymbol{e}(\boldsymbol{R}_1, \boldsymbol{R}_2) &= (\ln{(\boldsymbol{R}_1\boldsymbol{R}_2)})^\vee\\ &= (\ln{(\boldsymbol{R}_1^0\exp{(\boldsymbol{\phi}_1^\wedge})}\boldsymbol{R}_2^0)^\vee\\ \mathrm{SO(3) 的伴随性质}&= (\ln{(\boldsymbol{R}^0_1\boldsymbol{R}^0_2\exp{(\boldsymbol{R}_2^{0T}\boldsymbol{\phi}_1)^\wedge}})^\vee\\ \mathrm{BCH 近似}&\approx (\ln{(\boldsymbol{R}^0_1\boldsymbol{R}^0_2)})^\vee+\boldsymbol{J}^{-1}_r\boldsymbol{R}_{2}^{0T}\boldsymbol{\phi}_1\\ &= \boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)}\boldsymbol{\phi}_1\\ \Rightarrow \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)} &= \boldsymbol{J}_r^{-1}\boldsymbol{R}_2^T= \boldsymbol{J}_r^{-1}(\boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0))\boldsymbol{R}_2^T &\approx 1 \end{aligned}

左扰动模型

过程如下：

\begin{aligned} \boldsymbol{e}(\boldsymbol{R}_1, \boldsymbol{R}_2) &= (\ln{(\boldsymbol{R}_1\boldsymbol{R}_2)})^\vee\\ &= (\ln{(\exp{(\boldsymbol{\phi}_1^\wedge})}\boldsymbol{R}_1^0\boldsymbol{R}_2^0)^\vee\\ \mathrm{BCH 近似}&\approx \ln{(\boldsymbol{R}_1^0\boldsymbol{R}_2^0)^\vee} + \boldsymbol{J}_l^{-1}\boldsymbol{\phi}_1\\ &= \boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}_l^{-1}\boldsymbol{\phi}_1 \\ &= \boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)}\boldsymbol{\phi}_1\\ \Rightarrow \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)} &= \boldsymbol{J}_l^{-1} = \boldsymbol{J}_l^{-1}(\boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)) \end{aligned}

$\boldsymbol{R}_1$ $\boldsymbol{R}_2$ $\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

SLAM本质剖析番外-李群李代数的微分和导数

敢敢のwings

0. 简介

1. 旋转点求导

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

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

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

5 连乘李群的求导

6. 参考链接

为你推荐

精选【自动驾驶】路径规划算法Dijkstra与A*

经典文献阅读之--NeRF-SLAM（单目稠密重建）

经典文献阅读之--用于自动驾驶的高清地图生成技术

CUDA入门和网络加速学习(三)

经典文献阅读之--Bidirectional Camera-LiDAR Fusion（Camera-LiDAR双向融合新范式）

史上最简SLAM零基础解读(3) - 白话来说SVD奇异值分解(2)→超定方程求解,最小奇异值特征为最优解

评论（0）

关于作者

敢敢のwings

290

2

271

17

精选经典文献阅读之--LBEVPlace(鸟瞰图的LiDAR的位置识别)

在ros环境下的RealsenceT265标定以及Vins mono运行

openpilot了解与分析

相关推荐

Apollo星火计划学习笔记——第八讲Apollo控制模块解析与实践2

精选Intel深度摄像头RealSense D435（实感双目摄像头）和目标检测结合使用

KITTI结果领先地位！Progressive LiDAR Adaptation for Road Detection——PLARD算法

给地面盖一层布！——CSF算法提取地面点

激光雷达点云操作的工程化分享（二）——PCA

关于机器人状态估计(12)-VIO/VSLAM的稀疏与稠密

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

轨迹规划

大模型

Matlab-ros

深度学习

CUDA

FAST-LIO系列

规范学习

机器人仿真

传感器感知

ROS基础以及进阶

无人驾驶

机器人基础知识

从零到一的SLAM

经典论文阅读

C++模式

专栏导航

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

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

$\oplus$ $\wedge$ $_{\times}$ 的含义

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板