机器人学数学基础：线性代数、概率论与凸优化精要

机器人学是一门建立在数学基石上的工程科学。当我们在设计一个能够自主导航、操作物体并与环境实时交互的机器人系统时，所面临的每一个核心挑战——从感知 noisy 的传感器数据、理解自身在空间中的状态，到规划安全高效的轨迹并精确执行——最终都归结为一系列数学问题的建模与求解。线性代数提供了描述空间变换、系统状态和约束的语言；概率论是处理不确定性和进行数据融合的理论框架；而凸优化则是将复杂的规划与控制问题转化为可高效求解的数学模型的强大工具。本章并非简单的数学公式罗列，而是聚焦于这些工具在机器人学语境下的核心原理与工程视角，通过严格的推导和具体的机器人学案例，为后续章节的状态估计、运动规划与反馈控制构建一个坚实、连贯且实用的理论基础。我们将看到，贝叶斯定理如何统一传感器融合，奇异值分解（SVD）如何揭示运动学本质，而KKT条件又如何成为轨迹优化问题的“判决书”。

1.1 线性代数：刚体运动与系统描述的数学语言

线性代数是机器人学中描述几何变换、系统动态和约束的最基本语言。其核心价值在于将复杂的空间关系、物理定律和优化问题，转化为清晰、可计算的矩阵与向量运算。

1.1.1 矩阵运算基础与几何意义

矩阵乘法 $C = AB$ 在机器人学中远不止是数值运算。其几何意义是线性变换的复合。若矩阵 $A$ 代表一个变换（如旋转），矩阵 $B$ 代表另一个变换（如平移），则乘积 $AB$ 表示先应用 $B$ 变换，再应用 $A$ 变换。这一顺序至关重要，因为变换通常是非交换的（$AB \neq BA$）。例如，一个机械臂末端执行器在基坐标系下的位姿，是通过从基座到肩部、肘部、腕部等一系列关节变换的右乘顺序累积得到的。理解这一顺序是避免运动学模型错误的关键。

特征值与特征向量 $\mathbf{A}\mathbf{v} = \lambda \mathbf{v}$ 揭示了线性变换的“本征”方向。在机器人动力学中，系统的主模态（如机械臂的固有振动模式）常通过质量矩阵和刚度矩阵的特征值问题求解。特征值 $\lambda$ 的平方根关联到固有频率，特征向量 $\mathbf{v}$ 描述了该模态下各质量点的相对运动形态。这对于避免共振、设计控制器至关重要。

奇异值分解（SVD） $\mathbf{A} = \mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^T$ 是线性代数中最为强大的工具之一。它将任意矩阵 $\mathbf{A}$ 分解为旋转（$\mathbf{U}, \mathbf{V}$）和拉伸（$\boldsymbol{\Sigma}$，奇异值对角阵）的复合。在机器人学中，SVD 的应用无处不在：

• **运动学雅可比矩阵的秩分析**：雅可比矩阵 $\mathbf{J}(\mathbf{q})$ 将关节速度 $\dot{\mathbf{q}}$ 映射为末端执行器速度 $\dot{\mathbf{x}}$。对 $\mathbf{J}$ 进行SVD，$\mathbf{U}$ 的列是末端执行器速度空间的正交基，$\mathbf{V}$ 的列是关节速度空间的正交基，而奇异值 $\sigma_i$ 则精确量化了沿 $\mathbf{U}$ 对应方向的运动**传递比**。奇异值接近零，意味着该末端方向需要极大的关节速度（甚至无穷大），即处于**奇异位形**，此时机器人失去一个或多个自由度，控制失效。SVD 提供了分析奇异性的系统化方法，而不仅仅是求解 $\det(\mathbf{J})=0$。
• **点云配准与SLAM**：在迭代最近点（ICP）算法中，求解两个匹配点集间最优旋转和平移时，核心步骤即是对协方差矩阵进行SVD，从中直接提取最优旋转矩阵。
• **力/位混合控制**：在任务的笛卡尔空间中进行分解时，SVD 可用于将期望的力/速度指令投影到雅可比矩阵的零空间和值空间，实现优先级控制。

1.1.2 刚体变换：从旋转矩阵到齐次坐标

描述一个刚体在空间中的位姿（位置与姿态）需要6个自由度：3个平移，3个旋转。其数学表达必须满足刚体变换的性质：保持距离和角度不变。

