2. 人形机器人上肢运动学基础

人形机器人的双臂协调与灵巧操作，其底层数学基础在于上肢运动学模型的精确建立与求解。无论是规划一次自然的抓取动作，还是实现双臂协同搬运，本质上都是在运动学空间中求解“关节空间”与“操作空间”的映射关系。本章将系统梳理人形机器人上肢运动学的四大核心模块：D-H参数建模、正运动学求解、逆运动学解算以及雅可比矩阵与奇异性分析。理解这些内容，是后续进行速度规划、力控制与避障规划的前提。

2.1 D-H参数建模：从关节坐标系到变换矩阵

核心原理

Denavit-Hartenberg（D-H）参数法是一种系统化描述相邻连杆之间相对位姿的方法。对于人形机器人上肢，通常将肩关节（3自由度）、肘关节（1-2自由度）和腕关节（2-3自由度）抽象为一系列旋转关节。每个连杆通过四个参数唯一确定：连杆长度 a（沿 x 轴方向，从 z_i-1 到 z_i 的距离）、连杆扭转角 α（绕 x 轴，从 z_i-1 旋转到 z_i 的角度）、关节距离 d（沿 z_i-1 轴，从 x_i-1 到 x_i 的距离）以及关节转角 θ（绕 z_i-1 轴，从 x_i-1 旋转到 x_i 的角度）。对于旋转关节，θ 是变量，其余三个参数为常量。

建立 D-H 坐标系时，需遵循以下步骤：首先确定各关节的 z 轴方向（沿旋转轴线）；然后确定相邻 z 轴的公垂线方向作为 x 轴；最后根据右手定则确定 y 轴。相邻连杆坐标系间的齐次变换矩阵为：

T_i = Rot(z, θ_i) · Trans(z, d_i) · Trans(x, a_i) · Rot(x, α_i)

     | cosθ_i  -sinθ_i·cosα_i   sinθ_i·sinα_i   a_i·cosθ_i |
     | sinθ_i   cosθ_i·cosα_i  -cosθ_i·sinα_i   a_i·sinθ_i |
     | 0        sinα_i           cosα_i           d_i       |
     | 0        0                0                1         |

判断方法与操作流程

在实际建模中，人形机器人上肢常采用改进型 D-H（Modified D-H）或标准型 D-H（Standard D-H）。两者的区别在于变换顺序和坐标系附着位置不同。对于肩部具有球关节特性的机器人（如 SRS 构型），建议使用改进型 D-H 以避免奇异性描述歧义。建模流程如下：

1. 确定关节轴线：根据机器人 CAD 模型或实际结构，标定每个旋转关节的旋转轴方向。
2. 建立基坐标系：通常将基坐标系固定在机器人腰部或肩部基座，z 轴竖直向上。
3. 依次建立连杆坐标系：从基座开始，逐关节建立坐标系，确保每个坐标系满足 D-H 约束条件。
4. 填写 D-H 参数表：将每个关节的 a、α、d、θ 填入表格，注意符号约定。
5. 验证模型：将机器人置于已知位形（如零位），计算正运动学结果与实测位姿对比。

2.2 正运动学：从关节角到位姿矩阵

核心原理

正运动学解决的是“给定一组关节角度，求末端执行器的位姿”问题。对于 6 自由度上肢，末端位姿矩阵 T₀₆ 由各连杆变换矩阵连乘得到：T₀₆ = T₀₁ · T₁₂ · T₂₃ · T₃₄ · T₄₅ · T₅₆。该矩阵包含旋转部分 R（3×3）和平移部分 p（3×1），完整描述了末端在基坐标系下的位置和姿态。

在人形机器人中，正运动学计算频率通常要求达到 1kHz 以上，以支持实时控制。因此，需要优化矩阵连乘的计算效率。常用的优化手段包括：利用矩阵结构进行符号化简、使用 SIMD 指令集加速、以及预计算常量部分。

操作流程与案例拆解

以表 2-1 中的 6 自由度上肢为例，假设当前关节角为 θ = [0°, 45°, -30°, 60°, 20°, 0°]，计算末端位姿。首先逐项计算各 T_i-1,i 矩阵，然后依次相乘。实际计算中，可使用 MATLAB Robotics Toolbox 或 Python 的 Pinocchio 库进行验证。计算结果中，位置向量 p 的单位为 mm，旋转矩阵 R 的各列表示末端坐标系各轴在基坐标系中的方向余弦。

2.3 逆运动学：多解性与数值求解策略

核心原理

