SLAM文献之A micro Lie theory for state estimation in robotic（4）

相关内容地址：1、SLAM文献之A micro Lie theory for state estimation in robotic（1）2、SLAM文献之A micro Lie theory for state estimation in robotic（2）3、SLAM文献之A micro Lie theory for state estimation in robotic（3）我们以一种适合

花雨仙晨

1070人浏览 · 2025-12-22 19:45:54

花雨仙晨 · 2025-12-22 19:45:54 发布

VI. 结论

我们以一种适合具备状态估计背景读者的形式，介绍了李群理论的核心内容，并重点关注机器人学的应用。我们通过以下几个方面来实现这一目标：

首先，选择了尽可能避免抽象数学概念的材料。这有助于聚焦李群理论，使其工具更易于理解和使用。

其次，采用了教学性的方法，内容具有显著冗余性。正文内容是通用的，涵盖了李群理论的抽象要点。同时配有方框示例，将抽象概念具体化到特定李群，并提供大量配有详细说明的图示。

第三，我们推广了方便使用的算子，例如大写的 $E x p ()$ 和 $L o g ()$ 映射，以及加减算子 $\oplus, \ominus$ 等。它们允许我们在切空间的笛卡尔表示下工作，从而得到的导数和协方差处理公式与标准向量空间中的对应公式高度相似。

第四，我们特别强调了雅可比矩阵的定义、几何解释及计算方法。为此，我们引入了雅可比矩阵和协方差的符号，使得操作直观且易于理解。尤其是链式法则在此符号体系下非常清晰，有助于建立直觉并减少错误。

第五，附录中提供了机器人学中最常用李群的公式大全。在二维中，我们展示了单位复数旋转群 $S^1$ 、旋转矩阵群 $SO (2)$ 以及刚体运动群 $SE (2)$ ；在三维中，我们展示了单位四元数群 $S^3$ 、旋转矩阵群 $SO (3)$ 以及刚体运动群 $SE (3)$ 。同时，我们还提供了任意维度的平移群，可通过标准向量空间 $\mathbb{R}^n$ 的加法或矩阵平移群 $T (n)$ 的乘法来实现。

第六，我们给出了一些应用示例，展示了李群理论在机器人问题中以优雅且精确的方式求解问题的能力。复合群的概念虽略显朴素，但有助于将异构状态向量统一为李群理论形式。

最后，我们附带了新的 C++ 库 manif [7]，实现了本文描述的工具。manif 可在 https://github.com/artivis/manif 获取。第 V 节中的应用示例也在 manif 中得到了演示。

尽管本文并未引入新的理论内容，但我们相信，本文呈现李群理论的形式将帮助许多研究人员进入该领域，为其未来研究提供便利。我们也认为，这本身就是一项有价值的贡献。

附录 A

二维旋转群 $S^1$ 与 $SO (2)$

李群 $S^1$ 是单位复数在复数乘法下的群。其拓扑是单位圆或单位 1-球，因此得名 $S^1$ 。该群、李代数和向量元素形式为：

$\cos\theta + i \sin\theta, \quad \tau^\wedge = i \theta, \quad \tau = \theta. \tag{112}$

取逆和复合通过共轭实现： $z^{-1} = z^*$ ，复合为 $z_a \circ z_b = z_a z_b$ 。

群 $SO (2)$ 是平面内特殊正交矩阵群，即旋转矩阵群，在矩阵乘法下构成群。群、李代数和向量元素形式为：

$\begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}, \quad \tau^\wedge = [\theta]_\times = \begin{bmatrix} 0 & -\theta \\ \theta & 0 \end{bmatrix}, \quad \tau = \theta. \tag{113}$

取逆和复合通过转置实现： $R^{-1} = R^\top$ ，复合为 $R_a \circ R_b = R_a R_b$ 。

两者都作用于二维向量，并且切空间同构，因此可以一起研究。

A. Exp 与 Log 映射

$S^1$ 和 $SO (2)$ 的复数与旋转矩阵可以定义 Exp 与 Log 映射。

