旋量代数与李群李代数

阿睿睿2025-02-072025-02-11

三维空间刚体运动

刚体

在物理学里，理想刚体（英语：Rigid body或Rigid object）是一种有限尺寸，可以忽略形变的固体。不论是否感受到外力，在刚体内部，质点与质点之间的距离都不会改变。这种理想模型适用条件是，运动过程比固体中的弹性波的传播要缓慢得多。根据相对论，这种物体不可能实际存在，但物体通常可以假定为完美刚体，前提是必须满足运动速度远小于光速的条件。

在经典力学里，刚体通常被视为连续质量分布体；在量子力学里，刚体被视为一群粒子的聚集。例如，分子（由假定为质点的电子与核子组成）时常会被视为刚体。

刚体运动(PART1 旋转矩阵)

Preparation：

1.向量内外积(点乘与叉乘)

对于a,b两个向量，
内积表达式为

$\boldsymbol{a}\cdot\boldsymbol{b}=\boldsymbol{a}^\mathrm{T}\boldsymbol{b}=\sum_{i=1}^3a_ib_i=|\boldsymbol{a}||\boldsymbol{b}|\cos\left\langle\boldsymbol{a},\boldsymbol{b}\right\rangle$

外积表达式为

$a\times b= \begin{Vmatrix} e_1 & e_2 & e_3 \\ \\ a_1 & a_2 & a_3 \\ \\ b_1 & b_2 & b_3 \end{Vmatrix}= \begin{bmatrix} a_2b_3-a_3b_2 \\ \\ a_3b_1-a_1b_3 \\ \\ a_1b_2-a_2b_1 \end{bmatrix}= \begin{bmatrix} 0 & -a_3 & a_2 \\ \\ a_3 & 0 & -a_1 \\ \\ -a_2 & a_1 & 0 \end{bmatrix}b\overset{\mathrm{def}}{\operatorname*{=}}a^{\wedge}b$

$\langle a,b\rangle$是指a,b的夹角。内积可以看作b在a上的投影，外积代表a和b张成的平行四边形的面积。外积可以用反对称矩阵a^{\uparrow}乘b的形式。

Research 1：运动描述方式

1.1旋转矩阵

机器人学中世界坐标系与移动坐标系之间需要进行转换，我们需要一种数学手段来描述这个关系。刚体机器人两坐标系的变换在空间中通过平移和旋转进行变换，这种变换称为欧式变换。
欧式变换示意图

我们首先考虑旋转变换，设坐标系经过旋转变换后原坐标系基向量$(e_1, e_2, e_3)$转变成$(e_1’, e_2’, e_3’)$，通过向量不变，得出以下恒等关系式

$[\boldsymbol{e}_1,\boldsymbol{e}_2,\boldsymbol{e}_3] \begin{bmatrix} a_1 \\ \\ a_2 \\ \\ a_3 \end{bmatrix}=[\boldsymbol{e}_1^{\prime},\boldsymbol{e}_2^{\prime},\boldsymbol{e}_3^{\prime}] \begin{bmatrix} a_1^{\prime} \\ \\ a_2^{\prime} \\ \\ a_3^{\prime} \end{bmatrix}$

等式两边同时左乘$\left[\begin{array}{c} e_1^\top \ e_2^\top \ e_3^\top \end{array}\right]$，左边只剩下在变换前坐标系下的坐标向量

$\begin{bmatrix} a_1 \\ \\ a_2 \\ \\ a_3 \end{bmatrix}= \begin{bmatrix} e_1^\mathrm{T}e_1^{\prime} & e_1^\mathrm{T}e_2^{\prime} & e_1^\mathrm{T}e_3^{\prime} \\ e_2^\mathrm{T}e_1^{\prime} & e_2^\mathrm{T}e_2^{\prime} & e_2^\mathrm{T}e_3^{\prime} \\ e_3^\mathrm{T}e_1^{\prime} & e_3^\mathrm{T}e_2^{\prime} & e_3^\mathrm{T}e_3^{\prime} \end{bmatrix} \begin{bmatrix} a_1^{\prime} \\ \\ a_2^{\prime} \\ \\ a_3^{\prime} \end{bmatrix}\overset{\mathrm{def}}{\operatorname*{=}}Ra^{\prime}$

