李代数求导

李代数求导
李代数求导是SLAM的基础，由于SLAM待求解的状态量包含旋转，而旋转状态量为了避免歧义，往往采用更高维的四元素或者旋转矩阵表示。为了通过优化的方法更新旋转状态量，需要求出旋转后的点关于旋转状态量的雅可比。

1. 李群李代数部分整理

1.1. 群的概念

1.2. SO(3)推导

在 $\large t_0$ 附近假设 $\phi$ 不变，可解微分方程可以得到:

$\large \dot{\boldsymbol{R}}(t)=\boldsymbol{\phi}\left(t_{0}\right)^{\wedge} \boldsymbol{R}(t)=\boldsymbol{\phi}_{0}^{\wedge} \boldsymbol{R}(t)$

即

$\large \boldsymbol{R}(t)=\exp \left(\boldsymbol{\phi}_{0}^{\wedge} t\right)$

该式说明：
- 任意t都可以找到R与ϕ对应关系
- 该关系称为指数映射
- ϕ称为李代数
1.3. 李代数

$\large \mathfrak{s o}(3)= \left\{ \phi \in \mathbb{R}^{3}, \mathbf{\Phi}=\phi^{\wedge} \in \mathbb{R}^{3 \times 3} \right\}$

注: 不同书中对于se(3)中的顺序不一定都一样

1.4. 指数映射与对数映射

1.5. 伴随

SE(3)伴随

$\large T exp(\hat{\xi})T^{-1} = exp(\widehat{(Adj(T)\xi)}) Adj(T) = \begin{bmatrix}R & \hat{t}R \\0 &R\end{bmatrix}Texp(\hat{\xi})T^{-1} = exp(\widehat{(Adj(T)\xi)})Adj(T) = \begin{bmatrix}R & \hat{t}R \\0 &R\end{bmatrix}$

1.6. 求导与扰动

1.6.1. BCH公式与左右雅克比

李群对于加法不封闭，需要重新定义导数，解决方法两种:
- 利用李代数加法定义李群元素的导数
- 利用指数映射和对数映射完成变换关系
$\large \exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)=\exp \left(\left(\phi_{1}+\phi_{2}\right)^{\wedge}\right)$

对于李群不成立，于是利用BCH公式:

$\large \ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{ll}{J_{l}\left(\phi_{2}\right)^{-1} \phi_{1}+\phi_{2}} & {\text { if } \phi_{1} \text { is small }} \\ {J_{r}\left(\phi_{1}\right)^{-1} \phi_{2}+\phi_{1}} & {\text { if } \phi_{2} \text { is small. }}\end{array}\right.$

左雅克比

$\large \boldsymbol{J}_{l}=\boldsymbol{J}=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{T}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge}$

$\large 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^{\wedge}$

右雅克比

$\large \boldsymbol{J}{r}(\boldsymbol{\phi})=\boldsymbol{J}{l}(-\boldsymbol{\phi})$

$\large \exp \left(\Delta \phi^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\left(\phi+J_{l}^{-1}(\phi) \Delta \phi\right)^{\wedge}\right) \\$