旋转矩阵 $\mathbf{R} \in SO(3)$ 是一个 $3 \times 3$ 正交实矩阵（$\mathbf{R}^T\mathbf{R} = \mathbf{I}, \det(\mathbf{R})=+1$）。它直接对应坐标系间的基向量变换。若向量 $\mathbf{v}_B$ 在坐标系 $\{B\}$ 中表达，其在基坐标系 $\{A\}$ 中的表达为 $\mathbf{v}_A = \mathbf{R}_{A}^{B} \mathbf{v}_B$。$\mathbf{R}_{A}^{B}$ 的列向量即为 $\{B\}$ 的各轴在 $\{A\}$ 中的坐标。旋转矩阵的几何意义清晰，乘法即变换复合，但其有9个元素却只有3个自由度，存在冗余，且不便于插值和求导。

四元数 $\mathbf{q} = [q_w, q_x, q_y, q_z]^T$ 是表达旋转的紧凑、无奇异形式。单位四元数对应 $SO(3)$ 上的双覆盖映射。其乘法对应旋转复合，插值（如球面线性插值，Slerp）能生成平滑的旋转轨迹。四元数更新方程 $\dot{\mathbf{q}} = \frac{1}{2} \Omega(\boldsymbol{\omega}) \mathbf{q}$（其中 $\Omega(\boldsymbol{\omega})$ 由角速度 $\boldsymbol{\omega}$ 构造）是惯性导航和视觉里程计中姿态积分的基础。虽然其乘法不满足交换律，且直观性稍弱，但其数值稳定性和效率使其成为工程实现的首选。

齐次变换矩阵 将旋转与平移统一于一个 $4 \times 4$ 矩阵： $$ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{p} \\ \mathbf{0}^T & 1 \end{bmatrix} $$ 其中 $\mathbf{p}$ 为平移向量。点 $\mathbf{P}_B = [x_B, y_B, z_B, 1]^T$ 在 $\{A\}$ 中的齐次坐标为 $\mathbf{P}_A = \mathbf{T}_{A}^{B} \mathbf{P}_B$。这种表示使得变换复合变为简单的矩阵乘法：$\mathbf{T}_{A}^{C} = \mathbf{T}_{A}^{B} \mathbf{T}_{B}^{C}$，极大简化了运动学链（如机械臂、移动机器人）的建模。其逆变换有解析式： $$ (\mathbf{T}_{A}^{B})^{-1} = \begin{bmatrix} \mathbf{R}^T & -\mathbf{R}^T\mathbf{p} \\ \mathbf{0}^T & 1 \end{bmatrix} = \mathbf{T}_{B}^{A} $$ 这对于从末端求基座位姿，或在逆运动学中反向传播至关重要。

变换的微小表示：对于控制与状态估计，我们常需要变换的导数。旋转的微小变化由旋转向量（轴角表示）或角速度 $\boldsymbol{\omega}$ 描述，满足 $\dot{\mathbf{R}} = \mathbf{R}[\boldsymbol{\omega}]_\times$，其中 $[\boldsymbol{\omega}]_\times$ 是反对称矩阵（叉乘矩阵）。齐次变换的微小变化由空间速度（ twists ） $\xi = [\boldsymbol{\omega}^T, \mathbf{v}^T]^T$ 描述，满足 $\dot{\mathbf{T}} = \mathbf{T}[\xi]_\se$，其中 $[\xi]_\se$ 是 $4 \times 4$ 的运动旋量矩阵。这种表示在基于李群与李代数的机器人学（如 $SE(3)$ 上的滤波）中日益重要。

1.2 概率论：不确定世界的推理与滤波

机器人感知的本质是从带有噪声、可能稀疏甚至矛盾的传感器数据中，估计世界的状态。概率论为此提供了数学上一致的框架。

1.2.1 贝叶斯定理：传感器融合的核心

贝叶斯定理 $p(\mathbf{x}|\mathbf{z}) = \frac{p(\mathbf{z}|\mathbf{x}) p(\mathbf{x})}{p(\mathbf{z})}$ 是机器人状态估计的灵魂。它将后验概率 $p(\mathbf{x}|\mathbf{z})$（在观测到数据 $\mathbf{z}$ 后，状态 $\mathbf{x}$ 的概率分布）与先验概率 $p(\mathbf{x})$（观测前的信念）和似然 $p(\mathbf{z}|\mathbf{x})$（在状态 $\mathbf{x}$ 下产生观测 $\mathbf{z}$ 的概率）联系起来。分母 $p(\mathbf{z})$ 是归一化常数。

