1. 机器人运动学基础：刚体变换、齐次坐标、DH参数法、正逆运动学求解

各位同学，欢迎来到《机器人轨迹规划全流程》的第一章。

说实话，运动学是整个机器人学的「地基」。你后面做轨迹规划、动力学控制、甚至是视觉伺服，都离不开这一章的内容。我见过不少新手，一上来就调PID、跑路径，结果机器人撞了都不知道为啥——说白了，就是运动学没搞透。

这一章，我会带你从最基础的刚体变换开始，一步步走到正逆运动学求解。嗯，内容不少，但都是硬货。

1.1 刚体变换：机器人的「骨架」怎么动？

先问一个问题：一个机械臂在空间里运动，本质上是什么在变？

答案是——刚体的位置和姿态。刚体，就是不会变形的物体。机器人的每个连杆，我们都可以看作一个刚体。

刚体变换，说白了就是描述一个刚体从A位置到B位置，怎么转、怎么移。它包含两部分：

  • 旋转：描述刚体「朝向」的变化
  • 平移：描述刚体「位置」的变化

我个人习惯把旋转矩阵记作 R，平移向量记作 t。那么一个完整的刚体变换可以写成：

p' = R * p + t

其中 p 是原始点坐标，p' 是变换后的坐标。

我的经验： 刚开始学的时候，别死记旋转矩阵的9个元素。你只需要记住三个轴的旋转公式，剩下的用「右手定则」推就行。我在项目中调试视觉抓取时，经常需要手算旋转矩阵来验证算法，这个基本功帮了大忙。

1.2 齐次坐标：把旋转和平移「打包」

你发现没有？上面的公式 p' = R*p + t 写起来有点啰嗦。而且旋转和平移是分开的，不方便做连续变换。

这时候，齐次坐标就派上用场了。

齐次坐标的核心思想很简单：给三维坐标加一个维度，变成四维。这样，旋转和平移就可以合并成一个 4x4 的矩阵：

| R   t |   | x |
| 0   1 | * | y |
            | z |
            | 1 |

这个 4x4 矩阵，就是齐次变换矩阵。它把旋转和平移「打包」在一起，一次矩阵乘法就能完成整个变换。

为什么要用齐次坐标？
  • 连续变换可以写成矩阵连乘：T = T1 * T2 * T3 ...
  • 逆变换直接求矩阵逆就行，不用分开算旋转和平移
  • 代码实现简单，一个矩阵搞定所有

我记得第一次用齐次坐标写代码时，感觉整个世界都清爽了。之前用欧拉角做连续旋转，各种万向锁、奇异性问题，换成齐次矩阵后，这些问题基本都绕开了。

1.3 DH参数法：给机器人「建模」的标准语言

好了，现在我们知道怎么描述一个刚体的运动了。但一个机械臂有6个、7个甚至更多连杆，怎么描述它们之间的关系？

Denavit-Hartenberg参数法，也就是DH参数法，就是干这个的。它用4个参数来描述相邻两个连杆之间的变换关系：

参数 符号 含义
连杆长度 ai-1 沿xi-1轴，从zi-1到zi的距离
连杆扭角 αi-1 绕xi-1轴，从zi-1到zi的角度
连杆偏距 di 沿zi轴，从xi-1到xi的距离
关节角 θi 绕zi轴，从xi-1到xi的角度

有了这4个参数，相邻连杆的变换矩阵就可以写成标准形式：

T_i = Rot(x, α_{i-1}) * Trans(x, a_{i-1}) * Rot(z, θ_i) * Trans(z, d_i)

你想想看，整个机械臂的末端位姿，就是把这些矩阵一个一个乘起来：

T_0n = T_1 * T_2 * ... * T_n
避坑指南： 我曾经在搭建一个协作机器人模型时，把DH参数的坐标系定义搞反了。结果算出来的末端位置差了10厘米，找了半天bug。后来我养成了一个习惯：每次建完DH参数表，先手动验证几个特殊关节角度（比如所有关节归零）下的末端位置，确认无误再往下走。

下面这张图展示了DH参数法的核心流程：

DH参数法建模流程 定义连杆坐标系 提取DH参数 构建变换矩阵 矩阵连乘求末端位姿 关键点： ① 坐标系定义要统一（标准DH或改进DH） ② 参数提取后务必验证特殊位姿 常见机器人DH参数示例（6轴工业机器人） 关节1: a=0, α=-90°, d=d1, θ=θ1 关节2: a=a2, α=0°, d=0, θ=θ2 关节3: a=a3, α=0°, d=0, θ=θ3 关节4: a=0, α=-90°, d=d4, θ=θ4 关节5: a=0, α=90°, d=0, θ=θ5 关节6: a=0, α=0°, d=d6, θ=θ6

