旋转矩阵、变换矩阵，李群（Lie Group）、李代数（Lie Algebra）及扰动模型

0 前言

为什么需要李群&李代数？

• 在处理空间变换相关优化问题时，变换矩阵对于加法计算不封闭（任意两个变换矩阵相加后不是一个变换矩阵），这主要是由于旋转矩阵对加法计算不封闭造成的。李代数的出现即可解决该问题，我们把空间变换矩阵（$SE(3)$）映射到由向量组成的李代数（$se(3)$）空间中，就可以通过对向量（李代数$se(3)$）求导来间接实现对变换矩阵的求导，从而用来解决一些空间变换相关的优化问题。

1 李群（Lie Group）

1.1 群（Group）的定义

群是一种集合加上一种运算的代数结构。若集合$A\ne \varnothing$，在$A$上的二元运算为$\cdot$，则群可以记作$G=(A,\cdot )$。群要求这个运算满足以下条件：

• 封闭性：$\forall a,b\in A,a\cdot b\in A$；
• 结合律：$\forall a,b,c\in A,(a\cdot b)\cdot c=a\cdot (b\cdot c)$；
• 幺元/单位元：$\exists e\in A,s.t.\forall a\in A,e\cdot a=a\cdot e=a$；
• 逆元：$\forall a\in A,\exists a^{-1}\in A,s.t.a\cdot a^{-1}=e$；

1.2 一些特殊的群 - $SO(3)\&SE(3)$

（1）旋转矩阵 - 特殊正交群 $SO(n)$

旋转矩阵（定义为$R$）描述了旋转前后同一个向量的坐标变换关系。旋转矩阵是一个行列式为1的正交矩阵，反之，行列式为1的正交矩阵也是一个旋转矩阵。我们把旋转矩阵的集合称为特殊正交群（Special Orthogonal Group），定义为;

S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } (1-1) SO(n) = \{ R \in \mathbb{R}^{n \times n} | RR^T=I, det(R)=1 \} \tag{1-1} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}(1-1)

旋转矩阵还有一些正交矩阵的特殊性质，如$R^{-1}=R^T$等，旋转矩阵的逆或转置描述了一个相反的旋转。

（2）变换矩阵 - 特殊欧式群 $SE(n)$

在欧式变换中，变换由一个旋转矩阵$R$和平移向量$t$来描述，可以把它们组合成一个矩阵，使整个变换关系变成一个线性关系。我们将组合后的矩阵记为$T$，$T$被称为变换矩阵（Transform Matrix），表示为：

$$T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(1-2)}$$

所有变换矩阵$T$的集合被称为特殊欧式群（Special Euclidean Group），定义为：

S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } (1-3) SE(3)=\{ T= \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} | R \in SO(3), t \in \mathbb{R}^3 \} \tag{1-3} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}(1-3)

变换矩阵$T$的逆可以表示一个反向的变换：

$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(1-4)}$$

1.3 李群（Lie Group）

李群是指具有连续（光滑）性质的群，是群也是流形。刚体在空间中的运动是连续的，用于描述该运动的$SO(3)$和$SE(3)$都是李群；整数群$\mathbb{Z}$由于没有连续性质（是离散的），因此不是李群。

2 李代数（Lie Algebra）

2.1 基本定义

李代数是李群单位元处的正切空间，描述了李群的局部性质。每个李群都有对应的李代数，通常用小写的字母表示与李群相对应的李代数，如李群$SO(3)$对应的李代数为$so(3)$。

李代数由一个集合$\mathbb{V}$、一个数域$\mathbb{F}$和一个二元运算$[,]$（称为李括号）组成，如果它们满足以下性质，则称$(\mathbb{V},\mathbb{F},[,])$为一个李代数，记作$g$：

• 封闭性： $\forall X,Y\in \mathbb{V},[X,Y]\in \mathbb{V}$
• 双线性： $\forall X,Y,Z\in \mathbb{V},a,b\in \mathbb{F}$,有：
  $[aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]$
• 自反性： $\forall X\in \mathbb{V},[X,X]=0$
• 雅可比等价： $\forall X,Y,Z\in \mathbb{V},[X,[Y,Z]]+[Z,[Y,X]]+[Y,[Z,X]]=0$

例如，三维向量${\mathbb{R}}^3$上定义的叉积$\times$（$|\vec{a}\times \vec{b}|=|\vec{a}|\cdot |\vec{b}|\cdot \sin \theta$）是一种李括号，因此$g=({\mathbb{R}}^3,\mathbb{R},\times )$构成了一个李代数。

2.2 问题引入

下面由旋转矩阵推导其对应李代数。考虑任意旋转矩阵$R$满足$R{R}^T=I$，假设该旋转矩阵描述了空间中的连续运动（随时间$t$变化），则有：

$$R(t)R(t{)}^T=I\text{\hspace{1em}(2-1)}$$

等式两侧对时间求导可得：

$$\begin{gathered} \grave{R}(t)R(t{)}^T+R(t)\grave{R}(t{)}^T=0 \\ \Rightarrow \grave{R}(t)R(t{)}^T=-(\grave{R}(t)R(t{)}^T{)}^T\text{\hspace{1em}(2-2)} \end{gathered}$$

可以看出$\grave{R}(t)R(t{)}^T$是一个反对称矩阵。在继续推导之前，我们先定义符号$\wedge$表示向量到反对称矩阵的运算符，反之符号$\vee$表示反对称矩阵到向量的运算符，例如：

