4. 正运动学求解：从D-H参数到末端位姿

正运动学，说白了就是：给定关节角度，算出末端执行器在哪、朝哪。这是机器人学里最基础、也最绕不开的一步。我个人习惯把正运动学看作“正向链条”——你给电机发角度指令，它就得告诉你手爪到底到了什么位置。

我记得刚入行时，带我的老工程师丢给我一本Craig的教材，说“把D-H表列出来，剩下的就是矩阵乘法”。当时觉得这有什么难的？结果第一次在真机上跑，末端位置差了5厘米……嗯，后来才发现是坐标系定义搞反了。今天咱们就把这块彻底捋清楚。

4.1 正运动学的基本思路

正运动学求解，本质上就是连乘齐次变换矩阵。每个关节对应一个4×4的矩阵，把从基座到末端的变换串起来：

你想想看，这个公式其实很直观。每个关节的运动（旋转或平移）加上连杆的几何尺寸，一层层传递下去。我在做六轴协作机器人项目时，就是靠这个公式一步步验证每个关节的运动范围。

4.2 标准D-H参数回顾

在开始推导之前，咱们快速过一下D-H参数的四个值。每个连杆 i 由四个参数描述：

参数	符号	物理含义	关节类型影响
连杆长度	a_{i-1}	沿x_{i-1}轴，从z_{i-1}到z_i的距离	固定值
连杆扭转角	α_{i-1}	绕x_{i-1}轴，从z_{i-1}到z_i的角度	固定值
连杆偏距	d_i	沿z_i轴，从x_{i-1}到x_i的距离	移动关节为变量
关节角	θ_i	绕z_i轴，从x_{i-1}到x_i的角度	旋转关节为变量

4.3 相邻连杆变换矩阵的推导

从连杆 i-1 到连杆 i 的变换，可以分解为四个基本步骤。我个人习惯按这个顺序记忆：

1. 绕 x_{i-1} 轴旋转 α_{i-1} —— 让 z_{i-1} 与 z_i 方向对齐
2. 沿 x_{i-1} 轴平移 a_{i-1} —— 让原点移到 z_i 轴上
3. 绕 z_i 轴旋转 θ_i —— 让 x_{i-1} 与 x_i 方向对齐
4. 沿 z_i 轴平移 d_i —— 让原点移到连杆 i 的坐标系原点

写成矩阵形式就是：

T_{i-1}^i = Rot(x, α_{i-1}) · Trans(x, a_{i-1}) · Rot(z, θ_i) · Trans(z, d_i)

展开后：
T_{i-1}^i = 
[ cosθ_i,   -sinθ_i,         0,         a_{i-1} ]
[ sinθ_i·cosα_{i-1},  cosθ_i·cosα_{i-1},  -sinα_{i-1},  -d_i·sinα_{i-1} ]
[ sinθ_i·sinα_{i-1},  cosθ_i·sinα_{i-1},   cosα_{i-1},   d_i·cosα_{i-1} ]
[ 0,         0,           0,          1       ]

这个矩阵看着复杂，但别怕。你把它当成一个固定模板，每次只需要往里填四个参数就行。我在写代码时，通常直接把这个矩阵封装成一个函数，传入 a, α, d, θ 四个值就返回4×4矩阵。

4.4 完整推导示例：两连杆平面臂

咱们用一个最简单的例子来走一遍流程。两连杆平面臂，两个旋转关节，都在同一平面内运动。

第一步：建立D-H参数表

连杆 i	a_{i-1}	α_{i-1}	d_i	θ_i
1	0	0	0	θ₁
2	L₁	0	0	θ₂

第二步：计算每个连杆的变换矩阵

对于连杆1（a=0, α=0, d=0, θ=θ₁）：

T_0^1 = 
[ cosθ₁,  -sinθ₁,  0,  0 ]
[ sinθ₁,   cosθ₁,  0,  0 ]
[ 0,       0,      1,  0 ]
[ 0,       0,      0,  1 ]

对于连杆2（a=L₁, α=0, d=0, θ=θ₂）：

T_1^2 = 
[ cosθ₂,  -sinθ₂,  0,  L₁ ]
[ sinθ₂,   cosθ₂,  0,  0  ]
[ 0,       0,      1,  0  ]
[ 0,       0,      0,  1  ]

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

T_0^2 = T_0^1 · T_1^2