我们得到变换后坐标系下的坐标在变换前坐标系下的坐标，描述旋转的矩阵R被称为旋转矩阵。该矩阵各分量是两个坐标系的内积，由于单位矩阵相乘模为1，所以是各基向量夹角的余弦值，也被称为方向余弦矩阵。旋转矩阵的本质是一个行列式为1的正交矩阵。

下面我们同时考虑旋转和平移，一个比较直观的想法就是通过旋转矩阵和向量加法实现，但如下式所示，经过两次平移和旋转变换产生坐标向量c，但是这种处理方案容易造成计算繁琐。

$b=R_1a+t_1,\quad c=R_2b+t_2$
$c=R_2\left(R_1a+t_1\right)+t_2$

我们较为容易想到升维的方式将整个操作变为线性操作，在矩阵中就是扩充一维矩阵，将原始二维坐标扩充为三维齐次坐标，将旋转矩阵扩充为包含旋转和平移操作的2×2的变换矩阵。

$\begin{bmatrix} a^{\prime} \\ \\ 1 \end{bmatrix}= \begin{bmatrix} R & t \\ \\ \mathbf{0}^\mathrm{T} & 1 \end{bmatrix} \begin{bmatrix} a \\ \\ 1 \end{bmatrix}\overset{\mathrm{def}}{\operatorname*{=}}T \begin{bmatrix} a \\ \\ 1 \end{bmatrix}$

求解该矩阵的逆表示一个反向的变换，

$T^{-1}= \begin{bmatrix} R^\mathrm{T} & -R^\mathrm{T}t \\ \\ 0^\mathrm{T} & 1 \end{bmatrix}$

1.2旋转向量

在旋转矩阵中，我们使用4×4矩阵表示欧式变换，在这种情况下我们使用16个分量表示6自由度的运动，在计算机计算过程中容易增加内存开销。所以我们考虑用三维向量描述旋转。这里我们采用的方法是通过三维向量表达三维向量旋转所绕旋转轴的位置，通过向量的大小表达旋转的角度大小。从旋转向量到旋转矩阵的变换由罗德里格斯公式（Rodrigues’ Formula）表明，公式如下

$\boldsymbol{R}=\cos\theta\boldsymbol{I}+\left(1-\cos\theta\right)\boldsymbol{n}\boldsymbol{n}^\mathrm{T}+\sin\theta\boldsymbol{n}^\mathrm{\wedge}$

其中，n为转轴的单位方向向量，^是向量到反对称矩阵的转换符，通过等式两边取迹，我们也可以得到由旋转矩阵到旋转向量的变换

$\begin{aligned} \mathrm{tr}\left(R\right) & =\cos\theta\operatorname{tr}\left(\boldsymbol{I}\right)+\left(1-\cos\theta\right)\operatorname{tr}\left(\boldsymbol{n}\boldsymbol{n}^\mathrm{T}\right)+\sin\theta\operatorname{tr}(\boldsymbol{n}^\mathrm{\wedge}) \\ & =3\cos\theta+(1-\cos\theta) \\ & =1+2\cos\theta \end{aligned}$

$\theta=\arccos\frac{\mathrm{tr}(\boldsymbol{R})-1}{2}$

1.3四元数

所有采用三个实数表达旋转的方法都会不可避免地产生奇异性问题，即使原有系统丢失一个或几个自由度。在旋转向量中，当旋转0°或360°时，旋转轴可以任意选择，丢失三个自由度。

旋转这个概念在复数中也有出现，复数的乘法表示复平面上的旋转。在二维情况下，旋转可以使用单位复数来描述，在三维空间中，我们借助此概念使用单位四元数来描述。

一个四元数拥有一个实部和三个虚部。

$q=q_0+q_1\mathrm{i}+\mathrm{q}_2\mathrm{j}+\mathrm{q}_3\mathrm{k}$

其中，i、j、k满足以下关系

$\begin{cases} \mathrm{i^2=j^2=k^2=-1} \\ \mathrm{ij=k,ji=-k} \\ \mathrm{jk=i,kj=-i} \\ \mathrm{ki=j,ik=-j} & \end{cases}$

把i、j、k看成三个坐标轴，他们与自己的乘法和复数相同，相互之间的乘法与外积相同。

