机器人坐标变换：矩阵、四元数和TF

Contents — find the section you need

Change parameters and verify

Open the panel, then press Run to load Python. You can stop execution and reset parameters. Results are computed on this device. No Python installation is required.

Local execution steps below are optional for reproducing the source results; they are not required for the browser experiment.

The experiment controls are in English.

Open experiment panel in a new tab

Download reproduction source

坐标变换的开始是指定哪个坐标系描述输入点，哪个坐标系描述输出点。即使矩阵运算方向反转，物体的位置仍然会出错。

将下标解读为方向

设 R_AB 将 B 坐标系映射到 A 坐标系，t_AB 为 B 在 A 坐标系中的原点。我们使用列向量：

p_A=R_{AB}p_B+t_{AB}

旋转矩阵的列向量是 B 在 A 坐标系中的坐标轴。平移也用 A 坐标系表示。点进行平移；自由向量（例如方向）仅进行旋转。对每个三元素数组应用相同的处理方式会混淆这些含义。

旋转 90 度的传感器

在二维空间中，将 B 的原点放置在 A 坐标系中的 (1, 2) 米处，并将 B 的 x 轴指向 A 的 y 轴。 B 中的点 (2, 0) 米变为：

R_{AB}=\begin{bmatrix}0&-1\\1&0\end{bmatrix},\qquad p_A=\begin{bmatrix}0&-1\\1&0\end{bmatrix}\begin{bmatrix}2\\0\end{bmatrix}+\begin{bmatrix}1\\2\end{bmatrix}=\begin{bmatrix}1\\4\end{bmatrix}

坐标系平移至 (1, 2) 米并旋转 90 度；变换后的点为 (1, 4) 米。

Python 脚本中的 transforms 函数会检查此结果及其逆变换。该脚本使用 Python 3.12.3、NumPy 1.26.4 和 Matplotlib 3.6.3 执行。运行 python3 engineering_labs.py 以重新生成图形和结果。

要重新生成图形，请将链接的脚本另存为 engineering_labs.py 并保存到新目录中，然后在该目录中运行它。输出结果保存在工作目录中：此图位于 transforms.svg，检查的坐标位于 engineering-results.json 中的 transform。该脚本还会生成卡尔曼滤波图、ICP 图、频率响应图和混叠图以及相应的 CSV 文件。请使用空的工作目录，因为现有输出文件将被覆盖。

反转不仅仅是平移取反

旋转矩阵的逆矩阵是它的转置矩阵：

p_B=R_{AB}^T(p_A-t_{AB}),\qquad R_{BA}=R_{AB}^T,\qquad t_{BA}=-R_{AB}^Tt_{AB}

从 (1, 4) 中减去 (1, 2)，然后旋转 -90° 得到 (2, 0)。逆平移是 (-2, 1)，而不是简单的 (-1, -2)。往返任意点以检测方向错误。

组合齐次变换

在三维空间中，一个 4×4 矩阵组合了旋转和平移：

T_{AB}=\begin{bmatrix}R_{AB}&t_{AB}\\0&1\end{bmatrix},\qquad T_{AC}=T_{AB}T_{BC}

最右侧的变换首先生效。对于地图 A、物体 B 和传感器 C，一个点的移动顺序为：传感器→物体→地图。矩阵乘法通常不能重新排序。匹配中间的 B 也有助于变量命名。

四元数表示相同的旋转

对于单位轴 u 和角度 θ，ROS 分量顺序 (x, y, z, w) 为：

q=(u_x\sin(\theta/2),u_y\sin(\theta/2),u_z\sin(\theta/2),\cos(\theta/2))

绕 z 轴旋转 90° 的结果是 (0, 0, 0.7071, 0.7071)。一些库使用 (w, x, y, z)，因此在复制数组之前请检查顺序。q 和 -q 表示相同的旋转；仅分量相减并不能作为旋转误差度量。同时检查归一化。

REP-103 定义了 ROS 右手 SI 坐标系约定，即物体 x-forward/y-left/z-up 和光学 z-forward/x-right/y-down。当相机几何体出现旋转时，请首先检查坐标系约定。

TF 也取决于时间

将父/子变换读取为子物体在其父物体中的姿态，与上述 T_AB 相匹配。点变换 API 需要仔细选择目标/源坐标系。固定安装变换与随时间变化的车辆姿态不同。

REP-105 区分了全局校正地图（可能出现跳变）和连续但漂移的里程计。在错误的时间戳处使用校正矩阵仍然会导致移动传感器点的位置错误。请继续使用 sensor-fusion debugging 同时检查方向和时间。