第二章：机器人运动学基础——坐标变换、齐次矩阵、正运动学（DH参数）、逆运动学求解

各位同学，欢迎来到第二章。说实话，运动学是机器人控制的“地基”。你想想看，如果连机器人手在哪儿、怎么动都不知道，那力控拖动示教根本无从谈起。我当年刚入行时，就吃过这个亏——以为跳过基础直接调力控算法就行，结果调试现场机器人乱窜，差点把夹具撞坏。从那以后，我老老实实回来啃运动学。

这一章，我们就把地基打牢。我会带着你，从坐标变换一路走到逆运动学求解。别怕，咱们一步步来。

2.1 坐标变换：让机器人“知道”自己在哪儿

机器人运动学，说白了就是研究“位置”和“姿态”的关系。一个刚体在空间中有6个自由度：3个位置（x, y, z），3个姿态（绕x、y、z轴的旋转）。

我习惯把坐标变换分成两类：

  • 平移变换：两个坐标系原点不同，但方向一致。比如，你的手肘坐标系相对于肩膀坐标系，就是平移。
  • 旋转变换：两个坐标系原点重合，但方向不同。比如，你扭头看左边，你的眼睛坐标系相对于头坐标系，就是旋转。

实际项目中，平移和旋转往往是同时发生的。比如，机器人末端执行器相对于基座，既有平移又有旋转。这时候，我们就需要齐次矩阵了。

我的小经验： 刚开始学坐标变换时，别死记公式。拿个笔在桌上比划一下，或者用手机摄像头对着一个物体转一转，直观感受“旋转”和“平移”的区别。我当年就是这么干的。

2.2 齐次矩阵：把平移和旋转“打包”在一起

齐次矩阵，说白了就是一个4x4的矩阵，把旋转（3x3）和平移（3x1）塞进一个框里。为什么是4x4？因为要统一乘法和求逆的运算规则。

它的标准形式长这样：

| R   t |
| 0   1 |

其中，R是3x3旋转矩阵，t是3x1平移向量。最后一行是[0 0 0 1]，这是齐次坐标的“固定尾巴”。

举个例子，假设你想把坐标系A中的点p_A，变换到坐标系B中。你只需要知道A相对于B的齐次矩阵T_B_A，然后做一次矩阵乘法：

p_B = T_B_A * p_A

就这么简单。但要注意，矩阵乘法不满足交换律。先旋转再平移，和先平移再旋转，结果完全不同。我在项目中见过有人搞反顺序，结果机器人末端位置差了十几厘米，还好仿真阶段发现了。

避坑指南： 我曾经在调试六轴机器人时，因为齐次矩阵的乘法顺序写反，导致轨迹规划全乱套。记住：左乘是相对于固定坐标系，右乘是相对于当前坐标系。这个坑，我替你踩过了。

2.3 正运动学（DH参数）：从关节角度到末端位姿

正运动学，就是已知每个关节的角度，求末端执行器在空间中的位置和姿态。最经典的方法就是DH参数法（Denavit-Hartenberg）。

DH参数有4个：

  • a：连杆长度（沿x轴方向）
  • α：连杆扭转角（绕x轴旋转）
  • d：连杆偏距（沿z轴方向）
  • θ：关节角（绕z轴旋转）

每个关节对应一个DH参数表。比如一个六轴机器人，就有6行4列的参数。然后，每个关节的齐次矩阵可以写成：

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

把所有关节的矩阵连乘起来，就得到末端相对于基座的位姿：

T_0_n = T_1 * T_2 * ... * T_n

我个人的习惯是，在写代码前，先在纸上把DH参数表列出来，然后手动算一遍前两个关节的矩阵乘法。这样能快速发现参数错误。

核心要点： DH参数的关键在于坐标系建立。每个关节的z轴必须沿着关节旋转轴，x轴沿着相邻z轴的公垂线。这个规则不能乱，否则算出来的位姿全是错的。

下面是我用SVG画的一张DH参数建模流程图，帮你理清思路：

DH参数正运动学流程 建立连杆坐标系 填写DH参数表 计算各关节矩阵 连乘 末端位姿矩阵 注意：每个关节的坐标系必须严格遵循DH约定，否则结果全错。 我建议在仿真中先用简单模型验证，再上真机。

2.4 逆运动学求解：从末端位姿到关节角度

逆运动学，就是已知末端位姿，反求每个关节的角度。这比正运动学难得多，因为：

  • 解可能不唯一（多解）
  • 解可能不存在（超出工作空间）
  • 解可能不连续（奇异点）

常用的求解方法有：

  • 解析法：通过几何关系直接推导出角度公式。速度快，但只适用于特定构型（如6轴机器人有封闭解的条件）。
  • 数值法：用迭代优化逼近解。通用性强，但速度慢，且可能陷入局部最优。

我个人在实际项目中，90%的情况用解析法。为什么呢？因为力控拖动示教要求实时性，数值法动不动几十毫秒，根本扛不住。但解析法需要满足一个条件：机器人的后三个关节轴交于一点（即腕部）。大部分工业机器人都满足这个条件。

一个小技巧： 当你用解析法求解时，先解前三个关节（决定位置），再解后三个关节（决定姿态）。这样可以把6自由度问题拆成两个3自由度问题，大大简化计算。我当年在做一个焊接机器人项目时，就是用这个思路把求解时间从20ms降到了0.5ms。

下面是一个简单的逆运动学求解伪代码示例（以6轴机器人为例）：

// 输入：末端位姿矩阵 T_target
// 输出：关节角度数组 q[6]

function inverse_kinematics(T_target):
    // 1. 从T_target中提取位置p和姿态R
    p = T_target[0:3, 3]
    R = T_target[0:3, 0:3]
    
    // 2. 根据几何关系求解前三个关节
    // 这里以常见的球形腕部为例
    q[0] = atan2(p.y, p.x)
    // ... 省略中间推导过程
    q[2] = acos((p.x^2 + p.y^2 + p.z^2 - a2^2 - a3^2) / (2*a2*a3))
    
    // 3. 根据前三个关节和姿态R，求解后三个关节
    // 注意：这里需要处理多解情况
    // 我一般选择“最短路径”解，即离当前关节角度最近的解
    q[3] = atan2(...)
    q[4] = acos(...)
    q[5] = atan2(...)
    
    // 4. 检查解是否在关节限位内
    for i in 0..5:
        if q[i] < q_min[i] or q[i] > q_max[i]:
            return None  // 无有效解
    
    return q
避坑指南： 我曾经在逆解时忽略了奇异点，结果机器人经过奇异点附近时，关节速度瞬间飙升，差点触发急停。记住：在奇异点附近，即使有解，关节速度也可能趋于无穷大。一定要在代码中加入奇异点检测，比如检查雅可比矩阵的行列式是否接近0。

2.5 本章小结

这一章我们讲了：

  • 坐标变换：平移和旋转，以及它们的组合
  • 齐次矩阵：用4x4矩阵统一表示位姿变换
  • 正运动学（DH参数）：从关节角度到末端位姿
  • 逆运动学求解：从末端位姿到关节角度

这些内容，是后续力控拖动示教的基础。你想想看，如果连机器人的运动学模型都建不准，那力控算法再牛也是白搭。我建议你，学完这一章后，找个开源机器人模型（比如UR5的DH参数），自己动手写一遍正逆解代码。只有亲手写过，才能真正理解。

好了，这一章就到这里。下一章，我们会进入力控的核心——如何让机器人“感觉”到外力，并做出响应。

专注资料整理

← 上一章 📖 返回目录 下一章 →