第1章 数学基础I：线性代数与几何

机器人自主系统的核心挑战在于如何让机器人在未知或动态环境中感知、决策和执行。这一过程高度依赖于对空间位姿（pose）、运动变换和几何约束的精确数学描述。线性代数与微分几何为此提供了统一的语言和工具：从传感器数据中的点云配准到运动状态的估计，从轨迹优化到动力学控制，无处不在。本章将系统性地构建机器人学所需的线性代数与几何基础，重点聚焦于三维空间的旋转和平移表示。我们将从向量空间与矩阵运算的基本框架出发，深入探讨特征值分解（Eigenvalue Decomposition）与奇异值分解（Singular Value Decomposition, SVD）的几何内涵及其在机器人学中的典型应用。随后，本章将详细剖析三维旋转的三种主流表示——旋转矩阵（Rotation Matrix）、欧拉角（Euler Angles）与四元数（Quaternion），推导它们之间的相互转换关系，并客观比较其优缺点。最后，为了给后续基于优化的状态估计（如SLAM）奠定 rigorous 的数学基础，本章将引入李群（Lie Group）SO(3) 与李代数（Lie Algebra）se(3) 的概念，阐述其指数映射与对数映射的物理意义，揭示其在处理位姿增量更新与优化问题中的核心优势。通过本章的学习，读者将建立起一套完整、连贯且具有工程实用性的三维几何表示工具箱。

向量空间与矩阵运算

机器人学中，空间中的点、向量、力等物理量均可在向量空间中建模。设 $\mathbb{R}^n$ 为 n 维实向量空间，其元素为 n 元实数组。向量空间的核心结构由基（Basis） 定义：一组线性无关的向量 $\{\mathbf{b}_1, \mathbf{b}_2, ..., \mathbf{b}_n\}$ 若能张成整个空间，则称为一组基。任意向量 $\mathbf{v} \in \mathbb{R}^n$ 可唯一表示为基的线性组合：$\mathbf{v} = \sum_{i=1}^n c_i \mathbf{b}_i$，其中系数向量 $(c_1, c_2, ..., c_n)^T$ 即为 $\mathbf{v}$ 在该基下的坐标。标准正交基（$\mathbf{b}_i^T \mathbf{b}_j = \delta_{ij}$）因其计算简便而最常用，但根据问题背景选择特定基（如主轴方向）常能简化分析。

矩阵作为线性变换：矩阵 $\mathbf{A} \in \mathbb{R}^{m \times n}$ 定义了从 $\mathbb{R}^n$ 到 $\mathbb{R}^m$ 的线性映射 $\mathbf{y} = \mathbf{A} \mathbf{x}$。该映射保持向量加法和标量乘法：$\mathbf{A}(\mathbf{x}_1 + \mathbf{x}_2) = \mathbf{A}\mathbf{x}_1 + \mathbf{A}\mathbf{x}_2$，$\mathbf{A}(c\mathbf{x}) = c\mathbf{A}\mathbf{x}$。几何上，矩阵变换可分解为旋转、缩放、剪切等基本操作的复合。例如，对角矩阵 $\mathbf{\Lambda} = \text{diag}(\lambda_1, \lambda_2, \lambda_3)$ 沿坐标轴进行独立缩放；正交矩阵 $\mathbf{R}$（满足 $\mathbf{R}^T\mathbf{R}=\mathbf{I}$）表示纯旋转（或反射，若 $\det(\mathbf{R})=-1$）；而一般矩阵则可能混合这些效应。

```mermaid graph TD A[输入向量 x] --> B[线性变换 A] B --> C[输出向量 y = Ax] C --> D{变换类型?} D -->|正交矩阵 R| E[旋转/反射] D -->|对角矩阵 Λ| F[各向异性缩放] D -->|一般矩阵| G[旋转+缩放+剪切] ```

关键矩阵运算：

