3. 多体系统运动学建模
多体系统运动学建模的核心任务，是把一台由多个刚体通过铰链、滑轨、球铰等连接组成的装备，抽象成一张可计算的“拓扑图”，再在这张图上写出约束方程、算出自由度，最后把驱动加在正确的位置上，让模型按预期运动。本节围绕这四件事展开：拓扑描述、约束方程、自由度分析、运动学驱动设置。
一、拓扑描述：把装备画成“体—铰—体”的图
多刚体系统的拓扑描述，本质上是回答两个问题：系统里有哪些刚体（体），它们之间通过什么运动副连接（铰），以及连接关系是开链还是闭链。工程上常用“体—铰—体”的有向图表示：节点代表刚体，边代表运动副。例如某车载起竖装置可抽象为：车体（大地参考）—回转副—转台—俯仰副—起竖臂—伸缩副—负载。这是一条典型的开链拓扑，从根节点（车体）出发，每个体只有一个父体，运动学求解可以沿链逐级递推。
若装备中存在并联或闭环结构，例如六自由度平台、并联稳定平台，则拓扑图中会出现回路。此时不能简单逐级递推，需要把闭环拆成“生成树 + 余枝”，对余枝写出闭环约束方程。拓扑描述的关键输出通常包括：体编号、体质量与惯量、铰的类型（转动/移动/球铰/万向铰）、铰的轴线方向、铰的安装位置（父体坐标系下的位置与姿态）。这些信息决定了后续约束方程的形式。
二、约束方程：把“连接”写成数学关系
约束方程描述的是相邻刚体之间被运动副限制住的相对运动。以平面转动副为例，若父体上铰点位置为 r_p，子体上铰点位置为 r_c，则约束方程可写为两个标量方程：r_p - r_c = 0（x、y 方向各一个），即铰点重合。对于移动副，约束的是相对转动为零、相对移动沿指定轴线，通常写成“两点距离沿轴向可变、垂直方向分量为零”的形式。
在三维空间中，常见运动副的约束数如下：球铰约束 3 个平移自由度；万向铰约束 3 个平移自由度；转动副约束 3 个平移 + 2 个转动，共 5 个约束；移动副约束 3 个平移中的 2 个 + 3 个转动中的 2 个，共 5 个约束。把这些约束按体编号和铰编号组装成全局约束向量 Φ(q,t)=0，其中 q 是系统广义坐标向量。若存在驱动，则驱动方程作为附加约束加入，形式为 Φ_d(q,t)=0，例如指定某转动副转角 θ = θ(t)。
三、自由度分析：从约束数算出独立运动数
自由度分析的基本公式是：DOF = 6n - m（空间系统），其中 n 为活动刚体数（不含大地），m 为独立约束方程数。对于平面系统，公式变为 DOF = 3n - m。以起竖装置为例：车体固定，转台、起竖臂、负载共 3 个活动刚体，空间自由度基数为 18；回转副 5 个约束、俯仰副 5 个约束、伸缩副 5 个约束，共 15 个约束，则 DOF = 18 - 15 = 3。这 3 个自由度分别对应回转角、俯仰角、伸缩位移，正好与三个驱动一一对应。
若系统存在闭环，则不能简单用 6n - m 直接算，因为闭环约束之间存在相关性。此时应先用“生成树”方法：把闭环拆开，得到一棵开链树，树上的铰为独立铰，余枝铰对应的约束为闭环约束。独立坐标数等于树上铰的自由度数之和，再减去闭环约束中独立方程的个数。工程上更稳妥的做法是：在虚拟样机软件中先不加驱动，运行一次自由度检查，软件会报告“冗余约束”或“过约束”数量，据此判断模型是否合理。
| 运动副类型 | 空间约束数 | 保留自由度 | 典型装备示例 |
|---|---|---|---|
| 转动副 R | 5 | 1（绕轴转动） | 起竖臂俯仰铰 |
| 移动副 T | 5 | 1（沿轴移动） | 伸缩臂、液压缸 |
| 球铰 S | 3 | 3（三向转动） | 并联平台支链两端 |
| 万向铰 U | 4 | 2（两向转动） | 传动轴、稳定平台 |
| 圆柱副 C | 4 | 2（转动+移动） | 套筒式导向机构 |
四、运动学驱动设置：把“想让它怎么动”变成输入
驱动设置的本质，是在已确定的自由度上施加已知的时间函数，使系统从“欠驱动”变为“确定运动”。常见驱动类型有三种：位移驱动（直接给定铰的位移或转角随时间变化）、速度驱动（给定一阶导数）、加速度驱动（给定二阶导数）。在虚拟样机中，通常优先使用位移驱动，因为它最稳定、最容易与物理行程对应。
以起竖装置为例，假设回转角 θ1(t) 从 0° 匀速转到 90°，俯仰角 θ2(t) 按 0°→60° 的平滑 S 曲线变化，伸缩位移 s(t) 从 0 伸长到 800 mm。驱动函数可写成：
θ1(t) = 90° * t / 10 (0 ≤ t ≤ 10 s)
θ2(t) = 60° * (3*(t/8)^2 - 2*(t/8)^3) (0 ≤ t ≤ 8 s)
s(t) = 800 * (t/6) (0 ≤ t ≤ 6 s)
设置时要注意三点：第一，驱动必须加在独立自由度对应的铰上，不能加在闭环余枝铰上，否则会造成过约束；第二，驱动函数的时间区间要覆盖仿真时长，避免中途“断驱动”；第三，若同时存在多个驱动，应检查它们是否在几何上相容，例如伸缩位移与俯仰角不能同时任意给定，否则铰点会分离。判断方法是：运行仿真后查看约束残差，若残差超过 1e-6 量级，说明驱动与约束不匹配。
五、建模检查清单
完成上述四步后，建议按以下顺序做一次快速检查：拓扑图是否每个体都有唯一父体（开链）或明确的闭环回路；约束方程数量是否与运动副类型匹配；自由度计算结果是否等于驱动数量；驱动函数是否覆盖全部独立坐标；仿真初始时刻约束残差是否接近零。若这五项都通过，运动学模型基本可用，可以进入下一步的动力学建模或轨迹规划。