第10章：正运动学求解——DH参数推导6轴机器人运动学方程

各位同学，今天我们来啃一块硬骨头——正运动学。说白了，就是给你6个关节角度，算出机器人末端到底在哪儿、朝哪个方向。这玩意儿是机器人控制的基石，你后面做轨迹规划、插补、避障，全得靠它。

我个人习惯，拿到一个机器人，第一件事就是建DH坐标系。为什么？因为DH参数是工业界通用的语言，你换台机器人，只要参数表一给，代码几乎不用改。我当年刚入行时，有个项目需要把ABB的机器人换成KUKA的，就是因为用了DH建模，两天就搞定了运动学移植。

10.1 DH参数建模——先搭骨架

先说说DH参数是哪四个：

  • θ（关节角）：绕Z轴旋转的角度
  • d（连杆偏距）：沿Z轴平移的距离
  • a（连杆长度）：沿X轴平移的距离
  • α（连杆扭角）：绕X轴旋转的角度

你想想看，这四个参数其实就描述了两个相邻坐标系之间的变换。从坐标系{i-1}到坐标系{i}，先绕Z轴转θ，再沿Z轴移d，然后沿X轴移a，最后绕X轴转α。就这么简单。

核心公式：相邻连杆变换矩阵

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

展开成4x4矩阵就是：

T_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      ]

嗯，这里要注意：不同版本的DH定义略有差异，我用的这种是标准DH（Standard DH）。还有一种叫改进DH（Modified DH），区别在于坐标系附着的位置不同。我个人建议初学者先死磕标准DH，搞透了再去看别的。

10.2 6轴机器人DH参数表——以典型工业机器人为例

我拿一台典型的6轴串联机器人来举例。这种机器人的结构大致是：

  • J1：腰部旋转（绕Z0）
  • J2：大臂旋转（绕Z1）
  • J3：小臂旋转（绕Z2）
  • J4：腕部旋转1（绕Z3）
  • J5：腕部旋转2（绕Z4）
  • J6：腕部旋转3（绕Z5）

对应的DH参数表如下（单位：mm和度）：

连杆 i θ_i (关节角) d_i (mm) a_i (mm) α_i (度)
1 θ1 d1 = 400 a1 = 150 α1 = -90
2 θ2 d2 = 0 a2 = 600 α2 = 0
3 θ3 d3 = 0 a3 = 200 α3 = 90
4 θ4 d4 = 600 a4 = 0 α4 = -90
5 θ5 d5 = 0 a5 = 0 α5 = 90
6 θ6 d6 = 100 a6 = 0 α6 = 0

避坑指南：我曾经在给一个协作机器人建模时，把α3的符号搞反了，结果仿真出来的末端位置完全不对。后来花了整整一个下午才排查出来。所以建议你建完表后，先拿一个简单的姿态（比如所有关节归零）手动算一下末端位置，验证一下。

10.3 正运动学方程推导——连乘就完事了

有了DH参数表，正运动学就是简单的矩阵连乘：

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

其中T_06就是机器人基坐标系到末端坐标系的变换矩阵。它包含了位置信息（最后一列的前三个元素）和姿态信息（左上角3x3旋转矩阵）。

写成数学形式：

T_06 = [R_06    P_06
        0  0  0  1   ]

这里R_06是3x3旋转矩阵，表示末端的朝向；P_06是3x1位置向量，表示末端在基坐标系下的坐标。

你想想看，这个矩阵里每个元素都是θ1到θ6的三角函数组合。实际展开后，表达式会非常长。我当年手算过一次，写了整整两页A4纸。现在嘛，都交给计算机了。

10.4 MATLAB验证——让代码说话

光说不练假把式。咱们用MATLAB来验证一下。我习惯用符号计算，这样能直观看到表达式。

% 正运动学求解 - MATLAB版本
clear; clc;

% 定义符号变量
syms theta1 theta2 theta3 theta4 theta5 theta6 real

% DH参数（单位：mm, rad）
d1 = 400; a1 = 150; alpha1 = -pi/2;
d2 = 0;   a2 = 600; alpha2 = 0;
d3 = 0;   a3 = 200; alpha3 = pi/2;
d4 = 600; a4 = 0;   alpha4 = -pi/2;
d5 = 0;   a5 = 0;   alpha5 = pi/2;
d6 = 100; a6 = 0;   alpha6 = 0;

% 构建DH变换矩阵函数
function T = dh_transform(theta, d, a, alpha)
    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

% 计算各连杆变换矩阵
T01 = dh_transform(theta1, d1, a1, alpha1);
T12 = dh_transform(theta2, d2, a2, alpha2);
T23 = dh_transform(theta3, d3, a3, alpha3);
T34 = dh_transform(theta4, d4, a4, alpha4);
T45 = dh_transform(theta5, d5, a5, alpha5);
T56 = dh_transform(theta6, d6, a6, alpha6);