也可以用一个标量和一个向量表达四元数，如下
$\boldsymbol{q}=[s,\boldsymbol{v}]^\mathrm{T},\quad s=q_0\in\mathbb{R},\quad\boldsymbol{v}=[q_1,q_2,q_3]^\mathrm{T}\in\mathbb{R}^3$

其中，s称为四元数的实部，v称为四元数的虚部，只含s的四元数被称为实四元数，只含v的四元数被称为虚四元数。

其运算规则如下

1.加法和减法

四元数$q_a$,$q_b$的加减运算为
$\boldsymbol{q}_a\pm\boldsymbol{q}_b=\left[s_a\pm s_b,\boldsymbol{v}_a\pm\boldsymbol{v}_b\right]^\mathrm{T}$

2.乘法

乘法是将 $q_a$,$q_b$ 分别相乘再相加，

$\begin{aligned} q_{a}q_{b}= & s_as_b-x_ax_b-y_ay_b-z_az_b \\ & +(s_ax_b+x_as_b+y_az_b-z_ay_b)\mathrm{~i} \\ & +\left(s_ay_b-x_az_b+y_as_b+z_ax_b\right)\mathrm{j} \\ & +\left(s_az_b+x_ay_b-y_ax_b+z_as_b\right)\mathrm{k} \end{aligned}$

可以通过向量形式和向量的内外积表示

$\boldsymbol{q}_a\boldsymbol{q}_b= \begin{bmatrix} s_as_b-\boldsymbol{v}_a^\mathrm{T}\boldsymbol{v}_b,s_a\boldsymbol{v}_b+s_b\boldsymbol{v}_a+\boldsymbol{v}_a\times\boldsymbol{v}_b \end{bmatrix}^\mathrm{T}$

3.模长

$|q_a|=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$
模长满足乘积的模等于模的乘积
$|\boldsymbol{q}_a\boldsymbol{q}_b|=|\boldsymbol{q}_a||\boldsymbol{q}_b|$

4.共轭

$q_a^*=s_a-x_a\mathrm{i}-\mathrm{y}_a\mathrm{j}-\mathrm{z}_a\mathrm{k}=[\mathrm{s}_a,-\mathrm{v}_a]^\mathrm{T}$

共轭相乘等于模的平方

$q^*q=[s_a^2+v^\mathrm{T}v,0]^\mathrm{T}$

5.逆

$q^{-1}=q^*/|q|^2$
按照逆的定义逆与其自身相乘等于实四元数1

$qq^{-1}=q^{-1}q=1$

并且单位四元数的逆和共轭相同且有与矩阵相同的性质

$(\boldsymbol{q}_a\boldsymbol{q}_b)^{-1}=\boldsymbol{q}_b^{-1}\boldsymbol{q}_a^{-1}$

6.数乘

$\mathrm{k}\boldsymbol{q}=\left[\mathrm{k}s,\mathrm{k}\boldsymbol{v}\right]^\mathrm{T}$

我们通过四元数描述旋转，把三维空间点p用一个虚四元数描述，

$\boldsymbol{p}=[0,x,y,z]^\mathrm{T}=[0,\boldsymbol{v}]^\mathrm{T}$

旋转后的点p’可表示为，

$p^{\prime}=qpq^{-1}$

证明：

$\boldsymbol{q}=[s,\boldsymbol{v}]^\mathrm{T},\quad\boldsymbol{v}=[q_1,q_2,q_3]^\mathrm{T}\in\mathbb{R}^3$
$q^{-1}=[s,-\boldsymbol{v}]/|q|^2$

1.4其他变换

除了欧式变换，还存在其他几种变换方式。具体的总结如下表格

$\begin{array}{c c c c} \hline \textbf{转换名称} & \textbf{矩阵形式} & \textbf{自由度} & \textbf{不变性质} \\ \hline \text{欧氏变换} & \left[\begin{array}{cc} R & t \\ 0^T & 1 \end{array}\right] & 6 & \text{长度、夹角、体积} \\ \text{相似变换} & \left[\begin{array}{cc} sR & t \\ 0^T & 1 \end{array}\right] & 7 & \text{体积比} \\ \text{仿射变换} & \left[\begin{array}{cc} A & t \\ 0^T & 1 \end{array}\right] & 12 & \text{平行性、体积比} \\ \text{射影变换} & \left[\begin{array}{cc} A & t \\ a^T & v \end{array}\right] & 15 & \text{接触平面的相交和相切} \\ \hline \hline \end{array}$

