3、运动学基础（下）：改进DH参数建模、正运动学求解、逆运动学求解（解析法与数值法）

好，咱们接着聊。上一节我们把DH参数的基本概念和标准DH讲透了。这一节，我重点说说改进DH参数，以及正逆运动学到底怎么算。说实话，这部分是机械臂控制的“硬骨头”，但啃下来之后，你会发现整个机器人瞬间变得“透明”了。

3.1 改进DH参数建模：为什么我偏爱它？

标准DH参数有个小毛病——它把坐标系建在连杆的末端。这在处理树形结构或者闭链机构时，容易把自己绕晕。我个人习惯用改进DH（Modified DH），也叫Craig版DH。它把坐标系建在连杆的首端。

说白了，区别就一句话：标准DH的变换顺序是“绕x转、沿x移、绕z转、沿z移”；改进DH的顺序是“绕x转、沿x移、绕z转、沿z移”——等等，听起来一样？不对，关键是参考系不同。改进DH的变换矩阵是：

T = Rot_x(α_{i-1}) · Trans_x(a_{i-1}) · Rot_z(θ_i) · Trans_z(d_i)

你看，α和a的下标是i-1，θ和d的下标是i。这个细微差别，在实际建模型时能省掉很多麻烦。我在做六轴协作机器人项目时，一开始用的标准DH，结果在写运动学标定程序时，参数耦合得一塌糊涂。换成改进DH后，参数物理意义更清晰，调试起来顺手多了。

举个例子，一个简单的两连杆平面臂，用改进DH建模：

连杆1: α0=0, a0=0, θ1=变量, d1=0
连杆2: α1=0, a1=L1, θ2=变量, d2=0

嗯，这里要注意：α0和a0都是0，因为第一个连杆没有前一个连杆。这个“从0开始”的索引方式，写程序时特别自然，不容易出现索引越界。

3.2 正运动学求解：从关节空间到笛卡尔空间

正运动学，说白了就是“你给我每个关节的角度，我告诉你末端在哪儿、朝哪儿”。这事儿没什么悬念，就是矩阵连乘。

对于n轴机械臂，末端位姿就是：

T_0_n = T_0_1 · T_1_2 · ... · T_{n-1}_n

每个T都是4x4的齐次变换矩阵。我记得第一次手算六轴臂的正运动学时，算了整整三页草稿纸，最后发现符号写错一个，整个末端位置偏了十万八千里。后来我学乖了——先用符号计算软件（比如SymPy）验证一遍，再手写代码。

这里给出一段Python伪代码，用改进DH算正运动学：

import numpy as np

def dh_transform(alpha, a, theta, d):
    """改进DH参数转齐次变换矩阵"""
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = np.sin(alpha)
    
    T = np.array([
        [ct,    -st,    0,      a],
        [st*ca, ct*ca,  -sa,   -d*sa],
        [st*sa, ct*sa,  ca,    d*ca],
        [0,     0,      0,      1]
    ])
    return T

def forward_kinematics(dh_params):
    """
    dh_params: 列表，每个元素为 [alpha, a, theta, d]
    返回末端位姿矩阵
    """
    T = np.eye(4)
    for param in dh_params:
        T_i = dh_transform(*param)
        T = T @ T_i
    return T

3.3 逆运动学求解：解析法与数值法

逆运动学就难了。给你末端位姿，让你反推关节角度。这玩意儿有多难？你想想看，正运动学是单射，逆运动学是多对一映射——同一个末端位姿，可能有0个、1个、甚至8个解。

求解方法分两大类：解析法和数值法。

3.3.1 解析法：快，但有条件

解析法，也叫封闭解法。它直接通过代数或几何关系，把关节角度表达式写出来。优点是快——微秒级就能出结果。缺点是——不是所有机械臂都有解析解。

什么情况下有解析解？Pieper准则说：如果机械臂的三个相邻关节轴交于一点，或者三个相邻关节轴平行，就有解析解。大多数工业六轴臂都满足这个条件，因为它们的腕部三个轴交于一点。

我曾经给一个非标七轴臂写逆解程序，硬是没有解析解。折腾了两周，最后老老实实换数值法。

解析法的典型步骤（以六轴臂为例）：

1. 从末端位姿反推腕部中心位置
2. 解前三个关节（位置逆解）——通常用几何法
3. 解后三个关节（姿态逆解）——通常用欧拉角反算
4. 处理多解情况，选择最优解（比如关节位移最小）

举个平面两连杆臂的例子，解析解就很简单：

给定末端位置 (x, y)，连杆长度 L1, L2

θ2 = ±arccos((x² + y² - L1² - L2²) / (2*L1*L2))
θ1 = atan2(y, x) - atan2(L2*sin(θ2), L1 + L2*cos(θ2))

看到那个±号了吗？这就是“肘部向上”和“肘部向下”两种构型。实际项目中，我会根据避障需求或者关节限位来选择。

3.3.2 数值法：通用，但慢

数值法，说白了就是“猜+迭代”。最常用的是牛顿-拉夫森法，或者它的变种——雅可比伪逆法。

核心思想：

1. 给定一个初始关节角度猜测 q₀
2. 计算当前末端位姿 f(q₀) 与目标位姿的误差 Δx
3. 用雅可比矩阵 J 的伪逆，计算关节角度修正 Δq = J⁺ · Δx
4. 更新 q = q₀ + Δq，重复直到收敛

代码实现大概长这样：

def inverse_kinematics_numerical(target_pose, dh_params, q_init, max_iter=100, tol=1e-6):
    q = q_init.copy()
    for i in range(max_iter):
        T_current = forward_kinematics(dh_params, q)
        error = compute_pose_error(target_pose, T_current)
        if np.linalg.norm(error) < tol:
            return q
        J = compute_jacobian(dh_params, q)
        J_pinv = np.linalg.pinv(J)
        delta_q = J_pinv @ error
        q = q + delta_q
    raise Exception("逆解未收敛")

数值法的优点是通用——任何构型的机械臂都能用。缺点是慢（毫秒级），而且可能陷入局部最优或奇异点。实时控制中，我一般用解析法做轨迹规划，数值法做离线标定或奇异点附近的补偿。

3.4 本章知识体系

下面这张图，是我自己总结的运动学核心脉络。你看一眼，应该能对本章内容有个整体把握。

最后说一句，运动学这东西，光看是学不会的。我建议你找一份真实的机械臂DH参数表，自己动手写一遍正逆解代码。遇到bug不要慌——我当年调试逆解时，把末端位置算到了月球上，后来发现是角度单位搞混了（度vs弧度）。这种坑，踩过一次就记住了。

可可精品资料库