机器人 / 系统课时

排查坐标轴方向与矩阵乘法顺序

排查坐标轴方向与矩阵乘法顺序

末端 TCP 的位置突然偏向另一侧、姿态翻转,或测量位姿无法让变换链闭合时,先不要调整逆运动学参数。这里用一组可复算的世界系、基座系、法兰系和工具系数据,逐段识别轴向、角度单位及矩阵顺序错误,并给出可运行的对照检查。

学习目标

  • 按右手系和列向量约定核对坐标轴方向及变换含义。
  • 用分段数值和 Python 脚本区分乘法顺序、轴向与角度单位错误。
  • 使用独立参考位姿、旋转矩阵性质和闭环残差验证修复结果。

固定坐标约定后再计算

本例使用右手系和齐次列向量。TWBT_{WB} 表示把基座系 B 中的坐标表达转换为世界系 W 中的表达,即 pW=TWBpBp_W=T_{WB}p_B;不要将其误当成把世界向量主动旋转到基座系的操作。所有平移以 m 计,角度输入以度计,调用三角函数前转为弧度。

世界系 +XW+X_W 向右、+YW+Y_W 向上、+ZW+Z_W 指向纸外。从 +Z+Z 一侧观察,绕 +Z+Z 旋转 +90°+90° 为逆时针,因此基座的 +XB+X_B 在世界系指向 +YW+Y_W,+YB+Y_B 指向 −XW-X_W。在设备上可用小幅正向点动核对轴向,先确保机器人处于允许点动的安全状态。

列向量约定下,工具原点的世界位姿应为 TWT=TWBTBFTFTT_{WT}=T_{WB}T_{BF}T_{FT}。运算从右向左作用于工具系中的点,矩阵相邻的坐标系下标必须衔接。若输入给的是反向矩阵,需要对完整齐次矩阵求逆;仅转置旋转部分会漏掉平移修正。行向量实现则需整体转置并反转乘积次序,不能混用两套约定。

沿链路寻找第一处偏差

使用一组便于手算的标定数据:基座 B 在世界系位置 (1.00,0.00,0.00)(1.00,0.00,0.00) m,绕世界 +Z+Z 旋转 90°90°;法兰 F 在基座 +XB+X_B 方向偏移 0.500.50 m,工具 TCP 在法兰 +XF+X_F 方向偏移 0.200.20 m,两段均无额外转角。

检查点 正确世界系结果 现场核对方式
基座原点 B (1.00,0.00)(1.00,0.00) m 检查基座外参平移和 +XB+X_B 朝向
法兰原点 F (1.00,0.50)(1.00,0.50) m 只合成 TWBTBFT_{WB}T_{BF},与示教器或标定数据比对
工具原点 TCP (1.00,0.70)(1.00,0.70) m 再乘 TFTT_{FT},检查工具偏置是否沿局部轴

世界、基座、法兰与工具坐标系的变换链及错误顺序对照

如果 B 就不符合实测,先修正世界系到基座系的外参,不要继续调整 TCP。若 B 正确、F 错误,重点检查关节正方向及法兰位姿;只有 F 正确而 TCP 错误时,才检查工具偏置是否误填为世界系分量。

运行脚本对照四种错误

环境使用 Python 3 和 NumPy。未安装时可运行 python -m pip install numpy。将下面代码保存为 check_transform.py,执行 python check_transform.py。T_measured 在此是独立给定的教学参考位姿,实际项目要替换为测量或校准得到的 TCP 位姿。

python
import numpy as np

def T(x, y, yaw_deg):
    # 接口收度数,三角函数必须使用弧度
    theta = np.deg2rad(yaw_deg)
    c, s = np.cos(theta), np.sin(theta)
    out = np.eye(4)
    out[:3, :3] = [[c, -s, 0], [s, c, 0], [0, 0, 1]]
    out[:3, 3] = [x, y, 0]
    return out

T_WB = T(1.0, 0.0, 90)
T_BF = T(0.5, 0.0, 0)
T_FT = T(0.2, 0.0, 0)
T_measured = T(1.0, 0.7, 90)

