3、数学基础：向量、矩阵、旋转矩阵、齐次变换矩阵、欧拉角与四元数

机械臂运动学的本质是描述刚体在三维空间中的位置与姿态。无论是正运动学求解末端位姿，还是逆运动学反解关节角，都离不开严谨的数学工具。本章将系统梳理向量与矩阵的基本运算、旋转矩阵的构造与性质、齐次变换矩阵的工程意义，以及欧拉角与四元数在姿态描述中的优劣对比。这些内容不仅是后续DH参数建模、雅可比矩阵推导的基础，也是轨迹规划中插补算法实现的直接工具。

3.1 向量与矩阵：刚体位置与线性变换的基础

3.1.1 核心原理

在机械臂运动学中，空间中的点用三维列向量 p = [x, y, z]ᵀ 表示。向量既有大小又有方向，其点积与叉积分别用于投影计算和构造旋转轴。矩阵则是线性变换的紧凑表达，一个 3×3 矩阵可以将向量映射到新位置。对于刚体而言，位置变化用向量加法描述，姿态变化用矩阵乘法描述。

矩阵乘法不满足交换律，这直接对应了旋转顺序的不可交换性——先绕X轴转90°再绕Y轴转90°，与先绕Y轴后绕X轴的结果完全不同。这一性质在后续欧拉角约定中至关重要。

3.1.2 判断方法与操作流程

判断一个矩阵是否为合法的旋转矩阵，需验证两个条件：

• 正交性：RᵀR = I，即矩阵的逆等于其转置。
• 行列式：det(R) = +1，保证是右手系旋转而非镜像。

实际操作中，当从传感器或CAD软件获取旋转数据时，应首先检查上述条件。若 det(R) = -1，说明数据存在镜像错误，需要修正。

3.1.3 案例拆解：向量叉积在求解旋转轴中的应用

假设机械臂末端需要从当前姿态向量 a = [1, 0, 0]ᵀ 旋转到目标向量 b = [0, 1, 0]ᵀ。旋转轴可由叉积求得：k = a × b = [0, 0, 1]ᵀ，旋转角度 θ = arccos(a·b) = 90°。这一计算是Rodrigues旋转公式的基础，也是很多工业机器人示教器内部姿态插补的底层逻辑。

3.1.4 常见误区与总结

3.2 旋转矩阵：姿态的完整数学表达

3.2.1 核心原理

旋转矩阵 R ∈ SO(3) 是一个 3×3 正交矩阵，其列向量分别表示旋转后坐标系各轴在原坐标系中的方向余弦。例如，绕Z轴旋转 θ 角的旋转矩阵为：

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

旋转矩阵的9个元素并非独立，受6个正交约束限制，因此实际自由度为3。这一冗余性在数据存储和插值计算中带来不便，推动了欧拉角和四元数的引入。

3.2.2 判断方法与操作流程

当需要从旋转矩阵提取旋转轴与角度时，可使用以下流程：

1. 计算旋转角 θ = arccos((trace(R) - 1) / 2)，其中 trace(R) 为矩阵迹。
2. 若 θ 接近 0，则无旋转；若 θ 接近 π，需特殊处理（矩阵对称部分求特征向量）。
3. 旋转轴 k = (1 / (2 sinθ)) · [r32-r23, r13-r31, r21-r12]ᵀ。

3.2.3 案例拆解：从旋转矩阵判断奇异位形

某六轴机器人腕部在特定构型下，其末端旋转矩阵的第三列变为 [0, 0, -1]ᵀ。此时 trace(R) = -1，计算得 θ = π，旋转轴提取公式分母为 0，出现数值不稳定。这正是腕部奇异点的数学特征。工程中通过监测 trace(R) 是否接近 -1 来预警奇异位形，并切换至四元数表示以避免数值问题。

3.2.4 常见误区与总结

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

3.3.1 核心原理

齐次变换矩阵 T 是一个 4×4 矩阵，将旋转矩阵与平移向量整合为一体：

T = [R  p;
     0  1]

其中 R 为 3×3 旋转矩阵，p 为 3×1 平移向量。齐次变换的核心优势在于：连续坐标变换可通过矩阵乘法直接串联，即 T_03 = T_01 · T_12 · T_23，无需分别处理旋转和平移。这正是机械臂正运动学中DH参数法的数学基础。

3.3.2 判断方法与操作流程

构建齐次变换矩阵的步骤：

1. 确定当前坐标系相对于参考坐标系的旋转矩阵 R。
2. 确定当前坐标系原点在参考坐标系中的位置向量 p。
3. 组合为 4×4 矩阵，最后一行固定为 [0, 0, 0, 1]。

验证 T 的正确性：左上 3×3 子块必须满足正交性，右下角为 1，其余位置为 0。

3.3.3 案例拆解：多关节链式变换

考虑一个两连杆机械臂，关节1绕Z轴旋转 θ₁，连杆长度 a₁；关节2绕Z轴旋转 θ₂，连杆长度 a₂。末端相对于基座的齐次变换矩阵为：

T_02 = T_01(θ₁, a₁) · T_12(θ₂, a₂)

其中 T_01 表示关节1的旋转和平移，T_12 表示关节2相对于关节1的变换。通过矩阵乘法，末端位置为：

x = a₁·cosθ₁ + a₂·cos(θ₁+θ₂)
y = a₁·sinθ₁ + a₂·sin(θ₁+θ₂)

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

3.3.4 常见误区与总结

3.4 欧拉角与四元数：两种姿态参数化方法

3.4.1 核心原理

欧拉角通过三次绕坐标轴的旋转来描述姿态，常见约定有 ZYX（RPY角）和 ZYZ。其优点是直观，三个角度分别对应偏航、俯仰、横滚。然而欧拉角存在万向锁问题：当中间旋转角为 ±90° 时，第一次和第三次旋转轴重合，丢失一个自由度。

四元数 q = w + xi + yj + zk 是四维单位向量，满足 w² + x² + y² + z² = 1。它通过一个旋转轴和一个旋转角描述姿态，无奇异性，且插值平滑（如SLERP），非常适合轨迹规划中的姿态过渡。

3.4.2 判断方法与操作流程

欧拉角与四元数的转换流程：

• 欧拉角 → 四元数：分别计算绕各轴的四元数，然后按顺序相乘。
• 四元数 → 旋转矩阵：使用标准公式构造 3×3 矩阵。
• 旋转矩阵 → 欧拉角：根据矩阵元素反解角度，注意判断万向锁情况。

工程中推荐：存储与传输用四元数，人机交互显示用欧拉角，内部计算用旋转矩阵。

3.4.3 案例拆解：轨迹规划中的姿态插值

某焊接机器人需要从姿态 A（四元数 q₁）平滑过渡到姿态 B（四元数 q₂）。若使用欧拉角线性插值，在中间点可能出现角度跳变；而使用四元数SLERP插值：

q(t) = (sin((1-t)θ) / sinθ) · q₁ + (sin(tθ) / sinθ) · q₂

其中 θ = arccos(q₁·q₂)。该插值保证旋转轴恒定、角速度均匀，焊接轨迹平滑无抖动。实际测试中，四元数插值的末端加速度波动比欧拉角插值降低约 40%。

3.4.4 常见误区与总结

3.5 知识结构总结