[阅读笔记] A micro Lie theory for state estimation in robotics

机器人状态估计的微李理论

Abstract

​ 介绍李理论的最基本原理，包含一些应用示例，提供了C++库。

1.Introduction

• 流形：李群的光滑拓扑表面，其中状态表示发生变化。

• 李群：

  • 拓扑学：涉及到流形的形状，并传达了与切空间和指数映射的关系的强大直觉。

• 代数学：涉及到群操作及其具体实现，允许利用代数特性来发展封闭形式的公式或简化它们。

• 几何学：将群元素与物体或参考坐标系的位置、速度、方向和/或其他变化联系起来。

• 参考文献

  • Basic Lie theory（Abbaspour）
  • Very basic Lie theory（Howe）
  • Naive Lie theory（Stiilwell）

• 开源库：https://github.com/artivis/manif

2.Micro lie theory

2.1 李群

​ 一个李群$\mathcal{G}$是一个光滑流形，其元素满足群公理：

• 封闭性：$\mathcal{X}\circ \mathcal{Y}\in \mathcal{G}$
• 单位元：$\epsilon \circ \mathcal{X}=\mathcal{X}\circ \epsilon =\mathcal{X}$
• 逆元 ：${\mathcal{X}}^{-1}\circ \mathcal{X}=\mathcal{X}\circ {\mathcal{X}}^{-1}=\epsilon$
• 结合律：$(\mathcal{X}\circ \mathcal{Y})\circ \mathcal{Z}=\mathcal{X}\circ (\mathcal{Y}\circ \mathcal{Z})$

​ 群$(\mathcal{G},\circ )$是一个集合，$\circ$为组合运算。

​ 李群的李代数：单位元处的切空间称之为李群的李代数。

Exs.1 单位复数群$S^1$

​ 复数乘法下的单位复数群$\mathrm{z}=\cos \theta +i\sin \theta$

​ 上图$T_1{S}^1$为李代数，$T_x{S}^1$为任意切空间，群元素$\mathrm{z}$的作用就是把$\mathrm{x}$在平面内旋转$\theta$。

Exs.2 单位四元数群$S^3$

​ 四元数乘法下的单位四元数群$\mathrm{q}=\cos (\theta /2)+u\sin (\theta /2)$

​ 向量$\mathrm{x}=ix+jy+kz$通过双四元数乘积${\mathrm{x}}^{\prime }=\mathrm{qx}{\mathrm{q}}^{\ast }$绕$u$旋转$\theta$，其中$u$是单位旋转轴。图中右侧两图过圆心垂直纸面向上的方向即为$u$的方向。

​ 中间图说的是从单位元旋转到$\mathrm{q}$，右边是从$\mathrm{p}$开始。

​ 至于为什么四元数旋转格式为双四元数乘积，参见b站视频：【较难慎入】证明四元数三维旋转公式_哔哩哔哩_bilibili

2.2 群作用

​ 李群具有将其他集合的元素进行变换的能力，可以产生旋转、平移、缩放、组合。

​ 定义：给定李群$\mathcal{M}$和集合$\mathcal{V}$，用$\mathcal{X}\cdot v$表示$\mathcal{X}\in \mathcal{M}$在$v\in \mathcal{V}$上的作用，记作
$$\cdot :\mathcal{M}\times \mathcal{V}\to \mathcal{V};(\mathcal{X},v)\mapsto \mathcal{X}\cdot v$$
​ 这里的点乘可以理解为一种表示方式，例如，一个旋转矩阵群，$SO(n)$；$x$为三维空间向量；其中$R$数于群$SO(n)$，$R\cdot x$的意思就是$Rx$，用旋转矩阵$R$左乘$x$，将其旋转为另一个姿态，即被称作群元素$R$对$x$的作用。
$$\begin{gathered} SO(n):旋转矩阵R\cdot \mathrm{x}\triangleq R\mathrm{x} \\ SE(n):欧几里得矩阵H\cdot \mathrm{x}\triangleq R\mathrm{x}+t \end{gathered}$$

2.3 切空间和李代数

​ 举例说明 $\mathcal{X}(t)$是流形$\mathcal{M}$上的移动点，其速度$\dot{\mathcal{X}}=\frac{\partial X}{\partial t}$属于在点$\mathcal{X}$处切于$\mathcal{M}$的空间，表示为$T_{\mathcal{X}}\mathcal{M}$。

​ 流形上每一个点的切空间都是唯一的，且是同构的。

1. 李代数$\mathfrak{m}$：单位元处的切空间，$T_{\mathcal{X}}\mathcal{M}$，即$\mathcal{M}$​的李代数
   $$\mathfrak{m}\triangleq T_{\mathcal{X}}\mathcal{M}$$

   • 李代数$\mathfrak{m}$是一个向量空间。
   • 指数映射，$\exp :\mathfrak{m}\to \mathcal{M}$，将李代数的元素转化为群的元素。
   • 伴随变换，将$\mathcal{X}$处的切空间中的向量线性变换转化为单位元$\epsilon$处的切空间的向量。

​ 上图中点$\mathrm{z}$以恒定角速率运动，$\mathbf{z}(t)=\cos \omega t+i\sin \omega t$。

​ 绿线处的切向量（速度）$\dot{\mathbf{z}}=i\omega$，李代数${}^1{\mathbf{v}}^{\wedge }=i\omega$，上式对$\omega$求导，取$t=0$。

​ 红线处的切向量（速度）$\dot{\mathbf{z}}=-\omega \sin \omega t+i\omega \cos \omega t=\mathbf{z}\cdot i\omega$，李代数${}^{\mathbf{z}}{\mathbf{v}}^{\wedge }=i\omega$，同样是求导得到。

​ 注意到${}^{\mathbf{z}}{\mathbf{v}}^{\wedge }={\mathbf{z}}^{-1}\dot{\mathbf{z}}={\mathbf{z}}^{\ast }\dot{\mathbf{z}}$，其中${\mathbf{z}}^{\ast }=\cos \omega t-i\sin \omega t$ ，${\mathbf{z}}^{-1}=\cos (-\omega t)+i\sin (-\omega t)=\cos \omega t-i\sin \omega t$ 。

