机器人学数学基础

机器人学是一门融合数学、物理、计算机科学与工程技术的综合性学科，其核心任务是为机器人设计运动规划、感知算法和控制系统。要深入理解并有效应用机器人技术，扎实的数学功底是不可或缺的基础。从最基础的坐标变换到复杂的SLAM系统，从简单的位置控制到高精度的力控操作，几乎所有机器人问题的建模与求解都依赖于一系列精心设计的数学工具。本章将系统性地介绍机器人学所必需的四类数学基础：向量与矩阵运算、李群与李代数、概率论基础以及最优化方法。这些内容不仅在理论上相互关联，在实践中也往往需要联合使用。通过本章的学习，读者将建立起处理空间变换和不确定性问题所需的数学直觉，为后续章节的深入学习奠定坚实的理论基础。

向量与矩阵运算

向量和矩阵是线性代数的核心概念，它们在机器人学中扮演着不可替代的角色。从描述机械臂末端执行器的空间位置，到表示相机图像中的像素特征；从计算关节空间的雅可比矩阵，到推导动力学方程中的质量矩阵，向量与矩阵运算贯穿于机器人建模与控制的各个环节。

向量的基本概念与运算

向量是同时具有大小和方向的量，在数学上可以理解为有序的数值序列。在三维空间中，一个向量 $\mathbf{v}$ 可以表示为三个分量的有序三元组：$\mathbf{v} = (v_x, v_y, v_z)^T$，其中上标 $T$ 表示转置，将行向量转换为列向量。这种表示方式使得向量运算可以统一地使用矩阵操作来完成。在机器人学的语境下，向量常用于表示位置（如末端执行器坐标）、速度（如关节角速度）、力（如末端接触力）等物理量。

向量的基本运算包括加法、数乘、内积和叉积。两个维度相同的向量可以相加，对应分量逐一相加即可。向量与标量的乘法则将标量均匀地作用于向量的每个分量。这些运算满足交换律、结合律和分配律等代数性质，使得向量的处理具有高度的规律性和可预测性。

内积（点积）是两个向量之间的一种重要运算。对于两个 $n$ 维向量 $\mathbf{a}$ 和 $\mathbf{b}$，它们的内积定义为对应分量乘积之和：

$$\mathbf{a} \cdot \mathbf{b} = \sum_{i=1}^{n} a_i b_i = \mathbf{a}^T \mathbf{b} = \mathbf{b}^T \mathbf{a}$$

内积运算的结果是一个标量，它与两个向量的夹角余弦成正比：$\mathbf{a} \cdot \mathbf{b} = \|\mathbf{a}\|\|\mathbf{b}\|\cos\theta$，其中 $\theta$ 是两向量之间的夹角。当两向量垂直时，$\cos\theta = 0$，内积为零；当两向量同向时，内积取得最大值等于两向量模的乘积。这一几何性质使得内积成为判断向量正交性和计算投影的利器。在机器人学中，内积常用于计算力在某一方向上的分量、评估两个关节轴线的相对关系，或者判断空间点是否位于某一平面的某一侧。

叉积（叉乘）是三维向量特有的另一种重要运算。对于向量 $\mathbf{a} = (a_x, a_y, a_z)^T$ 和 $\mathbf{b} = (b_x, b_y, b_z)^T$，它们的叉积定义为：

$$\mathbf{a} \times \mathbf{b} = \begin{pmatrix} a_y b_z - a_z b_y \\ a_z b_x - a_x b_z \\ a_x b_y - a_y b_x \end{pmatrix}$$

叉积的结果是一个同时垂直于原两向量的新向量，其方向由右手定则确定，模长为 $\|\mathbf{a} \times \mathbf{b}\| = \|\mathbf{a}\|\|\mathbf{b}\|\sin\theta$，其中 $\theta$ 是两向量的夹角。这个模长几何上等于两向量张成的平行四边形的面积。叉积在机器人学中有着广泛的应用：它可以用于定义坐标系的方向（利用三个坐标轴之间的叉积关系）、计算力矩（力关于某点的力矩等于位置向量与力向量的叉积），以及表达旋转的微分关系（角速度与角度变化率之间的联系）。

