状态：草稿 (2026-10-18)，实测。 数字是通过笔记本电脑（Apple M1 Max，macOS 26.5）上的 Python 3.9.6 和 numpy 2.0.2 直接计算得出的。 时间为重复 7 次的中位数（舍弃第一次）。原始数据与代码：工作目录
measure/robot-arm-basics/中的m02_frames.py,m02b_float32.py,out/m02_frames.json。未进行使用实际相机的标定实验（未实测）。参考资料查阅于 2026-10-18。 编写规则：以实测数据为基础，无法测量的标记为「未实测」，未能确认的标记为「未确认」。
目录
1. 直觉：同一个球，四种描述
一个球悬在空中。相机说：「在我的镜头前方 3.1 m，偏左下方」。以乒乓球桌为基准，则是：「从中心向机械臂方向 0.8 m， 高度 25 cm」。以机械臂底座为基准则是：「向前 0.9 m，向上 0.4 m」，而以球拍为基准则是：「拍面正前方 3 cm」。所有这些都是同一个点。
机器人软件中的许多 bug 都源于混用这四种描述。因此，我们为每个坐标系（frame）命名，并且在移动点时 必须通过变换来实现。本系列使用以下四个名称。
| 名称 | 原点与轴 | 谁来定义 |
|---|---|---|
| {C} 相机 | 镜头中心，z 轴为视线方向 | 相机安装 |
| {T} 乒乓球桌 | 台面中心，x 轴宽度·y 轴长度·z 轴向上 | ITTF 规范与 DeepMind 数据集惯例 |
| {B} 机械臂底座 | 第一关节轴上方 | 机械臂安装 |
| {E} 球拍 | 拍面中心，z 轴为拍面法线 | 正向运动学（第 3 篇） |
2. 图解：坐标系链
- 每一个箭头代表一个 4×4 变换矩阵。相机→乒乓球桌通过标定（calibration）求得一次，乒乓球桌→机械臂底座在安装时测量， 而机械臂底座→球拍则在每次关节角度改变时通过正向运动学重新计算。
- 符号 ᴮT_T 表示「将 {T} 中表达的量转换为在 {B} 中表达的变换」。前一个符号的下标和下一个符号的上标必须匹配才能相乘， 因此遵循此符号约定可以避免搞错相乘顺序。
3. 计算
3.1 旋转矩阵
旋转矩阵（rotation matrix）R 是一个 3×3 矩阵， 其每一列都是旋转后的坐标系的轴在原坐标系中的单位向量。因此它总是 满足以下两个性质。
Rᵀ R = I (열들이 서로 수직이고 길이가 1)
det R = +1 (오른손 좌표계를 유지, -1 이면 거울상)
绕 z 轴旋转 θ 角的旋转如下所示。连续进行两次旋转只需将矩阵相乘，若要撤销旋转只需进行转置（transpose）。 无需单独计算逆矩阵。
Rz(θ) = [ cos θ −sin θ 0 ]
[ sin θ cos θ 0 ]
[ 0 0 1 ]
3.2 齐次变换
为了同时处理旋转和平移，我们使用 4×4 矩阵。这被称为齐次变换（homogeneous transformation）。我们在点的第四个分量上附加 一个 1。
T = [ R p ] 점 변환: [x'] = T [x]
[ 0 1 ] [1 ] [1]
역변환: T⁻¹ = [ Rᵀ −Rᵀ p ]
[ 0 1 ]
逆变换也不是通过一般的求逆矩阵计算，而是通过上述公式计算。在数值上更加安全且高效。
3.3 欧拉角
便于人类阅读的姿态表示法是欧拉角（Euler angles）。机器人学和航空航天中常见的 ZYX 顺序（偏航 yaw、俯仰 pitch、横滚 roll）是
R = Rz(요)·Ry(피치)·Rx(롤) 。从旋转矩阵中反向提取时，使用以下公式（r_ij 表示第 i 行第 j 列）。
피치 = asin(−r31)
요 = atan2(r21, r11)
롤 = atan2(r32, r33)
当俯仰角达到 ±90° 时，r11、r21、r32 和 r33 均变为 0，导致偏航角和横滚角无法独立确定。两个旋转轴重合导致自由度丢失一个， 这被称为万向锁（gimbal lock）。不仅在恰好 90° 时会出现问题， 在临近区域噪声会剧烈放大这一事实 在实际中更为重要。我们将在 4.3 节进行实测。
3.4 四元数
单位四元数（unit quaternion）q = (w, x, y, z) 通过旋转轴 k 和旋转角 θ 表示为 q = (cos θ/2, k·sin θ/2) 。它用四个数字表示旋转，
没有万向锁，并且易于在两个旋转之间进行平滑插值。不过，必须时刻记住 q 和 −q 表示相同的旋转（双重覆盖）。
如果在计算姿态误差时不统一符号，相近的两个姿态会被计算为相差将近 360°。
在本系列的代码中，四元数乘法只是一个纯 Python 函数。
def qmul(a, b):
aw, ax, ay, az = a
bw, bx, by, bz = b
return (aw*bw - ax*bx - ay*by - az*bz,
aw*bx + ax*bw + ay*bz - az*by,
aw*by - ax*bz + ay*bw + az*bx,
aw*bz + ax*by - ay*bx + az*bw)
3.5 从相机到乒乓球桌：通过标记点求解姿态
通过两个相机进行三角测量可以获得相机坐标系 {C} 中的三维点。如果在相机中也能测量到 N 个已知乒乓球桌坐标 {T} 的点（如乒乓球桌的边缘）， 就可以求出能让两组点完美重合的 R 和 p。该方法是去除两组点中心后对协方差矩阵进行奇异值分解（SVD）， 通常被称为 Kabsch 算法。
def kabsch(P, Q): # Q ≈ R·P + t 가 되는 R, t (P, Q 는 N×3)
pc, qc = P.mean(0), Q.mean(0)
U, _, Vt = np.linalg.svd((P - pc).T @ (Q - qc))
d = np.sign(np.linalg.det(Vt.T @ U.T)) # 거울상(det = -1) 방지
R = Vt.T @ np.diag([1, 1, d]) @ U.T
return R, qc - R @ pc
d 如果漏掉了用于修正符号的代码行，当点几乎共面时（乒乓球桌台面正是如此），可能会产生镜像矩阵。
4. 实测
4.1 长期相乘旋转会导致正交性崩溃吗
类似于陀螺仪积分，我们持续相乘微小的旋转（0.06~0.6°），并测量 RᵀR 偏离单位矩阵的程度。同样的旋转序列也通过四元数进行了相乘。
| 相乘次数 | float64 矩阵 ‖RᵀR − I‖ | float64 四元数 |‖q‖ − 1| | float32 矩阵 | float32 四元数 |
|---|---|---|---|---|
| 1,000 | - | - | 7.8×10⁻⁶ | 2.8×10⁻⁶ |
| 10,000 | - | - | 2.9×10⁻⁵ | 5.6×10⁻⁶ |
| 100,000 | 4.7×10⁻¹³ | 2.5×10⁻¹⁴ | 2.6×10⁻⁴ | 8.6×10⁻⁵ |
| 1,000,000 | 4.4×10⁻¹² | 3.1×10⁻¹⁴ | （未测量） | （未测量） |
如果在 float64 矩阵每相乘 1,000 次后通过 SVD 将其重置为最近的旋转矩阵，即使在一百万次之后误差也仅为 1.2×10⁻¹⁵。
有两种解读。首先， 在 PC 的 float64 下几乎不必担心漂移问题。 一百万次相当于 1 kHz 控制下的 17 分钟。其次， 在使用 float32 的 微控制器上情况则不同。 十万次（1 kHz 下 100 秒）产生的 2.6×10⁻⁴ 误差相当于轴扭曲了约 0.015°，并且会持续增长。 像电机控制器或 IMU 固件这样使用 float32 累积旋转的代码必须定期重新归一化。四元数只需要将长度除以 1 即可，因此这比矩阵更容易处理。
4.2 计算时间
| 运算 | 单次（µs） |
|---|---|
| numpy 3×3 矩阵乘法 | 0.68 |
| numpy 4×4 矩阵乘法 | 0.66 |
| numpy 3×3 矩阵 × 向量 | 0.60 |
| 纯 Python 四元数乘法 | 0.55 |
| 纯 Python 四元数旋转向量 | 1.33 |
无论是 3×3 还是 4×4，numpy 的耗时都在 0.6~0.7 µs 左右，几乎没有区别。这是因为在此规模下，函数调用的开销远大于乘法本身。 因此在 Python 中选择表示形式时，速度不应作为标准。只需根据万向锁、插值、归一化是否简便等性质进行选择即可。
4.3 万向锁附近的噪声放大
我们构造了一个 ZYX 姿态，并添加了大小为 0.0057°（10⁻⁴ rad）的小型随机旋转作为噪声，然后通过 3.3 节的公式重新提取角度。 在使俯仰角逐渐接近 90° 的过程中，每种情况各尝试了 2,000 次。
| 俯仰角与 90° 的差值 | 偏航·横滚误差中位数 | 95 百分位数 |
|---|---|---|
| 30° | 0.007° | 0.011° |
| 10° | 0.017° | 0.032° |
| 3° | 0.058° | 0.10° |
| 1° | 0.17° | 0.31° |
| 0.3° | 0.54° | 1.0° |
| 0.1° | 1.6° | 3.1° |
| 0.03° | 5.4° | 10.5° |
由于在对数坐标轴上的斜率接近 1，误差大致按「噪声 ÷ 距离 90° 剩余的角度（弧度）」的速度增长。乒乓球拍的拍面经常近乎垂直地竖起或放平， 因此会频繁经过俯仰角 90° 附近。 切勿将球拍姿态以欧拉角形式存储或进行滤波，最好将内部表示保留为旋转矩阵或四元数，仅在向人类展示时才转换为欧拉角，这样更为安全。
4.4 通过标记点求解相机姿态
我们将相机放置在机械臂侧底线后方 2 m、高 1.6 m 处，使其俯视乒乓球桌中心。我们以乒乓球桌的四个角、网柱上方的两个点以及中线两端作为基本标记， 如果需要更多点，则在乒乓球桌面上添加任意点。为相机坐标中的每个点添加高斯噪声 σ，并使用 3.5 节的函数求解姿态。 这是每种条件下 500 次试验的中位数。
| 点数 | σ | 旋转误差 | 相机位置误差 | 另一端底线点误差（中位数/95%） |
|---|---|---|---|---|
| 4 | 2 mm | 0.073° | 3.7 mm | 1.9 / 3.6 mm |
| 4 | 5 mm | 0.20° | 9.4 mm | 4.9 / 8.5 mm |
| 8 | 2 mm | 0.061° | 2.9 mm | 1.4 / 2.6 mm |
| 8 | 5 mm | 0.15° | 7.2 mm | 3.6 / 6.9 mm |
| 8 | 10 mm | 0.31° | 15.1 mm | 7.1 / 13.7 mm |
| 32 | 5 mm | 0.10° | 4.7 mm | 2.1 / 3.8 mm |
- 相机位置误差大于乒乓球桌面上点的误差。 由于相机距离标记点中心超过 3 m，微小的旋转误差会被像杠杆一样放大，但我们关注的不是相机本身，而是乒乓球桌上的球的位置。 由于球在标记点附近飞行，误差得以保持在较小范围内。
- 将点数从 8 个增加到 32 个时，误差缩减了 1.7 倍，而不是大约 √4 = 2 倍。我们推测这是因为随意添加的点仅位于乒乓球桌面平面上， 其包含的信息量不如边缘处分布广泛的点多（推测）。
- 第 1 篇的时间预算中提及的球的位置噪声将在第 6 篇中探讨，但标定误差与每帧的噪声不同，它是 在所有帧中朝同一方向残留的偏差。 卡尔曼滤波可以减小噪声，但无法消除偏差。
5. 局限性
- 我们假设已经通过双相机三角测量获得了三维点。未涉及从单相机图像坐标求解姿态的 PnP 问题、镜头畸变以及双相机之间的时间偏差（未实测）。
- 噪声被均匀施加到各个轴上。实际的三角测量噪声在相机视线方向上要大得多（未实测）。
6. 总结
- 为每个坐标系命名，并在变换中标注「从哪里到哪里」。旋转的逆是转置，齐次变换的逆有专用的计算公式。
- 内部计算使用旋转矩阵或四元数。欧拉角会使俯仰角 90° 附近的噪声放大数百倍。
- 如果使用 float32 长期累积旋转，则需要进行归一化。而在 float64 下，即使经过一百万次运算，误差也保持在 10⁻¹² 级别。
- 8个标记点，点噪声为 5 mm 时，乒乓球台上的位置误差为几毫米。此误差是每个帧中残留的相同偏差，无法通过滤波器消除。
乒乓球小结。 同样的球，根据观察者的不同，坐标也会发生变化。当球拍竖起或放倒时，千万不要用欧拉角，而要紧紧掌握四元数！
参考
所有资料均于 2026-10-18 查阅。
- Lynch, Park, Modern Robotics: Mechanics, Planning, and Control, Cambridge University Press 2017, 公开预印本（2017年5月）, 第 3 章 刚体运动。
- ITTF, Statutes 2026, 2.1.1·2.2.2（乒乓球台及网柱位置）。
- Google DeepMind, competitive_robot_table_tennis, 坐标系说明 (README), CC-BY 4.0。