​

​ 李代数的元素形式 对于一个乘法群${\mathcal{X}}^{-1}\mathcal{X}=\epsilon$，有约束${\mathcal{X}}^{-1}\dot{\mathcal{X}}+{\dot{\mathcal{X}}}^{-1}\mathcal{X}=0$，适用于在$\mathcal{X}$处的切向量，因此，李代数的元素形式为
$${\mathbf{v}}^{\wedge }={\mathcal{X}}^{-1}\dot{\mathcal{X}}=-{\dot{\mathcal{X}}}^{-1}\mathcal{X}$$

2. 笛卡尔空间向量${\mathbb{R}}^m$

   李代数的元素可以表示为基元素${\mathbf{E}}_i$的线性组合，互逆线性映射$hat$和$vee$实现了$\mathfrak{m}$和${\mathbf{R}}^m$之间的转换，
   $$\begin{gathered} \mathbf{Hat}:{\mathbf{R}}^m\to \mathfrak{m};\tau \mapsto {\tau }^{\wedge }=\sum _{i=1}^m{\tau }_i{E}_i \\ \mathbf{Vee}:\mathfrak{m}\to {\mathbf{R}}^m;{\tau }^{\wedge }\mapsto ({\tau }^{\wedge }{)}^{\vee }=\sum _{i=1}^m{\tau }_i{\mathbf{e}}_i \end{gathered}$$
   本文倾向于使用${\mathbb{R}}^m$。

Exs.3 旋转群$SO(3)$、其李代数$\mathfrak{so}(3)$和向量空间${\mathbb{R}}^3$

​ 旋转矩阵$\mathbf{R}$有约束（正交条件）：${\mathbf{R}}^{\mathrm{T}}\mathbf{R}=\mathbf{I}$，求时间导数可得切空间，
$$\begin{gathered} {\mathbf{R}}^{\mathrm{T}}\dot{\mathbf{R}}+{\dot{\mathbf{R}}}^{\mathrm{T}}\mathbf{R}=0 \\ {\mathbf{R}}^{\mathrm{T}}\dot{\mathbf{R}}=-({\mathbf{R}}^{\mathrm{T}}\dot{\mathbf{R}}{)}^{\mathrm{T}} \end{gathered}$$
​ 上面说明${\mathbf{R}}^{\mathrm{T}}\dot{\mathbf{R}}$是斜对称矩阵，记作$[\omega {]}_{\times }$
$$[\omega {]}_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]={\mathbf{R}}^{\mathrm{T}}\dot{\mathbf{R}}$$
​ 当$\mathbf{R}=\mathbf{I}$时，$\dot{\mathbf{R}}=[\omega {]}_{\times }$，说明$[\omega {]}_{\times }$就是该旋转矩阵群的李代数，其自由度数为3。

​ 李代数的元素可以用基元素${\mathbf{E}}_i$（也叫生成元）的线性组合来表示，映射为向量空间的基元素${\mathbf{e}}_i$，李代数和向量空间${\mathbf{R}}^m$同构，两基元的转换由$\mathrm{Hat}$和$\mathrm{Vee}$操作完成

Exs.5 单位四元数群$S^3$

​ 回顾例2，单位范数约束下，${\mathbf{q}}^{\ast }\dot{\mathbf{q}}=1$，对时间求导，
$${\mathbf{q}}^{\ast }\dot{\mathbf{q}}=-({\mathbf{q}}^{\ast }\dot{\mathbf{q}}{)}^{\ast }$$
​ ${\mathbf{q}}^{\ast }\dot{\mathbf{q}}$是纯四元数（实部为零），纯四元数取共轭求负之后还是自身。

​ 用$\mathbf{u}v$来表示一个纯四元数，展开如下
$$\mathbf{u}v=(i{u}_x+j{u}_y+k{u}_z)v=i{v}_x+j{v}_y+k{v}_z$$
​ 其中$\mathbf{u}$是单位纯四元数，$v$是范数，相当于对$\mathbf{u}$长度拉伸得到一个纯四元数，将其带入上面的式子
$$\dot{\mathbf{q}}=\mathbf{qu}v\in T_q{S}^3$$
​ 积分得到
$$\mathbf{q}={\mathbf{q}}_0\exp (\mathbf{u}vt)$$
​ 令${\mathbf{q}}_0=1$，定义$\mathbf{\varphi }\triangleq \mathbf{u}\varphi \triangleq \mathbf{u}vt$，由于$\mathbf{u}$的幂有规律$1,\mathbf{u},-1,-\mathbf{u},1,...$，有
$$\begin{gathered} \mathbf{q}=\exp (\mathbf{u}\varphi )\triangleq \sum \frac{{\varphi }^k}{k!}{\mathbf{u}}^k \\ \mathbf{q}=\exp (\mathbf{u}\varphi )=\cos (\varphi )+\mathbf{u}\sin (\varphi ) \end{gathered}$$
​ 李代数元素$\Phi \in {\mathfrak{s}}^3$，可以通过$hat$和$vee$映射到$\mathbf{\theta }\in {\mathbb{R}}^3$
$$\begin{gathered} \mathbf{Hat}:{\mathbb{R}}^3\to {\mathfrak{s}}^3\mathbf{\theta }\mapsto {\mathbf{\theta }}^{\wedge }=\mathbf{\theta }/2 \\ \mathbf{Vee}:{\mathfrak{s}}^3\to {\mathbb{R}}^3\mathbf{\varphi }\mapsto {\mathbf{\varphi }}^{\vee }=2\mathbf{\varphi } \end{gathered}$$
​ 2是为了计算旋转向量的因子，式中$\mathbf{\theta }\triangleq \mathbf{u}\theta \triangleq \mathbf{w}t$是角-轴向量，根据$hat$和$vee$，四元数的指数形式为
$$\mathbf{q}=\mathrm{Exp}(\mathbf{u}\theta )=\cos (\theta /2)+\mathbf{u}\sin (\theta /2)$$
​ 上式对应于例2，其指数形式同样可以参考b站视频。

