ch08_lie_groups_optimized

第八章　矩阵李群（优化版） §

Chapter 8   Matrix Lie Groups (Optimized) §

---

本章导航 / Chapter Navigator §

一句话总结（TL;DR）

旋转矩阵不能用向量加法——李群理论把旋转状态拆成”大的均值（在流形上）+ 小的扰动（在向量空间里）“，让 EKF/MAP 得以正常工作。

阅读路径建议：

• 🟢 快速浏览：读完”直觉图解”即可把握核心思想
• 🔵 理解算法：阅读”概念层”的公式和解释
• ⚫ 深度推导：展开”推导层”查看完整数学

---

8.1 为什么普通向量不够用？ §

🎯 直觉：旋转矩阵的”加法陷阱” §

问题 1：普通高斯如何用？

普通高斯分布：$\mathbf{x}=\mathbf{\mu }+\mathbf{ϵ}$，其中 $\mathbf{ϵ}\sim \mathcal{N}(0,\mathbf{\Sigma })$。

但旋转矩阵 $\mathbf{C}$ 不能这样写！为什么？

• 两个旋转矩阵相加：${\mathbf{C}}_1+{\mathbf{C}}_2$ 不是旋转矩阵（行列式不为 1）
• 零矩阵 $0$ 不是旋转矩阵

问题 2：欧拉角有奇点（万向锁），不能全局使用。

解决方案：李群的”两全其美”

分量	存放位置	好处
大的均值 $\bar{\mathbf{C}}$	李群 $SO(3)$	无奇点、满足所有约束
小的扰动 $\mathbf{ϵ}$	李代数 ${\mathbb{R}}^3$	可以做普通线性代数

---

🔑 概念层：李代数是什么？ §

李代数是李群在单位元处的切平面——一个平的向量空间！

• $SO(3)$ 的李代数 $\mathfrak{so}(3)$：所有 $3\times 3$ 反对称矩阵
• $SE(3)$ 的李代数 $\mathfrak{se}(3)$：$4\times 4$ 矩阵

两者通过指数映射连接： $\mathbf{C}=\exp ({\mathbf{\varphi }}^{\wedge }),\mathbf{\varphi }\in {\mathbb{R}}^3$

左扰动方案（核心工具）： $\mathbf{C}=\exp ({\mathbf{ϵ}}^{\wedge })\bar{\mathbf{C}},\mathbf{ϵ}\sim \mathcal{N}(0,\mathbf{\Sigma })$

用途：EKF 中更新 $\mathbf{ϵ}$ 时用普通向量加法，再指数映射回旋转矩阵。

---

8.2 指数映射：切平面到流形 §

🎯 直觉：从”线性速度”到”旋转” §

想象你在控制一个旋转的刚体：

• 角速度 $\mathbf{\omega }$ 是线性的（向量）
• 积分角速度得到旋转 $\mathbf{C}(t)$，需要处理非线性

指数映射 $\exp ({\mathbf{\varphi }}^{\wedge })$ 就是”把线性速度积分成旋转”的数学工具。

---

🔑 概念层：$SO(3)$ 的 Rodrigues 公式 §

指数映射有闭合形式（无需无穷级数）：

$\exp ({\mathbf{\varphi }}^{\wedge })=\cos \varphi 1+(1-\cos \varphi )\mathbf{a}{\mathbf{a}}^T+\sin \varphi {\mathbf{a}}^{\wedge }$

其中 $\varphi =|\mathbf{\varphi }|$，$\mathbf{a}=\mathbf{\varphi }/\varphi$ 是单位轴。

这正是 Rodrigues 公式！ 指数映射统一了所有旋转参数化。

---

🧮 数值示例：验证指数映射 §

问题：绕 z 轴旋转 90° 的旋转矩阵是什么？

解答：

• 轴：$\mathbf{a}=[0,0,1{]}^T$
• 角：$\varphi =\pi /2$

