3. 双臂协调运动学建模

双臂协调操作是人形机器人区别于单臂工业机器人的核心能力之一。当两条上肢同时抓持同一物体或共同完成一项任务时，双臂与物体之间便构成了一个闭链机构（Closed Kinematic Chain）。与单臂开链运动学不同，闭链系统引入了额外的几何约束，使得双臂的关节运动不再独立，而是必须满足一组严格的位姿耦合关系。本章将围绕双臂闭链约束的数学表达、双臂间相对位姿的描述方法以及协同工作空间的求解与评估展开，为后续的协调运动规划奠定理论基础。

上图展示了双臂协调运动学建模的整体框架。左右臂的基座分别固定于机器人躯干上，末端执行器通过抓持同一刚性负载形成闭链。图中红色虚线表示闭链约束的耦合关系，紫色椭圆区域代表双臂协同工作空间，底部标注了相对位姿描述的基本公式。下面将逐一展开各模块的数学原理与工程实现方法。

3.1 双臂闭链约束的数学表达

3.1.1 闭链约束的核心原理

当双臂末端同时抓持一个刚性物体时，左右末端执行器的位姿不再独立。设左臂末端坐标系为 {E_L}，右臂末端坐标系为 {E_R}，被抓持物体上的固连坐标系为 {O}。若物体为刚性，则左末端与物体之间的变换矩阵 T_{L_O} 和右末端与物体之间的变换矩阵 T_{R_O} 在抓持过程中保持不变。由此可推导出双臂末端之间的位姿约束关系：

T_E_R = T_E_L · T_L_O · (T_R_O)⁻¹

该方程表明，一旦抓持姿态确定，右臂末端的位姿完全由左臂末端位姿决定。这意味着双臂的关节空间运动必须满足一个隐式约束方程：

FK_L(q_L) · T_L_O = FK_R(q_R) · T_R_O

其中 FK_L 和 FK_R 分别为左右臂的正运动学函数，q_L 和 q_R 为左右臂的关节角度向量。这个方程将双臂的 2n 个关节变量（n 为单臂自由度）约束为 n+6 个独立变量（6 为物体在空间中的自由度），因此双臂协调系统实际自由度为 n+6。

3.1.2 约束方程的求解策略

在实际工程中，闭链约束方程的求解通常采用以下三种策略：

• 主从法（Leader-Follower）：将左臂设为主臂，右臂设为从臂。主臂按规划轨迹运动，从臂通过约束方程实时解算目标位姿，再通过逆运动学求解关节角。该方法计算量小，实时性好，但存在误差累积问题。
• 对称法（Symmetric Approach）：双臂同时参与规划，将闭链约束作为优化目标的一部分，通过数值优化方法（如拉格朗日乘子法）求解。该方法精度高，但计算复杂度较高。
• 虚拟连杆法（Virtual Link）：将物体视为一根虚拟连杆，将双臂闭链系统等效为一个并联机构。该方法便于进行工作空间分析和奇异位形判断，但建模过程较为复杂。

3.1.3 闭链约束的误差来源与补偿

实际系统中，闭链约束方程往往无法精确满足，主要误差来源包括：运动学标定误差（连杆长度、关节零位偏差）、机械柔性（连杆弹性变形、减速器回差）、以及抓持点位置的不确定性。为减小误差影响，可采取以下措施：

1. 在抓持前进行双臂末端相对位姿的在线标定，利用视觉或力觉传感器获取实际抓持位姿。
2. 在从臂控制回路中加入力/力矩反馈，当检测到内力超过阈值时，微调从臂目标位姿以释放内力。
3. 采用柔顺控制（如导纳控制）作为从臂的内环控制策略，允许从臂末端在约束方向上有微小偏差。

3.2 双臂相对位姿描述方法

3.2.1 相对位姿的数学定义

双臂相对位姿描述的是左右臂末端坐标系之间的空间关系。设左臂末端坐标系 {E_L} 相对于机器人基座坐标系 {B} 的齐次变换矩阵为 T_{B_E_L}，右臂末端坐标系 {E_R} 相对于基座的变换矩阵为 T_{B_E_R}，则右臂末端相对于左臂末端的位姿为：

T_E_L_E_R = (T_B_E_L)⁻¹ · T_B_E_R

该矩阵包含旋转矩阵 R_{E_L_E_R} 和平移向量 p_{E_L_E_R}。在实际应用中，通常将旋转矩阵转换为欧拉角或轴角表示，以便于直观理解和规划。

3.2.2 相对位姿的工程应用

相对位姿描述在双臂协调中具有多重用途：

3.2.3 相对位姿的测量与校准

相对位姿的获取方式有两种：一是通过正运动学计算得到，即利用编码器读数计算双臂末端位姿；二是通过外部传感器测量，如双目视觉系统或激光跟踪仪。前者存在运动学误差累积问题，后者精度较高但可能受遮挡影响。工程中常采用视觉伺服方式：在物体上粘贴标记点，通过视觉系统实时测量物体位姿，再结合抓持点偏移量计算双臂末端的目标相对位姿。