2.4 指数映射

​ 指数映射是将李代数精确的转化为群中的元素，其逆映射为$\log ()$，指数映射及其逆映射分为小写 和大写两种，一般将小写的直接称为指数映射
$$\begin{gathered} \exp :\mathfrak{m}\to \mathcal{M};{\mathbf{\tau }}^{\wedge }\mapsto \mathcal{X}=\exp ({\mathbf{\tau }}^{\wedge }) \\ \log :\mathcal{M}\to \mathfrak{m};\mathcal{X}\mapsto {\mathbf{\tau }}^{\wedge }=\log (\mathcal{X}) \\ \mathrm{Exp}:{\mathbb{R}}^m\to \mathcal{M};\mathbf{\tau }\mapsto \mathcal{X}=\mathrm{Exp}(\mathbf{\tau }) \\ \mathrm{Log}:\mathcal{M}\to {\mathbb{R}}^m;\mathcal{X}\mapsto \mathbf{\tau }=\mathrm{Exp}(\mathcal{X}) \end{gathered}$$
指数映射的关键性质
$$\begin{gathered} \exp ((t+s){\mathbf{\tau }}^{\wedge })=\exp (t{\mathbf{\tau }}^{\wedge })\exp (s{\mathbf{\tau }}^{\wedge }) \\ \exp (t{\mathbf{\tau }}^{\wedge })=\exp ({\tau }^{\wedge }{)}^t \\ \exp (-{\mathbf{\tau }}^{\wedge })=\exp ({\tau }^{\wedge }{)}^{-1} \\ \exp (\mathcal{X}{\mathbf{\tau }}^{\wedge }{\mathcal{X}}^{-1})=\mathcal{X}\exp ({\tau }^{\wedge }{)}^{-1}{\mathcal{X}}^{-1} \end{gathered}$$

2.5 plus 和 minus 运算符

​ 允许引入流形元素之间的增量，并在其切向量空间中表示。符号：$\oplus$和$\ominus$

​ 有左右两个版本：
$$\begin{gathered} \mathrm{right}-\oplus :\mathcal{Y}=\mathcal{X}\oplus {}^{\mathcal{X}}\mathbf{\tau }\triangleq \mathcal{X}\circ \mathrm{Exp}{(}^{\mathcal{X}}\mathbf{\tau })\in \mathcal{M} \\ \mathrm{right}-\ominus :{}^{\mathcal{X}}\mathbf{\tau }=\mathcal{Y}\ominus \mathcal{X}\triangleq \mathrm{Log}({\mathcal{X}}^{-1}\circ \mathcal{Y})\in {\mathcal{T}}_{\mathcal{X}}\mathcal{M} \end{gathered}$$
$$\begin{gathered} \mathrm{left}-\oplus :\mathcal{Y}={}^{\epsilon }\mathbf{\tau }\oplus \mathcal{X}\triangleq \mathrm{Exp}{(}^{\epsilon }\mathbf{\tau })\circ \mathcal{X}\in \mathcal{M} \\ \mathrm{left}-\ominus :{}^{\epsilon }\mathbf{\tau }=\mathcal{Y}\ominus \mathcal{X}\triangleq \mathrm{Log}(\mathcal{Y}\circ {\mathcal{X}}^{-1})\in {\mathcal{T}}_{\mathcal{X}}\mathcal{M} \end{gathered}$$

​ 本文默认采用右形式。

2.6伴随和伴随矩阵

​ 利用2.5中的右和左$-\oplus$操作，可以得到
$$\mathrm{Exp}{(}^{\epsilon }\mathbf{\tau })\mathcal{X}=\mathcal{X}\mathrm{Exp}{(}^{\mathcal{X}}\mathbf{\tau })$$
​ 从小写和大写指数映射操作及指数映射性质可以知道
$$\exp {(}^{\epsilon }{\mathbf{\tau }}^{\wedge })=\mathcal{X}\exp {(}^{\mathcal{X}}{\mathbf{\tau }}^{\wedge }){\mathcal{X}}^{-1}=\exp (\mathcal{X}{{}^{\mathcal{X}}\mathbf{\tau }}^{\wedge }{\mathcal{X}}^{-1})$$
​ 所以可以定义在$\mathcal{X}$处的伴随
$${\mathrm{Ad}}_{\mathcal{X}}:\mathfrak{m}\to \mathfrak{m};{\mathbf{\tau }}^{\wedge }\mapsto {\mathrm{Ad}}_{\mathcal{X}}({\mathbf{\tau }}^{\wedge })\triangleq \mathcal{X}{\mathbf{\tau }}^{\wedge }{\mathcal{X}}^{-1}$$
​ 作用是将点$\mathcal{X}$转移到单位元$\epsilon$处，${}^{\epsilon }{\mathbf{\tau }}^{\wedge }={\mathrm{Ad}}_{\mathcal{X}}{(}^{\mathcal{X}}{\mathbf{\tau }}^{\wedge })$。

• 伴随性质：

$$\begin{gathered} 线性:{\mathrm{Ad}}_{\mathcal{X}}(a{\mathbf{\tau }}^{\wedge }+b{\mathbf{\sigma }}^{\wedge })=a{\mathrm{Ad}}_{\mathcal{X}}({\mathbf{\tau }}^{\wedge })+b{\mathrm{Ad}}_{\mathcal{X}}({\mathbf{\sigma }}^{\wedge }) \\ 同态映射:{\mathrm{Ad}}_{\mathcal{X}}({\mathrm{Ad}}_{\mathcal{Y}}({\mathbf{\tau }}^{\wedge }))={\mathrm{Ad}}_{\mathcal{X}\mathcal{Y}}({\mathbf{\tau }}^{\wedge }) \end{gathered}$$

