tf:坐标变换库
摘要：
tf库提供了一种跟踪和管理整个系统坐标系变换关系和数据的工具，使得我们只需要关心所需要的坐标系之间的关系，而不必了解整个系统的坐标关系。 在机器人操作系统(ROS)开发的早期，发现对于开发者而言跟踪和管理坐标变换是一个痛点。这是一个很繁琐的过程，开发人员的任何疏忽都可能导致BUG。 当一个机器人由若干个独立的子系统构成时，坐标变换数据源将是分布式的，在这种情况下跟踪和管理坐标变换将更是挑战。本文将要讨论这一问题的复杂度并从中提炼出需求。 接着根据这些需求设计tf库。我们将展示一些成功应用这个库的使用案例。这个库还有一个强大的扩展功能，它不仅可以管理空间上的坐标变换，还可以管理时间维度上的变换。
1. 引言
机器人在执行任务的时候有一个关键的问题就是，明确自己在什么位置上，工作环境中其它物体与自己的相对位置关系是什么。 举一个简单的例子，我们让一个机器人寻找一个红色的小球并用手抓去碰它。本质上就是一个让手抓不断靠近小球的过程。但是要完成这样一个简单的任务，机器人就必须清楚手抓与球之间的相对位置关系。 如果在房间中有一个传感器可以定位球在空间中的位置，我们就需要将传感器坐标系下的位置数据转换到房间坐标系下，再转换到机器人的基座上、躯干上、肩膀、肘、腕上，最后转换到手抓的坐标系下。 为了比较球的位置，我们只能将两个物体都放到一个坐标系下。根据传感器的数据我们知道需要将手抓左移3cm，经过从传感器到房间再到机器人基座的坐标变换后，机器人就知道球在手抓下面3cm处。 此时机器人就控制手臂向下3cm让手抓碰到球就完成任务了。
这是一个相对简单的机器人系统，但是需要清楚整个系统的相对关系。我们在设计的时候是希望系统尽可能的简单，而实际上一个包含了传感器、电机、计算模块、通信模块等的机器人系统也是很复杂的。 随着机器人系统日趋复杂，让所有的子系统了解整个系统的所有配置是不现实的，因此模块的设计者就必须考虑什么数据是完成该任务所必须的。当系统运行在一台计算机上时， 设计一个为提供所有模块之间信息交换的接口是很难的。随着机器人系统日渐趋于分布式部署，由于带宽的限制并不是所有的信息都应该拿来共享的。
如果程序员可以简单的从一个库中请求数据，“相对于躯干我应该怎么移动手抓才能够碰到球”，将会是很方便强大的。这样程序员直接请求球相对于躯干的位置关系就好了，不需要再做任何中间计算， 因为库已经替我们完成了。对于程序员而言，传感器是在墙上挂着，还是另外有一个机器人脑袋上顶个传感器，都无所谓，他都是通过库请求数据。只要系统本身知道各个中间的坐标系，并可以计算它们之间的相对关系， 程序员就只需要关心与任务相关的坐标系关系，而不必了解整个系统。
tf库就是这样管理和维护整个系统的坐标系以及变换关系，使得每个模块都只需要关心与自己相关的坐标系而不必了解整个系统。随着机器人系统日益复杂，能够完全聚焦于任务相关的坐标系就变得很重要。 而大部分机器人系统都将不同坐标系下的传感器数据混淆在一起。
tf库的设计是ROS中提供这种能力的package[1]。tf库有两个标准的模块，广播器(Broadcaster)和监听器(Listener)。这两个模块被整合在ROS生态系统里，但它们在ROS系统之外也是通用的[2]。
2. 相关工作
tf库与场景图形(scene graph)的概念最接近。一个场景图形就是一个用来渲染3D场景的数据结构。
场景图形在渲染3D场景的可视化软件以及机器人模拟器中大量的使用[3][4][5]。
因为图形通常都是将要渲染的对象描述成一棵树。每一个对象通过位置和一些其它的信息与父对象建立联系。Depending on the application the other information can range from visualization meshes for pure rendering to update rules and inertial properties for the simulators.
一个有趣的场景图形扩展就是OSGAR项目，它是一个用于增强现实(Augmented Reality, AR)的开源图形项目(Open Scene Graph)[6][7]。
3D引擎OGRE也用场景图形[8]。tf树已经被整合在了rviz渲染工具的OGRE场景图形中了[9]。在图1中，你可以看到在OGRE场景图形中渲染的tf树，并用来指引OGRE场景图形渲染PR2机器人各个部分的位置。
 |