刚体运动(PART2 李群与李代数)

Preparation：

1.群(Group)

群是一种集合加上一种运算的代数结构，$G=(A,\cdot)$代表在集合A上进行$\cdot$运算。群满足以下几个条件：

1.封闭性：

$\forall a_1,a_2\in A,\quad a_1\cdot a_2\in A$

2.结合律：

$\forall a_1,a_2,a_3\in A,\quad(a_1\cdot a_2)\cdot a_3=a_1\cdot(a_2\cdot a_3)$

3.幺元：

$\exists a_0\in A,\quad\mathrm{s.t.}\quad\forall a\in A,\quad a_0\cdot a=a\cdot a_0=a$

4.逆：

$\forall a\in A,\quad\exists a^{-1}\in A,\quad\mathrm{s.t.}\quad a\cdot a^{-1}=a_{0}$

常见的群有：

一般线性群$\mathrm{GL}(n)$ 指n×n的可逆矩阵，它们对矩阵乘法成群。
特殊正交群$\mathrm{SO}(n)$ 指旋转矩阵群，它们对矩阵乘法成群。
特殊欧式群$\mathrm{SE}(n)$ 指欧式变换矩阵群，它们对矩阵乘法成群。

李群是具有连续（光滑）性质的群。群结构保证了在群上的运算具有良好的性质。

2.李代数

李代数由一个集合$\mathbb{V}$、一个数域$\mathbb{F}$和一个二元运算$\left[,\right]$组成。李代数用符号$(\mathbb{V},\mathbb{F},[,])$表示。其满足以下性质。

1.封闭性：

$\forall\boldsymbol{X},\boldsymbol{Y}\in\mathbb{V},[\boldsymbol{X},\boldsymbol{Y}]\in\mathbb{V}$

2.双线性：

$\forall X,Y,Z\in\mathbb{V},a,b\in\mathbb{F}，$有
$[a\mathbf{X}+b\mathbf{Y},\boldsymbol{Z}]=a[\boldsymbol{X},\boldsymbol{Z}]+b[\boldsymbol{Y},\boldsymbol{Z}],\quad[\boldsymbol{Z},a\boldsymbol{X}+b\boldsymbol{Y}]=a[\boldsymbol{Z},\boldsymbol{X}]+b[\boldsymbol{Z},\boldsymbol{Y}]$

3.自反性：

$\forall X\in\mathbb{V},[X,X]=0$

4.雅可比等价：

