机器人学的数学基础：流形、李群与凸优化 / Mathematical Foundations: Manifolds, Lie Groups, and Convex Optimization

第 1 章 机器人学的数学基础：流形、李群与凸优化

Chapter 1: Mathematical Foundations: Manifolds, Lie Groups, and Convex Optimization

自主机器人在物理世界中运行时，其位姿（pose）、速度、控制输入与传感器不确定性均无法被欧氏空间 $\mathbb{R}^n$ 忠实地承载。一个刚体的方向属于三维旋转群 $SO(3)$；一个刚体在三维空间中的位姿属于特殊欧氏群 $SE(3)$；四旋翼的姿态估计需要在一个非交换且无"自然加法"的集合上融合加速度计、陀螺仪与视觉观测；轨迹规划与模型预测控制（MPC）则需要在线求解带状态与输入约束的优化问题，并要求全局收敛性与计算可追踪的迭代复杂度。本章的目的，是为后续章节中"感知—规划—控制"全栈建立统一的数学语言：流形（manifold）与李群（Lie group）用于表示构型与不确定性；凸优化（convex optimization）用于在受约束的结构化空间中寻找最优决策。本章由四个分析模块组成：光滑流形与切空间的基本几何；$SO(3)$、$SE(3)$ 及其李代数 $\mathfrak{so}(3)$、$\mathfrak{se}(3)$ 的群结构与指数/对数映射；流形上的高斯统计与协方差传播；以及凸优化的 KKT 条件、对偶性与二阶锥规划形式化。

1.1 从欧氏空间到光滑流形：构型空间的几何重建

From Euclidean Spaces to Smooth Manifolds: A Geometric Reconstruction of Configuration Space

1.1.1 欧氏范式的崩溃与拓扑动机

在经典力学与早期机器人学中，刚体的位置被建模为 $\mathbb{R}^3$ 中的一个向量 $\mathbf{p} \in \mathbb{R}^3$，姿态被建模为三个独立的 Euler 角 $(\phi,\theta,\psi) \in \mathbb{R}^3$。这一选择在表面上给予了工具的便利——线性代数、协方差矩阵、Jacobian 均"照常"工作——却在三个关键层面失效：第一，Euler 角参数化是局部同胚的，但是具有万向锁（gimbal lock）等奇异点，在奇异点附近 Jacobian 退化、数值优化中断；第二，最小化误差的简单加法 $\mathbf{p}_1 + \mathbf{p}_2 - \mathbf{p}_3$ 在紧致的旋转流形上没有意义，因为不存在使旋转成为向量空间的连续双射；第三，对位置与方向的不确定性联合建模需要乘积空间 $\mathbb{R}^3 \times SO(3)$，其自然的几何性质无法由 $\mathbb{R}^6$ 上的朴素高斯分布保留。

对上述问题的系统化解决始于将机器人的构型空间（configuration space, $\mathcal{C}$）视为一个拓扑空间，并进一步施加光滑结构。拓扑空间保留了"邻域"与"连续"的概念，但无法定义微积分；我们需要的数学对象是光滑流形（smooth manifold）。

1.1.2 光滑流形的精确定义

设 $M$ 为一个 Hausdorff、第二可数的拓扑空间。$M$ 上的一个光滑结构（smooth structure）由一族坐标图（charts） $\{(U_\alpha, \varphi_\alpha)\}$ 构成，满足：

1. $\{U_\alpha\}$ 覆盖 $M$，即 $M = \bigcup_\alpha U_\alpha$； 2. 每个 $\varphi_\alpha: U_\alpha \to \mathbb{R}^n$ 是到开集 $\varphi_\alpha(U_\alpha)$ 的同胚（homeomorphism）； 3. 在任意两个坐标图 $(U_\alpha, \varphi_\alpha)$ 与 $(U_\beta, \varphi_\beta)$ 的重叠区域 $U_\alpha \cap U_\beta \neq \emptyset$ 上，转移映射（transition map） $\varphi_{\alpha\beta} = \varphi_\alpha \circ \varphi_\beta^{-1}: \mathbb{R}^n \to \mathbb{R}^n$ 是光滑的。