• **转置**：$\mathbf{A}^T$，行变列。对称矩阵满足 $\mathbf{A}=\mathbf{A}^T$。
• **逆**：$\mathbf{A}^{-1}$ 满足 $\mathbf{A}\mathbf{A}^{-1}=\mathbf{I}$，仅对方阵且满秩存在。逆矩阵对应变换的“撤销”。
• **行列式**：$\det(\mathbf{A})$，标量值。几何上，$|\det(\mathbf{A})|$ 表示变换前后体积的缩放比例；符号指示是否保持定向（$\det>0$ 保持，$\det<0$ 反转）。对于旋转矩阵，$\det(\mathbf{R})=1$。
• **迹**：$\text{tr}(\mathbf{A}) = \sum_i A_{ii}$，特征值之和。

在机器人学中，齐次坐标（Homogeneous Coordinates） 将 n 维向量扩展为 n+1 维，以统一表示旋转、平移及投影。例如，三维空间中的位姿（位置 $\mathbf{t} \in \mathbb{R}^3$ 与旋转 $\mathbf{R} \in SO(3)$）可合并为 $4 \times 4$ 变换矩阵： $$ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \in SE(3) $$ 点 $\mathbf{p} = (x, y, z)^T$ 表示为齐次向量 $(\mathbf{p}^T, 1)^T$，变换后 $\mathbf{p}' = \mathbf{T} (\mathbf{p}^T, 1)^T$。该表示使平移成为线性操作，极大简化了连续变换的链式乘法。

特征值分解与奇异值分解

矩阵分解旨在将复杂矩阵拆解为更简单、更具几何意义的因子，从而揭示其内在结构。

特征值分解 (Eigenvalue Decomposition)

对于方阵 $\mathbf{A} \in \mathbb{R}^{n \times n}$，若存在非零向量 $\mathbf{v}$ 和标量 $\lambda$ 使得 $\mathbf{A}\mathbf{v} = \lambda \mathbf{v}$，则 $\mathbf{v}$ 称为特征向量，$\lambda$ 称为特征值。特征向量在变换 $\mathbf{A}$ 下仅发生缩放，方向不变。将所有特征向量作为列构成矩阵 $\mathbf{V}$，特征值作为对角矩阵 $\mathbf{\Lambda}$ 的对角元，则当 $\mathbf{A}$ 有 n 个线性无关的特征向量时（如实对称矩阵），有： $$ \mathbf{A} = \mathbf{V} \mathbf{\Lambda} \mathbf{V}^{-1} $$ 若 $\mathbf{A}$ 对称（$\mathbf{A}=\mathbf{A}^T$），则 $\mathbf{V}$ 可取为正交矩阵（$\mathbf{V}^{-1}=\mathbf{V}^T$），即 $\mathbf{A} = \mathbf{V} \mathbf{\Lambda} \mathbf{V}^T$。

几何意义：特征值分解将线性变换 $\mathbf{A}$ 对角化，意味着在新基（特征向量方向）下，变换仅为各坐标轴的独立缩放。特征值大小指示了沿对应特征向量方向的“拉伸”或“压缩”程度。最大特征值对应的方向是数据方差最大的主轴方向。

机器人学应用： 1. 主成分分析 (PCA)：对点云或轨迹数据协方差矩阵进行特征值分解，提取主要运动方向或形状主轴。 2. 惯性张量对角化：刚体动力学中，惯性张量是对称矩阵，其特征向量给出惯量主轴，特征值为主惯量，使动力学方程解耦。 3. 振动分析：多自由度系统的特征值对应固有频率，特征向量为振型。

奇异值分解 (Singular Value Decomposition, SVD)

SVD 适用于任意矩阵 $\mathbf{A} \in \mathbb{R}^{m \times n}$（无需方阵），且总存在分解： $$ \mathbf{A} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^T $$ 其中：

• $\mathbf{U} \in \mathbb{R}^{m \times m}$ 和 $\mathbf{V} \in \mathbb{R}^{n \times n}$ 是正交矩阵（$\mathbf{U}^T\mathbf{U}=\mathbf{I}_m$, $\mathbf{V}^T\mathbf{V}=\mathbf{I}_n$）。
• $\mathbf{\Sigma} \in \mathbb{R}^{m \times n}$ 是“对角”矩阵，仅主对角线（可能非方阵）上有非负元素 $\sigma_1 \geq \sigma_2 \geq ... \geq \sigma_{\min(m,n)} \geq 0$，称为**奇异值**。

