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

多轴插补算法的本质，是在多维空间中精确描述刀具或执行末端的位姿（位置与姿态），并沿着期望轨迹连续计算中间点。无论是三轴加工中心、五轴联动数控机床，还是六轴工业机器人，其运动学正解、逆解、插补点计算与速度规划，都建立在统一的数学语言之上。本章将系统梳理向量、矩阵、齐次坐标变换、欧拉角与四元数这五大数学工具，并重点说明它们在多轴运动控制中的具体表示方法与工程应用边界。

2.1 向量与矩阵：多轴空间的基本描述单元

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

在多轴运动系统中，向量用于表示位置、速度、加速度以及刀具轴线方向。一个三维向量 p = [x, y, z]^T 可以唯一确定笛卡尔空间中的一个点。对于五轴机床，除了刀尖点位置，还需要定义刀轴方向向量 o = [i, j, k]^T，该向量通常被归一化为单位向量。矩阵则用于描述旋转、缩放和错切等线性变换。一个 3×3 旋转矩阵 R 满足正交性（R^TR = I）且行列式为 +1，它完整描述了刚体在三维空间中的姿态。

向量运算中，点乘 a·b = |a||b|cosθ 用于计算投影和夹角，叉乘 a×b 生成垂直于两向量的新向量，其模长等于两向量构成的平行四边形面积。这些运算在插补算法中频繁出现，例如计算刀轴与工件表面的法向夹角、判断奇异点位置等。

2.1.2 判断方法：如何选择向量或矩阵运算

• 位置插补：使用向量加法与标量乘法，如线性插补中 P(t) = P₀ + t·(P₁ - P₀)。
• 姿态旋转：使用旋转矩阵乘法，如绕 X 轴旋转 α 角的矩阵 R_x(α)。
• 坐标变换：将局部坐标系中的点转换到全局坐标系，使用矩阵乘法 p_global = R·p_local + t。
• 速度与雅可比：末端速度向量与关节速度向量的映射关系通过雅可比矩阵 J 建立，即 v = J·q̇。

2.1.3 案例拆解：五轴刀轴矢量计算

假设五轴机床的摆头结构为 AC 双摆头，A 轴绕 X 轴旋转，C 轴绕 Z 轴旋转。初始刀轴方向为 [0, 0, 1]^T。当 C 轴旋转 γ 角、A 轴旋转 α 角后，实际刀轴方向计算如下：

// 先绕 Z 轴旋转 C 角，再绕 X 轴旋转 A 角
R = Rx(α) * Rz(γ)
o_new = R * [0, 0, 1]^T
// 展开后：
o_new = [sin(α)·sin(γ), -sin(α)·cos(γ), cos(α)]^T

该结果直接用于后处理中的刀位文件生成。若刀轴矢量未归一化，会导致后续旋转角度计算错误，因此每次变换后需重新归一化。

2.1.4 常见误区与总结

矩阵的存储与运算效率也是工程中必须考虑的问题。在嵌入式控制器中，3×3 矩阵乘法需要 27 次乘法和 18 次加法，而四元数乘法仅需 16 次乘法和 12 次加法，因此在高频插补周期（如 1ms）内，四元数具有明显性能优势。

2.2 齐次坐标变换：统一位置与姿态的数学工具

2.2.1 核心原理：4×4 变换矩阵

齐次坐标将三维空间中的点 (x, y, z) 扩展为四维 (x, y, z, 1)，从而将旋转和平移统一到一个 4×4 矩阵中：

T = | R   t |
    | 0   1 |

其中 R 为 3×3 旋转矩阵，t 为 3×1 平移向量。齐次变换的核心优势在于：复合变换可以通过矩阵乘法直接串联。例如，从基坐标系到工具坐标系的变换为 T_base→tool = T_{base→flange} · T_{flange→tool}。在多轴插补中，每个轴的运动都可以表示为一个齐次变换矩阵，整条运动链的位姿即为所有轴变换矩阵的连乘。

2.2.2 操作流程：构建运动学正解

1. 为每个运动轴建立局部坐标系（D-H 参数法或 Product-of-Exponentials 法）。
2. 根据当前关节角计算每个轴的齐次变换矩阵 T_i(θ_i)。
3. 将所有变换矩阵按运动链顺序相乘：T_end = T₁·T₂·…·T_n。
4. 从 T_end 中提取末端位置（第 4 列前三个元素）和姿态（左上 3×3 子矩阵）。

2.2.3 实战场景：六轴机器人直线插补

在笛卡尔空间直线插补中，控制器需要将目标位姿分解为多个中间位姿。假设起点位姿为 T_start，终点位姿为 T_end，插补参数 t 从 0 到 1 变化。位置部分采用线性插值：P(t) = (1-t)·P_start + t·P_end。姿态部分则需使用球面线性插值（Slerp）处理四元数，避免欧拉角插值带来的万向锁问题。每个中间位姿 T(t) 再通过逆运动学求解各关节角，驱动伺服电机运动。

2.2.4 常见误区与复盘

另一个常见错误是混淆“点的变换”与“坐标系的变换”。点的变换是主动变换（点移动），坐标系的变换是被动变换（坐标系移动），两者矩阵互为逆矩阵。在编写插补代码时，务必明确当前操作的是点还是坐标系。

