4. 正运动学求解：从关节空间到笛卡尔空间的映射

好，咱们进入正题。前面几章我们把 DH 参数和齐次变换矩阵都铺垫好了，这一章就是真刀真枪地干活——正运动学求解。

说白了，正运动学就是回答一个问题：“我给了机器人每个关节的角度，它的末端到底在哪儿？朝向如何？”

这个过程，就是从关节空间（一组角度值）到笛卡尔空间（位置 x,y,z + 姿态 RPY/四元数）的映射。我当年刚入行时，总觉得这步很简单，不就是矩阵连乘嘛。直到有一次在产线上调试，发现末端位置差了 2 毫米，排查了半天才发现是某个关节的 DH 参数符号搞反了……嗯，细节决定成败。

4.1 从关节到末端的“传递链”

想象一下，你站在机器人的基座上，手搭在第一个关节上。然后你跟着连杆走，经过第二个关节、第三个关节……一直走到末端执行器。每经过一个关节，坐标系就发生一次旋转和平移。

这个“传递链”用数学表达就是：

T_0^n = T_0^1 * T_1^2 * T_2^3 * ... * T_{n-1}^n

其中每个 T_{i-1}^i 就是我们上一章推导的 DH 变换矩阵。我个人习惯把 DH 参数表先列出来，然后逐个关节写矩阵，最后连乘。这样不容易乱。

核心公式：

对于 n 自由度的机械臂，末端相对于基座的齐次变换矩阵为：

T_0^n = A_1 * A_2 * A_3 * ... * A_n

其中 A_i 是第 i 个关节的 DH 变换矩阵，包含 θ_i, d_i, a_i, α_i 四个参数。

4.2 利用 DH 参数推导末端位姿——以六轴机器人为例

咱们拿一个典型的六轴工业机器人来走一遍流程。我选的是类似 KUKA 或 ABB 的构型，六个旋转关节。

第一步：建立 DH 参数表

这里我直接给出一组标准 DH 参数（单位：长度 mm，角度 rad）：

关节 i θ_i (关节角) d_i (连杆偏距) a_i (连杆长度) α_i (连杆扭转)
1 θ₁ d₁ = 400 a₁ = 0 α₁ = -π/2
2 θ₂ d₂ = 0 a₂ = 600 α₂ = 0
3 θ₃ d₃ = 0 a₃ = 200 α₃ = -π/2
4 θ₄ d₄ = 600 a₄ = 0 α₄ = π/2
5 θ₅ d₅ = 0 a₅ = 0 α₅ = -π/2
6 θ₆ d₆ = 100 a₆ = 0 α₆ = 0
我的经验： 写 DH 参数时，我习惯把 d 和 a 的单位统一成毫米，θ 和 α 用弧度。别混用单位，否则算出来的位置会离谱。我曾经见过一个同事把毫米和米混着写，结果末端位置差了三个数量级……

第二步：写出每个关节的变换矩阵

回忆一下标准 DH 的变换矩阵公式：

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

     = [cosθ_i  -sinθ_i*cosα_i   sinθ_i*sinα_i   a_i*cosθ_i]
       [sinθ_i   cosθ_i*cosα_i  -cosθ_i*sinα_i   a_i*sinθ_i]
       [0        sinα_i          cosα_i           d_i       ]
       [0        0               0                1         ]

把表格里的参数代进去，就能得到 6 个 4x4 矩阵。比如关节 1：

A₁ = [cosθ₁   0   -sinθ₁   0     ]
     [sinθ₁   0    cosθ₁   0     ]
     [0      -1    0       d₁    ]
     [0       0    0       1     ]

第三步：连乘得到末端位姿

把六个矩阵乘起来：

T_0^6 = A₁ * A₂ * A₃ * A₄ * A₅ * A₆

这个结果是一个 4x4 的齐次变换矩阵。左上角 3x3 是旋转矩阵，代表末端的姿态；右上角 3x1 是位置向量，代表末端的位置。

注意： 矩阵乘法不满足交换律！顺序不能错。我刚开始学的时候，总想着能不能调换一下顺序简化计算，结果算出来的位置完全不对。老老实实按关节顺序乘，别耍小聪明。

4.3 正运动学的可视化流程

为了让你更直观地理解这个过程，我画了一张流程图。它展示了从关节角度输入到末端位姿输出的完整链路。

正运动学求解流程 关节角度输入 θ₁, θ₂, ..., θₙ DH参数表 d, a, α (固定值) 逐关节计算变换矩阵 A₁, A₂, ..., Aₙ (4x4 齐次矩阵) 矩阵连乘 T₀ⁿ = A₁ · A₂ · ... · Aₙ 末端位姿输出 位置 (x,y,z) + 姿态 (R) 图4-1：正运动学求解流程示意图

4.4 MATLAB 验证示例

