机器人 / 系统课时
用齐次矩阵计算位姿坐标变换
用齐次矩阵计算位姿坐标变换
基座、法兰和工具中心点(TCP)的坐标常常来自不同参考系,直接相加会把抓取点算错。这里固定一个绕 Z 轴旋转 90° 的法兰姿态,逐步求工具在基座系中的位姿、点坐标及逆变换,并用程序复核结果。
学习目标
- 读懂 的转换方向,区分旋转、平移与点的齐次坐标。
- 正确组合基座、法兰和工具的变换,算出 TCP 与工具点的基座坐标。
- 利用刚体变换求逆及数值回代定位方向、顺序和单位错误。
明确变换方向和坐标约定
设 B 为机器人基座坐标系,F 为法兰坐标系,T 为工具坐标系,其原点是标定的 TCP。采用右手坐标系、列向量、长度单位米,正转角按右手定则。记号 表示将 F 系的坐标换算成 B 系,并不是把 B 系坐标变到 F 系。
对点使用齐次向量 ,这样一个矩阵就能同时执行旋转和平移:
左上 是旋转矩阵 ,最右列前三项是 B 原点在 A 系中的位置 ;变换后的点满足 。如果换算的是速度方向而非空间点,齐次末项设为 0,平移不生效。
写出法兰与工具的位姿
假定法兰 F 相对基座 B 绕 旋转 ,法兰原点位于 B 系 m:
工具坐标系 T 与法兰 F 方向平行,仅沿 偏置 0.15 m;这个方向由工具安装定义,本例用它突出偏置必须随法兰旋转,并不表示所有夹具都沿法兰 X 轴伸出。
组合矩阵求 TCP 和目标点
按坐标链相邻字母匹配的方向相乘,先将 T 系换到 F 系,再将 F 系换到 B 系,矩阵乘法从右向左执行:
用等价的平移公式 可检查:工具偏置 在基座系中变为 ,因此 TCP 为 m。若直接算成 m,就是把法兰局部偏置误当作基座方向。
现在取工具系中的一个点 ,计算得 。工具上的 0.10 m 沿 ,对应基座中的 ;点的位置与 TCP 不相同,不要把点坐标变换误当成 TCP 位姿。

逆变换与往返验证
工位联调时还需要把 B 系中的目标点换回工具系。刚体变换满足 ,但逆变换平移不是简单取负,必须计算 。本例的逆矩阵为:
应用到 ,重新得到 。旋转矩阵还应满足 、;否则不能把含缩放或剪切的矩阵按刚体变换求逆。
用 NumPy 独立复算
在已安装 NumPy 的 Python 3 环境中运行以下完整示例;矩阵按行填写,点采用列向量,@ 表示矩阵乘法。代码用转置而非通用矩阵求逆计算反向变换:
import numpy as np
# B<-F:法兰绕基座 Z 轴正向旋转 90°
T_BF = np.eye(4)
T_BF[:3, :3] = [[0, -1, 0], [1, 0, 0], [0, 0, 1]]
T_BF[:3, 3] = [0.60, 0.20, 0.40]
# F<-T:TCP 沿法兰局部 +X 偏置 0.15 m
T_FT = np.eye(4)
T_FT[0, 3] = 0.15
T_BT = T_BF @ T_FT
p_T = np.array([0.10, 0.0, 0.0, 1.0])
p_B = T_BT @ p_T
# 刚体矩阵的逆:R 转置,平移项为 -R.T @ t
R, t = T_BT[:3, :3], T_BT[:3, 3]
T_TB = np.eye(4)
T_TB[:3, :3] = R.T
T_TB[:3, 3] = -R.T @ t
p_back = T_TB @ p_B
print("TCP_B(m):", np.round(t, 2).tolist())
print("point_B(m):", np.round(p_B[:3], 2).tolist())
print("point_T(m):", np.round(p_back[:3], 2).tolist())
print("round_trip_ok:", np.allclose(p_back, p_T, atol=1e-9))预期输出:
TCP_B(m): [0.6, 0.35, 0.4]
point_B(m): [0.6, 0.45, 0.4]
point_T(m): [0.1, 0.0, 0.0]
round_trip_ok: True如果结果不同,先确认变换链是 T_BF @ T_FT,再检查点末项是否为 1、所有平移是否统一为米。使用三角函数 API 计算旋转时,应先将 转为 弧度。本例的往返容差 1e-9 只验证数值计算一致性,不能替代机械臂实际标定精度或碰撞安全检查。