2. 数学基础

机械臂运动学研究的本质，是描述刚体在三维空间中的位置与姿态，以及这些描述方式之间的数学转换关系。无论是正运动学中由关节角求解末端位姿，还是逆运动学中由末端位姿反解关节角，都离不开一套严谨、高效的数学语言。本章将系统梳理向量、旋转矩阵、齐次变换矩阵、欧拉角与四元数这五类核心数学工具，并深入探讨它们在机械臂D-H建模、坐标系标定与轨迹插补中的具体应用逻辑。理解这些基础，是后续掌握轨迹规划与运动控制算法的前提。

上图展示了机械臂位姿描述的完整知识框架：向量负责位置，旋转矩阵、欧拉角与四元数负责姿态，而齐次变换矩阵则将两者统一为4×4矩阵形式，成为机械臂运动学建模的统一语言。下面我们逐一展开。

2.1 向量与旋转矩阵：位置与姿态的基本描述

核心原理

在机械臂运动学中，位置向量用于描述空间中的点，通常表示为 P = [px, py, pz]^T，其参考坐标系可以是基坐标系、工具坐标系或用户坐标系。旋转矩阵则用于描述刚体姿态，是一个3×3的正交矩阵 R，满足 R^T R = I 且 det(R) = +1。旋转矩阵的每一列代表被旋转坐标系各轴在参考坐标系中的方向余弦。例如，绕Z轴旋转θ角的旋转矩阵为：

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

旋转矩阵的优点是唯一且无奇异，但缺点是参数冗余（9个元素只有3个自由度），且直观性差。

判断方法与操作流程

在实际建模中，判断使用哪种描述方式可遵循以下流程：

1. 若仅需描述位置：直接使用3维向量，注意标明参考坐标系。
2. 若需描述姿态且用于矩阵运算：优先使用旋转矩阵，便于与平移向量组合成齐次矩阵。
3. 若需插值或避免万向锁：使用四元数。
4. 若需人机交互或参数标定：使用欧拉角（如RPY角），但需注意奇异问题。

案例拆解：工具坐标系姿态标定

假设某六轴机械臂末端法兰盘需要安装一个摄像头，要求摄像头光轴与法兰盘Z轴重合。通过CAD模型已知摄像头安装偏角为绕X轴旋转5°、绕Y轴旋转-3°。此时可直接构造旋转矩阵 R = Rx(5°) * Ry(-3°)，再与法兰盘位姿矩阵相乘，得到摄像头坐标系在基坐标系下的姿态。该过程避免了欧拉角顺序混淆的问题，因为矩阵乘法天然定义了旋转顺序。

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

核心原理

齐次变换矩阵 T 是一个4×4矩阵，其结构为：

T = [R   P;
     0   1]

其中 R 为3×3旋转矩阵，P 为3×1平移向量，最后一行固定为 [0 0 0 1]。齐次变换矩阵的核心优势在于：它将旋转和平移统一为一次矩阵乘法，使得复合变换可以表示为矩阵连乘。例如，从基坐标系到工具坐标系的变换可写为 T_base_to_tool = T_base_to_shoulder * T_shoulder_to_elbow * ... * T_wrist_to_tool。

操作流程：构建D-H变换矩阵

标准D-H法规定，相邻坐标系的变换矩阵为：

A_i = Rot(Z, θi) * Trans(Z, di) * Trans(X, ai) * Rot(X, αi)

该矩阵将第i个关节坐标系变换到第i-1个关节坐标系。正运动学即依次连乘所有A矩阵，得到末端位姿。

案例拆解：六轴机器人正运动学建模

以典型的六轴串联关节机器人为例，其D-H参数表如下（单位：mm/度）：

给定一组关节角 [0°, 30°, -45°, 0°, 60°, 0°]，通过连乘6个A矩阵，即可得到末端工具坐标系在基坐标系下的位姿矩阵 T_06。该矩阵的左上3×3子块为姿态，右上3×1子块为位置。实际编程中，这一过程通常封装为函数 fkine(theta)，输入关节角向量，输出4×4齐次矩阵。

2.3 欧拉角与四元数：姿态的两种实用表达

核心原理

