第二节：机器人运动学基础——齐次变换矩阵、DH参数法、正运动学建模

各位工程师朋友，大家好。今天我们来聊聊机器人运动学里最基础、也最绕不开的三个东西：齐次变换矩阵、DH参数法，还有正运动学建模。

说实话，我刚入行那会儿，觉得这些就是一堆矩阵乘法，枯燥得很。直到有一次，我在现场调试一台六轴机器人，发现末端位置怎么都对不上理论值。折腾了两天，最后发现是某个关节的DH参数写反了。嗯，从那以后，我再也不敢小看这些“基础”了。

1. 齐次变换矩阵：机器人的“坐标翻译官”

先说说齐次变换矩阵。你想想看，机器人身上有那么多关节，每个关节都有自己的坐标系。我们要知道末端在哪儿，就得在这些坐标系之间来回“翻译”。

齐次变换矩阵，说白了就是个4x4的矩阵，它能把一个点从一个坐标系搬到另一个坐标系去。它的样子是这样的：

| R(3x3)   T(3x1) |
| 0 0 0     1     |

左上角3x3是旋转矩阵，描述姿态；右上角3x1是平移向量，描述位置。最后一行固定是[0 0 0 1]，这是为了保持矩阵乘法的统一性。

我个人习惯：在写代码时，我会把齐次变换矩阵定义成一个结构体，包含旋转矩阵和平移向量。这样后续做乘法、求逆都方便。

举个例子，假设坐标系{A}到坐标系{B}的变换是T_AB，那么一个在{B}中的点P_B，转换到{A}中就是：

P_A = T_AB * P_B

这里P_B是齐次坐标，也就是在普通三维坐标后面加个1，变成[x y z 1]^T。

避坑指南：我曾经在项目里直接用3x3旋转矩阵加平移向量做变换，结果发现连续变换时容易出错。后来统一用4x4齐次矩阵，问题就解决了。建议你也这么做。

2. DH参数法：给机器人关节“上户口”

有了齐次变换矩阵，我们还得知道每个关节之间的变换关系具体怎么算。这时候，DH参数法就登场了。

DH参数法，全称是Denavit-Hartenberg参数法。它用四个参数来描述相邻两个关节坐标系之间的关系：

a：连杆长度，也就是两个关节轴之间的公垂线长度
α：连杆扭转角，两个关节轴之间的夹角
d：连杆偏距，沿关节轴方向的偏移
θ：关节角，绕关节轴旋转的角度

这四个参数，就能唯一确定相邻两个坐标系之间的齐次变换矩阵。公式是这样的：

T = Rot_z(θ) * Trans_z(d) * Trans_x(a) * Rot_x(α)

展开来写就是：

| cosθ   -sinθ cosα    sinθ sinα    a cosθ |
| sinθ    cosθ cosα   -cosθ sinα    a sinθ |
|   0       sinα         cosα         d    |
|   0        0            0           1    |

我在项目中遇到过：有一次标定一台协作机器人，发现它的DH参数里α角是负的。一开始我以为是数据错了，后来查了手册才知道，有些厂家习惯用不同的符号约定。所以，拿到DH参数表，先确认一下符号规则，别直接套公式。

DH参数法有两种常见形式：标准DH和改进DH。区别在于坐标系建立的位置不同。标准DH把坐标系建在连杆的末端，改进DH建在连杆的起始端。我个人更习惯用改进DH，因为它在处理树形结构时更直观。

类型	坐标系位置	适用场景
标准DH	连杆末端	串联机器人
改进DH	连杆起始端	串联、并联、树形结构

3. 正运动学建模：从关节角到末端位姿

有了DH参数和齐次变换矩阵，正运动学建模就水到渠成了。

正运动学，说白了就是：给你六个关节的角度，算出末端执行器在空间中的位置和姿态。这个过程就是一连串的矩阵乘法。

对于六轴机器人，从基座到末端，一共有6个关节变换：

T_06 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56

其中T_ij表示从关节i到关节j的齐次变换矩阵。每个T_ij都由对应的DH参数计算得到。

最终结果T_06是一个4x4矩阵，左上角3x3是末端姿态，右上角3x1是末端位置。

我的经验：写正运动学代码时，建议先用一个简单的2自由度机器人验证。比如只动关节1和关节2，看末端位置变化是否符合直觉。这样能快速发现DH参数或矩阵乘法中的错误。

下面是一个简单的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

# 示例：六轴机器人DH参数表（单位：mm, rad）
dh_params = [
    [0,    np.pi/2, 0.3,   0],      # 关节1
    [0.4, 0,       0,     -np.pi/2], # 关节2
    [0,    np.pi/2, 0,     0],       # 关节3
    [0,   -np.pi/2, 0.3,   0],       # 关节4
    [0,    np.pi/2, 0,     0],       # 关节5
    [0,    0,       0.1,   0]        # 关节6
]

# 关节角度（单位：rad）
joint_angles = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]

# 计算正运动学
T = np.eye(4)
for i, (a, alpha, d, theta) in enumerate(dh_params):
    T_i = dh_transform(a, alpha, d, theta + joint_angles[i])
    T = T @ T_i

print("末端位姿矩阵：")
print(T)
print("末端位置：", T[:3, 3])
print("末端姿态（旋转矩阵）：")
print(T[:3, :3])

注意：上面的代码里，我把关节角加到了DH参数的θ上。这是因为DH参数里的θ是关节的初始偏置，实际关节角是在这个基础上变化的。如果你拿到的DH参数表已经包含了初始偏置，那就直接代入θ值即可。

知识体系总览

为了让你更直观地理解这三者之间的关系，我画了一张图：

从这张图你可以看到，整个正运动学建模的流程非常清晰：先确定每个关节的DH参数，然后用这些参数生成对应的齐次变换矩阵，最后把这些矩阵按顺序乘起来，就得到了末端位姿。

嗯，到这里，运动学基础这部分就讲完了。这些内容看似简单，但它们是后续做标定、做轨迹规划、做动力学分析的基础。我建议你动手写写代码，把上面的示例跑一遍，再换一组DH参数试试。实践出真知嘛。

最后说一句：标定工作里，正运动学模型是“理论值”的来源。如果你的模型不准，后面再怎么标定也是白搭。所以，花时间把这块吃透，值。

可可精品资料库

📚 课程目录

第二节：机器人运动学基础——齐次变换矩阵、DH参数法、正运动学建模

1. 齐次变换矩阵：机器人的“坐标翻译官”

2. DH参数法：给机器人关节“上户口”

3. 正运动学建模：从关节角到末端位姿

知识体系总览