$\exp ({\mathbf{\varphi }}^{\wedge })=\cos (\pi /2)1+(1-\cos (\pi /2))\mathbf{a}{\mathbf{a}}^T+\sin (\pi /2){\mathbf{a}}^{\wedge }$

$=0+[0,0,1{]}^T[0,0,1]+\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$

✓ 这正是绕 z 轴转 90° 的旋转矩阵。

---

📐 推导层：指数映射的级数展开 §

展开：从矩阵指数到 Rodrigues 公式

矩阵指数定义为： $\exp ({\mathbf{\varphi }}^{\wedge })={\sum }_{n=0}^{\infty }\frac{1}{n!}({\mathbf{\varphi }}^{\wedge }{)}^n$

对反对称矩阵，有恒等式：

• $({\mathbf{a}}^{\wedge }{)}^2=\mathbf{a}{\mathbf{a}}^T-1$
• $({\mathbf{a}}^{\wedge }{)}^3=-{\mathbf{a}}^{\wedge }$

因此高阶幂可以化简： $({\mathbf{a}}^{\wedge }{)}^4=-({\mathbf{a}}^{\wedge }{)}^2,({\mathbf{a}}^{\wedge }{)}^5={\mathbf{a}}^{\wedge },\dots$

将级数按奇偶分开，利用三角函数的泰勒级数，即得 Rodrigues 公式。

---

8.3 $SE(3)$：位姿的李群 §

🎯 直觉：旋转+平移的联合 §

位姿 $\mathbf{T}$ 同时包含旋转 $\mathbf{C}$ 和平移 $\mathbf{r}$：

$\mathbf{T}=\left[\begin{array}{cc}\mathbf{C}& \mathbf{r}\\ 0^T& 1\end{array}\right]$

问题：平移 $\mathbf{r}$ 和旋转 $\mathbf{C}$ 不是独立的——先转再移和先移再转结果不同！

李代数 $\mathfrak{se}(3)$ 用 6 维向量描述： ${\mathbf{\xi }}^{\wedge }={\left[\begin{array}{c}\mathbf{\rho }\\ \mathbf{\varphi }\end{array}\right]}^{\wedge }=\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]$

其中 $\mathbf{\rho }\in {\mathbb{R}}^3$ 是”平移方向”（不是真实平移！），$\mathbf{\varphi }$ 是旋转轴。

---

🔑 概念层：指数映射与左雅可比 §

$SE(3)$ 的指数映射： $\exp ({\mathbf{\xi }}^{\wedge })=\left[\begin{array}{cc}\mathbf{C}& \mathbf{J}\mathbf{\rho }\\ 0^T& 1\end{array}\right]$

左雅可比矩阵 $\mathbf{J}$（$3\times 3$）： $\mathbf{J}=\frac{\sin \varphi }{\varphi }1+\left(1-\frac{\sin \varphi }{\varphi }\right)\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \varphi }{\varphi }{\mathbf{a}}^{\wedge }$

作用：把李代数中的平移 $\mathbf{\rho }$ “拉伸”成真实的平移 $\mathbf{r}=\mathbf{J}\mathbf{\rho }$。

情形	$\mathbf{J}$ 近似	物理意义
$\varphi \approx 0$	$\mathbf{J}\approx 1$	小旋转时，$\mathbf{\rho }\approx \mathbf{r}$
$\varphi$ 大	$\mathbf{J}$ 显著偏离 $1$	旋转导致平移被”扭曲”

---

8.4 伴随表示：协方差的坐标变换 §

🎯 直觉：不确定性随坐标系移动 §

假设机器人相对于世界坐标系的位姿有不确定性。如果机器人移动到一个新的位姿，它相对于新坐标系的不确定性如何变化？

答案：通过伴随矩阵 $\mathcal{T}=\mathrm{Ad}(\mathbf{T})$ 变换。

---

🔑 概念层：伴随矩阵的定义与作用 §

对 $\mathbf{T}\in SE(3)$，伴随矩阵（$6\times 6$）为： $\mathcal{T}=\mathrm{Ad}(\mathbf{T})=\left[\begin{array}{cc}\mathbf{C}& {\mathbf{r}}^{\wedge }\mathbf{C}\\ 0& \mathbf{C}\end{array}\right]$

