学习笔记：UR5协作机器人正运动学计算
实战练习：求UR5机器人末端坐标
UR5 机器人 DH 参数表
UR5 是工业界最经典的 6 自由度协作机器人。以下是其标准 DH 参数（注意：不同厂商或文献可能使用改进型 DH，即 MDH，这里展示标准 DH 以符合本文档 2 的定义）。
单位： d,a (毫米 mm), α,θ (弧度 rad)
| 关节 Joint (i) | αi (Twist) | ai (Link Length) | di (Link Offset) | θi (Range) |
|---|---|---|---|---|
| 1 (Base) | π/2 | 0 | 89.16 | q1 |
| 2 () | 0 | -425.00 | 0 | q2 |
| 3 (Elbow) | 0 | -392.25 | 0 | q3 |
| 4 (Wrist 1) | π/2 | 0 | 109.15 | q4 |
| 5 (Wrist 2) | −π/2 | 0 | 94.65 | q5 |
| 6 (Wrist 3) | 0 | 0 | 82.30 | q6 |
练习题：计算 UR5 处于零位 (q=[0,0,0,0,0,0]) 时的末端坐标。
正运动学简介
正运动学 (Forward Kinematics) 解决的问题是：已知所有关节角度，计算机器人末端执行器的位姿。
从"关节空间"到"任务空间"
plain
┌─────────────┐ 正运动学 ┌─────────────┐
│ 关节空间 │ ─────────────────────→ │ 任务空间 │
│ (Joint │ │ (Cartesian │
│ Space) │ │ Space) │
└─────────────┘ └─────────────┘
• 关节1角度 θ₁ • 末端X坐标
• 关节2角度 θ₂ • 末端Y坐标
• 关节3角度 θ₃ • 末端Z坐标
• 关节4角度 θ₄ • 末端朝向（姿态）
... ...- 每个关节建立一个坐标系，机械臂是"一串坐标系"，串联结构。
- 坐标系之间用齐次变换矩阵连接。
- 计算末端位姿在哪里，就相当于从基座到末端，一路"转发"坐标
Denavit-Hartenberg (DH) 参数法是建立连杆坐标系的标准方法。
DH 参数详解 (标准型)
描述相邻两个连杆 i−1 和 i 之间的关系，仅需 4 个参数：
| 参数 | 符号 | 物理含义 | 测量基准 |
|---|---|---|---|
| 连杆长度 | ai | 沿 xi 轴，从 zi−1 到 zi 的距离 | 连杆本身结构 |
| 连杆扭角 | αi | 绕 xi 轴，从 zi−1 旋转到 zi 的角度 | 连杆本身结构 |
| 关节偏距 | di | 沿 zi−1 轴，从 xi−1 到 xi 的距离 | 关节变量 (移动副) |
| 关节角度 | θi | 绕 zi−1 轴，从 xi−1 旋转到 xi 的角度 | 关节变量 (旋转副) |
相邻连杆的变换矩阵公式为：
正运动学的计算：两步走
第一步：建立DH参数表
第二步：连乘齐次变换矩阵
每个关节对应一个变换矩阵：
plain
[ cosθ -sinθcosα sinθsinα a·cosθ ]
i-1Ti = [ sinθ cosθcosα -cosθsinα a·sinθ ]
[ 0 sinα cosα d ]
[ 0 0 0 1 ]正运动学公式：
结果：4×4矩阵，包含末端的位置（右上3×1）和姿态（左上3×3）。
计算UR5协作机器人末端位姿
由UR5 机器人 DH 参数表可知
| 列名 | 符号 | 在 UR5 中的体现 |
|---|---|---|
| αi (Twist) | 连杆扭角 | 只有第1、4、5轴有扭转（π/2 或 -π/2），其他为0。决定 6 自由度 |
| ai (Link Length) | 连杆长度 | 第2、3轴有长度（-425mm, -392.25mm）。决定工作半径（约 800mm 臂长） |
| di (Link Offset) | 连杆偏置 | 第1、4、5、6轴有偏置。6轴只是工具法兰的偏移 |
| θi (Range) | 关节角度 | 就是关节变量 q₁~q₆ |
由正运动学公式，我们需要先得到每个关节的齐次变换矩阵，最后进行连乘即可得到末端位姿。
以 Joint 2 (Shoulder) 为例，写出变换矩阵：
将表Shoulder的参数代入，
- α₂ = 0, a₂ = -0.425, d₂ = 0, θ₂ = q₂
plain
1T2 = [ cosq₂ -sinq₂·1 sinq₂·0 -0.425·cosq₂ ]
[ sinq₂ cosq₂·1 -cosq₂·0 -0.425·sinq₂ ]
[ 0 0 1 0 ]
[ 0 0 0 1 ]
简化后：
1T2 = [ cosq₂ -sinq₂ 0 -0.425·cosq₂ ]
[ sinq₂ cosq₂ 0 -0.425·sinq₂ ]
[ 0 0 1 0 ]
[ 0 0 0 1 ]以下使用python代码进行计算
python
import numpy as np
def dh_matrix(theta, d, a, alpha):
"""
根据标准DH参数计算变换矩阵 T_{i-1, i}
单位: 角度使用弧度, 长度使用米
"""
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
return np.array([
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
def forward_kinematics_UR5(joint_angles):
"""
计算UR5机械臂的末端位置
"""
theta1, theta2, theta3, theta4, theta5, theta6 = joint_angles
a1, a2, a3, a4, a5, a6 = 0, -0.425, -0.39225, 0, 0, 0
d1, d2, d3, d4, d5, d6 = 0.089159, 0, 0, 0.10915, 0.09465, 0.0823
alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = np.pi/2, 0, 0, np.pi/2, -np.pi/2, 0
# 连杆 1: frame 0 -> frame 1
# theta=theta1, d=d1, a=a1, alpha=alpha1
T_01 = dh_matrix(theta1, d1, a1, alpha1)
# 连杆 2: frame 1 -> frame 2
T_12 = dh_matrix(theta2, d2, a2, alpha2)
# 连杆 3: frame 2 -> frame 3
T_23 = dh_matrix(theta3, d3, a3, alpha3)
# 连杆 4: frame 3 -> frame 4
T_34 = dh_matrix(theta4, d4, a4, alpha4)
# 连杆 5: frame 4 -> frame 5
T_45 = dh_matrix(theta5, d5, a5, alpha5)
# 连杆 6: frame 5 -> frame 6
T_56 = dh_matrix(theta6, d6, a6, alpha6)
# 总变换矩阵
T_06 = T_01 @ T_12 @ T_23 @ T_34 @ T_45 @ T_56
return T_06
# --- 验证 ---
q_case = [0, 0, 0, 0, 0, 0] # 全零关节角
T_final = forward_kinematics_UR5(q_case)
pos_final = T_final[:3, 3]
print(f"输入关节角: {np.degrees(q_case)}")
print(f"计算出的末端位置 (x, y, z): {np.round(pos_final, 4)}")运行后得到的结果为：
shell
输入关节角: [0. 0. 0. 0. 0. 0.]
计算出的末端位置 (x, y, z): [-0.8172 -0.1914 -0.0055]