3.3 协同工作空间分析

3.3.1 协同工作空间的定义与求解

协同工作空间（Collaborative Workspace）是指双臂末端能够同时到达且满足闭链约束的空间区域。与单臂工作空间不同，协同工作空间不仅要求双臂末端各自位于其可达范围内，还要求双臂末端之间的相对位姿满足抓持约束。数学上，协同工作空间可定义为：

W_collab = {p ∈ R³ | ∃ q_L, q_R, 使得 FK_L(q_L) = p, FK_R(q_R) = p + Δp, 且满足闭链约束}

其中 Δp 为双臂末端之间的固定偏移向量。求解协同工作空间通常采用数值采样法：在机器人基座周围的空间网格中采样候选点，对每个点判断是否存在满足约束的双臂关节角组合。

3.3.2 协同工作空间的性能指标

评估协同工作空间的质量，不能仅看体积大小，还需关注以下指标：

• 可达性比率：协同工作空间体积与双臂独立工作空间交集体积之比，反映闭链约束对工作空间的缩减程度。
• 灵巧度分布：在工作空间内各点的双臂可操作度（Manipulability）均值与方差，衡量双臂在该点附近运动的灵活性。
• 力传递性能：双臂末端能够施加的合力/合力矩范围，对于搬运重物或装配任务至关重要。
• 奇异位形分布：工作空间内双臂雅可比矩阵奇异点的分布密度，应尽量避开奇异区域。

3.3.3 协同工作空间的优化策略

当协同工作空间不满足任务需求时，可通过以下途径优化：

1. 调整双臂安装位置：增大双臂基座间距可扩大横向工作范围，但会减小前方重叠区域；减小间距则相反。
2. 优化抓持点位置：合理选择物体上的抓持点，使双臂末端在任务轨迹上保持较好的力传递性能。
3. 利用躯干运动：人形机器人可通过腰部或躯干的俯仰、旋转运动扩展协同工作空间，这相当于增加了额外的运动自由度。

3.4 案例拆解：双臂协同搬运大型面板

3.4.1 任务背景与约束

某人形机器人需要协同搬运一块尺寸为 1200mm × 800mm、重量约 8kg 的玻璃面板。双臂末端采用真空吸盘抓持，吸盘中心间距为 600mm。机器人躯干固定，双臂各有 7 个自由度（肩部3+肘部1+腕部3）。任务要求：在搬运过程中，面板始终保持水平（俯仰角误差 < 2°），且双臂末端之间的相对位姿变化不超过 ±5mm 和 ±1°。

3.4.2 建模与求解过程

首先建立双臂闭链约束方程。设左吸盘中心为 E_L，右吸盘中心为 E_R，面板中心为 O。由于吸盘与面板刚性连接，T_{E_L_O} 和 T_{E_R_O} 为固定变换。根据约束方程，右臂末端位姿完全由左臂末端位姿决定。采用主从法，将左臂设为主臂，规划其末端沿一条从起始点到目标点的直线轨迹运动，轨迹上共采样 200 个路径点。

对于每个路径点，通过约束方程计算右臂末端的目标位姿，然后使用解析逆运动学求解右臂的 7 个关节角。在仿真环境中验证所有关节角均在限位范围内，且双臂雅可比矩阵的条件数小于 50（避免奇异位形）。

3.4.3 结果分析与优化

仿真结果显示，协同工作空间在面板水平姿态下覆盖了任务所需的全部路径点，但靠近路径中段时，右臂的灵巧度较低（可操作度仅为最大值的 0.3）。分析发现，该区域右臂接近肩关节奇异位形。优化措施：将面板的抓持点略微外移（从 600mm 增至 650mm），同时调整左臂轨迹的中间路径点高度（降低 30mm），使右臂避开奇异区域。优化后，全程可操作度提升至 0.6 以上，且面板姿态误差控制在 0.5°以内。

3.4.4 案例复盘

本案例表明，双臂协调运动学建模的关键在于：准确建立闭链约束方程，合理选择主从臂策略，并通过协同工作空间分析提前识别灵巧度不足的区域。实际系统中，还需考虑吸盘抓持的柔性，建议在从臂控制中加入力/位混合控制，以吸收面板的微小变形。此外，搬运过程中的加速度应限制在 0.3g 以内，避免因惯性力导致吸盘滑移。

3.5 总结与工程要点

双臂协调运动学建模是协调控制的理论基石。本章的核心要点可归纳为：

• 闭链约束是双臂协调的本质特征，其数学表达为 T_{E_L}·T_{L_O} = T_{E_R}·T_{R_O}，求解策略包括主从法、对称法和虚拟连杆法。
• 相对位姿描述通过齐次变换矩阵实现，需结合视觉测量与运动学计算，并定期校准基座间相对位姿。
• 协同工作空间分析应综合体积、灵巧度、力传递性能和奇异位形分布，通过优化安装位置、抓持点和轨迹规划来提升性能。
• 工程实现中，主从法配合力/位混合控制是当前最稳健的方案，但需注意误差累积和内力控制问题。