$\forall X,Y,Z\in\mathbb{V},[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$

每个李群都有与之对应的李代数。李代数描述了单位元附近的正切空间。

3.矩阵求导

矩阵求导规则如下

$\begin{aligned} & \frac{\mathrm{d} \begin{bmatrix} a \\ b \end{bmatrix}}{\mathrm{d} \begin{bmatrix} x \\ y \end{bmatrix}}=\left(\frac{\mathrm{d}[a,b]^\mathrm{T}}{\mathrm{d} \begin{bmatrix} x \\ y \end{bmatrix}}\right)^\mathrm{T}= \begin{bmatrix} \frac{\mathrm{d}a}{\mathrm{d}x} & \frac{\mathrm{d}b}{\mathrm{d}x} \\ \frac{\mathrm{d}a}{\mathrm{d}y} & \frac{\mathrm{d}b}{\mathrm{d}y} \end{bmatrix}^\mathrm{T}= \begin{bmatrix} \frac{\mathrm{d}a}{\mathrm{d}x} & \frac{\mathrm{d}a}{\mathrm{d}y} \\ \frac{\mathrm{d}b}{\mathrm{d}x} & \frac{\mathrm{d}b}{\mathrm{d}y} \end{bmatrix} \end{aligned}$

Research 2：李群与李代数

SO(n)和SE(n)都在实数空间上是连续的，所以为具有连续（光滑）性质的李群。

我们知道SO(n)和SE(n)满足矩阵乘法，而对加法不封闭，所以我们难以定义微分的概念。我们需要引出李代数的概念，将李群的乘法转换为李代数的加法，从而实现李群的微分计算。

由于旋转矩阵为反对称矩阵，满足如下式子
$RR^{\mathrm{T}}=I$
在等式两边对时间求导

$\dot{\mathbf{R}}(t)=\boldsymbol{\phi}(t)^{\wedge}\mathbf{R}(t)= \begin{bmatrix} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{bmatrix}\mathbf{R}(t)$

可以看出$\dot{\boldsymbol{R}}(t)\boldsymbol{R}(t)^\mathrm{T}$是一个反对称矩阵，我们可以找到一个三维的向量$\phi(t)\in\mathbb{R}^3$与之对应，等式右乘${R}(t)$，可得到以下式子

$\dot{\boldsymbol{R}}(t)=\boldsymbol{\phi}(t)^\wedge\boldsymbol{R}(t)=\boldsymbol{\phi}^\wedge\boldsymbol{R}(t)$

由于$\phi(t)$反映了$R$的导数的性质，故称它为SO(3)求导点附近的正切空间。

我们求解微分方程，得到
$\boldsymbol{R}(t)=\exp{(\boldsymbol{\phi}^{\wedge}t)}$

下面我们通过李代数定义，实现上式的计算

已知$\phi(t)$是定义在$R^3$上的向量，我们在此集合上定义李括号为

$[\phi_1,\phi_2]=(\boldsymbol{\Phi}_1\boldsymbol{\Phi}_2-\boldsymbol{\Phi}_2\boldsymbol{\Phi}_1)^\vee$

验证：

由验证知，$\phi(t)$事实上是SO(3)的李代数$\mathfrak{so}(3)$，其定义如下

$\mathfrak{so}(3) ={\phi\in\mathbb{R}^3,\boldsymbol{\Phi}=\phi^{\wedge}\in\mathbb{R}^{3\times3}}$

SO(3)与$\mathfrak{so}(3)$的关系由指数映射给定

$R=\exp(\phi^{\wedge})$

我们通过泰勒展开定义其指数映射

$\exp(\phi^{\wedge})=\sum_{n=0}^{\infty}\frac{1}{n!}(\phi^{\wedge})^{n}$

该式无法直接计算，我们将三维向量用模长和方向向量表示$\phi=\theta\boldsymbol{a}$

我们发现方向向量具有如下的性质

$\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}= \begin{bmatrix} -a_2^2-a_3^2 & a_1a_2 & a_1a_3 \\ \\ a_1a_2 & -a_1^2-a_3^2 & a_2a_3 \\ \\ a_1a_3 & a_2a_3 & -a_1^2-a_2^2 \end{bmatrix}=\boldsymbol{a}\boldsymbol{a}^\mathrm{T}-\boldsymbol{I}$

$a^\wedge a^\wedge a^\wedge=a^\wedge(aa^\mathrm{T}-I)=-a^\wedge$

我们将指数映射泰勒级数展开
$\exp\left(\phi^\wedge\right)=\exp\left(\theta\boldsymbol{a}^\wedge\right)=\sum_{n=0}^\infty\frac{1}{n!}\left(\theta\boldsymbol{a}^\wedge\right)^n$

$\begin{aligned} & =\boldsymbol{I}+\theta\boldsymbol{a}^{\wedge}+\frac{1}{2!}\theta^{2}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}+\frac{1}{3!}\theta^{3}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}+\frac{1}{4!}\theta^{4}(\boldsymbol{a}^{\wedge})^{4}+\cdots \\ & =\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}+\theta\boldsymbol{a}^{\wedge}+\frac{1}{2!}\theta^{2}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}-\frac{1}{3!}\theta^{3}\boldsymbol{a}^{\wedge}-\frac{1}{4!}\theta^{4}(\boldsymbol{a}^{\wedge})^{2}+\cdots \\ & =\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}+\underbrace{\left(\theta-\frac{1}{3!}\theta^{3}+\frac{1}{5!}\theta^{5}-\cdots\right)}_{\sin\theta}\boldsymbol{a}^{\wedge}-\underbrace{\left(1-\frac{1}{2!}\theta^{2}+\frac{1}{4!}\theta^{4}-\cdots\right)}_{\cos\theta}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge} \\ & =a^\wedge a^\wedge+I+\sin\theta\boldsymbol{a}^\wedge-\cos\theta\boldsymbol{a}^\wedge\boldsymbol{a}^\wedge \\ & =(1-\cos\theta)\boldsymbol{a}^\wedge\boldsymbol{a}^\wedge+\boldsymbol{I}+\sin\theta\boldsymbol{a}^\wedge \\ & =\cos\theta\boldsymbol{I}+(1-\cos\theta)\boldsymbol{aa}^\mathrm{T}+\sin\theta\boldsymbol{a}^\mathrm{\wedge} \end{aligned}$

