机器人基础知识 · 第 4 篇
阅读准备：知道坐标系、旋转矩阵与矩阵乘法即可。本文接着第二篇的坐标表达，进一步讨论“如何改变位姿”和“如何描述位姿误差”。所有数值均为教学设定，不代表八界实机参数或测试结果。

给机械臂一个指令：“沿 x 方向前进 10 厘米。”

这句话还不能直接执行。如果工具已经转了 90°，世界的 x 方向与工具的 x 方向就不再重合。同样三个平移数值，放在矩阵左边和右边，会让工具朝不同方向走。

另一个更隐蔽的问题是：当前姿态和目标姿态各有一个矩阵，把它们相减，是否就得到控制器需要的误差？把两者平均，是否就得到合法的中间姿态？

这些问题都与 SE(3) 的结构有关。理解它，不需要一开始就背李群定义。先抓住一件事：刚体可以移动和旋转，但不能因为我们做了一次数值运算，就被缩小、拉长或剪切。

图 1：基于八界 URDF 与作者提供的外观参考制作的概念示意，非实机拍摄。机器人结构以模型渲染底图约束，外壳材质由图像工具补充；画面中的几何平面不表示标定结果。

1. 4×4 矩阵里，为什么只有六个自由度

本文统一采用列向量，记 T_A_B 为“把 B 系坐标变到 A 系的变换”：

p_A = R_A_B · p_B + t_A_B

T_A_B = [ R_A_B   t_A_B ]
        [ 0 0 0     1   ]

[p_A; 1] = T_A_B · [p_B; 1]

这里 t_A_B 是 B 系原点在 A 系中的坐标；R_A_B 的三列，是 B 系三个单位轴在 A 系中的表达。平移用米，计算中的角度用弧度。[1][4]

一个 4×4 矩阵有 16 个数字，但合法刚体位姿不能随意填写：

RᵀR = I
 det(R) = +1
最后一行 = [0, 0, 0, 1]

RᵀR = I 要求三个轴长度为 1 且互相垂直；行列式为 +1 则排除镜像反射。旋转只有三个独立自由度，加上平移三个，一共六个。

满足这些约束的齐次变换构成 SE(3)，即三维特殊欧氏群。这里“群”最有用的工程含义是：两个合法变换相乘，结果仍然合法；一个合法变换求逆，仍然合法。[1]

求逆也不是把矩阵每个元素取负：

T⁻¹ = [ Rᵀ   −Rᵀt ]
      [ 0       1  ]

因为要撤销移动，先要把方向转回去，再在转回后的坐标中撤销平移。第二篇中的坐标链 T_A_C = T_A_B · T_B_C，依靠的正是这种可组合性。

还要区分“位置”和“方向”。齐次坐标中，位置写成 [p; 1]，方向写成 [v; 0]。因此平移会改变点的位置，但不会给一个方向向量额外加上位移。把法向量或速度方向误按位置处理，会无端改变它的方向与模长。

2. 左乘与右乘：不是语法细节，而是运动语义

设工具 B 在世界 A 中的位置为 (1.0, 0.5, 0.2) m，已经绕世界 z 轴转了 90°。此时工具的 x 轴指向世界的 y 轴。

现在定义一个只有平移的增量 ΔT，数值为 (0.2, 0, 0) m。

如果增量表达在世界系，更新是：

T_new = ΔT_world · T_current
新原点 = (1.2, 0.5, 0.2) m

如果同样的增量数值表达在当前工具系，更新是：

T_new = T_current · ΔT_body
新原点 = (1.0, 0.7, 0.2) m

矩阵乘在哪一边，决定增量属于哪个坐标系。 左乘在参考系中表达运动，右乘在当前物体系中表达运动。[1]

图 2：两种更新从同一原点出发，位移大小都是 0.20 m，方向却相差 90°。图中仅展示 xy 平面，z 坐标保持 0.20 m。

下面的代码可以直接验证。需要 Python 3 和 NumPy，无需 ROS 环境。

import numpy as np

T = np.eye(4)
T[:3, :3] = [[0, -1, 0], [1, 0, 0], [0, 0, 1]]
T[:3, 3] = [1.0, 0.5, 0.2]

D = np.eye(4)
D[:3, 3] = [0.2, 0.0, 0.0]

world_update = D @ T
body_update = T @ D
print("世界增量:", world_update[:3, 3])
print("工具增量:", body_update[:3, 3])
assert np.allclose(world_update[:3, 3], [1.2, 0.5, 0.2])
assert np.allclose(body_update[:3, 3], [1.0, 0.7, 0.2])

输出分别是 [1.2 0.5 0.2] 与 [1.0 0.7 0.2]。如果机器人朝“错误的方向”走，首先应检查增量的坐标语义，而不是马上调控制增益。

旋转更新还多一层容易忽略的含义。把一个绕世界原点的纯旋转矩阵左乘到 T 上，会同时旋转 R 和 t，原点可能绕世界原点走一段圆弧。它不等于“原地转动工具，位置不变”。

