3. 双臂机器人系统建模
双臂机器人不是两台单臂机器人的简单叠加。当两只机械臂共同抓持同一工件、协同装配或进行双手协调操作时，系统会形成闭链约束，两臂的运动学、动力学与力传递关系被强耦合在一起。因此，双臂系统建模的核心任务是：先建立单臂模型，再通过耦合关系与闭链约束把两臂统一到同一坐标系与同一约束方程中，最后用相对雅可比与协同运动学描述“末端相对位姿”和“绝对位姿”的联合控制。
一、单臂模型：从关节空间到末端位姿
单臂建模是所有协同建模的基础。对第 i 条臂（i=L 或 R），设关节角向量为 q_i ∈ R^n，正运动学给出末端位姿：
p_i = f_i(q_i) # 末端位置
R_i = R_i(q_i) # 末端姿态
T_i = [R_i, p_i; 0, 1] # 齐次变换矩阵对时间求导得到单臂速度级模型：
ẋ_i = J_i(q_i) · q̇_i
J_i = ∂f_i / ∂q_i # 单臂雅可比矩阵其中 J_i 把关节速度映射为末端线速度与角速度。单臂模型还包含关节限位、速度限位、力矩限位以及奇异位形分析。双臂建模时，每条臂的 J_i 仍然成立，但必须把两臂统一到同一基坐标系与同一任务坐标系下，否则后续耦合关系无法正确叠加。
二、双臂耦合关系：从“各自独立”到“共享任务”
双臂耦合关系体现在三个层面：
- 运动学耦合：两臂末端通过工件或接触约束产生相对位姿关系，任一臂运动都会影响另一臂的可行域。
- 动力学耦合：工件作为公共负载，把两臂的关节力矩通过内力与外力耦合在一起。
- 任务耦合：协同搬运、装配、拧紧等任务要求两臂同时满足绝对轨迹与相对轨迹。
常用描述方式是定义绝对运动与相对运动：
x_abs = (x_L + x_R) / 2 # 双臂中点/绝对位姿
x_rel = x_L - x_R # 两臂相对位姿绝对运动决定工件整体位姿，相对运动决定工件内部受力与装配关系。二者共同构成双臂协同任务空间。
三、闭链约束：协同建模的关键
当两臂共同抓持刚性工件时，系统形成闭链。设工件上两个抓取点之间的相对位姿为常量 T_LR，则闭链约束可写为：
Φ(q_L, q_R) = T_L(q_L)⁻¹ · T_R(q_R) - T_LR = 0对时间求导得到速度级约束：
J_L(q_L) · q̇_L - J_R(q_R) · q̇_R = 0该式说明：两臂末端速度必须满足同一刚体运动关系，不能任意独立运动。闭链约束把两臂的关节速度空间压缩到一个约束流形上，控制律必须在该流形内求解。若忽略闭链约束，两臂会互相“拉扯”，导致工件变形、关节过载或抓取滑移。
四、相对雅可比与协同运动学建模
为了同时控制绝对位姿与相对位姿，需要构造相对雅可比。由 x_abs 与 x_rel 的定义可得：
x_abs = (x_L + x_R)/2
x_rel = x_L - x_R
ẋ_abs = (J_L·q̇_L + J_R·q̇_R)/2
ẋ_rel = J_L·q̇_L - J_R·q̇_R写成矩阵形式：
[ẋ_abs] [ J_L/2 J_R/2 ] [q̇_L]
[ẋ_rel] = [ J_L -J_R ] [q̇_R]其中组合矩阵即为协同雅可比。它把两臂关节速度映射为绝对与相对任务速度。若该矩阵满秩，则可对 x_abs 与 x_rel 独立规划；若秩不足，则说明系统处于奇异或约束冲突状态，需要调整抓取构型或任务优先级。
协同运动学建模的典型流程如下：
- 建立两臂各自的正运动学与单臂雅可比
J_L、J_R。 - 统一基坐标系与任务坐标系，确定抓取点与工件相对位姿
T_LR。 - 写出闭链约束
Φ(q_L,q_R)=0及其速度级形式。 - 构造绝对/相对任务变量与协同雅可比矩阵。
- 在约束流形上求解关节速度或关节力矩，并进行奇异与限位检查。
五、案例：双臂协同搬运刚性工件
设两臂共同搬运一块刚性板，要求板中心沿直线运动，同时保持板姿态不变。此时：
x_abs按直线轨迹规划；x_rel保持常量，即两臂相对位姿不变；- 闭链约束保证板不被拉伸或压缩；
- 协同雅可比用于把任务速度分配到两臂关节。
若只给两臂分别发送独立位置指令，即使单臂轨迹都正确，也可能因基座标定误差、关节柔性和同步延迟导致内力过大。正确做法是把相对位姿误差反馈到协同控制器中，通过内力/外力分解抑制不必要的内力。
六、常见误区与复盘
| 误区 | 后果 | 修正方法 |
|---|---|---|
| 把双臂当作两台独立单臂控制 | 闭链内力过大、工件变形 | 引入闭链约束与协同雅可比 |
| 忽略基坐标系标定误差 | 相对位姿偏差、抓取滑移 | 标定基座与工具坐标系，补偿 T_LR |
| 只控制绝对轨迹，不控制相对轨迹 | 工件姿态漂移、装配失败 | 同时规划 x_abs 与 x_rel |
| 在奇异位形附近直接求逆 | 关节速度突变、力矩饱和 | 使用阻尼最小二乘或任务优先级 |
综上，双臂系统建模的关键在于把单臂模型、耦合关系、闭链约束与相对雅可比统一到同一数学框架中。只有先建立准确的单臂模型，再通过闭链约束描述两臂的刚性联系，最后用协同雅可比把绝对与相对任务解耦，才能为后续的协同控制、力分配与轨迹规划提供可靠基础。