詳解SLAM中的李群和李代數(shù)（中）

詳解介紹了SLAM中的李群和李代數(shù)的相關(guān)的知識，并對其中的公式進行了詳細的推導(dǎo)。

1 概述

在上一篇文章《詳解SLAM中的李群和李代數(shù)（上）》中，我們已經(jīng)通過對李群求導(dǎo)引出了李代數(shù)。在這篇文章中，我們就系統(tǒng)總結(jié)一下李代數(shù)的相關(guān)知識。

2 李代數(shù)

2.1 定義

李代數(shù)是一個向量空間\(\mathfrak{g}\)與一個二元運算的組合。其中的二元運算稱為李括號（Lie bracket），記為\([\cdot,\cdot]: \mathfrak{g} \times \mathfrak{g} \rightarrow \mathfrak{g}\)。在這個公式中：

\([\cdot,\cdot]\)是一個二元運算符，表示兩個元素之間的某種“乘法”操作。當然它不是普通的乘法，常見的運算操作是向量叉乘 \(\times\) 或矩陣的換位子\([A,B] = AB - BA\)。
\(\mathfrak{g}\) 是李代數(shù)的集合（通常是一個向量空間），\(\mathfrak{g} \times\mathfrak{g}\) 就是說我們要從這個集合里取兩個元素做運算，\(\rightarrow \mathfrak{g}\)表示運算的結(jié)果仍然屬于同一個集合 \(\mathfrak{g}\)。也就是說李括號運算把兩個李代數(shù)中的元素變成另一個李代數(shù)中的元素。其實就是封閉性的意思。

李代數(shù)需要滿足以下三個公理：

雙線性：對于所有\(x, y, z \in \mathfrak{g}\)和所有標量\(a, b\)，有：

\[[ax + by, z] = a[x, z] + b[y, z],\quad [z, ax + by] = a[z, x] + b[z, y] \]
反對稱性：對于所有\(x, y \in \mathfrak{g}\)，有：

\[[x, y] = -[y, x] \]
特別地，這意味著\([x,x]=0\)。
雅可比恒等式：對于所有\(x, y, z \in \mathfrak{g}\)，有：

\[[x,[y,z]] + [y,[z,x]] + [z,[x,y]] = 0 \]

如果是第一次接觸到李代數(shù)這個概念，這三個特性公理并不太好理解，因為缺少實際的使用場景。但是這里還是舉一個例子來加深印象。例如，三維向量空間\(\mathbb{R}^3\)和其上定義的叉乘\(\times\)構(gòu)成了一個李代數(shù)\((\mathbb{R}^3, \times)\)，李括號為\([\boldsymbol{u}, \boldsymbol{v}] = \boldsymbol{u} \times \boldsymbol{v}\)。它符合李代數(shù)的三個公理：

雙線性：假設(shè)有向量\(\boldsymbol{u} = (1, 0, 0)\)，\(\boldsymbol{v} = (0, 1, 0)\)，\(\boldsymbol{w} = (0, 0, 1)\)，以及標量\(a=2\)，\(b=3\)。
- 計算\([a\boldsymbol{u} + b\boldsymbol{v}, \boldsymbol{w}]\)：
\[[(2, 0, 0) + (0, 3, 0), (0, 0, 1)] = [(2, 3, 0), (0, 0, 1)] = (3, -2, 0) \]
- 計算\(a[\boldsymbol{u}, \boldsymbol{w}] + b[\boldsymbol{v}, \boldsymbol{w}]\)：
\[2[(1, 0, 0), (0, 0, 1)] + 3[(0, 1, 0), (0, 0, 1)] = 2(0, 1, 0) + 3(-1, 0, 0) = (3, -2, 0) \]
兩式結(jié)果相等，符合雙線性公理。
反對稱性：假設(shè)\(\boldsymbol{u} = (1, 0, 0)\)和\(\boldsymbol{v} = (0, 1, 0)\)。
- 計算\([\boldsymbol{u}, \boldsymbol{v}]\)：
  \[(1, 0, 0) \times (0, 1, 0) = (0, 0, 1) \]
- 計算 \([\boldsymbol{v}, \boldsymbol{u}]\)：
  \[(0, 1, 0) \times (1, 0, 0) = (0, 0, -1) \]
可以看到\([\boldsymbol{u}, \boldsymbol{v}] = -( [\boldsymbol{v}, \boldsymbol{u}] )\)，這符合反對稱性。
雅可比恒等式：假設(shè)\(\boldsymbol{u} = (1, 0, 0)\), \(\boldsymbol{v} = (0, 1, 0)\), \(\boldsymbol{w} = (0, 0, 1)\)。
- 計算\([\boldsymbol{u}, [\boldsymbol{v}, \boldsymbol{w}]]\)：
  \[[\boldsymbol{u}, (0, 1, 0) \times (0, 0, 1)] = [(1, 0, 0), (1, 0, 0)] = (0, 0, 0) \]
- 計算 \([\boldsymbol{v}, [\boldsymbol{w}, \boldsymbol{u}]]\)：
  \[[\boldsymbol{v}, (0, 0, 1) \times (1, 0, 0)] = [(0, 1, 0), (0, 1, 0)] = (0, 0, 0) \]
- 計算 \([\boldsymbol{w}, [\boldsymbol{u}, \boldsymbol{v}]]\)：
  \[[\boldsymbol{w}, (1, 0, 0) \times (0, 1, 0)] = [(0, 0, 1), (0, 0, 1)] = (0, 0, 0) \]
因此，

\[[\boldsymbol{u}, [\boldsymbol{v}, \boldsymbol{w}]] + [\boldsymbol{v}, [\boldsymbol{w}, \boldsymbol{u}]] + [\boldsymbol{w}, [\boldsymbol{u}, \boldsymbol{v}]] = (0, 0, 0) + (0, 0, 0) + (0, 0, 0) = (0, 0, 0) \]
這驗證了雅可比恒等式成立。