${\mathrm{Ad}}_{\mathcal{X}}()$是线性的，可以找到一个等效的矩阵算子${\mathbf{Ad}}_{\mathcal{X}}$，作用于笛卡尔向量空间，定义伴随矩阵，
$$\begin{gathered} {\mathbf{Ad}}_{\mathcal{X}}:{\mathbb{R}}^m\to {\mathbb{R}}^m;{}^{\mathcal{X}}\mathbf{\tau }{\mapsto }^{\epsilon }\mathbf{\tau }={\mathbf{Ad}}_{\mathcal{X}}{}^{\mathcal{X}}\mathbf{\tau } \\ {\mathbf{Ad}}_{\mathcal{X}}\mathbf{\tau }=(\mathcal{X}{\mathbf{\tau }}^{\wedge }{\mathcal{X}}^{-1}{)}^{\vee } \end{gathered}$$
​ 作用是把任一点的切向量空间的向量线性变换为原点的切向量空间的向量。

• 伴随矩阵性质：

$$\begin{gathered} \mathcal{X}\oplus \tau =({\mathbf{Ad}}_{\mathcal{X}}\mathbf{\tau })\oplus \mathcal{X} \\ {\mathbf{Ad}}_{{\mathcal{X}}^{-1}}={{\mathbf{Ad}}_{\mathcal{X}}}^{-1} \\ {\mathbf{Ad}}_{\mathcal{X}\mathcal{Y}}={\mathbf{Ad}}_{\mathcal{X}}{\mathbf{Ad}}_{\mathcal{Y}} \end{gathered}$$

2.7李群上的导数

1. 向量空间上的雅可比矩阵

​ 多变量函数$f:{\mathbb{R}}^m\to {\mathbb{R}}^n$，雅可比矩阵为$n\times m$，
$$\mathbf{J}=\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\triangleq \left[\begin{array}{ccc}\frac{\partial f_1}{\partial x_1}& \cdots & \frac{\partial f_1}{\partial x_m}\\ ⋮& & ⋮\\ \frac{\partial f_n}{\partial x_1}& \cdots & \frac{\partial f_n}{\partial x_m}\end{array}\right]\in {\mathbb{R}}^{n\times m}$$
​ 上式雅可比矩阵可以分块为列向量组成，这样每一列对应于，
$$\mathbf{j}=\frac{\partial f(\mathbf{x})}{\partial x_i}\triangleq \underset{h\to 0}{\lim }\frac{f(\mathbf{x}+h{\mathbf{e}}_i)-f(\mathbf{x})}{h}\in {\mathbb{R}}^n$$
​ 分子可以表示为${\mathbf{v}}_i(h)$，是$\mathbf{x}$沿${\mathbf{e}}_i$方向扰动时，$f(\mathbf{x})$的增量。

​ 紧凑形式为
$$\mathbf{J}=\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\triangleq \underset{\mathbf{h}\to 0}{\lim }\frac{f(\mathbf{x}+\mathbf{h})-f(\mathbf{x})}{\mathbf{h}}\in {\mathbb{R}}^{n\times m}$$

2. 李群上的右雅可比矩阵

​ 使用$\mathrm{right}-\oplus ,\ominus$替代紧凑形式的$+,-$
$$\frac{{}^{\mathcal{X}}Df(\mathcal{X})}{D\mathcal{X}}\triangleq \underset{\mathbf{\tau }\to 0}{\lim }\frac{f(\mathcal{X}\oplus \mathbf{\tau })\ominus f(\mathcal{X})}{\mathbf{\tau }}\in {\mathbb{R}}^{n\times m}$$

​ 上式可按规则展开，同样的，${\mathbf{e}}_i$方向扰动增量为${\sigma }_i(h)=f(\mathcal{X}\oplus h{\mathbf{e}}_i)\ominus f(\mathcal{X})\in {\mathbb{R}}^n$，上图为流形上函数$f:\mathcal{M}\to \mathcal{N}$的右雅可比矩阵，映射局部切空间$T_{\mathcal{X}}\mathcal{M}\to T_{f(\mathcal{X})}\mathcal{N}$。
\quad 例子参考，$f:SO(3)\to {\mathbb{R}}^3;f(\mathbf{R})=\mathbf{Rp},\mathcal{M}=SO(3),\mathcal{N}={\mathbb{R}}^3$
$$\frac{{}^{\mathbf{R}}D\mathbf{Rp}}{D\mathbf{R}}=-\mathbf{R}[\mathbf{p}{]}_{\times }\in {\mathbb{R}}^{3\times 3}$$

​ 对于小的$\mathbf{\tau }$有线性近似
$$f(\mathcal{X}\oplus {}^{\mathcal{X}}\mathbf{\tau })\underset{{}^{\mathcal{X}}\mathbf{\tau }\to 0}{\to }f(\mathcal{X})\oplus \frac{{}^{\mathcal{X}}Df(\mathcal{X})}{D\mathcal{X}}{}^{\mathcal{X}}\mathbf{\tau }\in \mathcal{N}$$

3. 李群上的左雅可比矩阵

​ 其导数是通过左操作来定义的，
$$\frac{{}^{\epsilon }Df(\mathcal{X})}{D\mathcal{X}}\triangleq \underset{\mathbf{\tau }\to 0}{\lim }\frac{f(\mathbf{\tau }\oplus \mathcal{X})\ominus f(\mathcal{X})}{\mathbf{\tau }}\in {\mathbb{R}}^{n\times m}$$
​ 映射全局切空间$T_{\epsilon }\mathcal{M}\to T_{\epsilon }\mathcal{N}$。这些切空间分别是$\mathcal{M}$和$\mathcal{N}$的李代数。

4. 交叉右-左雅可比矩阵

​ 可以使用右加和左减定义雅可比矩阵，反之亦可。它们可以将局部切线映射到全局，或反之。

