3. 运动学基础（下）：DH参数法、正运动学建模、逆运动学求解

好，咱们接着聊运动学。上一章我们把刚体变换和旋转矩阵讲透了，这一章直接上硬菜——DH参数法、正运动学、逆运动学。这三板斧你要是拿下了，市面上90%的机械臂你都能建模。

说实话，我刚入行那会儿，最怕的就是逆运动学。正运动学还好说，给角度算位置，套公式就行。逆运动学？那是给位置反推角度，有时候解不出来，有时候解出来一堆，选哪个都头疼。后来做项目做多了，才慢慢摸清门道。

3.1 DH参数法：机械臂建模的通用语言

DH参数法，全称Denavit-Hartenberg参数法。说白了，就是用四个参数来描述相邻两个关节之间的变换关系。你想想看，一个机械臂再复杂，也就是一堆连杆和关节串起来的。DH参数就是给每个关节-连杆对建立一套标准化的坐标系。

我个人习惯用改进型DH（Modified DH），也就是Craig那套。标准DH和改进DH的区别在于坐标系附着的位置不同，但核心思想一样。这里我以改进DH为例。

四个参数分别是：

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

核心公式：相邻连杆的齐次变换矩阵

i-1T_i = RotX(α_i-1) · TransX(a_i-1) · RotZ(θ_i) · TransZ(d_i)

展开后就是4x4矩阵，我直接给出来，你写代码时直接套用：

i-1T_i = [
  [cosθ,  -sinθ·cosα,   sinθ·sinα,   a·cosθ],
  [sinθ,   cosθ·cosα,  -cosθ·sinα,   a·sinθ],
  [0,      sinα,         cosα,         d    ],
  [0,      0,            0,            1    ]
]

嗯，这里要注意：α和a是连杆本身的几何属性，一旦机械臂设计定型，这两个值就固定了。d和θ则取决于关节类型——旋转关节θ是变量，d固定；移动关节d是变量，θ固定。

我的经验：建DH参数表时，坐标系的原点尽量选在关节轴线上。我在项目中遇到过有人把原点选偏了，结果算出来的正运动学总是差几毫米，排查了半天才发现是坐标系没对齐。

3.2 正运动学建模：从关节空间到笛卡尔空间

正运动学，就是已知每个关节的角度（或位移），求末端执行器的位姿。说白了，你给机器人发了一组角度指令，它末端到底在哪儿、朝向如何？

步骤很简单：

建立各连杆坐标系
填写DH参数表
写出相邻连杆的变换矩阵
连乘得到末端相对于基座的变换矩阵

举个例子，一个6轴工业机器人，从基座到末端，变换矩阵就是：

0T_6 = 0T_1 · 1T_2 · 2T_3 · 3T_4 · 4T_5 · 5T_6

这个矩阵的左上角3x3是姿态（旋转矩阵），右上角3x1是位置。你拿到这个矩阵，末端的位置和姿态就全知道了。

我记得有一次调试一个协作机器人，正运动学算出来末端位置总是偏了2cm。我反复检查DH参数，最后发现是d₃的符号写反了。这种低级错误，新手特别容易犯。

避坑指南：我曾经因为DH参数表中α的符号搞反，导致整个运动学模型镜像了。建议你每次建完模型，先用一个已知位姿验证一下——比如让所有关节归零，看看末端是不是在机械臂的“零位”上。

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

逆运动学就难了。已知末端位姿，求各关节角度。为什么难？因为正运动学是单射，逆运动学是多射——同一个末端位姿，可能有多个关节角度组合能到达。

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

3.3.1 解析法（封闭解法）

解析法，就是通过代数或几何推导，直接写出关节角度的表达式。速度快、精度高，但不是所有机械臂都有解析解。

Pieper准则：如果六轴机器人的三个相邻关节轴交于一点（比如腕部），则存在解析解。市面上大多数工业机器人都满足这个条件。

