ros的tf庫

為了這個題目，我是拿出了擠溝的精神擠時間，是下了功夫的，線性代數(shù)、矩陣論復習了，慣性導航里的dcm、四元數(shù)也了解了，剛體力學也翻了，wiki里的歐拉角也讀了，tf的tutorial、paper、source code也都看了。說實在的，經(jīng)過這次努力，我是有點了然于胸了，我也非常想了然于紙上與小伙伴們分享，但嘗試了幾次失敗了，我也不跟自己過不去了，還是扯吧。

1）tf不是坐標變換那么簡單。

很多小伙伴認為tf的作用是便捷的進行坐標變換。這個沒錯，但沒這么簡單。

在很多api中，存在著target frame，source frame，parent frame，child frame，這些名字的參數(shù)。

看起來很讓人糊涂，也很讓讓人煩，但里面隱藏著很多信息。source、target frame是在進行坐標變換時的概念，source是坐標變換的源坐標系，target是目標坐標系，這個時候，這個變換代表的是坐標變換。

parent、child frame是在描述坐標系變換時的概念，parent是原坐標系，child是變換后的坐標系，這個時候這個變換描述的是坐標系變換，也是child坐標系在parent坐標系下的描述。如果把child固結于一個剛體，那么這個變換描述的就是剛體在parent坐標系下的姿態(tài)。這個用處就大多了，可以用它來描述剛體的運動了。

這里有個巧合，當然也不是巧合，那就是從child到parent的坐標變換等于從parent到child的frame transform，等于child在parent frame的姿態(tài)描述。這里牽扯到了線性代數(shù)里的基變換、線性變換、過渡矩陣的概念。

2）簡單的說

在線性空間里，一個矩陣不但可以描述在同一個基下把一個點運動到另一個點的線性變換，還可以描述一個基在另外一個基下的表示，也可以表示一個基到另一個基的線性變換。還是比較復雜呀。那就看書吧，這樣比較簡單。

不過，如果單說坐標變換就簡單了。所謂坐標變換就是把一個點在某個坐標系的描述，變換成在另外一個坐標系下的描述。
要實現(xiàn)這個變換，比較簡單的做法就是用當前坐標系在參考坐標系下的描述（一個矩陣）去描述（乘以）這個點在當前坐標系下的描述（坐標）。

所以只要我們能夠知道當前坐標系在參考坐標系的描述，我們就可以把當前坐標系里任何一個點的坐標變換成參考坐標系里的坐標。

好吧，不考慮平移，只考慮旋轉(zhuǎn)，參考坐標系換個名字叫固定坐標系，當前坐標系叫動坐標系，那么該如何得到這個描述呢？

用矩陣。矩陣的各列分別是動坐標系在固定坐標系上的投影。當然如果你喜歡用坐標向量右乘矩陣，你也可以把在各軸的投影放在各行上。

一個基本的旋轉(zhuǎn)關系構成的矩陣是很好描述的。那么多次旋轉(zhuǎn)呢？

3）多次旋轉(zhuǎn)、三次旋轉(zhuǎn)、歐拉角、四元數(shù)

經(jīng)過多次旋轉(zhuǎn)的坐標系該怎么在固定坐標系下描述呢。這里有一個前乘和后乘的問題。什么時候前，什么時候后，很簡單，繞固定坐標系的軸旋轉(zhuǎn)就前乘，繞定坐標系就后乘。為什么是這樣呢？解釋起來很復雜，但可以這樣理解，繞固定軸旋轉(zhuǎn)時，你把這個旋轉(zhuǎn)看做是線性變換，這個變換把原來的矩陣里的各列向量旋轉(zhuǎn)變換成了新的向量，所以前乘。當繞動坐標系旋轉(zhuǎn)時，你把這個旋轉(zhuǎn)矩陣看做是一個描述了，并且這個描述是在未乘之前的矩陣這個描述下的描述，所以要放到后面。

剛體的任一姿態(tài)（任一動坐標系）可以經(jīng)由三次基本旋轉(zhuǎn)得到，用三個角來描述，這就是歐拉角。在tf里eulerYPR指的是繞動坐標系的zyx旋轉(zhuǎn)，RPY指的是繞固定坐標系xyz旋轉(zhuǎn)，這二者等價，坐標系定義為右手，x前，y左，z上。

