机器人数学基础I：线性代数与流形

在机器人学中，精确描述刚体运动——尤其是旋转——是许多核心问题的基础。传统的欧几里得空间框架在处理旋转时显得力不从心，因为旋转集合不具有线性结构，而是形成一个紧致、不可交换的流形。为了在保持几何精确性的同时进行有效的计算，我们需要借助李群和李代数这一数学工具。本章将首先回顾线性代数中与机器人学密切相关的核心概念，如特征值分解和奇异值分解，然后系统性地引入李群SO(3)与SE(3)及其李代数so(3)与se(3)，深入探讨指数映射、对数映射以及伴随表示，最终阐明这些概念在状态估计中的关键作用。这些数学构造不仅是理解传感器融合、运动规划和控制的基石，也是实现高精度定位与建图（如SLAM）的核心。

1.1 线性代数核心概念复习

机器人学中的许多问题最终归结为线性代数运算，从点云配准到运动估计，从降维到优化。然而，并非所有线性代数工具都同等重要。本节聚焦于两个最常出现且具有深刻几何意义的分解：特征值分解（Eigenvalue Decomposition）和奇异值分解（Singular Value Decomposition, SVD）。我们将复习其数学原理，并通过机器人学中的具体实例展示其应用。

1.1.1 特征值分解与主成分分析

对于一个方阵 $A \in \mathbb{R}^{n \times n}$，如果存在非零向量 $v \in \mathbb{R}^n$ 和标量 $\lambda \in \mathbb{C}$ 使得 $A v = \lambda v$，则称 $\lambda$ 为特征值，$v$ 为对应的特征向量。当 $A$ 是实对称矩阵（$A = A^\top$）时，其特征值均为实数，且不同特征值对应的特征向量相互正交。此时，特征值分解可写为 $A = Q \Lambda Q^\top$，其中 $Q$ 是正交矩阵（$Q^\top Q = I$），其列向量为特征向量，$\Lambda$ 是实对角矩阵，对角线元素为特征值。

几何意义：特征值分解揭示了线性变换 $A$ 在特征向量方向上的缩放因子。最大特征值对应的方向是数据方差最大的方向，最小特征值对应的方向是方差最小的方向。这一性质在主成分分析（PCA）中得到直接应用。

机器人学应用：点云主方向估计与姿态初始化

在三维点云处理中，PCA常用于估计点云的主方向（即点云分布的三个主轴），这在物体识别、抓取点选取以及初始姿态猜测中至关重要。给定一个点集 $\{p_i \in \mathbb{R}^3\}_{i=1}^N$，我们首先计算其协方差矩阵： $$ \Sigma = \frac{1}{N} \sum_{i=1}^N (p_i - \bar{p})(p_i - \bar{p})^\top, \quad \bar{p} = \frac{1}{N} \sum_{i=1}^N p_i. $$ 对 $\Sigma$ 进行特征值分解，得到特征值 $\lambda_1 \geq \lambda_2 \geq \lambda_3 \geq 0$ 及对应的正交单位特征向量 $e_1, e_2, e_3$。特征向量 $e_1$ 指向点云延伸最长的方向（第一主成分），$e_3$ 指向最短方向。例如，对于一个圆柱形物体的点云，$e_1$ 沿圆柱轴线方向，$e_2$ 和 $e_3$ 在横截面内。这一信息可用于初始化物体姿态估计，或作为后续配准算法的先验。

数值稳定性：在实际计算中，当点云分布接近平面时（$\lambda_3 \approx 0$），特征向量 $e_3$ 的估计可能不稳定。此时常通过添加小扰动或使用SVD替代，因为SVD对病态矩阵更鲁棒。

1.1.2 奇异值分解与最小二乘问题

