机器人学数学基础

机器人学数学基础

引言

机器人技术的核心问题可以归结为三个基本问题：机器人如何感知周围环境（感知），如何规划从当前位置到目标位置的轨迹（规划），以及如何精确控制各关节执行期望运动（控制）。这三个问题的解决都建立在坚实的数学基础之上。机器人学之所以被认为是一个高度数学化的领域，正是因为它需要借助向量空间、矩阵分析、李群李代数等数学工具来精确描述刚体的位姿、运动和相互作用。

本章将系统回顾机器人学必备的数学工具，为后续章节中姿态估计、运动建模、视觉伺服控制等内容奠定理论基础。我们将从向量空间与矩阵的基本运算出发，逐步深入到旋转矩阵、欧拉角、李群与李代数以及四元数的理论与应用。特别地，本章将重点阐述如何利用李代数进行旋转与变换的求导，这一技术是现代机器人学中姿态优化、视觉惯性里程计以及非线性控制器设计的基础。

掌握这些数学工具不仅有助于理解机器人学中的各种算法，更是进行机器人研究开发的必备技能。本章将通过大量例题演示李括号、指数映射与对数映射的计算方法，帮助读者建立起对机器人数学的直观理解。

向量空间与矩阵分析基础

向量空间的定义与性质

向量空间（Vector Space）是线性代数的研究对象，也是描述机器人状态空间的基本数学结构。一个向量空间由一个域（通常为实数域 $\mathbb{R}$ 或复数域 $\mathbb{C}$）和一个集合 $V$ 组成，集合中的元素称为向量，满足以下八条公理：

设 $V$ 为一向量空间，$u, v, w \in V$，$\alpha, \beta \in \mathbb{R}$，则：

1. 加法封闭性：$u + v \in V$ 2. 加法交换律：$u + v = v + u$ 3. 加法结合律：$(u + v) + w = u + (v + w)$ 4. 零向量存在：存在零向量 $\mathbf{0} \in V$，使得 $u + \mathbf{0} = u$ 5. 负向量存在：对每个 $u$，存在 $-u \in V$，使得 $u + (-u) = \mathbf{0}$ 6. 数乘封闭性：$\alpha u \in V$ 7. 数乘分配律：$\alpha(u + v) = \alpha u + \alpha v$，$(\alpha + \beta)u = \alpha u + \beta u$ 8. 数乘结合律：$(\alpha\beta)u = \alpha(\beta u)$，$1 \cdot u = u$

在机器人学中，最常用的向量空间是 $\mathbb{R}^n$，即 $n$ 维实数向量空间。例如，三维空间中质点的位置可以用 $\mathbb{R}^3$ 中的向量表示，六维关节空间中的关节位置和速度同样可以用 $\mathbb{R}^6$ 中的向量描述。

线性变换与矩阵表示

线性变换（Linear Transformation）是向量空间之间保持向量加法和数乘运算的映射。设 $T: V \to W$ 为一线性变换，则对任意 $u, v \in V$ 和标量 $\alpha$，有：

$$T(u + v) = T(u) + T(v)$$ $$T(\alpha u) = \alpha T(u)$$

在有限维向量空间中，每个线性变换都可以用一个矩阵唯一表示。设 $T: \mathbb{R}^n \to \mathbb{R}^m$ 为一线性变换，$\{e_1, e_2, \ldots, e_n\}$ 为 $\mathbb{R}^n$ 的标准基，则矩阵 $A \in \mathbb{R}^{m \times n}$ 的第 $j$ 列为 $T(e_j)$。

矩阵的秩（Rank）是线性变换的重要不变量。对于矩阵 $A \in \mathbb{R}^{m \times n}$，其秩定义为行空间或列空间的维度。机器人学中，矩阵的秩常用于判断机构是否奇异。当雅可比矩阵 $J(q)$ 满秩时，机构处于非奇异构型；当雅可比矩阵降秩时，对应奇异构型，此时某些方向上的运动变得不可控。

例题 1.1：已知机械臂雅可比矩阵 $J \in \mathbb{R}^{6 \times n}$，判断以下两种构型是否为奇异构型。