# 后三项分别人为注入顺序、轴向和角度单位错误
candidates = {
    "正确链": T_WB @ T_BF @ T_FT,
    "交换顺序": T_BF @ T_WB @ T_FT,
    "基座轴反向": T(1, 0, -90) @ T_BF @ T_FT,
    "角度单位误用": T(1, 0, np.pi / 2) @ T_BF @ T_FT,
}

for name, predicted in candidates.items():
    delta = np.linalg.inv(T_measured) @ predicted
    position_error = np.linalg.norm(delta[:3, 3])
    # 旋转矩阵迹得到0到180度的姿态差角
    cosine = np.clip((np.trace(delta[:3, :3]) - 1) / 2, -1, 1)
    angle_error = np.rad2deg(np.arccos(cosine))
    p = predicted[:3, 3]
    print(f"{name}: TCP=({p[0]:.2f}, {p[1]:.2f}) m, "
          f"位置误差={position_error:.2f} m, 姿态误差={angle_error:.2f}°")

R = T_WB[:3, :3]
loop = T_measured @ np.linalg.inv(T_FT) @ np.linalg.inv(T_BF) @ np.linalg.inv(T_WB)
print(f"旋转正交误差={np.max(np.abs(R.T @ R - np.eye(3))):.6f}")
print(f"旋转行列式={np.linalg.det(R):.6f}")
print(f"闭环最大元素误差={np.max(np.abs(loop - np.eye(4))):.6f}")

预期输出:

text
正确链: TCP=(1.00, 0.70) m, 位置误差=0.00 m, 姿态误差=0.00°
交换顺序: TCP=(1.50, 0.20) m, 位置误差=0.71 m, 姿态误差=0.00°
基座轴反向: TCP=(1.00, -0.70) m, 位置误差=1.40 m, 姿态误差=180.00°
角度单位误用: TCP=(1.70, 0.02) m, 位置误差=0.98 m, 姿态误差=88.43°
旋转正交误差=0.000000
旋转行列式=1.000000
闭环最大元素误差=0.000000

交换顺序 的错误表达式没有相接的坐标系下标,虽然数值矩阵仍可相乘,却不再具有正确的物理意义;其 TCP 偏移了约 0.710.71 m。角度单位误用 则把弧度值 π/2\pi/2 错当成度数输入,最终只旋转约 1.57°1.57°,而不是 90°90°。

用残差特征排除误判

观测现象 优先检查 修正动作
位置明显错而朝向正确 变换顺序、局部平移放错坐标系 逐项标注 TWBT_{WB}、TBFT_{BF},核对相邻下标
位置镜像且朝向相差 180°180° 轴正方向、左右手坐标系混用 检查基轴正向和旋转方向,不要靠取绝对值掩盖
预期 90°90° 实际仅约 1.57°1.57° 度数与弧度混用 在 API 边界统一转为 rad 后调用三角函数
旋转矩阵行列式接近 −1-1 镜像或坐标系反射被当成旋转 重建右手正交基,确保 det⁡(R)=+1\det(R)=+1

姿态多轴旋转还要记录具体约定,例如外旋 XYZ 和内旋 ZYX 不能只看“三个角数值相同”。同样的三个角,若改变旋转轴所在坐标系或矩阵乘法先后顺序,最终方向就可能不同。比对姿态时宜使用旋转矩阵之间的相对转角,避免欧拉角在 +180°/−180°+180°/-180° 附近产生的表示跳变。

用独立观测检验闭环

脚本用独立给定的 TWTT_{WT} 参考值乘以三段变换的逆,得到接近单位矩阵的闭环;同时检查 RTRR^TR 接近单位阵、det⁡(R)\det(R) 接近 +1+1。对于仅含理想浮点运算的示例,可用 10−610^{-6} 作为矩阵元素误差阈值;现场应改用设备精度和标定规范规定的平移、角度容差,例如某项目可能要求位置误差不超过 22 mm、角度误差不超过 0.2°0.2°。

实操时按基座、法兰、工具的顺序只修复首个异常环节,每次重新比较 TCP 位置残差、姿态残差和闭环误差;不要同时修改轴号、角度正负和工具偏置,否则无法判断哪一项修复真正有效。

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

目录与阅读设置

阅读设置

17

记录一个想法

排查坐标轴方向与矩阵乘法顺序

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

登录哪都通