机器人学数学基础

机器人学数学基础

引言

移动机器人自主导航的实现依赖于一套完整而精密的数学工具链。从传感器数据的处理与融合，到环境地图的构建与更新，再到运动规划与控制决策，每一个环节都建立在坚实的数学基础之上。本章将系统梳理机器人学中最核心的数学工具，为后续各章的算法推导与分析提供理论支撑。

现代移动机器人系统通常配备多种异构传感器——如轮式编码器、惯性测量单元（IMU）、激光雷达、深度相机等。这些传感器采集的原始数据本质上是噪声污染的离散观测，必须经过恰当的数学处理才能提取出有价值的环境信息。同时，机器人运动学模型的建立与控制器的设计涉及刚体变换的表示与操作，而状态估计问题则需要在概率框架下对不确定性进行严格建模。此外，几乎所有机器人学中的核心问题——如同时定位与地图构建（SLAM）、运动规划、视觉里程计等——最终都归结为某种形式的最优化问题。

本章将依次介绍线性代数、概率论、最优化理论以及李群与李代数这四大数学支柱。在阐述理论的同时，我们将紧密结合机器人学的实际应用场景，通过具体案例展示这些数学工具如何在实际问题中发挥作用。读者将会看到，看似抽象的数学概念如何转化为解决真实工程问题的有力武器。

---

线性代数基础

向量空间与线性映射

线性代数是描述和处理多维数据的基本语言。在机器人学语境下，向量空间的概念贯穿于状态表示、坐标变换、数据处理等各个环节。一个 $n$ 维向量空间 $\mathbb{R}^n$ 由所有 $n$ 维实数列向量构成，配备向量加法和标量乘法两种运算。向量的几何直观——大小（范数）和方向（单位向量）——在刚体运动描述中具有明确的物理意义。

向量的 $L_p$ 范数定义为 $\| \mathbf{v} \|_p = \left( \sum_{i=1}^n |v_i|^p \right)^{1/p}$，其中最常用的是 $L_2$ 范数（欧几里得范数），它对应向量的几何长度。在传感器数据处理中，我们经常需要度量两个向量的接近程度。例如，激光雷达扫描匹配问题可以形式化为在位姿参数空间中的最小二乘搜索，其目标函数涉及观测点云与参考点云之间距离的平方和。

矩阵作为线性映射的表示，编码了向量空间之间的变换关系。给定矩阵 $\mathbf{A} \in \mathbb{R}^{m \times n}$，它将 $\mathbb{R}^n$ 中的向量映射到 $\mathbb{R}^m$ 中的向量。对于刚体运动描述，旋转矩阵 $\mathbf{R} \in \mathbb{R}^{3 \times 3}$ 具有特殊性质：

$$\mathbf{R}^\top \mathbf{R} = \mathbf{I}, \quad \det(\mathbf{R}) = 1$$

这意味着旋转矩阵是正交矩阵的一个子类，其列向量构成 $\mathbb{R}^3$ 中的一组标准正交基。正交性保证了旋转操作保持向量的长度和向量间的夹角不变，这在刚体运动描述中是必需的。

矩阵运算与数值稳定性

在实际机器人软件实现中，矩阵运算的数值稳定性是一个不可忽视的问题。当涉及病态矩阵（condition number 很大）时，浮点运算的舍入误差可能被放大到不可接受的程度。矩阵的条件数定义为 $\kappa(\mathbf{A}) = \frac{\sigma_{\max}}{\sigma_{\min}}$，即最大与最小奇异值之比，它刻画了矩阵对输入扰动的敏感程度。

考虑一个典型场景：在视觉里程计中，我们需要通过求解线性方程组 $\mathbf{A} \mathbf{x} = \mathbf{b}$ 来恢复相机运动。当 $\mathbf{A}$ 接近奇异时，直接求逆可能导致数值爆炸。更好的做法是使用基于奇异值分解（SVD）的方法，将解表示为 $\mathbf{x} = \mathbf{V} \boldsymbol{\Sigma}^{-1} \mathbf{U}^\top \mathbf{b}$，其中通过设置过小的奇异值为零来实现稳健的伪逆计算。