$$a^{\wedge }=A=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right],A^{\vee }=a\text{\hspace{1em}(2-3)}$$

则$\grave{R}(t)R(t{)}^T$可记作$\varphi (t{)}^{\wedge }$，即：

$$\grave{R}(t)R(t{)}^T=\varphi (t{)}^{\wedge }\text{\hspace{1em}(2-4)}$$

等式（2-4）两侧同时右乘$R(t)$，有：

$$\grave{R}(t)=\varphi (t{)}^{\wedge }R(t)=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]R(t)\text{\hspace{1em}(2-5)}$$

因此，对$R(t)$求导只需左乘一个${\varphi }^{\wedge }(t)$即可。

假设初始值$t_0=0,R(0)=I$，则$R(t)$在0附近的一阶泰勒展开为：

$$\begin{gathered} \grave{R}(t_0)=\varphi (t_0{)}^{\wedge }R(t_0)=\varphi (t_0{)}^{\wedge } \\ R(t)\approx R(t_0)+\grave{R}(t_0)(t-t_0)=I+\varphi (t_0{)}^{\wedge }(t)\text{\hspace{1em}(2-6)} \end{gathered}$$

可以看出，$\varphi$反映了$R$的导数性质，所以称它在$SO(3)$原点附近的正切空间（Tangent Space）上。在$t_0$附近，设$\varphi$保持为常数$\varphi (t_0)={\varphi }_0$，则根据公式（2-5）有：

$$\grave{R}(t)={\varphi }_0^{\wedge }R(t)\text{\hspace{1em}(2-7)}$$

上式是一个关于$R$的形如$y^{\prime }+ay=0$的微分方程，且初始值$R(0)=I$，则可以解得：

$$R(t)=exp({\varphi }_0^{\wedge }t)\text{\hspace{1em}(2-8)}$$

该式说明对于任意$t$都能找到一个$R$和一个$\varphi$的对应关系。换句话说，当我们知道某个时刻的$R$时，存在一个向量$\varphi$，它们满足这个矩阵指数关系，该关系称为指数映射（Exponential Map）。这个$\varphi$就是与$SO(3)$相对应的李代数$so(3)$，它描述了$R$在局部的导数关系。

2.3 $so(3)$

$SO(3)$对应的李代数$so(3)$是定义在${\mathbb{R}}^3$上的向量，记作$\varphi$，每个$\varphi$都可以生成一个反对称矩阵：

$$\Phi ={\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\in {\mathbb{R}}^{3\times 3}\text{\hspace{1em}(2-9)}$$

两个向量${\varphi }_1,{\varphi }_2$的李括号为：

$$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }\text{\hspace{1em}(2-10)}$$

可以说$so(3)$的元素是3维向量或3维反对称矩阵，或者说$so(3)$是一个由三维向量组成的集合，且每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。$so(3)$表示为：

s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } (2-11) so(3)=\{ \phi \in \R^3, \Phi = \phi^{\land} \in \R^{3\times3} \} \tag{2-11} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}(2-11)

$so(3)$与$SO(3)$的关系由指数映射给定：

$$R=exp({\varphi }^{\wedge })\text{\hspace{1em}(2-12)}$$

2.4 $se(3)$

与$SO(3)$ ~ $so(3)$类似，$SE(3)$对应的李代数$se(3)$位于${\mathbb{R}}^6$空间中，定义为：

s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } (2-13) se(3)=\{ \xi = \begin{bmatrix} \rho \\ \phi \end{bmatrix} \in \R^6, \rho \in \R^3, \phi \in so(3), \xi^{\land}=\begin{bmatrix} \phi^{\land} & \rho \\ 0^T & 0 \end{bmatrix} \in \R^{4\times4} \} \tag{2-13} se(3)={ξ=[ρϕ​]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0T​ρ0​]∈R4×4}(2-13)

把每个$se(3)$元素记作$\xi$，它是一个6维向量。前三维为平移，记作$\rho$；后三维为旋转，记作$\varphi$，实际上是$so(3)$元素。注意，这里的${}^{\wedge }$符号表示将一个六维向量转换成四维矩阵，但这里不再表示反对称。

可以简单地把$se(3)$理解为“由一个平移加上一个$so(3)$元素构成的向量”（这里的$\rho$不直接是平移）。$se(3)$也有类似于$so(3)$的李括号：

$$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }\text{\hspace{1em}(2-14)}$$

2.5 $SO(3)$与$SE(3)$上的指数映射、对数映射

$SO(3)/SE(3)$到$so(3)/se(3)$的映射被称为指数映射，也即罗德里格斯公式，都是通过泰勒公式来近似。有关指数映射的具体推导过程直接参考《视觉SLAM十四讲》。旋转矩阵的导数可以由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。

旋转的表示方法包括：旋转矩阵、旋转向量、欧拉角和四元数，而罗德里格斯变换就是表示从旋转向量到旋转矩阵的转换过程。

相应的对数映射可以看做时指数映射的反向映射，以书中一幅图来说明：

3 李代数求导与扰动模型

$SO(3)$上的（左）扰动模型求导如下：

参考

[1] 高翔.2017.视觉SLAM十四讲.电子工业出版社.400pp.
[2] 从零开始一起学习SLAM | 为啥需要李群与李代数？