四元數(shù)是剛體姿態(tài)的另一種描述方式，理論基礎是，剛體姿態(tài)可以經(jīng)過某一特定軸經(jīng)一次旋轉(zhuǎn)一定角度得到，等價于歐拉角，等價于旋轉(zhuǎn)矩陣。一個四個參數(shù)，一個三個參數(shù)，一個九個參數(shù)，之所以等價是因為四個參數(shù)里有一個約束，九個參數(shù)里有六個約束。

ps：這次就這樣了，下次再說tf的機制吧。
（8.2）ros的tf庫

4）tf的實現(xiàn)機制

4.1）有一棵樹
tf庫的目的是實現(xiàn)系統(tǒng)中任一個點在所有坐標系之間的坐標變換，也就是說，只要給定一個坐標系下的一個點的坐標，就能獲得這個點在其他坐標系的坐標。

為了達到這個目的，就需要確定各個坐標系之間的關系，也就是獲得任一坐標系在其他任一個坐標系下的描述。

假設有n個坐標系，那么他們之間的組合關系有c（n，2）個，如果n=4，那就是6種，如果n=100，那就是4950個，好多呀。不過，世界沒那么復雜，我們可以給這些坐標系之間構建一個結構，比如單叉樹或者單層多叉樹，那么100個坐標系之間的關系，就可以用99個樹杈搞定。tf里的選擇則是多層多叉樹。

樹的結構特點是單親，一個孩子只有父親，沒有母親，不允許兩個樹枝搞對象，長出果實，這也是tf里說的沒有回路的意思，為什么這樣呢？因為怕矛盾，有孩子的都知道，對孩子的問題上，兩口子很難一致，不一致時，孩子會比較茫然，但每個小孩子都會趨利避害，它們往往會選擇比較厲害的一方，那就是母老虎，把不厲害的一方灰太狼屏蔽忽略掉。坐標變換里可沒有主體和選擇機制，所以還是傻傻的單親比較好處理。不過，有利必然有弊，弊端后面再說。

4.2）維護一顆樹
維護一顆樹，需要先設計一個樹的結構，也就是一定要在樹被風吹來吹去變換的時候，讓小樹杈找個大樹杈依附，讓大樹杈找個樹干依附，讓樹干有個根固定。（這里提一下我的建議，免得一會忘了，建議是，如果打算用tf解決你的坐標變換問題，請一定要先清晰的畫出這棵樹的結構，再開始寫程序；因為這個結構是維護和使用的基礎）。

這個結構的建立和維護靠的是tf提供的tfbroadcastor類的sendtransform接口。在tb第一次發(fā)布一個從已有的parent frame到新的child frame的坐標系變換時，這棵樹就會添加一個樹枝，之后就是維護。

這里，提醒一下，在用tf時，一定要細心照料整棵大樹，時刻保持這棵大樹能夠描述整個外部世界里的關系，不能有斷裂。這樣才能保證在風吹時，整個樹都擺來擺去還保持穩(wěn)定。

4.3）使用這棵樹
很簡單，用tf的tflisener監(jiān)聽某一個指定的從一個a frame到b frame的變換即可，當然前提是樹上的樹杈們能把a，b聯(lián)通，并已經(jīng)準備好。再重復一次，這個變換是a frame到b frame的變換，也表示了b frame在a frame的描述，也代表了把一個點在b frame里的坐標變換成在a frame里的坐標的坐標變換。有了這個變換，你就可以盡情的變換了。

4.4）實現(xiàn)的原理

實現(xiàn)的原理，非常簡單，也非常粗暴。
基本原理是，tb的類里有個publisher，tl的類里有個subscriber，一個發(fā)布叫/tf的topic，一個訂閱這個topic，傳送的消息tfmessage里包含了parent frameid和child frameid的信息。

這個機制意味著，所有的tb會發(fā)布某一特定的parent到child的變換，而所有tl會收到所有的這些變換，然后tl利用一個tfbuffercore的數(shù)據(jù)結構維護一個完整的樹結構及其狀態(tài)。基于此，tl在使用這棵樹時，會用lookuptransform或waitfortransform來獲得任意坐標系之間的變換。

非常簡單，對吧，但粗暴的也非常顯然。那就是只要是一個tl，就要跟所有tb建立連接，就要收取所有的/tf消息，來維護一棵完整的樹，并且，還要負責搜索這棵樹，找到一條變換的路徑，然后乘呀乘，并且每次都要如此。太粗暴了！粗暴到一定程就是愚蠢了，可是為了活得簡單，我們往往會玩粗暴。
下一節(jié)（8.3）里再扯第5）節(jié)tf庫的優(yōu)缺點。
還是寫完再補覺吧，晚上還有其他作業(yè)。

