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

灰太狼 2023-02-11 13:27 19阅读 0赞

### 文章目录 ###

*  0 前言
 *  1 李群（Lie Group）
 *   *  1.1 群（Group）的定义
     *  1.2 一些特殊的群 -  S O ( 3 ) & S E ( 3 ) SO(3) \\& SE(3) SO(3)&SE(3)
     *  1.3 李群（Lie Group）
 *  2 李代数（Lie Algebra）
 *   *  2.1 基本定义
     *  2.2 问题引入
     *  2.3  s o ( 3 ) so(3) so(3)
     *  2.4  s e ( 3 ) se(3) se(3)
     *  2.5  S O ( 3 ) SO(3) SO(3)与 S E ( 3 ) SE(3) SE(3)上的指数映射、对数映射
 *  3 李代数求导与扰动模型
 *   *  参考

# 0 前言 #

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

# 1 李群（Lie Group） #

## 1.1 群（Group）的定义 ##

> 群是**一种集合**加上**一种运算**的代数结构。若集合 A ≠ ∅ A\\not=\\varnothing A=∅，在 A A A上的二元运算为 ⋅ \\cdot ⋅，则群可以记作 G = ( A , ⋅ ) G=(A,\\cdot) G=(A,⋅)。群要求这个运算满足以下条件：
> 
>  *  **封闭性**： ∀ a , b ∈ A , a ⋅ b ∈ A \\forall a,b \\in A, a \\cdot b \\in A ∀a,b∈A,a⋅b∈A；
>  *  **结合律**： ∀ a , b , c ∈ A , ( a ⋅ b ) ⋅ c = a ⋅ ( b ⋅ c ) \\forall a,b,c \\in A, (a \\cdot b) \\cdot c = a \\cdot (b \\cdot c) ∀a,b,c∈A,(a⋅b)⋅c=a⋅(b⋅c)；
>  *  **幺元/单位元**： ∃ e ∈ A ,    s . t .    ∀ a ∈ A , e ⋅ a = a ⋅ e = a \\exist e \\in A, \\ \\ s.t. \\ \\ \\forall a \\in A, e \\cdot a = a \\cdot e = a ∃e∈A,  s.t.  ∀a∈A,e⋅a=a⋅e=a；
>  *  **逆元**： ∀ a ∈ A , ∃ a − 1 ∈ A ,    s . t .    a ⋅ a − 1 = e \\forall a \\in A, \\exist a^\{-1\} \\in A, \\ \\ s.t. \\ \\ a \\cdot a^\{-1\}=e ∀a∈A,∃a−1∈A,  s.t.  a⋅a−1=e；

## 1.2 一些特殊的群 -  S O ( 3 ) & S E ( 3 ) SO(3) \\& SE(3) SO(3)&SE(3) ##

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

> 旋转矩阵（定义为 R R 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 R^\{-1\}=R^T R−1=RT等，旋转矩阵的逆或转置描述了一个相反的旋转。

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

> 在欧式变换中，变换由一个旋转矩阵 R R R和平移向量 t t t来描述，可以把它们组合成一个矩阵，使整个变换关系变成一个线性关系。我们将组合后的矩阵记为 T T T， T T T被称为**变换矩阵（Transform Matrix）**，表示为：
> 
>  T = \[ R t 0 T 1 \] (1-2) T= \\begin\{bmatrix\} R & t \\\\ 0^T & 1 \\end\{bmatrix\} \\tag\{1-2\} T=\[R0Tt1\](1\-2)
> 
> 所有变换矩阵 T T 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=\[R0Tt1\]∈R4×4∣R∈SO(3),t∈R3\}(1\-3)
> 
> 变换矩阵 T T T的逆可以表示一个反向的变换：
> 
>  T − 1 = \[ R T − R T t 0 T 1 \] (1-4) T^\{-1\} = \\begin\{bmatrix\} R^T & -R^Tt \\\\ 0^T & 1 \\end\{bmatrix\} \\tag\{1-4\} T−1=\[RT0T−RTt1\](1\-4)

## 1.3 李群（Lie Group） ##

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

# 2 李代数（Lie Algebra） #

## 2.1 基本定义 ##

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