对于任意矩阵 $A \in \mathbb{R}^{m \times n}$（不一定为方阵），奇异值分解将 $A$ 分解为三个矩阵的乘积：$A = U \Sigma V^\top$，其中 $U \in \mathbb{R}^{m \times m}$ 和 $V \in \mathbb{R}^{n \times n}$ 是正交矩阵，$\Sigma \in \mathbb{R}^{m \times n}$ 是实对角矩阵，对角线元素 $\sigma_1 \geq \sigma_2 \geq \cdots \geq \sigma_{\min(m,n)} \geq 0$ 称为奇异值。奇异值平方后等于 $A^\top A$ 的特征值，左奇异向量 $U$ 的列是 $A A^\top$ 的特征向量，右奇异向量 $V$ 的列是 $A^\top A$ 的特征向量。

几何意义：SVD揭示了线性变换 $A$ 将空间中的正交基（$V$ 的列）映射到另一组正交基（$U$ 的列），并在每个方向上按奇异值缩放。它提供了矩阵的“最优”低秩近似（通过保留前 $k$ 个奇异值），在数据压缩和噪声过滤中广泛应用。

机器人学应用：点云配准中的ICP算法

迭代最近点（ICP）算法是点云配准的经典方法，其核心是在已知对应关系下求解最优刚体变换 $(R, t)$，使得 $\min_{R \in \mathrm{SO(3)}, t \in \mathbb{R}^3} \sum_{i=1}^N \| R p_i + t - q_i \|^2$。当对应点集已知时，问题可分解为旋转和平移的求解。旋转部分可通过SVD高效求解。

给定两组已配对的点云 $\{p_i\}$ 和 $\{q_i\}$，定义去中心化点集： $$ \tilde{p}_i = p_i - \bar{p}, \quad \tilde{q}_i = q_i - \bar{q}, \quad \bar{p} = \frac{1}{N}\sum p_i, \quad \bar{q} = \frac{1}{N}\sum q_i. $$ 构造矩阵 $H = \sum_{i=1}^N \tilde{p}_i \tilde{q}_i^\top \in \mathbb{R}^{3 \times 3}$。则最优旋转 $R^*$ 由 $H$ 的SVD给出：设 $H = U \Sigma V^\top$，则 $R^* = V U^\top$。若 $\det(R^*) = -1$（即包含反射），则需将 $V$ 的最后一列取反后重新计算。最优平移为 $t^* = \bar{q} - R^* \bar{p}$。

推导简述：目标函数展开后，旋转和平移解耦。固定 $R$ 时，最优 $t$ 为 $\bar{q} - R \bar{p}$。代入后目标函数化为 $\sum \| \tilde{q}_i - R \tilde{p}_i \|^2 = \sum (\|\tilde{q}_i\|^2 + \|\tilde{p}_i\|^2) - 2 \sum \langle \tilde{q}_i, R \tilde{p}_i \rangle$。最大化 $\sum \langle \tilde{q}_i, R \tilde{p}_i \rangle = \mathrm{tr}(R H)$。由于 $\mathrm{tr}(R H) = \mathrm{tr}(U^\top R V \Sigma)$ 且 $|\mathrm{tr}(AB)| \leq \sum \sigma_i(A)\sigma_i(B)$，等号成立当 $U^\top R V = I$，即 $R = V U^\top$（需确保 $\det(R)=1$）。

代码示例：以下Python函数实现了基于SVD的刚性配准： ```python import numpy as np

def svd_rigid_registration(P, Q): """ 求解从点集P到点集Q的最优刚体变换 (R, t) 输入: P, Q 为 3xN 的numpy数组，每列一个点 输出: R (3x3), t (3x1) """ assert P.shape == Q.shape and P.shape[0] == 3 N = P.shape[1]

计算质心

centroid_P = np.mean(P, axis=1, keepdims=True) centroid_Q = np.mean(Q, axis=1, keepdims=True)

去中心化

P_centered = P - centroid_P Q_centered = Q - centroid_Q

计算协方差矩阵 H

H = P_centered @ Q_centered.T

SVD分解

U, S, Vt = np.linalg.svd(H) V = Vt.T

计算旋转

R = V @ U.T

处理反射情况

if np.linalg.det(R) < 0: V[:, 2] *= -1 R = V @ U.T

计算平移

t = centroid_Q - R @ centroid_P return R, t ```