| 图1. PR2机器人及其坐标系 |
在设计过程中证明树这种数据结构对于tf的很实用的。
3. 需求
tf库可以被分为两个不同的部分。第一个部分是向整个系统广播变换信息。第二部分监听变换信息并将之保存留待后用。第二部分就可以用来响应不同坐标关系的请求。
在一个系统中有多个坐标系时通常就会有几个不同的消息源。每一个消息源都与实际的硬件相连以不同的频率产生数据(比如，传感器的数据，执行器的反馈)，而且有可能有延迟或者丢包的现象。 因此tf库必须能够异步的接收这些数据而且对于延迟和丢包具有一定的鲁棒性。
tf库被设计成为ROS生态系统的一个核心库。为了能够支持ROS应用程序，它必须具有一定的鲁棒性，尤其是在不可靠网络链接的分布式计算环境下。tf的设计也需要考虑匿名发布和接收消息的通信机制。
这个库将需要能够提供指定时间点的两个坐标系之间变换关系。如果没有可以使用的数据它必须给用户提供一个合适的报错提醒而不是一个异常的数据。它也是建立在数据分发的基础上的也有鲁棒性的需求。
我们并没有假设系统的结构是固定不变的，也就是说它应该提供动态修改坐标系关系的能力，包括对坐标系的增、删、改操作。
4. 设计
因为需求中要求将tf系统拆分为两个部分，所以在设计时就拆开了。其中分发消息的部分就是广播器(Broadcaster)，而接收消息并响应变换请求的模块被称为监听器(Listener)。
4.1 数据结构
如果我们以坐标系为节点，坐标系之间的变换关系为边，就可以把整个坐标系统描述成一个图。在这种表述下，网络的变换仅仅是连接节点的边的变换(In this representation the net tranform is simply the product of the edges connecting any two nodes.)。这个图可以存在一个或者多个不连接的子图，可以在子图内部计算变换关系，但是不能计算子图之间的变换关系。
变换是有方向的，我们可以通过变换的逆来回溯一条边。但是对于一个给定的图，两个节点之间可能存在多条路径，这将导致两个或者更多的潜在网络变换(resulting in two or more potential net transforms)，导致结果存在歧义。为了避免这种情况，图中一定不能有环。
为了提供快速的查找，树就必须能够被快速的搜索。限定图为树的结构可以快速的搜索连接特性。这一点在图的复杂度增加时就显得尤其重要。比如图2中显示的PR2机器人的复杂的树。
 |
| 图2. RVIZ下显示的PR2机器人tf树 |
参考场景图形的开发选择树这种数据结构。两者之间的不同在于场景图形是周期性的遍历这棵树，而tf树则是异步的响应特定请求。
树这种数据结构还有方便动态修改的优点，除了有向边之外不需要任何其它的信息。When an edge is published to a node referencing a different parent node, the tree will resolve to the new parent without extra information.
对树中每一条边的更新都有一个时间。类似的，请求查找关系时也需要声明时间。为了快速的查找，图中每条边的历史数据都保存进了一个时间序列中。图3的调试信息就显示了每条边的最近5秒的统计信息。
 |
