机器人 / 系统课时
连乘 DH 矩阵计算末端位姿
连乘 DH 矩阵计算末端位姿
给定每个关节的 DH 参数后,可以把相邻关节之间的运动写成齐次变换矩阵,再从基座沿关节链依次连乘,得到末端相对基座的位置与姿态。本篇用平面双旋转关节机械臂完成建表、计算和结果核验。
学习目标
- 按标准 DH 约定填写四个参数及其单位。
- 从参数表生成逐关节矩阵,并按正确顺序求末端变换。
- 用 Python 与几何公式交叉检查位置和朝向。
统一 DH 约定并填写参数表
这里采用标准 DH(Classic DH),不与修改 DH(Modified DH)混用。使用列向量与右手坐标系: 绕 旋转, 沿 平移, 沿 平移, 绕 旋转。长度统一为米,表中角度为度。
以两根连杆长度分别为 1.0 m、0.8 m 的平面 2R 机械臂为例,关节 1 逆时针转 30°,关节 2 相对第一连杆顺时针转 60°:
| 关节 | (°) | (m) | (m) | (°) |
|---|---|---|---|---|
| 1 | 30 | 0 | 1.0 | 0 |
| 2 | -60 | 0 | 0.8 | 0 |
两个关节的转轴均垂直纸面,所以扭转角均为 0;末端连杆相对基座的绝对朝向是 ,不能把第二关节的相对角直接当作末端朝向。

从单关节参数构造矩阵
标准 DH 的固定运算顺序是绕 Z 旋转、沿 Z 平移、沿 X 平移、绕 X 旋转。记 、、、,则相邻坐标系的齐次变换为:
矩阵左上角 是旋转矩阵 ,右上角三行是位置向量 ,最后一行固定为 。对本例,从单位矩阵开始,沿基座到末端执行右侧连乘:
顺序不能写成 ,因为 的位移和旋转是相对坐标系 1 定义的,必须先经过 才能表达在基座坐标系 0 中。
运行 Python 求解末端位姿
下面的完整程序只使用 Python 标准库。把代码保存为 dh_fk.py,执行 python dh_fk.py。参数元组依次为角度、偏距、连杆长度和扭转角;三角函数使用弧度,因此入口统一转换。
from math import cos, sin, radians, atan2, degrees
def multiply(left, right):
return [
[sum(left[r][k] * right[k][c] for k in range(4))
for c in range(4)]
for r in range(4)
]
def dh(theta_deg, d, a, alpha_deg):
# 标准 DH:角度先转弧度,四种变换顺序固定。
theta, alpha = radians(theta_deg), radians(alpha_deg)
ct, st = cos(theta), sin(theta)
ca, sa = cos(alpha), sin(alpha)
return [
[ct, -st * ca, st * sa, a * ct],
[st, ct * ca, -ct * sa, a * st],
[0.0, sa, ca, d],
[0.0, 0.0, 0.0, 1.0],
]
parameters = [(30, 0.0, 1.0, 0), (-60, 0.0, 0.8, 0)]
T = [[float(r == c) for c in range(4)] for r in range(4)]
# 按关节编号逐次在右侧乘入变换矩阵。
for row in parameters:
T = multiply(T, dh(*row))
print("T_0_2:")
for line in T:
print(" ".join(f"{value:7.4f}" for value in line))
x, y, z = (T[r][3] for r in range(3))
# 仅对本例平面运动,从旋转块读取绕 Z 轴朝向。
yaw = degrees(atan2(T[1][0], T[0][0]))
print(f"position(m): ({x:.4f}, {y:.4f}, {z:.4f})")
print(f"yaw(deg): {yaw:.1f}")预期输出:
T_0_2:
0.8660 0.5000 0.0000 1.5588
-0.5000 0.8660 0.0000 0.1000
0.0000 0.0000 1.0000 0.0000
0.0000 0.0000 0.0000 1.0000
position(m): (1.5588, 0.1000, 0.0000)
yaw(deg): -30.0核验几何结果与排除典型错误
对于平面 2R 结构,末端位置可以独立按连杆投影计算,便于发现矩阵顺序或角度单位问题:
两式与齐次矩阵最后一列一致,末端绕 Z 轴的姿态角为 。还应检查旋转块满足 、;若数值计算存在舍入误差,可用 作为本例的检查容差。
排查时优先看三处:将角度 30 直接传入 sin 会被当成 30 弧度;把 颠倒会改变基座系结果;把标准 DH 表直接交给修改 DH 的公式则会改变相邻坐标架含义。三维机械臂不能只报告 yaw,应保留完整 和 。若工具法兰还有固定安装偏置,需要在关节链末尾继续右乘工具变换 。