局限性：SVD方法假设对应关系已知且正确。在实际ICP中，对应关系通过最近邻搜索迭代确定，可能引入错误匹配，导致H矩阵污染。此时需要采用鲁棒核函数或概率模型。

1.1.3 矩阵函数与指数对数映射（线性代数视角）

在后续讨论李群时，我们将频繁遇到矩阵指数和矩阵对数。这里先在线性代数框架下介绍。

对于方阵 $A \in \mathbb{R}^{n \times n}$，其矩阵指数定义为收敛的泰勒级数： $$ \exp(A) = \sum_{k=0}^{\infty} \frac{A^k}{k!} = I + A + \frac{1}{2}A^2 + \frac{1}{6}A^3 + \cdots. $$ 矩阵对数则是矩阵指数的逆，对于满足 $\det(\exp(A)) > 0$ 且特征值不在负实轴的矩阵 $B$，存在唯一矩阵 $A$ 使得 $\exp(A) = B$，记为 $A = \log(B)$。

性质：若 $A$ 与 $B$ 可交换（$AB = BA$），则 $\exp(A+B) = \exp(A)\exp(B)$；否则不成立。矩阵指数将任意方阵映射到可逆矩阵（$\det(\exp(A)) = e^{\mathrm{tr}(A)} > 0$）。对于反对称矩阵 $A^\top = -A$，$\exp(A)$ 是正交矩阵（$(\exp(A))^\top \exp(A) = I$），这直接关联到旋转矩阵。