对于 $S^1$ ：

$\text{Exp}(\theta) = \cos\theta + i \sin\theta \in \mathbb{C}, \tag{114}$

$\theta = \text{Log}(z) = \arctan(\text{Im}(z), \text{Re}(z)) \in \mathbb{R}, \tag{115}$

其中 (114) 为欧拉公式。

对于 $SO (2)$ ：

$\text{Exp}(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \in \mathbb{R}^{2 \times 2}, \tag{116}$

$\theta = \text{Log}(R) = \arctan(r_{21}, r_{11}) \in \mathbb{R}. \tag{117}$

B. 取逆、复合与指数映射

考虑通用二维旋转元素，用无衬线字体表示 $Q, R$ ：

$R(\theta)^{-1} = R(-\theta), \tag{118}$

$\circ R = R \circ Q, \tag{119}$

即平面旋转是可交换的。因此有：

$\text{Exp}(\theta_1 + \theta_2) = \text{Exp}(\theta_1) \circ \text{Exp}(\theta_2), \tag{120}$

$\text{Log}(Q \circ R) = \text{Log}(Q) + \text{Log}(R), \tag{121}$

$\ominus R = \theta_Q - \theta_R. \tag{122}$

C. 雅可比块

由于我们定义的导数映射切向量空间，并且对于 $S^1$ 和 $SO (2)$ 的平面旋转流形，这些空间相同，即 $\theta = \text{Log}(z) = \text{Log}(R)$ ，所以雅可比与表示方式（ $z$ 或 $R$ ）无关。

1) 伴随与其他平凡雅可比：

由 (41a)、第 III-B 节及以上性质，有以下标量导数块为平凡值：

$\text{Ad}_R = 1 \in \mathbb{R}, \tag{123}$

$J_{R^{-1}}^R = -1 \in \mathbb{R}, \tag{124}$

$J_{Q}^{Q \circ R} = J_Q^{R \circ R} = 1 \in \mathbb{R}, \tag{125}$

$J_r(\theta) = J_l(\theta) = 1 \in \mathbb{R}, \tag{126}$

$J_R^{R \oplus \theta} = J_R^{\theta \oplus \theta} = 1 \in \mathbb{R}, \tag{127}$

$J_Q^{Q \ominus R} = -J_R^{Q \ominus R} = 1 \in \mathbb{R}. \tag{128}$

2) 旋转作用

对于作用 $\cdot v$ ：

$J_R^{R \cdot v} = \lim_{\theta \to 0} \frac{R \text{Exp}(\theta) v - Rv}{\theta}\\ = \lim_{\theta \to 0} \frac{R(I + [\theta]\times)v - Rv}{\theta}\\ = \lim{\theta \to 0} \frac{R[\theta]\times v}{\theta}\\ = R[1]\times v \in \mathbb{R}^{2 \times 1}, \tag{129}$

$J^{Rv \cdot }_v = \frac{D(Rv)}{Dv} = R \in \mathbb{R}^{2 \times 2}. \tag{130}$

附录 B

三维旋转群 $S^3$ 与 $SO (3)$

李群 $S^3$ 是单位四元数在四元数乘法下的群。其拓扑是 $\mathbb{R}^4$ 中的单位 3-球，因此得名 $S^3$ 。四元数（详见参考文献 [8]）可以用以下等价形式表示：

$\mathbf{v} \in \mathbb{H}\\ = \begin{bmatrix} w & x & y & z \end{bmatrix}^\top = \begin{bmatrix} w \\ \mathbf{v} \end{bmatrix} \in \mathbb{H}, \tag{131}$

其中 $\in \mathbb{R}$ ， $i, j, k$ 为三个单位虚数，满足 $i^2 = j^2 = k^2 = ijk = -1$ 。标量 $w$ 为实部， $\mathbf{v} \in \mathbb{H}_p$ 为向量部（纯虚部），维度为 3。取逆与复合通过共轭实现 $q^{-1} = q^*$ ，其中 $q^* = w - \mathbf{v}$ ，复合为 $q_a \circ q_b = q_a q_b$ 。

