数学基础：机器人学的语言与工具 / Mathematical Foundations: The Language and Tools of Robotics

第 1 章：数学基础：机器人学的语言与工具

1.1 概述

当我们凝视一台工业机械臂完成精密装配任务时，或许不会立刻意识到：那些流畅而精准的动作背后，是一套完整而优美的数学语言在默默指挥。从末端执行器的位置控制，到多关节的协调运动，再到复杂环境中的自主导航，机器人学的每一个分支都深深植根于数学的土壤。正如学习任何一门专业学科需要掌握其术语和工具一样，机器人学的学习者必须首先建立扎实的数学基础。

本章将系统介绍机器人学中最重要的数学工具，为后续章节的学习铺平道路。我们将从最基础的向量与矩阵运算开始，逐步深入到刚体变换与旋转表示的核心内容，最后引入状态估计中不可或缺的最小二乘法原理。在整个学习过程中，我们将始终注重数学概念的几何直观理解，并通过 Python 代码示例帮助读者建立实践能力。

值得强调的是，数学在机器人学中绝非抽象的理论游戏，而是解决实际问题的有力武器。每一项数学技术都有其明确的物理意义和应用场景，理解这一点将帮助读者不仅“学会”这些工具，更能“用好”它们。

1.2 向量与向量空间

1.2.1 向量的基本概念

向量（Vector）是机器人学中最基本也是最重要的数学对象之一。在物理世界中，向量用来表示同时具有大小和方向的量，如力、速度、加速度等。在数学上，我们可以将向量理解为有序的数值序列。

在二维空间中，一个向量可以表示为： $$\mathbf{v} = \begin{bmatrix} v_x \\ v_y \end{bmatrix}$$

在三维空间中： $$\mathbf{v} = \begin{bmatrix} v_x \\ v_y \\ v_z \end{bmatrix}$$

在机器人学的上下文中，向量的含义远比数值序列丰富。一个三维向量可能表示：

• **位置**：空间中某点的坐标，如机械臂末端执行器在基坐标系中的位置
• **方向**：如相机光轴指向或某个关节的旋转轴方向
• **速度**：物体运动的瞬时速度矢量
• **力/力矩**：作用在机器人上的外力和外力矩

这种语义上的多样性要求我们不仅要掌握向量的数学运算，更要理解不同场景下向量的物理含义。

1.2.2 向量的基本运算

向量的加法和数乘是最基本的运算。给定两个向量 $\mathbf{a}$ 和 $\mathbf{b}$，它们的加法定义为对应分量相加：

$$\mathbf{a} + \mathbf{b} = \begin{bmatrix} a_1 + b_1 \\ a_2 + b_2 \\ \vdots \\ a_n + b_n \end{bmatrix}$$

数乘运算则是将标量与向量相乘：

$$k \cdot \mathbf{a} = \begin{bmatrix} k \cdot a_1 \\ k \cdot a_2 \\ \vdots \\ k \cdot a_n \end{bmatrix}$$

这两个运算满足向量空间的基本公理：结合律、交换律、零向量存在、加法逆元存在等。

向量的点积（Dot Product，也称内积）是另一个核心运算。对于两个 $n$ 维向量：

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

其中 $\theta$ 是两个向量之间的夹角，$\|\mathbf{a}\|$ 表示向量 $\mathbf{a}$ 的模（长度）。点积的几何意义非常重要：它可以用来计算向量之间的夹角、判断两个向量是否正交（点积为零），以及计算向量在某方向上的投影长度。

向量的叉积（Cross Product）仅在三维空间中有定义：

$$\mathbf{a} \times \mathbf{b} = \begin{bmatrix} 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{bmatrix}$$

叉积的结果是一个与 $\mathbf{a}$ 和 $\mathbf{b}$ 都正交的新向量，其方向由右手定则确定。叉积的模等于两个向量张成的平行四边形面积：

$$\|\mathbf{a} \times \mathbf{b}\| = \|\mathbf{a}\| \|\mathbf{b}\| \sin\theta$$

在机器人学中，叉积被广泛应用于计算力矩、确定旋转轴方向、以及表示刚体的角速度。

1.2.3 向量空间与基

向量空间（Vector Space）是由向量组成的集合，在这个集合中，向量的加法和数乘运算都是封闭的，并且满足特定的公理。理解向量空间的概念对于理解坐标系变换至关重要。