简单例子：考虑二维旋转矩阵 $R(\theta) = \begin{pmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{pmatrix}$。其对应的李代数为 $so(2) = \{ A \in \mathbb{R}^{2 \times 2} \mid A^\top = -A \}$，一般形式为 $A = \begin{pmatrix} 0 & -\omega \\ \omega & 0 \end{pmatrix}$，其中 $\omega \in \mathbb{R}$。计算矩阵指数： $$ \exp(A) = I + A + \frac{A^2}{2!} + \frac{A^3}{3!} + \cdots = \begin{pmatrix} 1 - \frac{\omega^2}{2!} + \frac{\omega^4}{4!} - \cdots & -\omega + \frac{\omega^3}{3!} - \frac{\omega^5}{5!} + \cdots \\ \omega - \frac{\omega^3}{3!} + \frac{\omega^5}{5!} - \cdots & 1 - \frac{\omega^2}{2!} + \frac{\omega^4}{4!} - \cdots \end{pmatrix} = \begin{pmatrix} \cos\omega & -\sin\omega \\ \sin\omega & \cos\omega \end{pmatrix} = R(\omega). $$ 因此，$\theta = \omega$，即旋转角度等于李代数元素的“长度”。矩阵对数则给出 $\log(R(\theta)) = \begin{pmatrix} 0 & -\theta \\ \theta & 0 \end{pmatrix}$。这一对应在三维中将更复杂，但思想一致：李代数元素（小扰动）通过指数映射生成李群元素（旋转），而对数映射从旋转提取李代数表示。

这些线性代数工具为理解流形上的运算奠定了基础。然而，旋转集合 $\mathrm{SO(3)}$ 本身不是线性空间，我们不能直接对其做加法或标量乘法。李群和李代数提供了在保持几何结构的同时进行线性化计算的框架。

1.2 流形与李群基础

1.2.1 从欧几里得空间到流形

欧几里得空间 $\mathbb{R}^n$ 具有全局线性坐标，任意两点间有唯一直线距离，向量可自由加减。但许多机器人学中的集合——如旋转矩阵集合 $\mathrm{SO(3)}$、位姿集合 $\mathrm{SE(3)}$——不具备这些性质。它们局部类似 $\mathbb{R}^n$（允许微分和切空间定义），但全局拓扑可能复杂（如 $\mathrm{SO(3)}$ 拓扑同胚于实射影空间 $\mathbb{RP}^3$，而非 $\mathbb{R}^3$）。这类空间称为流形（Manifold）。

形式定义：一个 $n$ 维流形是一个拓扑空间，其中每一点都有一个邻域同胚于 $\mathbb{R}^n$ 的开集。同胚映射称为坐标卡（Chart），所有坐标卡的集合构成图册（Atlas）。流形上可定义光滑函数、切向量等。

例子：二维球面 $S^2 = \{ x \in \mathbb{R}^3 \mid \|x\|=1 \}$ 是二维流形。在北极和南极附近，可用球坐标或平面投影作为坐标卡，但单一坐标卡无法覆盖整个球面（如墨卡托投影在极点处失效）。

1.2.2 李群：群结构与流形的结合

李群（Lie Group）是一个同时具有群结构和光滑流形结构的数学对象，且群运算（乘法 $G \times G \to G$ 和逆 $G \to G$）是光滑映射。

关键性质： 1. 群公理：封闭性、结合律、单位元、逆元。 2. 光滑性：群运算无限可微，允许使用微积分工具。 3. 局部欧几里得：每点附近有坐标卡，群运算局部可线性化。

机器人学中最常见的李群：

• **特殊正交群** $\mathrm{SO(3)} = \{ R \in \mathbb{R}^{3 \times 3} \mid R^\top R = I, \det(R)=1 \}$。描述三维旋转。维度为3，单位元为 $I_{3 \times 3}$。
• **特殊欧几里得群** $\mathrm{SE(3)} = \{ T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \mid R \in \mathrm{SO(3)}, t \in \mathbb{R}^3 \}$。描述三维刚体变换（旋转+平移）。维度为6，单位元为 $I_{4 \times 4}$。

群运算：$\mathrm{SO(3)}$ 中为矩阵乘法，$\mathrm{SE(3)}$ 中为变换矩阵乘法。例如，连续两次旋转 $R_1, R_2$ 的合成是 $R = R_2 R_1$（注意顺序，通常右乘表示新坐标系到旧坐标系的变换）。

为什么李群？ 旋转和刚体变换满足群公理：连续两次变换仍是变换（封闭性），变换可逆（逆元为单位变换），结合律成立。同时，它们作为 $\mathbb{R}^{3 \times 3}$ 或 $\mathbb{R}^{4 \times 4}$ 的子集是光滑流形（多项式方程定义的闭子集）。

1.2.3 李代数：李群的切空间

李群的李代数（Lie Algebra）是群单位元处的切空间，记为 $\mathfrak{g} = T_e G$，配备一个额外的运算——李括号（Lie Bracket）$[\cdot, \cdot]: \mathfrak{g} \times \mathfrak{g} \to \mathfrak{g}$，使其成为李代数。

定义：对于李群 $G$，其李代数 $\mathfrak{g}$ 由所有从单位元出发的光滑曲线 $\gamma(t)$ 的切向量组成，即 $\mathfrak{g} = \{ \dot{\gamma}(0) \mid \gamma(0)=e, \gamma: (-\epsilon,\epsilon) \to G \text{ 光滑} \}$。李括号由交换子定义：$[X, Y] = XY - YX$（对于矩阵李群）。

$\mathrm{SO(3)}$ 的李代数 $\mathfrak{so(3)}$： $$ \mathfrak{so(3)} = \{ \phi \in \mathbb{R}^{3 \times 3} \mid \phi^\top = -\phi \}. $$ 即所有三维反对称矩阵的集合。维度为3。一般参数化为： $$ \phi = \phi^\wedge = \begin{pmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{pmatrix}, \quad \phi = (\phi_1, \phi_2, \phi_3)^\top \in \mathbb{R}^3. $$ 这里 $\wedge$ 操作将三维向量映射到反对称矩阵，其逆 $\vee$ 将反对称矩阵映射回向量（$\phi^\vee = (\phi_{32}, \phi_{13}, \phi_{21})^\top$，注意索引顺序）。

$\mathrm{SE(3)}$ 的李代数 $\mathfrak{se(3)}$： $$ \mathfrak{se(3)} = \left\{ \xi = \begin{bmatrix} \phi^\wedge & \rho \\ 0 & 0 \end{bmatrix} \mid \phi \in \mathbb{R}^3, \rho \in \mathbb{R}^3 \right\}. $$ 其中 $\phi^\wedge \in \mathfrak{so(3)}