​ 通过伴随算子将它们和其他雅可比矩阵相关联。
$$\begin{gathered} \frac{{}^{\epsilon }D\mathcal{Y}}{{}^{\mathcal{X}}D\mathcal{X}}=\frac{{}^{\epsilon }D\mathcal{Y}}{{}^{\epsilon }D\mathcal{X}}{\mathbf{Ad}}_{\mathcal{X}}={\mathbf{Ad}}_{\mathcal{Y}}\frac{{}^{\mathcal{Y}}D\mathcal{Y}}{{}^{\mathcal{X}}D\mathcal{X}} \\ \frac{{}^{\mathcal{Y}}D\mathcal{Y}}{{}^{\epsilon }D\mathcal{X}}=\frac{{}^{\mathcal{Y}}D\mathcal{Y}}{{}^{\mathcal{X}}D\mathcal{X}}{{\mathbf{Ad}}_{\mathcal{X}}}^{-1}={{\mathbf{Ad}}_{\mathcal{Y}}}^{-1}\frac{{}^{\epsilon }D\mathcal{Y}}{{}^{\epsilon }D\mathcal{X}} \end{gathered}$$
​ 式中，$\mathcal{Y}=f(\mathcal{X})$。

​ 左右雅可比矩阵可以通过$\mathcal{M}$和$\mathcal{N}$的伴随关系相关联，
$$\frac{{}^{\epsilon }Df(\mathcal{X})}{D\mathcal{X}}{\mathbf{Ad}}_{\mathcal{X}}={\mathbf{Ad}}_{f(\mathcal{X})}\frac{{}^{\mathcal{X}}Df(\mathcal{X})}{D\mathcal{X}}$$

2.8流形中的不确定性，协方差传播

​ 使用右-$\oplus$和$\ominus$来定义流形中某点$\bar{\mathcal{X}}\in \mathcal{M}$的切向量空间$T_{\bar{\mathcal{X}}}\mathcal{M}$中的局部扰动$\mathbf{\tau }$,
$$\mathcal{X}=\bar{\mathcal{X}}\oplus \mathbf{\tau },\mathbf{\tau }=\mathcal{X}\ominus \bar{\mathcal{X}}\in T_{\bar{\mathcal{X}}}\mathcal{M}$$

​ 图中切点为$\bar{\mathcal{X}}$，在微小扰动$\mathbf{\tau }$下，偏移后的$\mathcal{X}$的范围分别表示在流形和切空间中。

​ 在$\bar{\mathcal{X}}$处的切向量空间定义协方差矩阵，
$${\sum }_{\mathcal{X}}\triangleq \mathbb{E}[\mathbf{\tau }{\mathbf{\tau }}^{-1}]=\mathbb{E}[(\mathcal{X}\ominus \bar{\mathcal{X}})(\mathcal{X}\ominus \bar{\mathcal{X}}{)}^{\mathrm{T}}]\in {\mathbb{R}}^{m\times m}$$
​ 局部扰动$\mathbf{\tau }$的方向是任意的，所以$\mathcal{X}$也是在范围内任意（$m$维有$m$个随机变量），$\bar{\mathcal{X}}$可类比为该随机变量的期望。

​ 扰动也可以全局（左-$\oplus$和$\ominus$）表示，就是原点切向量空间中$T_{\epsilon }\mathcal{M}$，
$$\mathcal{X}=\mathbf{\tau }\oplus \bar{\mathcal{X}}\mathbf{\tau }=\mathcal{X}\ominus \bar{\mathcal{X}}\in T_{\epsilon }\mathcal{M}$$
​ 局部和全局可以通过伴随关联在一起，
$${}^{\epsilon }{\sum }_{\mathcal{X}}={\mathbf{Ad}}_{\mathcal{X}}{}^{\mathcal{X}}{\sum }_{\mathcal{X}}{{\mathbf{Ad}}_{\mathcal{X}}}^{\mathrm{T}}$$
​ 函数$f:\mathcal{M}\to \mathcal{N};\mathcal{X}\mapsto \mathcal{Y}=f(\mathcal{X})$的协方差传播使用雅可比矩阵的线性近似，
$${\sum }_{\mathcal{Y}}\approx \frac{Df}{D\mathcal{X}}{\sum }_{\mathcal{X}}{\frac{Df}{D\mathcal{X}}}^{\mathrm{T}}\in {\mathbb{R}}^{n\times n}$$

2.9流形上的离散积分

​ 指数映射$\mathcal{X}(t)={\mathcal{X}}_0\circ \mathrm{Exp}(\mathbf{v}t)$是连续积分，对于非恒定速度，可将其分段为恒定速度，每一段在流形的不同切空间中。定义${\mathbf{\tau }}_k={\mathbf{v}}_k\delta t_k$，则
$${\mathcal{X}}_k={\mathcal{X}}_0\oplus {\mathbf{\tau }}_1\oplus {\mathbf{\tau }}_2\oplus \cdots \oplus {\mathbf{\tau }}_k$$
​ 迭代形式为
$${\mathcal{X}}_k={\mathcal{X}}_{k-1}\oplus {\mathbf{\tau }}_k={\mathcal{X}}_{k-1}\circ \mathrm{Exp}({\mathbf{\tau }}_k)$$

​ 例子，将角速度$\omega$积分成旋转矩阵或四元数，（$\mathfrak{m}\to \mathcal{M}$）,
$$\begin{gathered} {\mathbf{R}}_k={\mathbf{R}}_{k-1}\mathrm{Exp}({\mathbf{\omega }}_k\delta t) \\ {\mathbf{q}}_k={\mathbf{q}}_{k-1}\mathrm{Exp}({\mathbf{\omega }}_k\delta t) \end{gathered}$$

3. Differentiation rules on manifolds

​ 这里开发的所有雅可比矩阵都是右雅可比矩阵。使用符号${\mathbf{J}}_{\mathcal{X}}^{\mathcal{Y}}\triangleq \frac{D\mathcal{Y}}{D\mathcal{X}}$，由伴随矩阵性质，${{\mathbf{Ad}}_{\mathcal{X}}}^{-1}$由${\mathbf{Ad}}_{{\mathcal{X}}^{-1}}$实现。

3.1链式法则

