机器人学概述与核心数学基础

机器人学作为一门交叉学科，其核心使命是赋予机器人在物理世界中自主感知、决策与行动的能力。这一使命的实现，本质上依赖于对三个核心问题的数学建模与求解：机器人“在哪里”（状态估计与定位）、“如何去”（运动规划与路径搜索）、“如何稳定地到达”（反馈控制与轨迹跟踪）。然而，物理世界的复杂性——传感器噪声、模型误差、动态扰动与计算约束——使得这些问题的精确解析解通常不可得。因此，机器人学的发展史，在相当程度上是一部不断引入、发展并精炼数学工具以应对这些挑战的历史。从早期的刚体运动学建模，到现代概率SLAM（同步定位与地图构建）与基于优化的控制，数学不仅是描述问题的语言，更是设计算法、分析性能与保证鲁棒性的基石。

本章旨在为全书确立一个统一、严谨且工程导向的数学框架。我们不追求数学理论的全面性，而是聚焦于那些在机器人学感知、规划与控制三大支柱中反复出现、且具有深刻洞察力的核心数学分支。重点将深入探讨四个相互关联的领域：矩阵分析与线性代数，它是处理高维状态与观测数据的基础；贝叶斯概率论与随机过程，为不确定性建模提供概率语义；优化理论，特别是非线性数值优化，它是求解高维、非凸机器人问题的通用引擎；以及李群与李代数，为描述旋转与刚体运动提供几何直观与计算友好的微积分结构。通过本章的梳理，读者将不仅掌握这些工具的“如何用”，更能理解其“为何用”——即它们是如何源于机器人学的具体问题，并如何塑造了从滤波器到图优化、从PID控制到轨迹优化的现代算法范式。

1.1 机器人学核心问题与数学框架的必要性

机器人系统的核心闭环可抽象为：通过传感器获取环境信息（感知），基于内部模型与任务目标计算期望运动（规划），然后通过执行器驱动关节或轮子产生实际运动（控制），同时持续感知反馈以修正误差。这个闭环的每一步都充满了数学挑战。

状态估计的挑战：机器人需要从带有噪声的传感器测量（如IMU的角速度、激光雷达的距离、相机的像素）中，推断自身在环境中的位姿（位置与朝向）以及环境的地图（如路标点位置）。这是一个典型的逆问题，通常病态且不确定。例如，仅凭单目相机图像，无法直接度量物体的真实距离（尺度模糊性）；激光雷达在远距离时测距噪声增大。如何融合多模态、异步、频率各异的传感器数据，并量化估计的不确定性，是概率状态估计的核心。

运动规划的挑战：给定起点、终点与障碍物信息，寻找一条无碰撞、满足动力学约束（如最大速度、加速度、转向角速率）且最优（最短时间、最短路径、最节能）的轨迹。这通常形成一个在非凸、高维配置空间（Configuration Space, C-space）中的最优控制问题或路径搜索问题。配置空间的维度等于机器人的自由度（DOF），对于6自由度的机械臂，即6维空间，其复杂度随维度指数增长（维数灾难）。如何高效地搜索或优化，同时保证完备性或概率完备性，是规划算法的关键。

反馈控制的挑战：即使规划出一条理想轨迹，机器人的实际运动也会因模型不精确、外部干扰（如地面摩擦变化、风扰）而偏离。控制器需要实时计算控制量（如电机扭矩、轮速），以最小化跟踪误差。这通常归结为求解一个动态系统的反馈律问题。对于非线性系统（如无人机、双足机器人），线性控制理论（如LQR）可能不适用，需要更复杂的非线性控制方法（如反馈线性化、模型预测控制MPC），而这些方法往往涉及复杂的在线优化。

面对这些环环相扣的挑战，一个松散、ad-hoc的数学工具箱是远远不够的。一个统一的数学框架能带来三大优势：第一，概念一致性，例如，无论是视觉里程计还是激光SLAM，其最大后验概率（MAP）估计问题最终都可写成同一形式的非线性最小二乘优化，从而可以使用相同的求解器（如g2o, Ceres Solver）；第二，算法可迁移性，在SLAM中成熟的图优化思想，被成功迁移到多机器人协同定位、时空同步等新问题；第三，理论分析可 rigor，能够严格证明滤波器的收敛性、优化算法的局部收敛速率、控制系统的稳定性。因此，本章选择的数学工具，正是那些能够贯穿感知-规划-控制链条，提供这种统一性的基石。

1.2 矩阵分解与特征值：线性代数的视角