在机器人学中，我们通常面对序列估计问题：在时刻 $k$，我们拥有历史观测 $\mathbf{z}_{1:k}$ 和控制输入 $\mathbf{u}_{1:k}$，目标是估计状态 $\mathbf{x}_k$。贝叶斯滤波递归地执行两个步骤： 1. 预测（先验更新）：基于运动模型 $p(\mathbf{x}_k | \mathbf{x}_{k-1}, \mathbf{u}_k)$ 和上一后验 $p(\mathbf{x}_{k-1}|\mathbf{z}_{1:k-1})$，计算预测分布 $p(\mathbf{x}_k|\mathbf{z}_{1:k-1}) = \int p(\mathbf{x}_k | \mathbf{x}_{k-1}, \mathbf{u}_k) p(\mathbf{x}_{k-1}|\mathbf{z}_{1:k-1}) d\mathbf{x}_{k-1}$。 2. 更新（校正）：利用新的观测 $\mathbf{z}_k$ 和观测模型 $p(\mathbf{z}_k|\mathbf{x}_k)$，通过贝叶斯规则将预测分布修正为后验：$p(\mathbf{x}_k|\mathbf{z}_{1:k}) = \frac{p(\mathbf{z}_k|\mathbf{x}_k) p(\mathbf{x}_k|\mathbf{z}_{1:k-1})}{p(\mathbf{z}_k)}$。

这个框架是通用的，其具体实现形式（如卡尔曼滤波、粒子滤波）取决于模型 $p(\cdot)$ 的形式。

1.2.2 高斯分布与卡尔曼滤波

当状态转移模型和观测模型都是线性高斯的： $$ \mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k) $$ $$ \mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k) $$ 其中所有噪声均为零均值高斯白噪声，且相互独立。那么，后验分布 $p(\mathbf{x}_k|\mathbf{z}_{1:k})$ 也必然是高斯分布，其均值 $\boldsymbol{\mu}_k$ 和协方差 $\mathbf{P}_k$ 可以通过卡尔曼滤波递归精确计算。其核心创新在于利用正交投影原理，将预测和更新的计算分解为两步，避免了高维积分，计算复杂度为 $O(n^3)$（$n$ 为状态维度）。

卡尔曼滤波的更新步骤可直观理解：观测残差 $\tilde{\mathbf{y}}_k = \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1}$ 反映了预测状态与真实观测的差异。卡尔曼增益 $\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k)^{-1}$ 决定了我们应多大程度相信这个残差。它本质上是预测协方差 $\mathbf{P}_{k|k-1}$ 与观测噪声 $\mathbf{R}_k$ 之间的权衡：若预测很准（$\mathbf{P}$ 小）或观测很噪（$\mathbf{R}$ 大），则 $\mathbf{K}$ 小，我们更相信预测；反之，则 $\mathbf{K}$ 大，我们更相信观测。更新后的估计 $\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \tilde{\mathbf{y}}_k$ 是预测和观测的最优线性无偏估计（在最小均方误差意义下）。

扩展卡尔曼滤波（EKF） 和无迹卡尔曼滤波（UKF） 是处理非线性系统的常用近似。EKF 在均值处对非线性函数进行一阶泰勒展开，可能导致显著误差甚至发散，尤其在强非线性或非高斯噪声下。UKF 则通过一组精心选择的西格玛点来近似状态分布的非线性变换，能更准确地捕捉均值和协方差的变化，通常比 EKF 更鲁棒。

1.2.3 信息滤波与因子图