$$J(q_1) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ 2 & 1 & 3 \\ 1 & 2 & 1 \\ 0 & 0 & 0 \end{bmatrix}, \quad J(q_2) = \begin{bmatrix} 1 & 0 \\ 0 & 1 \\ 1 & 0 \end{bmatrix}$$

解：计算两个矩阵的秩。

对于 $J(q_1)$，观察可得前三行线性无关（构成单位阵的一部分），因此 $\text{rank}(J(q_1)) = 3$，满秩（若 $n=3$），非奇异构型。

对于 $J(q_2)$，两列向量线性无关，$\text{rank}(J(q_2)) = 2$，满秩，非奇异构型。

矩阵的范数与条件数

矩阵范数（Matrix Norm）是衡量矩阵"大小"的度量，对于机器人学中的数值稳定性分析至关重要。常用的矩阵范数包括：

• **Frobenius 范数**：$\|A\|_F = \sqrt{\sum_{i=1}^m \sum_{j=1}^n |a_{ij}|^2} = \sqrt{\text{tr}(A^T A)}$
• **谱范数**：$\|A\|_2 = \sqrt{\lambda_{\max}(A^T A)}$，即 $A^T A$ 最大特征值的平方根
• **$\ell_1$ 范数**：$\|A\|_1 = \max_{1 \leq j \leq n} \sum_{i=1}^m |a_{ij}|$

矩阵的条件数（Condition Number）定义为 $\kappa(A) = \|A\| \cdot \|A^{-1}\|$，反映了线性方程组 $Ax = b$ 解对系数扰动的敏感程度。当 $\kappa(A)$ 很大时，矩阵接近奇异，数值计算可能不稳定。在机械臂动力学参数辨识中，高条件数的辨识方程会导致参数估计精度下降。

特殊矩阵类型

对称矩阵：满足 $A = A^T$ 的矩阵。对称矩阵的特征值均为实数，且存在正交特征向量系。在机器人动力学中，质量矩阵、刚度矩阵都是对称矩阵。

反对称矩阵（Skew-symmetric Matrix）：满足 $A^T = -A$ 的矩阵。反对称矩阵的对角元素为零，每个反对称矩阵可以唯一对应一个三维向量。这一性质在旋转表示中具有重要作用：

$$[x]_\times = \begin{bmatrix} 0 & -x_3 & x_2 \\ x_3 & 0 & -x_1 \\ -x_2 & x_1 & 0 \end{bmatrix}$$

这里 $[x]_\times$ 表示向量 $x = (x_1, x_2, x_3)^T$ 对应的反对称矩阵。反对称矩阵的指数映射给出了旋转矩阵，这一性质将在后续章节详细讨论。

正交矩阵：满足 $R^T R = I$ 的矩阵。正交矩阵的列（行）向量构成标准正交基，行列式为 $\pm 1$。旋转矩阵是正交矩阵的典型代表，且行列式必须为 $+1$。

旋转矩阵

旋转矩阵的定义与性质

旋转矩阵（Rotation Matrix）是表示刚体姿态的核心数学工具。三维旋转矩阵的集合构成特殊正交群 $SO(3)$：

$$SO(3) = \{R \in \mathbb{R}^{3 \times 3} \mid R^T R = I, \det(R) = +1\}$$

旋转矩阵具有以下重要性质：

1. 正交性：$R^T = R^{-1}$，即转置等于逆 2. 行列式：$\det(R) = +1$ 3. 保持向量长度：$\|Rv\| = \|v\|$，对任意 $v \in \mathbb{R}^3$ 4. 保持内积：$\langle R u, R v \rangle = \langle u, v \rangle$ 5. 保持叉积方向：$R(u \times v) = Ru \times Rv$ 6. 闭合性：两个旋转矩阵的乘积仍是旋转矩阵

例题 1.2：验证矩阵 $R_z(\theta)$ 是否为有效的旋转矩阵：

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

解：验证正交性：

$$R_z(\theta)^T R_z(\theta) = \begin{bmatrix} \cos\theta & \sin\theta & 0 \\ -\sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} = I$$

计算行列式：

$$\det(R_z(\theta)) = \cos^2\theta + \sin^2\theta = 1$$

因此 $R_z(\theta) \in SO(3)$。

旋转矩阵的合成与逆