关键性质：

• 将李代数向量从一个坐标系”传输”到另一个：${\mathbf{\xi }}^{\prime }=\mathcal{T}\mathbf{\xi }$
• 协方差精确变换：${\mathbf{\Sigma }}^{\prime }=\mathcal{T}\mathbf{\Sigma }{\mathcal{T}}^T$

位姿复合中的协方差传播： 若 $\mathbf{T}={\mathbf{T}}_1{\mathbf{T}}_2$，则 $\mathbf{\Sigma }\approx {\mathbf{\Sigma }}_1+{\mathcal{T}}_1{\mathbf{\Sigma }}_2{\mathcal{T}}_1^T$

类比：这类似于线性系统 $\mathbf{y}=\mathbf{Ax}$ 的协方差传播 ${\mathbf{\Sigma }}_y=\mathbf{A}{\mathbf{\Sigma }}_x{\mathbf{A}}^T$，只是这里的”变换矩阵”是 $6\times 6$ 的伴随矩阵。

---

8.5 扰动微积分：在流形上优化 §

🎯 直觉：“围绕当前猜测的小步调整” §

普通高斯-牛顿：更新 $\mathbf{x}\leftarrow \mathbf{x}+\delta \mathbf{x}$

李群上的高斯-牛顿：更新 $\mathbf{C}\leftarrow \exp ({\mathbf{\psi }}^{\wedge }){\mathbf{C}}_{\mathrm{op}}$

核心区别：

1. 用指数映射保证结果仍在 $SO(3)$ 中
2. 对扰动 $\mathbf{\psi }$ 做普通向量优化（无约束）
3. 避免了欧拉角的奇点问题

---

🔑 概念层：左李导数 §

问题：计算 $\mathbf{Cv}$ 关于扰动 $\mathbf{\psi }$ 的导数。

左李导数： $\frac{\partial (\mathbf{Cv})}{\partial \mathbf{\psi }}=-({\mathbf{C}}_{\mathrm{op}}\mathbf{v}{)}^{\wedge }$

意义：导数只依赖于当前估计点旋转后的向量 ${\mathbf{C}}_{\mathrm{op}}\mathbf{v}$，再取反对称。

$SE(3)$ 版本： $\frac{\partial (\mathbf{Tp})}{\partial \mathbf{ϵ}}=({\mathbf{T}}_{\mathrm{op}}\mathbf{p}{)}^{\odot }$

其中 $(\cdot {)}^{\odot }$ 是”点算子”，将齐次坐标转化为 $4\times 6$ 矩阵。

---

🧮 数值示例：位姿优化 §

场景：用 ICP（迭代最近点）对齐两个点云。

目标：最小化 ${\sum }_m\Vert \mathbf{T}{\mathbf{p}}_m-{\mathbf{q}}_m{\Vert }^2$

左扰动 GN 迭代：

1. 在当前位姿 ${\mathbf{T}}_{\mathrm{op}}$ 处线性化
2. 求解 6 维线性系统得到最优扰动 ${\mathbf{ϵ}}^{\ast }$
3. 更新：${\mathbf{T}}_{\mathrm{op}}\leftarrow \exp ({\mathbf{ϵ}}^{\ast \wedge }){\mathbf{T}}_{\mathrm{op}}$
4. 重复直到收敛

好处：

• 无奇点（对比欧拉角）
• 每步只需解 6×6 系统（对比四元数+约束）
• 结果自动满足 $SE(3)$ 约束

---

8.6 概率：李群上的高斯分布 §

🎯 直觉：均值在流形上，协方差在切平面上 §

定义：$SO(3)$ 上的随机变量为 $\mathbf{C}=\exp ({\mathbf{ϵ}}^{\wedge })\bar{\mathbf{C}},\mathbf{ϵ}\sim \mathcal{N}(0,\mathbf{\Sigma })$

