机器人位姿变换推导与交互演示

位姿（pose）同时包含位置和姿态。机器人要把“相机看到的物体”交给机械臂抓取，必须先知道相机、机械臂基座、末端和物体分别在哪个坐标系中，再把同一个点变换到控制器理解的坐标系。本文用二维例子把正变换、逆变换和代码中的计算一步步推导出来，最后用按钮实时观察公式变化。

注意：坐标系错误可能直接造成碰撞

米/毫米混用、角度/弧度混用、矩阵方向写反或使用过期 TF，都会把视觉目标变成错误的机械臂目标。真机抓取前必须用已知标定点闭环验证，并限制首次运动的速度、工作空间和单步位移。

1. 先约定记号，避免方向写反

记 $T_{AB}$ 表示：把 B 坐标系中的坐标变换成 A 坐标系中的坐标。例如：

• $T_{WB}$：机器人坐标系 B 在世界坐标系 W 中的位姿。
• $p_B$：一个点在 B 系中的坐标。
• $p_W$：同一个点在 W 系中的坐标。

因此正变换写作：

$${\overline{p}}_W=T_{WB}{\overline{p}}_B$$

下标是有方向的，不能把 $T_{WB}$ 误读成“从 W 到 B”。写程序时建议把坐标系名称写进变量名，少使用没有含义的 T1、T2。

2. 从二维旋转开始

二维坐标系旋转 $\theta$ 后，单位 x 轴变成 $(\cos \theta ,\sin \theta )$，单位 y 轴变成 $(-\sin \theta ,\cos \theta )$。因此旋转矩阵是：

$$R(\theta )=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]$$

对 B 系中的点 $p_B=[x_B,y_B{]}^T$，只考虑旋转时：

$$\begin{array}{rl}{x}_W^{\prime }& =\cos \theta x_B-\sin \theta y_B\\ y_W^{\prime }& =\sin \theta x_B+\cos \theta y_B\end{array}$$

这说明：旋转不是分别给 x、y 加一个角度，而是对两个坐标分量做矩阵乘法。

3. 加上平移，得到位姿正变换

设 B 原点在 W 系中的位置为：

$$t_{WB}=\left[\begin{array}{c}{t}_x\\ t_y\end{array}\right]$$

先旋转，再把 B 原点平移到世界坐标中的位置：

$$p_W=R_{WB}{p}_B+t_{WB}$$

展开后是：

$$\begin{array}{rl}{x}_W& =\cos \theta x_B-\sin \theta y_B+t_x\\ y_W& =\sin \theta x_B+\cos \theta y_B+t_y\end{array}$$

这就是“移动某一个坐标系后，点的世界坐标如何变化”的核心公式：

• 按钮左右移动改变 $t_x$。
• 按钮上下移动改变 $t_y$。
• 旋转按钮改变 $\theta$。
• 如果点固定在 B 系中，改变 B 的位姿会改变它在 W 系中的坐标。

4. 齐次坐标：把旋转和平移放进一个矩阵

为了让旋转和平移可以连续相乘，把二维点扩充为三维齐次坐标：

$${\overline{p}}_B=\left[\begin{array}{c}{x}_B\\ y_B\\ 1\end{array}\right]$$

位姿矩阵写成：

$$T_{WB}=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & t_x\\ \sin \theta & \cos \theta & t_y\\ 0& 0& 1\end{array}\right]$$

于是：

$${\overline{p}}_W=T_{WB}{\overline{p}}_B$$

矩阵乘法展开就是：

$$\left[\begin{array}{ccc}\cos \theta & -\sin \theta & t_x\\ \sin \theta & \cos \theta & t_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_B\\ y_B\\ 1\end{array}\right]=\left[\begin{array}{c}\cos \theta x_B-\sin \theta y_B+t_x\\ \sin \theta x_B+\cos \theta y_B+t_y\\ 1\end{array}\right]$$

4.1 具体传入什么矩阵，结果怎样变化

初始示例的输入不是抽象符号，而是以下数据：

数据	shape	数值	物理含义
T_W_B	[3,3]	旋转和平移组成的齐次矩阵	B 坐标系在 W 中的位姿
p_B	[3]	[0.8,0.4,1.0]	点在 B 系中的齐次坐标
p_W	[3]	矩阵乘法结果	同一个点在 W 系中的坐标

当 $t_x=0.8,t_y=0.55,\theta ={20}^{\circ }$ 时：

$$T_{WB}=\left[\begin{array}{ccc}0.9397& -0.3420& 0.8000\\ 0.3420& 0.9397& 0.5500\\ 0& 0& 1\end{array}\right],{\overline{p}}_B=\left[\begin{array}{c}0.8\\ 0.4\\ 1\end{array}\right]$$

逐行相乘得到：

$${\overline{p}}_W=T_{WB}{\overline{p}}_B=\left[\begin{array}{c}1.4149\\ 1.1995\\ 1\end{array}\right]$$