2.3 欧拉角与四元数：姿态表示的工程选择

2.3.1 核心原理：两种姿态描述方式

欧拉角通过三个依次旋转的角度描述姿态，常见的有 ZYX 顺序（即 RPY 角：滚转 Roll、俯仰 Pitch、偏航 Yaw）。其优点是直观、易于理解，但存在万向锁问题：当中间旋转角为 ±90° 时，第一次和第三次旋转轴重合，丢失一个自由度，导致姿态突变。

四元数 q = w + xi + yj + zk 是一种超复数，满足 i² = j² = k² = ijk = -1。单位四元数（|q| = 1）可以唯一表示三维空间中的旋转，且不存在万向锁问题。旋转矩阵与四元数的转换关系为：

R = | 1-2(y²+z²)   2(xy-wz)     2(xz+wy) |
    | 2(xy+wz)     1-2(x²+z²)   2(yz-wx) |
    | 2(xz-wy)     2(yz+wx)     1-2(x²+y²) |

2.3.2 判断方法：何时选择欧拉角或四元数

• 人机交互界面：使用欧拉角，便于操作人员理解与输入。
• 插补计算：使用四元数，避免万向锁且计算效率高。
• 姿态存储：使用四元数，占用内存小（4 个浮点数 vs 9 个浮点数）。
• 奇异点检测：使用欧拉角时需额外判断中间角是否接近 ±90°。

2.3.3 案例拆解：五轴刀轴姿态插补

在五轴加工中，刀轴方向从 [0, 0, 1] 变化到 [0.707, 0, 0.707]（即从竖直方向倾斜 45°）。若使用欧拉角插补，假设采用 ZYX 顺序，初始 RPY 角为 (0°, 0°, 0°)，终点 RPY 角为 (0°, 45°, 0°)。线性插补中间角为 (0°, 22.5°, 0°)，得到的中间姿态矩阵与四元数 Slerp 插补结果一致。但当终点姿态经过万向锁区域时，欧拉角插补会产生剧烈跳变，而四元数插补始终保持平滑。

// 四元数 Slerp 插补
q(t) = (q_start * sin((1-t)θ) + q_end * sin(tθ)) / sin(θ)
其中 cos(θ) = q_start · q_end

实际加工中，若 CAM 软件输出的刀轴矢量变化剧烈，控制器内部应统一转换为四元数进行插补，再在插补周期结束时转换回旋转矩阵用于逆运动学求解。

2.3.4 常见误区与总结

此外，从旋转矩阵提取欧拉角时，需注意角度范围的定义。例如，ZYX 顺序中，俯仰角 β = atan2(-R₃₁, √(R₁₁² + R₂₁²))，当 β 接近 ±90° 时，atan2 的参数接近 0/0，数值稳定性变差。工程上建议在控制器内部统一使用四元数作为姿态存储与插补的标准格式，仅在外部接口处转换为欧拉角。

2.4 多轴运动中的综合应用与工程实践

2.4.1 运动学正解与逆解中的数学工具协同

以六轴关节机器人为例，正运动学通过 6 个齐次变换矩阵连乘得到末端位姿，计算量约为 6 次 4×4 矩阵乘法。逆运动学则复杂得多，通常需要根据末端位姿求解 6 个关节角。解析法利用代数消元或几何投影，将问题分解为多个子问题，计算速度快且精度高；数值法（如雅可比迭代）通用性强但实时性较差。在插补周期内，控制器通常采用解析法，并针对奇异位形进行特殊处理。

2.4.2 插补算法中的数学优化

在高速高精加工中，插补周期通常为 0.125ms~1ms。每个周期内需要完成：位置插补（向量运算）、姿态插补（四元数 Slerp）、逆运动学求解（矩阵运算与三角函数）、速度规划（加减速控制）。为了满足实时性要求，工程上常采用以下优化策略：

• 使用定点数或单精度浮点数替代双精度浮点数，减少计算开销。
• 预计算与关节角无关的常量矩阵，减少重复计算。
• 采用查表法计算三角函数，牺牲少量精度换取速度。
• 利用 SIMD 指令集并行处理多个轴的向量运算。

2.4.3 案例复盘：五轴 RTCP 功能中的数学实现

RTCP（Rotational Tool Center Point）是五轴机床的核心功能，其目的是在旋转轴运动时保持刀尖点位置不变。实现 RTCP 需要实时计算旋转轴运动引起的刀尖点偏移，并通过平移轴补偿。具体步骤如下：

1. 读取当前各轴位置，计算当前刀尖点在工件坐标系中的实际位置 P_actual。
2. 根据目标刀轴方向，计算旋转轴应达到的角度。
3. 计算旋转后刀尖点的新位置 P_new，并求出补偿量 ΔP = P_new - P_actual。
4. 将 ΔP 叠加到平移轴插补指令中，实现刀尖点位置不变。

该过程中，齐次变换矩阵用于描述旋转轴运动对刀尖点的影响，四元数用于平滑过渡刀轴方向，向量运算用于计算补偿量。若数学工具使用不当，例如未考虑旋转中心与刀尖点的偏移，会导致加工误差累积。