机器人 / 系统课时

连乘 DH 矩阵计算末端位姿

连乘 DH 矩阵计算末端位姿

给定每个关节的 DH 参数后,可以把相邻关节之间的运动写成齐次变换矩阵,再从基座沿关节链依次连乘,得到末端相对基座的位置与姿态。本篇用平面双旋转关节机械臂完成建表、计算和结果核验。

学习目标

  • 按标准 DH 约定填写四个参数及其单位。
  • 从参数表生成逐关节矩阵,并按正确顺序求末端变换。
  • 用 Python 与几何公式交叉检查位置和朝向。

统一 DH 约定并填写参数表

这里采用标准 DH(Classic DH),不与修改 DH(Modified DH)混用。使用列向量与右手坐标系:θi\theta_i 绕 zi−1z_{i-1} 旋转,did_i 沿 zi−1z_{i-1} 平移,aia_i 沿 xix_i 平移,αi\alpha_i 绕 xix_i 旋转。长度统一为米,表中角度为度。

以两根连杆长度分别为 1.0 m、0.8 m 的平面 2R 机械臂为例,关节 1 逆时针转 30°,关节 2 相对第一连杆顺时针转 60°:

关节 ii θi\theta_i(°) did_i(m) aia_i(m) αi\alpha_i(°)
1 30 0 1.0 0
2 -60 0 0.8 0

两个关节的转轴均垂直纸面,所以扭转角均为 0;末端连杆相对基座的绝对朝向是 30∘−60∘=−30∘30^\circ-60^\circ=-30^\circ,不能把第二关节的相对角直接当作末端朝向。

平面双关节机械臂的坐标架与 DH 变换连乘

从单关节参数构造矩阵

标准 DH 的固定运算顺序是绕 Z 旋转、沿 Z 平移、沿 X 平移、绕 X 旋转。记 ci=cos⁡θic_i=\cos\theta_i、si=sin⁡θis_i=\sin\theta_i、cα=cos⁡αic_\alpha=\cos\alpha_i、sα=sin⁡αis_\alpha=\sin\alpha_i,则相邻坐标系的齐次变换为:

Ai=i−1Ti=[ci−sicαsisαaicisicicα−cisαaisi0sαcαdi0001]A_i={}^{i-1}T_i= \begin{bmatrix} c_i & -s_i c_\alpha & s_i s_\alpha & a_i c_i\\ s_i & c_i c_\alpha & -c_i s_\alpha & a_i s_i\\ 0 & s_\alpha & c_\alpha & d_i\\ 0 & 0 & 0 & 1 \end{bmatrix}

矩阵左上角 3×33\times3 是旋转矩阵 RR,右上角三行是位置向量 pp,最后一行固定为 (0,0,0,1)(0,0,0,1)。对本例,从单位矩阵开始,沿基座到末端执行右侧连乘:

0T2=A1A2{}^0T_2=A_1A_2

顺序不能写成 A2A1A_2A_1,因为 A2A_2 的位移和旋转是相对坐标系 1 定义的,必须先经过 A1A_1 才能表达在基座坐标系 0 中。

运行 Python 求解末端位姿

下面的完整程序只使用 Python 标准库。把代码保存为 dh_fk.py,执行 python dh_fk.py。参数元组依次为角度、偏距、连杆长度和扭转角;三角函数使用弧度,因此入口统一转换。

python
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}")

预期输出:

text
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 结构,末端位置可以独立按连杆投影计算,便于发现矩阵顺序或角度单位问题:

x=1.0cos⁡30∘+0.8cos⁡(−30∘)≈1.5588x=1.0\cos30^\circ+0.8\cos(-30^\circ)\approx1.5588
y=1.0sin⁡30∘+0.8sin⁡(−30∘)=0.1000y=1.0\sin30^\circ+0.8\sin(-30^\circ)=0.1000

两式与齐次矩阵最后一列一致,末端绕 Z 轴的姿态角为 −30∘-30^\circ。还应检查旋转块满足 RTR=IR^TR=I、det⁡(R)=1\det(R)=1;若数值计算存在舍入误差,可用 10−910^{-9} 作为本例的检查容差。

排查时优先看三处:将角度 30 直接传入 sin 会被当成 30 弧度;把 A1A2A_1A_2 颠倒会改变基座系结果;把标准 DH 表直接交给修改 DH 的公式则会改变相邻坐标架含义。三维机械臂不能只报告 yaw,应保留完整 RR 和 pp。若工具法兰还有固定安装偏置,需要在关节链末尾继续右乘工具变换 2Ttool{}^2T_{\mathrm{tool}}。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

连乘 DH 矩阵计算末端位姿

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通