群 $SO (3)$ 是三维空间中的特殊正交矩阵群，即旋转矩阵群，在矩阵乘法下构成群。取逆与复合通过转置和矩阵乘法实现，与 $SO (n)$ 类似。

两者都作用于三维向量，其切空间同构，可用 $\mathbb{R}^3$ 的旋转向量表示，因此可一起研究。在此空间中，我们定义旋转速度向量 $\omega \triangleq u\omega$ ，轴角 $\theta \triangleq u\theta$ 以及所有扰动和不确定性。

四元数流形 $S^3$ 是 $SO (3)$ 的双覆盖，即 $q$ 和 $- q$ 表示同一旋转 $R$ 。第一覆盖对应实部 $w > 0$ 的四元数。两群在第一覆盖下可视为同构。

A. Exp 与 Log 映射

四元数 $q=(w,\mathbf{v}) \in \mathbb{H}$ 的 Exp 与 Log 映射（参见例 5）：

$\text{Exp}(\theta \mathbf{u}) \triangleq \cos(\theta/2) + \mathbf{u} \sin(\theta/2) \in \mathbb{H}, \tag{132}$

$\theta \mathbf{u} = \text{Log}(q) \triangleq 2 \frac{\mathbf{v}}{||\mathbf{v}||} \arctan\ （||\mathbf{v}||{,w}） \in \mathbb{R}^3. \tag{133}$

为避免双覆盖问题，可确保实部 $w > 0$ ，若不满足，可用 $- q$ 替换 $q$ 。

旋转矩阵的 Exp 与 Log（参见例 4）：

$\text{Exp}(\theta \mathbf{u}) \triangleq I + \sin\theta [\mathbf{u}]\times + (1-\cos\theta) [\mathbf{u}]\times^2 \in \mathbb{R}^{3\times 3}, \tag{134}$

$\theta \mathbf{u} = \text{Log}(R) \triangleq \frac{\theta(R-R^\top)^\vee}{2 \sin\theta} \in \mathbb{R}^3, \tag{135}$

其中 $\theta = \cos^{-1}\left(\frac{\text{trace}(R)-1}{2}\right)$ 。

B. 旋转作用

四元数对三维向量的旋转作用通过双四元数乘法实现：

$\mathbf{x}' = q \mathbf{x} q^*, \tag{136}$

旋转矩阵通过单矩阵乘法实现：

$\mathbf{x}' = R \mathbf{x}. \tag{137}$

两者均对应绕轴 $\mathbf{u}$ 顺时针旋转 $\theta$ 弧度。由此可得到 $q$ 对应的旋转矩阵：

$\begin{bmatrix} w^2+x^2-y^2-z^2 & 2(xy-wz) & 2(xz+wy) \\ 2(xy+wz) & w^2-x^2+y^2-z^2 & 2(yz-wx) \\ 2(xz-wy) & 2(yz+wx) & w^2-x^2-y^2+z^2 \end{bmatrix}. \tag{138}$

C. 基本雅可比块

由于定义的导数映射切向量空间，对于 $S^3$ 和 $SO (3)$ 的 3D 旋转流形， $\theta = \text{Log}(q) = \text{Log}(R)$ ，雅可比与表示方式无关( $q$ 或 $R$ )。

考虑通用 3D 旋转元素，记为 $R$ ：

1) 伴随

由 (31) 得：

$\text{Ad}_R \theta = (R [\theta]\times R^\top)^\vee = ([R\theta]_\times)^\vee = R \theta \implies \\ \text{Ad}_R = R. \tag{139}$

说明 $\text{Ad}_q = R(q)$ （见 (138)）， $\text{Ad}_R = R$ 。

2) 取逆与复合

由 III-B 节得：

$J_{R^{-1}}^R = -\text{Ad}_R = -R, \tag{140}$

$J^{QR}_Q = \text{Ad}_{R}^{-1} = R^\top, \tag{141}$

$J^{QR}_R = I. \tag{142}$

3) 左右雅可比

闭式形式（[11, 页 40]）：