（8.3）ros里的tf庫

5）tf庫的優(yōu)缺點

先說優(yōu)點，能用且易上手。
a、各種數(shù)值計算的細節(jié)，你不用考慮，tf庫可以幫你；
b、接口很簡潔，會廣播和監(jiān)聽就ok；
c、問題找的很準，那就是需要維護坐標系之間的關系；
d、提供了很多工具程序；
e、考慮了與時間相關的變換；
f、支持tf-prefix，可以在多機器人上用
g、基本能用，且不需要深入理解，死搬硬套就能用起來。

當然還有很多優(yōu)點，我就不說了，不太擅長，還是說缺點吧。

還是那句話，簡單粗暴！
a、樹的結構很簡單，但有時候很笨拙。舉個例子吧。有部電影叫手機，葛優(yōu)扮演的叫石頭，范偉演的叫磚頭，兩個人是叔伯兄弟，從小到大的小伙伴，應該很熟悉，關系也很明確，但要放到樹的結構下，就需要從下到上找共同先輩，然后從這個先輩再往下找，進而確定二者的關系，這個比較笨拙。于是有了范偉坐在門檻上的一句臺詞：恁這是弄啥勒，恁奶不是俺奶？！

b、每個tl都要維護同一顆樹，這樣的開銷太大，主要是網(wǎng)絡傳輸?shù)呢摵杀容^大。這一點，tf的設計者是完全承認的。舉個例子吧，北方的有些地方的農(nóng)村過年時要拜年，拜年要磕頭，磕頭的對象是寫有逝去的先輩名字的樹狀圖，一個同姓大家族里有很多小家庭，每個家里都有那么一張圖，挨家拜年，街上人來人往，每個人都磕了很多頭，磕的其實是同一張。如果有個祠堂，集中處理，維護一張圖，就不用看到早早起床、滿街跑的情況，效率也會高多了。

c、很難滿足實時性的要求，這一點比較顯然。要不tf也不會每個變換存10秒鐘的數(shù)據(jù)，不過源碼里好像是開了一個存100個消息的隊列。

d、還有很多細節(jié)不易理解。比如，now（）和time（0）；比如，技術文檔里的一些術語名詞；比如，采用了機器人里的習慣，與飛行器，慣導，車輛里的習慣區(qū)別較大，使用時不能相當然。

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

6）扯點其他的
有個我最喜歡的系列電影，叫黑客帝國，英文名字叫matrix，也就是矩陣。之所以喜歡是因為從一開始到現(xiàn)在我都沒看懂。

一個矩陣實際上定義了一個描述的視角。對空間里的一個點做描述，一個點比較絕對的描述是MX，其中M是坐標系的姿態(tài)，X是坐標向量，MX的含義就是在坐標系M里坐標為X的那個點。對于Y=MX，你可以看作是IY=MX。也就是說，在I坐標系里坐標為Y的一個點和在M系里坐標為X的是同一個點。

所以，虛擬世界里的工程師安德森與錫安里的尼歐是同一個點在不同坐標系下不同的坐標描述。

問題的核心是這兩個坐標系沒有一個是I系，所以盡管你清楚，N安德森=M尼歐，但在N的世界里卻不能確定N是什么，僅知道M是什么，在M的世界則相反。所以在錫安鏈接虛擬世界時，實際上是用N的逆乘以M尼歐得到了安德森。

問題來了，當the one I 出現(xiàn)的時候，變成了Ntheone=M尼歐，既NI=M尼歐，也就是N=M尼歐，所以M世界里就知道了M的絕對意義，就會認為M是真實世界。也獲取了通過改變尼歐的坐標屬性，然后使得N逆乘以M再乘以尼歐變成N世界里最牛的人。不過，這個M只有尼歐和the one在變換過程中才能知道。

隨著尼歐在錫安這個真實世界里也把自己當作了the one，奇跡發(fā)生了。那就是NI=MI，既N=M，錫安和虛擬世界沒有區(qū)別，都在同一個坐標系下，這樣尼歐在錫安世界里也具備了在虛擬世界里的超能力，以the one的身份展開了與機器人的對話，告訴機器人別太認真了，大家都tm的是程序。

以上純屬閑扯，不許反駁，不是我怕反駁，而是我更喜歡聽到您的閑扯。

總結

以上是生活随笔為你收集整理的老王说ros的tf库的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。