线性代数是处理高维数据的语言。在机器人学中，状态向量（如位姿、地图点坐标）和测量向量（如图像特征点像素、激光点云）通常构成高维向量或矩阵。矩阵分解提供了理解这些数据内在结构、降维、求解线性系统以及分析稳定性的强大工具。

奇异值分解（SVD）的普适性：对于任意实矩阵 $\mathbf{A} \in \mathbb{R}^{m \times n}$，其SVD分解为 $\mathbf{A} = \mathbf{U}\mathbf{\Sigma}\mathbf{V}^\top$，其中 $\mathbf{U} \in \mathbb{R}^{m \times m}$ 和 $\mathbf{V} \in \mathbb{R}^{n \times n}$ 是正交矩阵，$\mathbf{\Sigma} \in \mathbb{R}^{m \times n}$ 是对角矩阵，对角线元素 $\sigma_1 \ge \sigma_2 \ge \dots \ge \sigma_{\min(m,n)} \ge 0$ 为奇异值。

在机器人学中，SVD的应用无处不在：

• **最小二乘解与伪逆**：对于线性系统 $\mathbf{Ax} = \mathbf{b}$，其最小二乘解为 $\mathbf{x}^* = \mathbf{A}^\dagger \mathbf{b}$，其中 $\mathbf{A}^\dagger = \mathbf{V}\mathbf{\Sigma}^\dagger\mathbf{U}^\top$ 是Moore-Penrose伪逆。$\mathbf{\Sigma}^\dagger$ 通过对 $\mathbf{\Sigma}$ 的非零奇异值取倒数得到。这直接应用于**点云配准**（如ICP算法）、**手眼标定**（求解 $\mathbf{AX} = \mathbf{XB}$ 形式的方程）等线性化问题。
• **数值稳定性与截断SVD**：当 $\mathbf{A}$ 接近奇异（存在小奇异值）时，直接求逆会放大噪声。**截断SVD** 通过仅保留大于某个阈值 $\tau$ 的奇异值，即 $\mathbf{A}_\tau^\dagger = \mathbf{V}\text{diag}(1/\sigma_i, 0)\mathbf{U}^\top$（$\sigma_i > \tau$），可以得到病态问题的稳定解。这在**视觉三角化**（从多个视角恢复3D点）中至关重要，当基线很小时，三角化矩阵条件数差，使用截断SVD能有效抑制深度估计的噪声放大。
• **主成分分析（PCA）与降维**：SVD是计算PCA的标准方法。在机器人学中，PCA用于**环境结构识别**（如平面、线段拟合）、**特征降维**（如将高维激光反射强度数据降维）、以及**协方差矩阵分析**（见下）。

特征值分解与稳定性分析：对于方阵 $\mathbf{A} \in \mathbb{R}^{n \times n}$，特征值分解 $\mathbf{A} = \mathbf{Q}\mathbf{\Lambda}\mathbf{Q}^{-1}$（$\mathbf{\Lambda}$ 为特征值对角阵）在离散时间系统稳定性分析中起核心作用。考虑线性离散系统 $\mathbf{x}_{k+1} = \mathbf{A}\mathbf{x}_k$，其稳定性要求所有特征值的模 $|\lambda_i| < 1$。在机器人控制中，这用于分析离散化后控制器的稳定性，或卡尔曼滤波器中状态转移矩阵 $\mathbf{F}$ 的特性。对于对称正定矩阵（如协方差矩阵 $\mathbf{P}$），其特征值直接反映了状态估计的不确定性在各个主方向上的大小。特征值分解也用于雅可比矩阵的奇异值分析，以判断机器人工作空间的灵巧度（Manipulability）或可操作性椭球。

案例：从SVD到视觉里程计中的三角化 假设一个3D点 $P$ 在两个相机坐标系（位姿 $\mathbf{T}_1, \mathbf{T}_2 \in SE(3)$）下的归一化坐标分别为 $\mathbf{x}_1 = [u_1, v_1, 1]^\top$ 和 $\mathbf{x}_2 = [u_2, v_2, 1]^\top$。根据投影模型，存在深度 $\lambda_1, \lambda_2$ 使得 $\lambda_1 \mathbf{x}_1 = \mathbf{K}_1 [\mathbf{R}_1|\mathbf{t}_1] P$ 和 $\lambda_2 \mathbf{x}_2 = \mathbf{K}_2 [\mathbf{R}_2|\mathbf{t}_2] P$，其中 $\mathbf{K}$ 为内参。消去 $P$ 和深度，可得线性方程 $\mathbf{A} P = 0$。对 $\mathbf{A}$ 做SVD，$P$ 对应最小奇异值对应的右奇异向量。若两个视角基线很小，$\mathbf{A}$ 的两个大奇异值接近，第三个非常小，此时解对噪声极其敏感。实践中，常使用归一化坐标（将像点变换到零均值、单位方差的坐标系）来改善 $\mathbf{A}$ 的条件数，这本质上是一种预条件化。