我们发现该式与罗德里格斯公式相同。这表明，$\mathfrak{so}(3)$就是旋转向量组成的空间，而指数映射就是罗德里格斯公式。我们也可以定义对数映射，将SO(3)的元素对应到$\mathfrak{so}(3)$中：

$\phi=\ln\left(\boldsymbol{R}\right)^\vee=\left(\sum_{n=0}^\infty\frac{\left(-1\right)^n}{n+1}\left(\boldsymbol{R}-\boldsymbol{I}\right)^{n+1}\right)^\vee$

该式泰勒展开计算过于繁琐，我们可以通过迹的性质反求转角和转轴简化计算。

需要注意的是指数映射是SO(3)到$\mathfrak{so}(3)$的满射，而不是单射。也就是说SO(3)中的元素都可以在$\mathfrak{so}(3)$中找到一个映射元素，但反之不可。

我们也可以理解为旋转角多转360°相当于没有旋转，它具有周期性。我们只需要将角度固定在$\pm\pi$之间即可保证一一对应。

回到一开始的问题，定义李代数的概念是为了对李群进行求导，由于李群对加法不封闭，我们将其映射到李代数上进行求导。两个指数映射乘积与李代数的关系由BCH公式给出：

$\ln\left(\exp\left(A\right)\exp\left(B\right)\right)=A+B+\frac{1}{2}\left[A,B\right]+\frac{1}{12}\left[A,\left[A,B\right]\right]-\frac{1}{12}\left[B,\left[A,B\right]\right]+\cdots$

BCH公式告诉我们，两个指数映射乘积并不能直接映射为两个李代数元素的加法，而是产生了由李括号组成的余项。特别地，考虑SO(3)上的李代数$\ln\left(\exp\left(\phi_1^\wedge\right)\exp\left(\phi_2^\wedge\right)\right)^\vee$，当$\phi_1$和$\phi_2$其中一个为小量时，小量二次以上的项都可以忽略。此时，BCH拥有近似线性表达。

$\ln\left(\exp\left(\phi_{1}^{\wedge}\right)\exp\left(\phi_{2}^{\wedge}\right)\right)^{\vee}\approx \begin{cases} J_{l}\left(\phi_{2}\right)^{-1}\phi_{1}+\phi_{2} & \text{当}\phi_{1}\text{为小量} \\ J_{r}\left(\phi_{1}\right)^{-1}\phi_{2}+\phi_{1} & \text{当}\phi_{2}\text{为小量} & \end{cases}$

在小量近似下，BCH公式被分为左乘和右乘小量两种形式。左乘近似雅可比和右乘近似雅可比如下

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

$\boldsymbol{J}_l^{-1}=\frac{\theta}{2}\cot\frac{\theta}{2}\boldsymbol{I}+\left(1-\frac{\theta}{2}\cot\frac{\theta}{2}\right)\boldsymbol{a}\boldsymbol{a}^\mathrm{T}-\frac{\theta}{2}\boldsymbol{a}^\mathrm{\wedge}$

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

通过近似我们将李群中元素和小量的乘法，映射到李代数中元素和带近似雅可比矩阵小量的加法，于是我们可以定义李群的导数。

第一种定义是直接利用李代数加法求导，