逆运动学是正运动学的逆映射，即已知末端期望位姿 T_des，求解满足 T₀₆(θ) = T_des 的关节角 θ。由于正运动学是非线性映射，逆运动学通常存在多解、无解或无穷解的情况。对于 6 自由度上肢，若满足 Pieper 准则（相邻三个关节轴交于一点），可得到解析解；否则需采用数值迭代方法。

人形机器人上肢通常满足球腕条件（腕部三轴交于一点），因此可先求解腕部中心位置，再分解为位置逆解和姿态逆解。位置逆解求解前三个关节角，姿态逆解求解后三个关节角。但需要注意，由于肩关节存在机械限位，某些解析解可能超出关节范围，需要根据关节限位进行筛选。

判断方法与数值求解流程

当解析解不存在或求解困难时，采用数值方法。常用方法包括：

• 雅可比转置法：利用 J^T 将末端位姿误差映射为关节力矩，迭代更新关节角。该方法简单但收敛慢。
• 阻尼最小二乘法（DLS）：在雅可比矩阵中加入阻尼项 λ，避免奇异点附近的数值发散。更新公式为 Δθ = (J^TJ + λ²I)⁻¹J^Te。
• Levenberg-Marquardt 算法：自适应调整阻尼系数，兼顾收敛速度与稳定性。

// 阻尼最小二乘逆运动学迭代伪代码
for iter = 1:max_iter
    T_current = fkine(θ);
    e = log_se3(T_des * inv(T_current));  // 6维误差向量
    if norm(e) < tol
        break;
    end
    J = jacobian(θ);
    Δθ = (J'*J + λ²*eye(6)) \ (J' * e);
    θ = θ + Δθ;
    λ = adapt(λ, norm(e));
end

2.4 雅可比矩阵与奇异性分析

核心原理

雅可比矩阵 J(q) 建立了关节速度与末端速度之间的线性映射：v = J(q)·q̇，其中 v 为 6 维速度向量（线速度+角速度），q̇ 为关节速度向量。雅可比矩阵的每一列对应一个关节的单位速度对末端速度的贡献。对于人形机器人双臂系统，还需考虑双臂耦合雅可比矩阵，用于协调控制。

奇异性是指雅可比矩阵秩亏的位形。在奇异位形附近，末端在某些方向上的运动能力丧失，关节速度趋于无穷大，导致控制失效。奇异性的数学判据为 det(J·J^T) = 0 或 J 的最小奇异值 σ_min = 0。

奇异性分类与判断方法

工程上常用可操作度 w = sqrt(det(J·J^T)) 来度量末端在各方向上的运动能力。w 值越小，越接近奇异。当 w 低于阈值时，应触发奇异规避策略。此外，条件数 cond(J) = σ_max/σ_min 也是重要指标，条件数越大，数值稳定性越差。

案例拆解：双臂协调中的奇异规避

在某双臂装配任务中，要求左右臂末端保持 100mm 间距并同步运动。当右臂运动到肩部正上方时，右臂腕部奇异被触发，导致右臂末端角速度指令发散。通过分析雅可比矩阵的奇异值分解（SVD），发现 σ_min 降至 0.02，而正常区域 σ_min 约为 0.5。解决方案是：在规划阶段引入可操作度约束，将右臂肘部略微外展 15°，使 σ_min 提升至 0.3。同时，在控制层加入阻尼最小二乘逆解，阻尼系数 λ 根据 σ_min 自适应调整：当 σ_min < 0.1 时，λ 从 0.01 线性增加至 0.1。实际测试中，末端位置误差从 8mm 降至 1.2mm，装配成功率从 72% 提升至 96%。

2.5 常见误区与总结复盘

常见误区

• 误区一：认为 D-H 参数是唯一的。实际上，同一机械臂可有多种 D-H 参数表示，关键在于坐标系建立的约定是否一致。不同文献中的参数不可直接混用。
• 误区二：忽略关节限位对逆解的影响。解析解可能给出超出限位的关节角，必须结合限位筛选。
• 误区三：将单臂雅可比分析直接套用于双臂。双臂协调时，闭链约束使得自由度减少，雅可比矩阵的维度和性质发生变化，需使用双臂耦合雅可比。
• 误区四：认为奇异只发生在工作空间边界。内部奇异同样常见，且更难直观发现，必须通过数值方法实时监测。

总结复盘

本章从 D-H 参数建模出发，建立了人形机器人上肢运动学的完整理论框架。正运动学提供了从关节空间到操作空间的确定性映射，逆运动学则解决了轨迹规划中的关节角求解问题，而雅可比矩阵及其奇异性分析是速度控制、力控制和灵巧度评估的基础。在实际工程中，需要将解析法与数值法结合，并始终关注奇异规避与计算实时性。对于双臂协调系统，还需进一步考虑双臂闭链约束和负载分配问题，这将在后续章节中深入探讨。