解析法的核心思路是“分而治之”：

先解前三个关节（位置关节），确定腕部位置
再解后三个关节（姿态关节），确定腕部姿态

以典型的6轴机器人为例，逆解过程大致是：

// 伪代码示例
function inverseKinematics(T_06):
    // 1. 从T_06中提取末端位置p和姿态R
    p = T_06[0:3, 3]
    R = T_06[0:3, 0:3]
    
    // 2. 计算腕部位置（假设后三轴交于一点）
    p_wrist = p - d6 * R[:, 2]
    
    // 3. 解前三个关节（几何法）
    θ1 = atan2(p_wrist.y, p_wrist.x)
    // ... 继续解θ2, θ3
    
    // 4. 根据前三个关节，解后三个关节（欧拉角法）
    // ... 解θ4, θ5, θ6
    
    return [θ1, θ2, θ3, θ4, θ5, θ6]

注意：解析解通常有多个。比如θ1可能有2个解，θ2可能有2个解，组合起来就是8组解。你需要根据关节限位、避障、能耗等条件选一组。

3.3.2 数值法（迭代法）

如果机械臂不满足Pieper准则，或者你懒得推导解析式，那就用数值法。数值法的本质是优化——不断调整关节角度，让正运动学算出来的末端位姿逼近目标位姿。

最常用的是牛顿-拉夫森法和雅可比伪逆法：

// 雅可比伪逆法伪代码
function numericalIK(T_target, θ_init, max_iter=100, tol=1e-6):
    θ = θ_init
    for i in range(max_iter):
        // 正运动学计算当前位姿
        T_current = forwardKinematics(θ)
        
        // 计算位姿误差
        error = computeError(T_target, T_current)
        if norm(error) < tol:
            break
        
        // 计算雅可比矩阵
        J = computeJacobian(θ)
        
        // 用伪逆更新关节角度
        Δθ = pinv(J) * error
        θ = θ + Δθ
    
    return θ

数值法的优点是通用性强，什么构型的机械臂都能解。缺点是慢、依赖初值、可能陷入局部最优。

我的建议：能解析就解析，解析不了再用数值。实际项目中，我通常先用解析法算一组初值，再用数值法微调。这样既快又准。

3.4 知识体系总览

下面这张图，把本章的核心逻辑串起来了。你顺着箭头看，就能明白运动学建模的完整流程。

3.5 实战要点总结

最后，我把这些年做运动学建模踩过的坑和心得，浓缩成几条：

DH参数表一定要反复核对。我见过太多人因为一个符号错，整个模型废掉。建议用仿真软件（如MATLAB Robotics Toolbox）验证。
正运动学是逆运动学的基础。正运动学算不对，逆运动学全是扯淡。先把正运动学调准了，再搞逆解。
解析解优先，数值解兜底。能推导解析式就推导，实在不行再用数值法。数值法记得给初值，别从零开始瞎迭代。
多解问题要处理。逆解出来8组解，你得根据关节限位、运动平滑性、避障等因素选一组。我一般用“最短路径”原则——选离当前关节角度最近的那组解。

重要提醒：逆运动学求解时，一定要考虑奇异位形。当机械臂处于奇异位形时，雅可比矩阵秩亏，数值法会失效。解析法虽然能算，但关节速度会趋于无穷大。实际控制中，要避开奇异点附近区域。

好了，运动学基础就讲到这里。你把这些搞懂了，机械臂的“骨架”就有了。下一章我们聊动力学——那才是真正让机械臂“动起来”的关键。

📚 课程目录

3. 运动学基础（下）：DH参数法、正运动学建模、逆运动学求解

3.1 DH参数法：机械臂建模的通用语言

3.2 正运动学建模：从关节空间到笛卡尔空间

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

3.3.1 解析法（封闭解法）

3.3.2 数值法（迭代法）

3.4 知识体系总览

3.5 实战要点总结