$J_r(\theta) = I - \frac{1-\cos\theta}{\theta^2} [\theta]\times + \frac{\theta-\sin\theta}{\theta^3} [\theta]^2\times, \tag{143}$

$J_r^{-1}(\theta) = I + \frac{1}{2}[\theta]\times + \left(\frac{\theta}{2} - \frac{1+\cos\theta}{2\theta \sin\theta}\right)[\theta]^2\times, \tag{144}$

$J_l(\theta) = I + \frac{1-\cos\theta}{\theta^2} [\theta]\times + \frac{\theta-\sin\theta}{\theta^3} [\theta]^2\times, \tag{145}$

$J_l^{-1}(\theta) = I - \frac{1}{2}[\theta]\times + \left(\frac{\theta}{2} - \frac{1+\cos\theta}{2\theta \sin\theta}\right)[\theta]^2\times. \tag{146}$

可观察到：

$J_l = J_r^\top, \quad J_l^{-1} = J_r^{- \top}. \tag{147}$

4) 右加与右减

对于 $\theta = Q \ominus R$ ：

$J_R^{R \oplus \theta} = R(\theta)^\top, \quad J_R^{\theta \oplus \theta} = J_r(\theta), \tag{148}$

$J_Q^{Q \ominus R} = J_{r1}^-(\theta), \quad J_R^{Q \ominus R} = -J_{l1}^-(\theta). \tag{149}$

5) 旋转作用

在这里插入图片描述

利用了 $\text{Exp}(\theta) \approx I + [\theta]\times$ 与 $[a]\times b = -[b]_\times a$ 。

第二个雅可比为：

$J^{R \cdot v}_v = \lim_{\Delta v \to 0} \frac{R(v+\Delta v)-Rv}{\Delta v} = R. \tag{151}$

附录 C

二维刚体运动群 $SE (2)$

我们将刚体运动群 $SE (2)$ 的元素表示为

$\begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \in SE(2) \subset \mathbb{R}^{3 \times 3}, \tag{152}$

其中 $\in SO(2)$ 为旋转， $\in \mathbb{R}^2$ 为平移。李代数和向量切空间由如下元素构成：

$\tau^\wedge = \begin{bmatrix} [\theta]_ \times & \rho \\ 0 & 0 \end{bmatrix} \in se(2), \quad \tau = \begin{bmatrix} \rho \\ \theta \end{bmatrix} \in \mathbb{R}^3. \tag{153}$

A. 取逆与复合

取逆与复合分别通过矩阵求逆与矩阵乘法实现：

$M^{-1} = \begin{bmatrix} R^\top & -R^\top t \\ 0 & 1 \end{bmatrix}, \tag{154}$

$M_a M_b = \begin{bmatrix} R_a & R_b t_a + t_b \\ 0 \ & 1 \end{bmatrix}. \tag{155}$

B. Exp 与 Log 映射

Exp 与 Log 通过标量切空间 $\mathbb{R}^3 \simeq se(2) = TSE(2)$ 的指数映射实现（参见 [5]）：

$\text{Exp}(\tau) \triangleq \begin{bmatrix} \text{Exp}([0\ \theta]) & V_1(\theta) \rho \\ 0 & 1 \end{bmatrix}, \tag{156}$

$\tau = \text{Log}(M) \triangleq \begin{bmatrix} V^{-1}(\theta) t \\ \text{Log}(R) \end{bmatrix}, \tag{157}$

其中

$V(\theta) = \frac{\sin \theta}{\theta} I + \frac{1-\cos\theta}{\theta} [1]_\times. \tag{158}$

C. 雅可比块

1) 伴随

由 (31) 并利用平面旋转可交换性，可得：

$\text{Ad}_M \tau = (M \tau^\wedge M^{-1})^\vee = \begin{bmatrix} R \rho - \theta [1]\times t \\ \theta \end{bmatrix} = \text{Ad}_M \begin{bmatrix} \rho \\ \theta \end{bmatrix},\\ \quad \text{Ad}_M = \begin{bmatrix} R & -[1]\times t \\0 \ & 1 \end{bmatrix}. \tag{159}$

