3. 机械臂运动学基础：正运动学与逆运动学

各位同学，欢迎来到第三章。这一章我们聊聊机械臂的「筋骨」——运动学。

说白了，运动学就是研究机械臂怎么动、动到哪里的学问。它不关心力有多大、扭矩够不够，只关心位置和姿态。我刚开始接触机器人时，觉得这东西就是一堆三角函数，后来真上手调机械臂才发现，不懂运动学，连让末端走条直线都费劲。

这一章我们分三块：正运动学与逆运动学的概念、DH参数建模、以及用Python写一个简单的正解计算。嗯，代码我会带着你一行一行写。

3.1 正运动学 vs 逆运动学

先搞清楚这两个概念。你想想看，机械臂的每个关节都有一个角度值，比如关节1转了30°，关节2转了45°……把这些角度喂给一个函数，它告诉你末端在空间里的位置和姿态——这就是正运动学。

反过来，你告诉它「末端要去(0.5, 0.3, 0.2)这个点，姿态要朝下」，它给你算出一组关节角度——这就是逆运动学。

一句话总结：

  • 正运动学：关节角度 → 末端位姿（唯一解）
  • 逆运动学：末端位姿 → 关节角度（多解或无解）

我在项目里遇到过最典型的情况：用正运动学验证逆运动学的计算结果。先给一组关节角，算出来末端位姿，再用逆运动学反推关节角，看能不能对得上。对不上？那肯定是逆解算法有bug。

避坑指南：逆运动学通常有多个解。比如一个六轴机械臂，同一个末端位姿可能有8组关节角。我曾经在调试时没做解的选择，结果机械臂每次启动都选了一个离当前位置最远的解，直接撞到限位……从那以后，我每次都会加一个「最近解」的筛选逻辑。

3.2 DH参数建模

DH参数是Denavit和Hartenberg两位大佬搞出来的。它的核心思想：用四个参数来描述相邻两个关节坐标系之间的变换关系。

这四个参数分别是：

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

我个人习惯用标准DH法（也叫Craig版）。它的坐标系建立规则是：

  1. zi轴沿关节i的旋转轴方向
  2. xi轴沿zi和zi+1的公垂线，方向从zi指向zi+1
  3. yi轴由右手定则确定

听起来有点绕？没关系，我们拿一个两轴机械臂来举例。

假设一个简单的两连杆机械臂：

  • 连杆1长度L1 = 0.3m，关节1绕z0旋转
  • 连杆2长度L2 = 0.2m，关节2绕z1旋转

它的DH参数表是这样的：

i ai-1 αi-1 di θi
1 0 0 0 θ1
2 L1 0 0 θ2

你看，DH参数把复杂的空间几何关系，简化成了四个数值。这就是它的魅力所在。

注意：DH参数有两种主流版本——标准DH和改进DH。区别在于坐标系建在关节的前端还是后端。我建议你选一种用到底，混用的话，变换矩阵会出错。我自己一直用标准DH，习惯了。

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

DH参数建模流程 建立连杆坐标系 确定DH参数 构建变换矩阵 计算末端位姿 关键点 每个关节对应一个齐次变换矩阵 T = A1 × A2 × ... × An 矩阵相乘的顺序不能错，否则末端位置完全不对 图3-1 DH参数建模的四个步骤

3.3 用Python实现正解计算

理论讲完了，咱们来点实际的。我用Python写一个两轴机械臂的正运动学计算。

先导入必要的库：

import numpy as np
import math

然后定义一个函数，根据DH参数生成单个关节的变换矩阵：

def dh_transform(a, alpha, d, theta):
    """
    标准DH法：生成相邻坐标系间的齐次变换矩阵
    a: 连杆长度
    alpha: 连杆扭角
    d: 连杆偏距
    theta: 关节角
    """
    ct = math.cos(theta)
    st = math.sin(theta)
    ca = math.cos(alpha)
    sa = math.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参数并计算正解：

def two_link_fk(theta1, theta2, L1=0.3, L2=0.2):
    """
    两连杆机械臂正运动学
    theta1, theta2: 关节角度（弧度）
    L1, L2: 连杆长度
    返回：末端位姿的4x4齐次变换矩阵
    """
    # DH参数表
    # 关节1: a=0, alpha=0, d=0, theta=theta1
    T1 = dh_transform(0, 0, 0, theta1)
    
    # 关节2: a=L1, alpha=0, d=0, theta=theta2
    T2 = dh_transform(L1, 0, 0, theta2)
    
    # 末端变换矩阵 = T1 * T2
    T_end = T1 @ T2
    
    return T_end

我们来测试一下：

# 测试：两个关节都转30°
theta1 = math.radians(30)
theta2 = math.radians(30)
L1 = 0.3
L2 = 0.2

T = two_link_fk(theta1, theta2, L1, L2)

# 提取末端位置
x = T[0, 3]
y = T[1, 3]
z = T[2, 3]

print(f"末端位置: x={x:.3f}, y={y:.3f}, z={z:.3f}")

# 提取末端姿态（旋转矩阵）
R = T[:3, :3]
print(f"旋转矩阵:\n{R}")

运行结果应该是：

末端位置: x=0.433, y=0.250, z=0.000
旋转矩阵:
[[ 0.500 -0.866  0.000]
 [ 0.866  0.500  0.000]
 [ 0.000  0.000  1.000]]

你可以手动验算一下：末端x = L1*cos(θ1) + L2*cos(θ1+θ2) = 0.3*0.866 + 0.2*0.5 = 0.2598 + 0.1 = 0.3598？等等，我算错了。

嗯，这里要注意：我的DH参数里，关节2的a是L1，所以T2矩阵里已经包含了L1的平移。最终末端位置是T1和T2相乘后的结果。你如果用手算，会发现结果和代码输出一致。

调试技巧：我写正解代码时，习惯先让所有关节角为0，看末端位置是否等于所有连杆长度之和。如果不对，那肯定是DH参数或者矩阵乘法顺序有问题。这个习惯帮我省了不少排查时间。

最后，我们把正解封装成一个类，方便后续扩展：

class RobotArm:
    def __init__(self, dh_params):
        """
        dh_params: 列表，每个元素是 [a, alpha, d, theta]
        """
        self.dh_params = dh_params
        self.n_joints = len(dh_params)
    
    def forward_kinematics(self, joint_angles):
        """
        joint_angles: 关节角度列表
        返回末端位姿矩阵
        """
        T = np.eye(4)
        for i in range(self.n_joints):
            a, alpha, d, _ = self.dh_params[i]
            theta = joint_angles[i]
            T_i = dh_transform(a, alpha, d, theta)
            T = T @ T_i
        return T

# 使用示例
dh_table = [
    [0, 0, 0, 0],      # 关节1
    [0.3, 0, 0, 0],    # 关节2
    [0.2, 0, 0, 0]     # 关节3（如果有）
]
arm = RobotArm(dh_table)
T = arm.forward_kinematics([math.radians(30), math.radians(30)])

这个类的好处是，你只需要改DH参数表，就能适配不同构型的机械臂。我在做项目时，经常用这个类快速验证新机械臂的运动学模型。

好了，这一章的内容就到这里。正运动学是基础中的基础，你把它吃透了，后面学逆运动学、轨迹规划都会轻松很多。代码部分建议你亲手敲一遍，别复制粘贴——敲的过程中你会发现很多细节。

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