整数 $n$ 对所有局部同胚于 $\mathbb{R}^n$ 的坐标图而言是相同的，称为 $M$ 的维数（dimension），记作 $\dim M$。对覆盖 $M$ 的所有相容坐标图的极大集合称为图册（atlas）。光滑流形的精确定义为：

Definition 1.1 (Smooth Manifold). 一个光滑流形是一个 Hausdorff、第二可数的拓扑空间 $M$，配上一个极大光滑图册。

机器人的构型空间几乎都是有限维光滑流形：刚性机械臂的 $\mathcal{C} \cong \mathbb{R}^n$（退化情形，但视作 $\mathbb{R}^n$ 上的标准光滑结构）；刚体在平面上的位姿 $\mathcal{C} \cong SE(2)$；刚体在三维空间的位姿 $\mathcal{C} \cong SE(3)$；一个闭链机构可能落在更复杂的代数簇（algebraic variety）上，例如 Bricard 闭链的可移动空间是 3 维的，但它并非线性空间。本书假设我们处理的所有 $\mathcal{C}$ 都可光滑地嵌入到某 $SE(3)$ 的子流形中。

1.1.3 切空间：从方向到导数

流形上的微积分发生在切空间（tangent space）$T_x M$ 中。$T_x M$ 可以通过三种等价方式定义：

1. 几何定义：$T_x M$ 是 $M$ 在 $x$ 点所有光滑曲线的等价类，对曲线 $\gamma: (-\epsilon, \epsilon) \to M$ 满足 $\gamma(0) = x$ 的速度向量定义为 $[\gamma]$。 2. 代数定义：$T_x M$ 是 $x$ 处所有光滑函数 $f \in C^\infty(M)$ 上的局部光滑函数芽（germ）构成的实向量空间，其对偶 $T_x^* M$ 是余切空间（cotangent space）。 3. 浸没定义：若 $\varphi: U \subset M \to \mathbb{R}^n$ 是某坐标图，则 $T_x M$ 与 $\varphi_*(T_x M) = \mathbb{R}^n$ 自然同构。

维数为 $n$ 的流形在每点具有 $n$ 维切空间。群作用于流形意味着每个群元携带一个自然的切空间映射，这正是李群行为的代数源头。

1.1.4 为何机器人学必须采用流形语言

流形语言对机器人学的价值不止是形式上的严谨性。其工程价值可归纳为三点：

1. 消除参数化奇异性：用 $SO(3)$ 上的旋转矩阵替代 Euler 角，不存在 gimbal lock；用四元数（unit quaternion）作局部参数化可以表达任意姿态，但四元数与 $SO(3)$ 的覆盖关系是 2:1，需要保持规范化。 2. 保持几何运算的封闭性：旋转的复合是旋转矩阵的乘法，平移与旋转的复合是 $SE(3)$ 的群乘法。这避免了 Euler 角加法在远离原点时的不可加性问题。 3. 支撑切空间统计量：在滤波器（如 EKF、IEKF）中把误差建模为切空间中的零均值高斯变量，可以保证协方差矩阵的正定性，并通过指数映射自然地得到流形上的更新。

```mermaid flowchart LR A["se(3) xi in R^6"] -->|"exp(xi)"| B["SE(3) T"] B -->|"log(T)"| A C["Covariance P"] -->|"P_<--Jl(xi)*P*Jl(xi)^T"| D["Manifold Covariance"] B -->|"Left Jacobian Jl"| C ```

图 1.1 — 李群 $SE(3)$ 与李代数 $\mathfrak{se}(3)$ 的指数-对数映射以及协方差通过左雅可比 $J_l$ 在切空间与流形之间的传播路径。

1.2 $SO(3)$、$SE(3)$ 及其李代数：群结构与指数/对数映射

$SO(3)$, $SE(3)$ and Their Lie Algebras: Group Structure and Exponential/Logarithm Maps

1.2.1 特殊正交群 $SO(3)$

