从 AprilTag 到世界坐标：读懂四足机器人接球论文的定位方案
在机器人接球系统中，球的位置和机器人的位置必须落在同一个坐标系里，控制器才能判断该往哪里移动。相机看到标签只是第一步：标签相对于相机在哪里，相机相对于场地在哪里，标签又对应机器人身上的哪个点，这些关系都需要说清楚。
本文讨论论文 Spatiotemporal Agility: Time-Constrained Reinforcement Learning for Vision-Guided Dynamic Quadrupedal Interception（arXiv:2608.06907v1）中的定位方案，重点回答两个问题：外部鱼眼相机与 AprilTag 如何提供世界坐标下的位置？论文是否明确了机器人参考点，以及参考点到深度相机的坐标变换？
先给出结论：论文中的鱼眼相机和 RealSense D415 都是固定的外部相机；机器人通过背部 AprilTag 被定位。论文没有交代标签到机身参考点的完整外参，也没有提出机载深度相机的标定方案。
以下引文均来自 v1 PDF，页码按该文件标注。涉及 PnP 和变换矩阵连乘的展开，是对方案的几何解释，不代表论文公开了这些实现细节。
1. 先看清实验布置：D415 不在机器人身上
论文第 4.1 节 System Setup 对相机布置有直接说明：
The perception system utilizes two stationary cameras, explicitly forming a decoupled, off-board perception setup.
中文释义：感知系统使用两台固定相机，构成与机器人本体分离的外部感知系统。
同节随后写道：
An overhead RGB fisheye camera, calibrated for undistortion, provides a global view for localizing the robot via an AprilTag on its back. A second Intel RealSense D415 camera is positioned near the ball’s release point, with its field of view encompassing the flight path and the base coordinate system defined by the AprilTag.
中文释义：一台经过标定、用于去畸变的俯视 RGB 鱼眼相机提供全局视野，通过机器人背部的 AprilTag 定位机器人。另一台 Intel RealSense D415 放在球的释放点附近，视野覆盖球的飞行路径，以及由 AprilTag 定义的基准坐标系。
出处：第 9 页，§4.1、图 3。
这套装置的分工可以整理为：
| 设备或标记 | 布置位置 | 作用 |
|---|---|---|
| RGB 鱼眼相机 | 场地上方，固定安装 | 观察背部标签，定位机器人 |
| RealSense D415 | 抛球点附近，固定安装 | 获取球的图像和深度信息 |
| 地面 AprilTag | 地面基准处 | 定义世界坐标基准 |
| 机器人 AprilTag | 机器人背部 | 提供机器人的位置观测 |
| Jetson Orin Nano | 机器人上 | 运行运动控制策略 |
图 3 的实验照片也与上述描述一致：D415 和鱼眼相机都安装在机器人之外。因此，不能把系统理解成“鱼眼相机先定位机器人，再推算机载 D415 的位置”。论文采用的是两台外部相机各自观测不同对象，再把测量结果统一到同一个世界坐标系。
这里的 “base coordinate system” 也不应直接理解成机器人的 <code>base_link</code>。结合第 3.1.1 节，它指的是 AprilTag 定义的场地坐标基准；这句话并未定义机器人机身坐标系。
2. 地面标签如何建立世界坐标基准？
第 3.1.1 节 Globally Coordinated Multi-Camera Framework 写道：
The ground AprilTag defines the world origin, while the onboard AprilTag provides robot position states …
中文释义：地面 AprilTag 定义世界原点，而机器人上的 AprilTag 提供机器人位置状态。省略处是论文使用的位置变量。
出处：第 4 页，§3.1.1。
紧接着，论文用式（1）表示从相机坐标到世界坐标的转换。用便于阅读的记号写成：
$$
\mathbf p{\mathrm{world}} = T{\mathrm{world},\mathrm{cam}}\,\mathbf p_{\mathrm{cam}}
$$
其中，变换不仅要处理原点的平移，还要处理坐标轴方向的旋转。若使用包含旋转和平移的 4×4 齐次变换矩阵，式中的点应写成齐次坐标；若点保留为三维向量，则应展开成“旋转矩阵乘以点，再加平移向量”。
论文还解释了如何缓解标签估计的帧间抖动：
To deal with the jitter of AprilTag between frames and raise the agility of our project, we average the basis vectors and origin position across a sliding window before orthonormalization. This yields a temporally stable transformation matrix.
中文释义：为处理 AprilTag 在相邻帧之间的抖动，作者先在滑动窗口内对坐标基向量和原点位置求平均，再做正交归一化，从而得到随时间更稳定的变换矩阵。
这里平均的不只是位置，还包括描述坐标轴方向的基向量。方向向量直接平均后，通常不再满足单位长度和相互正交的要求，因此还需要正交归一化。论文说明了这个处理思路，但没有给出滑动窗口长度、标签尺寸或完整的外参标定流程。
3. 从鱼眼图像到机器人标签的三维位置
下面把论文简写的坐标关系展开。这是标准几何推导，不是对作者具体代码的还原。
3.1 统一变换记号
本文使用以下坐标系：
| 记号 | 含义 |
|---|---|
| $W$ | 世界坐标系，由地面标签建立基准 |
| $F$ | 鱼眼相机坐标系 |
| $A$ | 机器人背部 AprilTag 坐标系 |
| $B$ | 机器人机身参考坐标系 |
| $D$ | D415 相机坐标系 |
记 $T{XY}$ 为“把 $Y$ 坐标系中的坐标转换到 $X$ 坐标系”的刚体变换。它由旋转 $R{XY}$ 和平移 $\mathbf t_{XY}$ 组成。对三维点，变换关系为：
$$
\mathbf pX = R{XY}\,\mathbf pY + \mathbf t{XY}
$$
其中 $\mathbf t_{XY}$ 就是 $Y$ 坐标系原点在 $X$ 坐标系中的位置。
3.2 没有深度的 RGB 相机，为什么也能估计三维位姿？
关键在于 AprilTag 的几何尺寸已知。
一个标签的四个角点，在标签自身坐标系中的位置可以由其实际边长确定。相机检测出这四个角点的像素坐标后，结合相机内参和畸变模型，就可以通过 PnP 或等价的平面标签位姿算法，估计标签相对于相机的旋转和平移。
因此，一种标准实现是：
- 标定鱼眼相机，处理图像或角点坐标的畸变。
- 检测地面标签和背部标签的四个角点。
- 使用各标签已知的实际尺寸，分别求解它们相对于相机的位姿。
在将地面标签坐标系选作世界系的约定下，这会得到 $T{FW}$ 和 $T{FA}$：前者描述世界基准在鱼眼相机中的位姿，后者描述背部标签在鱼眼相机中的位姿。如果另设世界轴方向或原点偏移，还需加入相应的固定变换。
单目相机能够恢复有尺度的三维位姿，依赖的是已知标签尺寸与相机模型。只知道标签中心落在哪个像素上，并不足以确定它在三维空间中的位置。论文没有说明具体用了哪一种位姿求解器，不能把 PnP 的某个实现当成作者已公开的做法。
3.3 由共同观测相机连接两个坐标系
拿到上述两个位姿后，背部标签在世界坐标系中的位姿为：
$$
T{WA} = T{FW}^{-1}\,T_{FA}
$$
按从右向左的顺序理解，就是：
背部标签坐标 → 鱼眼相机坐标 → 世界坐标结果矩阵的平移部分 $\mathbf t{WA}$，是背部标签坐标系原点的世界三维位置；旋转部分 $R{WA}$，是标签相对于世界系的朝向。
注意，此时得到的是背部标签的位姿。要把它称为机身中心或其他机器人参考点的位姿，还差一段安装外参。
4. 外部 D415 如何使用同一个世界坐标系？
第 4.1 节已经说明，D415 的视野覆盖球的飞行路径和 AprilTag 定义的基准坐标系。这为将球的观测变换到世界系提供了共同基准，但论文没有逐步描述 D415 外参的求解过程。
与该布置相符的一种标准实现，是让 D415 也观测地面基准标签，得到 $T_{DW}$，再取逆：
$$
T{WD} = T{DW}^{-1}
$$
其中，$\mathbf t{WD}$ 就是 D415 光学坐标系原点在世界系中的三维位置，$R{WD}$ 则描述相机朝向。
当 D415 由图像和深度得到球在自身坐标系中的三维位置后，便可转换到世界系：
$$
\mathbf p{\mathrm{ball},W} = R{WD}\,\mathbf p{\mathrm{ball},D} + \mathbf t{WD}
$$
这样，球和机器人标签的位置就可以在同一个坐标系中比较。第 3.1.4 节进一步说明，感知模块输出机器人世界位置、预测落点和预计落地时间；机器人位置与落点之差，以及时间信息，经 LCM 发送给机载 Jetson Orin Nano，作为控制策略的一部分输入。第 6 页，§3.1.4
这条变换链不要求鱼眼相机看见 D415，也不需要先知道机器人在哪里。两台相机通过同一个世界基准建立联系。相机与地面基准的安装关系不变时，D415 的世界位姿也是固定的。
实现时还要核对坐标系：标签位姿估计所对应的相机光学坐标系，必须与球的三维点所用的坐标系一致；若不一致，还要串接它们之间的外参。论文没有展开这些细节，不能据此认定所有光学坐标系已经自动对齐。
5. “机器人位置”究竟指机器人身上的哪个点？
这是论文没有充分交代的地方。
从第 3.1.1 节的“机器人上的 AprilTag 提供位置状态”和第 4.1 节的“通过背部 AprilTag 定位机器人”，能够确认直接观测对象是背部标签。按照常见的标签位姿约定，输出位置对应标签坐标系原点，通常位于标签中心。但这是常见实现约定，不能替代论文对机器人参考点的定义。
需要区分以下几件事：
| 问题 | 论文提供的信息 |
|---|---|
| 标签装在哪里？ | 机器人背部 |
| 标签是否用于机器人定位？ | 是 |
| 论文中的机器人位置是否严格等于标签中心？ | 未明确说明 |
| 是否换算到了机身几何中心、质心或 <code>base_link</code>？ | 未说明 |
| 是否换算到了接球篮筐中心？ | 未说明 |
| 是否公开了标签到上述参考点的旋转和平移？ | 未给出 |
如果控制器需要机身参考坐标系 $B$ 的位姿，就应补上标签与机身之间的关系：
$$
T{WB} = T{WA}\,T_{AB}
$$
其中 $T_{AB}$ 是机身参考坐标系在标签坐标系中的位姿；对于刚性固定的标签和机身参考坐标系，它是固定安装外参。
论文没有给出这段外参的数值、测量方式或标定方法。因此，既不能断言论文以机器人质心为定位基准，也不能反过来断言作者完全没有补偿标签偏移。能从文章中确认的是：这段关系没有被说明。
6. 如果把深度相机改成机载，变换链会怎样？
论文没有给出“机器人参考点到机载 D415”的固定变换，因为实验中的 D415 并不安装在机器人上。机器人在场地内运动时，它与外部 D415 的相对位姿会变化，不存在可直接套用的固定机身安装外参。
第 3.1.1 节对外部感知方案的限制也有明确表述：
Compared to an onboard binocular depth camera, this design provides stable off-board localization in our workspace, although it constrains deployment to the external-camera field of view.
中文释义：与机载双目深度相机相比，这一设计在实验工作空间中提供了稳定的外部定位，但部署范围受到外部相机视野的限制。
出处：第 5 页，§3.1.1。
论文结论还把以下方向列为未来工作的一部分：
… enabling onboard perception to remove the external camera restriction …
中文释义：引入机载感知，以摆脱外部相机的限制。
出处：第 14 页，§5。这里描述的是未来工作，不能当作论文已经实现的系统能力。
若另行设计一套机载 D415 方案，假设相机与机身刚性连接，且仍由外部鱼眼相机观测背部标签，那么完整变换链可以写成：
$$
T{WD}(t) = T{WA}(t)\,T{AB}\,T{BD}
$$
三项分别表示：
- $T_{WA}(t)$：背部标签随时间变化的世界位姿；
- $T_{AB}$：机身参考坐标系相对于标签的固定外参；
- $T_{BD}$：D415 光学坐标系相对于机身的固定外参。
后两项需要通过机械模型、安装测量或外参标定获得。这里必须明确相机坐标系指向哪个光学坐标系，不能用相机外壳上某个点的位置替代光学坐标系原点。
还有一个常见遗漏：推算机载相机的世界位置，通常需要机器人的姿态，而不只是三维位置。
相机原点的位置满足：
$$
\mathbf p{D,W} = \mathbf p{B,W} + R{WB}\,\mathbf p{D,B}
$$
其中，$\mathbf p{B,W}$ 是机身原点的世界位置，$\mathbf p{D,B}$ 是相机原点在机身系中的安装偏移，$R_{WB}$ 把该偏移旋转到世界系。
安装偏移固定在机身坐标系里，却会随着机器人转身、俯仰或横滚而改变世界方向。因此，不能把安装偏移当作一个始终不变的世界向量，直接加到机器人位置上。
这段推导说明了机载方案需要哪些几何关系，并不意味着论文已经完成这套标定，也不意味着仅仅把 D415 装上机器人就能摆脱外部定位。
结语
这篇论文的定位思路是：用地面 AprilTag 建立世界基准，用外部鱼眼相机观测背部标签，用固定 D415 观测球，再将两路观测统一到世界坐标系中。
它说明了感知与控制之间如何交换位置信息，却没有完整公开标签到机器人控制参考点的几何关系。复现时，需要自行明确“机器人位置”究竟对应哪个点，并补齐相关外参；如果要改成机载深度相机，还需要额外标定相机与机身的关系。
标签位置、机身参考点位置和相机位置不是同一个量。明确各坐标系的原点、朝向和变换方向，才能判断论文已经说明了什么，以及实现时还缺什么。
参考文献与原文定位
Yidong Zhu, Zibo Dai, Tongning Zhang, Leixin Chang, Hua Chen. Spatiotemporal Agility: Time-Constrained Reinforcement Learning for Vision-Guided Dynamic Quadrupedal Interception. arXiv:2608.06907v1。标题按 v1 PDF。