向量空间中的基（Basis）是一组线性无关的向量，空间中的任意向量都可以唯一地表示为这组基向量的线性组合。基的选择不是唯一的，不同的基对应着不同的坐标系。例如，标准基 $\{ \mathbf{e}_1, \mathbf{e}_2, \mathbf{e}_3 \}$ 对应着我们常用的直角坐标系，其中：

$$\mathbf{e}_1 = \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix}, \quad \mathbf{e}_2 = \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix}, \quad \mathbf{e}_3 = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}$$

在机器人学中，我们经常需要在不同坐标系之间切换。一个向量在不同坐标系中的坐标不同，但向量本身是客观存在的。这种相对性是理解刚体变换的基础。

1.2.4 线性无关与正交基

一组向量 $\{ \mathbf{v}_1, \mathbf{v}_2, \ldots, \mathbf{v}_n \}$ 被称为线性无关，如果不存在不全为零的标量 $c_1, c_2, \ldots, c_n$ 使得：

$$c_1 \mathbf{v}_1 + c_2 \mathbf{v}_2 + \cdots + c_n \mathbf{v}_n = \mathbf{0}$$

直观地说，线性无关意味着这些向量指向不同的方向，没有任何冗余。$n$ 维空间中任意 $n$ 个线性无关的向量都构成一组基。

正交基（Orthogonal Basis）是一组两两正交的基向量，即任意两个不同基向量的点积为零。使用正交基有很多便利之处：向量的坐标可以直接通过点积计算，坐标分量之间没有耦合，便于分析和计算。

更进一步的规范正交基（Orthonormal Basis）要求每个基向量的模为1。在机器人学中，我们通常使用规范正交基来描述坐标系，这使得坐标变换的数学表达更加简洁优美。

1.3 矩阵基础

1.3.1 矩阵的定义与运算

矩阵（Matrix）是由数排列成的矩形数组，是线性代数的核心研究对象。在机器人学中，矩阵被广泛应用于表示线性变换、坐标系变换、以及联立方程组的求解。

一个 $m \times n$ 的矩阵 $A$ 有 $m$ 行 $n$ 列：

$$A = \begin{bmatrix} a_{11} & a_{12} & \cdots & a_{1n} \\ a_{21} & a_{22} & \cdots & a_{2n} \\ \vdots & \vdots & \ddots & \vdots \\ a_{m1} & a_{m2} & \cdots & a_{mn} \end{bmatrix}$$

矩阵的加法要求两个矩阵具有相同的维度，对应元素相加。矩阵的数乘将标量与矩阵的每个元素相乘。矩阵乘法是更为重要的运算：$C = AB$，其中 $C_{ij} = \sum_{k} A_{ik} B_{kj}$。

理解矩阵乘法的几何意义非常重要。当我们用矩阵乘以一个向量时，实际上是在对这个向量进行某种线性变换——可能包括旋转、缩放、剪切等操作。连续应用多个矩阵变换相当于这些变换的复合：

$$\mathbf{v}' = A(B\mathbf{v}) = (AB)\mathbf{v}$$

1.3.2 矩阵乘法的性质与注意事项

矩阵乘法满足结合律：$(AB)C = A(BC)$，但一般情况下不满足交换律：$AB \neq BA$。这种非交换性在机器人学中有重要含义：先旋转后平移与先平移后旋转通常会产生不同的结果。

在编程实现中，需要特别注意矩阵乘法的维度匹配。设 $A$ 是 $m \times n$ 矩阵，$B$ 是 $n \times p$ 矩阵，则乘积 $AB$ 是 $m \times p$ 矩阵。以下 Python 代码演示了基本的矩阵运算：

```python import numpy as np

定义两个矩阵

A = np.array([[1, 2], [3, 4]]) B = np.array([[5, 6], [7, 8]])

矩阵乘法

C = np.dot(A, B) # 或者 A @ B print("A @ B =") print(C)

矩阵与向量乘法

v = np.array([1, 2]) result = A @ v print("A @ v =", result)

注意矩阵乘法通常不满足交换律

print("A @ B =", A @ B) print("B @ A =", B @ A) print("Are they equal?", np.allclose(A @ B, B @ A)) ```

