机器人 / 系统课时

用齐次矩阵合成并求逆刚体变换

用齐次矩阵合成并求逆刚体变换

工具点从工具坐标系转换到机体和世界坐标系时,旋转、平移与乘法顺序必须保持一致。本篇用两个具体的三维变换组成 4×4 齐次矩阵,求出反向变换,并用同一点的坐标往返检查结果。

学习目标

  • 读懂 4×44\times4 齐次矩阵中旋转、平移和齐次坐标的含义。
  • 按参考系匹配顺序合成两段刚体变换,算出末端点坐标。
  • 利用旋转矩阵的正交性求逆,并用程序核验往返误差。

固定坐标约定与矩阵方向

设 A 为世界坐标系、B 为机体坐标系、C 为工具坐标系;均使用右手系、米作为长度单位,并采用列向量左乘矩阵。ATB{}^{A}T_B 表示把同一空间点的 B 系坐标转换成 A 系坐标,而不是把物体“从 A 移动到 B”。

ATB=[RABtAB01×31]{}^{A}T_B=\begin{bmatrix}R_{AB}&\mathbf t_{AB}\\\mathbf 0_{1\times3}&1\end{bmatrix}

其中 RABR_{AB} 为 3×33\times3 旋转矩阵,它的三列分别表示 B 系的三根单位轴在 A 系中的方向;tAB\mathbf t_{AB} 为 B 原点在 A 系中的位置。空间点写成 p~=(x,y,z,1)T\tilde{\mathbf p}=(x,y,z,1)^{\mathsf T},满足 p~A=ATBp~B\tilde{\mathbf p}_A={}^{A}T_B\tilde{\mathbf p}_B。方向向量末位写 0,因此不会受平移影响。

让连续变换的参考系接得上

取 B 相对 A 绕 A 的正 Z 轴旋转 90∘90^\circ,并平移 tAB=(1,2,0)T\mathbf t_{AB}=(1,2,0)^{\mathsf T} 米;取 C 相对 B 绕 B 的正 X 轴旋转 90∘90^\circ,并平移 tBC=(2,0,1)T\mathbf t_{BC}=(2,0,1)^{\mathsf T} 米。第二个平移量是 B 系分量,不能直接当成 A 系分量使用。

A、B、C 三坐标系与两段齐次变换的正确串接

ATC=ATB BTC,RAC=RABRBC,tAC=RABtBC+tAB{}^{A}T_C={}^{A}T_B\,{}^{B}T_C,\qquad R_{AC}=R_{AB}R_{BC},\qquad\mathbf t_{AC}=R_{AB}\mathbf t_{BC}+\mathbf t_{AB}

按列向量约定,右侧矩阵先作用。此例中 RABR_{AB} 将 (2,0,1)(2,0,1) 旋到 (0,2,1)(0,2,1),所以合成平移为 (1,4,1)(1,4,1) 米,而不是误把平移直接相加得到的 (3,2,1)(3,2,1) 米。

通过旋转转置求逆变换

纯刚体变换不含缩放或剪切,其旋转块满足 RTR=IR^{\mathsf T}R=I 且 det⁡(R)=1\det(R)=1。因此无须对整个 4×44\times4 矩阵做通用求逆:

(ATC)−1=CTA=[RACT−RACTtAC01×31]({}^{A}T_C)^{-1}={}^{C}T_A=\begin{bmatrix}R_{AC}^{\mathsf T}&-R_{AC}^{\mathsf T}\mathbf t_{AC}\\\mathbf 0_{1\times3}&1\end{bmatrix}

逆变换的平移不是简单的 −tAC-\mathbf t_{AC},而是先用 RACTR_{AC}^{\mathsf T} 将该位移表达在 C 系,再取负。此例逆矩阵的平移列为 (−4,−1,−1)T(-4,-1,-1)^{\mathsf T} 米。用逆矩阵将世界系坐标恢复为工具系坐标时,齐次坐标最后一项仍是 1。

用 NumPy 复算并检验往返

在 Python 3.10+ 环境安装 NumPy 后,将以下代码保存为 transform_check.py,执行 python transform_check.py。矩阵元素直接写为上述两个 90∘90^\circ 旋转的精确值,避免三角函数带来的微小打印误差。

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

用三项检查排除常见错误

  1. 先核对参考系下标。 ${}^{A}T_B\,{}^{B}T_C$ 的中间都是 B,能够相消并形成从 C 到 A 的坐标表达变换;把矩阵倒序相乘虽然数组维度相容,但此处物理参考链不匹配。
  2. 再核对旋转和平移。 合成平移应为 (1,4,1)(1,4,1) 米。若得到 (3,2,1)(3,2,1) 米,说明遗漏了前一段的旋转;若得到 (−1,−4,−1)(-1,-4,-1) 米作为逆平移,则遗漏了逆旋转。
  3. 最后做数值验收。 检查 RTRR^{\mathsf T}R 与单位阵、det⁡(R)\det(R) 与 1 的偏差,并测试 T−1TT^{-1}T 与单位阵是否在 np.allclose 默认容差内一致;同时确认长度没有混用米与毫米。

当这三项都通过时,程序中每个矩阵的输入系、输出系与点坐标的单位才是可追踪的,后续多关节变换也能沿同一规则逐段相乘。

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

目录与阅读设置

阅读设置

17

记录一个想法

用齐次矩阵合成并求逆刚体变换

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

登录哪都通