$\begin{aligned} \frac{\partial\left(\exp\left(\phi^{\wedge}\right)\boldsymbol{p}\right)}{\partial\phi} & =\lim_{\delta\phi\to0}\frac{\exp\left(\left(\phi+\delta\phi\right)^{\wedge}\right)p-\exp\left(\phi^{\wedge}\right)p}{\delta\phi} \\ & =\lim_{\delta\phi\to0}\frac{\exp\left(\left(\boldsymbol{J}_l\delta\boldsymbol{\phi}\right)^\wedge\right)\exp\left(\boldsymbol{\phi}^\wedge\right)\boldsymbol{p}-\exp\left(\boldsymbol{\phi}^\wedge\right)\boldsymbol{p}}{\delta\phi} \\ & =\lim_{\delta\phi\to0}\frac{\left(\boldsymbol{I}+\left(\boldsymbol{J}_l\delta\boldsymbol{\phi}\right)^\wedge\right)\exp\left(\boldsymbol{\phi}^\wedge\right)\boldsymbol{p}-\exp\left(\boldsymbol{\phi}^\wedge\right)\boldsymbol{p}}{\delta\phi} \\ & =\lim_{\delta\boldsymbol{\phi}\to\mathbf{0}}\frac{\left(\boldsymbol{J}_l\delta\boldsymbol{\phi}\right)^\wedge\exp\left(\boldsymbol{\phi}^\wedge\right)\boldsymbol{p}}{\delta\boldsymbol{\phi}} \\ & =\lim_{\delta\boldsymbol{\phi}\to\mathbf{0}}\frac{-(\exp\left(\phi^{\wedge}\right)\boldsymbol{p})^{\wedge}\boldsymbol{J}_{l}\delta\phi}{\delta\phi}=-(\boldsymbol{R}\boldsymbol{p})^{\wedge}\boldsymbol{J}_{l}. \end{aligned}$

第二种定义是对$R$进行一次扰动$\Delta R$，看结果相对于扰动的变化率。扰动有左乘和右乘两种方式，我们首先看左扰动

$\begin{aligned} \frac{\partial\left(Rp\right)}{\partial\varphi} & =\lim_{\varphi\to0}\frac{\exp\left(\varphi^{\wedge}\right)\exp\left(\phi^{\wedge}\right)p-\exp\left(\phi^{\wedge}\right)p}{\varphi} \\ & =\lim_{\varphi\to0}\frac{\left(\boldsymbol{I}+\boldsymbol{\varphi}^\wedge\right)\exp\left(\boldsymbol{\phi}^\wedge\right)\boldsymbol{p}-\exp\left(\boldsymbol{\phi}^\wedge\right)\boldsymbol{p}}{\varphi} \\ & =\lim_{\varphi\to0}\frac{\varphi^\wedge Rp}{\varphi}=\lim_{\varphi\to0}\frac{-(\boldsymbol{Rp})^\wedge\varphi}{\varphi}=-(\boldsymbol{Rp})^\wedge \end{aligned}$

我们一般使用扰动模型，因为省去了雅可比近似矩阵的计算。

继续分析SE(3)上的指数映射和李代数定义。

SE(3)对应的李代数定义

$\mathfrak{sc}(3)=\left\{\boldsymbol{\xi}= \begin{bmatrix} \rho \\ \phi \end{bmatrix}\in\mathbb{R}^6,\boldsymbol{\rho}\in\mathbb{R}^3,\boldsymbol{\phi}\in\mathfrak{so}(3),\boldsymbol{\xi}^\wedge= \begin{bmatrix} \phi^\wedge & \rho \\ \mathbf{0}^\mathrm{T} & 0 \end{bmatrix}\in\mathbb{R}^{4\times4}\right\}$

SE(3)上的李括号定义为

$[\xi_1,\xi_2]=(\xi_1^\wedge\xi_2^\wedge-\xi_2^\wedge\xi_1^\wedge)^\vee$

SE(3)上的指数映射为

$\begin{aligned} \exp\left(\xi^{\wedge}\right) & = \begin{bmatrix} \sum_{n=0}^{\infty}\frac{1}{n!}{\left(\phi^{\wedge}\right)}^{n} & \sum_{n=0}^{\infty}\frac{1}{\left(n+1\right)!}{\left(\phi^{\wedge}\right)}^{n}\boldsymbol{\rho} \\ \\ \mathbf{0}^{\mathrm{T}} & 1 \end{bmatrix} \\ & \overset{\Delta}{\operatorname*{=}} \begin{bmatrix} R & J\rho \\ \\ \mathbf{0}^\mathrm{T} & 1 \end{bmatrix}=T. \end{aligned}$

指数映射矩阵11处即SO(3)的指数映射，矩阵12处计算如下