如果要求位置保持原值、仅改变朝向，应根据任务分别构造新的 R 和原来的 t，或者明确旋转中心再组合变换。轴方向和旋转中心是两件不同的事。

3. 当前位姿到目标位姿，应该怎样求误差

假设当前位姿为 T_current，目标为 T_target，两者都表达在同一个世界系。

若希望得到“站在当前工具系中，目标在什么位置、朝什么方向”，应计算：

E_body = T_current⁻¹ · T_target

从坐标链就能看懂：先由目标系走到世界系，再由世界系走回当前工具系。结果是目标系相对当前工具系的完整位姿。[3]

继续使用上一节的当前位姿。设目标相对当前工具系平移 (0.10, −0.02, 0) m，再具有绕局部 z 轴 20° 的相对朝向。对应目标原点在世界中是 (1.02, 0.60, 0.20) m。

于是有两种都正确、但含义不同的量：

世界中的位置差 = t_target − t_current
                = (0.02, 0.10, 0) m

E_body 的平移部分 = (0.10, −0.02, 0) m
E_body 的旋转角   = 20°

图 3：世界位置差与局部平移分量不是两次不同的移动，而是同一几何关系的两种表达。两者的位置差模长都约为 0.102 m。

为什么不直接做 T_target − T_current？矩阵相减能产生数字，也可以构造某种优化损失，但结果并不是一个刚体变换，更不会自动说明它是在哪个系中表达的六维运动误差。

相对旋转还解决了角度绕回的问题。当前偏航为 +179°，目标为 −179°，直接相减得到 −358°；相对旋转的最短转角却只有 2°。这是两种角度表示跨过了边界，并不意味着机器人要转一整圈。

判断“到位”时，位置和姿态也应分别处理。例如教学任务可定义：位置误差小于 2 mm，旋转误差小于 1°。这两个数只是阈值示例，真实取值应由装配公差、接触要求和感知能力决定。

不要把“0.002 米”和“1 度”直接塞进同一个未经加权的范数，再称之为总误差。若优化器需要统一代价，应说明单位、权重或特征长度的选择；换单位会改变未经规范化的数值比较。

4. 为什么矩阵平均不是合法的姿态插值

平移通常可以线性插值：t(s) = (1−s)t₀ + st₁。这很容易诱导我们对旋转矩阵做同样的事。

考虑从不旋转到绕 z 轴旋转 90°，把两个旋转矩阵各取一半：

M = 0.5 I + 0.5 Rz(90°)

M = [ 0.5  −0.5  0 ]
    [ 0.5   0.5  0 ]
    [  0     0   1 ]

它的前两个列向量长度为 √0.5 ≈ 0.707，行列式为 0.5。也就是说，它把 xy 平面里的物体缩小了。

import numpy as np

R0 = np.eye(3)
R1 = np.array([[0., -1., 0.], [1., 0., 0.], [0., 0., 1.]])
M = (R0 + R1) / 2
print("第一列长度:", np.linalg.norm(M[:, 0]))
print("行列式:", np.linalg.det(M))
print("仍然正交:", np.allclose(M.T @ M, np.eye(3)))

结果是 0.7071、0.5、False。这不是计算精度不够，而是运算本身离开了旋转矩阵所在的集合。

图 4：橙色形状是矩阵平均的结果，绿色形状是合法的 45° 旋转。二者朝向看起来接近，但只有后者保持刚体的尺寸。

工程中可以使用旋转指数映射，或对单位四元数做适当的球面插值。关键不在于选一个听起来高级的算法名，而在于每个中间状态都仍然是合法旋转，并且路径符合任务需要。

5. 六维小量为什么要经过 Exp，而不能直接加到矩阵上

控制和优化通常希望用六个数字表达一次调整：三个平移相关分量，三个旋转相关分量。但一个六维向量不能直接与 4×4 位姿矩阵相加。

常见办法是在 SE(3) 的局部线性空间中表达增量，再通过指数映射回到合法位姿。[2]

本文把六维向量按平移在前、旋转在后排列：

ξ = [ρx, ρy, ρz, φx, φy, φz]ᵀ

φ = θu
θ：旋转角，单位 rad
u：旋转轴的单位向量
ρ：平移相关的指数坐标，单位 m

这个排列不是全行业唯一约定。有的书和库把角分量放在前面，调用接口前必须核对。这里的 ξ 表示一个有限位姿增量的坐标；把它与“每秒的速度旋量”联系起来时，还需要明确时间参数与积分方式。

令 Φ = [φ]×，其中叉乘矩阵满足 Φv = φ × v。对应的 4×4 矩阵形式为：

ξ^ = [ Φ   ρ ]
     [ 0   0 ]

Exp(ξ^) 的结果仍然是合法齐次变换：

Exp(ξ^) = [ Exp(Φ)   Jₗ(φ)ρ ]
          [   0          1  ]