注意，筆者這里對于李代數(shù)的定義與《視覺SLAM十四講》上的定義略有不同，但是表達的實際的含義是一樣的，讀者可以自行體會。

2.2 李代數(shù)\(\mathfrak{so}(3)\)

在上一篇文章《詳解SLAM中的李群和李代數(shù)（上）》中筆者實現(xiàn)了對特殊正交群\(SO(3)\)的求導(dǎo)：

\[R(t) = \exp([\boldsymbol{\omega}(t)]_\times t) \]

其中\(\exp\)表示矩陣指數(shù)運算，\(\boldsymbol{\omega}(t)\)是表達旋轉(zhuǎn)矩陣的旋轉(zhuǎn)向量，\([\boldsymbol{\omega}(t)]_{\times}\)是其對應(yīng)的反對稱矩陣。去掉用于求導(dǎo)的自變量\(t\)，那么可以定義\(SO(3)\)對應(yīng)的李代數(shù)是：

\[\mathfrak{so}(3) = \left\{ [\boldsymbol{\omega}]_\times \in \mathbb{R}^{3 \times 3} \mid {\omega} \in \mathbb{R}^3 \right\} \]

也就是說，\(\mathfrak{so}(3)\)是所有實\(3\times 3\)反對稱矩陣構(gòu)成的集合，對應(yīng)的李括號是兩個元素之間的換位子（commutator）。

2.3 \(\mathfrak{so}(3)\)的李括號

這里的李括號也就是換位子運算具體定義是：對于任意\(X, Y \in \mathfrak{so}(3)\)，有：

\[[X, Y] = XY - YX \]

例如，在\(\mathfrak{so}(3)\)中，如果\(X = [\boldsymbol{\omega}_1]_\times\)和\(Y = [\boldsymbol{\omega}_2]_\times\)，則：

\[[[\boldsymbol{\omega}_1]_\times, [\boldsymbol{\omega}_2]_\times] = [\boldsymbol{\omega}_1]_\times [\boldsymbol{\omega}_2]_\times - [\boldsymbol{\omega}_2]_\times [\boldsymbol{\omega}_1]_\times \]

這個表達式的結(jié)果仍然是一個反對稱矩陣，并且它對應(yīng)于向量\(\boldsymbol{\omega}_1 \times \boldsymbol{\omega}_2\)的反對稱矩陣，即：

\[[[\boldsymbol{\omega}_1]_\times, [\boldsymbol{\omega}_2]_\times] = [\boldsymbol{\omega}_1 \times \boldsymbol{\omega}_2]_\times \]

這意味著在\(\mathfrak{so}(3)\)中，李括號運算等價于三維向量空間中的叉積運算，\(\mathfrak{so}(3)\)與上一小節(jié)中介紹的李代數(shù)\((\mathbb{R}^3, \times)\)的含義基本相同。

舉例驗證一下，假設(shè)有兩個向量\(\boldsymbol{\omega}_1 = (1, 0, 0)^T\)和\(\boldsymbol{\omega}_2 = (0, 1, 0)^T\)，它們對應(yīng)的反對稱矩陣分別是：