$\large \exp \left((\phi+\Delta \phi)^{\wedge}\right)=\exp \left(\left(J_{l} \Delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\phi^{\wedge}\right) \exp \left(\left(J_{r} \Delta \phi\right)^{\wedge}\right)$

对于SE3形式更为复杂

$\large \begin{array}{l}{\exp \left(\Delta \xi^{\wedge}\right) \exp \left(\xi^{\wedge}\right) \approx \exp \left(\left(\mathcal{J}{l}^{-1} \Delta \xi+\xi\right)^{\wedge}\right)} \ {\exp \left(\xi^{\wedge}\right) \exp \left(\Delta \xi^{\wedge}\right) \approx \exp \left(\left(\mathcal{J}{r}^{-1} \Delta \xi+\xi\right)^{\wedge}\right)}\end{array}$ 对于旋转后的点坐标关于旋转的导数:

$\large \frac{\partial(\boldsymbol{R} p)}{\partial \boldsymbol{R}}$

由于R没有加法，导数无法定义，于是如下解决方法:
- 对R对应的李代数加上小量，求相对于小量的变化率(导数模型)
- 对R左乘或者右乘一个小量，求相对于小量的李代数的变化率(扰动模型)
导数模型:

1.6.2. 需要计算雅克比矩阵

扰动模型:

左扰动:

$\large \begin{aligned} \frac{\partial(R p)}{\partial \varphi} &=\lim _{\varphi \rightarrow 0} \frac{\exp \left(\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\varphi} \\ & \approx \lim _{\varphi \rightarrow 0} \frac{\left(1+\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\varphi} \\ &=\lim _{\varphi \rightarrow 0} \frac{\varphi^{\wedge} R p}{\varphi}=\lim _{\varphi \rightarrow 0} \frac{-(R p)^{\wedge} \varphi}{\varphi}=-(R p)^{\wedge} \end{aligned}$

右扰动的推导类似.

SE(3)扰动模型

$\frac{\partial T^{-1}P}{\partial \xi} \\ = \lim_{\delta \xi \to 0} \frac{ \left(Exp(\xi + \delta \xi)^{-1} - Exp(\xi)^{-1} \right)P}{\delta \xi} \\ = \lim_{\delta \xi \to 0} \frac{ \left((I - \delta \xi_{\times} ) Exp(\xi)^{-1} - Exp(\xi)^{-1} \right) P}{\delta \xi} \\ = \lim_{\delta \xi \to 0} \frac{ - \delta \xi_{\times} T^{-1} P}{\delta \xi} \\ = \lim_{\delta \xi \to 0} \frac{ (T^{-1} P)_{\times} \delta \xi }{\delta \xi} \\ = (T^{-1} P)_{\times}$

1.7. 补充

SO(3)重要性质与证明

1.8. 导数

2. 四元数的两种不同形式

2.1. 分类

通常四元数分为JPL四元数和Hamilton四元数两种，JPL在航空中常用而Hamilton则在机器人领域较为常用，ROS、Eigen、Ceres中使用的都是Hamilton四元数.

实部顺序:

实际使用中，需要注意一下即可，仅在计算中有些差别
- Hamilton四元数实部在前，虚部在后
- JPL四元数实部是最后一个元素
- 左手系与右手系:
两种定义下的四元数是共轭关系:

旋转操作:
- 坐标轴(frame)不变，旋转的是向量，类似相机不动，相机所观察的目标在运动，称为:Active Rotation
- 向量不变，坐标轴在变化，类似相机视角在变，目标不动，一个向量v可以用不同坐标系下的坐标表示，即:Passive Rotation
- JPL和Hamilton都是Passive表示
旋转变换:

从数值上来看，JPL四元数和Hamilton四元数是相等的.
- Local-to-Global: 将旋转后坐标系下的坐标变换到旋转前坐标系下
- Global-to-Local: 将旋转前坐标系下的坐标变换到旋转后坐标系下
- 对于添加扰动的区别：Local-to-Global 是右扰动，Global-to-Local 是左扰动
2.2. 扰动

2.2.1. 局部扰动

Local-to-Global的形式下，局部扰动表示为:

$\large \Delta R_L=\Delta R_{L\tilde{L}} \\\tilde{R}=R_{GL}\Delta R_{L\tilde{L}}\Delta R_L=\Delta R_{L\tilde{L}} \\\tilde{R}=R_{GL}\Delta R_{L\tilde{L}}$

2.2.2. 全局扰动

Global-to-Local形式下，全局扰动表示为:

2.3. 角速度

关于Barfoot中的四元数: Barfoot书中使用的四元数:　实部在后，右手系，Passive方式，左扰动

3. 旋转角速度

3.1. 扰动量的定义

局部扰动:

$\large \tilde{q}=q\otimes\Delta q_{\mathcal{L}}, \tilde{R}=R\Delta R_{\mathcal{L}}$

之所以乘在右边是由Hamilton形式四元数决定的. 从局部扰动可以获得对应在局部切空间的等效向量 $\large \Delta \phi_{\mathcal{L}}=\mathbf{u}\Delta\phi_{\mathcal{L}}$ 再利用指数映射:

$\large \large \tilde{q}_\mathcal{L}=q_{\mathcal{L}}\otimes Exp(\Delta\phi_{\mathcal{L}}) ,\quad \tilde{R}_{\mathcal{L}}=R_{{\mathcal{L}}}\cdot Exp(\Delta\phi{_{\mathcal{L}}})$

可以得到

$\large \large \Delta \phi_{{\mathcal{L}}} = Log(q_{{\mathcal{L}}}^*\otimes \tilde{q}_{\mathcal{L}}) = Log(R^T_{\mathcal{L}}\tilde{R}_{\mathcal{L}})$

如果扰动很小，那么可以进行近似，有如下结果:

$\large \large \Delta q_{{\mathcal{L}}} \approx \begin{bmatrix} 1 \\ \frac{1}{2}\Delta \phi_{{\mathcal{L}}} \end{bmatrix}, \Delta R_{\mathcal{L}}\approx I+[\Delta \phi_{\mathcal{L}}]_{\times}$

扰动量为对应的$SO(3)$流形处切空间的元素，方便去定义协方差矩阵.

全局扰动

$\large \large \tilde{q}_\mathcal{G}=Exp(\Delta\phi_{\mathcal{G}})\otimes q_{\mathcal{G}} ,\quad \tilde{R}_{\mathcal{G}}=Exp(\Delta\phi_{{\mathcal{L}}})\cdot R_{{\mathcal{L}}}$

扰动值:

$\large \large \Delta \phi_{{\mathcal{G}}} = Log(\tilde{q}_{\mathcal{G}}\otimes q_{{\mathcal{G}}}^*) = Log(\tilde{R}_{\mathcal{G}} R^T_{\mathcal{G}})$

对应原点处的切空间元素。

3.2. 对时间的导数

局部扰动求导:

初始状态 $\large \mathbf{q}=\mathbf{q}(t)$ 扰动状态为 $\large \tilde{\mathbf{q}} = \mathbf{q}(t+\Delta t)$ 求导公式可得:

$\large \omega_{\mathcal{L}}(t)\triangleq\frac{d\phi_{\mathcal{L}}(t)}{dt}\triangleq\lim_{\Delta t\rightarrow0}\frac{\Delta \phi_{\mathcal{L}}}{\Delta t}$

$\large \Delta \phi_{\mathcal{L}}$ 为q所在的局部坐标系的角速度向量对应的的局部扰动量，具体求导形式如下:

$\large \begin{aligned} \dot{\mathbf{q}} &\triangleq \lim_{\Delta \rightarrow0} \frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t}\\ &=\lim_{\Delta t \rightarrow0}\frac{\mathbf{q}\otimes\Delta \mathbf{q}{\mathcal{L}}-\mathbf{q}}{\Delta t} \\ &= \lim{\Delta t \rightarrow0}\frac{\mathbf{q}\otimes\left(\begin{bmatrix} 1 \\ \Delta \phi_{\mathcal{L}/2} \end{bmatrix}-\begin{bmatrix} 1 \\ \mathbf{0}\\ \end{bmatrix} \right)}{\Delta t} \\ &= \lim_{\Delta \rightarrow0}\frac{\mathbf{q}\otimes \begin{bmatrix} 0 \\ \Delta \phi_{\mathcal{L}}/2 \end{bmatrix} }{\Delta t} \\ &= \frac{1}{2}\mathbf{q}\otimes \begin{bmatrix} 0 \\ \omega_{\mathcal{L}} \end{bmatrix} \end{aligned}$

定义:

$\large \Omega(\omega)= [\omega]R=\begin{bmatrix} 0 & -\omega^T \\ \omega & -[\omega]{\times} \end{bmatrix}$

可以得到:

$\large \dot{\mathbf{q}}=\frac{1}{2}\Omega(\omega_{\mathcal{L}})\mathbf{q}=\frac{1}{2}\mathbf{q}\otimes\omega_{\mathcal{L}}, \quad \dot{R}= R[\omega_{\mathcal{L}}]{\times}$

可以看到，角速度 $\large \omega_{\mathcal{L}}$

是在特定参考系中的量，可以从IMU传感器直接获取.

全局扰动求导:

$\large \dot{\mathbf{q}}=\frac{1}{2}\omega_{\mathcal{G}}\otimes\mathbf{q}, \quad \dot{\mathbf{R}}=[\omega_{\mathcal{G}}]{\times}\mathbf{R}$

其中:

$\omega_{\mathcal{G}}\triangleq \frac{d\phi_{\mathcal{G}}(t)}{dt}$

对应全局坐标系下的角速度.

3.3. 局部与全局之间的变换关系

3.4. 利用Adjoint来变换

利用性质: $\large (Rv)^\vee=Rv^{\vee}R^T$

也可以得到对应的结果:

对于

$\large \dot{R}= R[\omega_{\mathcal{L}}]{\times}$

$\large \dot{R}= R[\omega_{\mathcal{L}}]{\times} =[R\omega_{\mathcal{L}}]{\times}R=[\omega_{\mathcal{G}}]_{\times}R$

4. 流形上的优化

参考文献

slam/liegroup/README.md · Zixin-YANG/SummerCamp - Gitee.com

g2o中 EdgeSE3Expmap类型Jacobian的计算_白巧克力亦唯心的博客-CSDN博客

A micro Lie theory for state estimation in robotics
相关阅读:
电商小程序实战教程-类别导航
 Pr 时间重映射卡点
 querySelectorAll()和querySelector()
Linux | 缓冲区理解 | 模拟实现C语言文件操作函数
 大龄程序员的出路究竟在何处？从369个过来人问答贴里，我们得到了答案
 为RPC服务增加异步日志模块
 列表初始化与右值引用
 【数据结构】图的存储结构及实现（邻接表和十字链表）
异步加载JS的方法
 Linux（3）：Linux 的文件权限与目录配置
原文地址：https://blog.csdn.net/xhtchina/article/details/126495655

1. 李群李代数部分整理

1.1. 群的概念

1.2. SO(3)推导

$\large \dot{\boldsymbol{R}}(t)=\boldsymbol{\phi}\left(t_{0}\right)^{\wedge} \boldsymbol{R}(t)=\boldsymbol{\phi}_{0}^{\wedge} \boldsymbol{R}(t)$

1.3. 李代数

1.4. 指数映射与对数映射

1.5. 伴随

1.6. 求导与扰动

1.6.1. BCH公式与左右雅克比

1.6.2. 需要计算雅克比矩阵

1.7. 补充

1.8. 导数

2. 四元数的两种不同形式

2.1. 分类

2.2. 扰动

2.2.1. 局部扰动

2.2.2. 全局扰动

2.3. 角速度

3. 旋转角速度

3.1. 扰动量的定义

3.2. 对时间的导数

3.3. 局部与全局之间的变换关系

3.4. 利用Adjoint来变换

4. 流形上的优化

参考文献