​ 对于$\mathcal{Y}=f(\mathcal{X})$和$\mathcal{Z}=g(\mathcal{Y})$，有$\mathcal{Z}=g(f(\mathcal{X}))$。链式法则陈述如下，
$$\frac{D\mathcal{Z}}{D\mathcal{X}}=\frac{D\mathcal{Z}}{D\mathcal{Y}}\frac{D\mathcal{Y}}{D\mathcal{X}}or{\mathbf{J}}_{\mathcal{X}}^{\mathcal{Z}}={\mathbf{J}}_{\mathcal{Y}}^{\mathcal{Z}}{\mathbf{J}}_{\mathcal{X}}^{\mathcal{Y}}$$

3.2基本雅可比矩阵块

1. 求逆

​ 用右雅可比矩阵的定义式$$\frac{{}^{\mathcal{X}}Df(\mathcal{X})}{D\mathcal{X}}\triangleq \underset{\mathbf{\tau }\to 0}{\lim }\frac{f(\mathcal{X}\oplus \mathbf{\tau })\ominus f(\mathcal{X})}{\mathbf{\tau }}$$进行定义，然后展开可知其为伴随的负，
$$\begin{gathered} {\mathbf{J}}_{\mathcal{X}}^{{\mathcal{X}}^{-1}}\triangleq \frac{{}^{\mathcal{X}}D{\mathcal{X}}^{-1}}{D\mathcal{X}}\in {\mathbb{R}}^{m\times m} \\ {\mathbf{J}}_{\mathcal{X}}^{{\mathcal{X}}^{-1}}=-{\mathbf{Ad}}_{\mathcal{X}} \end{gathered}$$

2. 复合

​ 定义同上，
$$\begin{gathered} {\mathbf{J}}_{\mathcal{X}}^{\mathcal{X}\circ \mathcal{Y}}\triangleq \frac{{}^{\mathcal{X}}D\mathcal{X}\circ \mathcal{Y}}{D\mathcal{X}}\in {\mathbb{R}}^{m\times m} \\ {\mathbf{J}}_{\mathcal{Y}}^{\mathcal{X}\circ \mathcal{Y}}\triangleq \frac{{}^{\mathcal{Y}}D\mathcal{X}\circ \mathcal{Y}}{D\mathcal{Y}}\in {\mathbb{R}}^{m\times m} \end{gathered}$$
​ 展开并使用$\exp (\mathcal{X}{\mathbf{\tau }}^{\wedge }{\mathcal{X}}^{-1})=\mathcal{X}\exp ({\tau }^{\wedge }{)}^{-1}{\mathcal{X}}^{-1}$、${\mathbf{Ad}}_{\mathcal{X}}\mathbf{\tau }=(\mathcal{X}{\mathbf{\tau }}^{\wedge }{\mathcal{X}}^{-1}{)}^{\vee }$、${\mathbf{Ad}}_{{\mathcal{X}}^{-1}}={{\mathbf{Ad}}_{\mathcal{X}}}^{-1}$，得到
$$\begin{gathered} {\mathbf{J}}_{\mathcal{X}}^{\mathcal{X}\circ \mathcal{Y}}={{\mathbf{Ad}}_{\mathcal{Y}}}^{-1} \\ {\mathbf{J}}_{\mathcal{Y}}^{\mathcal{X}\circ \mathcal{Y}}=\mathbf{I} \end{gathered}$$

3. $\mathcal{M}$的雅可比矩阵

​ 将$\mathcal{M}$的右雅可比矩阵定义为$\mathcal{X}=\mathrm{Exp}(\mathbf{\tau })$的右雅可比矩阵，对于$\mathbf{\tau }\in {\mathbb{R}}^m$，
$${\mathbf{J}}_r(\mathbf{\tau })\triangleq \frac{{}^{\mathbf{\tau }}D\mathrm{Exp}(\mathbf{\tau })}{D\mathbf{\tau }}\in {\mathbb{R}}^{m\times m}$$
​ 左雅可比矩阵定义为
$${\mathbf{J}}_l(\mathbf{\tau })\triangleq \frac{{}^{\mathbf{\epsilon }}D\mathrm{Exp}(\mathbf{\tau })}{D\mathbf{\tau }}\in {\mathbb{R}}^{m\times m}$$
​ 用伴随相关联，
$${\mathbf{Ad}}_{\mathrm{Exp}(\mathbf{\tau })}={\mathbf{J}}_l(\mathbf{\tau }){{\mathbf{J}}_r}^{-1}(\mathbf{\tau })$$
​ 根据链式法则
$${\mathbf{J}}_r(-\mathbf{\tau })={\mathbf{J}}_l(\mathbf{\tau })$$

4. 群作用

​ 对于$\mathcal{X}\in \mathcal{M};v\in \mathcal{V}$，定义
$$\begin{gathered} {\mathbf{J}}_{\mathcal{X}}^{\mathcal{X}\cdot v}\triangleq \frac{{}^{\mathcal{X}}D\mathcal{X}\cdot v}{D\mathcal{X}} \\ {\mathbf{J}}_v^{\mathcal{X}\cdot v}\triangleq \frac{{}^{v}D\mathcal{X}\cdot v}{Dv} \end{gathered}$$

3.3额外推导

1. $\mathrm{Log}$映射：对于$\mathbf{\tau }=\mathrm{Log}(\mathcal{X})$，有${\mathbf{J}}_{\mathcal{X}}^{\mathrm{Log}(\mathcal{X})}={\mathbf{J}}_r^{-1}(\mathbf{\tau })$

2. 加与减：
   $$\begin{gathered} {\mathbf{J}}_{\mathcal{X}}^{\mathcal{X}\oplus \mathbf{\tau }}={{\mathbf{Ad}}_{\mathrm{Exp}(\mathbf{\tau })}}^{-1} \\ {\mathbf{J}}_{\mathbf{\tau }}^{\mathcal{X}\oplus \mathbf{\tau }}={\mathbf{J}}_r(\mathbf{\tau }) \end{gathered}$$

