4、运动学建模：D-H参数法、正运动学与逆运动学求解、雅可比矩阵及其在人形机器人中的应用

人形机器人的运动学建模是连接机械本体设计与运动控制的核心桥梁。与工业机械臂固定在基座上不同，人形机器人具有浮动基座、多分支链（双腿、双臂、躯干）以及闭链约束（双脚同时着地）等显著特征，这使其运动学问题远比传统串联机械臂复杂。本章将系统阐述D-H参数法的建模规范、正逆运动学的求解策略、雅可比矩阵的构造方法，并结合双足支撑与全身协调控制场景，剖析这些理论工具在工程实践中的具体应用路径。

上图给出了本章的知识体系结构。D-H参数法解决“如何描述关节与连杆”的问题，正运动学解决“给定关节角求末端位姿”的问题，逆运动学解决“给定末端位姿求关节角”的问题，而雅可比矩阵则建立了关节空间与操作空间之间的速度与力映射关系。四者环环相扣，构成人形机器人运动学分析与控制的理论基石。

4.1 D-H参数法：从串联臂到人形多分支链的建模规范

4.1.1 核心原理：标准型与改进型的本质区别

D-H参数法由Denavit和Hartenberg于1955年提出，其核心思想是在每个连杆上固连一个坐标系，通过四个参数（连杆长度a_i-1、连杆转角α_i-1、连杆偏距d_i、关节角θ_i）描述相邻连杆间的相对位姿关系。标准型（Standard D-H）将坐标系固连在连杆远端，而改进型（Modified D-H）将坐标系固连在连杆近端。对于人形机器人而言，改进型D-H参数在描述树形拓扑结构时更为直观，因为它允许每个关节的坐标系独立定义，便于处理分支链的合并与拆分。

人形机器人建模的特殊性体现在三个方面：浮动基座（骨盆或躯干作为虚拟基座，其位姿由世界坐标系下的6个自由度描述）、多分支链（双腿、双臂、躯干构成多个串联链的集合）、闭链约束（双足支撑时，两条腿与地面构成闭链，导致关节角之间存在耦合关系）。因此，建模时通常以骨盆中心为根节点，分别建立左腿链、右腿链、躯干-头链、左臂链、右臂链的D-H参数表。

4.1.2 操作流程：人形机器人D-H建模五步法

1. 定义基座坐标系：以骨盆中心为原点，X轴指向机器人前进方向，Y轴指向左侧，Z轴垂直向上，建立世界坐标系下的浮动基座。
2. 划分运动链：将机器人拆分为左腿链（髋-膝-踝）、右腿链、躯干链（骨盆-胸-颈）、左臂链（肩-肘-腕）、右臂链，每条链独立编号。
3. 确定关节轴：对每个旋转关节，确定其旋转轴方向，并标注在机构简图上。人形机器人主要包含俯仰（Pitch）、滚转（Roll）、偏航（Yaw）三类关节轴。
4. 建立连杆坐标系：按照改进D-H规则，从基座开始依次为每个连杆建立坐标系，确保Z_i轴沿关节轴i+1方向，X_i轴沿相邻Z轴的公垂线方向。
5. 填写参数表：逐一测量或从CAD模型提取a_i-1、α_i-1、d_i、θ_i四个参数，形成完整的D-H参数表。

4.1.3 案例拆解：双足机器人腿部D-H建模

以一款典型的6自由度双腿人形机器人为例（每条腿3个自由度：髋关节Roll/Pitch、膝关节Pitch、踝关节Pitch/Roll），其左腿链的改进D-H参数表可设计如下（单位：mm，角度变量）：

关节i   a(i-1)   α(i-1)   d(i)    θ(i)
1(髋Roll)  0       0       85     θ1
2(髋Pitch) 0       -90°    0      θ2-90°
3(膝Pitch) 300     0       0      θ3
4(踝Pitch) 300     0       0      θ4
5(踝Roll)  0       90°     0      θ5

该参数表的关键在于：髋关节Roll与Pitch的轴线垂直相交，因此α₂=-90°；膝关节与踝关节Pitch轴线平行，因此α₃=α₄=0°。通过该表，可以计算任意关节角组合下踝关节（或足底）相对于骨盆的位姿矩阵。实际工程中，还需在踝关节下方增加一个固定偏置（足底高度），以描述足底与地面的接触点。

4.1.4 常见误区与总结

正确的做法是：在建模阶段就明确浮动基座的处理方式，并在仿真环境中通过对比正运动学计算结果与三维模型实际位姿进行验证。建议使用Robotics Toolbox或Pinocchio等库进行D-H参数校验。

