机器人 / 系统课时
排查坐标轴方向与矩阵乘法顺序
排查坐标轴方向与矩阵乘法顺序
末端 TCP 的位置突然偏向另一侧、姿态翻转,或测量位姿无法让变换链闭合时,先不要调整逆运动学参数。这里用一组可复算的世界系、基座系、法兰系和工具系数据,逐段识别轴向、角度单位及矩阵顺序错误,并给出可运行的对照检查。
学习目标
- 按右手系和列向量约定核对坐标轴方向及变换含义。
- 用分段数值和 Python 脚本区分乘法顺序、轴向与角度单位错误。
- 使用独立参考位姿、旋转矩阵性质和闭环残差验证修复结果。
固定坐标约定后再计算
本例使用右手系和齐次列向量。 表示把基座系 B 中的坐标表达转换为世界系 W 中的表达,即 ;不要将其误当成把世界向量主动旋转到基座系的操作。所有平移以 m 计,角度输入以度计,调用三角函数前转为弧度。
世界系 向右、 向上、 指向纸外。从 一侧观察,绕 旋转 为逆时针,因此基座的 在世界系指向 , 指向 。在设备上可用小幅正向点动核对轴向,先确保机器人处于允许点动的安全状态。
列向量约定下,工具原点的世界位姿应为 。运算从右向左作用于工具系中的点,矩阵相邻的坐标系下标必须衔接。若输入给的是反向矩阵,需要对完整齐次矩阵求逆;仅转置旋转部分会漏掉平移修正。行向量实现则需整体转置并反转乘积次序,不能混用两套约定。
沿链路寻找第一处偏差
使用一组便于手算的标定数据:基座 B 在世界系位置 m,绕世界 旋转 ;法兰 F 在基座 方向偏移 m,工具 TCP 在法兰 方向偏移 m,两段均无额外转角。
| 检查点 | 正确世界系结果 | 现场核对方式 |
|---|---|---|
| 基座原点 B | m | 检查基座外参平移和 朝向 |
| 法兰原点 F | m | 只合成 ,与示教器或标定数据比对 |
| 工具原点 TCP | m | 再乘 ,检查工具偏置是否沿局部轴 |

如果 B 就不符合实测,先修正世界系到基座系的外参,不要继续调整 TCP。若 B 正确、F 错误,重点检查关节正方向及法兰位姿;只有 F 正确而 TCP 错误时,才检查工具偏置是否误填为世界系分量。
运行脚本对照四种错误
环境使用 Python 3 和 NumPy。未安装时可运行 python -m pip install numpy。将下面代码保存为 check_transform.py,执行 python check_transform.py。T_measured 在此是独立给定的教学参考位姿,实际项目要替换为测量或校准得到的 TCP 位姿。
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}")预期输出:
正确链: 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 偏移了约 m。角度单位误用 则把弧度值 错当成度数输入,最终只旋转约 ,而不是 。
用残差特征排除误判
| 观测现象 | 优先检查 | 修正动作 |
|---|---|---|
| 位置明显错而朝向正确 | 变换顺序、局部平移放错坐标系 | 逐项标注 、,核对相邻下标 |
| 位置镜像且朝向相差 | 轴正方向、左右手坐标系混用 | 检查基轴正向和旋转方向,不要靠取绝对值掩盖 |
| 预期 实际仅约 | 度数与弧度混用 | 在 API 边界统一转为 rad 后调用三角函数 |
| 旋转矩阵行列式接近 | 镜像或坐标系反射被当成旋转 | 重建右手正交基,确保 |
姿态多轴旋转还要记录具体约定,例如外旋 XYZ 和内旋 ZYX 不能只看“三个角数值相同”。同样的三个角,若改变旋转轴所在坐标系或矩阵乘法先后顺序,最终方向就可能不同。比对姿态时宜使用旋转矩阵之间的相对转角,避免欧拉角在 附近产生的表示跳变。
用独立观测检验闭环
脚本用独立给定的 参考值乘以三段变换的逆,得到接近单位矩阵的闭环;同时检查 接近单位阵、 接近 。对于仅含理想浮点运算的示例,可用 作为矩阵元素误差阈值;现场应改用设备精度和标定规范规定的平移、角度容差,例如某项目可能要求位置误差不超过 mm、角度误差不超过 。
实操时按基座、法兰、工具的顺序只修复首个异常环节,每次重新比较 TCP 位置残差、姿态残差和闭环误差;不要同时修改轴号、角度正负和工具偏置,否则无法判断哪一项修复真正有效。