几何意义：SVD 揭示了任意线性变换的“标准型”。考虑单位球面 $\{\mathbf{x} : \|\mathbf{x}\|=1\}$ 在 $\mathbf{A}$ 下的像：$\mathbf{A}$ 首先通过 $\mathbf{V}^T$ 将球面旋转到新坐标系（$\mathbf{V}$ 的列向量是输入空间的主方向），然后沿新坐标轴独立缩放 $\sigma_i$（$\mathbf{\Sigma}$），最后再旋转（$\mathbf{U}$）到输出空间。奇异值 $\sigma_i$ 量化了变换在对应方向上的“强度”。秩 $r$ 等于非零奇异值的个数。

与特征值分解的关系：对于对称正定矩阵 $\mathbf{A}$，SVD 与特征值分解一致（$\mathbf{U}=\mathbf{V}$，$\mathbf{\Sigma}=\mathbf{\Lambda}$ 对角元为特征值）。但 SVD 更通用，且奇异值恒非负，数值更稳定。

机器人学应用： 1. 最小二乘问题：求解超定方程 $\mathbf{A}\mathbf{x} \approx \mathbf{b}$。SVD 给出最小范数最小二乘解 $\mathbf{x} = \mathbf{V} \mathbf{\Sigma}^+ \mathbf{U}^T \mathbf{b}$，其中 $\mathbf{\Sigma}^+$ 是 $\mathbf{\Sigma}$ 的伪逆（非零奇异值倒数，零奇异值对应零）。 2. 点云配准 (ICP)：计算两个点集间最优旋转平移时，常需对协方差矩阵进行 SVD 以提取旋转（见后续章节）。 3. 矩阵秩估计与数值稳定性：通过奇异值衰减情况判断矩阵是否病态，用于滤波或降维。 4. 手眼标定 (Hand-Eye Calibration)：求解 $\mathbf{AX} = \mathbf{XB}$ 形式方程，SVD 是经典解法的基础。

三维空间几何表示

在三维空间中，旋转是描述坐标系或刚体方向变化的核心操作。由于旋转矩阵有 9 个元素但仅 3 个自由度（受正交性和单位行列式约束），直接使用冗余参数进行优化会导致病态问题。因此，开发了多种紧凑表示，各有优劣。

旋转矩阵 (Rotation Matrix)

三维旋转矩阵 $\mathbf{R} \in \mathbb{R}^{3 \times 3}$ 满足： $$ \mathbf{R}^T \mathbf{R} = \mathbf{I}, \quad \det(\mathbf{R}) = 1 $$ 其列向量（或行向量）构成标准正交基，分别表示旋转后坐标系轴在原坐标系中的方向。旋转矩阵直接对应线性变换：$\mathbf{p}' = \mathbf{R} \mathbf{p}$。对于包含平移的刚体运动，使用 $4 \times 4$ 变换矩阵 $\mathbf{T} \in SE(3)$。

优点：

• 表示唯一（无歧义），计算直接，复合旋转即矩阵乘法。
• 几何直观，易于变换向量和坐标。

缺点：

• 冗余（9 参数 vs 3 自由度），优化时需施加约束（如 $\mathbf{R}^T\mathbf{R}-\mathbf{I}=0$），增加计算复杂度。
• 插值（如生成平滑旋转轨迹）需特殊处理（如使用四元数或对矩阵元素插值后重新正交化）。

欧拉角 (Euler Angles)

欧拉角用三个绕坐标轴旋转的角度序列描述旋转。常见顺序为 ZYX（航偏角 Yaw、俯仰角 Pitch、滚转角 Roll），即： 1. 绕全局 Z 轴旋转 $\psi$（yaw） 2. 绕新的 Y 轴旋转 $\theta$（pitch） 3. 绕新的 X 轴旋转 $\phi$（roll）

