机器人 / 系统课时

用齐次矩阵计算位姿坐标变换

用齐次矩阵计算位姿坐标变换

基座、法兰和工具中心点(TCP)的坐标常常来自不同参考系,直接相加会把抓取点算错。这里固定一个绕 Z 轴旋转 90° 的法兰姿态,逐步求工具在基座系中的位姿、点坐标及逆变换,并用程序复核结果。

学习目标

  • 读懂 ATB{}^{A}T_B 的转换方向,区分旋转、平移与点的齐次坐标。
  • 正确组合基座、法兰和工具的变换,算出 TCP 与工具点的基座坐标。
  • 利用刚体变换求逆及数值回代定位方向、顺序和单位错误。

明确变换方向和坐标约定

设 B 为机器人基座坐标系,F 为法兰坐标系,T 为工具坐标系,其原点是标定的 TCP。采用右手坐标系、列向量、长度单位米,正转角按右手定则。记号 BTF{}^{B}T_F 表示将 F 系的坐标换算成 B 系,并不是把 B 系坐标变到 F 系。

对点使用齐次向量 [x,y,z,1]T[x,y,z,1]^{\mathsf T},这样一个矩阵就能同时执行旋转和平移:

ATB=[r11r12r13txr21r22r23tyr31r32r33tz0001]{}^{A}T_B=\begin{bmatrix}r_{11}&r_{12}&r_{13}&t_x\\r_{21}&r_{22}&r_{23}&t_y\\r_{31}&r_{32}&r_{33}&t_z\\0&0&0&1\end{bmatrix}

左上 3×33\times3 是旋转矩阵 RR,最右列前三项是 B 原点在 A 系中的位置 t\mathbf t;变换后的点满足 Ap=ATB Bp{}^{A}\mathbf p={}^{A}T_B\,{}^{B}\mathbf p。如果换算的是速度方向而非空间点,齐次末项设为 0,平移不生效。

写出法兰与工具的位姿

假定法兰 F 相对基座 B 绕 +ZB+Z_B 旋转 90∘90^\circ,法兰原点位于 B 系 (0.60,0.20,0.40)(0.60,0.20,0.40) m:

BTF=[0−100.601000.200010.400001]{}^{B}T_F=\begin{bmatrix}0&-1&0&0.60\\1&0&0&0.20\\0&0&1&0.40\\0&0&0&1\end{bmatrix}

工具坐标系 T 与法兰 F 方向平行,仅沿 +XF+X_F 偏置 0.15 m;这个方向由工具安装定义,本例用它突出偏置必须随法兰旋转,并不表示所有夹具都沿法兰 X 轴伸出。

FTT=[1000.15010000100001]{}^{F}T_T=\begin{bmatrix}1&0&0&0.15\\0&1&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix}

组合矩阵求 TCP 和目标点

按坐标链相邻字母匹配的方向相乘,先将 T 系换到 F 系,再将 F 系换到 B 系,矩阵乘法从右向左执行:

BTT=BTF FTT=[0−100.601000.350010.400001]{}^{B}T_T={}^{B}T_F\,{}^{F}T_T=\begin{bmatrix}0&-1&0&0.60\\1&0&0&0.35\\0&0&1&0.40\\0&0&0&1\end{bmatrix}

用等价的平移公式 tBT=tBF+RBFtFT\mathbf t_{BT}=\mathbf t_{BF}+R_{BF}\mathbf t_{FT} 可检查:工具偏置 (0.15,0,0)(0.15,0,0) 在基座系中变为 (0,0.15,0)(0,0.15,0),因此 TCP 为 (0.60,0.35,0.40)(0.60,0.35,0.40) m。若直接算成 (0.75,0.20,0.40)(0.75,0.20,0.40) m,就是把法兰局部偏置误当作基座方向。

现在取工具系中的一个点 Tp=(0.10,0,0,1)T{}^{T}\mathbf p=(0.10,0,0,1)^{\mathsf T},计算得 Bp=(0.60,0.45,0.40,1)T{}^{B}\mathbf p=(0.60,0.45,0.40,1)^{\mathsf T}。工具上的 0.10 m 沿 +XT+X_T,对应基座中的 +YB+Y_B;点的位置与 TCP 不相同,不要把点坐标变换误当成 TCP 位姿。

法兰、TCP及工具点在基座坐标系中的位置和变换链

逆变换与往返验证

工位联调时还需要把 B 系中的目标点换回工具系。刚体变换满足 R−1=RTR^{-1}=R^{\mathsf T},但逆变换平移不是简单取负,必须计算 −RTt-R^{\mathsf T}\mathbf t。本例的逆矩阵为:

TTB=(BTT)−1=[010−0.35−1000.60001−0.400001]{}^{T}T_B=({}^{B}T_T)^{-1}=\begin{bmatrix}0&1&0&-0.35\\-1&0&0&0.60\\0&0&1&-0.40\\0&0&0&1\end{bmatrix}

应用到 (0.60,0.45,0.40,1)T(0.60,0.45,0.40,1)^{\mathsf T},重新得到 (0.10,0,0,1)T(0.10,0,0,1)^{\mathsf T}。旋转矩阵还应满足 RTR=IR^{\mathsf T}R=I、det⁡(R)=+1\det(R)=+1;否则不能把含缩放或剪切的矩阵按刚体变换求逆。

用 NumPy 独立复算

在已安装 NumPy 的 Python 3 环境中运行以下完整示例;矩阵按行填写,点采用列向量,@ 表示矩阵乘法。代码用转置而非通用矩阵求逆计算反向变换:

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

预期输出:

text
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 计算旋转时,应先将 90∘90^\circ 转为 π/2\pi/2 弧度。本例的往返容差 1e-9 只验证数值计算一致性,不能替代机械臂实际标定精度或碰撞安全检查。

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

目录与阅读设置

阅读设置

17

记录一个想法

用齐次矩阵计算位姿坐标变换

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

登录哪都通