信息滤波是卡尔曼滤波的对偶形式。它不直接维护状态的后验均值 $\boldsymbol{\mu}$ 和协方差 $\mathbf{P}$，而是维护信息矩阵 $\mathbf{\Lambda} = \mathbf{P}^{-1}$ 和信息向量 $\boldsymbol{\eta} = \mathbf{P}^{-1}\boldsymbol{\mu}$。其预测和更新在信息域进行。对于稀疏观测（如机器人仅在少数时刻有GPS定位），信息滤波具有优势：在无观测期间，信息矩阵 $\mathbf{\Lambda}$ 保持不变（对应协方差 $\mathbf{P}$ 的增长），更新仅发生在有观测的时刻，且是加法操作（$\boldsymbol{\eta} \leftarrow \boldsymbol{\eta} + \mathbf{H}^T\mathbf{R}^{-1}\mathbf{z}$, $\mathbf{\Lambda} \leftarrow \mathbf{\Lambda} + \mathbf{H}^T\mathbf{R}^{-1}\mathbf{H}$），这在实现上有时更简单。更重要的是，信息形式天然地将问题与因子图优化联系起来。

因子图是一种概率图模型，用变量节点（如 $\mathbf{x}_k$）和因子节点（如 $p(\mathbf{z}_k|\mathbf{x}_k)$）的二分图表示联合概率分布的因子分解。最大后验估计（MAP）问题 $\max_{\mathbf{x}} p(\mathbf{x}|\mathbf{z}) \propto \prod_i \phi_i(\mathbf{x}_{\mathcal{N}(i)})$ 等价于最小化负对数后验，即最小化和函数： $$ \mathbf{x}^* = \arg\min_{\mathbf{x}} \sum_i \phi_i(\mathbf{x}_{\mathcal{N}(i)}) $$ 每个因子 $\phi_i$ 通常对应一个高斯项（来自线性高斯模型）或一个非线性误差项。当所有因子均为高斯时，和函数是状态变量的二次型，其最优解满足正态方程 $(\sum_i \mathbf{J}_i) \mathbf{x}^* = \sum_i \mathbf{b}_i$，其中 $\mathbf{J}_i$ 和 $\mathbf{b}_i$ 是因子 $\phi_i$ 对应的信息矩阵和信息向量。这正是信息滤波更新步骤的矩阵形式。非线性最小二乘问题（如SLAM、 bundle adjustment）的求解（如高斯-牛顿法、Levenberg-Marquardt算法）每一步都要求解这样一个线性系统，其系数矩阵正是整个因子图的信息矩阵（稀疏且对称正定）。因此，信息滤波为理解这些大规模优化问题提供了概率视角。

1.3 凸优化：从轨迹规划到模型预测控制

当我们将机器人的规划与控制问题（如寻找最短路径、最平滑轨迹、能耗最低的运动）表述为优化问题时，问题的结构（凸性）决定了求解的难易程度和全局最优性保证。

1.3.1 凸集、凸函数与凸优化问题

凸集 $C$ 满足：对任意 $\mathbf{x}, \mathbf{y} \in C$ 和 $\theta \in [0,1]$，有 $\theta\mathbf{x} + (1-\theta)\mathbf{y} \in C$。几何上，集合中任意两点连线仍在集合内。机器人状态空间、可行控制输入集（如关节角度限位、速度限制定义的超矩形）通常是凸集。

凸函数 $f$ 满足：$f(\theta\mathbf{x} + (1-\theta)\mathbf{y}) \le \theta f(\mathbf{x}) + (1-\theta) f(\mathbf{y})$。其定义域为凸集，且图像位于其弦的下方。二阶条件：若 $f$ 可微，则 $f(\mathbf{y}) \ge f(\mathbf{x}) + \nabla f(\mathbf{x})^T(\mathbf{y}-\mathbf{x})$（一阶条件）；若二阶可微，则 Hessian 矩阵 $\nabla^2 f(\mathbf{x}) \succeq 0$（半正定）对所有 $\mathbf{x}$ 成立。在机器人轨迹优化中，常用的代价函数如轨迹长度的平方和 $\sum \|\mathbf{x}_{k+1}-\mathbf{x}_k\|^2$、控制量平方和 $\sum \|\mathbf{u}_k\|^2$ 都是凸的。

凸优化问题的标准形式为： $$ \begin{aligned} \min_{\mathbf{x}} \quad & f_0(\mathbf{x}) \\ \text{s.t.} \quad & f_i(\mathbf{x}) \le 0, \quad i=1,\dots,m \\ & \mathbf{A}\mathbf{x} = \mathbf{b} \end{aligned} $$ 其中 $f_0, f_1, \dots, f_m$ 均为凸函数，$\mathb