验证均值：$E[\ln (\mathbf{C}{\bar{\mathbf{C}}}^T{)}^{\vee }]=E[\mathbf{ϵ}]=0$

关键性质：

• $\bar{\mathbf{C}}$ 是”均值旋转”
• $\mathbf{\Sigma }$ 是普通的 $3\times 3$ 协方差矩阵
• 扰动 $\mathbf{ϵ}$ 小，远离 $2\pi$ 奇点

---

🔑 概念层：不确定性变换 §

确定性变换：若 ${\mathbf{C}}^{\prime }=\mathbf{RC}$（$\mathbf{R}$ 确定），则

• 均值：${\bar{\mathbf{C}}}^{\prime }=\mathbf{R}\bar{\mathbf{C}}$
• 协方差：${\mathbf{\Sigma }}^{\prime }=\mathbf{R}\mathbf{\Sigma }{\mathbf{R}}^T$（普通旋转协方差）

$SE(3)$ 版本：若 ${\mathbf{T}}^{\prime }=\mathbf{RT}$，协方差通过伴随变换 ${\mathbf{\Sigma }}^{\prime }=\mathcal{R}\mathbf{\Sigma }{\mathcal{R}}^T,\mathcal{R}=\mathrm{Ad}(\mathbf{R})$

注意：这是精确变换，无需线性化近似！

---

📐 推导层：旋转向量的期望偏差 §

展开：为什么 $E[\mathbf{C}\mathbf{x}] \neq \bar{\mathbf{C}}\mathbf{x}$

对 $\mathbf{C}=\exp ({\mathbf{ϵ}}^{\wedge })\bar{\mathbf{C}}$，展开到二阶： $\mathbf{C}\approx (1+{\mathbf{ϵ}}^{\wedge }+\frac{1}{2}{\mathbf{ϵ}}^{\wedge }{\mathbf{ϵ}}^{\wedge })\bar{\mathbf{C}}$

取期望，$E[{\mathbf{ϵ}}^{\wedge }]=0$，但 $E[{\mathbf{ϵ}}^{\wedge }{\mathbf{ϵ}}^{\wedge }]\ne 0$：

$E[\mathbf{Cx}]\approx \left(1+\frac{1}{2}E[{\mathbf{ϵ}}^{\wedge }{\mathbf{ϵ}}^{\wedge }]\right)\bar{\mathbf{C}}\mathbf{x}$

利用 $E[{\mathbf{ϵ}}^{\wedge }{\mathbf{ϵ}}^{\wedge }]=-\mathrm{tr}(\mathbf{\Sigma })1+\mathbf{\Sigma }$，得到：

$E[\mathbf{Cx}]\approx \left(1-\frac{1}{2}\mathrm{tr}(\mathbf{\Sigma })1+\frac{1}{2}\mathbf{\Sigma }\right)\bar{\mathbf{C}}\mathbf{x}$

这就是二阶”Jensen 偏差”——类似于非线性函数的期望不等于期望的函数。

---

8.7 运动学：时间上的李群积分 §

🎯 直觉：角速度积分得到旋转 §

泊松方程（李群形式）： $\dot{\mathbf{C}}={\mathbf{\omega }}^{\wedge }\mathbf{C}$

数值积分（恒定角速度）： $\mathbf{C}(t_2)=\exp ({\mathbf{\omega }}^{\wedge }\Delta t)\mathbf{C}(t_1)$

结果自动在 $SO(3)$ 中，无需后处理归一化！

---

🔑 概念层：$SE(3)$ 运动学 §

位姿运动学： $\dot{\mathbf{T}}={\mathbf{\varpi }}^{\wedge }\mathbf{T},\mathbf{\varpi }=[{\mathbf{\nu }}^T,{\mathbf{\omega }}^T{]}^T$

其中 $\mathbf{\nu }$ 是线速度，$\mathbf{\omega }$ 是角速度。

数值积分（恒定广义速度）： $\mathbf{T}(t_2)=\exp ({\mathbf{\varpi }}^{\wedge }\Delta t)\mathbf{T}(t_1)$

