机器人 / 系统课时

通过正运动学求末端位置姿态

通过正运动学求末端位置姿态

给定六轴机械臂的关节角,怎样计算工具中心点(TCP)在基座坐标系中的位置和姿态?本课使用一组六轴标准 DH 参数,依次连乘关节齐次变换矩阵,通过 Python 计算法兰与 TCP 位姿,再用相同的仿真关节角进行对照,验证运动学模型是否正确。

学习目标

  • 根据标准 DH 参数构造单关节齐次变换矩阵,理解六轴变换的连乘顺序。
  • 使用 Python 计算法兰和 TCP 的位置、旋转矩阵,并正确处理工具偏置。
  • 将计算结果与仿真机器人对照,根据位置和姿态偏差定位模型错误。

固定坐标约定和六轴模型参数

正运动学的输入是六个关节角及机器人几何参数,输出是末端在指定参考坐标系中的位姿。开始计算前,必须统一关节零位、旋转正方向、坐标轴定义和长度单位。

这里采用标准 DH 约定,而不是改进 DH 约定。每个关节使用四个参数:连杆长度 aia_i、连杆扭角 αi\alpha_i、关节偏距 did_i 和关节角 θi\theta_i。

为方便复现,使用一台教学用六轴旋转关节机械臂,参数如下。这些参数定义了本课的示例模型,不代表某个商业机器人型号。

关节 aia_i(m) did_i(m) αi\alpha_i(°) θi\theta_i(°)
J1 0.00 0.40 90 25
J2 0.35 0.00 0 -35
J3 0.25 0.00 0 55
J4 0.00 0.30 90 20
J5 0.00 0.00 -90 30
J6 0.00 0.10 0 -15

规定机器人基座坐标系 B 与 DH 坐标系 0 重合,坐标系 6 与工具法兰坐标系 F 重合。六个关节角均以表中数值作为本次测试姿态,没有额外关节零位偏置。

工具安装后,TCP 相对法兰沿法兰局部 Z 轴正方向偏移 0.12 m,不包含额外旋转。这个偏置必须在六个关节变换完成后再计算,不能直接加到基座 Z 坐标上。

从关节变换连乘到 TCP 位姿

对标准 DH 模型,第 ii 个关节的齐次变换按固定顺序构造:先绕当前关节 Z 轴旋转,再沿 Z 轴平移,然后沿 X 轴平移,最后绕 X 轴旋转。

对应公式为:

Ai=Rz(θi)Tz(di)Tx(ai)Rx(αi)A_i=R_z(\theta_i)T_z(d_i)T_x(a_i)R_x(\alpha_i)

每个 AiA_i 都是 4×44\times4 齐次变换矩阵。左上角的 3×33\times3 子矩阵表示旋转,第四列前三个元素表示平移,最后一行固定为 [0,0,0,1][0,0,0,1]。

从基座到 TCP 的完整变换为:

BTTCP=A1A2A3A4A5A6 6TTCP{}^{B}T_{TCP}=A_1A_2A_3A_4A_5A_6\,{}^{6}T_{TCP}

其中 A1A2A3A4A5A6A_1A_2A_3A_4A_5A_6 得到法兰位姿,最后一个矩阵表示法兰到 TCP 的固定工具变换。本例的工具变换只包含沿局部 Z 轴的 0.12 m 平移。

六轴机械臂关节坐标系与法兰TCP正运动学变换链

矩阵乘法不能随意交换顺序。例如先将工具相对法兰平移,再把这一偏置旋转到基座坐标系,和直接在基座坐标系平移 0.12 m 得到的结果通常不同。

最终位姿可拆成两个部分:位置向量 p=[x,y,z]Tp=[x,y,z]^T 表示 TCP 原点在基座系中的坐标;旋转矩阵 RR 的三列分别表示 TCP 局部 X、Y、Z 轴在基座坐标系中的方向。

使用 Python 计算法兰和 TCP 位姿

下面使用 Python 3.10 及以上版本的标准库 math,不依赖第三方软件包。代码直接实现标准 DH 矩阵和 4×44\times4 矩阵乘法,可用于检查仿真器或控制器提供的运动学结果。

将以下代码保存为 forward_kinematics.py 并运行。

python
import math


def identity():
    return [[float(i == j) for j in range(4)] for i in range(4)]


def multiply(left, right):
    # 齐次变换矩阵相乘,左右顺序不可交换
    return [[sum(left[i][k] * right[k][j] for k in range(4))
             for j in range(4)] for i in range(4)]


def dh(a, alpha_deg, d, theta_deg):
    # 标准 DH:输入角度单位为度,计算前转换为弧度
    alpha = math.radians(alpha_deg)
    theta = math.radians(theta_deg)
    ct, st = math.cos(theta), math.sin(theta)
    ca, sa = math.cos(alpha), math.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],
    ]


params = [
    (0.0, 90.0, 0.40),
    (0.35, 0.0, 0.0),
    (0.25, 0.0, 0.0),
    (0.0, 90.0, 0.30),
    (0.0, -90.0, 0.0),
    (0.0, 0.0, 0.10),
]
angles = [25.0, -35.0, 55.0, 20.0, 30.0, -15.0]

# 逐关节右乘,得到基座到法兰的变换
t_flange = identity()
for (a, alpha, d), theta in zip(params, angles):
    t_flange = multiply(t_flange, dh(a, alpha, d, theta))

