机器人 / 系统课时

把世界系目标换算到基座系

把世界系目标换算到基座系

视觉系统通常给出世界坐标系中的抓取目标,而机械臂逆运动学求解器通常要求基座系目标。本篇使用已知的基座位姿,将目标位置与姿态一起转换到基座系,并通过数值算例和往返变换检查计算结果。

学习目标

  • 辨认世界系、基座系与目标坐标系之间的变换方向。
  • 使用齐次变换矩阵计算基座系中的完整目标位姿。
  • 通过数值回代、单位检查与工具坐标系核对,确认逆运动学输入正确。

确认坐标系和变换方向

约定三个右手坐标系:世界系 WW、机械臂基座系 BB 和目标系 GG。采用列向量表示坐标,长度单位为米,旋转角计算使用弧度。

记 WTB{}^{W}T_B 为基座系相对于世界系的位姿。它将基座系中的点坐标映射到世界系,而不是反过来。记 WTG{}^{W}T_G 为目标系在世界系中的位姿。机械臂逆运动学需要的是 BTG{}^{B}T_G,即同一目标在基座系中的表达。

例如,若基座原点在世界系中的位置为 (1,2,0)(1,2,0) m,且基座绕世界正 ZZ 轴旋转 90°90°,则 WTB{}^{W}T_B 应把基座原点 (0,0,0)(0,0,0) 映射到世界坐标 (1,2,0)(1,2,0)。这个简单测试可以帮助辨认外部接口返回的矩阵方向。

用逆变换得到基座系目标

已知 WTB{}^{W}T_B 和 WTG{}^{W}T_G 时,先求基座位姿的逆矩阵,再从左侧乘世界系目标:

BTG=(WTB)−1WTG{}^{B}T_G = ({}^{W}T_B)^{-1}{}^{W}T_G

对于合法刚体变换,旋转矩阵 RWBR_{WB} 满足 RWB−1=RWBTR_{WB}^{-1}=R_{WB}^{T}。如果基座原点的位置为 pWBp_{WB}、目标位置为 pWGp_{WG},那么目标在基座系中的位置为:

pBG=RWBT(pWG−pWB)p_{BG}=R_{WB}^{T}(p_{WG}-p_{WB})

目标姿态则按照相同的坐标变换方向计算:

RBG=RWBTRWGR_{BG}=R_{WB}^{T}R_{WG}

位置计算必须先减去基座原点的世界坐标,再旋转到基座坐标轴。姿态不能简单地照搬世界系旋转,也不能在一般三维情况下直接逐项相减欧拉角;应先计算旋转矩阵,再按需要转换为四元数或欧拉角。

世界系目标位姿向基座系的转换关系

运行包含位置与姿态的数值算例

设基座在世界系中的位置为 (1,2,0)(1,2,0) m,偏航角为 90°90°;目标在世界系中的位置为 (2,4,0)(2,4,0) m,偏航角为 30°30°。本例只有绕 ZZ 轴的旋转,但使用完整 4×44\times4 齐次矩阵,因此可以沿用同样的矩阵计算方式处理一般三维位姿。

先在世界坐标中求目标相对基座的位移,得到 (1,2,0)(1,2,0) m。再绕 ZZ 轴旋转 −90°-90°,获得基座系目标位置 (2,−1,0)(2,-1,0) m。目标相对基座的偏航角则是 30°−90°=−60°30°-90°=-60°。

使用 Python 3.10 或更高版本、NumPy 1.26 或 2.x。可通过 python -m pip install numpy 安装依赖。下面是完整可运行示例:

python
import numpy as np


def make_tf(yaw_deg, translation):
    angle = np.deg2rad(yaw_deg)
    c, s = np.cos(angle), np.sin(angle)
    tf = np.eye(4)
    tf[:3, :3] = [[c, -s, 0], [s, c, 0], [0, 0, 1]]
    tf[:3, 3] = translation
    return tf


# 两个输入均表示相应坐标系在世界系中的位姿
world_T_base = make_tf(90, (1, 2, 0))
world_T_goal = make_tf(30, (2, 4, 0))

# 从左侧乘基座位姿的逆矩阵,转换完整目标位姿
base_T_goal = np.linalg.inv(world_T_base) @ world_T_goal
x, y, z = base_T_goal[:3, 3]
yaw = np.rad2deg(np.arctan2(base_T_goal[1, 0], base_T_goal[0, 0]))

print(f"base position (m): {x:.3f}, {y:.3f}, {z:.3f}")
print(f"base yaw (deg): {yaw:.3f}")

# 回代验证位置与旋转组成的完整齐次变换
print("round trip:", np.allclose(
    world_T_base @ base_T_goal, world_T_goal, atol=1e-9
))

预期输出:

text
base position (m): 2.000, -1.000, 0.000
base yaw (deg): -60.000
round trip: True

round trip: True 表明把基座系计算结果重新映射回世界系,可以在给定数值容差内恢复原始目标矩阵。示例使用刚体矩阵求逆,工程实现中也可以直接使用旋转矩阵转置和对应的逆平移,以减少重复计算。

送入逆运动学前核对三个条件

  1. 坐标系和时间一致。 视觉目标若最初在相机系,应先通过正确标定的相机外参转换到世界系。对于移动底座,还需要使用与视觉观测时间对应的基座位姿,不能混用不同时刻的变换。
  2. 单位和旋转表示一致。 例如视觉输出位置为毫米,而逆运动学输入使用米时,必须先除以1000。若求解器接收弧度,不要把示例的 −60°-60° 直接作为弧度传入。完整旋转矩阵还应满足正交性及行列式接近 11。
  3. 目标坐标架与逆解末端一致。 如果视觉目标描述的是工具中心点(TCP),但逆解目标定义在法兰坐标架,就必须利用已知的法兰到TCP固定变换继续换算,不能把TCP位姿直接当作法兰位姿。

对于第三种情况,记法兰系为 FF、工具中心点系为 GG,且已知固定工具变换 FTG{}^{F}T_G,则逆解所需的法兰目标为:

BTF=BTG(FTG)−1{}^{B}T_F = {}^{B}T_G({}^{F}T_G)^{-1}

验证时可以设置位置容差 10−610^{-6} m,对完整齐次矩阵执行往返变换比较。但要注意,往返变换只能证明矩阵运算自洽,不能证明相机外参或世界到基座的实际标定正确。标定精度仍需使用已知位置的独立测试点检查。

最终交给逆运动学求解器的应是以基座为参考、以求解器要求的末端坐标架为对象的完整目标位姿,而不是单独的世界坐标位置。

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

目录与阅读设置

阅读设置

17

记录一个想法

把世界系目标换算到基座系

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

登录哪都通