ros的tf库

骑猪看日落 2022-05-15 01:49 154阅读 0赞

本文转自老王说ros [http://blog.exbot.net/archives/1686][http_blog.exbot.net_archives_1686]

为了这个题目，我是拿出了挤沟的精神挤时间，是下了功夫的，线性代数、矩阵论复习了，惯性导航里的dcm、四元数也了解了，刚体力学也翻了，wiki里的欧拉角也读了，tf的tutorial、paper、source code也都看了。说实在的，经过这次努力，我是有点了然于胸了，我也非常想了然于纸上与小伙伴们分享，但尝试了几次失败了，我也不跟自己过不去了，还是扯吧。

1）tf不是坐标变换那么简单。

很多小伙伴认为tf的作用是便捷的进行坐标变换。这个没错，但没这么简单。

在很多api中，存在着target frame，source frame，parent frame，child frame，这些名字的参数。

看起来很让人糊涂，也很让让人烦，但里面隐藏着很多信息。source、target frame是在进行坐标变换时的概念，source是坐标变换的源坐标系，target是目标坐标系，这个时候，这个变换代表的是坐标变换。

parent、child frame是在描述坐标系变换时的概念，parent是原坐标系，child是变换后的坐标系，这个时候这个变换描述的是坐标系变换，也是child坐标系在parent坐标系下的描述。如果把child固结于一个刚体，那么这个变换描述的就是刚体在parent坐标系下的姿态。这个用处就大多了，可以用它来描述刚体的运动了。

这里有个巧合，当然也不是巧合，那就是从child到parent的坐标变换等于从parent到child的frame transform，等于child在parent frame的姿态描述。这里牵扯到了线性代数里的基变换、线性变换、过渡矩阵的概念。

2）简单的说

在线性空间里，一个矩阵不但可以描述在同一个基下把一个点运动到另一个点的线性变换，还可以描述一个基在另外一个基下的表示，也可以表示一个基到另一个基的线性变换。还是比较复杂呀。那就看书吧，这样比较简单。

不过，如果单说坐标变换就简单了。所谓坐标变换就是把一个点在某个坐标系的描述，变换成在另外一个坐标系下的描述。  
要实现这个变换，比较简单的做法就是用当前坐标系在参考坐标系下的描述（一个矩阵）去描述（乘以）这个点在当前坐标系下的描述（坐标）。

所以只要我们能够知道当前坐标系在参考坐标系的描述，我们就可以把当前坐标系里任何一个点的坐标变换成参考坐标系里的坐标。

好吧，不考虑平移，只考虑旋转，参考坐标系换个名字叫固定坐标系，当前坐标系叫动坐标系，那么该如何得到这个描述呢？

用矩阵。矩阵的各列分别是动坐标系在固定坐标系上的投影。当然如果你喜欢用坐标向量右乘矩阵，你也可以把在各轴的投影放在各行上。

一个基本的旋转关系构成的矩阵是很好描述的。那么多次旋转呢？

3）多次旋转、三次旋转、欧拉角、四元数

经过多次旋转的坐标系该怎么在固定坐标系下描述呢。这里有一个前乘和后乘的问题。什么时候前，什么时候后，很简单，绕固定坐标系的轴旋转就前乘，绕定坐标系就后乘。为什么是这样呢？解释起来很复杂，但可以这样理解，绕固定轴旋转时，你把这个旋转看做是线性变换，这个变换把原来的矩阵里的各列向量旋转变换成了新的向量，所以前乘。当绕动坐标系旋转时，你把这个旋转矩阵看做是一个描述了，并且这个描述是在未乘之前的矩阵这个描述下的描述，所以要放到后面。

刚体的任一姿态（任一动坐标系）可以经由三次基本旋转得到，用三个角来描述，这就是欧拉角。在tf里eulerYPR指的是绕动坐标系的zyx旋转，RPY指的是绕固定坐标系xyz旋转，这二者等价，坐标系定义为右手，x前，y左，z上。

四元数是刚体姿态的另一种描述方式，理论基础是，刚体姿态可以经过某一特定轴经一次旋转一定角度得到，等价于欧拉角，等价于旋转矩阵。一个四个参数，一个三个参数，一个九个参数，之所以等价是因为四个参数里有一个约束，九个参数里有六个约束。

ps：这次就这样了，下次再说tf的机制吧。  
（8.2）ros的tf库

4）tf的实现机制

4.1）有一棵树  
tf库的目的是实现系统中任一个点在所有坐标系之间的坐标变换，也就是说，只要给定一个坐标系下的一个点的坐标，就能获得这个点在其他坐标系的坐标。

为了达到这个目的，就需要确定各个坐标系之间的关系，也就是获得任一坐标系在其他任一个坐标系下的描述。

假设有n个坐标系，那么他们之间的组合关系有c（n，2）个，如果n=4，那就是6种，如果n=100，那就是4950个，好多呀。不过，世界没那么复杂，我们可以给这些坐标系之间构建一个结构，比如单叉树或者单层多叉树，那么100个坐标系之间的关系，就可以用99个树杈搞定。tf里的选择则是多层多叉树。

树的结构特点是单亲，一个孩子只有父亲，没有母亲，不允许两个树枝搞对象，长出果实，这也是tf里说的没有回路的意思，为什么这样呢？因为怕矛盾，有孩子的都知道，对孩子的问题上，两口子很难一致，不一致时，孩子会比较茫然，但每个小孩子都会趋利避害，它们往往会选择比较厉害的一方，那就是母老虎，把不厉害的一方灰太狼屏蔽忽略掉。坐标变换里可没有主体和选择机制，所以还是傻傻的单亲比较好处理。不过，有利必然有弊，弊端后面再说。

4.2）维护一颗树  
维护一颗树，需要先设计一个树的结构，也就是一定要在树被风吹来吹去变换的时候，让小树杈找个大树杈依附，让大树杈找个树干依附，让树干有个根固定。（这里提一下我的建议，免得一会忘了，建议是，如果打算用tf解决你的坐标变换问题，请一定要先清晰的画出这棵树的结构，再开始写程序；因为这个结构是维护和使用的基础）。

这个结构的建立和维护靠的是tf提供的tfbroadcastor类的sendtransform接口。在tb第一次发布一个从已有的parent frame到新的child frame的坐标系变换时，这棵树就会添加一个树枝，之后就是维护。

这里，提醒一下，在用tf时，一定要细心照料整棵大树，时刻保持这棵大树能够描述整个外部世界里的关系，不能有断裂。这样才能保证在风吹时，整个树都摆来摆去还保持稳定。

4.3）使用这棵树  
很简单，用tf的tflisener监听某一个指定的从一个a frame到b frame的变换即可，当然前提是树上的树杈们能把a，b联通，并已经准备好。再重复一次，这个变换是a frame到b frame的变换，也表示了b frame在a frame的描述，也代表了把一个点在b frame里的坐标变换成在a frame里的坐标的坐标变换。有了这个变换，你就可以尽情的变换了。

4.4）实现的原理

实现的原理，非常简单，也非常粗暴。  
基本原理是，tb的类里有个publisher，tl的类里有个subscriber，一个发布叫/tf的topic，一个订阅这个topic，传送的消息tfmessage里包含了parent frameid和child frameid的信息。

这个机制意味着，所有的tb会发布某一特定的parent到child的变换，而所有tl会收到所有的这些变换，然后tl利用一个tfbuffercore的数据结构维护一个完整的树结构及其状态。基于此，tl在使用这棵树时，会用lookuptransform或waitfortransform来获得任意坐标系之间的变换。

非常简单，对吧，但粗暴的也非常显然。那就是只要是一个tl，就要跟所有tb建立连接，就要收取所有的/tf消息，来维护一棵完整的树，并且，还要负责搜索这棵树，找到一条变换的路径，然后乘呀乘，并且每次都要如此。太粗暴了！粗暴到一定程就是愚蠢了，可是为了活得简单，我们往往会玩粗暴。  
下一节（8.3）里再扯第5）节tf库的优缺点。  
还是写完再补觉吧，晚上还有其他作业。

（8.3）ros里的tf库

5）tf库的优缺点

先说优点，能用且易上手。  
a、各种数值计算的细节，你不用考虑，tf库可以帮你；  
b、接口很简洁，会广播和监听就ok；  
c、问题找的很准，那就是需要维护坐标系之间的关系；  
d、提供了很多工具程序；  
e、考虑了与时间相关的变换；  
f、支持tf-prefix，可以在多机器人上用  
g、基本能用，且不需要深入理解，死搬硬套就能用起来。

当然还有很多优点，我就不说了，不太擅长，还是说缺点吧。

还是那句话，简单粗暴！  
a、树的结构很简单，但有时候很笨拙。举个例子吧。有部电影叫手机，葛优扮演的叫石头，范伟演的叫砖头，两个人是叔伯兄弟，从小到大的小伙伴，应该很熟悉，关系也很明确，但要放到树的结构下，就需要从下到上找共同先辈，然后从这个先辈再往下找，进而确定二者的关系，这个比较笨拙。于是有了范伟坐在门槛上的一句台词：恁这是弄啥勒，恁奶不是俺奶？！

b、每个tl都要维护同一颗树，这样的开销太大，主要是网络传输的负荷比较大。这一点，tf的设计者是完全承认的。举个例子吧，北方的有些地方的农村过年时要拜年，拜年要磕头，磕头的对象是写有逝去的先辈名字的树状图，一个同姓大家族里有很多小家庭，每个家里都有那么一张图，挨家拜年，街上人来人往，每个人都磕了很多头，磕的其实是同一张。如果有个祠堂，集中处理，维护一张图，就不用看到早早起床、满街跑的情况，效率也会高多了。

c、很难满足实时性的要求，这一点比较显然。要不tf也不会每个变换存10秒钟的数据，不过源码里好像是开了一个存100个消息的队列。

d、还有很多细节不易理解。比如，now（）和time（0）；比如，技术文档里的一些术语名词；比如，采用了机器人里的习惯，与飞行器，惯导，车辆里的习惯区别较大，使用时不能相当然。

综上，用不用tf需要平衡，那里用，那里不用，用什么代替，也需要平衡，不是一件很轻松愉快的事。

6）扯点其他的  
有个我最喜欢的系列电影，叫黑客帝国，英文名字叫matrix，也就是矩阵。之所以喜欢是因为从一开始到现在我都没看懂。

一个矩阵实际上定义了一个描述的视角。对空间里的一个点做描述，一个点比较绝对的描述是MX，其中M是坐标系的姿态，X是坐标向量，MX的含义就是在坐标系M里坐标为X的那个点。对于Y=MX，你可以看作是IY=MX。也就是说，在I坐标系里坐标为Y的一个点和在M系里坐标为X的是同一个点。

所以，虚拟世界里的工程师安德森与锡安里的尼欧是同一个点在不同坐标系下不同的坐标描述。

问题的核心是这两个坐标系没有一个是I系，所以尽管你清楚，N安德森=M尼欧，但在N的世界里却不能确定N是什么，仅知道M是什么，在M的世界则相反。所以在锡安链接虚拟世界时，实际上是用N的逆乘以M尼欧得到了安德森。

问题来了，当the one I 出现的时候，变成了Ntheone=M尼欧，既NI=M尼欧，也就是N=M尼欧，所以M世界里就知道了M的绝对意义，就会认为M是真实世界。也获取了通过改变尼欧的坐标属性，然后使得N逆乘以M再乘以尼欧变成N世界里最牛的人。不过，这个M只有尼欧和the one在变换过程中才能知道。

随着尼欧在锡安这个真实世界里也把自己当作了the one，奇迹发生了。那就是NI=MI，既N=M，锡安和虚拟世界没有区别，都在同一个坐标系下，这样尼欧在锡安世界里也具备了在虚拟世界里的超能力，以the one的身份展开了与机器人的对话，告诉机器人别太认真了，大家都tm的是程序。

以上纯属闲扯，不许反驳，不是我怕反驳，而是我更喜欢听到您的闲扯。

[http_blog.exbot.net_archives_1686]: http://blog.exbot.net/archives/1686