4. 机器人运动学与动力学模型构建
数字孪生体要“像”真实机器人一样运动，核心不在三维外观，而在于它内部是否运行着一套与物理样机一致的运动学与动力学模型。本节围绕四个层次展开：用 DH 参数描述连杆几何关系，用 URDF/SDF 把几何与惯性落到可解析的模型文件，用正逆运动学建立关节空间与笛卡尔空间的映射，再用动力学方程描述力矩、惯量与运动之间的因果。四者共同构成孪生体的“可计算骨架”。
一、DH 参数：把连杆链写成四元组
标准 DH（Denavit-Hartenberg）约定用四个参数描述相邻两个连杆坐标系之间的变换：连杆长度 a_i、连杆扭角 alpha_i、关节偏置 d_i、关节角 theta_i。其中转动关节的 theta_i 是变量，移动关节的 d_i 是变量。相邻变换为：
T_i = Rz(theta_i) * Tz(d_i) * Tx(a_i) * Rx(alpha_i)以一台六轴串联机械臂为例，假设其 DH 参数如下（示例参数，非实测值）：
| 关节 i | a_i (m) | alpha_i (°) | d_i (m) | theta_i |
|---|---|---|---|---|
| 1 | 0 | 90 | 0.30 | θ1 |
| 2 | 0.45 | 0 | 0 | θ2 |
| 3 | 0.05 | 90 | 0 | θ3 |
| 4 | 0 | -90 | 0.42 | θ4 |
| 5 | 0 | 90 | 0 | θ5 |
| 6 | 0 | 0 | 0.10 | θ6 |
把六个 T_i 依次相乘，得到末端相对基座的总变换 T_0_6 = T_1·T_2·…·T_6。这个 4×4 矩阵的左上 3×3 是姿态旋转矩阵，右上 3×1 是末端位置。孪生体每一帧只要拿到六个关节角，就能用这条链算出末端位姿，这是最基础的正运动学实现。
二、URDF/SDF：让模型可被解析与仿真
DH 参数适合手算与推导，但孪生平台通常需要标准模型描述。URDF 用 <link> 与 <joint> 树形组织机器人：link 承载视觉、碰撞与惯性，joint 定义父子关系、类型（revolute/prismatic/fixed）、轴向与限位。SDF 在此基础上更适合多机器人、多物理引擎场景，支持更丰富的传感器与嵌套模型。
URDF 中每个 link 的惯性块必须给出质量、质心与惯性张量，否则动力学仿真会退化为“无质量骨架”。一个关节的典型写法如下：
<joint name="joint2" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="0 0 0.30" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-2.0" upper="2.0" effort="150" velocity="3.0"/>
</joint>孪生体加载 URDF 后，会构建一棵运动学树。此时要注意：URDF 的关节原点与 DH 坐标系原点通常不重合，二者之间需要一次固定变换对齐。若直接把 DH 参数填进 URDF 的 origin，末端位置会出现系统性偏差，这是建模阶段最常见的错误来源之一。
三、正逆运动学在孪生体中的实现
正运动学实现直接、无歧义：给定 q，逐级乘变换矩阵即可。孪生体每帧调用一次，输出末端位姿用于渲染与碰撞检测。逆运动学则复杂得多：解析法对满足 Pieper 条件的六轴臂可求闭式解，通常存在 8 组解；数值法（如阻尼最小二乘、雅可比伪逆迭代）通用但需处理初值、收敛与奇异。
工程上常用雅可比矩阵 J(q) 建立关节速度与末端速度的关系：v = J(q)·q̇。当 J 接近奇异（如腕部关节共线），行列式趋近于零，逆解会剧烈跳变。孪生体应实时计算 det(J) 或最小奇异值，低于阈值时切换阻尼伪逆：
q̇ = J^T (J J^T + λ² I)^(-1) · v其中 λ 为阻尼因子，示例可取 λ = 0.05。这样在奇异附近牺牲少量精度换取关节速度的连续性，避免孪生体出现“抖动”或“飞车”。
四、动力学方程：从几何到力矩
运动学只回答“在哪”，动力学回答“需要多大力”。刚体动力学标准形式为：
M(q)·q̈ + C(q, q̇)·q̇ + G(q) + F(q̇) = τ其中 M(q) 为惯性矩阵，C 为科氏力与离心力项，G 为重力项，F 为摩擦项，τ 为关节驱动力矩。孪生体实现时通常采用递归牛顿-欧拉（RNEA）算法，从基座向末端递推速度与加速度，再反向递推力与力矩，复杂度为 O(n)，适合实时计算。
以二连杆平面臂为例（示例参数：m1=2 kg，m2=1.5 kg，l1=0.4 m，l2=0.3 m，质心在连杆中点），当 q1=30°、q2=45°、q̇=0、q̈=0 时，重力项 G(q) 可算出关节 1 需约 12.6 N·m、关节 2 需约 4.4 N·m 的保持力矩（示例计算值）。孪生体若忽略 G，在静止姿态下就会与真机产生明显偏差。
五、模型一致性校验
孪生体上线前应做三项校验。第一，正运动学校验：给定一组关节角，比较孪生体末端位姿与真机示教器读数，位置误差应小于 1 mm、姿态误差小于 0.5°。第二，逆运动学校验：从末端位姿反解 q，再正解回位姿，闭环误差应接近零。第三，动力学校验：在静止姿态下比较孪生体计算的保持力矩与真机电流换算力矩，偏差应控制在 10% 以内。三项通过后，孪生体才具备与真机同步运动的基础可信度。