旋转矩阵的乘法对应旋转的合成。设 $R_1, R_2 \in SO(3)$ 分别表示先进行旋转 $R_1$ 再进行旋转 $R_2$，则合成后的旋转为 $R_2 R_1$。注意旋转合成不满足交换律，即 $R_1 R_2 \neq R_2 R_1$。

旋转矩阵的逆即其转置：$R^{-1} = R^T$。几何上，$R^T$ 表示与 $R$ 相反方向的旋转，即按相同角度绕相同轴反方向旋转。

在实际机器人应用中，经常需要在不同坐标系之间转换点的坐标。设点 $P$ 在坐标系 $\{A\}$ 中的坐标为 $^AP$，在坐标系 $\{B\}$ 中的坐标为 $^BP$，两坐标系之间的姿态关系由旋转矩阵 $_B^AR$ 表示，则：

$$^AP = \,_B^AR \, ^BP$$

注意符号约定：$_B^AR$ 表示从 $\{B\}$ 到 $\{A\}$ 的旋转变换，即 $_B^AR \, ^BP = \, ^AP$。

旋转矩阵的几何解释

旋转矩阵的每一列（行）给出了目标坐标系各坐标轴在原坐标系中的表示。以 $_B^AR$ 为例：

• 第一列：坐标系 $\{B\}$ 的 $x$ 轴在 $\{A\}$ 中的方向
• 第二列：坐标系 $\{B\}$ 的 $y$ 轴在 $\{A\}$ 中的方向
• 第三列：坐标系 $\{B\}$ 的 $z$ 轴在 $\{A\}$ 中的方向

这一几何解释对于理解坐标变换具有重要意义。当需要判断一个旋转矩阵是否"正确"时，可以检查其列向量是否构成右手坐标系且长度为1。

例题 1.3：给定坐标系 $\{B\}$ 绕坐标系 $\{A\}$ 的 $z$ 轴旋转 $30^\circ$，求 $_B^AR$。

解：按照右手定则，绕 $z$ 轴旋转时，$x$ 轴和 $y$ 轴的方向会改变。应用几何推导：

• 旋转后 $\{B\}$ 的 $x$ 轴：$(\cos30^\circ, \sin30^\circ, 0) = (\frac{\sqrt{3}}{2}, \frac{1}{2}, 0)$
• 旋转后 $\{B\}$ 的 $y$ 轴：$(-\sin30^\circ, \cos30^\circ, 0) = (-\frac{1}{2}, \frac{\sqrt{3}}{2}, 0)$
• 旋转后 $\{B\}$ 的 $z$ 轴：$(0, 0, 1)$（保持不变）

因此：

$$_B^AR = \begin{bmatrix} \frac{\sqrt{3}}{2} & -\frac{1}{2} & 0 \\ \frac{1}{2} & \frac{\sqrt{3}}{2} & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

欧拉角

欧拉角的概念与约定

欧拉角（Euler Angles）用三个角度参数化旋转，提供了一种直观且存储效率高的旋转表示方法。然而，由于旋转的不可交换性，不同的欧拉角约定会产生不同的旋转顺序和轴定义。

常用的欧拉角约定包括：

• **ZYZ**：绕 $z$ 轴旋转 $\alpha$，再绕新的 $y$ 轴旋转 $\beta$，最后绕新的 $z$ 轴旋转 $\gamma$
• **ZYX**（常用于航空航天）：绕 $z$ 轴旋转 $\psi$，再绕新的 $y$ 轴旋转 $\theta$，最后绕新的 $x$ 轴旋转 $\phi$
• **XYZ**（常用于机器人学）：绕 $z$ 轴旋转 $\alpha$，再绕新的 $y$ 轴旋转 $\beta$，最后绕新的 $x$ 轴旋转 $\gamma$

本书约定使用 ZYX 欧拉角（Roll-Pitch-Yaw），因为该约定与大多数工业机器人的配置一致。

ZYX 欧拉角的旋转矩阵

ZYX 欧拉角 $(\phi, \theta, \psi)$ 对应的旋转矩阵为：

$$R_{ZYX}(\phi, \theta, \psi) = R_z(\phi) R_y(\theta) R_x(\psi)$$

展开后得到：