1.3 贝叶斯概率框架：不确定性建模的语义

机器人学处理的物理世界本质上是随机的。传感器读数包含噪声，运动执行存在误差，环境模型不完整。概率论，特别是贝叶斯框架，为这种不确定性提供了连贯的、可计算的语义。其核心是贝叶斯公式： $$ p(\mathbf{x} | \mathbf{z}) = \frac{p(\mathbf{z} | \mathbf{x}) p(\mathbf{x})}{p(\mathbf{z})} $$ 其中，$p(\mathbf{x})$ 是先验（状态 $\mathbf{x}$ 在观测前的信念），$p(\mathbf{z}|\mathbf{x})$ 是似然（在状态 $\mathbf{x}$ 下观测到 $\mathbf{z}$ 的概率），$p(\mathbf{x}|\mathbf{z})$ 是后验（融合观测后的信念），$p(\mathbf{z})$ 是证据（归一化常数）。

传感器建模：似然函数 $p(\mathbf{z}|\mathbf{x})$ 直接编码了传感器模型。对于激光雷达，测量距离 $z$ 在真实距离 $d$ 下的分布常被建模为高斯分布：$z \sim \mathcal{N}(d, \sigma^2)$，其中 $\sigma^2$ 包含测量噪声和表面反射特性。对于泊松分布，则适用于计数型传感器，如光子计数成像（SPAD相机）或事件相机（Event Camera）的事件生成率建模：给定场景亮度梯度，事件产生的时间间隔服从指数分布，其参数与亮度变化率相关。

状态估计的贝叶斯滤波：当状态 $\mathbf{x}$ 随时间演变，且观测异步时，贝叶斯更新需递归进行。卡尔曼滤波器（KF） 是线性高斯系统的精确解：

• **预测**：$p(\mathbf{x}_k) = \int p(\mathbf{x}_k | \mathbf{x}_{k-1}) p(\mathbf{x}_{k-1}|\mathbf{z}_{1:k-1}) d\mathbf{x}_{k-1}$，其中运动模型 $p(\mathbf{x}_k|\mathbf{x}_{k-1})$ 通常为高斯 $\mathcal{N}(\mathbf{F}\mathbf{x}_{k-1}, \mathbf{Q})$。
• **更新**：$p(\mathbf{x}_k|\mathbf{z}_k) \propto p(\mathbf{z}_k|\mathbf{x}_k) p(\mathbf{x}_k)$，观测模型 $p(\mathbf{z}_k|\mathbf{x}_k)$ 为高斯 $\mathcal{N}(\mathbf{H}\mathbf{x}_k, \mathbf{R})$。

高斯先验与高斯似然的后验仍是高斯，其均值与协方差可通过信息滤波或协方差滤波形式解析计算。

非线性与非高斯：粒子滤波与最大后验估计：当系统非线性或非高斯时，KF失效。扩展卡尔曼滤波器（EKF） 通过线性化（泰勒展开）近似，但会引入偏差，且无法处理多模态分布。无迹卡尔曼滤波器（UKF） 使用确定性采样（Sigma点）来近似概率分布，精度更高。对于严重非线性或多模态问题（如全局定位中的 kidnapped robot 问题），粒子滤波 通过蒙特卡洛采样直接表示后验分布 $p(\mathbf{x}_{0:k}|\mathbf{z}_{1:k}) \approx \sum_{i=1}^N w_i \delta(\mathbf{x}_{0:k} - \mathbf{x}_{0:k}^{(i)})$，但计算量大。

在实际的机器人SLAM系统中，更常见的范式是最大后验概率估计（MAP） 或最大似然估计（MLE）。它不维护完整的后验分布，而是直接寻找使后验（或负对数后验）最大的状态向量： $$ \mathbf{x}^* = \arg\max_{\mathbf{x}} p(\mathbf{x}|\mathbf{z}) = \arg\min_{\mathbf{x}} -\log p(\mathbf{x}|\mathbf{z}) = \arg\min_{\mathbf{x}} \underbrace{-\log p(\mathbf{z}|\mathbf{x})}_{\text{数据项}} + \underbrace{-\log p(\mathbf{x})}_{\text{先验/正则项}} $$ 这自然地将问题转化为一个优化问题。例如，在图优化SLAM中，节点是机器人位姿和地图点，边是传感器测量（如视觉重投影误差、激光扫描匹配残差），整个负对数后验就是所有边误差的加权平方和（假设高斯噪声），即一个大规模非线性最小二乘问题。