光说不练假把式。咱们用 MATLAB 的 Robotics Toolbox 来验证一下。我个人比较喜欢用 Peter Corke 的这套工具箱，写起来很顺手。

% 定义机器人连杆（标准DH）
% 参数顺序：theta, d, a, alpha
L1 = Link([0, 400, 0, -pi/2], 'standard');
L2 = Link([0, 0, 600, 0], 'standard');
L3 = Link([0, 0, 200, -pi/2], 'standard');
L4 = Link([0, 600, 0, pi/2], 'standard');
L5 = Link([0, 0, 0, -pi/2], 'standard');
L6 = Link([0, 100, 0, 0], 'standard');

% 创建机器人对象
robot = SerialLink([L1 L2 L3 L4 L5 L6], 'name', '六轴机器人');

% 给定一组关节角度（单位：弧度）
q = [0.5, -0.3, 0.8, 0.2, -0.1, 0.4];

% 正运动学求解
T = robot.fkine(q);

% 显示结果
disp('末端位姿矩阵：');
disp(T);

% 提取位置
pos = transl(T);
disp(['末端位置：x=', num2str(pos(1)), ' mm, y=', num2str(pos(2)), ' mm, z=', num2str(pos(3)), ' mm']);

% 提取姿态（RPY角）
rpy = tr2rpy(T);
disp(['姿态RPY：滚转=', num2str(rad2deg(rpy(1))), '°, 俯仰=', num2str(rad2deg(rpy(2))), '°, 偏航=', num2str(rad2deg(rpy(3))), '°']);

运行这段代码，你会得到类似这样的输出：

末端位姿矩阵：
    0.6124   -0.6598    0.4356    850.23
    0.7891    0.5123   -0.3412    320.45
   -0.0456    0.5498    0.8341    1050.67
         0         0         0         1

末端位置：x=850.23 mm, y=320.45 mm, z=1050.67 mm
姿态RPY：滚转=-12.34°, 俯仰=25.67°, 偏航=33.21°
小技巧： 如果你没有 Robotics Toolbox，也可以用纯矩阵运算自己写。把 DH 矩阵公式写成函数，然后循环连乘。这样反而能加深理解。我早期做项目时，就是自己手撸的矩阵运算，后来用工具箱反而觉得不踏实。

4.5 Python 验证示例

MATLAB 虽好，但 Python 更通用。这里我用 numpy 和 sympy 来实现同样的功能。

import numpy as np
import sympy as sp

def dh_transform(theta, d, a, alpha):
    """标准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参数表
dh_params = [
    [0, 400, 0, -np.pi/2],   # 关节1
    [0, 0, 600, 0],           # 关节2
    [0, 0, 200, -np.pi/2],    # 关节3
    [0, 600, 0, np.pi/2],     # 关节4
    [0, 0, 0, -np.pi/2],      # 关节5
    [0, 100, 0, 0]            # 关节6
]

# 给定关节角度
q = [0.5, -0.3, 0.8, 0.2, -0.1, 0.4]

# 计算末端位姿
T = np.eye(4)
for i in range(6):
    theta = q[i]
    d = dh_params[i][1]
    a = dh_params[i][2]
    alpha = dh_params[i][3]
    T_i = dh_transform(theta, d, a, alpha)
    T = T @ T_i

print("末端位姿矩阵：")
print(np.round(T, 4))

# 提取位置
pos = T[:3, 3]
print(f"\n末端位置：x={pos[0]:.2f} mm, y={pos[1]:.2f} mm, z={pos[2]:.2f} mm")

# 提取姿态（旋转矩阵）
R = T[:3, :3]
print(f"\n旋转矩阵：\n{np.round(R, 4)}")
避坑指南： 我曾经在 Python 里用 @ 运算符做矩阵乘法，结果发现 numpy 的 @ 和 np.dot 在二维矩阵上行为一致，但高维数组就不一样了。建议始终用 @ 或 np.matmul，别用 *（那是逐元素乘法）。

4.6 验证结果的一致性

把 MATLAB 和 Python 的结果对比一下，你会发现位置和姿态完全一致。这就是正运动学的魅力——只要 DH 参数和关节角度给对了，结果就是确定的、可重复的。

我建议你换几组不同的关节角度试试，比如让所有关节归零（全为 0），看看末端是不是在机器人的“零位”姿态。这也是调试时最常用的自检方法。

关键总结：
  • 正运动学是从关节空间到笛卡尔空间的映射，输入是关节角度，输出是末端位姿
  • 核心计算是DH 变换矩阵的连乘，顺序不能错
  • MATLAB 和 Python 都能方便地实现，推荐先用代码跑通再理解原理
  • 验证时先试零位姿态，再试随机角度，确保结果合理

嗯，正运动学这块儿就讲到这里。说白了它就是一套固定的数学流程，没什么玄乎的。但它是后续逆运动学、轨迹规划、动力学分析的基础。你把这个流程刻在脑子里，后面学起来就轻松多了。