\[[\boldsymbol{\omega}_1]_\times = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{bmatrix}, \quad [\boldsymbol{\omega}_2]_\times = \begin{bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0 \end{bmatrix} \]

計算它們的李括號：

\[[[\boldsymbol{\omega}_1]_\times, [\boldsymbol{\omega}_2]_\times] = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0 \end{bmatrix} - \begin{bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0 \end{bmatrix} \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{bmatrix}\]

\[= \begin{bmatrix} 0 & 0 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & -1 \end{bmatrix} - \begin{bmatrix} 0 & 0 & -1 \\ 0 & 0 & 0 \\ 1 & 0 & 0 \end{bmatrix} = \begin{bmatrix} 0 & 0 & 1 \\ -1 & 0 & 0 \\ 0 & -1 & 0 \end{bmatrix}\]

這正是\([0, 0, 1]^T\)向量對應(yīng)的反對稱矩陣，也就是\(\boldsymbol{\omega}_1 \times \boldsymbol{\omega}_2 = (0, 0, 1)^T\)。

2.4 李代數(shù)\(\mathfrak{se}(3)\)

與引出特殊正交群\(SO(3)\)的李代數(shù)\(\mathfrak{so}(3)\)的推導(dǎo)一樣，也可以對特殊歐式群 \(SE(3)\)對時間\(t\)微分來引出對應(yīng)的李代數(shù)\(\mathfrak{se}(3)\)。假設(shè)一個剛體在三維空間隨時間做歐式運動（即剛體運動，同時包括旋轉(zhuǎn)和平移），那么歐式變換矩陣可以表示為：

\[T(t) = \begin{bmatrix} R(t) & \boldsymbol{p}(t) \\ \mathbf{0}^T & 1 \end{bmatrix} \in SE(3) \]

其中：

\(R(t) \in SO(3)\) 表示旋轉(zhuǎn)；
\(\boldsymbol{p}(t) \in \mathbb{R}^3\) 表示平移；
整個矩陣是一個\(4 \times 4\)的齊次變換矩陣。

對\(T(t)\)的自變量時間\(t\)進行微分，即：

\[\fracw0obha2h00{dt} T(t) = \fracw0obha2h00{dt} \begin{bmatrix} R(t) & \boldsymbol{p}(t) \\ \mathbf{0}^T & 1 \end{bmatrix}= \begin{bmatrix} \fracw0obha2h00{dt} R(t) & \fracw0obha2h00{dt} \boldsymbol{p}(t) \\ \mathbf{0}^T & 0 \end{bmatrix} \]

其中\(\fracw0obha2h00{dt} R(t) = [\boldsymbol{\omega}(t)]_\times R(t)\)，這一點我們在前面引出李代數(shù)\(\mathfrak{so}(3)\)的時候推導(dǎo)過。\(\boldsymbol{p}(t)\)表示位移，那么\(\boldsymbol{p}(t)\)對時間\(t\)求導(dǎo)，即\(\fracw0obha2h00{dt} \boldsymbol{p}(t) = \boldsymbol{v}(t)\)是瞬時線速度。

因此：

\[\fracw0obha2h00{dt} T(t) = \begin{bmatrix} [\boldsymbol{\omega}(t)]_\times R(t) & \boldsymbol{v}(t) \\ \mathbf{0}^T & 0 \end{bmatrix} \tag{1} \]

需要進一步說明的是，\(\boldsymbol{v}(t)\) 是空間坐標系下的線速度。我們知道速度是一種矢量，而歐式變換包含旋轉(zhuǎn)和平移兩種變換，那么空間坐標系下線速度\(\boldsymbol{v}(t)\)可以分解成：

由于旋轉(zhuǎn)引起的“附加速度”：即使沒有自身的平移，只要它在旋轉(zhuǎn)，它上面的點也會因為旋轉(zhuǎn)而產(chǎn)生一個速度。
由于自身平移產(chǎn)生的速度：這是物體本身在自己坐標系下的移動速度。

數(shù)學(xué)表達如下：

\[\boldsymbol{v}(t) = [\boldsymbol{\omega}(t)]_\times \boldsymbol{p}(t) + \boldsymbol{v}_{\text{body}}(t) \tag{2} \]

這個公式其實就是剛體運動學(xué)中的速度合成公式：

\(\boldsymbol{v}_{\text{body}}\)就是剛體自身平移產(chǎn)生的速度，即體坐標系下的線速度；
\(\boldsymbol{\omega}(t)\)是剛體的角速度，通常也在體坐標系下表示；
\(\boldsymbol{p}(t)\)表示相對于剛體原點的位置向量。
\([\boldsymbol{\omega}(t)]_\times \boldsymbol{p}(t)\)就是由于旋轉(zhuǎn)引起的附加速度，其是一種典型的圓周運動的速度：\(\boldsymbol{\omega}(t)\)方向垂直于圓周面，\(\boldsymbol{p}(t)\)定義了到旋轉(zhuǎn)中心的距離半徑，兩者叉乘，即得因剛體旋轉(zhuǎn)而具有的切向速度。

將式(2)代入式(1)，有：

\[\begin{align*} \fracw0obha2h00{dt} T(t) &= \begin{bmatrix} [\boldsymbol{\omega}(t)]_\times R(t) & [\boldsymbol{\omega}(t)]_\times \boldsymbol{p}(t) + \boldsymbol{v}_{\text{body}}(t) \\ \mathbf{0}^T & 0 \end{bmatrix} \\ &= \begin{bmatrix} [\boldsymbol{\omega}(t)]_\times & \boldsymbol{v}_{\text{body}}(t) \\ \mathbf{0}^T & 0 \end{bmatrix} \begin{bmatrix} R(t) & \boldsymbol{p}(t) \\ \mathbf{0}^T & 1 \end{bmatrix} \\ \end{align*} \]

如果令：

\[\xi(t) = \begin{bmatrix} [\boldsymbol{\omega}(t)]_\times & \boldsymbol{v}_{\text{body}}(t) \\ \mathbf{0}^T & 0 \end{bmatrix} \]

那么有：

\[\fracw0obha2h00{dt} T(t) = \xi(t) T(t) \]

顯然，在初始時刻歐式變換矩陣為單位矩陣，即\(T(0) = I_4\)，那么有微分方程：

\[\fracw0obha2h00{dt} T(t) = \xi(t) T(t), \quad T(0) = I_4 \]

這就又回到了上一篇文章《詳解SLAM中的李群和李代數(shù)（上）》中介紹的一階線性微分方程，其解為：

\[T(t) = \exp(\xi(t) t) \]

這就是從李代數(shù)\(\mathfrak{se}(3)\)到李群\(SE(3)\)的指數(shù)映射，矩陣\(\xi(t)\)就是\(SE(3)\)的李代數(shù)中集合的元素。

進一步的，去掉自變量\(t\)，將\(\boldsymbol{v}_{\text{body}}\)重命名為\(\boldsymbol{v}\)，李代數(shù) \(\mathfrak{se}(3)\) 是所有形如：

\[\xi = \begin{bmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ \mathbf{0}^T & 0 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \]

的矩陣構(gòu)成的集合，其中 \(\boldsymbol{\omega}, \boldsymbol{v} \in \mathbb{R}^3\)。

將其簡記為：

\[\mathfrak{se}(3) = \left\{ \begin{bmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ \mathbf{0}^T & 0 \end{bmatrix} \,\middle|\, \boldsymbol{\omega}, \boldsymbol{v} \in \mathbb{R}^3 \right\} \]

如果，\(\mathfrak{se}(3)\)也可以用一個六維向量來表示：

\[\boldsymbol{\eta} = \begin{bmatrix} \boldsymbol{\omega} \\ \boldsymbol{v} \end{bmatrix} \in \mathbb{R}^6 \]

并定義對應(yīng)的映射：

\[\hat{\boldsymbol{\eta}} = \begin{bmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ \mathbf{0}^T & 0 \end{bmatrix} \]

所以\(\mathfrak{se}(3)\)也可以表示為：

\[\mathfrak{se}(3) = \left\{ \hat{\boldsymbol{\eta}} \,\middle|\, \boldsymbol{\eta} \in \mathbb{R}^6 \right\} \]

2.5 \(\mathfrak{se}(3)\)的李括號

\(\mathfrak{se}(3)\) 中的李括號仍然是矩陣換位子。對于任意兩個 \(\xi_1, \xi_2 \in \mathfrak{se}(3)\)，有：

\[[\xi_1, \xi_2] = \xi_1 \xi_2 - \xi_2 \xi_1 \]

如果寫成向量形式 \(\boldsymbol{\eta}_1 = (\boldsymbol{\omega}_1, \boldsymbol{v}_1)^T\), \(\boldsymbol{\eta}_2 = (\boldsymbol{\omega}_2, \boldsymbol{v}_2)^T\)，則可以推出：

\[[\boldsymbol{\eta}_1, \boldsymbol{\eta}_2] = \begin{bmatrix} \boldsymbol{\omega}_1 \times \boldsymbol{\omega}_2 \\ \boldsymbol{\omega}_1 \times \boldsymbol{v}_2 - \boldsymbol{\omega}_2 \times \boldsymbol{v}_1 \end{bmatrix} \]

這是通過直接計算矩陣乘積和利用叉積與反對稱矩陣的關(guān)系推導(dǎo)出來的，估計也不是很常用，因此略掉推導(dǎo)過程。

3 指數(shù)映射

3.1 預(yù)備知識

3.1.1 泰勒公式

設(shè)一個函數(shù) \(f(x)\) 在點 \(x = a\) 處無窮可導(dǎo)，那么它可以展開為如下形式的泰勒級數(shù)：

\[f(x) = \sum_{n=0}^\infty \frac{f^{(n)}(a)}{n!}(x - a)^n \]

這個式子的意思是：用無窮多個多項式項來逼近原函數(shù) \(f(x)\)。

如果取 \(a = 0\)，則稱為麥克勞林級數(shù)（Maclaurin series）：

\[f(x) = \sum_{n=0}^\infty \frac{f^{(n)}(0)}{n!}x^n \]

根據(jù)這個定義，有：

\[e^x = \sum_{n=0}^\infty \frac{1}{n!}x^n = 1 + x + \frac{x^2}{2!} + \frac{x^3}{3!} + \cdots \\ \sin(x) = x - \frac{x^3}{3!} + \frac{x^5}{5!} - \frac{x^7}{7!} + \cdots = \sum_{n=0}^\infty (-1)^n \frac{x^{2n+1}}{(2n+1)!}\\ \cos(x) = 1 - \frac{x^2}{2!} + \frac{x^4}{4!} - \frac{x^6}{6!} + \cdots = \sum_{n=0}^\infty (-1)^n \frac{x^{2n}}{(2n)!} \]

3.1.2 反對稱矩陣的冪次規(guī)律

給定一個單位三維向量 \(\boldsymbol{\omega} = (\omega_1, \omega_2, \omega_3)^T\)，其對應(yīng)的反對稱矩陣 \([\boldsymbol{\omega}]_\times\) 為：

\[[\boldsymbol{\omega}]_\times = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} \]

假設(shè) \(A = [\boldsymbol{\omega}]_\times\)，我們首先計算 \(A^2\)：

\[A^2 = [\boldsymbol{\omega}]_\times^2 = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} \]

進行矩陣乘法運算后得到：

\[A^2 = \begin{bmatrix} -\omega_2^2 - \omega_3^2 & \omega_1 \omega_2 & \omega_1 \omega_3 \\ \omega_1 \omega_2 & -\omega_1^2 - \omega_3^2 & \omega_2 \omega_3 \\ \omega_1 \omega_3 & \omega_2 \omega_3 & -\omega_1^2 - \omega_2^2 \end{bmatrix} \]

因為這里\(\boldsymbol{\omega}\) 是單位向量（即 \(\omega_1^2 + \omega_2^2 + \omega_3^2 = 1\)），則：

\[A^2 = \begin{bmatrix} -(1 - \omega_1^2) & \omega_1 \omega_2 & \omega_1 \omega_3 \\ \omega_1 \omega_2 & -(1 - \omega_2^2) & \omega_2 \omega_3 \\ \omega_1 \omega_3 & \omega_2 \omega_3 & -(1 - \omega_3^2) \end{bmatrix} \]

進一步簡化為：

\[A^2 = \begin{bmatrix} -\omega_1^2 & \omega_1 \omega_2 & \omega_1 \omega_3 \\ \omega_1 \omega_2 & -\omega_2^2 & \omega_2 \omega_3 \\ \omega_1 \omega_3 & \omega_2 \omega_3 & -\omega_3^2 \end{bmatrix} = \boldsymbol{\omega} \boldsymbol{\omega}^T - \mathbf{I} \]

這里 \(\mathbf{I}\) 是單位矩陣，\(\boldsymbol{\omega} \boldsymbol{\omega}^T\) 是外積矩陣。

接下來計算 \(A^3\)：

\[A^3 = A \cdot A^2 = [\boldsymbol{\omega}]_\times \cdot (\boldsymbol{\omega} \boldsymbol{\omega}^T - \mathbf{I}) \]

注意到 \([\boldsymbol{\omega}]_\times \boldsymbol{\omega} = \mathbf{0}\) （因為反對稱矩陣作用于自身方向的向量結(jié)果為零向量），因此：

\[A^3 = -[\boldsymbol{\omega}]_\times + [\boldsymbol{\omega}]_\times (\boldsymbol{\omega} \boldsymbol{\omega}^T) = -[\boldsymbol{\omega}]_\times = -A \]

所以：

\[A^3 = -A \]

再來看計算\(A^4\)：

\[A^4 = A \cdot A^3 = A \cdot (-A) = -A^2 \]

根據(jù)前面的結(jié)果 \(A^2 = \boldsymbol{\omega} \boldsymbol{\omega}^T - \mathbf{I}\)，于是：

\[A^4 = -(\boldsymbol{\omega} \boldsymbol{\omega}^T - \mathbf{I}) = -A^2 \]

因此：

\[A^4 = -A^2 \]

最后計算 \(A^5\)：

\[A^5 = A \cdot A^4 = A \cdot (-A^2) = -A^3 = -(-A) = A \]

所以：

\[A^5 = A \]

通過上述推導(dǎo)，我們可以總結(jié)出反對稱矩陣 \(A = [\boldsymbol{\omega}]_\times\) 的一些重要冪次規(guī)律：

\(A = [\boldsymbol{\omega}]_\times\)
\(A^2 = \boldsymbol{\omega} \boldsymbol{\omega}^T-\mathbf{I}\)
\(A^3 = -A\)
\(A^4 = -A^2\)
\(A^5 = A\)
\(A^6 = A^2\)
\(A^7 = -A\)
\(A^8 = -A^2\)

這些規(guī)律表明，對于反對稱矩陣 \(A = [\boldsymbol{\omega}]_\times\)，其高次冪具有周期性，并且可以歸約為 \(A\)、\(A^2\) 和 \(-A\) 的線性組合。

3.1.3 Chasles定理

從\(SO(3)\)李代數(shù)的推導(dǎo)可以知道，任何旋轉(zhuǎn)變換可以等效為繞某個固定軸的旋轉(zhuǎn)來表示，那么歐式變換（剛體變換）是不是也有類似的性質(zhì)呢？確實如此，剛體運動學(xué)的Chasles定理指出：

任何一個剛體的位移都可以視為繞某條直線（稱為螺旋軸或瞬時轉(zhuǎn)動軸）的旋轉(zhuǎn)加上沿這條直線的平移。

換句話說，任何剛體的位移可以分解為繞一個特定方向的旋轉(zhuǎn)加上沿著這個方向的平移。這種組合運動被稱為螺旋運動，這條特定方向確定的直線就是螺旋軸。在剛體運動過程中，這條直線是保持不變的。

根據(jù)前面\(SE(3)\)李代數(shù)推導(dǎo)可知，歐式變換可以表示為：

\[T = \exp(\xi) \]

如同旋轉(zhuǎn)變換的旋轉(zhuǎn)向量可以分解成單位軸向量和旋轉(zhuǎn)角度一樣，螺旋運動也可以由以下兩個量來確定：

單位螺旋軸 \(\hat{s}\)：描述運動的方向（包括旋轉(zhuǎn)軸和平移方向）。
標量 \(\theta\)：描述沿這個螺旋軸運動了多遠。

即：

\[\xi = \hat{s} \theta \]

于是整個運動就可以寫成：

\[T = \exp(\hat{s} \theta) \]

又因為：

\[\xi = \begin{bmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ \mathbf{0}^T & 0 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \]

對這個矩陣提取\(\theta\)，很顯然：

\([\boldsymbol{\omega}]_\times = \theta \cdot [\boldsymbol{\omega}_{\text{unit}}]_\times\)
\(\mathbf{0}^T = \theta \cdot \mathbf{0}^T\)
\(0 = \theta \cdot 0\)

那么必然存在單位螺旋軸 \(\hat{s}\) 的矩陣：

\[\hat{s} = \begin{bmatrix} [\boldsymbol{\omega}_{\text{unit}}]_\times & \boldsymbol{v}_{\text{unit}} \\ \mathbf{0}^T & 0 \end{bmatrix} \]

能滿足\(\boldsymbol{v} = \theta \cdot \boldsymbol{v}_{\text{unit}}\)。這里的\(\boldsymbol{\omega}_{\text{unit}}\) 是單位旋轉(zhuǎn)軸，\(\boldsymbol{v}_{\text{unit}}\) 是單位平移速度向量。

可能在這里有的讀者會覺得有點難以理解，平移和旋轉(zhuǎn)是兩個獨立的運動，為什么會共享同一個參數(shù)\(\theta\)？其實這里把螺旋運動理解成向一個木板擰螺絲就行了，擰螺絲擰的角度越大，打進木板的距離就越遠。這里的\(\theta\)是一個統(tǒng)一的“運動參數(shù)”，它決定了旋轉(zhuǎn)了多少、平移了多少，就像是“沿某個固定運動方向走多遠”的度量。

數(shù)學(xué)上的表達總結(jié)如下：

\[{\xi} = \theta \cdot \hat{s} = \theta \cdot \begin{bmatrix} [\boldsymbol{\omega}_{\text{unit}}]_\times & \boldsymbol{v}_{\text{unit}} \\ \mathbf{0}^T & 0 \end{bmatrix}= \begin{bmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ \mathbf{0}^T & 0 \end{bmatrix} \]

3.2 \(SO(3)\)上的指數(shù)映射

根據(jù)前面的推導(dǎo)，已知旋轉(zhuǎn)矩陣 \(R \in SO(3)\) 可以通過李代數(shù) \(\mathfrak{so}(3)\) 的指數(shù)映射來表示：

\[R = \exp([\boldsymbol{\omega}]_\times) \]

這里的\(\boldsymbol{\omega}\)就是旋轉(zhuǎn)向量，我們知道向量都等于自身的單位向量與模長的乘積，那么可以令\(a\)為\(\boldsymbol{\omega}\)的單位向量，\(\theta\) 為 \(\boldsymbol{\omega}\)的模長。也即是：

\[\boldsymbol{\omega} = {\theta}a \]

取反對稱矩陣，有：

\[[\boldsymbol{\omega}]_\times = [{\theta}a]_\times = {\theta}[a]_\times \]

令\(A = [a]_\times\)，根據(jù)預(yù)備知識介紹的泰勒公式，將標量指數(shù)函數(shù)推廣到矩陣指數(shù)函數(shù)上，有：

\[R = \exp({\theta}A) = \sum_{n=0}^\infty \frac{({\theta}A)^n}{n!} \]

將 \(\exp(\theta A)\) 展開為泰勒級數(shù)：

\[\exp(\theta A) = I + \theta A + \frac{\theta^2}{2!} A^2 + \frac{\theta^3}{3!} A^3 + \frac{\theta^4}{4!} A^4 + \cdots \]

利用預(yù)備知識中提到的反對稱矩陣的（如 \(A^3 = -A\), \(A^4 = -A^2\) 等），我們可以把所有項都寫成 \(I, A, A^2\) 的線性組合：

\[\begin{aligned} \exp(\theta A) &= I + \theta A + \frac{\theta^2}{2!} A^2 -\frac{\theta^3}{3!} A -\frac{\theta^4}{4!} A^2 +\frac{\theta^5}{5!} A +\cdots \\ &= I + \left(\theta - \frac{\theta^3}{3!} + \frac{\theta^5}{5!} - \cdots\right) A \\ &\quad + \left(\frac{\theta^2}{2!} - \frac{\theta^4}{4!} + \cdots\right) A^2 \end{aligned} \]

再利用預(yù)備知識中的正弦和余弦的泰勒公式展開：

\(\sin\theta = \theta - \frac{\theta^3}{3!} + \frac{\theta^5}{5!} - \cdots\)
\(1 - \cos\theta = \frac{\theta^2}{2!} - \frac{\theta^4}{4!} + \cdots\)

所以：

\[\exp(\theta A) = I + \sin\theta \cdot A + (1 - \cos\theta) \cdot A^2 \]

也可以表達為：

\[\exp(\theta [a]_\times) = \cos\theta I + (1 - \cos\theta) a{a}^T +\sin\theta [a]_\times \]

其中，\(a\)為\(\mathfrak{so}(3)\)集合中元素\(\boldsymbol{\omega}\)的單位向量，\(A\)為\(a\)的反對稱矩陣，\(\theta\) 為\(\boldsymbol{\omega}\)的模長。其實這里的\(\boldsymbol{\omega}\)就是旋轉(zhuǎn)矩陣的旋轉(zhuǎn)向量，這個公式也就是著名的羅德里格公式。

通過以上推導(dǎo)表明，\(\mathfrak{so}(3)\)其實就是旋轉(zhuǎn)向量組成的向量空間，指數(shù)映射就是羅德里格公式。通過羅德里格公式，任意一個向量都可以對應(yīng)到\(SO(3)\)中的旋轉(zhuǎn)矩陣。反之，將\(SO(3)\)對應(yīng)到\(\mathfrak{so}(3)\)就是對數(shù)映射，當然這里就是不用列出對數(shù)表達式，在《視覺SLAM十四講》的第3講中介紹了更簡便的計算方法：

\[\theta = \arccos\left( \frac{\operatorname{tr}(R) - 1}{2} \right),Ra = a \]

在書上沒有給出\(Ra = a\)的具體解的過程，因為可能有多個解的情況，表述起來比較麻煩，大概的求解過程是：

步驟	內(nèi)容
1.	計算旋轉(zhuǎn)角度：\(\theta = \arccos\left( \frac{\operatorname{tr}(R) - 1}{2} \right)\)
2.	若 \(\theta = 0\)：返回 \(\boldsymbol{\omega} = \mathbf{0}\) 或任意方向
3.	若 \(\theta = \pi\)：構(gòu)造 \(\boldsymbol{\omega} \boldsymbol{\omega}^T = \frac{1}{2}(R + I)\)，取最大特征值對應(yīng)的單位特征向量
4.	否則：計算反對稱部分 \(S = \frac{1}{2}(R - R^T)\)，提取 \(\boldsymbol{\omega} = \frac{1}{2\sin\theta} \begin{bmatrix} S_{32} \\ S_{13} \\ S_{21} \end{bmatrix}\)，然后歸一化

其實從求解對數(shù)映射的解就可以看出，指數(shù)映射是一個滿射，而不是單射。也就是任意\(SO(3)\)集合中的元素，都能在\(\mathfrak{so}(3)\)中找到一個元素與之對應(yīng)；但是可能存在多個\(\mathfrak{so}(3)\)中的元素對應(yīng)于一個\(SO(3)\)集合中的元素。其實這個不難理解，旋轉(zhuǎn)是有周期性的，旋轉(zhuǎn)360度和不轉(zhuǎn)是一樣的，只有范圍限定在\((-\pi, +\pi]\)之間，兩者才可以一一對應(yīng)。

3.3 \(SE(3)\)上的指數(shù)映射

根據(jù)前面的推導(dǎo)，特殊歐式群\(SE(3)\)的李代數(shù)\(\mathfrak{se}(3)\)，可以用一個4維矩陣表示，包含旋轉(zhuǎn)和平移信息：

\[\xi = \begin{bmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ \mathbf{0}^T & 0 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \]

其中 \(\boldsymbol{\omega}\) 就是特殊正交群\(SO(3)\)的李代數(shù)的向量表示，\(\boldsymbol{v}\) 表示平移速度。

根據(jù)準備知識中介紹的Chasles定理，有：

\[T = \exp(\xi) = \exp(\hat{s} \theta) \]

其中\(\hat{s}\)是單位螺旋軸矩陣：

\[\hat{s} = \begin{bmatrix} [\boldsymbol{\omega}_{\text{unit}}]_\times & \boldsymbol{v}_{\text{unit}} \\ \mathbf{0}^T & 0 \end{bmatrix} \]

\(\theta\)則描述沿這個螺旋軸運動了多遠。使用泰勒公式展開指數(shù)函數(shù)，有：

\[\exp({\xi}) = \sum_{n=0}^\infty \frac{(\hat{s}\theta)^n}{n!} = I + \hat{s} \theta + \frac{(\hat{s} \theta)^2}{2!} + \frac{(\hat{s} \theta)^3}{3!} + \cdots \]

因此關(guān)鍵在于計算每一項\((\hat{s}\theta)^n\)。對于第0項：

\[I = \begin{bmatrix} I & \mathbf{0} \\ \mathbf{0}^T & 1 \end{bmatrix} \]

對于第1項，前面求\(SO(3)\)的時候已經(jīng)定義\(A = [\boldsymbol{\omega}_{\text{unit}}]_\times\)，則：

\[\hat{s}\theta = \begin{bmatrix} \theta A & \theta \boldsymbol{v}_{\text{unit}} \\ \mathbf{0}^T & 0 \end{bmatrix} = \begin{bmatrix} \theta A & \boldsymbol{v} \\ \mathbf{0}^T & 0 \end{bmatrix} \]

對于第2項：

\[\frac{(\hat{s} \theta)^2}{2!} = \frac{1}{2!} \begin{bmatrix} (\theta A)^2 & \theta A \boldsymbol{v} \\ \mathbf{0}^T & 0 \end{bmatrix}= \begin{bmatrix} \frac{(\theta A)^2}{2!} & \frac{(\theta A) \boldsymbol{v}}{2!} \\ \mathbf{0}^T & 0 \end{bmatrix} \]

對于第3項：

\[\frac{(\hat{s} \theta)^3}{3!} = \frac{1}{3!} \begin{bmatrix} (\theta A)^3 & (\theta A)^2 \boldsymbol{v} \\ \mathbf{0}^T & 0 \end{bmatrix}= \begin{bmatrix} \frac{(\theta A)^3}{3!} & \frac{(\theta A)^2 \boldsymbol{v}}{3!} \\ \mathbf{0}^T & 0 \end{bmatrix} \]

以此類推，把所有項加起來，分塊相加，有：

\[\begin{align*} \exp({\xi}) &= I + \hat{s} \theta + \frac{(\hat{s} \theta)^2}{2!} + \frac{(\hat{s} \theta)^3}{3!} + \cdots \\ &= \begin{bmatrix} I & \mathbf{0} \\ \mathbf{0}^T & 1 \end{bmatrix} + \begin{bmatrix} \theta A & \boldsymbol{v} \\ \mathbf{0}^T & 0 \end{bmatrix} + \begin{bmatrix} \frac{(\theta A)^2}{2!} & \frac{(\theta A) \boldsymbol{v}}{2!} \\ \mathbf{0}^T & 0 \end{bmatrix}+ \begin{bmatrix} \frac{(\theta A)^3}{3!} & \frac{(\theta A)^2 \boldsymbol{v}}{3!} \\ \mathbf{0}^T & 0 \end{bmatrix} + \cdots\\ &=\begin{bmatrix} \sum_{n=0}^\infty \frac{({\theta}A)^n}{n!} & \sum_{n=1}^\infty \frac{(\theta A)^{n-1} \boldsymbol{v}}{n!} \\ \mathbf{0}^T & 1 \\ \end{bmatrix} \end{align*} \]

對于這個結(jié)果的左上角元素，其實就是\(SO(3)\)的指數(shù)映射：

\[\exp(\theta A) = \sum_{n=0}^\infty \frac{({\theta}A)^n}{n!} \]

而對于右上角元素，可以將索引平移一下，令 \(k = n - 1\)，則：

\[\sum_{k=0}^\infty \frac{(\theta A)^k \boldsymbol{v}}{(k+1)!} \]

因此，\(SE(3)\)指數(shù)映射可以寫成：

\[\exp({\xi}) = \begin{bmatrix} \exp(\theta A) & \sum_{n=0}^\infty \frac{(\theta A)^n \boldsymbol{v}}{(n+1)!} \\ \mathbf{0}^T & 1 \end{bmatrix} \]

但是為了進一步方便表達右上角元素的表達式，引入左不變雅可比矩陣\(J\)：

\[J = \sum_{n=0}^\infty \frac{(\theta A)^n}{(n+1)!} \]

接下來利用預(yù)備知識中介紹的反對稱矩陣的周期性規(guī)律計算\(J\)：

\[\begin{align*} J &= I + \frac{\theta}{2!}{A} + \frac{{\theta}^{2}}{3!}{A^2} + \frac{{\theta}^{3}}{4!}{A^3} + \cdots \\ &= I + \frac{1}{\theta}(\frac{\theta^{2}}{2!} - \frac{{\theta}^{4}}{4!} + \cdots) {A} + \frac{1}{\theta}(\frac{{\theta}^{3}}{3!}-\frac{{\theta}^{5}}{5!} + \cdots) {A^2} \end{align*} \]

利用預(yù)備知識中的正弦和余弦的泰勒公式展開：

\(\theta - \sin\theta = \frac{\theta^3}{3!} + \frac{\theta^5}{5!} - \cdots\)
\(1 - \cos\theta = \frac{\theta^2}{2!} - \frac{\theta^4}{4!} + \cdots\)

可以得到：

\[J = I + \frac{1 - \cos\theta}{\theta} A + \frac{\theta - \sin\theta}{\theta} A^2 \]

由于\(A = [a]_\times\)，因此\(J\)也可以表達為：

\[\begin{align*} J &= I + \frac{1 - \cos\theta}{\theta} [a]_\times + \frac{\theta - \sin\theta}{\theta} ({a}{a^T}-\mathbf{I})\\ &= \frac{\sin\theta}{\theta}I + (1 - \frac{\sin\theta}{\theta}){a}{a^T} + \frac{1 - \cos\theta}{\theta} [a]_\times\\ \end{align*} \]

\(SE(3)\)的指數(shù)映射最終結(jié)果如下：

\[\exp({\xi}) = \begin{bmatrix} \exp([\boldsymbol{\omega}]_\times) & J\boldsymbol{v} \\ \mathbf{0}^T & 1 \end{bmatrix} \]

其中：

旋轉(zhuǎn)部分 \(R = \exp([\boldsymbol{\omega}]_\times)\)即\(SO(3)\)的指數(shù)映射。
平移部分 \(\boldsymbol{t} = J\boldsymbol{v}\), 其中J是左不變雅可比矩陣：
\[J = I + \frac{1 - \cos\theta}{\theta} A + \frac{\theta - \sin\theta}{\theta} \]
或
\[J=\frac{\sin\theta}{\theta}I + (1 - \frac{\sin\theta}{\theta}){a}{a^T} + \frac{1 - \cos\theta}{\theta} [a]_\times \]

與\(SO(3)\)類似，也可以推導(dǎo)出\(SE(3)\)到\(\mathfrak{se}(3)\)的對數(shù)映射。旋轉(zhuǎn)部分可以按照之前介紹的\(SO(3)\)到\(\mathfrak{so}(3)\)的對數(shù)映射求出；平移部分由于\(J\)已知（通過左不變雅可比矩陣求得），可以解\(\boldsymbol{t} = J\boldsymbol{v}\)這個線性方程組來解得。具體數(shù)學(xué)表達式如下：

\[\theta = \arccos\left( \frac{\operatorname{tr}(R) - 1}{2} \right),Ra = a,\boldsymbol{t} = J\boldsymbol{v} \]

4 總結(jié)

作為本篇的結(jié)束，整理了一張?zhí)厥庹蝗旱膬?nèi)容的表格，如下所示：

內(nèi)容	特殊正交群
李群	\(SO(3) = \{ R \in \mathbb{R}^{3\times3} \mid R^T R = I, \det(R) = 1\}\)
李代數(shù)	\(\mathfrak{so}(3) = \left\{ [\boldsymbol{\omega}]_\times \in \mathbb{R}^{3 \times 3} \mid {\omega} \in \mathbb{R}^3 \right\}\)
向量表示	\({\omega} \in \mathbb{R}^3\)
李括號	\([[\boldsymbol{\omega}_1]_\times, [\boldsymbol{\omega}_2]_\times] = [\boldsymbol{\omega}_1]_\times [\boldsymbol{\omega}_2]_\times - [\boldsymbol{\omega}_2]_\times [\boldsymbol{\omega}_1]_\times\) 或者叉乘\(\times\)
指數(shù)映射	\(\exp(\theta [a]_\times) = \cos\theta I + (1 - \cos\theta) a{a}^T +\sin\theta [a]_\times\)
對數(shù)映射	\(\theta = \arccos\left( \frac{\operatorname{tr}(R) - 1}{2} \right),Ra = a\)

特殊歐式群的內(nèi)容則如下表所示：

內(nèi)容	特殊歐式群
李群	\(SE(3)=\bigg\{ T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \in \mathbb{R}^{4\times4} \mid R \in SO(3) ,t \in \mathbb{R}^3 \bigg\}\)
李代數(shù)	\(\mathfrak{se}(3) = \left\{\xi = \begin{bmatrix}[\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ \mathbf{0}^T & 0 \\ \end{bmatrix} \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{\omega}, \boldsymbol{v} \in \mathbb{R}^3 \right\}\)
向量表示	\(\boldsymbol{\eta} = \begin{bmatrix} \boldsymbol{\omega} \\ \boldsymbol{v} \end{bmatrix} \in \mathbb{R}^6\)
李括號	\([\xi_1, \xi_2] = \xi_1 \xi_2 - \xi_2 \xi_1\) 或者 \([\boldsymbol{\eta}_1, \boldsymbol{\eta}_2] = \begin{bmatrix} \boldsymbol{\omega}_1 \times \boldsymbol{\omega}_2 \\ \boldsymbol{\omega}_1 \times \boldsymbol{v}_2 - \boldsymbol{\omega}_2 \times \boldsymbol{v}_1 \end{bmatrix}\)
指數(shù)映射	\(\exp({\xi}) = \begin{bmatrix} \exp([\boldsymbol{\omega}]_\times) & J\boldsymbol{v} \\ \mathbf{0}^T & 1 \\ \end{bmatrix}\\J=\frac{\sin\theta}{\theta}I + (1 - \frac{\sin\theta}{\theta}){a}{a^T} + \frac{1 - \cos\theta}{\theta} [a]_\times\)
對數(shù)映射	\(\theta = \arccos\left( \frac{\operatorname{tr}(R) - 1}{2} \right),Ra = a,\boldsymbol{t} = J\boldsymbol{v}\)

注意可能符號與書上的符號略有差異，讀者可以自行對照參考。

另外，筆者也想說一點內(nèi)容之外的感受。除非具有相當?shù)某橄竽芰Γ谝淮谓佑|到像李群和李代數(shù)這樣的知識是很難理解到位的，因為你完全沒有應(yīng)用這些知識的場景，再怎么認真看書，也不過是泛泛而過。在這種情況下，不如好好推導(dǎo)一遍其中的原理，舉一些實際的例子來加深映像。另外，這些數(shù)學(xué)知識既高深又抽象（筆者大概翻閱了一下《李理論》的相關(guān)知識，本文的這些推導(dǎo)在行家面前估計不值一哂），我們也不用過多去糾結(jié)，主打一個能自己說服自己即可。如果讀者只是像我一樣的普通人，可能需要在以后的應(yīng)用和實踐中來更深入地理解這些知識了；目前我們需要做的，就是不要排斥這個新的工具，然后理解它，利用它——真理之道，就在其中。

posted @ 2025-05-11 21:57 charlee44 閱讀(420) 評論(0) 收藏舉報

刷新頁面返回頂部

人生海海山山而川

分說，不分說，不由分說！

詳解SLAM中的李群和李代數(shù)（中）

1 概述

2 李代數(shù)

2.1 定義

2.2 李代數(shù)\(\mathfrak{so}(3)\)

2.3 \(\mathfrak{so}(3)\)的李括號

2.4 李代數(shù)\(\mathfrak{se}(3)\)

2.5 \(\mathfrak{se}(3)\)的李括號

3 指數(shù)映射

3.1 預(yù)備知識

3.1.1 泰勒公式

3.1.2 反對稱矩陣的冪次規(guī)律

3.1.3 Chasles定理

3.2 \(SO(3)\)上的指數(shù)映射

3.3 \(SE(3)\)上的指數(shù)映射

4 總結(jié)

公告

人生海海 山山而川

分說，不分說，不由分說！

詳解SLAM中的李群和李代數(shù)（中）

1 概述

2 李代數(shù)

2.1 定義

2.2 李代數(shù)\(\mathfrak{so}(3)\)

2.3 \(\mathfrak{so}(3)\)的李括號

2.4 李代數(shù)\(\mathfrak{se}(3)\)

2.5 \(\mathfrak{se}(3)\)的李括號

3 指數(shù)映射

3.1 預(yù)備知識

3.1.1 泰勒公式

3.1.2 反對稱矩陣的冪次規(guī)律

3.1.3 Chasles定理

3.2 \(SO(3)\)上的指數(shù)映射

3.3 \(SE(3)\)上的指數(shù)映射

4 總結(jié)

公告

人生海海山山而川