在实际应用中，向量的叉积运算可以通过反对称矩阵转化为矩阵乘法形式。设向量 $\mathbf{a} = (a_1, a_2, a_3)^T$，其对应的反对称矩阵定义为：

$$[\mathbf{a}]_{\times} = \begin{pmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{pmatrix}$$

利用这个记号，向量的叉积可以表示为 $[\mathbf{a}]_{\times} \mathbf{b} = \mathbf{a} \times \mathbf{b}$。这种矩阵形式的表示在推导运动学和动力学方程时非常有用，因为它将非线性运算转化为线性矩阵运算，便于符号推导和数值计算。

矩阵运算与特殊矩阵

矩阵是数或符号按照矩形阵列排列的数学对象，它是向量空间之间线性映射的代数表示。一个 $m \times n$ 的矩阵 $A$ 由 $m$ 行 $n$ 列共 $mn$ 个元素组成，其中第 $i$ 行第 $j$ 列的元素记为 $a_{ij}$。在机器人学中，矩阵广泛用于表示坐标变换（如齐次变换矩阵）、线性方程组（如运动学方程）、二次型能量函数（如势能函数）以及各种雅可比关系。

矩阵的基本运算包括加法、减法、数乘和乘法。两个维度相同的矩阵可以相加减，对应元素逐一运算。矩阵与标量的乘法将标量均匀作用于每个元素。矩阵乘法则是最为重要也最为复杂的运算：对于矩阵 $A \in \mathbb{R}^{m \times n}$ 和 $B \in \mathbb{R}^{n \times p}$，它们的乘积 $C = AB$ 是一个 $m \times p$ 的矩阵，其元素由下式给出：

$$c_{ij} = \sum_{k=1}^{n} a_{ik} b_{kj}$$

矩阵乘法满足结合律和分配律，但不满足交换律，即一般来说 $AB \neq BA$。这一性质在机器人学中有着重要的物理含义：两次相继的旋转变换或平移变换的合成结果与变换的顺序有关，先旋转再平移与先平移再旋转往往得到不同的最终姿态。

矩阵的转置是将矩阵的行和列互换得到的矩阵。对于矩阵 $A \in \mathbb{R}^{m \times n}$，其转置 $A^T \in \mathbb{R}^{n \times m}$ 满足 $(A^T)^T = A$，且 $(AB)^T = B^T A^T$。当矩阵满足 $A^T = A$ 时，称该矩阵为对称矩阵；当满足 $A^T = -A$ 时，则为反对称矩阵。反对称矩阵在对角线上元素必为零，且非对角元素满足 $a_{ij} = -a_{ji}$。

对于方阵 $A \in \mathbb{R}^{n \times n}$，若存在另一个同阶方阵 $B$ 使得 $AB = BA = I$（其中 $I$ 是单位矩阵），则称 $A$ 是可逆的，$B$ 称为 $A$ 的逆矩阵，记作 $A^{-1}$。矩阵可逆的充要条件是其行列式不为零。可逆矩阵在机器人学中对应着非奇异的坐标变换或非退化的约束关系。例如，当机械臂处于奇异构型时，其雅可比矩阵变为不可逆的奇异矩阵，此时末端执行器在某些方向上失去运动能力。

正交矩阵是另一类极为重要的特殊矩阵。一个实矩阵 $Q \in \mathbb{R}^{n \times n}$ 称为正交矩阵，如果它满足 $Q^T Q = Q Q^T = I$，这等价于 $Q^{-1} = Q^T$。正交矩阵的列向量和行向量分别构成 $\mathbb{R}^n$ 空间中的标准正交基。正交矩阵具有一个关键性质：它保持向量的欧氏范数不变，即对于任意向量 $\mathbf{v}$，都有 $\|Q\mathbf{v}\| = \|\mathbf{v}\|$。在三维空间中，正交矩阵同时保持向量的点积和叉积，这些性质使得正交矩阵成为描述旋转的天然工具。

旋转矩阵是行列式为 $+1$ 的正交矩阵，它是 $SO(3)$ 群的元素（将在下一节详细介绍）。旋转矩阵满足 $R^T R = I$ 和 $\det(R) = +1$，这意味着旋转既不改变向量的长度（正交性），也不改变空间的定向（行列式为正）。常见的旋转矩阵包括绕 $x$ 轴、$y$ 轴和 $z$ 轴的基本旋转：

$$R_x(\theta) = \begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{pmatrix}$$

$$R_y(\theta) = \begin{pmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{pmatrix}$$

$$R_z(\theta) = \begin{pmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{pmatrix}$$

矩阵指数与旋转表示

矩阵指数是将指数函数的概念推广到矩阵领域的重要工具。对于任意实矩阵 $A$，其矩阵指数定义为幂级数：

$$e^A = I + A + \frac{A^2}{2!} + \frac{A^3}{3!} + \cdots = \sum_{k=0}^{\infty} \frac{A^k}{k!}$$

这个级数对所有矩阵都是收敛的。矩阵指数具有许多与标量指数类似的性质，例如 $e^{A+B} = e^A e^B$ 当且仅当 $A$ 和 $B$ 可交换时成立，以及 $\frac{d}{dt} e^{At} = A e^{At} = e^{At} A$。

矩阵指数在旋转表示中扮演着核心角色。三维空间中的任意旋转都可以表示为某个反对称矩阵的矩阵指数：

$$R = e^{[\boldsymbol{\omega}]_{\times}\theta}$$

其中 $\boldsymbol{\omega}$ 是旋转轴方向的单位向量，$\theta$ 是旋转角度。这个关系称为Rodrigues旋转公式，它是连接李代数（反对称矩阵）与李群（旋转矩阵）的指数映射。对于小角度旋转，矩阵指数可以线性近似为：

$$e^{[\boldsymbol{\omega}]_{\times}\theta} \approx I + [\boldsymbol{\omega}]_{\times}\theta$$

这种线性化关系在旋转的微分运动学中有着广泛的应用。

向量与矩阵求导

向量和矩阵的求导运算是机器人学中推导梯度信息、设计优化算法的基础。由于涉及高维变量，需要仔细区分不同形式的导数定义。

对于标量函数 $f(\mathbf{x})$，其中 $\mathbf{x} \in \mathbb{R}^n$ 是向量变量，函数对向量的导数（梯度）定义为：

$$\nabla_{\mathbf{x}} f = \frac{\partial f}{\partial \mathbf{x}} = \begin{pmatrix} \frac{\partial f}{\partial x_1} \\ \frac{\partial f}{\partial x_2} \\ \vdots \\ \frac{\partial f}{\partial x_n} \end{pmatrix}$$

梯度的物理意义是函数值增长最快的方向，其大小表示增长率。对于多元函数 $f: \mathbb{R}^n \rightarrow \mathbb{R}^m$（向量值函数），其导数是一个 $m \times n$ 的雅可比矩阵：

$$\frac{\partial \mathbf{f}}{\partial \mathbf{x}} = J = \begin{pmatrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} & \cdots & \frac{\partial f_2}{\partial x_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial x_1} & \frac{\partial f_m}{\partial x_2} & \cdots & \frac{\partial f_m}{\partial x_n} \end{pmatrix}$$

一些常用的向量矩阵求导恒等式在机器人学推导中非常有用：$\nabla_{\mathbf{x}}(\mathbf{b}^T \mathbf{x}) = \mathbf{b}$，以及当 $A$ 对称时 $\nabla_{\mathbf{x}}(\mathbf{x}^T A \mathbf{x}) = 2A\mathbf{x}$。这些恒等式使得许多复杂的优化问题可以得到闭式解。

李群与李代数

李群和李代数是现代机器人学中处理姿态变换的核心数学工具。不同于传统的欧拉角或四元数等参数化方法，李群与李代数的框架提供了处理旋转和变换的统一、严密且易于求导的理论基础。这一框架在计算机视觉中的视觉SLAM、机器人控制中的姿态插值与轨迹规划、以及3D视觉中的点云配准等领域都有广泛应用。

旋转群 $SO(3)$

三维旋转群（特殊正交群）$SO(3)$ 是所有 $3 \times 3$ 旋转矩阵的集合，它满足以下两个条件：$R^T R = I$（正交性）和 $\det(R) = +1$（行列式为正）。正交性条件来源于旋转不改变向量长度的几何要求，而行列式条件则保证了旋转不改变空间的定向（不产生镜像翻转）。

$SO(3)$ 是一个紧致（compact）的李群。紧致性意味着群流形是有界的且闭合的，这对于优化和数值计算是一个很好的性质。$SO(3)$ 作为流形的维度是 $3$，这意味着需要且仅需要 $3$ 个独立参数来参数化旋转。这一维度数与刚体旋转的 $3$ 个自由度（绕三个轴的旋转）相吻合。

从几何角度看，$SO(3)$ 流形可以想象为一个三维球面 $S^3$ 的某种商空间，这种几何直觉有助于理解旋转插值和平均等操作的行为。然而，由于旋转存在双覆盖关系（$SO(3)$ 中每一点对应四元数空间中的两个点），在处理这些问题时需要格外小心。

旋转的李代数 $\mathfrak{so}(3)$

李代数 $\mathfrak{so}(3)$ 是与李群 $SO(3)$ 相关联的李代数，它由所有 $3 \times 3$ 反对称矩阵组成：

$$\mathfrak{so}(3) = \{[\boldsymbol{\omega}]_{\times} \in \mathbb{R}^{3 \times 3} | \boldsymbol{\omega} \in \mathbb{R}^3\}$$

李代数与李群之间存在指数映射与对数映射的对应关系。对于李代数元素 $[\boldsymbol{\omega}]_{\times} \theta$，其中 $\boldsymbol{\omega}$ 是单位旋转向量，$\theta$ 是旋转角度，其指数映射给出对应的李群元素：

$$R = \exp([\boldsymbol{\omega}]_{\times}\theta) = I + [\boldsymbol{\omega}]_{\times}\sin\theta + [\boldsymbol{\omega}]_{\times}^2(1-\cos\theta)$$

这个公式就是著名的Rodrigues公式。指数映射将李代数中的向量空间操作（加法）映射到李群中的群操作（乘法），这使得姿态的微小扰动可以通过线性化的李代数进行计算，然后通过指数映射投影回李群。

对数映射是指数映射的逆过程。对于给定的旋转矩阵 $R \neq I$，可以通过以下公式计算对应的李代数元素：

$$\theta = \arccos\left(\frac{\text{tr}(R) - 1}{2}\right)$$

$$\boldsymbol{\omega} = \frac{1}{2\sin\theta}\begin{pmatrix} R_{32} - R_{23} \\ R_{13} - R_{31} \\ R_{21} - R_{12} \end{pmatrix}$$

当 $\theta$ 接近 $0$ 时，上述对数映射公式会面临数值不稳定的问题，此时需要采用特殊处理。

李代数的一个重要应用是旋转的扰动模型。假设当前姿态用旋转矩阵 $R$ 表示，需要施加一个微小的旋转扰动 $\delta\boldsymbol{\phi}$（三维向量）。这个扰动在李代数中可以线性叠加：$R_{\text{new}} = \exp(\delta\boldsymbol{\phi}_{\times}) R \approx (I + \delta\boldsymbol{\phi}_{\times})R$。这种线性化的扰动模型是许多姿态估计算法（如BA优化、PnP求解）的理论基础。

刚体变换群 $SE(3)$

欧氏变换群（特殊欧氏群）$SE(3)$ 是描述三维空间中刚体变换的群。它由旋转部分和平移部分组成，其矩阵形式为：

$$T = \begin{pmatrix} R & \mathbf{t} \\ \mathbf{0}^T & 1 \end{pmatrix} \in \mathbb{R}^{4 \times 4}$$

其中 $R \in SO(3)$ 是旋转矩阵，$\mathbf{t} \in \mathbb{R}^3$ 是平移向量。$SE(3)$ 群满足群的四大公理：封闭性（两个变换的合成仍是变换）、结合律、单位元（单位变换）存在以及逆元存在。

$SE(3)$ 的维度是 $6$，这与刚体在三维空间中的自由度数量一致：$3$ 个转动自由度和 $3$ 个移动自由度。$SE(3)$ 不是紧致群，因为平移部分是欧氏空间而非紧致空间，但它是半直积结构 $SO(3) \ltimes \mathbb{R}^3$。

与 $SO(3)$ 类似，$SE(3)$ 也有对应的李代数 $\mathfrak{se}(3)$，其元素是 $4 \times 4$ 的矩阵：

$$\mathfrak{se}(3) = \left\{ \left. \begin{pmatrix} [\bolds