4.2 正运动学求解：齐次变换矩阵链乘与基座传播

4.2.1 核心原理：从关节空间到操作空间的映射

正运动学的目标是给定所有关节变量（包括浮动基座位姿），计算机器人任意连杆（尤其是末端执行器、质心、足底）在世界坐标系下的位置和姿态。对于人形机器人，这一过程分为两步：基座传播（将浮动基座位姿从世界坐标系传递到骨盆坐标系）和链式传播（沿各运动链依次计算每个连杆的位姿）。

数学上，相邻连杆间的齐次变换矩阵为：

T(i,i-1) = Rot(x,α(i-1)) * Trans(x,a(i-1)) * Rot(z,θ(i)) * Trans(z,d(i))

对于一条包含n个关节的运动链，末端连杆相对于基座的位姿为：

T(n,base) = T(1,base) * T(2,1) * ... * T(n,n-1)

人形机器人的全身正运动学需要同时计算所有运动链的末端位姿，并考虑各链之间的几何约束（如双腿同时着地时，两足底之间的相对位姿必须与骨盆位姿一致）。

4.2.2 操作流程：全身正运动学计算步骤

1. 输入关节状态：获取所有关节的当前角度值以及浮动基座在世界坐标系下的位姿（通常由IMU和里程计融合估计）。
2. 计算基座变换：将浮动基座位姿转换为齐次变换矩阵T(base,world)。
3. 逐链计算：对每条运动链，从骨盆坐标系出发，依次乘以各关节的齐次变换矩阵，得到该链末端（如足底、手掌）的位姿。
4. 提取关键点：从计算结果中提取质心位置（需结合各连杆质量参数）、足底接触点、手部末端等关键信息，供后续控制使用。

4.2.3 案例拆解：双足支撑时的正运动学一致性校验

假设机器人双足平放于地面，已知骨盆位姿为(x=0.1m, y=0, z=0.85m, roll=0°, pitch=2°, yaw=0°)，左腿关节角为(θ1=5°, θ2=10°, θ3=-20°, θ4=15°, θ5=-5°)，右腿关节角为(θ1=-5°, θ2=10°, θ3=-20°, θ4=15°, θ5=5°)。通过正运动学计算，左足底在世界坐标系下的位置应为(0.05m, 0.12m, 0m)，右足底位置应为(0.05m, -0.12m, 0m)。若计算结果与预期不符，则说明D-H参数或关节角定义存在错误。

实际工程中，这种一致性校验是每次上电后的必检项目。操作方法是：将机器人置于水平地面，手动调整各关节至零位，然后读取IMU数据，计算骨盆位姿，再通过正运动学计算两足底位置。若两足底高度差超过2mm，或水平位置偏差超过5mm，则需检查机械装配误差或D-H参数标定值。

4.2.4 常见误区与总结

建议在仿真环境中使用随机关节角组合，对比正运动学计算结果与三维模型的实际位姿，进行批量自动化验证。同时，将正运动学封装为独立模块，便于在控制器、仿真器、可视化工具中复用。

4.3 逆运动学求解：解析法、数值法与多解处理

4.3.1 核心原理：从操作空间到关节空间的映射

逆运动学解决的是“已知末端期望位姿，求关节角”的问题。人形机器人的逆运动学比工业机械臂更复杂，原因在于：①浮动基座的存在使得目标位姿可以是全身任意连杆（如质心、足底、手掌），而不仅仅是末端执行器；②多分支链之间存在运动协调约束；③闭链约束（双足支撑）导致关节角不能独立求解。

求解方法分为两大类：解析法（基于几何关系或代数消元，得到封闭解）和数值法（基于雅可比矩阵的迭代优化）。解析法计算快、精度高，但仅适用于特定构型（如满足Pieper准则的6自由度串联臂）；数值法通用性强，但计算量大、可能陷入局部极小值。

4.3.2 操作流程：人形机器人逆运动学求解策略

1. 任务分解：将全身运动任务分解为若干子任务，如“保持质心高度不变”“左足跟踪期望轨迹”“右手到达目标点”。每个子任务对应一个或几个运动链。
2. 优先级设定：为不同子任务分配优先级。例如，双足支撑时，质心位置优先级最高，其次是足底接触力分布，最后是手臂运动。
3. 求解器选择：对于单链6自由度以下的任务，优先使用解析法；对于全身多任务协调，使用基于加权最小二乘的数值法。
4. 多解选择：当存在多个解时，根据关节限位、运动平滑性、能耗最小等准则选择最优解。

4.3.3 案例拆解：单腿支撑时的踝关节逆运动学