三维旋转群定义为：

$$SO(3) := \left\{ R \in \mathbb{R}^{3 \times 3} \,\middle|\, R^\top R = I_3, \det R = 1 \right\}.$$

$SO(3)$ 是一个 3 维紧致连通李群（compact connected Lie group）。其光滑流形结构由 Frobenius 范数诱导的拓扑给出，群运算为矩阵乘法 $R_1 \cdot R_2$，单位元为 $I_3$，逆为 $R^\top$。

对应地，反对称矩阵的集合定义了李代数 $\mathfrak{so}(3)$：

$$\mathfrak{so}(3) := \left\{ [\omega]_\times \in \mathbb{R}^{3 \times 3} \,\middle|\, [\omega]_\times^\top = -[\omega]_\times \right\},$$

其中 hat 算子 $[\cdot]_\times: \mathbb{R}^3 \to \mathfrak{so}(3)$ 定义为：

$$[\omega]_\times := \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix}, \quad \omega = (\omega_1, \omega_2, \omega_3)^\top.$$

向量与反对称矩阵之间的对合映射写为 $\vee$：$[\omega]_\times^\vee = \omega$。对任意 $\omega \in \mathbb{R}^3$ 与 $u, v \in \mathbb{R}^3$，反对称矩阵的关键性质为 $[\omega]_\times u = \omega \times u$，且 Jacobi 恒等式 $[u]_\times [v]_\times - [v]_\times [u]_\times = [\omega]_\times$（其中 $\omega = u \times v$）成立，这恰是 $\mathfrak{so}(3)$ 构成李代数的核心。

1.2.2 $\mathfrak{so}(3)$ 到 $SO(3)$ 的指数映射（Rodrigues 公式）

指数映射将李代数中的反对称矩阵映射到群中的旋转矩阵。其推导基于矩阵指数的封闭形式（Rodrigues 公式）：

$$\exp([\omega]_\times) = I + \frac{\sin\theta}{\theta} [\omega]_\times + \frac{1 - \cos\theta}{\theta^2} [\omega]_\times^2,$$

其中 $\theta = \|\omega\|_2$。当 $\theta = 0$ 时退化为 $I$。证明基于 $[\omega]_\times^3 = -\theta^2 [\omega]_\times$（来自向量叉乘的 Jacobi 恒等式），将 $e^{[\omega]_\times}$ 的 Taylor 级数按 $[\omega]_\times^0, [\omega]_\times^1, [\omega]_\times^2$ 分组后求和即得。这一公式具有三个工程含义：

1. 数值稳定性：当 $\theta$ 非常小时，$\sin\theta/\theta$ 与 $(1-\cos\theta)/\theta^2$ 接近奇异性，应使用 Taylor 展开的数值稳定版本（详见 1.2.5 节代码示例）。 2. 物理含义：$\omega = \theta u$ 对应绕单位轴 $u$ 旋转 $\theta$ 弧度，因此指数映射在"旋转的角速度表示"与"旋转的物理意义"之间建立了最直接的桥梁。 3. 可逆性：对数映射 $\log: SO(3) \to \mathfrak{so}(3)$ 由 $R$ 的 trace 反求 $\cos\theta = (\text{tr}\,R - 1)/2$，再由反对称部分 $(R - R^\top)/(2\sin\theta)$ 恢复轴。这是 4 元数与旋转矩阵转换的几何本质。

1.2.3 特殊欧氏群 $SE(3)$ 与李代数 $\mathfrak{se}(3)$

$SE(3)$ 描述刚体在三维空间中的位姿（pose）：

$$SE(3) := \left\{ T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \,\middle|\, R \in SO(3),\ p \in \mathbb{R}^3 \right\},$$

维数为 6，群运算为：

$$T_1 \cdot T_2 = \begin{bmatrix} R_1 R_2 & R_1 p_2 + p_1 \\ 0 & 1 \end{bmatrix}.$$

