3. 人形机器人运动学建模：D-H参数法、正运动学、逆运动学求解

运动学建模，说白了就是回答两个问题：「我的关节转到某个角度，末端到底在哪儿？」 和 「我想让末端去某个位置，关节该怎么转？」。前者叫正运动学，后者叫逆运动学。

我刚开始做人形机器人那会儿，总觉得运动学是「纯数学」，跟实际控制关系不大。直到有一次，我给机器人规划了一个迈步动作，结果脚掌直接插进地板里了……嗯，从那以后我才明白，运动学模型不准，后面所有控制都是空中楼阁。

这一章，我们就来把 D-H 参数法、正运动学、逆运动学这三块硬骨头啃下来。我会结合人形机器人的实际结构来讲，而不是泛泛地讲通用机械臂。

3.1 D-H 参数法：给每个关节一个「身份证」

D-H 参数法，是 Denavit 和 Hartenberg 两位老先生在 1955 年提出来的。它的核心思想很简单：用四个参数，唯一确定相邻两个连杆之间的坐标变换关系。

这四个参数分别是：

• a_i-1（连杆长度）：从 z_i-1 轴到 z_i 轴，沿 x_i-1 轴方向的距离。
• α_i-1（连杆扭角）：从 z_i-1 轴到 z_i 轴，绕 x_i-1 轴旋转的角度。
• d_i（关节距离）：从 x_i-1 轴到 x_i 轴，沿 z_i 轴方向的距离。
• θ_i（关节转角）：从 x_i-1 轴到 x_i 轴，绕 z_i 轴旋转的角度。

你可能会问：「为什么是这四个参数，而不是三个或五个？」

我个人理解是：刚体在空间中有 6 个自由度，但相邻连杆之间有一个旋转或移动关节约束掉了 1 个自由度，剩下 5 个自由度。再用 D-H 的坐标系约定固定掉 1 个自由度，正好剩下 4 个独立参数。 很巧妙，对吧？

3.1.1 人形机器人腿部 D-H 参数示例

以人形机器人的左腿为例，从髋关节到脚踝，我们通常建立 6 个关节的 D-H 参数表（髋关节 3 个自由度 + 膝关节 1 个 + 踝关节 2 个）。

关节 i	ai-1 (mm)	αi-1 (°)	di (mm)	θi (°)	关节类型
1（髋关节偏摆）	0	0	Lhip	θ1	旋转
2（髋关节俯仰）	0	-90	0	θ2	旋转
3（髋关节横滚）	0	90	Lthigh	θ3	旋转
4（膝关节俯仰）	0	-90	0	θ4	旋转
5（踝关节俯仰）	0	90	Lshank	θ5	旋转
6（踝关节横滚）	0	-90	0	θ6	旋转

注意看，这里的 a_i-1 几乎全是 0。为什么？因为人形机器人的连杆大多是「共线」的，不像工业机械臂那样有偏置。但 α_i-1 的 ±90° 很关键，它体现了关节轴之间的垂直关系。

3.2 正运动学：从关节空间到任务空间

正运动学，就是已知关节角度，求末端位姿。说白了，就是「正向计算」。

有了 D-H 参数表，正运动学就变成了一个机械化的矩阵乘法过程。相邻连杆的变换矩阵为：

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

展开后：
T(i-1,i) = 
[ cosθ(i)   -sinθ(i)   0          a(i-1)          ]
[ sinθ(i)*cosα(i-1)   cosθ(i)*cosα(i-1)   -sinα(i-1)   -d(i)*sinα(i-1) ]
[ sinθ(i)*sinα(i-1)   cosθ(i)*sinα(i-1)    cosα(i-1)    d(i)*cosα(i-1)  ]
[ 0          0          0          1                ]

然后，从基座到末端的总变换矩阵就是：

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

这个 T(0,n) 矩阵的左上角 3×3 是姿态（旋转矩阵），右上角 3×1 是位置（平移向量）。

我在项目中通常会用 Python 的 SymPy 库来做符号推导，而不是手算。原因很简单：人形机器人动辄 12 个自由度（双腿），手算 12 个 4×4 矩阵乘法，不出错才怪。

3.3 逆运动学：从任务空间回到关节空间