1.3.3 特殊矩阵类型

在机器人学中，有几类特殊矩阵需要特别关注。

方阵（Square Matrix）：行数和列数相等的矩阵。某些重要操作（如行列式、逆矩阵）只在方阵上有定义。

转置矩阵（Transpose）：矩阵 $A$ 的转置 $A^T$ 将行变成列、列变成行。对于向量来说，转置将列向量变成行向量：$\mathbf{v}^T = [v_1, v_2, \ldots, v_n]$。

对称矩阵（Symmetric Matrix）：满足 $A = A^T$ 的方阵。对称矩阵在物理上通常对应于某些守恒量或平衡关系。

正交矩阵（Orthogonal Matrix）：满足 $Q^T Q = Q Q^T = I$ 的方阵。正交矩阵的列（和行）是两两正交的单位向量。正交矩阵的一个重要性质是它的转置等于它的逆：$Q^T = Q^{-1}$。在后续学习中我们将看到，旋转矩阵正是正交矩阵的典型代表。

反对称矩阵（Skew-symmetric Matrix）：满足 $A^T = -A$ 的方阵。反对称矩阵在机器人学中常用于表示旋量的生成元。

1.3.4 矩阵的秩与线性方程组

矩阵的秩（Rank）定义为矩阵中线性无关的行（或列）的最大数目。秩给出了许多重要信息：

• 对于 $m \times n$ 矩阵 $A$，方程组 $A\mathbf{x} = \mathbf{b}$ 有解当且仅当 $\text{rank}(A) = \text{rank}([A|\mathbf{b}])$
• 解的唯一性取决于 $\text{rank}(A)$ 与 $n$ 的关系
• 满秩（$\text{rank}(A) = \min(m,n)$）的矩阵具有某些良好性质

在机器人运动学中，矩阵的秩有明确的物理含义。雅可比矩阵的秩决定了机器人末端执行器能够实现的速度方向的数量——即所谓的可操作性问题。

1.4 行列式与逆矩阵

1.4.1 行列式的定义与性质

行列式（Determinant）是一个与方阵相关的标量值，记作 $\det(A)$ 或 $|A|$。行列式有很多重要的几何和代数意义。

对于 $2 \times 2$ 矩阵：

$$\det\begin{pmatrix} a & b \\ c & d \end{pmatrix} = ad - bc$$

对于 $3 \times 3$ 矩阵，可以使用拉普拉斯展开或萨吕法则计算。

行列式具有以下重要性质：

1. $\det(AB) = \det(A)\det(B)$ 2. $\det(A^T) = \det(A)$ 3. $\det(cA) = c^n \det(A)$，其中 $n$ 是矩阵的维度 4. 矩阵可逆的充分必要条件是 $\det(A) \neq 0$

行列式的几何意义是：它给出了矩阵对应的线性变换对“体积”的缩放因子。对于 $2 \times 2$ 矩阵，行列式的绝对值等于平行四边形的面积；对于 $3 \times 3$ 矩阵，等于平行六面体的体积。

1.4.2 逆矩阵的概念与计算

如果 $A$ 是一个 $n \times n$ 方阵，且 $\det(A) \neq 0$，则存在唯一的矩阵 $A^{-1}$ 使得：

$$A A^{-1} = A^{-1} A = I$$

其中 $I$ 是 $n \times n$ 单位矩阵。$A^{-1}$ 称为 $A$ 的逆矩阵（Inverse Matrix）。

逆矩阵在机器人学中有重要应用。例如，在坐标变换中，如果我们知道如何从坐标系 A 到坐标系 B 的变换矩阵，那么从 B 到 A 的变换就是该矩阵的逆。

逆矩阵可以通过多种方法计算：

• 伴随矩阵法：$A^{-1} = \frac{1}{\det(A)} \text{adj}(A)$
• 高斯-乔丹消元法
• LU 分解法
• QR 分解法

在数值计算中，直接计算逆矩阵通常不是最高效的方法。对于求解线性方程组 $A\mathbf{x} = \mathbf{b}$，更推荐使用矩阵分解（如 LU 分解）或其他专门的算法，而非显式计算 $A^{-1}$。