$\begin{aligned} \sum_{n=0}^{\infty}\frac{1}{(n+1)!}(\phi^{\wedge})^{n} & =\boldsymbol{I}+\frac{1}{2!}\theta\boldsymbol{a}^{\wedge}+\frac{1}{3!}\theta^{2}{(\boldsymbol{a}^{\wedge})}^{2}+\frac{1}{4!}\theta^{3}{(\boldsymbol{a}^{\wedge})}^{3}+\frac{1}{5!}\theta^{4}{(\boldsymbol{a}^{\wedge})}^{4}\cdots \\ & =\frac{1}{\theta}\left(\frac{1}{2!}\theta^{2}-\frac{1}{4!}\theta^{4}+\cdots\right)(\boldsymbol{a}^{\wedge})+\frac{1}{\theta}\left(\frac{1}{3!}\theta^{3}-\frac{1}{5}\theta^{5}+\cdots\right)\left(\boldsymbol{a}^{\wedge}\right)^{2}+\boldsymbol{I} \\ & =\frac{1}{\theta}\left(1-\cos\theta\right)\left(\boldsymbol{a}^\wedge\right)+\frac{\theta-\sin\theta}{\theta}\left(\boldsymbol{a}\boldsymbol{a}^\mathrm{T}-\boldsymbol{I}\right)+\boldsymbol{I} \\ & =\frac{\sin\theta}{\theta}\boldsymbol{I}+\left(1-\frac{\sin\theta}{\theta}\right)\boldsymbol{a}\boldsymbol{a}^\mathrm{T}+\frac{1-\cos\theta}{\theta}\boldsymbol{a}^\mathrm{\wedge}\overset{\mathrm{def}}{\operatorname*{=}}\boldsymbol{J}. \end{aligned}$

11处可以由旋转向量方便求得，12处可以由以下的线性方程

$t=J\rho$

接着我们定义SE(3)的扰动模型如下

$\frac{\partial\left(\boldsymbol{T}\boldsymbol{p}\right)}{\partial\delta\boldsymbol{\xi}}=\lim_{\delta\boldsymbol{\xi}\to\mathbf{0}}\frac{\exp\left(\delta\boldsymbol{\xi}^{\wedge}\right)\exp\left(\boldsymbol{\xi}^{\wedge}\right)\boldsymbol{p}-\exp\left(\boldsymbol{\xi}^{\wedge}\right)\boldsymbol{p}}{\delta\boldsymbol{\xi}}$ $\begin{aligned} & =\lim_{\delta\boldsymbol{\xi}\to\mathbf{0}}\frac{\left(\boldsymbol{I}+\delta\boldsymbol{\xi}^{\wedge}\right)\exp\left(\boldsymbol{\xi}^{\wedge}\right)\boldsymbol{p}-\exp\left(\boldsymbol{\xi}^{\wedge}\right)\boldsymbol{p}}{\delta\boldsymbol{\xi}} \\ & =\lim_{\delta\boldsymbol{\xi}\to\mathbf{0}}\frac{\delta\boldsymbol{\xi}^{\wedge}\exp\left(\boldsymbol{\xi}^{\wedge}\right)\boldsymbol{p}}{\delta\boldsymbol{\xi}} \\ & =\lim_{\delta\boldsymbol{\xi}\to\mathbf{0}}\frac{ \begin{bmatrix} \delta\boldsymbol{\phi}^\wedge & \delta\boldsymbol{\rho} \\ \mathbf{0}^\mathrm{T} & 0 \end{bmatrix} \begin{bmatrix} \boldsymbol{R}\boldsymbol{p}+\boldsymbol{t} \\ \\ 1 \end{bmatrix}}{\delta\boldsymbol{\xi}} \\ & =\lim_{\delta\boldsymbol{\xi}\to\mathbf{0}}\frac{ \begin{bmatrix} \delta\phi^\wedge\left(\boldsymbol{Rp}+\boldsymbol{t}\right)+\delta\boldsymbol{\rho} \\ \mathbf{0}^\mathrm{T} \end{bmatrix}}{[\delta\boldsymbol{\rho},\delta\boldsymbol{\phi}]^\mathrm{T}}= \begin{bmatrix} \boldsymbol{I} & -\left(\boldsymbol{Rp}+\boldsymbol{t}\right)^\wedge \\ \mathbf{0}^\mathrm{T} & \mathbf{0}^\mathrm{T} \end{bmatrix}\overset{\mathrm{def}}{\operatorname*{=}}\left(\boldsymbol{Tp}\right)^\odot \end{aligned}$