​ 给定$\mathcal{Z}={\mathcal{X}}^{-1}\circ \mathcal{Y}$和$\mathbf{\tau }=\mathcal{Y}\ominus \mathcal{X}=\mathrm{Log}(\mathcal{Z})$，
$$\begin{gathered} {\mathbf{J}}_{\mathcal{X}}^{\mathcal{Y}\ominus \mathcal{X}}=-{\mathbf{J}}_l^{-1}(\mathbf{\tau }) \\ {\mathbf{J}}_{\mathcal{Y}}^{\mathcal{Y}\ominus \mathcal{X}}={\mathbf{J}}_r^{-1}(\mathbf{\tau }) \end{gathered}$$

4.Composite manifolds

​ 概念

​ 一个复合流形是$M$个不相关的流形的串联，$\mathcal{M}=⟨{\mathcal{M}}_1,\cdots ,{\mathcal{M}}_M⟩$。

​ 类似于单个流形：

​ 单位元、逆元、组合
$${\epsilon }_{\diamond }\triangleq \left[\begin{array}{c}{\epsilon }_1\\ ⋮\\ {\epsilon }_M\end{array}\right],{\mathcal{X}}^{\diamond }\triangleq \left[\begin{array}{c}{\mathcal{X}}^{-1}\\ ⋮\\ {\mathcal{X}}_M^{-1}\end{array}\right],\mathcal{X}\diamond \mathcal{Y}\triangleq \left[\begin{array}{c}\mathcal{X}\circ {\mathcal{Y}}_1\\ ⋮\\ {\mathcal{X}}_M\circ {\mathcal{Y}}_M\end{array}\right]$$
​ 指数映射
$$\mathrm{Exp}⟨\mathbf{\tau }⟩\triangleq \left[\begin{array}{c}\mathrm{Exp}({\mathbf{\tau }}_1)\\ ⋮\\ \mathrm{Exp}({\mathbf{\tau }}_M)\end{array}\right],\mathrm{Log}⟨\mathcal{X}⟩\triangleq \left[\begin{array}{c}\mathrm{Log}(\mathcal{X})\\ ⋮\\ \mathrm{Log}({\mathcal{X}}_M)\end{array}\right]$$
​ 右加和减
$$\begin{gathered} \mathcal{X}(\mathrm{diamondplus}){}^{\mathcal{X}}\mathbf{\tau }\triangleq \mathcal{X}\diamond \mathrm{Exp}(\mathbf{\tau }) \\ \mathcal{Y}(\mathrm{diamondminus})\mathcal{X}\triangleq \mathrm{Log}({\mathcal{X}}^{\diamond }\diamond \mathcal{Y}) \end{gathered}$$
这里的(diamondplus/minus)是$\diamond$中间有加减符号，打不出来就用文字表示了。
​ 右导数
$$\frac{Df(\mathcal{X})}{D\mathcal{X}}\triangleq \underset{\mathbf{\tau }\to 0}{\lim }\frac{f(\mathcal{X}(\mathrm{diamondplus})\mathbf{\tau })(\mathrm{diamondminus})f(\mathcal{X})}{\mathbf{\tau }}$$
​ 雅可比矩阵
$$\frac{Df(\mathcal{X})}{D\mathcal{X}}=\left[\begin{array}{ccc}\frac{D{f}_1}{D{\mathcal{X}}_1}& \cdots & \frac{D{f}_1}{D{\mathcal{X}}_M}\\ ⋮& & ⋮\\ \frac{D{f}_N}{D{\mathcal{X}}_1}& \cdots & \frac{D{f}_N}{D{\mathcal{X}}_M}\end{array}\right]$$
​ 每一项用右雅可比矩阵定义式计算。对于较小的$\mathbf{\tau }$有线性近似，$f(\mathcal{X}(\mathrm{diamondplus})\mathbf{\tau })\underset{\mathbf{\tau }\to 0}{\to }f(\mathcal{X})(\mathrm{diamondplus})\frac{Df(\mathcal{X})}{D\mathcal{X}}\mathbf{\tau }\in \mathcal{N}$

​ 协方差矩阵
$${\sum }_{\mathcal{X}}\triangleq \mathbb{E}[(\mathcal{X}(\mathrm{diamondminus})\bar{\mathcal{X}})(\mathcal{X}(\mathrm{diamondminus})\bar{\mathcal{X}}{)}^{\mathrm{T}}]\in {\mathbb{R}}^{n\times n}$$

5.Landmark-based localization and mapping

​ 提供了三个应用示例：

• 用于基于地标定位的卡尔曼滤波器
• 基于图的平滑方法，用于同时定位和建图
• 增加了传感器自校正

​ 引例

​ 机器人位于$SE(2)$中，通过$\mathbf{u}\in \mathfrak{se}(2)$控制纵向速度和角速度$v,\omega$，没有横向分量
$$\mathcal{X}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0& 1\end{array}\right]\in SE(2),{\mathbf{b}}_k=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]\in {\mathbb{R}}^2$$

$$\begin{gathered} \mathbf{u}=\left[\begin{array}{c}{u}_v\\ u_s\\ u_w\end{array}\right]=\left[\begin{array}{c}v\delta t\\ 0\\ \omega \delta t\end{array}\right]+\mathbf{w}\in \mathfrak{se}(2) \\ \mathbf{W}=\left[\begin{array}{ccc}{\sigma }_v^2\delta t& 0& 0\\ 0& {\sigma }_s^2\delta t& 0\\ 0& 0& {\sigma }_{\omega }^2\delta t\end{array}\right]\in {\mathbb{R}}^{3\times 3} \end{gathered}$$

​ 其中的高斯噪声$\mathbf{w}\sim \mathcal{N}(0,\mathbf{W})$是可能的车轮横向打滑$u_s$，${\mathbf{b}}_k$为信标位置。

​ 位姿更新${\mathcal{X}}_j={\mathcal{X}}_i\oplus {\mathbf{u}}_j\triangleq {\mathcal{X}}_i\mathrm{Exp}({\mathbf{u}}_j)$