矩阵的 QR 分解在求解最小二乘问题和进行正交化操作时非常有用。对于满秩矩阵 $\mathbf{A} \in \mathbb{R}^{m \times n}$（$m \geq n$），存在正交矩阵 $\mathbf{Q} \in \mathbb{R}^{m \times m}$ 和上三角矩阵 $\mathbf{R} \in \mathbb{R}^{m \times n}$ 使得 $\mathbf{A} = \mathbf{Q} \mathbf{R}$。当使用 Gram-Schmidt 正交化过程时，数值稳定性较差；Householder 变换和 Givens 旋转提供了更稳健的实现方式。

特征值分解与主成分分析

特征值分解将方阵 $\mathbf{A} \in \mathbb{R}^{n \times n}$ 表示为 $\mathbf{A} = \mathbf{V} \mathbf{\Lambda} \mathbf{V}^{-1}$ 的形式，其中 $\mathbf{\Lambda} = \operatorname{diag}(\lambda_1, \ldots, \lambda_n)$ 是由特征值组成的对角矩阵，$\mathbf{V}$ 的列向量是对应的特征向量。当 $\mathbf{A}$ 是对称矩阵时，特征向量两两正交，分解具有简化形式 $\mathbf{A} = \mathbf{Q} \mathbf{\Lambda} \mathbf{Q}^\top$。

特征值在机器人学中有丰富的物理意义。对于刚体的惯性张量矩阵，其特征值表示沿主惯性轴的惯性矩，特征向量指向主惯性轴方向。在振动分析中，系统的固有频率与特征值直接相关。在状态估计中，信息矩阵的特征值分布反映了状态变量之间的相关性结构。

主成分分析（PCA）是最常用的降维技术之一，它利用数据协方差矩阵的特征值分解来识别数据变异的主要方向。给定数据矩阵 $\mathbf{X} \in \mathbb{R}^{m \times n}$（$m$ 个样本，$n$ 维特征），首先计算均值归一化后的协方差矩阵 $\mathbf{\Sigma} = \frac{1}{m-1} \mathbf{X}^\top \mathbf{X}$。对 $\mathbf{\Sigma}$ 进行特征值分解，得到特征值 $\lambda_1 \geq \lambda_2 \geq \cdots \geq \lambda_n$ 和对应的特征向量 $\mathbf{v}_1, \mathbf{v}_2, \ldots, \mathbf{v}_n$。保留前 $k$ 个特征向量构成变换矩阵 $\mathbf{W} = [\mathbf{v}_1, \ldots, \mathbf{v}_k]$，则降维后的数据为 $\mathbf{Y} = \mathbf{X} \mathbf{W}$。

```python import numpy as np

def pca(X, k): """ 主成分分析降维

参数: X: 数据矩阵 (m, n)，m个样本，n维特征 k: 保留的主成分个数

返回: Y: 降维后的数据 (m, k) explained_variance: 各主成分解释的方差比例 """

均值归一化

X_centered = X - X.mean(axis=0)

计算协方差矩阵

cov = np.cov(X_centered.T)

特征值分解

eigenvalues, eigenvectors = np.linalg.eigh(cov)

按特征值降序排列

idx = np.argsort(eigenvalues)[::-1] eigenvalues = eigenvalues[idx] eigenvectors = eigenvectors[:, idx]

保留前k个主成分

W = eigenvectors[:, :k] Y = X_centered @ W

计算解释方差比例

explained_variance = eigenvalues[:k] / eigenvalues.sum()

return Y, explained_variance ```

在移动机器人应用中，PCA 常用于激光雷达点云压缩、环境特征提取、传感器数据去噪等场景。例如，在激光SLAM的前端处理中，通过对局部点云进行PCA可以估计表面法向量和平面法参数，从而支持点云分割与特征提取。

奇异值分解的几何意义

奇异值分解（SVD）是特征值分解在非方阵情形下的推广。对于任意矩阵 $\mathbf{A} \in \mathbb{R}^{m \times n}$，存在正交矩阵 $\mathbf{U} \in \mathbb{R}^{m \times m}$、$\mathbf{V} \in \mathbb{R}^{n \times n}$ 和对角矩阵 $\boldsymbol{\Sigma} \in \mathbb{R}^{m \times n}$ 使得：

$$\mathbf{A} = \mathbf{U} \boldsymbol{\Sigma} \mathbf{V}^\top$$