案例：激光雷达运动畸变校正 高速移动的激光雷达在单次旋转扫描期间，机器人自身位姿在变化，导致原始点云处于一个时间上不连续的“混合”坐标系中。为精确建图，需要将点云校正到某一统一时间（如扫描中点）的坐标系。这需要知道机器人的运动模型（如从IMU积分得到的位姿插值）。设点 $p$ 在扫描开始时刻 $t_0$ 的位姿为 $\mathbf{T}_{r0}$，在扫描中点 $t_m$ 为 $\mathbf{T}_{rm}$，点 $p$ 的测量时间 $t_p$。其校正后的坐标 $\mathbf{p}_{corrected} = \mathbf{T}_{rm}^{-1} \mathbf{T}_{rp}(t_p) \mathbf{p}_{raw}$，其中 $\mathbf{T}_{rp}(t_p)$ 是从机器人基座到测量时刻的变换。这里，IMU提供的位姿插值本身带有噪声和不确定性。贝叶斯框架允许我们将校正过程视为一个状态估计问题：将扫描中点位姿 $\mathbf{T}_{rm}$ 作为待优化变量，将原始点云与已有点云地图（或相邻扫描）的匹配误差作为观测，同时将IMU积分得到的先验 $\mathbf{T}_{rp}(t_m)$ 作为正则项，共同构建优化问题，从而在运动畸变校正的同时，提升位姿估计的精度。

1.4 优化理论：从梯度下降到Levenberg-Marquardt

如前所述，现代机器人学中的许多核心问题——Bundle Adjustment、图优化SLAM、轨迹规划、模型预测控制——最终都归结为求解一个非线性优化问题： $$ \min_{\mathbf{x}} f(\mathbf{x}) = \sum_{i=1}^m \| \mathbf{r}_i(\mathbf{x}) \|^2_{\mathbf{\Sigma}_i^{-1}} $$ 其中 $\mathbf{r}_i(\mathbf{x}) \in \mathbb{R}^n$ 是第 $i$ 个残差（如重投影误差、控制误差），$\mathbf{\Sigma}_i$ 是其协方差（定义范数 $\|\mathbf{e}\|^2_{\mathbf{\Sigma}^{-1}} = \mathbf{e}^\top \mathbf{\Sigma}^{-1} \mathbf{e}$），$\mathbf{x} \in \mathbb{R}^N$ 是待优化变量（位姿、地图点、控制轨迹等），$N$ 可能极大（数万至数百万）。

一阶方法：梯度下降：最直观的方法是沿负梯度方向迭代：$\mathbf{x}_{k+1} = \mathbf{x}_k - \alpha_k \nabla f(\mathbf{x}_k)$。梯度 $\nabla f = \sum_i \mathbf{J}_i^\top \mathbf{\Sigma}_i^{-1} \mathbf{r}_i$，其中 $\mathbf{J}_i = \partial \mathbf{r}_i / \partial \mathbf{x}$ 是雅可比矩阵。梯度下降简单、内存需求小（只需计算当前梯度），但收敛速度慢（线性收敛），且对病态问题（Hessian矩阵条件数大）非常敏感，容易在狭窄山谷中震荡。步长 $\alpha_k$ 的选择（如线搜索）至关重要。

二阶方法：牛顿法：利用Hessian矩阵 $\mathbf{H} = \nabla^2 f \approx \sum_i \mathbf{J}_i^\top \mathbf{\Sigma}_i^{-1} \mathbf{J}_i$（忽略二阶项），迭代步为 $\Delta \mathbf{x} = -\mathbf{H}^{-1} \nabla f$。牛顿法具有局部二次收敛速率，但每步需计算并求逆Hessian，计算复杂度 $O(N^3)$，内存 $O(N^2)$，对于大规模问题不可行。且Hessian可能非正定，导致步长方向非下降方向。

高斯-牛顿法（Gauss-Newton, GN）：这是机器人学中最常用的方法之一。它忽略残差对雅可比的二阶导数，即用 $\mathbf{H} \approx \mathbf{J}^\top \mathbf{J}$ 近似Hessian（在最小二乘问题中，$\mathbf{J}^\top \mathbf{J}$ 近似于Hessian的正定部分）。求解线性系统： $$ (\mathbf{J}^\top \mathbf{J}) \Delta \mathbf{x} = -\mathbf{J}^\top \mathbf{r} $$ 其中 $\mathbf{J}$ 是所有残差雅可比的分块堆叠