对应旋转矩阵为： $$ \mathbf{R}_{\text{ZYX}}(\phi, \theta, \psi) = \mathbf{R}_z(\psi) \mathbf{R}_y(\theta) \mathbf{R}_x(\phi) $$ 其中： $$ \mathbf{R}_x(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix},\quad \mathbf{R}_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix},\quad \mathbf{R}_z(\psi) = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} $$ 相乘后得： $$ \mathbf{R} = \begin{bmatrix} c_\psi c_\theta & c_\psi s_\theta s_\phi - s_\psi c_\phi & c_\psi s_\theta c_\phi + s_\psi s_\phi \\ s_\psi c_\theta & s_\psi s_\theta s_\phi + c_\psi c_\phi & s_\psi s_\theta c_\phi - c_\psi s_\phi \\ -s_\theta & c_\theta s_\phi & c_\theta c_\phi \end{bmatrix} $$ 其中 $c$ 和 $s$ 分别代表 $\cos$ 和 $\sin$。

优点：

• 直观，符合人类对“偏航、俯仰、滚转”的认知。
• 仅 3 个参数，紧凑。

缺点：

• **万向锁 (Gimbal Lock)**：当俯仰角 $\theta = \pm 90^\circ$ 时，绕 Z 和 X 的旋转轴重合，丢失一个自由度（此时 $\phi$ 和 $\psi$ 等价），导致奇异性。
• 表示不唯一：同一旋转可有多种欧拉角组合（尤其当某些角为特殊值时）。
• 插值不平滑：直接对角度线性插值可能导致非均匀旋转速度。
• 坐标顺序敏感：不同顺序（如 XYZ、ZYZ）有不同奇异性。

四元数 (Quaternion)

四元数是复数的扩展，形式为 $q = w + xi + yj + zk$，其中 $i,j,k$ 满足 $i^2=j^2=k^2=ijk=-1$。通常写作向量形式 $q = [s, \mathbf{v}]^T = [s, x, y, z]^T$，$s$ 为实部，$\mathbf{v} = (x,y,z)^T$ 为虚部向量。单位四元数（$\|q\|=1$）表示三维旋转： $$ q = \cos\frac{\theta}{2} + \sin\frac{\theta}{2} (u_x i + u_y j + u_z k) = [\cos\frac{\theta}{2}, \sin\frac{\theta}{2} \mathbf{u}]^T $$ 其中 $\theta$ 为旋转角，$\mathbf{u} \in \mathbb{R}^3$ 为单位旋转轴。

四元数乘法：对应旋转的复合。设 $q_1 = [s_1, \mathbf{v}_1]$，$q_2 = [s_2, \mathbf{v}_2]$，则： $$ q_1 \otimes q_2 = \begin{bmatrix} s_1 s_2 - \mathbf{v}_1^T \mathbf{v}_2 \\ s_1 \mathbf{v}_2 + s_2 \mathbf{v}_1 + \mathbf{v}_1 \times \mathbf{v}_2 \end{bmatrix} $$ 注意：四元数乘法不满足交换律，顺序至关重要（通常从右向左应用：$q_{\text{total}} = q_2 \otimes q_1$ 表示先旋转 $q_1$ 再旋转 $q_2$）。

四元数到旋转矩阵的转换：给定单位四元数 $q = [w, x, y, z]^T$，对应旋转矩阵为： $$ \mathbf{R}(q) = \begin{bmatrix} 1 - 2y^2 - 2z^2 & 2xy - 2wz & 2xz + 2wy \\ 2xy + 2wz & 1 - 2x^2 - 2z^2 & 2yz - 2wx \\ 2xz - 2wy & 2yz + 2wx & 1 - 2x^2 - 2y^2 \end{bmatrix} $$ 推导：利用 $q$ 与向量 $\mathbf{p}$ 的乘积 $q \otimes [0, \mathbf{p}]^T \otimes q^{-1}$ 的虚部即为 $\mathbf{Rp}$，展开后可得上述公式。

旋转矩阵到四元数：为避免数值不稳定，通常基于最大元素的方法。设 $\mathbf{R}$ 的迹 $t = \text{tr}(\mathbf{R})$，若 $t \geq 0$，则 $w = \frac{1}{2}\sqrt{1+t}$，$x = \frac{R_{32}-R_{23}}{4w}$，$y = \frac{R_{13}-R_{31}}{4w}$，$z = \frac{R_{21}-R_{12}}{4w}$；否则，找到最大对角元，类似计算。

优点：

• 紧凑（4 参数 vs 9），无万向锁奇异性。
• 插值平滑：球面线性插值 (Slerp) 可在