结果：
T_0^2 = 
[ cos(θ₁+θ₂),  -sin(θ₁+θ₂),  0,  L₁·cosθ₁ + L₂·cos(θ₁+θ₂) ]
[ sin(θ₁+θ₂),   cos(θ₁+θ₂),  0,  L₁·sinθ₁ + L₂·sin(θ₁+θ₂) ]
[ 0,            0,           1,  0                         ]
[ 0,            0,           0,  1                         ]

看到没？末端位置就是 (L₁·cosθ₁ + L₂·cos(θ₁+θ₂), L₁·sinθ₁ + L₂·sin(θ₁+θ₂))。这个结果和咱们用几何法算出来的一模一样。矩阵乘法不会骗人，前提是你参数没填错。

4.5 MATLAB实现示例

我个人更喜欢用MATLAB做原型验证，矩阵运算写起来很顺手。下面是一个完整的正运动学求解函数：

function T = dh_transform(a, alpha, d, theta)
    % 标准D-H参数转齐次变换矩阵
    T = [cos(theta), -sin(theta)*cos(alpha),  sin(theta)*sin(alpha), a*cos(theta);
         sin(theta),  cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
         0,           sin(alpha),             cos(alpha),            d;
         0,           0,                      0,                     1];
end

% 两连杆平面臂正运动学
L1 = 1.0; L2 = 0.8;
theta1 = pi/4; theta2 = pi/6;

% D-H参数表
dh_params = [
    0,   0, 0, theta1;   % 连杆1
    L1,  0, 0, theta2    % 连杆2
];

% 计算末端位姿
T = eye(4);
for i = 1:size(dh_params, 1)
    a = dh_params(i, 1);
    alpha = dh_params(i, 2);
    d = dh_params(i, 3);
    theta = dh_params(i, 4);
    T = T * dh_transform(a, alpha, d, theta);
end

% 提取末端位置
x = T(1, 4);
y = T(2, 4);
z = T(3, 4);
fprintf('末端位置: (%.3f, %.3f, %.3f)\n', x, y, z);

4.6 Python实现示例

转到Python做算法部署时，我习惯用NumPy来替代MATLAB的矩阵运算。代码结构几乎一样：

import numpy as np

def dh_transform(a, alpha, d, theta):
    """标准D-H参数转齐次变换矩阵"""
    T = np.array([
        [np.cos(theta), -np.sin(theta)*np.cos(alpha), 
         np.sin(theta)*np.sin(alpha), a*np.cos(theta)],
        [np.sin(theta), np.cos(theta)*np.cos(alpha), 
         -np.cos(theta)*np.sin(alpha), a*np.sin(theta)],
        [0, np.sin(alpha), np.cos(alpha), d],
        [0, 0, 0, 1]
    ])
    return T

# 六轴机器人D-H参数示例（单位：米，弧度）
dh_params = [
    [0,     0,         0.4,   0],      # 关节1
    [0.3,   -np.pi/2,  0,     0],      # 关节2
    [0.25,  0,         0,     0],      # 关节3
    [0,     -np.pi/2,  0.3,   0],      # 关节4
    [0,      np.pi/2,  0,     0],      # 关节5
    [0,     -np.pi/2,  0.1,   0]       # 关节6
]

# 给定关节角度（弧度）
joint_angles = [0.2, -0.5, 0.8, 0.1, 0.3, 0.0]

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

# 提取位置和姿态
position = T[:3, 3]
rotation = T[:3, :3]

print(f"末端位置: {np.round(position, 4)}")
print(f"旋转矩阵:\n{np.round(rotation, 4)}")

4.7 正运动学知识体系

下面这张图把正运动学的核心流程串起来了。我每次给团队新人培训时，都会先画这张图——理解了它，后面逆运动学、轨迹规划都好说。

4.8 常见问题与调试技巧

正运动学代码写起来不难，但调试起来可能会让人抓狂。我总结了几条经验：

• 零位验证：把所有关节角设为零，末端位置应该在机器人的“零位姿态”上。如果不对，先检查D-H参数。
• 单关节运动：只动一个关节，看末端轨迹是不是绕着对应的轴转。比如关节1转动，末端应该画圆弧。
• 矩阵乘法顺序：一定是 T_0^1 * T_1^2 * ...，顺序反了结果完全不一样。
• 打印中间矩阵：我习惯在每个关节变换后打印一下T矩阵，看看数值是否合理。