2) 取逆与复合

由 III-B 节得：

$J_M^{M^{-1}} = -\text{Ad}_M = \begin{bmatrix} -R & -[1]\times t \\0 \ & -1 \end{bmatrix}, \tag{160}$

$J^{M_a M_b}_{M_a} = \text{Ad}{M_b}^{-1} = \begin{bmatrix} R_b^\top & R_b^\top [1]_\times t_b \\ 0 & 1 \end{bmatrix}, \tag{161}$

$J^{M_a M_b}_{M_b} = I. \tag{162}$

3) 左右雅可比

由 [11, 页 36] 得：

$J_r = \begin{bmatrix} \sin\theta/\theta & (1-\cos\theta)/\theta & (\theta \rho_1 - \rho_2 + \rho_2 \cos\theta - \rho_1 \sin\theta)/\theta^2 \\ (\cos\theta-1)/\theta & \sin\theta/\theta & (\rho_1 + \theta \rho_2 - \rho_1 \cos\theta - \rho_2 \sin\theta)/\theta^2 \\ 0 & 0 & 1 \end{bmatrix}, \tag{163}$

$J_l = \begin{bmatrix} \sin\theta/\theta & (\cos\theta-1)/\theta & (\theta \rho_1 + \rho_2 - \rho_2 \cos\theta - \rho_1 \sin\theta)/\theta^2 \\ (1-\cos\theta)/\theta & \sin\theta/\theta & (-\rho_1 + \theta \rho_2 + \rho_1 \cos\theta - \rho_2 \sin\theta)/\theta^2 \\ 0 & 0 & 1 \end{bmatrix}. \tag{164}$

4) 刚体运动作用

刚体对点 $\mathbf{p}$ 的作用为：

$\cdot \mathbf{p} = t + R \mathbf{p}. \tag{165}$

因此，当 $\tau \to 0$ 时，有 $\text{Exp}(\tau) \to I + \tau^\wedge$ ，可得

$J_M^{M \cdot p} = \lim_{\tau \to 0} \frac{M \text{Exp}(\tau) \cdot p - M \cdot p}{\tau} = \begin{bmatrix} R & R [1]_\times p \end{bmatrix}, \tag{166}$

$J_p^{M \cdot p} = R. \tag{167}$

附录 D

三维刚体运动群 $SE (3)$

我们将三维刚体运动群 $SE (3)$ 的元素表示为

$\begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \in SE(3) \subset \mathbb{R}^{4 \times 4}, \tag{168}$

其中 $\in SO(3)$ 为旋转矩阵， $\in \mathbb{R}^3$ 为平移向量。李代数和向量切空间由如下元素构成：

$\tau^\wedge = \begin{bmatrix} [\theta]_\times & \rho \\ 0 & 0 \end{bmatrix} \in se(3), \quad \tau = \begin{bmatrix} \rho \\ \theta \end{bmatrix} \in \mathbb{R}^6. \tag{169}$

A. 取逆与复合

取逆与复合分别通过矩阵求逆与矩阵乘法实现：

$M^{-1} = \begin{bmatrix} R^\top & -R^\top t \\ 0 & 1 \end{bmatrix}, \tag{170}$

$M_a M_b = \begin{bmatrix} R_a & R_b t_a + t_b \\0 & 1 \end{bmatrix}. \tag{171}$

B. Exp 与 Log 映射

Exp 与 Log 通过向量切空间 $\mathbb{R}^6 \simeq se(3) = TSE(3)$ 的指数映射实现（参见 [5]）：

$\text{Exp}(\tau) \triangleq \begin{bmatrix} \text{Exp}([\theta]_ \times) & V(\theta) \rho \\ 0 & 1 \end{bmatrix}, \tag{172}$

$\tau = \text{Log}(M) \triangleq \begin{bmatrix} V^{-1}(\theta) t \\ \text{Log}(R) \end{bmatrix}, \tag{173}$

其中（注意 $\text{Log}(M)$ 中 $\theta = \theta u = \text{Log}(R)$ ）

