机器人 / 系统课时
把世界系目标换算到基座系
把世界系目标换算到基座系
视觉系统通常给出世界坐标系中的抓取目标,而机械臂逆运动学求解器通常要求基座系目标。本篇使用已知的基座位姿,将目标位置与姿态一起转换到基座系,并通过数值算例和往返变换检查计算结果。
学习目标
- 辨认世界系、基座系与目标坐标系之间的变换方向。
- 使用齐次变换矩阵计算基座系中的完整目标位姿。
- 通过数值回代、单位检查与工具坐标系核对,确认逆运动学输入正确。
确认坐标系和变换方向
约定三个右手坐标系:世界系 、机械臂基座系 和目标系 。采用列向量表示坐标,长度单位为米,旋转角计算使用弧度。
记 为基座系相对于世界系的位姿。它将基座系中的点坐标映射到世界系,而不是反过来。记 为目标系在世界系中的位姿。机械臂逆运动学需要的是 ,即同一目标在基座系中的表达。
例如,若基座原点在世界系中的位置为 m,且基座绕世界正 轴旋转 ,则 应把基座原点 映射到世界坐标 。这个简单测试可以帮助辨认外部接口返回的矩阵方向。
用逆变换得到基座系目标
已知 和 时,先求基座位姿的逆矩阵,再从左侧乘世界系目标:
对于合法刚体变换,旋转矩阵 满足 。如果基座原点的位置为 、目标位置为 ,那么目标在基座系中的位置为:
目标姿态则按照相同的坐标变换方向计算:
位置计算必须先减去基座原点的世界坐标,再旋转到基座坐标轴。姿态不能简单地照搬世界系旋转,也不能在一般三维情况下直接逐项相减欧拉角;应先计算旋转矩阵,再按需要转换为四元数或欧拉角。

运行包含位置与姿态的数值算例
设基座在世界系中的位置为 m,偏航角为 ;目标在世界系中的位置为 m,偏航角为 。本例只有绕 轴的旋转,但使用完整 齐次矩阵,因此可以沿用同样的矩阵计算方式处理一般三维位姿。
先在世界坐标中求目标相对基座的位移,得到 m。再绕 轴旋转 ,获得基座系目标位置 m。目标相对基座的偏航角则是 。
使用 Python 3.10 或更高版本、NumPy 1.26 或 2.x。可通过 python -m pip install numpy 安装依赖。下面是完整可运行示例:
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
))预期输出:
base position (m): 2.000, -1.000, 0.000
base yaw (deg): -60.000
round trip: Trueround trip: True 表明把基座系计算结果重新映射回世界系,可以在给定数值容差内恢复原始目标矩阵。示例使用刚体矩阵求逆,工程实现中也可以直接使用旋转矩阵转置和对应的逆平移,以减少重复计算。
送入逆运动学前核对三个条件
- 坐标系和时间一致。 视觉目标若最初在相机系,应先通过正确标定的相机外参转换到世界系。对于移动底座,还需要使用与视觉观测时间对应的基座位姿,不能混用不同时刻的变换。
- 单位和旋转表示一致。 例如视觉输出位置为毫米,而逆运动学输入使用米时,必须先除以1000。若求解器接收弧度,不要把示例的 直接作为弧度传入。完整旋转矩阵还应满足正交性及行列式接近 。
- 目标坐标架与逆解末端一致。 如果视觉目标描述的是工具中心点(TCP),但逆解目标定义在法兰坐标架,就必须利用已知的法兰到TCP固定变换继续换算,不能把TCP位姿直接当作法兰位姿。
对于第三种情况,记法兰系为 、工具中心点系为 ,且已知固定工具变换 ,则逆解所需的法兰目标为:
验证时可以设置位置容差 m,对完整齐次矩阵执行往返变换比较。但要注意,往返变换只能证明矩阵运算自洽,不能证明相机外参或世界到基座的实际标定正确。标定精度仍需使用已知位置的独立测试点检查。
最终交给逆运动学求解器的应是以基座为参考、以求解器要求的末端坐标架为对象的完整目标位姿,而不是单独的世界坐标位置。