数学基础：向量、矩阵、坐标系变换、四元数、欧拉角等运动控制中必需的数学工具

运动控制算法的本质，是对“位置、姿态、速度、加速度”等物理量进行精确的数学描述与运算。无论是机器人关节的伺服控制、无人机姿态解算，还是数控机床的轨迹插补，底层都离不开向量、矩阵、坐标系变换、欧拉角与四元数这五大数学工具。本章将从工程应用视角出发，系统梳理这些工具的核心定义、运算规则、相互转换关系及典型应用场景，帮助读者建立从“数学公式”到“控制代码”的完整映射。

核心原理：从物理量到数学表达

运动控制中，所有物理量都需要用数学语言精确描述。向量用于表达具有大小和方向的物理量，如位置向量 p = [x, y, z]^T、速度向量 v、角速度向量 ω。矩阵则用于描述线性变换，包括旋转、缩放、平移等操作。在三维空间中，一个刚体的姿态可以用 3×3 旋转矩阵 R 表示，其列向量构成正交基，满足 R^TR = I 且 det(R) = 1。

坐标系变换是连接不同参考基准的桥梁。例如，机器人末端执行器的位姿需要从关节坐标系转换到世界坐标系，这通常通过齐次变换矩阵 T = [R p; 0 1] 实现。欧拉角以三个依次旋转的角度（如 ZYX 顺序的偏航-俯仰-滚转）描述姿态，直观但存在万向锁问题。四元数则通过四个参数（一个实部 + 三个虚部）避免奇异性，且计算效率更高，成为现代运动控制中姿态表达的首选。

判断方法与操作流程：如何选择合适的数学工具

在实际工程中，选择哪种数学工具取决于具体场景的需求。下表总结了各工具的特点与适用场景：

工具	维度	优点	缺点	典型应用
向量	3	直观、运算简单	无法表达旋转	位置、速度、力描述
旋转矩阵	9	无奇异性、组合方便	冗余参数多、需正交化	坐标变换、机器人运动学
欧拉角	3	直观、易理解	万向锁、插值不平滑	飞行器姿态显示、交互界面
四元数	4	无奇异性、插值平滑、紧凑	不直观、需归一化	姿态解算、SLAM、动画

选择流程建议：若仅需描述位置，直接用向量；若需连续旋转且要求实时性，优先四元数；若用于人机交互或可视化，欧拉角更友好；若进行多坐标系级联变换，旋转矩阵或齐次矩阵最方便。

案例拆解：六轴机械臂末端姿态解算

以六轴工业机械臂为例，控制器需要实时计算末端执行器在世界坐标系中的位姿。初始状态：各关节角度 θ = [0°, 30°, -60°, 90°, 45°, 0°]，连杆参数已知。目标：求末端位置 p 和姿态四元数 q。

分析过程：首先，利用 D-H 参数法建立各连杆坐标系，通过相邻坐标系的齐次变换矩阵 _iT_i+1 连乘得到末端位姿矩阵 ₀T₆。该矩阵的左上 3×3 子块为旋转矩阵 R，右上 3×1 子块为位置向量 p。然后，将旋转矩阵 R 转换为四元数 q，转换公式为：

// 旋转矩阵转四元数（基于轨迹法）
double trace = R[0][0] + R[1][1] + R[2][2];
if (trace > 0) {
    double s = 0.5 / sqrt(trace + 1.0);
    q.w = 0.25 / s;
    q.x = (R[2][1] - R[1][2]) * s;
    q.y = (R[0][2] - R[2][0]) * s;
    q.z = (R[1][0] - R[0][1]) * s;
} else {
    // 处理 trace <= 0 的情况（选最大对角元分支）
}

操作步骤：① 建立 D-H 参数表；② 计算各关节变换矩阵；③ 连乘得到末端位姿；④ 提取旋转矩阵并转换为四元数；⑤ 将四元数归一化后用于后续插补控制。

结果与复盘：计算得到末端位置约为 (0.612, -0.354, 0.832) m，姿态四元数约为 (0.924, 0.127, -0.289, 0.213)。该流程在 1 kHz 控制周期内完成，耗时约 12 μs，满足实时性要求。经验：四元数转换时需注意符号一致性，且归一化操作不可省略，否则累积误差会导致姿态漂移。

常见误区与总结复盘