$$R = \begin{bmatrix} \cos\theta\cos\phi & \sin\psi\sin\theta\cos\phi - \cos\psi\sin\phi & \cos\psi\sin\theta\cos\phi + \sin\psi\sin\phi \\ \cos\theta\sin\phi & \sin\psi\sin\theta\sin\phi + \cos\psi\cos\phi & \cos\psi\sin\theta\sin\phi - \sin\psi\cos\phi \\ -\sin\theta & \sin\psi\cos\theta & \cos\psi\cos\theta \end{bmatrix}$$

其中：

• $\phi$（Roll）：绕 $z$ 轴旋转的角度
• $\theta$（Pitch）：绕 $y$ 轴旋转的角度
• $\psi$（Yaw）：绕 $x$ 轴旋转的角度

例题 1.4：已知欧拉角 $(\phi, \theta, \psi) = (30^\circ, 45^\circ, 60^\circ)$，求对应的旋转矩阵。

解：将角度转换为弧度并计算：

$\phi = 30^\circ = \pi/6$，$\theta = 45^\circ = \pi/4$，$\psi = 60^\circ = \pi/3$

计算各元素：

$R_{31} = -\sin\theta = -\frac{\sqrt{2}}{2} \approx -0.7071$

$R_{32} = \sin\psi\cos\theta = \sin(60^\circ)\cos(45^\circ) = \frac{\sqrt{3}}{2} \cdot \frac{\sqrt{2}}{2} = \frac{\sqrt{6}}{4} \approx 0.6124$

$R_{33} = \cos\psi\cos\theta = \cos(60^\circ)\cos(45^\circ) = \frac{1}{2} \cdot \frac{\sqrt{2}}{2} = \frac{\sqrt{2}}{4} \approx 0.3536$

完整矩阵需要按公式计算所有九个元素。

欧拉角与旋转矩阵的转换

从旋转矩阵提取欧拉角需要处理万向锁（Gimbal Lock）问题。以 ZYX 为例，当 $\theta = \pm 90^\circ$ 时，$R_{31} = \mp 1$，出现奇异性，此时 $\phi$ 和 $\psi$ 无法唯一确定。

提取 ZYX 欧拉角的算法：

``` if |R31| < 0.99999: θ = -asin(R31) ψ = atan2(R32 / cosθ, R33 / cosθ) φ = atan2(R21 / cosθ, R11 / cosθ) else:

万向锁情况

if R31 > 0: θ = -π/2 ψ = atan2(R12, R13) φ = 0 else: θ = π/2 ψ = atan2(-R12, -R13) φ = 0 ```

欧拉角的优缺点

优点：

• 参数少（仅需3个参数）
• 直观易懂，符合人类对角度的认知
• 适合人机交互界面

缺点：

• 存在奇异性（万向锁）
• 旋转合成不满足交换律，需要严格约定旋转顺序
• 插值不平滑，可能出现"跳跃"现象

由于奇异性问题，欧拉角不适合用于需要连续旋转的场合，如轨迹规划和姿态优化。在这些应用中，通常使用四元数或李群表示，需要时再转换为欧拉角进行显示或输入。

李群与李代数基础

群论基础

群（Group）是具有闭合运算的代数结构，是描述对称性的有力工具。形式上，一个群 $(G, \cdot)$ 由集合 $G$ 和二元运算 $\cdot$ 组成，满足：

1. 封闭性：若 $a, b \in G$，则 $a \cdot b \in G$ 2. 结合律：$(a \cdot b) \cdot c = a \cdot (b \cdot c)$ 3. 单位元存在：存在 $e \in G$，使得 $e \cdot a = a \cdot e = a$ 4. 逆元存在：对每个 $a \in G$，存在 $a^{-1} \in G$，使得 $a \cdot a^{-1} = a^{-1} \cdot a = e$

特殊正交群 $SO(3)$ 和特殊欧几里得群 $SE(3)$ 是机器人学中最重要的两个李群。

李代数的定义

李代数（Lie Algebra）是李群在单位元处的切空间，提供了研究李群局部结构的线性化工具。每个李群都对应一个李代数，它们之间存在指数映射和与之对数映射的对应关系。

李代数 $\mathfrak{g}$ 的定义：一个