线性化扰动运动学： $\delta \dot{\mathbf{\xi }}={\mathbf{\varpi }}^{⋏}\delta \mathbf{\xi }+\delta \mathbf{\varpi }$

这是 EKF 中过程噪声传播的基础。

---

本章自检清单 §

阅读完本章，你应该能回答：

• 为什么旋转矩阵不能直接用向量加法？（不封闭）
• 李群的”两全其美”是什么？（均值在流形上，扰动在向量空间里）
• 指数映射的作用是什么？（连接李代数与李群）
• 左雅可比矩阵 $\mathbf{J}$ 的物理意义？（把李代数平移”拉伸”成真实平移）
• 伴随矩阵的作用？（协方差的精确坐标变换）
• 左扰动 Gauss-Newton 的三个好处？（无奇点、无约束、自动满足约束）

如果以上都清楚，你掌握了 Lie 群估计的核心！

---

延伸阅读与代码 §

Python 实现（核心函数）：

import numpy as np
 
def skew(v):
    """向量转反对称矩阵"""
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0]])
 
def unskew(M):
    """反对称矩阵转向量"""
    return np.array([M[2,1], M[0,2], M[1,0]])
 
def so3_exp(phi):
    """SO(3) 指数映射（Rodrigues 公式）"""
    phi_norm = np.linalg.norm(phi)
    if phi_norm < 1e-10:
        return np.eye(3)
    a = phi / phi_norm
    sin_phi = np.sin(phi_norm)
    cos_phi = np.cos(phi_norm)
    return (cos_phi * np.eye(3) +
            (1 - cos_phi) * np.outer(a, a) +
            sin_phi * skew(a))
 
def so3_log(C):
    """SO(3) 对数映射"""
    cos_phi = (np.trace(C) - 1) / 2
    cos_phi = np.clip(cos_phi, -1, 1)
    phi = np.arccos(cos_phi)
    if abs(phi) < 1e-10:
        return unskew(C - C.T) / 2
    return phi / (2 * np.sin(phi)) * unskew(C - C.T)
 
def left_jacobian_so3(phi):
    """SO(3) 左雅可比矩阵"""
    phi_norm = np.linalg.norm(phi)
    if phi_norm < 1e-10:
        return np.eye(3)
    a = phi / phi_norm
    sin_phi = np.sin(phi_norm)
    cos_phi = np.cos(phi_norm)
    return (sin_phi / phi_norm * np.eye(3) +
            (1 - sin_phi / phi_norm) * np.outer(a, a) +
            (1 - cos_phi) / phi_norm * skew(a))
 
def se3_exp(xi):
    """SE(3) 指数映射，xi = [rho; phi]"""
    rho, phi = xi[:3], xi[3:]
    C = so3_exp(phi)
    J = left_jacobian_so3(phi)
    r = J @ rho
    T = np.eye(4)
    T[:3, :3] = C
    T[:3, 3] = r
    return T
 
def se3_adjoint(T):
    """SE(3) 伴随矩阵"""
    C = T[:3, :3]
    r = T[:3, 3]
    Ad = np.zeros((6, 6))
    Ad[:3, :3] = C
    Ad[:3, 3:] = skew(r) @ C
    Ad[3:, 3:] = C
    return Ad
 
def gauss_newton_so3(C_op, v, target, max_iter=10):
    """
    用左扰动 GN 优化旋转使 C*v 接近 target
    C_op: 初始旋转矩阵 (3x3)
    v: 被旋转的向量
    target: 目标向量
    """
    C = C_op.copy()
    for _ in range(max_iter):
        residual = C @ v - target
        # 左李导数: - (C_op @ v)^wedge
        J = -skew(C @ v)
        # 求解线性系统
        delta_phi = np.linalg.solve(J.T @ J, -J.T @ residual)
        # 更新
        C = so3_exp(delta_phi) @ C
        if np.linalg.norm(delta_phi) < 1e-6:
            break
    return C

---

下一章将用这些工具解决三维位姿估计的经典问题。