1. 流形上使用误差状态卡尔曼滤波器进行定位

​ 待估计位姿$ \mathcal{\hat X}\in SE(2)$，估计误差，协方差,
$$\begin{gathered} \delta \mathbf{x}\triangleq \mathcal{X}\ominus \hat{\mathcal{X}}\in {\mathbb{R}}^3 \\ \mathbf{P}\triangleq \mathbb{E}[(\mathcal{X}\ominus \hat{\mathcal{X}})((\mathcal{X}\ominus \hat{\mathcal{X}}{)}^{\mathrm{T}}]\in {\mathbb{R}}^{3\times 3} \end{gathered}$$
​ 每个机器人运动中使用ESKF预测，
$$\begin{gathered} {\hat{\mathcal{X}}}_j={\hat{\mathcal{X}}}_i\oplus {\mathbf{u}}_j \\ {\mathbf{P}}_j={\mathbf{FP}}_i{\mathbf{F}}^{\mathrm{T}}+{\mathbf{GW}}_j{\mathbf{G}}^{\mathrm{T}} \end{gathered}$$
​ 其雅可比行列式
$$\begin{gathered} \mathbf{F}\triangleq {\mathbf{J}}_{{\mathcal{X}}_i}^{{\mathcal{X}}_j}={\mathbf{J}}_{{\hat{\mathcal{X}}}_i}^{{\hat{\mathcal{X}}}_j\oplus {\mathbf{u}}_j}={{\mathbf{Ad}}_{\mathrm{Exp}({\mathbf{u}}_j)}}^{-1} \\ \mathbf{G}\triangleq {\mathbf{J}}_{{\mathbf{u}}_j}^{{\mathcal{X}}_j}={\mathbf{J}}_{{\mathbf{u}}_j}^{{\hat{\mathcal{X}}}_i\oplus {\mathbf{u}}_j}={\mathbf{J}}_r({\mathbf{u}}_j) \end{gathered}$$
​ 在每次测量信标${\mathbf{y}}_k$时，用ESKF校正，其雅可比行列式
$$\mathbf{H}\triangleq {\mathbf{J}}_{\mathcal{X}}^{{\mathcal{X}}^{-1}\cdot {\mathbf{b}}_k}={\mathbf{J}}_{{\mathcal{X}}^{-1}}^{{\mathcal{X}}^{-1}\cdot {\mathbf{b}}_k}{\mathbf{J}}_{\mathcal{X}}^{{\mathcal{X}}^{-1}}=-\left[\begin{array}{cc}\mathbf{I}& {\mathbf{R}}^{\mathrm{T}}[1{]}_{\times }({\mathbf{b}}_k-\mathbf{t})\end{array}\right]$$

2. 使用基于图的优化进行平滑和映射

​ 目的：估计变量是信标位置和机器人轨迹

​ 假设轨迹由三个机器人姿态和三个信标组成复合状态，如上图
$$\mathcal{X}=⟨{\mathcal{X}}_1,{\mathcal{X}}_2,{\mathcal{X}}_3,{\mathbf{b}}_4,{\mathbf{b}}_5,{\mathbf{b}}_6⟩,{\mathcal{X}}_i\in SE(2),{\mathbf{b}}_k\in {\mathbb{R}}^2$$
​ 从位姿$i$到 $j$ 的运动测量，从位姿$i$ 到信标 $k$ 的测量对应于
$$\begin{gathered} {\mathbf{u}}_{ij}={\mathcal{X}}_j\ominus {\mathcal{X}}_i+{\mathbf{w}}_{ij}=\mathrm{Log}({\mathcal{X}}_i^{-1}{\mathcal{X}}_j)+{\mathbf{w}}_{ij} \\ {\mathbf{y}}_{ik}={\mathcal{X}}_i^{-1}\cdot {\mathbf{b}}_k+{\mathbf{n}}_{ik} \end{gathered}$$
​ 上面对应黑实线，下面对应灰实线。

3. 平滑与自标定的映射

​ 运动传感器受未知校准偏差$\mathbf{c}=(c_v,c_{\omega }{)}^{\mathrm{T}}$的影响，控制输入和复合状态改变
$$\begin{gathered} \tilde{\mathbf{u}}=(v\delta t+c_v,0,\omega \delta t+c_{\omega }{)}^{\mathrm{T}}+\mathbf{w} \\ \mathbf{u}=c(\tilde{\mathbf{u}},\mathbf{c})\triangleq \left[\begin{array}{c}{\tilde{u}}_v-c_v\\ {\tilde{u}}_s\\ {\tilde{u}}_{\omega }-c_{\omega }\end{array}\right]\in {\mathbb{R}}^3\cong \mathfrak{se}(2) \\ \mathcal{X}=⟨\mathbf{c},{\mathcal{X}}_1,{\mathcal{X}}_2,{\mathcal{X}}_3,{\mathbf{b}}_4,{\mathbf{b}}_5,{\mathbf{b}}_6⟩,{\mathcal{X}}_i\in SE(2);{\mathbf{b}}_k,\mathbf{c}\in {\mathbb{R}}^2 \end{gathered}$$
​ 导致运动残差改变，修改总雅可比矩阵。

​ 4. 3D场景下的实现

​ 定义变量在正确的空间中即可
$$\begin{gathered} \mathcal{X}\in SE(3) \\ \mathbf{u}\in {\mathbb{R}}^6\cong \mathfrak{se}(3) \\ {\mathbf{b}}_k,\mathbf{y}\in {\mathbb{R}}^3 \end{gathered}$$

6.Conclusion

1. 选择尽可能避免抽象数学概念的材料。
2. 选择了一种具有大量冗余的教学方法。
3. 推广了便捷运算符的使用。
4. 特别强调了雅可比行列式的定义、几何解释和计算。
5. 在附录中介绍了机器人技术中最常见组 的广泛公式纲要。
6. 提出了一些应用示例来说明李理论优雅而精确地解决机器人问题的能力。