假设机器人处于单腿支撑状态，骨盆位姿已知，需要求解支撑腿的5个关节角，使得足底与地面保持水平接触。这是一个典型的闭链逆运动学问题。具体步骤如下：

• 输入：骨盆位姿T(pelvis,world)，期望足底位姿T(foot,world)（通常为水平放置，高度为0）。
• 计算相对位姿：T(foot,pelvis) = inv(T(pelvis,world)) * T(foot,world)。
• 解析求解：由于腿部为5自由度（少于6），存在冗余。通常固定踝关节Roll角为0（假设地面水平），然后利用几何法求解髋Roll、髋Pitch、膝Pitch、踝Pitch。
• 验证：将求解结果代入正运动学，检查足底位姿误差是否小于1mm和0.1°。

该案例中，若骨盆前倾2°，则踝关节Pitch角需要相应调整，以保持足底水平。实际控制中，还需考虑地面高度变化，通过力传感器反馈实时修正。

4.3.4 常见误区与总结

建议在求解器中加入关节限位约束和奇异规避策略。对于数值法，可采用阻尼最小二乘法（DLS）或Levenberg-Marquardt算法，在接近奇异时自动增加阻尼因子，保证求解稳定性。

4.4 雅可比矩阵：速度映射、奇异性与力传递

4.4.1 核心原理：关节空间与操作空间的线性映射

雅可比矩阵J(q)建立了关节速度与操作空间速度之间的线性关系：v = J(q)·q̇，其中v为操作空间速度（包括线速度和角速度），q̇为关节速度。对于人形机器人，雅可比矩阵的维度取决于操作空间的定义。例如，质心雅可比矩阵J_CoM将关节速度映射为质心线速度；足底雅可比矩阵J_foot将关节速度映射为足底六维速度。

雅可比矩阵的构造方法有两种：微分变换法（对正运动学方程求偏导）和矢量积法（利用各关节轴的单位向量和位置向量直接构造）。对于人形机器人，矢量积法更为高效，因为它不需要显式求导。

4.4.2 操作流程：雅可比矩阵的构造与应用

1. 确定操作空间：明确需要映射的末端（如质心、足底、手掌）及其自由度（3D位置、3D姿态或仅位置）。
2. 计算各关节贡献：对于旋转关节i，其对末端的线速度贡献为：J_vi = z_i × (p_end - p_i)；角速度贡献为：J_ωi = z_i。其中z_i为关节轴方向单位向量，p_i为关节位置，p_end为末端位置。
3. 组装矩阵：将所有关节的贡献按列排列，形成6×n或3×n的雅可比矩阵。
4. 应用场景：①速度控制：给定期望末端速度，求解关节速度q̇ = J⁺·v（J⁺为伪逆）；②奇异性分析：计算det(J·J^T)，判断是否接近奇异；③力映射：关节力矩τ = J^T·F，其中F为末端外力。

4.4.3 案例拆解：基于雅可比矩阵的全身柔顺控制

考虑一个全身柔顺控制场景：机器人双手推墙，同时保持双足稳定站立。控制目标为：①保持质心位置不变；②双手施加期望力F_des；③保持足底接触力在安全范围内。

• 构建任务雅可比：分别构建质心雅可比J_CoM、左手雅可比J_L、右手雅可比J_R。
• 设置优先级：质心任务优先级最高，双手力任务次之，足底力约束作为软约束。
• 求解关节力矩：τ = J_CoM^T·F_CoM + J_L^T·F_L + J_R^T·F_R + N·τ_null，其中N为质心任务的零空间投影矩阵，τ_null用于优化内部力分布。
• 实时更新：在每个控制周期（1ms），根据当前关节角重新计算雅可比矩阵，并更新控制力矩。

该方案在实际测试中，能够使机器人在受到外部推力时保持稳定，同时双手力误差小于5N。关键调优点在于：雅可比矩阵的数值稳定性（需加入奇异值滤波）和优先级矩阵的权重调节。

4.4.4 常见误区与总结

建议在控制器中加入雅可比矩阵的条件数监测，当条件数超过阈值时，自动切换为阻尼最小二乘解。同时，定期在仿真中验证雅可比矩阵的正确性，方法是将关节速度设为已知值，比较计算出的末端速度与正运动学差分结果。

运动学建模是人形机器人控制的基础，D-H参数法提供了统一的描述框架，正逆运动学解决了位姿映射问题，雅可比矩阵则架起了速度与力控制的桥梁。在实际工程中，这三者必须紧密结合，并充分考虑浮动基座、多分支链和闭链约束等特殊因素，才能构建出稳定、高效的运动控制系统。