数学基础回顾：向量、矩阵、叉积、点积、坐标变换、旋转矩阵、齐次变换矩阵及其在机器人运动学中的应用

机器人动力学与建模仿真的核心，是描述刚体在三维空间中的位置、姿态以及它们随时间的变化规律。无论是六轴工业机械臂的轨迹规划，还是移动机器人的里程计解算，其底层数学工具都高度统一：向量与矩阵用于表达物理量，点积与叉积用于计算投影和力矩，旋转矩阵与齐次变换矩阵则用于描述坐标系之间的映射关系。本章将系统梳理这些数学工具，并重点阐明它们在机器人运动学（正解、逆解、速度雅可比）中的具体落点，为后续的动力学建模（拉格朗日方程、牛顿-欧拉递推）奠定坚实的符号与逻辑基础。

3.1 向量与矩阵：机器人建模的“语言”

3.1.1 核心原理：从标量到多维空间

在机器人学中，单一标量无法描述三维空间中的位置、速度或力。向量（Vector）是既有大小又有方向的量，通常用列矩阵表示，例如位置向量 p = [x, y, z]^T。矩阵（Matrix）则是线性变换的紧凑表示，用于描述旋转、缩放、投影等操作。在机器人运动学中，矩阵的乘法天然对应坐标变换的复合，这是齐次变换矩阵能够串联多个关节的基础。

3.1.2 点积与叉积：物理意义的几何解释

点积（内积）定义为 a·b = |a||b|cosθ，其结果是一个标量。在机器人学中，点积用于计算力在位移方向上的做功（W = F·d），以及判断两个向量是否正交（点积为零）。叉积（外积）定义为 a×b = |a||b|sinθ·n，其结果是一个垂直于原两向量所在平面的新向量。叉积在机器人学中至关重要：力矩 τ = r×F，角速度与线速度的关系 v = ω×r，以及雅可比矩阵中旋转分量的构造都依赖叉积。

3.2 旋转矩阵与坐标变换

3.2.1 核心原理：SO(3) 群与正交约束

旋转矩阵 R ∈ SO(3) 描述了两个坐标系之间的姿态关系，满足正交性 R^TR = I 和行列式 det(R) = +1。旋转矩阵的每一列表示被旋转坐标系各轴在原坐标系中的方向余弦。例如，绕 Z 轴旋转 θ 角的旋转矩阵为：

Rz(θ) = [cosθ  -sinθ  0;
          sinθ   cosθ  0;
          0      0     1]

旋转矩阵的逆等于其转置（R^-1 = R^T），这一性质使得坐标反变换变得极为简单。

3.2.2 操作流程：坐标变换的标准步骤

1. 确定参考坐标系：明确是固定坐标系（世界系）还是运动坐标系（工具系）。
2. 选择旋转顺序：固定角（Fixed Angles）与欧拉角（Euler Angles）的旋转顺序不同，结果也不同。例如 ZYX 欧拉角与 XYZ 固定角互为转置关系。
3. 构造旋转矩阵：根据旋转轴和角度，使用基本旋转矩阵 R_x、R_y、R_z 的乘积。
4. 应用变换：对于点 p 在新坐标系中的坐标 p'，有 p' = R·p（主动旋转）或 p' = R^T·p（被动旋转），需注意约定。

3.2.3 案例拆解：六轴机械臂腕部姿态描述

考虑一个六轴关节臂的腕部，其姿态由最后三个关节决定。假设末端工具坐标系 {T} 相对于腕部坐标系 {W} 的姿态为：先绕 Z_W 旋转 30°，再绕新的 Y 轴旋转 45°，最后绕新的 X 轴旋转 60°（ZYX 欧拉角）。则旋转矩阵为：

R = Rz(30°) · Ry(45°) · Rx(60°)

计算得到的具体数值矩阵即为工具坐标系在腕部坐标系中的姿态描述。若已知腕部在世界系中的姿态 R_W，则工具在世界系中的姿态为 R_T = R_W · R。这种复合规则是正运动学的基础。

3.3 齐次变换矩阵：统一位置与姿态

3.3.1 核心原理：SE(3) 与 4×4 矩阵

齐次变换矩阵 T ∈ SE(3) 将旋转和平移统一在一个 4×4 矩阵中：

T = [ R   p ;
      0   1 ]

其中 R 是 3×3 旋转矩阵，p 是 3×1 平移向量。齐次变换矩阵的逆为：

T⁻¹ = [ Rᵀ  -Rᵀ·p ;
         0    1    ]

这一形式使得坐标变换的复合（T₁₃ = T₁₂·T₂₃）和求逆运算变得非常高效，是机器人运动学正解的标准工具。

3.3.2 操作流程：建立 DH 参数表与正运动学

Denavit-Hartenberg（DH）参数法是最常用的运动学建模方法。其流程为：

1. 为每个关节建立坐标系（Z 轴沿关节轴方向）。
2. 确定四个 DH 参数：a_i-1（连杆长度）、α_i-1（连杆扭转角）、d_i（关节偏距）、θ_i（关节角）。
3. 构造相邻坐标系的齐次变换矩阵 T_i^i-1 = R_x(α_i-1)·D_x(a_i-1)·R_z(θ_i)·D_z(d_i)。
4. 正运动学：T_n⁰ = T₁⁰·T₂¹·…·T_n^n-1。

3.3.3 实战场景：平面两连杆机械臂的正解

考虑一个平面两连杆机械臂，连杆长度分别为 L₁ = 1.0m，L₂ = 0.8m，关节角 θ₁ = 30°，θ₂ = 45°。使用 DH 参数法，各关节的齐次变换矩阵为：

T₁⁰ = [cos30  -sin30  0  L₁cos30;
        sin30   cos30  0  L₁sin30;
        0       0      1  0;
        0       0      0  1]

T₂¹ = [cos45  -sin45  0  L₂cos45;
        sin45   cos45  0  L₂sin45;
        0       0      1  0;
        0       0      0  1]

末端位置为 T₂⁰ = T₁⁰·T₂¹ 的平移部分，计算得：

x = L₁cosθ₁ + L₂cos(θ₁+θ₂) = 1.0×0.866 + 0.8×cos75° ≈ 1.073m
y = L₁sinθ₁ + L₂sin(θ₁+θ₂) = 1.0×0.5 + 0.8×sin75° ≈ 1.273m

该结果与几何法完全一致，验证了齐次变换矩阵的正确性。

3.4 常见误区与总结复盘