4. 设备机械运动学建模与验证

虚拟调试要回答的第一个机械问题是：设备在运动过程中，各部件到底会走到哪里、能不能走到、会不会撞上。运动学建模就是把这些“走到哪里”的问题变成可计算、可验证的数字模型。本节以一台典型的直角坐标搬运机构为例，从刚体运动学、关节约束、运动副定义出发，完成干涉检查与工作空间验证。

一、刚体运动学：位姿与坐标系的约定

刚体运动学描述的是构件在空间中的位置与姿态，而不涉及力与质量。一个刚体在三维空间中有 6 个自由度：3 个平移（X、Y、Z）和 3 个旋转（绕 X、Y、Z 轴）。描述位姿最常用的方式是“齐次变换矩阵”，它把旋转和平移统一到一个 4×4 矩阵中：

T = [ R  p ]
    [ 0  1 ]

R：3×3 旋转矩阵，描述姿态
p：3×1 平移向量，描述位置

建模前必须先约定坐标系，否则后续所有正负号都会混乱。本例约定：基坐标系 B 固定在设备底座，Z 轴竖直向上，X 轴指向搬运起点方向；工具坐标系 T 固定在夹爪中心，其原点即抓取点 TCP（Tool Center Point）。所有关节角、位移的正方向都相对基坐标系定义，全文与图示保持一致。

串联机构的位姿由各关节变换依次相乘得到。若从基座到夹爪依次经过关节 1、2、3，则：

T_BT = T_B1 · T_12 · T_23 · T_3T

这就是正运动学的基本形式。给定各关节变量，就能算出 TCP 在基坐标系中的位姿；反过来，给定目标位姿求关节变量，就是逆运动学，通常用于轨迹规划。

二、关节约束与运动副定义

运动副定义的是两个构件之间的相对运动关系，它决定了“允许怎么动、限制怎么动”。常见运动副与自由度对应关系如下：

本例搬运机构采用三个移动副：X 轴滑台、Y 轴滑台、Z 轴升降。每个移动副只保留 1 个平移自由度，因此整机共 3 个自由度，属于典型的直角坐标机器人结构。

关节约束不仅是“类型”，还包括行程范围、速度上限和加速度上限。以本例为例，设定如下示例参数（属于教学假设值，非实测）：

X 轴：行程 0 ~ 800 mm，最大速度 1000 mm/s
Y 轴：行程 0 ~ 500 mm，最大速度 800 mm/s
Z 轴：行程 0 ~ 300 mm/s，最大速度 500 mm/s
夹爪：开合 0 ~ 60 mm，开合时间 0.2 s

这些约束会直接进入运动学模型，成为后续干涉检查和工作空间验证的边界条件。若忽略行程限制，仿真中机构可能“穿过”导轨末端，导致虚假的通过结果。

三、机械干涉检查

干涉检查的目标是判断运动过程中任意两个构件是否发生体积重叠。工程上通常分两步：先用包围盒（AABB 或 OBB）做快速粗筛，再对可能相交的构件做精确网格检测。粗筛阶段计算量小，能快速排除大量不可能碰撞的组合；精筛阶段才调用精确几何求交。

在本例中，需要重点检查的干涉对包括：Z 轴滑台与 Y 轴滑台、夹爪与工件、夹爪与料台边缘、线缆拖链与机架。检查时不能只看起点和终点，而要在整条轨迹上按固定步长采样。例如轨迹总长 1200 mm，采样步长 10 mm，则需检查 120 个位姿点。

判断结果通常有三种：无干涉、瞬时接触、持续干涉。瞬时接触若发生在抓取瞬间且接触力可控，可视为正常；持续干涉则必须修改轨迹或结构。本例中，若夹爪在 Z 轴下降到底部时与料台边缘持续重叠超过 5 mm，则判定为不合格，需要抬高抓取点或调整料台位置。

四、工作空间与可达性验证

工作空间是所有可达 TCP 位置的集合。对直角坐标机构，工作空间是一个长方体，边界由各轴行程直接决定：X ∈ [0,800]，Y ∈ [0,500]，Z ∈ [0,300]（单位 mm）。但“在工作空间内”不等于“可达”，还要满足姿态约束和轨迹连续性。

可达性验证通常分三层：第一层检查目标点是否落在行程范围内；第二层检查从当前点到目标点是否存在连续无干涉路径；第三层检查路径上各轴速度、加速度是否超限。三层都通过，才判定该点可达。

本例的搬运任务是从起点 P1(100, 100, 50) 抓取工件，搬运到终点 P2(700, 400, 250) 放置。验证时先确认 P1、P2 均在行程内，再规划一条“先抬升、再平移、后下降”的路径，避免斜向直插造成干涉。路径关键点如下：

P1   (100, 100,  50)  抓取点
P1'  (100, 100, 150)  抬升离开料台
P2'  (700, 400, 250)  平移到位
P2   (700, 400, 150)  下降放置

对每个关键点及中间插补点做干涉检查，并核算各轴速度是否超过上限。若某段路径上 X 轴需求速度达到 1100 mm/s，超过 1000 mm/s 上限，则需要降低节拍或调整路径分配。

五、典型搬运机构的建模与验证示例

把上述内容串起来，本例的完整验证过程如下。首先在仿真环境中建立三个移动副，分别绑定 X、Y、Z 轴，输入行程与速度上限。然后建立夹爪工具坐标系，标定 TCP 相对 Z 轴末端的偏移量，例如 TCP 偏移为 (0, 0, -80) mm。接着导入工件与料台的简化几何模型，设置干涉检查对。

轨迹规划采用“门型路径”：从 P1 抬升到安全高度 150 mm，再平移到 P2 上方，最后下降到放置高度。按 10 mm 步长采样，共约 120 个位姿点。对每个位姿点执行 AABB 粗筛，若发现包围盒重叠，再对该构件对做网格精检。同时记录各轴速度，检查是否超过 1000/800/500 mm/s 的上限。

示例验证结果（教学假设）：路径上未发现持续干涉；最大 X 轴速度 920 mm/s，最大 Y 轴速度 760 mm/s，最大 Z 轴速度 480 mm/s，均在限值内；P1、P2 及所有中间点均位于工作空间内。因此判定该搬运任务在当前参数下可达且无干涉。若把节拍缩短 20%，X 轴需求速度将升至约 1150 mm/s，超过上限，此时需要降低节拍或优化路径分配。

最后要强调的是，运动学模型的价值在于“可复现、可量化”。坐标系约定、运动副类型、行程与速度限值、采样步长、干涉判定阈值，这些参数都应记录在模型说明中。只有这样，当现场设备参数变化或节拍调整时，才能快速重新验证，而不是凭经验猜测。