点击“右移”后，只把 T_W_B[0,2] 从 0.8 改成 0.9，所以输出变成 [1.5149,1.1995,1]；旋转矩阵和 y 坐标都没有变化。点击“左转”把角度改为 ${30}^{\circ }$ 后，矩阵左上角四项一起改变，输出变为约 [1.2928,1.2964,1]。

下面代码把 shape、方向和检查条件都写在变量名及注释里：

import numpy as np  # NumPy 用于矩阵构造、乘法和数值校验


def make_transform_w_b(tx: float, ty: float, theta_deg: float) -> np.ndarray:
    """返回把 B 系坐标变到 W 系的 3×3 齐次矩阵。"""
    theta = np.deg2rad(theta_deg)  # 角度转弧度，三角函数不能直接使用 20
    c, s = np.cos(theta), np.sin(theta)  # 两个标量
    return np.array([
        [c, -s, tx],  # 第一行计算世界系 x
        [s,  c, ty],  # 第二行计算世界系 y
        [0.0, 0.0, 1.0],  # 齐次行让平移也能参与矩阵乘法
    ], dtype=np.float64)  # shape=[3,3]


T_W_B = make_transform_w_b(0.8, 0.55, 20.0)  # 输入位姿 → [3,3]
p_B = np.array([0.8, 0.4, 1.0], dtype=np.float64)  # shape=[3]
p_W = T_W_B @ p_B  # [3,3] @ [3] → [3]

T_B_W = np.linalg.inv(T_W_B)  # 求逆后，方向变为 W → B
p_B_recovered = T_B_W @ p_W  # 正变换结果再逆变换

print("T_W_B shape:", T_W_B.shape)  # (3,3)
print("p_W:", p_W)  # [1.414946...,1.199495...,1.0]
assert np.allclose(T_W_B @ T_B_W, np.eye(3), atol=1e-9)  # T×T⁻¹≈I
assert np.allclose(p_B_recovered, p_B, atol=1e-9)  # 必须还原原始输入点

注意：不要依赖“矩阵能相乘”判断方向正确

3×3 @ 3×3 在方向写反时仍能正常运行，不会报 shape 错误，但物理含义已经错了。必须同时在变量名中写出源/目标坐标系，并用已知点、单位矩阵和正逆闭环验证。

齐次矩阵的真正价值是可以拼接多段变换。例如末端 E 相对基座 B，基座 B 相对世界 W：

$$T_{WE}=T_{WB}{T}_{BE}$$

右边的变换先作用，左边的变换后作用。矩阵乘法通常不可交换，所以 $T_{WB}{T}_{BE}$ 不能写成 $T_{BE}{T}_{WB}$。

5. 逆变换：从世界坐标还原到机器人坐标

已知：

$$p_W=R{p}_B+t$$

先把平移移到等式左边：

$$p_W-t=R{p}_B$$

旋转矩阵是正交矩阵，因此：

$$R^{-1}=R^T$$

两边左乘 $R^T$，得到：

$$p_B=R^T(p_W-t)$$

把它写成齐次矩阵：

$$T_{BW}=T_{WB}^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0& 1\end{array}\right]$$

二维展开为：

$$T_{BW}=\left[\begin{array}{ccc}\cos \theta & \sin \theta & -\cos \theta t_x-\sin \theta t_y\\ -\sin \theta & \cos \theta & \sin \theta t_x-\cos \theta t_y\\ 0& 0& 1\end{array}\right]$$

注意逆矩阵的平移部分不是简单写成 $[-t_x,-t_y{]}^T$。必须先用 $-R^{T}t$ 旋转回 B 系，这正是初学时最容易出错的地方。

验证正逆变换是否写对，可以检查：

$$T_{WB}{T}_{BW}=I$$

以及对任意点：

$$T_{BW}(T_{WB}{\overline{p}}_B)={\overline{p}}_B$$

6. 交互演示：按钮如何对应数学公式

在演示中，绿色 W 系固定不动，蓝色 B 系代表机器人：

操作	改变的变量	公式中变化的部分
左移 / 右移	$t_x$	正变换矩阵第三列第一行
上移 / 下移	$t_y$	正变换矩阵第三列第二行
左转 / 右转	$\theta$	$\cos \theta$ 和 $\sin \theta$ 四个元素
点左移 / 点右移	$p_B$	输入点坐标，不改变坐标系位姿
重置	全部参数	恢复初始示例数值

例如初始状态 $t_x=0.80$、$t_y=0.55$、$\theta ={20}^{\circ }$，B 系中的点为 $p_B=(0.80,0.40)$。正变换计算为：

$$\begin{array}{rl}{x}_W& =\cos {20}^{\circ }\times 0.80-\sin {20}^{\circ }\times 0.40+0.80\\ y_W& =\sin {20}^{\circ }\times 0.80+\cos {20}^{\circ }\times 0.40+0.55\end{array}$$

点击“右移”后，只有 $t_x$ 增加 $0.10$，所以 $x_W$ 也增加 $0.10$，旋转项和 B 系点坐标不变。点击“左转”后，$\theta$ 增加 ${10}^{\circ }$，$\cos \theta$、$\sin \theta$ 同时变化，点在 W 系中的 x、y 会一起变化。