Jₗ(φ) = I + (1−cosθ)/θ² · Φ
          + (θ−sinθ)/θ³ · Φ²

这里 Jₗ 是 SO(3) 的左雅可比，用来描述旋转过程中平移方向的累计效应；它不是第三篇把关节速度映射到末端速度的机械臂雅可比。二者都与局部线性关系有关，但自变量和用途不同。

ρ 不等于最终的 t

令 ρ = (0.20, 0, 0) m，φ = (0, 0, π/2) rad。指数映射得到的终点平移是：

t = (0.1273, 0.1273, 0) m

不是 (0.20, 0, 0) m。直观原因是：若按这组固定旋量连续运动，平移方向会随着旋转改变，累计出来的是一段弧线。

图 5：绿色曲线由 Exp(sξ^) 计算，s 从 0 到 1。橙色箭头表示把 ρ 直接当作终点位移的错误解释；两者终点不同。

当 φ = 0 时，Jₗ = I，才退化为普通纯平移。极小角度附近，应使用级数或经过验证的数值实现，避免直接计算很小的分母造成消减误差。

反过来，Log(E) 可把相对位姿变为局部六维误差。前面例子的 E_body 平移为 (0.10, −0.02, 0)，但它的对数坐标中，ρ 约为 (0.09549, −0.03725, 0)。这两组数不相同，正是旋转与平移耦合的结果。

如果将该局部误差交给数值逆运动学求解器，还必须使用匹配表达坐标系与分量排列的雅可比。误差在工具系、雅可比在世界系，六维数值看起来都完整，也不能直接相乘。[3]

6. 合法位姿路径，距离可执行运动还有多远

一种常见的群上插值写法是：

E = T₀⁻¹ T₁
T(s) = T₀ · Exp(s · Log(E))，0 ≤ s ≤ 1

这里把 Log(E) 理解为对应的李代数矩阵。它保证中间点仍在 SE(3) 中，但不保证末端走直线，也不保证机械臂能够执行。

第三篇讨论过的条件仍然存在：每个位姿是否有满足关节范围的逆解，是否靠近奇异位形，是否发生碰撞，以及在给定时间内是否满足速度和加速度限制。几何参数 s 本身也不是时间；只有给出 s(t)，才能进一步讨论速度与加速度。

另外，接近 180° 旋转时，旋转轴存在符号和分支选择问题。单个姿态能正确重建，并不意味着一串姿态的对数坐标自动连续。需要连续控制时，应结合上一时刻的分支与轨迹语义处理，不能只看单帧结果。

所以，一个完整实现通常需要分别回答三件事：目标位姿是否合法，几何路径是否适合任务，机器人能否按指定时间执行。SE(3) 主要解决前两者中的几何表达问题。

7. 放到八界模型里，先检查哪几件事

对八界这样的移动操作机器人，坐标链会经过底盘、升降机构、机械臂和工具。阅读 URDF/Xacro 时，应把固定安装变换与关节运动区分开：

T_parent_child(q) = T_origin · T_joint(q)

关节值为零，表示相对关节参考状态的位移或转角为零，不代表父子坐标系必然重合。当前八界模型的头部关节还包含固定安装旋转；它只有俯仰自由度，不能因配图或控制需要凭空增加独立偏航。

排查一个方向错误的目标，可以沿着三个问题逐层检查：

• T_A_B 的方向到底是 B 到 A，还是 A 到 B？变量名和接口文档是否一致？
• 增量表达在世界、底盘还是工具系？乘法顺序是否与这个含义匹配？
• 误差与雅可比的参考系、分量顺序、单位是否完全一致？

本文随附 examples/04_rigid_transforms.py，实现了教学用 Exp/Log、左右乘对比、矩阵平均反例和相对误差演算。运行方式为：

python3 examples/04_rigid_transforms.py

04_rigid_transforms.py
代码验证了逆变换、点与方向的区别，以及 0°、极小角、20°、90°、179°、接近 180° 和 180° 等 21 组位姿重建。它是可复算的教学实现，不是八界控制接口，也不是可以直接接到机器人上的轨迹执行器。

欢迎关注科沃斯八界 AtomGit [5]，相关代码将逐步释放，实际开放内容与使用说明以仓库为准。

下一篇将进一步讨论：即使坐标变换写对了，传感器给出的点仍然会抖动和偏移。机器人应当怎样表达自己对这个点的“不确定程度”？

参考链接

[1] Modern Robotics：Homogeneous Transformation Matrices，齐次变换、组合以及左右乘的含义。
[2] Modern Robotics：Exponential Coordinates of Rigid-Body Motion，刚体运动的指数坐标。
[3] Modern Robotics：Numerical Inverse Kinematics, Part 2，相对位姿、对数映射与数值逆运动学。
[4] ROS REP-103 官方源文档：Standard Units of Measure and Coordinate Conventions，单位与坐标约定。
[5] 科沃斯八界 AtomGit，项目代码关注入口，相关代码逐步释放。