# TCP 沿法兰局部 Z 轴正向偏移 120 mm
t_offset = identity()
t_offset[2][3] = 0.12
t_tcp = multiply(t_flange, t_offset)

print('法兰位置(m): (' + ', '.join(f'{t_flange[i][3]:.4f}' for i in range(3)) + ')')
print('TCP位置(m): (' + ', '.join(f'{t_tcp[i][3]:.4f}' for i in range(3)) + ')')
print('TCP旋转矩阵R:')
for row in t_tcp[:3]:
    print(' '.join(f'{value:.4f}' for value in row[:3]))

预期输出:

text
法兰位置(m): (0.6014, -0.1461, 0.2526)
TCP位置(m): (0.6037, -0.2597, 0.2140)
TCP旋转矩阵R:
0.9357 -0.3524 0.0189
-0.0966 -0.3071 -0.9468
0.3394 0.8840 -0.3214

输出统一采用米。可以用法兰和 TCP 的坐标差进行第一次人工检查:TCP 相对法兰的位移约为 (0.0023,−0.1136,−0.0386)(0.0023,-0.1136,-0.0386) m,其长度约为 0.12 m。

这个位移方向还应与旋转矩阵的第三列一致。第三列为 (0.0189,−0.9468,−0.3214)(0.0189,-0.9468,-0.3214),乘以 0.12 m 后,正好得到相同的工具偏置方向。它说明工具偏置已经正确旋转到基座坐标系。

使用仿真关节角验证计算结果

只有在仿真模型和计算模型采用相同的几何参数、关节零位、旋转方向以及工具定义时,两者的位姿结果才具有直接可比性。不要拿本课示例参数直接验证几何尺寸不同的现成商品机器人模型。

可以按以下顺序操作:

  1. 在支持自定义关节链的仿真器中建立六个旋转关节,按照参数表配置标准 DH 几何关系。若仿真器使用关节对象及固定装配变换,需要保证最终坐标关系与标准 DH 连乘等价。
  2. 将基座 B 设置为位置读取的参考坐标系。本次计算假定世界坐标系与基座坐标系重合;如果不重合,应先用世界到基座的坐标变换统一参考系。
  3. 将关节 J1 至 J6 分别设置为 25∘25^\circ、−35∘-35^\circ、55∘55^\circ、20∘20^\circ、30∘30^\circ、−15∘-15^\circ,确认角度正方向与计算模型相同。
  4. 在第六关节法兰坐标系下设置 TCP:局部位置为 (0,0,0.12)(0,0,0.12) m,局部姿态不旋转。确认仿真器显示的是工具坐标系原点,而不是法兰中心。
  5. 读取仿真器中 TCP 相对基座 B 的 XYZ 位置,以及局部坐标轴方向或旋转矩阵,与 Python 结果逐项比较。

对于这组教学参数,计算得到的 TCP 参考坐标约为 (0.6037,−0.2597,0.2140)(0.6037,-0.2597,0.2140) m。比较时应使用仿真器提供的完整精度数值,而不是仅靠画面中机械臂看起来是否到达同一位置。

位置偏差可以用欧氏距离计算:

ep=(xs−xc)2+(ys−yc)2+(zs−zc)2e_p=\sqrt{(x_s-x_c)^2+(y_s-y_c)^2+(z_s-z_c)^2}

其中下标 ss 表示仿真读数,cc 表示计算结果。对于几何参数完全相同、无外部测量误差的教学模型,可暂以位置误差不超过 1 mm、姿态角误差不超过 0.5∘0.5^\circ 作为练习检查阈值;理论上仅有浮点计算误差时,两者应更加接近。这不是工业机器人真实定位精度的验收标准。

姿态比较不要只看未经说明的欧拉角,因为不同软件可能采用不同旋转顺序。更可靠的做法是将双方统一为旋转矩阵,比较三个轴向量,或由两个旋转矩阵的相对旋转求出角度误差。

根据偏差特征定位错误

如果计算值与仿真读数不一致,可以先观察错误集中在位置、姿态还是工具偏置,再决定检查哪一部分。

偏差现象 优先检查 修正方法
法兰位置正确,TCP 位置错误 工具偏置与 TCP 定义 检查 0.12 m 是否沿法兰局部 Z 轴设置,避免毫米和米混用
位置与姿态均不一致 DH 约定、关节角、变换顺序 核对标准 DH 与改进 DH、正负角方向,以及矩阵右乘顺序
结果整体存在固定旋转或平移 基座与世界坐标系关系 统一参考坐标系,必要时补充世界到基座的固定变换
只有部分关节角姿态出错 零位偏置、关节轴方向 每次只改变一个关节 10°,检查末端变化方向是否一致

还可以进行一次增量验证:固定其余五个关节,只将 J1 从 25∘25^\circ 改为 35∘35^\circ,分别记录计算值和仿真值。如果两者在初始姿态吻合,但增量运动方向不同,通常说明关节轴方向、角度正负号或零位配置存在差异。

实际工位联调时,应分别保存关节角、法兰位姿、TCP 位姿、工具参数和基座参考系,不能只记录末端 XYZ 三个数字。这样才能区分运动学几何错误、工具标定错误和参考坐标系错误,并为后续逆运动学与轨迹规划提供可靠的位姿基准。

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

目录与阅读设置

阅读设置

17

记录一个想法

通过正运动学求末端位置姿态

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

登录哪都通