| 图3. 两只乌龟的简单tf树 |
为了方便操作，所有tf库中的数据都有两个部分的信息：描述的额坐标系和有效时间。这也称为有时间磋的数据(These two pieces of data are referred to as a Stamp)。 Stamp中的数据可以转换为已知的数据类型。
4.2 变换的广播
广播器的设计是很简单的。它在每次听到关于特定变换的更新动作时以最低的频率广播消息(It broadcasts messages every time an update is heard about a specific transform with a minimum frequency)。
4.3 变换的监听
无论是否有坐标发生了变化，广播器都会周期的发送更新数据。监听器收集这些数据到一个有序的列表中，而且对于请求时间的数据在两个最近值之间做了插值。 因为广播器规律的发送变换，监听器就不需要猜坐标系在未来中的位置。广播器的频率应当选择一个足够球形线性插值(spherical linear interpolation SLERP)运算的值[10]。 虽然更高的频率可以增加准确度，但将增大对带宽的需求，建议在保证计算精度的前提下采用尽可能低的频率。
插值是一个关键的操作。它允许广播器们不同步，以不同的频率广播。只要更新频率足够，SLERP都可以及时的给出两个相邻采样之间任何一个位置(SLERP can allow a look up of an arbitrary position in time between two closely spaced samples.)。插值也在一定程度上提高了系统针对对包的鲁棒性。几个包的丢失都可以通过插值来补偿。
4.4 计算变换
计算任何两个节点之间的变换关系，需要先计算一个包含这两个节点的生成树(spanning set)。监听器通过对这两个节点向上回溯直到找到一个共同的父节点来计算生成树。 如果无法找到一个共同的父节点，就会报错返回。如果找到了，监听器就会根据这个共同的父节点计算从源坐标系到目标坐标系之间的变换。 通过坐标系之间的反变换向上回溯，根据向下查找时就直接使用坐标变换(When travelling up an edge in the tree the inverse transform is used, and when travelling down an edge the value of the transform is used)。
假如我们要计算坐标系\(a\)到坐标系\(c\)之间的变换关系，其中\(b\)是它们之间的共有的父节点，那么有公式： $$ \begin{equation}\label{f1} T_a^c = T_a^b T_b^c \end{equation} $$
4.5 能力
核心数据结构的朴素性贯穿整个系统(The simplicity of the core data structures carries through to the overall system)。它对于系统的效率(efficiency)和松耦合特性(flexibility)都是有好处的。 使得我们在分布式异步的环境中也可以进行调试。
1) Efficiency:开发人员只需要广播一次传感器或者坐标变换的数据就可以了。变换数据只发布最权威的一次(The transform data is broadcast one time by the authority for that transform)。 比如说，电机控制器可以发布很多变换数据。但是通常会有像定位进程这样更权威的存在。每一个权威都广播一个对变换的最优估计，监听器直接接收这些数据。而具有直接连接关系的单一广播器可以最小化带宽和延迟。 (译者按，不知所云，还是看原文吧。)
对于接收到的每个消息，监听器都按照时间先后顺序保存到一个列表中。监听器只需要将接收到的消息插入到有序列表中就好了。对于监视而言最小化操作数量是必要的， 因为这样监听器可以一直接收更新而且占用的计算资源有限。(译者按，应该就是一个链表的实现)
保持链表是有序的是重要的，因为变换数据在传送过程中可能存在延迟。最好的情况是数据都是按照需要插入的顺序接收的，这样就不需要特殊的操作。如果接收到的顺序是乱的也无所谓。 (译者按，应该就是一个与时间相关的堆)
对于一个图的搜索操作的复杂度是\(O(|E| + |N|\log{|N|})\)，其中，\(E\)为图中边的数量，\(N\)为图中点的数量[11]。而对于一棵树而言，其搜索复杂度则是\(O(D)\)，其中\(D\)为树的深度。 在请求两个坐标系之间的变换时，我们不需要修改或者分析数据结构，这样监听器只需要进行简单的链表插入操作而不需要不断地分析图。(译者按，就是树比较简单)
tf库允许传感器数据，或者任何Stamp的数据在原始坐标系的网络中传播，或者保存在原始坐标系中。当一个算法需要用这些数据，就可以请求tf库获取需要的变换数据。 tf库避免了大量不必要的中间变换的计算。
请求时如果给定时间参数为0就会返回最新的数据。如果请求的时间不存在，监听者就会抛出一个异常。
2) Flexibility:给用户提供高效的存储和传播数据的能力的同时，tf给用户带来了很大的灵活度(flexibility)。用户可以动态的改变坐标系，也可以在一个与采集数据时不同的坐标系下回放数据。 这简化了离线处理日志数据的过程，只需要记录原始的传感器数据和变换数据，在回放的时候重新导入这些数据就好了。比如说，对SLAM算法中跟踪移动障碍物的数据的回放。 使用相同的数据集，不仅可以跟新SLAM算法也可以更新障碍物跟踪算法。
广播器和监听器的配合使用可以支持匿名广播和订阅消息，给开发者提供了很大的灵活度，可以自由的添加或者移除模块。只要有一个新的广播器开始发布变换数据，所有的监听器都可以接收到并保存。
类似的，任何监听者模块一运行就可以监视所有的广播器，而且可以马上建立一个缓存响应查询。这使得监听者不需要任何配置信息就可以启动，也不需要任何中心坐标系(central coordination)。 尽管需要维护一个很大的状态机来管理这些数据，也不需要从用户哪里获得什么配置。只有一种情况需要用户注意，就是监听器刚启动的时候，可能因为还没有完全接收整个系统的信息，导致用户的查询失败。 当然还有很多其它原因导致查询失败。一个鲁棒的用户代码应该捕获这些异常并重试。
网络延迟是分布式系统中常见的问题。在传输数据的时候，变换数据必须是带有时间戳的。网络延迟有两种情况：时间戳较于数据后到，这种情况下使用历史记录的数据就可以解决。 如果时间戳先于数据到达。(译者按，总之就是数据的各种不同步的问题，不想翻译了)
5. 使用案例
支持多个监听器模块的功能是有用的，我们可以通过库函数或者脚本的形式实现多个监听器，而完全不必关心他们之间是否存在冲突。 一个常用的使用案例就是在库中集成一个监听器模块来实现库的核心工作，比如说PR2的导航库就是这样实现的。导航库根据传感器的数据构建碰撞地图(collision map)， 其内部使用的监听器模块可以用来描述机器人朝着一个路标移动的过程。当机器人到达了它的目标时，就直接销毁监听器而不会影响导航器内部的功能， 它仍然可以计算障碍地图。这种方法在PR2办公室马拉松(PR2 office marathon)中得到了广泛的应用[12][13]。
且不说支持多个监听器，凭借这能够在任何坐标系中转换的功能，tf都是一个强大的工具。比如说PR2上用于检测门和自身插头任务的坐标系[13]。 在这两个例子中，感知算法用于检测世界中固定的位置，用于从障碍物坐标系转换为置信度最高的任务坐标系，进而完成特定的任务。(不知所云……)
即使没有实际的物理表示，我们也可以使用任务坐标系。一个应用案例就是在定位信息很少的时候的导航系统(A use case which has shown this to be the case is simple navigation when localization information is poor)。一个简单的例子就是机器人在障碍物之间前进并把它们记录到地图中。如果机器人的定位产生了突变， 比如说，重新请求(reacquired)GPS信号时，最近记录的障碍可能就与机器人碰撞在一起了，尽管实际上机器人并没有怎么移动。 这一问题可以通过局部连续坐标来解决，局部连续坐标只依赖与类似里程计这样的传感器，记录了机器人的相对运动关系。如果在这个坐标系中记录了障碍物， 他们将不会受到位置更新的影响。但是这个坐标系将随着时间发生漂移，需要在积累较大误差之前做出修正。使用这种技术时， 定位过程就可以变更为只发布里程计的校准信息，而不是发布机器人在世界中的位置。这是控制PR2通过标准门的关键技术，因为它使得机器人能够以较低的误差通过门而不是完全的定位[12][13]。 诸如路标这样的物体将在全局地图中打上标记，将他们变换到局部连续坐标中，我们就可以做路径规划了。
能够动态修改tf树的结构的功能可以在基本桌面任务(basic table top manipulation task)中看到[14][15]。在这个任务中，机器人靠近一个桌子， 抓起一个物体，并将他放在自己的底盘上并带走。在机器人接近物体的同时，它可能通过一个或者多个传感器对这个物体做了很多测量。 进而估计出物体在世界中的位置和尺寸。但是当机器人伸手去抓这个物体的时候，就是要计算物体相对于夹具的位置了。抓起物体后， 就要将物体添加到机器人的碰撞模型中。当要将物体放到底盘上的时候，就需要将物体从夹具上移到底盘上。在机器人再次移动时，又需要准确的描述物体和机器人一起移动这一过程。
6. 扩展
在一个框架下还可以计算在一个特定时间下两个坐标系之间的变换关系。tf实现了两个扩展用于支持基本的速度变换和计算一个物体在过去一段时间的位置。
6.1 对速度的支持
tf库利用了历史记录的每个变换并通过离散差分得到速度变换： $$ Velocity_{t,\Delta t} = \frac{Positon_t - Position_{t - \Delta t}}{\Delta t} $$ API提供了一个参数使得用户可以选择差分的时间周期。该参数的选择对于获取的准确的信息是很重要的。如果时间周期太小，那么位置测量时的误差就会十分显著。 放大时间周期相当于一个低通滤波器，因此如果速度变化很快的话，将很有可能不会采集到。
6.2 时间尺度上的数据变换
tf库的另一个扩展就是在时间尺度上计算坐标变换。
对于某一个瞬时公式\(\ref{f1}\)是成立的。但如果是在\(t_0\)做观察而在计算时已经是\(t_1\)时刻，那么公式\(\ref{f1}\)就少了一项。 这就需要一个\(t_0\)和\(t_1\)之间的变换。它可以在公式\(\ref{f1}\)中间添加一个时间的变换得到： $$ \begin{equation}\label{f2} T_{a@t_0}^{c@t_1} = T_{a@t_0}^{b@t_0} * T_{b@t_1}^{b@t_0} * T_{b@t_1}^{c@t_1} \end{equation} $$ 使用监听器的标准接口可以得到公式中的第一项和最后一项。但是\(T_{b@t_1}^{b@t_0}\)仍然是未知的。在一中情况下可以获知\(T_{b@t_1}^{b@t_0}\)， 就是物体在\(t_0\)和\(t_1\)时刻之间是精致的，那么这一项就是仅仅是一个单位矩阵。
因此通过选择一个目标数据是静止的坐标系，就可以计算不同时间下的两个坐标系之间的变换关系。需要用到如下的请求：源帧，源时间，修正帧，目标帧，目标时间。
下面以两个乌龟在地面上摩擦的例子详细解释。首先"turtle1"跟踪"turtle2"5秒钟。为了计算"turtle1"在其自身坐标系中的目标点， 我们需要计算公式\(\ref{f3}\)的值。 $$ \begin{equation}\label{f3} T_{turtle1@t}^{turtle2@t-5} = T_{world@t-5}^{turtle2@t-5} * T_{world@t}^{world@t-5} * T_{turtle1@t}^{world@t} \end{equation} $$
长时间数据存储(Long Term Data Storage)：上面的这个例子假设你能够在时间0计算ab坐标系之间的而变换以及在时间1时bc之间的坐标变换。 这只能在你的监听器缓存足够大能够保存下从时间0到时间1的数据。为了能够记录长时间的数据，在保存的时候需要先转换到固定坐标系b下。 这样原坐标系就和固定坐标系b一致了,\(T_{b@t_1}^{b@t_0}\)就会是一个单位矩阵。就只需要计算T_{b@t_1}^{c@t_1}来获知当前的定位。 这个简单的例子在很多机器人系统中都会用到，因为他们没有工具计算更复杂的例子了(这TM是什么鬼？不理解)。只有当上述的假设成立可以使用单位矩阵时这种方法才能够有效。
7. 后续工作
尽管tf库是一个活跃的库，而且它的核心功能已经相对稳定，但也仍在不断的提高和扩展这个库。后续的一些挑战将驱使优化tf库在有限的带宽下更好的工作， 将可能关注如何扩展tf来支持部分分割(partially partitioned)或者桥接(bridged)网络以及由此产生的tf树。比如说，PR2默认发布变换的频率是1kHz发送60帧将占用3Mbps以上的带宽。 占用的带宽与发布的频率和坐标系数量有关，在一般的工作环境下使用大量的标准WIFI链接。降低带宽也能够使得tf在低功耗的CPU上使用，as subscribing to the full stream can take non-trivial CPU load.
一个高度关注的特征，也是一个开放的问题，就是如何将不确定性带到tf树中。
还有很多软件工程上的改进需要做。我们期望更少的软件依赖，也希望支持down-sampling数据以降低内存需求，建立更大的缓存。 在低带宽链接上的潜在的功能也意味着远程的展现和发送请求。
也需要很多工具来调试运行系统。这些工具很有用但也留下了扩展和改进的空间，使得经验少的开发人员更方便的使用系统。
8. 结论
tf库工作的核心功能已经在上述的文章中介绍了。以及novel的时间尺度上的变换的扩展。这个库被ROS社区接受用作主要的跟踪位置信息的工具。 在本文中也展示一些有机器人专家开发的tf库应用案例。该库求坐标变换的简易性也使得人们能够更方便的处理传感器数据。
致谢
作者在这里需要感谢很多人，他们测试tf库并提供了反馈。尤其感谢Wim Meeussen, Eitan Marder-Eppstein, Jsh Faust,和Jeremy Leibs他们帮忙改进了速度和质量。
参考文献
[1]. “Ros tf package homepage,” http://www.ros.org/wiki/tf.
[2].