% 正运动学连乘
T06 = T01 * T12 * T23 * T34 * T45 * T56;

% 简化表达式
T06_simplified = simplify(T06);

% 显示结果
disp('正运动学变换矩阵 T06:');
disp(T06_simplified);

% 数值验证：取一组关节角
theta_vals = [0, pi/4, -pi/4, 0, pi/6, 0];
T06_num = subs(T06, {theta1,theta2,theta3,theta4,theta5,theta6}, theta_vals);
T06_num = double(T06_num);
disp('数值验证结果：');
disp(T06_num);

% 提取位置和姿态
position = T06_num(1:3, 4);
rotation = T06_num(1:3, 1:3);
disp(['末端位置: x=', num2str(position(1)), ', y=', num2str(position(2)), ', z=', num2str(position(3))]);

运行结果示例：

当关节角为 [0, 45°, -45°, 0, 30°, 0] 时：

末端位置: x = 150.0, y = 424.3, z = 800.0

这个结果你可以手算验证一下，基本是对的。

10.5 Python验证——换个口味

有些同学可能更习惯Python。没问题，咱们也用Python撸一遍。我推荐用numpy和sympy，跟MATLAB思路一模一样。

# 正运动学求解 - Python版本
import numpy as np
import sympy as sp

# 定义符号变量
theta1, theta2, theta3, theta4, theta5, theta6 = sp.symbols('theta1:7')

# DH参数
dh_params = [
    (theta1, 400, 150, -np.pi/2),
    (theta2, 0,   600, 0),
    (theta3, 0,   200, np.pi/2),
    (theta4, 600, 0,   -np.pi/2),
    (theta5, 0,   0,   np.pi/2),
    (theta6, 100, 0,   0)
]

def dh_transform(theta, d, a, alpha):
    """构建DH变换矩阵"""
    return sp.Matrix([
        [sp.cos(theta), -sp.sin(theta)*sp.cos(alpha),  sp.sin(theta)*sp.sin(alpha), a*sp.cos(theta)],
        [sp.sin(theta),  sp.cos(theta)*sp.cos(alpha), -sp.cos(theta)*sp.sin(alpha), a*sp.sin(theta)],
        [0,              sp.sin(alpha),                sp.cos(alpha),               d],
        [0,              0,                            0,                           1]
    ])

# 计算各连杆变换矩阵
T = [dh_transform(*params) for params in dh_params]

# 正运动学连乘
T06 = sp.eye(4)
for i in range(6):
    T06 = T06 * T[i]

# 简化
T06_simplified = sp.simplify(T06)

print("正运动学变换矩阵 T06:")
sp.pprint(T06_simplified)

# 数值验证
theta_vals = {theta1:0, theta2:np.pi/4, theta3:-np.pi/4, 
              theta4:0, theta5:np.pi/6, theta6:0}
T06_num = T06_simplified.subs(theta_vals).evalf()
T06_num = np.array(T06_num).astype(np.float64)

print("\n数值验证结果:")
print(T06_num)

position = T06_num[:3, 3]
print(f"\n末端位置: x={position[0]:.1f}, y={position[1]:.1f}, z={position[2]:.1f}")

个人经验：我建议你在写代码时，把DH变换矩阵封装成一个函数。这样不仅代码整洁，而且万一要换DH定义（比如改成改进DH），只需要改这一个函数就行。我在实际项目中就是这么干的，后来客户要求换机器人型号，我改了参数表就完事了。

10.6 验证与调试——别让bug坑了你

代码写完了，怎么知道对不对？我分享几个我常用的验证方法：

  1. 零位验证：所有关节角为0时，末端应该在什么位置？根据你的机器人结构，应该能大致判断出来。
  2. 单关节运动：只动一个关节，看末端轨迹是不是一个圆弧。比如只动J1，末端应该在水平面画圆。
  3. 与已知结果对比：网上有很多标准机器人的DH参数和正解结果，拿来对比一下。
  4. 逆运动学反推：先正解算出一个位姿，再用逆解算回去，看关节角是否一致。

警告：千万不要以为代码跑通了就万事大吉。我曾经在一个项目中，正运动学算出来的位置看起来没问题，但姿态矩阵的符号反了。结果机器人一运动，末端工具直接撞上了工件。所以一定要做多组数值验证，特别是姿态部分。

10.7 小结——正运动学其实不难

说白了，正运动学就是四个步骤：

  • 建DH坐标系，确定参数表
  • 写出每个连杆的变换矩阵
  • 连乘得到末端位姿
  • 数值验证确保正确

你只要把这一步搞扎实了，后面学逆运动学、速度雅可比、动力学，都会轻松很多。我当年带团队时，要求每个新人都必须能手算一遍正运动学，再用代码验证。虽然过程痛苦，但基础打牢了，后面就顺了。

下一章咱们聊逆运动学，那才是真正考验数学功底的地方。做好准备！

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