机器人 / 系统课时
用齐次矩阵合成并求逆刚体变换
用齐次矩阵合成并求逆刚体变换
工具点从工具坐标系转换到机体和世界坐标系时,旋转、平移与乘法顺序必须保持一致。本篇用两个具体的三维变换组成 4×4 齐次矩阵,求出反向变换,并用同一点的坐标往返检查结果。
学习目标
- 读懂 齐次矩阵中旋转、平移和齐次坐标的含义。
- 按参考系匹配顺序合成两段刚体变换,算出末端点坐标。
- 利用旋转矩阵的正交性求逆,并用程序核验往返误差。
固定坐标约定与矩阵方向
设 A 为世界坐标系、B 为机体坐标系、C 为工具坐标系;均使用右手系、米作为长度单位,并采用列向量左乘矩阵。 表示把同一空间点的 B 系坐标转换成 A 系坐标,而不是把物体“从 A 移动到 B”。
其中 为 旋转矩阵,它的三列分别表示 B 系的三根单位轴在 A 系中的方向; 为 B 原点在 A 系中的位置。空间点写成 ,满足 。方向向量末位写 0,因此不会受平移影响。
让连续变换的参考系接得上
取 B 相对 A 绕 A 的正 Z 轴旋转 ,并平移 米;取 C 相对 B 绕 B 的正 X 轴旋转 ,并平移 米。第二个平移量是 B 系分量,不能直接当成 A 系分量使用。

按列向量约定,右侧矩阵先作用。此例中 将 旋到 ,所以合成平移为 米,而不是误把平移直接相加得到的 米。
通过旋转转置求逆变换
纯刚体变换不含缩放或剪切,其旋转块满足 且 。因此无须对整个 矩阵做通用求逆:
逆变换的平移不是简单的 ,而是先用 将该位移表达在 C 系,再取负。此例逆矩阵的平移列为 米。用逆矩阵将世界系坐标恢复为工具系坐标时,齐次坐标最后一项仍是 1。
用 NumPy 复算并检验往返
在 Python 3.10+ 环境安装 NumPy 后,将以下代码保存为 transform_check.py,执行 python transform_check.py。矩阵元素直接写为上述两个 旋转的精确值,避免三角函数带来的微小打印误差。
python
import numpy as np
np.set_printoptions(precision=3, suppress=True)
# T_ab 将 B 系点坐标转为 A 系;T_bc 将 C 系转为 B 系
T_ab = np.array([
[0, -1, 0, 1],
[1, 0, 0, 2],
[0, 0, 1, 0],
[0, 0, 0, 1],
], dtype=float)
T_bc = np.array([
[1, 0, 0, 2],
[0, 0, -1, 0],
[0, 1, 0, 1],
[0, 0, 0, 1],
], dtype=float)
T_ac = T_ab @ T_bc
R, t = T_ac[:3, :3], T_ac[:3, 3]
# 正交旋转的逆是转置,逆平移还需乘这个转置
T_ca = np.eye(4)
T_ca[:3, :3] = R.T
T_ca[:3, 3] = -R.T @ t
p_c = np.array([1., 2., 3., 1.])
p_a = T_ac @ p_c
p_c_back = T_ca @ p_a
print("T_AC:\n", T_ac)
print("T_CA:\n", T_ca)
print("p_A:", p_a[:3])
print("p_C:", p_c_back[:3])
print("orthonormal:", np.allclose(R.T @ R, np.eye(3)))
print("roundtrip:", np.allclose(T_ca @ T_ac, np.eye(4)))预期输出:
text
T_AC:
[[0. 0. 1. 1.]
[1. 0. 0. 4.]
[0. 1. 0. 1.]
[0. 0. 0. 1.]]
T_CA:
[[ 0. 1. 0. -4.]
[ 0. 0. 1. -1.]
[ 1. 0. 0. -1.]
[ 0. 0. 0. 1.]]
p_A: [4. 5. 3.]
p_C: [1. 2. 3.]
orthonormal: True
roundtrip: True用三项检查排除常见错误
- 先核对参考系下标。
${}^{A}T_B\,{}^{B}T_C$的中间都是 B,能够相消并形成从 C 到 A 的坐标表达变换;把矩阵倒序相乘虽然数组维度相容,但此处物理参考链不匹配。 - 再核对旋转和平移。 合成平移应为 米。若得到 米,说明遗漏了前一段的旋转;若得到 米作为逆平移,则遗漏了逆旋转。
- 最后做数值验收。 检查 与单位阵、 与 1 的偏差,并测试 与单位阵是否在
np.allclose默认容差内一致;同时确认长度没有混用米与毫米。
当这三项都通过时,程序中每个矩阵的输入系、输出系与点坐标的单位才是可追踪的,后续多关节变换也能沿同一规则逐段相乘。
按自己的节奏,确认真正理解。