演示还会把 $p_W$ 再代入逆变换，显示：

$$T_{BW}{\overline{p}}_W={\overline{p}}_B$$

如果还原结果与原始 $p_B$ 一致，说明正变换和逆变换形成了闭环。

7. 三维机器人如何推广

三维情况下，二维的 $R(\theta )$ 变成 3×3 旋转矩阵，平移向量变成三维：

$$T_{AB}=\left[\begin{array}{cc}{R}_{3\times 3}& t_{3\times 1}\\ 000& 1\end{array}\right]$$

点的正变换仍然是：

$$p_A=R_{AB}{p}_B+t_{AB}$$

逆变换仍然是：

$$p_B=R_{AB}^T(p_A-t_{AB})$$

7.1 矩阵相乘得到角度，为什么得不到力矩

要先区分矩阵在描述什么：

• $R_1{R}_2$ 组合的是两次旋转，结果仍是一个旋转矩阵，表示新的姿态。
• $T_1{T}_2$ 组合的是两段刚体变换，结果表示新的位姿。
• $J(q)\dot{q}$ 把关节速度映射成末端速度。
• $J(q{)}^{T}F$ 把末端力映射成关节力矩。
• 动力学方程才把质量、惯量、重力、加速度和外力组合成驱动力矩。

二维旋转都绕同一个 Z 轴，因此：

$$R({\theta }_1)R({\theta }_2)=R({\theta }_1+{\theta }_2)$$

这时可以说组合角度是 ${\theta }_1+{\theta }_2$。但三维旋转通常绕不同轴，$R_x{R}_y\ne R_y{R}_x$；结果应理解为一个新姿态，不能随意把三个欧拉角逐项相加。若要显示欧拉角，需要从最终旋转矩阵重新分解，还要处理角度跳变和万向节锁。

位置、方向和力也不能使用完全相同的公式：

$$\begin{array}{rlrl}{p}_A& =R_{AB}{p}_B+t_{AB}& & \text{点：受平移影响}\\ v_A& =R_{AB}{v}_B& & \text{方向向量：不加平移}\\ F_A& =R_{AB}{F}_B& & \text{力：只旋转表达方向}\\ {\tau }_A& =R_{AB}{\tau }_B+t_{AB}\times (R_{AB}{F}_B)& & \text{换参考点后的力矩}\end{array}$$

最后一行的叉乘非常重要。同一股力若作用点离新原点更远，关于新原点的力矩就会改变；直接拿齐次位姿矩阵乘 [Fx,Fy,Fz] 是错误的。机器人软件中常把三维力和三维力矩组成 6 维 wrench=[F_x,F_y,F_z,\tau_x,\tau_y,\tau_z]，通过伴随矩阵的对偶变换，而不是用点坐标公式。

关节力矩通常由：

$$\tau =M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)+J(q{)}^T{F}_{external}+{\tau }_{friction}$$

计算或由底层控制器间接产生。矩阵乘法即使数学正确，也仍可能遇到这些物理问题：

1. 接近奇异位形时，雅可比矩阵病态，小末端速度/力需求可能对应很大的关节速度或力矩。
2. 轨迹时间太短时，所需加速度和惯性力矩会迅速增大。
3. 负载质量、质心或惯量写错时，重力补偿和前馈力矩会错误。
4. 力矩超过电机、减速器或驱动器上限时，会过流、发热、饱和或跟踪失败。
5. 位置目标可达不代表路径无碰撞，也不代表每个中间姿态都满足关节限制。

注意：位姿正确不等于动作安全

正运动学只证明“给定角度会到哪里”，不会检查达到该角度需要多大速度、加速度和力矩。真机命令还必须经过轨迹规划、奇异性检查、碰撞检查以及关节位置/速度/加速度/力矩限制。

这就是 ROS2 tf2、相机手眼标定、机械臂末端位姿和视觉抓取中反复出现的基本公式。区别只在于三维旋转可以用旋转矩阵、欧拉角或四元数表示；工程实现通常优先使用四元数或矩阵，避免欧拉角的万向节锁问题。

8. 自学时的验证清单

每次写坐标变换代码，都用下面的顺序自检：

1. 画出所有坐标系的原点和 x/y/z 轴，并写清右手系方向。
2. 明确 $T_{AB}$ 的含义：输入在哪个系，输出在哪个系。
3. 用一个平移为零、旋转为零的简单例子检查单位矩阵行为。
4. 检查 $R^{T}R=I$ 和 $det(R)=1$。
5. 检查 $T{T}^{-1}$ 是否接近单位矩阵。
6. 用已知点做正变换，再用逆变换还原，记录最大误差。
7. 统一单位，特别是米/毫米、角度/弧度和时间戳。

只要把“坐标系、方向、单位、矩阵顺序、逆变换”这五件事写清楚，后面的相机标定、手眼标定、视觉跟随和机械臂抓取就有了可靠基础。