> 李代数由一个集合 V \\mathbb\{V\} V、一个数域 F \\mathbb\{F\} F和一个二元运算 \[ , \] \[,\] \[,\]（称为**李括号**）组成，如果它们满足以下性质，则称 ( V , F , \[ , \] ) (\\mathbb\{V\},\\mathbb\{F\},\[,\]) (V,F,\[,\])为一个李代数，记作 g \\mathbb\{g\} g：
> 
>  *  **封闭性：**  ∀ X , Y ∈ V , \[ X , Y \] ∈ V \\forall X,Y \\in \\mathbb\{V\}, \[X,Y\] \\in \\mathbb\{V\} ∀X,Y∈V,\[X,Y\]∈V
>  *  **双线性：**  ∀ X , Y , Z ∈ V , a , b ∈ F \\forall X,Y,Z \\in \\mathbb\{V\}, a,b \\in \\mathbb\{F\} ∀X,Y,Z∈V,a,b∈F,有：  
>      \[ a X + b Y , Z \] = a \[ X , Z \] + b \[ Y , Z \] , \[ Z , a X + b Y \] = a \[ Z , X \] + b \[ Z , Y \] \[aX+bY,Z\]=a\[X,Z\]+b\[Y,Z\], \[Z,aX+bY\]=a\[Z,X\]+b\[Z,Y\] \[aX\+bY,Z\]=a\[X,Z\]\+b\[Y,Z\],\[Z,aX\+bY\]=a\[Z,X\]\+b\[Z,Y\]
>  *  **自反性：**  ∀ X ∈ V , \[ X , X \] = 0 \\forall X \\in \\mathbb\{V\}, \[X,X\] = 0 ∀X∈V,\[X,X\]=0
>  *  **雅可比等价：**  ∀ X , Y , Z ∈ V , \[ X , \[ Y , Z \] \] + \[ Z , \[ Y , X \] \] + \[ Y , \[ Z , X \] \] = 0 \\forall X,Y,Z \\in \\mathbb\{V\}, \[X,\[Y,Z\]\]+\[Z,\[Y,X\]\]+\[Y,\[Z,X\]\]=0 ∀X,Y,Z∈V,\[X,\[Y,Z\]\]\+\[Z,\[Y,X\]\]\+\[Y,\[Z,X\]\]=0

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

## 2.2 问题引入 ##

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

R ( t ) R ( t ) T = I (2-1) R(t)R(t)^T=I \\tag\{2-1\} R(t)R(t)T=I(2\-1)

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

R ˋ ( t ) R ( t ) T + R ( t ) R ˋ ( t ) T = 0 ⇒ R ˋ ( t ) R ( t ) T = − ( R ˋ ( t ) R ( t ) T ) T (2-2) \\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 \\tag\{2-2\} Rˋ(t)R(t)T\+R(t)Rˋ(t)T=0⇒Rˋ(t)R(t)T=−(Rˋ(t)R(t)T)T(2\-2)

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

a ∧ = A = \[ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 \] , A ∨ = a (2-3) a^\{\\land\} = A = \\begin\{bmatrix\} 0 & -a\_3 & a\_2 \\\\ a\_3 & 0 & -a\_1 \\\\ -a\_2 & a\_1 & 0 \\end\{bmatrix\}, A^\{\\lor\}=a \\tag\{2-3\} a∧=A=⎣⎡0a3−a2−a30a1a2−a10⎦⎤,A∨=a(2\-3)

则 R ˋ ( t ) R ( t ) T \\grave\{R\}(t)R(t)^T Rˋ(t)R(t)T可记作 ϕ ( t ) ∧ \\phi(t)^\{\\land\} ϕ(t)∧，即：

R ˋ ( t ) R ( t ) T = ϕ ( t ) ∧ (2-4) \\grave\{R\}(t)R(t)^T = \\phi(t)^\{\\land\} \\tag\{2-4\} Rˋ(t)R(t)T=ϕ(t)∧(2\-4)

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

R ˋ ( t ) = ϕ ( t ) ∧ R ( t ) = \[ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 \] R ( t ) (2-5) \\grave\{R\}(t)=\\phi(t)^\{\\land\}R(t) = \\begin\{bmatrix\} 0 & -\\phi\_3 & \\phi\_2 \\\\ \\phi\_3 & 0 & -\\phi\_1 \\\\ -\\phi\_2 & \\phi\_1 & 0 \\end\{bmatrix\} R(t) \\tag\{2-5\} Rˋ(t)=ϕ(t)∧R(t)=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤R(t)(2\-5)

因此，对 R ( t ) R(t) R(t)求导只需左乘一个 ϕ ∧ ( t ) \\phi^\{\\land\}(t) ϕ∧(t)即可。

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

R ˋ ( t 0 ) = ϕ ( t 0 ) ∧ R ( t 0 ) = ϕ ( t 0 ) ∧ R ( t ) ≈ R ( t 0 ) + R ˋ ( t 0 ) ( t − t 0 ) = I + ϕ ( t 0 ) ∧ ( t ) (2-6) \\grave\{R\}(t\_0)=\\phi(t\_0)^\{\\land\}R(t\_0)=\\phi(t\_0)^\{\\land\} \\\\ R(t) \\approx R(t\_0)+\\grave\{R\}(t\_0)(t-t\_0)=I+\\phi(t\_0)^\{\\land\}(t) \\tag\{2-6\} Rˋ(t0)=ϕ(t0)∧R(t0)=ϕ(t0)∧R(t)≈R(t0)\+Rˋ(t0)(t−t0)=I\+ϕ(t0)∧(t)(2\-6)

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