```python import numpy as np

A = np.array([[4, 7], [2, 6]])

计算行列式

det_A = np.linalg.det(A) print(f"行列式: {det_A:.4f}")

计算逆矩阵

if det_A != 0: A_inv = np.linalg.inv(A) print("逆矩阵:") print(A_inv)

验证 AA^(-1) = I

identity = A @ A_inv print("A @ A^(-1) =") print(identity) ```

1.4.3 伪逆矩阵与最小范数解

当矩阵不是方阵，或虽然是方阵但不可逆时，我们需要引入伪逆（Pseudoinverse，也称 Moore-Penrose 逆）的概念。

对于任意 $m \times n$ 矩阵 $A$，其伪逆 $A^+$ 满足以下四个彭罗斯条件：

1. $A A^+ A = A$ 2. $A^+ A A^+ = A^+$ 3. $(A A^+)^T = A A^+$ 4. $(A^+ A)^T = A^+ A$

当 $A$ 是列满秩的 $m \times n$ 矩阵（$m > n$，$\text{rank}(A) = n$）时，伪逆有闭式解：

$$A^+ = (A^T A)^{-1} A^T$$

这个公式在后续学习最小二乘法时将再次出现。

伪逆矩阵在求解欠定方程组（方程数少于未知数）或矛盾方程组（无精确解）时特别有用，它能够给出在最小二乘意义上最优的解。

1.5 刚体变换基础

1.5.1 刚体的数学描述

刚体（Rigid Body）是在运动和受力过程中形状和大小保持不变的物体。在机器人学中，我们通常将机器人连杆、工具、传感器载体等都视为刚体进行分析。

刚体在空间中的位姿（Pose）由两个部分组成：

1. 位置（Position）：刚体上某参考点（如质心或坐标系原点）的三维坐标 2. 姿态（Orientation）：刚体相对于某个参考坐标系的旋转方向

数学上，刚体的位姿可以统一表示为一个 $4 \times 4$ 的齐次变换矩阵（Homogeneous Transformation Matrix）：

$$T = \begin{bmatrix} R & \mathbf{p} \\ \mathbf{0}^T & 1 \end{bmatrix}$$

其中 $R$ 是 $3 \times 3$ 的旋转矩阵（Rotation Matrix），$\mathbf{p}$ 是 $3 \times 1$ 的位置向量（Position Vector）。

这种表示方式的美妙之处在于：它将旋转和平移统一在一个矩阵中，使得复合变换可以用简单的矩阵乘法实现。

1.5.2 齐次坐标与齐次变换

齐次坐标（Homogeneous Coordinates）是处理平移变换的有力工具。对于三维空间中的一个点 $\mathbf{p} = (p_x, p_y, p_z)^T$，其齐次坐标为 $\tilde{\mathbf{p}} = (p_x, p_y, p_z, 1)^T$。增加的最后一个坐标“1”纯粹是为了数学处理上的便利。

使用齐次坐标，一个通用的刚体变换可以写成：

$$\begin{bmatrix} \mathbf{p}' \\ 1 \end{bmatrix} = \begin{bmatrix} R & \mathbf{p} \\ \mathbf{0}^T & 1 \end{bmatrix} \begin{bmatrix} \mathbf{p} \\ 1 \end{bmatrix} = T \mathbf{p}$$

其中 $R$ 是 $3 \times 3$ 旋转矩阵，$\mathbf{p}$ 是 $3 \times 1$ 平移向量。

齐次变换矩阵具有以下性质：

• $T$ 的左上角 $3 \times 3$ 子矩阵是旋转矩阵 $R$，满足 $R^T R = I$ 和 $\det(R) = 1$
• $T$ 的逆矩阵为：

$$T^{-1} = \begin{bmatrix} R^T & -R^T \mathbf{p} \\ \mathbf{0}^T & 1 \end{bmatrix}$$

1.5.3 坐标系变换的几何意义

考虑两个坐标系 $\{A\}$ 和 $\{B\}$。空间中的任意点 $\mathbf{p}$ 在这两个坐标系中的坐标关系为：

$$\mathbf{p}_A = R_{AB} \mathbf{p}_B + \mathbf{p}_{A, B_{origin}}$$

其中 $R_{AB}$ 表示从 $\{B\}$ 到 $\{A\}$ 的旋转变换，$\mathbf{p}_{A, B_{origin}}$ 是 $\{