1.4 正运动学求解：已知关节角，求末端位姿

正运动学，就是已知每个关节的角度（或位移），求机器人末端在空间中的位置和姿态。

说白了，就是「输入关节角，输出末端位姿」。

求解方法我们已经有了——就是上面说的DH参数法。把每个关节的变换矩阵乘起来，得到的就是末端相对于基座的位姿。

正运动学求解步骤：
  1. 建立DH参数表（每个关节一行）
  2. 根据当前关节角，计算每个关节的变换矩阵 T_i
  3. 连乘所有 T_i，得到 T_0n
  4. 从 T_0n 中提取位置向量和旋转矩阵

代码实现其实很直接。我习惯用Python写一个通用函数：

import numpy as np

def dh_transform(a, alpha, d, theta):
    """根据DH参数计算单个关节的变换矩阵"""
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = np.sin(alpha)
    
    T = np.array([
        [ct,   -st*ca,  st*sa,   a*ct],
        [st,    ct*ca,  -ct*sa,  a*st],
        [0,     sa,     ca,      d   ],
        [0,     0,      0,       1   ]
    ])
    return T

def forward_kinematics(dh_params, joint_angles):
    """正运动学求解"""
    T = np.eye(4)
    for i in range(len(joint_angles)):
        a, alpha, d = dh_params[i]
        theta = joint_angles[i]
        T_i = dh_transform(a, alpha, d, theta)
        T = T @ T_i
    return T
我的习惯： 写正运动学代码时，我会把每个关节的变换矩阵都存下来，而不是只存最终结果。这样调试时，可以逐关节检查，看是哪个关节的矩阵算错了。这个习惯帮我省了不少排查时间。

1.5 逆运动学求解：已知末端位姿，求关节角

逆运动学正好反过来——已知末端要到达的位置和姿态，反推每个关节应该转多少度。

这比正运动学难多了。为什么？因为：

  • 解可能不唯一（同一个末端位姿，可能有多种关节角度组合）
  • 可能无解（末端位姿超出了机器人的工作空间）
  • 解析解只存在于特定构型的机器人（比如有相邻三轴交于一点的机器人）

求解方法主要有两种：

方法 优点 缺点 适用场景
解析法（封闭解） 速度快、精度高、所有解都能找到 需要机器人满足特定几何条件 工业机器人（如6轴机器人有封闭解）
数值法（迭代法） 通用性强，任何机器人都能用 速度慢、可能陷入局部最优、需要初值 冗余机器人、特殊构型机器人
我曾经踩过的坑： 有一次做离线编程，我用数值法解逆运动学，结果仿真时机器人动作很顺，一到实际现场就撞了。后来发现是数值法收敛到了「错误」的解——虽然数学上正确，但关节角度超出了物理限位。从那以后，我每次解完逆运动学，都会加一步关节限位检查，确保每个关节角度都在安全范围内。

对于常见的6轴工业机器人，因为有相邻三轴交于一点（腕部），可以用解析法求解。基本思路是：

  1. 先解前三个关节（决定腕部位置）
  2. 再解后三个关节（决定末端姿态）
  3. 组合成完整的关节角度

这里我给出一个简化的解析法框架：

def inverse_kinematics(T_target, dh_params):
    """
    逆运动学求解（简化版）
    假设机器人满足Pieper准则（相邻三轴交于一点）
    """
    # 1. 从目标位姿矩阵中提取位置和姿态
    position = T_target[:3, 3]
    rotation = T_target[:3, :3]
    
    # 2. 计算腕部位置（前三个关节决定）
    wrist_pos = compute_wrist_position(position, rotation, dh_params)
    
    # 3. 解前三个关节角
    q1, q2, q3 = solve_first_three_joints(wrist_pos, dh_params)
    
    # 4. 解后三个关节角
    q4, q5, q6 = solve_last_three_joints(rotation, q1, q2, q3, dh_params)
    
    return [q1, q2, q3, q4, q5, q6]
逆运动学求解的实用建议：
  • 优先用解析法，速度快且稳定
  • 如果必须用数值法，给一个好的初值（比如上一时刻的关节角）
  • 始终检查解的可行性（关节限位、奇异位形）
  • 保留多组解，让上层规划器选择最优的一组

好了，这一章的内容就到这里。运动学基础是后面所有章节的基石，尤其是DH参数法和正逆运动学求解，在轨迹规划中会反复用到。建议你把上面的代码自己跑一遍，改改参数，看看末端位姿怎么变——动手做一遍，比看十遍都管用。

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