R ˋ ( t ) = ϕ 0 ∧ R ( t ) (2-7) \\grave\{R\}(t)=\\phi\_0^\{\\land\}R(t) \\tag\{2-7\} Rˋ(t)=ϕ0∧R(t)(2\-7)

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

R ( t ) = e x p ( ϕ 0 ∧ t ) (2-8) R(t)=exp(\\phi\_0^\{\\land\}t) \\tag\{2-8\} R(t)=exp(ϕ0∧t)(2\-8)

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

## 2.3  s o ( 3 ) so(3) so(3) ##

S O ( 3 ) SO(3) SO(3)对应的李代数 s o ( 3 ) so(3) so(3)是定义在 R 3 \\R^3 R3上的向量，记作 ϕ \\phi ϕ，每个 ϕ \\phi ϕ都可以生成一个反对称矩阵：

Φ = ϕ ∧ = \[ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 \] ∈ R 3 × 3 (2-9) \\Phi=\\phi^\{\\land\}= \\begin\{bmatrix\} 0 & -\\phi\_3 & \\phi\_2 \\\\ \\phi\_3 & 0 & -\\phi\_1 \\\\ -\\phi\_2 & \\phi\_1 & 0 \\end\{bmatrix\} \\in \\R ^\{3\\times3\} \\tag\{2-9\} Φ=ϕ∧=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤∈R3×3(2\-9)

两个向量 ϕ 1 , ϕ 2 \\phi\_1,\\phi\_2 ϕ1,ϕ2的李括号为：

\[ ϕ 1 , ϕ 2 \] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ (2-10) \[\\phi\_1,\\phi\_2\]=(\\Phi\_1\\Phi\_2-\\Phi\_2\\Phi\_1)^\{\\lor\} \\tag\{2-10\} \[ϕ1,ϕ2\]=(Φ1Φ2−Φ2Φ1)∨(2\-10)

可以说 s o ( 3 ) so(3) so(3)的元素是3维向量或3维反对称矩阵，或者说 s o ( 3 ) so(3) so(3)是一个由三维向量组成的集合，且每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。 s o ( 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)

s o ( 3 ) so(3) so(3)与 S O ( 3 ) SO(3) SO(3)的关系由指数映射给定：

R = e x p ( ϕ ∧ ) (2-12) R=exp(\\phi^\{\\land\}) \\tag\{2-12\} R=exp(ϕ∧)(2\-12)

## 2.4  s e ( 3 ) se(3) se(3) ##

与 S O ( 3 ) SO(3) SO(3) ~  s o ( 3 ) so(3) so(3)类似， S E ( 3 ) SE(3) SE(3)对应的李代数 s e ( 3 ) se(3) se(3)位于 R 6 \\R^6 R6空间中，定义为：

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)

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

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

\[ ξ 1 , ξ 2 \] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ (2-14) \[\\xi\_1,\\xi\_2\]=(\\xi\_1^\{\\land\}\\xi\_2^\{\\land\}-\\xi\_2^\{\\land\}\\xi\_1^\{\\land\})^\{\\lor\} \\tag\{2-14\} \[ξ1,ξ2\]=(ξ1∧ξ2∧−ξ2∧ξ1∧)∨(2\-14)

## 2.5  S O ( 3 ) SO(3) SO(3)与 S E ( 3 ) SE(3) SE(3)上的指数映射、对数映射 ##

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

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

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

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NmYW45Mjc_size_16_color_FFFFFF_t_70_pic_center]

# 3 李代数求导与扰动模型 #

S O ( 3 ) SO(3) SO(3)上的（左）扰动模型求导如下：  
![在这里插入图片描述][20210102155055908.png_pic_center]

## 参考 ##

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

[Link 1]: https://blog.csdn.net/cfan927/article/details/104342592
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NmYW45Mjc_size_16_color_FFFFFF_t_70_pic_center]: https://img-blog.csdnimg.cn/20201231102027997.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NmYW45Mjc=,size_16,color_FFFFFF,t_70#pic_center
[20210102155055908.png_pic_center]: https://img-blog.csdnimg.cn/20210102155055908.png#pic_center
[.2017._SLAM_._.400pp.]: https://book.douban.com/subject/27028215/
[SLAM _]: https://www.sohu.com/a/270402234_100007727