$V(\theta) = I + \frac{1-\cos \theta}{\theta^2} [\theta]\times + \frac{\theta - \sin \theta}{\theta^3} [\theta]\times^2, \tag{174}$

与 (145) 完全一致。

C. 雅可比块

1) 伴随

由例 6 可得：

$\text{Ad}_M \tau = (M \tau^\wedge M^{-1})^\vee = \begin{bmatrix} R \rho + [ t]\times R \theta \\ R\theta \end{bmatrix} = \text{Ad}_M \begin{bmatrix} \rho \\ \theta \end{bmatrix},$

因此

$\text{Ad}_M = \begin{bmatrix} R & [t]\times R\\0 \ & R \end{bmatrix} \in \mathbb{R}^{6 \times 6}. \tag{175}$

2) 取逆与复合

由 III-B 节可得：

$J_M^{M^{-1}} = - \begin{bmatrix} R & [t]_\times R \\ 0& R \end{bmatrix}, \tag{176}$

$J^{M_a M_b}_{M_a }= \begin{bmatrix} R_b^\top & -R_b^\top [t_b]\times \\ 0 & R_b^\top \end{bmatrix}, \tag{177}$

$J^{M_a M_b}_{M_b} = I_6. \tag{178}$

3) 左右雅可比

Barfoot [10] 给出了左雅可比及其逆的闭式表达式：

$J_l(\rho, \theta) = \begin{bmatrix} J_l(\theta) & Q(\rho, \theta) \\ 0 & J_l(\theta) \end{bmatrix}, \tag{179a}$

$J_l^{-1}(\rho, \theta) = \begin{bmatrix} J_l^{-1}(\theta) & -J_l^{-1}(\theta) Q(\rho, \theta) J_l^{-1}(\theta) \\ 0 & J_l^{-1}(\theta) \end{bmatrix}, \tag{179b}$

其中 $J_l(\theta)$ 为 $SO (3)$ 的左雅可比（见 (145)），
在这里插入图片描述

右雅可比及其逆由 (76) 得：

$J_r(\rho, \theta) = J_l(-\rho, -\theta), \quad J_r^{-1}(\rho, \theta) = J_l^{-1}(-\rho, -\theta).$

4) 刚体运动作用

刚体对点 $\mathbf{p}$ 的作用为：

$\cdot \mathbf{p} = t + R \mathbf{p}. \tag{181}$

因此，当 $\tau \to 0$ 时，有 $\text{Exp}(\tau) \to I + \tau^\wedge$ ，可得

$J_M^{M \cdot p} = \lim_{\tau \to 0} \frac{M \text{Exp}(\tau) \cdot p - M \cdot p}{\tau} = \begin{bmatrix} R & -R [p]_\times \end{bmatrix}, \tag{182}$

$J^{M \cdot p}_p = R. \tag{183}$

附录 E

平移群 $(\mathbb{R}^n, +)$ 与 $T (n)$

群 $(\mathbb{R}^n, +)$ 是向量在加法下的群，可视为平移群。它被认为是平凡的，因为群元素、李代数和切空间都是相同的，因此有 $t^\wedge = \text{Exp}(t)$ 。其等价的矩阵群（在乘法下）为平移群 $T (n)$ ，其群、李代数和切向量元素为：

$\triangleq \begin{bmatrix} I & t \\ 0 & 1 \end{bmatrix} \in T(n), \quad t^\wedge \triangleq \begin{bmatrix} 0 & t \\ 0 & 0 \end{bmatrix} \in t(n), \quad t \in \mathbb{R}^n.$

等价性可通过观察 $T (0) = I$ ， $T(-t) = T(t)^{-1}$ ，以及可交换的复合
$T_1 T_2 = \begin{bmatrix} I & t_1 + t_2 \\ 0 &1 & \end{bmatrix}$
有效地将向量 $t_1$ 与 $t_2$ 相加来验证。由于 $\mathbb{R}^n$ 中的加法可交换， $T (n)$ 中的复合乘积也同样可交换。