逆运动学就麻烦多了。给定末端的目标位姿，求关节角度。这本质上是一个非线性方程组求解问题。

对于人形机器人，逆运动学有两个主要流派：

• 解析法（封闭解）： 利用机器人结构的几何特性，直接推导出关节角度的表达式。速度快，精度高，但只适用于满足 Pieper 准则的机器人（三个相邻关节轴交于一点）。
• 数值法（迭代法）： 用牛顿-拉夫森、LM 算法等迭代求解。通用性强，但速度慢，且可能陷入局部最优。

我个人更偏爱解析法。为什么？因为人形机器人的腿部和手臂结构，通常都满足 Pieper 准则——踝关节和腕关节的三个轴交于一点。这就意味着我们可以用解析法快速求解。

3.3.1 解析法求解思路（以腿部为例）

假设我们要让左脚掌到达某个目标位姿。求解步骤大致如下：

1. 位置分离： 从目标位姿矩阵中提取出脚掌位置 p_foot 和姿态 R_foot。
2. 求髋关节位置： 利用脚掌位置和已知的踝关节到脚掌的变换，反推出髋关节的位置 p_hip。
3. 求膝关节角度： 已知髋关节和踝关节的位置，用余弦定理求出膝关节的俯仰角 θ_knee。
4. 求髋关节角度： 根据髋关节到踝关节的向量，分解出髋关节的三个角度。
5. 求踝关节角度： 用剩余的旋转分量，确定踝关节的两个角度。

听起来不复杂？但实际编码时，多解选择是最头疼的。比如膝关节可以「正弯」也可以「反弯」，你选哪个？

3.3.2 数值法代码示例（Python + NumPy）

当解析法不适用时（比如机器人结构不满足 Pieper 准则），我会用数值法兜底。下面是一个基于雅可比矩阵的迭代求解框架：

import numpy as np

def inverse_kinematics_numerical(fk_func, target_pose, q_init, max_iter=100, tol=1e-6):
    """
    数值法逆运动学求解
    fk_func: 正运动学函数，输入关节角度，输出末端位姿
    target_pose: 目标位姿 (4x4 齐次矩阵)
    q_init: 初始关节角度
    """
    q = q_init.copy()
    for i in range(max_iter):
        # 当前位姿
        current_pose = fk_func(q)
        # 位置误差
        pos_error = target_pose[:3, 3] - current_pose[:3, 3]
        # 姿态误差（用轴角表示）
        R_error = target_pose[:3, :3] @ current_pose[:3, :3].T
        angle = np.arccos((np.trace(R_error) - 1) / 2)
        if angle < 1e-10:
            axis = np.array([0, 0, 0])
        else:
            axis = np.array([R_error[2,1]-R_error[1,2],
                             R_error[0,2]-R_error[2,0],
                             R_error[1,0]-R_error[0,1]]) / (2*np.sin(angle))
        rot_error = angle * axis
        # 总误差
        error = np.concatenate([pos_error, rot_error])
        if np.linalg.norm(error) < tol:
            break
        # 计算雅可比矩阵（数值法）
        J = compute_jacobian(fk_func, q)
        # 阻尼最小二乘法
        lambda_reg = 0.01
        delta_q = np.linalg.inv(J.T @ J + lambda_reg * np.eye(len(q))) @ J.T @ error
        q = q + delta_q
    return q

3.4 人形机器人运动学的特殊问题

跟工业机械臂不同，人形机器人有几个特有的运动学问题：

• 闭链约束： 双脚同时着地时，两条腿和地面形成一个闭链。此时不能独立求解每条腿的逆运动学，必须考虑两条腿之间的耦合关系。
• 支撑脚切换： 走路时，支撑脚和摆动脚的角色会切换。每次切换，运动学模型的基坐标系都要重新定义。
• 躯干姿态控制： 人形机器人的躯干不是固定的，它的俯仰和横滚会影响双腿的运动学解。

我在做双足步行控制时，最常用的策略是：把躯干作为浮动基座，双腿分别做逆运动学，然后用躯干的姿态误差来修正双腿的关节角度。 这样既能保证双脚的位置精度，又能保持躯干的平衡。