对应的李代数 $\mathfrak{se}(3)$ 由 6 维向量 $\xi = (\rho, \phi)^\top \in \mathbb{R}^6$ 参数化（这里使用 split 形式 $\rho \in \mathbb{R}^3$ 对应平移部分，$\phi \in \mathbb{R}^3$ 对应旋转部分）。Hat 算子定义为：

$$[\xi]_\wedge := \begin{bmatrix} [\phi]_\times & \rho \\ 0 & 0 \end{bmatrix} \in \mathbb{R}^{4 \times 4}.$$

关键代数性质：$\mathfrak{se}(3)$ 是一个 6 维李代数，李括号（Lie bracket）由 adjoint 形式表示——这一构造在形式上比 $\mathfrak{so}(3)$ 的叉乘更复杂，源于 $SE(3)$ 的"半直积"（semidirect product）本质 $SE(3) \cong \mathbb{R}^3 \rtimes SO(3)$。

1.2.4 $\mathfrak{se}(3)$ 到 $SE(3)$ 的指数映射

指数映射 $\exp: \mathfrak{se}(3) \to SE(3)$ 的封闭形式由 Barfoot 等人系统化。设 $\xi = (\rho, \phi)^\top$，$\theta = \|\phi\|_2$，$V \in \mathbb{R}^{3 \times 3}$ 为左雅可比的积分形式：

$$V = I + \frac{1 - \cos\theta}{\theta^2} [\phi]_\times + \frac{\theta - \sin\theta}{\theta^3} [\phi]_\times^2,$$

指数映射为：

$$\exp([\xi]_\wedge) = \begin{bmatrix} \exp([\phi]_\times) & V \rho \\ 0 & 1 \end{bmatrix}.$$

对数映射（$SE(3) \to \mathfrak{se}(3)$）由下式恢复：

$$\phi = \log(R), \quad \rho = V^{-1} p,$$

其中 $V^{-1}$ 由 $(V \theta^2 + \theta [\phi]_\times + [\phi]_\times^2)/(\theta^2 (\theta - \sin\theta))$ 等公式给出（在 $\theta$ 极小时的极限情形需另行推导）。这些公式在 Sophus 与 Okvis 等机器人开源库中均有闭包实现，是工程实践的关键工具。

1.2.5 关键设计判断：为什么"加法"是错误的更新范式

一个经常被忽视但工程上致命的错误范式是：给定当前位姿估计 $T \in SE(3)$ 与观测残差 $\delta \in \mathbb{R}^6$，将更新写为 $T \leftarrow T + \delta$。这条路径有两个不可修复的缺陷：

1. 群结构的破坏：$SE(3)$ 对加法不封闭，$T + \delta$ 既不满足 $R^\top R = I$ 的约束，也未保持齐次坐标形式，几何上无意义。 2. 不确定性传播的破坏：加法意味着协方差矩阵 $P$ 在 $\mathbb{R}^{12}$ 中传播，远超切空间的 6 维，导致矩阵病态与不正定。

正确的范式是左扰动更新（或右扰动更新）：

$$T \leftarrow \exp(\delta^\wedge) \cdot T, \quad \delta \in \mathbb{R}^6,$$

其中 $\delta$ 是切空间中的局部坐标。协方差矩阵保持在 $\mathbb{R}^{6 \times 6}$ 中，并通过左雅可比 $J_l(\xi) := \frac{\partial \exp(\xi)}{\partial \xi}$ 在需要时映射到流形邻域。这一范式在 Barfoot 的 *State Estimation for Robotics* 与 Sola 的 *Quaternion kinematics for the error-state Kalman filter* 中有详尽论述。

1.2.6 Adjoint 表示与雅可比

$SE(3)$ 的伴随表示（adjoint representation）$\text{Ad}: SE(3) \to \mathbb{R}^{6 \times 6}$ 将群元线性化作用于李代数：

$$\text{Ad}(T) = \begin{bmatrix} R & [p]_\times R \\ 0 & R \end{bmatrix} \in \mathbb{R}^{6 \times 6}.$$

伴随的李代数版本 $\mathfrak{ad}: \mathfrak{se}(3) \to \mathbb{R