其中 $\boldsymbol{\Sigma} = \begin{bmatrix} \operatorname{diag}(\sigma_1, \ldots, \sigma_r) & \mathbf{0} \end{bmatrix}$，$\sigma_1 \geq \sigma_2 \geq \cdots \geq \sigma_r \geq 0$ 是矩阵的奇异值，$r = \operatorname{rank}(\mathbf{A})$。

SVD 提供了矩阵的秩、外积、范数等核心属性的简洁计算方法。特别地，矩阵的谱范数（最大奇异值）和核范数（奇异值之和）分别在 робаст 控制和低秩矩阵恢复问题中扮演关键角色。

在机器人视觉领域，SVD 是估计基础矩阵和本质矩阵的核心工具。以双目视觉三角测量为例，假设两视图之间的对应点满足极线约束 $\mathbf{x}_2^\top \mathbf{F} \mathbf{x}_1 = 0$，其中 $\mathbf{F}$ 是 $3 \times 3$ 的基础矩阵。通过八点算法求解 $\mathbf{F}$，最后利用 SVD 将不满足几何约束的 $\mathbf{F}$ 投影到合法的秩-2空间，确保其对应于物理上可实现的相机运动。

---

概率论与统计推断

概率分布的参数化表示

机器人感知的不确定性建模是概率机器人学的核心议题。传感器观测天然带有噪声，环境的部分可观测性导致状态估计问题必须在概率框架下处理。随机变量的概率分布描述了其取值的统计规律，是连接物理世界与数学模型的关键桥梁。

离散随机变量的概率质量函数（PMF）$p(X = x)$ 描述了各离散取值的概率。对于连续随机变量，概率密度函数（PDF）$p(x)$ 满足 $\int_{-\infty}^{\infty} p(x) dx = 1$，特定区间的概率通过积分计算。

在机器人学中最常用的分布包括：

高斯分布（正态分布） 是连续随机变量最重要的分布之一。一维高斯分布的概率密度函数为：

$$p(x) = \frac{1}{\sqrt{2\pi\sigma^2}} \exp\left(-\frac{(x-\mu)^2}{2\sigma^2}\right)$$

其中 $\mu$ 是均值，$\sigma^2$ 是方差。高斯分布在误差理论中占据核心地位，依据中心极限定理，大量独立随机因素的总和近似服从高斯分布，这为传感器噪声建模提供了理论依据。多元高斯分布在状态估计中扮演核心角色，其概率密度函数为：

$$p(\mathbf{x}) = \frac{1}{(2\pi)^{n/2} |\mathbf{\Sigma}|^{1/2}} \exp\left(-\frac{1}{2}(\mathbf{x}-\boldsymbol{\mu})^\top \mathbf{\Sigma}^{-1}(\mathbf{x}-\boldsymbol{\mu})\right)$$

其中 $\boldsymbol{\mu} \in \mathbb{R}^n$ 是均值向量，$\mathbf{\Sigma} \in \mathbb{R}^{n \times n}$ 是协方差矩阵。

拉普拉斯分布 具有更重的尾部，适合建模具有尖峰和厚尾特征的噪声分布：

$$p(x) = \frac{1}{2b} \exp\left(-\frac{|x-\mu|}{b}\right)$$

在鲁棒状态估计和图像处理中，拉普拉斯先验常被用作稀疏促进正则项。

均匀分布 用于描述完全没有先验信息的随机变量，其概率密度在定义域上恒定。

贝叶斯推断框架

贝叶斯推断为机器人状态估计提供了统一而严谨的数学框架。设 $\mathbf{x}$ 表示待估计的隐状态（如机器人位姿），$\mathbf{z}$ 表示观测数据（如传感器读数）。贝叶斯定理给出后验概率的表达式：

$$p(\mathbf{x}|\mathbf{z}) = \frac{p(\mathbf{z}|\mathbf{x}) p(\mathbf{x})}{p(\mathbf{z})} = \frac{p(\mathbf{z}|\mathbf{x}) p(\mathbf{x})}{\int p(\mathbf{z}|\mathbf{x}) p(\mathbf{x}) d\mathbf{x}}$$