欧拉角通过三次绕坐标轴的旋转来描述姿态，常见的有ZYX（RPY角）和ZYZ两种。以RPY角为例，其旋转顺序为：先绕固定参考系的X轴旋转（Roll），再绕Y轴旋转（Pitch），最后绕Z轴旋转（Yaw）。对应的旋转矩阵为 R = Rz(Yaw) * Ry(Pitch) * Rx(Roll)。欧拉角的优点是直观、参数少（3个），但存在万向锁问题：当Pitch角为±90°时，Roll和Yaw的旋转轴重合，导致自由度丢失。

四元数是一种超复数，表示为 q = q0 + q1*i + q2*j + q3*k，其中 q0 为实部，(q1,q2,q3) 为虚部，且满足 q0² + q1² + q2² + q3² = 1。四元数可以表示绕任意轴 [nx,ny,nz] 旋转角度θ的旋转：

q = [cos(θ/2), nx*sin(θ/2), ny*sin(θ/2), nz*sin(θ/2)]

四元数无奇异、无冗余，且插值平滑（如Slerp），非常适合轨迹规划中的姿态插补。

对比分析：欧拉角与四元数

案例拆解：姿态插补中的四元数应用

在机械臂笛卡尔空间轨迹规划中，若起点姿态为 q_start，终点姿态为 q_end，需要在时间t内平滑过渡。使用四元数的球面线性插值（Slerp）公式：

q(t) = (sin((1-t)Ω) / sinΩ) * q_start + (sin(tΩ) / sinΩ) * q_end

其中 Ω = arccos(q_start · q_end)。该插值保证姿态变化均匀且角速度恒定。若使用欧拉角直接线性插值，在接近万向锁区域时会出现姿态抖动。实际工程中，控制器通常将目标姿态以四元数形式下发，内部再转换为旋转矩阵用于逆运动学求解。

2.4 数学工具在机械臂建模中的综合应用

核心原理

机械臂建模的核心任务是将物理关节空间映射到笛卡尔空间。这一映射关系完全由上述数学工具构建：

• 正运动学：利用D-H参数和齐次变换矩阵连乘，实现关节角→末端位姿的映射。
• 逆运动学：给定末端位姿（通常为齐次矩阵），通过解析法或数值法求解关节角。解析法常利用旋转矩阵的元素关系进行反三角运算。
• 速度级运动学：引入雅可比矩阵 J，将关节速度映射到末端笛卡尔速度，其推导过程同样依赖旋转矩阵的微分。
• 轨迹规划：在笛卡尔空间规划时，需要将末端位姿分解为位置向量和姿态四元数，分别进行插值后再合成齐次矩阵。

操作流程：从位姿描述到运动学求解

1. 根据机械臂结构确定D-H参数表。
2. 构建各关节的齐次变换矩阵 A_i。
3. 正运动学：连乘得到 T_06，提取位置向量和旋转矩阵。
4. 若需逆解：将 T_06 分解，利用旋转矩阵元素求解关节角（如Pieper法则）。
5. 轨迹规划：将目标位姿转换为四元数，进行Slerp插值，再转回旋转矩阵。

案例拆解：视觉引导抓取中的坐标变换

某视觉引导场景中，相机固定于机械臂上方，识别到工件在相机坐标系下的位姿为 T_cam_obj。已知相机到机械臂基座的标定矩阵为 T_base_cam，则工件在基坐标系下的位姿为：

T_base_obj = T_base_cam * T_cam_obj

随后，控制器需要规划机械臂末端从当前位姿 T_base_tool 运动到抓取位姿 T_base_grasp。该过程涉及：将目标位姿分解为位置和姿态，位置用直线插补，姿态用四元数Slerp插补，再合成齐次矩阵，最后通过逆运动学求解各关节角。整个链路中，齐次矩阵负责坐标系转换，四元数负责姿态平滑，旋转矩阵负责逆解计算，三者缺一不可。

2.5 总结与复盘

本章的核心逻辑可归纳为：向量描述位置，旋转矩阵/欧拉角/四元数描述姿态，齐次变换矩阵统一两者。在机械臂建模中，D-H参数法依赖齐次矩阵连乘实现正运动学；逆运动学依赖旋转矩阵的元素关系进行解析求解；轨迹规划依赖四元数实现平滑姿态插补。实际工程中，建议建立统一的数学库，封装各类转换函数（如欧拉角↔旋转矩阵、四元数↔旋转矩阵），并严格测试边界条件（如万向锁、四元数归一化）。