其中 $p(\mathbf{x})$ 是先验分布（Prior），编码了在观测之前关于状态的先验知识；$p(\mathbf{z}|\mathbf{x})$ 是似然（Likelihood），描述了在给定状态下观测到当前数据的概率；$p(\mathbf{x}|\mathbf{z})$ 是后验分布（Posterior），即我们希望计算的量。分母 $p(\mathbf{z})$ 是观测的边缘概率，起归一化常数的作用。

在后验分布难以解析计算时，我们可以采用变分推断或蒙特卡洛采样方法近似求解。马尔可夫链蒙特卡洛（MCMC）方法通过构造一个平稳分布为后验的马尔可夫链来采样，是统计推断的重要工具。

参数估计方法

参数估计问题关注如何从观测数据中推断潜在的概率分布参数。频率派和贝叶斯派提供了两种互补的估计范式。

最大似然估计（MLE） 寻求使观测数据出现概率最大的参数值：

$$\hat{\boldsymbol{\theta}}_{\text{MLE}} = \arg\max_{\boldsymbol{\theta}} \prod_{i=1}^{N} p(\mathbf{z}_i | \boldsymbol{\theta}) = \arg\max_{\boldsymbol{\theta}} \sum_{i=1}^{N} \log p(\mathbf{z}_i | \boldsymbol{\theta})$$

当数据服从独立同分布且噪声为高斯分布时，MLE 等价于最小二乘估计。对于线性高斯系统，MLE 可以给出闭式解。

最大后验估计（MAP） 结合了先验信息：

$$\hat{\boldsymbol{\theta}}_{\text{MAP}} = \arg\max_{\boldsymbol{\theta}} p(\boldsymbol{\theta}) \prod_{i=1}^{N} p(\mathbf{z}_i | \boldsymbol{\theta})$$

当先验分布为均匀分布时，MAP 退化为 MLE。当先验为高斯分布时，MAP 等价于带 $L_2$ 正则化的最大似然估计。

贝叶斯估计 给出完整的参数后验分布而非点估计，这在需要量化参数不确定性时不可或缺。

非参数估计方法

当数据的分布形式未知或过于复杂时，非参数方法提供了灵活的替代方案。核密度估计（KDE）通过核函数的平滑叠加来逼近真实分布：

$$\hat{p}(\mathbf{x}) = \frac{1}{N h^d} \sum_{i=1}^{N} K\left(\frac{\mathbf{x} - \mathbf{x}_i}{h}\right)$$

其中 $K(\cdot)$ 是核函数（如高斯核），$h$ 是带宽参数，$d$ 是数据维度。带宽的选择在偏差与方差之间取得平衡——过小的带宽导致高方差（过拟合），过大的带宽导致高偏差（欠拟合）。Silverman 规则给出高斯核的带宽建议：$h = 1.06 \hat{\sigma} N^{-1/5}$。

高斯混合模型（GMM）通过多个高斯分量的加权混合来逼近复杂分布：

$$p(\mathbf{x}) = \sum_{k=1}^{K} \pi_k \mathcal{N}(\mathbf{x} | \boldsymbol{\mu}_k, \mathbf{\Sigma}_k)$$

其中 $\pi_k$ 是混合系数，满足 $\sum_{k=1}^{K} \pi_k = 1$。GMM 的参数通过 EM 算法迭代优化，在机器人定位和环境建模中有广泛应用。

卡尔曼滤波的数学原理

卡尔曼滤波是线性高斯系统下最优状态估计的解析解，是贝叶斯滤波在线性高斯假设下的具体实现。考虑如下线性动态系统：

$$\mathbf{x}_k = \mathbf{A}_{k-1} \mathbf{x}_{k-1} + \mathbf{B}_{k-1} \mathbf{u}_{k-1} + \mathbf{w}_{k-1}$$ $$\mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k$$

其中 $\mathbf{w}_{k-1} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{k-1})$ 是过程噪声，$\mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k)$ 是观测噪声。

卡尔曼滤波递归地交替执行预测步和更新步。设 $\hat{\mathbf{x}}_{k-1|k-1}$ 和 $\mathbf{P}_{k-1|k-1}$ 分别为 $k-1$ 时刻的后验均值和协方差，预测步为：

$$\hat{\mathbf{x}}_{k|k-1} = \mathbf{A}_{k-1} \hat{\mathbf{x}