1. 先說(shuō)清楚坐標(biāo)變換和張量運(yùn)算到底在搞什么如果你做過(guò)機(jī)器人、做過(guò)三維視覺(jué)、搞過(guò)流體力學(xué)或者寫過(guò)游戲引擎八成和這兩個(gè)詞打過(guò)照面。但很多朋友一開(kāi)始接觸它們的時(shí)候都是各學(xué)各的——坐標(biāo)變換在“機(jī)器人學(xué)”那一章學(xué)張量運(yùn)算在“數(shù)學(xué)物理方法”那一章學(xué)中間隔著一整個(gè)學(xué)期結(jié)果到后面做多傳感器融合、做機(jī)械臂標(biāo)定、做有限元仿真的時(shí)候才發(fā)現(xiàn)兩者原來(lái)是一家人。這篇是系列的第16篇前面已經(jīng)覆蓋了向量基礎(chǔ)、矩陣運(yùn)算、常用坐標(biāo)系定義等內(nèi)容這篇開(kāi)始進(jìn)入一個(gè)比較關(guān)鍵的轉(zhuǎn)折點(diǎn)把坐標(biāo)變換和張量運(yùn)算統(tǒng)一到一個(gè)框架下去看。特別地很多做ROS開(kāi)發(fā)的朋友都聽(tīng)過(guò)“搭建tf樹(shù)坐標(biāo)變換”這個(gè)說(shuō)法但tf樹(shù)本質(zhì)上到底是什么它背后做的運(yùn)算是什么為什么俯瞰rviz里的坐標(biāo)軸樹(shù)狀結(jié)構(gòu)時(shí)最底層能對(duì)齊、最上層能跟隨中間到底算了幾輪這些問(wèn)題的答案其實(shí)都在今天要聊的內(nèi)容里。先說(shuō)結(jié)論坐標(biāo)變換描述的是“同一個(gè)客觀實(shí)體在不同觀察者眼里的坐標(biāo)數(shù)值如何互相換算”張量運(yùn)算描述的是“當(dāng)我們換坐標(biāo)系時(shí)一個(gè)物理量的表達(dá)形式如何保持一致”。一個(gè)偏幾何、一個(gè)偏代數(shù)但兩者是同一枚硬幣的兩面——坐標(biāo)變換是張量運(yùn)算在歐幾里得空間里的具體表現(xiàn)形式tf樹(shù)則是這兩個(gè)數(shù)學(xué)工具在工程中的一次經(jīng)典落地。這篇我會(huì)盡量用大白話把原理講透然后帶大家動(dòng)手搭一套完整的tf樹(shù)坐標(biāo)變換把實(shí)操中會(huì)踩的坑也一并列出來(lái)。適合有三四個(gè)月ROS或三維數(shù)學(xué)基礎(chǔ)、但一直沒(méi)把坐標(biāo)變換和張量運(yùn)算串起來(lái)的人也適合正在做多傳感器標(biāo)定、機(jī)械臂運(yùn)動(dòng)學(xué)、SLAM建圖被各種坐標(biāo)系搞到頭大的朋友。2. 坐標(biāo)變換的核心機(jī)制與實(shí)操要點(diǎn)2.1 旋轉(zhuǎn)矩陣的由來(lái)與選擇坐標(biāo)變換里最基礎(chǔ)的問(wèn)題就一句話同一個(gè)點(diǎn)在兩個(gè)坐標(biāo)系下的坐標(biāo)如何互相轉(zhuǎn)換。我們把它拆成兩步來(lái)看首先忽略平移只討論旋轉(zhuǎn)這就是旋轉(zhuǎn)矩陣存在的意義。旋轉(zhuǎn)矩陣的構(gòu)造方式有好幾種。最傳統(tǒng)的思路是把坐標(biāo)軸看成一組基向量從舊坐標(biāo)系到新坐標(biāo)系的旋轉(zhuǎn)矩陣每一列就是舊坐標(biāo)系的基向量在新坐標(biāo)系下的坐標(biāo)表示。另一個(gè)思路是從歐拉角出發(fā)按繞固定軸或繞自身軸的順序把三個(gè)基礎(chǔ)旋轉(zhuǎn)矩陣依次乘起來(lái)。這里特別強(qiáng)調(diào)一點(diǎn)繞固定軸和繞自身軸內(nèi)旋的乘積順序是相反的一不留神就翻車。實(shí)際操作中我不太推薦直接用歐拉角做內(nèi)部計(jì)算這是個(gè)從血淚里得出的教訓(xùn)。歐拉角最明顯的問(wèn)題就是萬(wàn)向節(jié)鎖Gimbal Lock當(dāng)俯仰角接近90度時(shí)滾轉(zhuǎn)和偏航會(huì)變得無(wú)法區(qū)分旋轉(zhuǎn)的自由度從三個(gè)降為兩個(gè)。這會(huì)導(dǎo)致在某些姿態(tài)附近數(shù)值會(huì)變得極度敏感輕微擾動(dòng)就能讓輸出姿態(tài)劇烈跳動(dòng)。所以工程上一般把歐拉角用于顯示、交互和參數(shù)輸入內(nèi)部計(jì)算優(yōu)先選擇四元數(shù)或者旋轉(zhuǎn)矩陣。旋轉(zhuǎn)矩陣本身有幾個(gè)天然約束值得需要明確它是一個(gè)正交矩陣即矩陣的轉(zhuǎn)置等于矩陣的逆這保證了旋轉(zhuǎn)不會(huì)產(chǎn)生拉伸或扭曲且行列式嚴(yán)格等于1這保證了它不會(huì)引入鏡像翻轉(zhuǎn)。在做數(shù)值計(jì)算時(shí)因?yàn)楦↑c(diǎn)誤差累積矩陣的正交性會(huì)被逐漸破壞所以每經(jīng)過(guò)大量累乘之后建議用格拉姆-施密特正交化或者奇異值分解逼近處理一下把矩陣“拉”回正交陣這一步在長(zhǎng)期運(yùn)行的SLAM系統(tǒng)里不是可選項(xiàng)而是剛需。2.2 四元數(shù)為什么更香說(shuō)到姿態(tài)表示繞不開(kāi)四元數(shù)。很多初學(xué)者第一次接觸四元數(shù)都會(huì)覺(jué)得它是天書(shū)但工程用下來(lái)四元數(shù)確實(shí)好用。我們不去背誦公式先理解它到底在干什么四元數(shù)本質(zhì)上是一種用四個(gè)數(shù)來(lái)表示三維旋轉(zhuǎn)的方式其中前三個(gè)數(shù)可以理解為旋轉(zhuǎn)軸的三個(gè)分量經(jīng)過(guò)歸一化處理第四個(gè)數(shù)則和旋轉(zhuǎn)角度的半角余弦有關(guān)。從計(jì)算角度看四元數(shù)的優(yōu)勢(shì)太明顯了。首先是拼接效率高兩個(gè)旋轉(zhuǎn)的組合用四元數(shù)乘法計(jì)算只需要16次浮點(diǎn)乘法而旋轉(zhuǎn)矩陣相乘需要27次乘法更關(guān)鍵的是將一個(gè)矢量本質(zhì)上是純四元數(shù)應(yīng)用旋轉(zhuǎn)時(shí)只需要兩次四元數(shù)乘法加上若干次加減法整個(gè)過(guò)程沒(méi)有三角函數(shù)運(yùn)算這一點(diǎn)在嵌入式平臺(tái)、MCU上尤其重要。另一個(gè)容易被忽略的優(yōu)勢(shì)是插值的平滑性。四元數(shù)可以通過(guò)球面線性插值SLERP在兩個(gè)姿態(tài)之間生成一條“最短路徑”的旋轉(zhuǎn)過(guò)渡而歐拉角的線性插值會(huì)產(chǎn)生明顯的抖動(dòng)和非自然繞路。做云臺(tái)控制、機(jī)械臂軌跡規(guī)劃、相機(jī)平滑跟隨這類需求時(shí)SLERP幾乎是標(biāo)配。需要記住四元數(shù)的一個(gè)特點(diǎn)四元數(shù)q和-q表達(dá)同一個(gè)旋轉(zhuǎn)因?yàn)樾D(zhuǎn)角度加360度并不產(chǎn)生實(shí)際變化。但在插值計(jì)算時(shí)要格外小心符號(hào)的一致性如果兩個(gè)四元數(shù)之間的點(diǎn)積是負(fù)數(shù)說(shuō)明它們?cè)谒木S球面上相距超過(guò)90度直接插值會(huì)繞遠(yuǎn)路可以先對(duì)其中一個(gè)取負(fù)再插值。2.3 齊次變換矩陣的工程意義到這一步我們還只有旋轉(zhuǎn)沒(méi)有平移。工程里不可能只有旋轉(zhuǎn)機(jī)械臂末端既要轉(zhuǎn)也要?jiǎng)佑谑俏覀冃枰环N能同時(shí)編碼旋轉(zhuǎn)和平移的表達(dá)方式齊次變換矩陣就此登場(chǎng)。齊次變換矩陣是一個(gè)4x4矩陣左上角3x3是旋轉(zhuǎn)矩陣或正交化后的旋轉(zhuǎn)分量右上角3x1是平移向量底下是[0 0 0 1]這一行用來(lái)保證矩陣乘法結(jié)構(gòu)的一致性。通過(guò)這一個(gè)矩陣我們可以把繞任意軸的旋轉(zhuǎn)和任意方向的平移合并為一次線性變換并且可以將多次變換直接累乘起來(lái)得到一個(gè)描述“從初始坐標(biāo)系到最終坐標(biāo)系”的復(fù)合變換矩陣。這里有一個(gè)經(jīng)典的概念要反復(fù)強(qiáng)調(diào)矩陣乘法不滿足交換律所以變換的順序極其重要。先旋轉(zhuǎn)再平移和先平移再旋轉(zhuǎn)得到的結(jié)果完全不同。在代碼層面常見(jiàn)的錯(cuò)誤就是把變換矩陣乘反了方向——一個(gè)是把點(diǎn)從左邊的坐標(biāo)系變換到右邊的坐標(biāo)系另一個(gè)是把同一變換作用到點(diǎn)本身這分別對(duì)應(yīng)數(shù)學(xué)上的“坐標(biāo)系變換”和“點(diǎn)的變換”本質(zhì)是逆運(yùn)算關(guān)系但工程里混亂操作的情況非常多。實(shí)操建議在代碼注釋里固定一種約定并且明確標(biāo)注“這個(gè)矩陣是body系到world系”還是“world系到body系”。凡是矩陣作用在被變換的對(duì)象上時(shí)一定要搞清當(dāng)前的點(diǎn)是在哪個(gè)坐標(biāo)系下表達(dá)的。我的習(xí)慣是在所有變換矩陣命名的后綴里帶上源坐標(biāo)系和目標(biāo)坐標(biāo)系比如T_wb表示從body到world然后在代碼里強(qiáng)烈禁止不標(biāo)注的裸矩陣出現(xiàn)這個(gè)習(xí)慣救回來(lái)無(wú)數(shù)次。3. 張量運(yùn)算的物理本質(zhì)與變換規(guī)則3.1 從矢量和矩陣到“張量”的一次升級(jí)在普通線性代數(shù)里我們習(xí)慣把數(shù)據(jù)分成標(biāo)量、向量、矩陣這三類。但在坐標(biāo)變換的語(yǔ)境下這個(gè)分類不夠用因?yàn)樗鼪](méi)有區(qū)分“向量本身”和“向量的坐標(biāo)分量表示”之間的區(qū)別。張量概念的引入就是為了更準(zhǔn)確地區(qū)分幾何對(duì)象和它的坐標(biāo)表像。一個(gè)粗略但管用的定義張量是一種“在不同坐標(biāo)系下按照特定的坐標(biāo)變換規(guī)則變化”的多線性量。零階張量就是標(biāo)量它沒(méi)有方向無(wú)論坐標(biāo)系怎么旋轉(zhuǎn)數(shù)值都不變一階張量就是向量它的分量會(huì)隨坐標(biāo)系的旋轉(zhuǎn)而變化但向量本身作為幾何實(shí)體不隨觀察者改變二階張量可以看成矩陣的推廣但必須要按照特定變換規(guī)則改變比如應(yīng)力張量、慣性張量、旋轉(zhuǎn)矩陣本身這些都屬于物理上具有明確意義的二階張量。很多人第一次聽(tīng)到“張量”這個(gè)詞還以為它是物理的專屬但實(shí)際上深度學(xué)習(xí)里的“張量”就是沿用這個(gè)概念只是把高維數(shù)組作為基本數(shù)據(jù)結(jié)構(gòu)來(lái)用并沒(méi)有嚴(yán)格套用協(xié)變逆變的物理變換規(guī)則。而在工程力學(xué)、相對(duì)論、機(jī)器人學(xué)等領(lǐng)域“張量的分量必須按照坐標(biāo)變換規(guī)則變化”這一條是不可動(dòng)搖的。3.2 協(xié)變與逆變張量變換的一條分水嶺張量運(yùn)算中最容易繞暈的點(diǎn)就是協(xié)變covariant和逆變contravariant的區(qū)別但它的物理直覺(jué)其實(shí)并不復(fù)雜。我們可以這樣理解想象一個(gè)向量它既可以使用基向量坐標(biāo)軸方向的線性組合來(lái)表達(dá)也可以使用坐標(biāo)標(biāo)架基向量之間的對(duì)偶標(biāo)架的線性組合來(lái)表達(dá)。如果坐標(biāo)軸本身變長(zhǎng)了那個(gè)需要變“短”才能保持物理數(shù)值不變的分量就是逆變分量那個(gè)跟著坐標(biāo)軸一起變“長(zhǎng)”的分量就是協(xié)變分量。用一組坐標(biāo)變換來(lái)說(shuō)可能更順口。假設(shè)有一個(gè)坐標(biāo)變換x^i x^i(x^j)其雅可比矩陣為 J^i_j ?x^i/?x^j。一個(gè)逆變向量比如速度矢量的分量按“逆變換雅可比”的方式改變V^i ?x^i/?x^j V^j。而一個(gè)協(xié)變向量比如梯度?f的分量按“正變換雅可比”的方式改變W_i ?x^j/?x^i W_j。這個(gè)i在角標(biāo)上面還是下面就是這個(gè)區(qū)別的精髓。為什么要區(qū)分這兩個(gè)變形方向因?yàn)槿绻粎^(qū)分梯度方向和速度方向在坐標(biāo)系變形時(shí)會(huì)被當(dāng)成同一類對(duì)象來(lái)處理從而丟失物理含義。工程上最常見(jiàn)的誤解是認(rèn)為梯度就是一個(gè)普通向量可以任意與速度方向點(diǎn)乘并期望得到一個(gè)標(biāo)量。但除非度規(guī)是標(biāo)準(zhǔn)的也就是坐標(biāo)系是勻速直線運(yùn)動(dòng)再加上正交標(biāo)準(zhǔn)基否則這個(gè)點(diǎn)乘是不具有坐標(biāo)不變性的。機(jī)器人學(xué)中當(dāng)使用關(guān)節(jié)空間坐標(biāo)q和笛卡爾空間坐標(biāo)x進(jìn)行速度與力的映射時(shí)速度和力的角標(biāo)逆變和協(xié)變關(guān)系如果搞錯(cuò)雅可比矩陣轉(zhuǎn)置的用法就很可能出現(xiàn)問(wèn)題。3.3 度規(guī)張量與內(nèi)積的不變性既然提到了點(diǎn)乘就必須介紹度規(guī)張量。度規(guī)張量是一個(gè)二階張量它定義了兩個(gè)向量之間的內(nèi)積也就是如何計(jì)算投影、長(zhǎng)度、夾角這些幾何量。在笛卡爾直角坐標(biāo)系里度規(guī)張量恰好是單位矩陣所以點(diǎn)乘公式很簡(jiǎn)單a·b a_x b_x a_y b_y a_z b_z。但一旦到了柱坐標(biāo)、球坐標(biāo)或者任意曲線坐標(biāo)系度規(guī)張量就會(huì)變成非對(duì)角的、甚至隨位置變化的函數(shù)。舉個(gè)例子在柱坐標(biāo)(r, θ, z)下兩個(gè)向量的點(diǎn)積不能只是簡(jiǎn)單地把對(duì)應(yīng)分量相乘再相加而是需要乘上度規(guī)張量的相應(yīng)分量。其中角向的度規(guī)分量為r^2所以同樣的坐標(biāo)分量在離軸較遠(yuǎn)的地方“實(shí)際長(zhǎng)度”會(huì)被放大。如果我們拿著笛卡爾系的直覺(jué)直接硬套算出來(lái)的模長(zhǎng)和夾角都會(huì)錯(cuò)。而度規(guī)張量存在的意義正是保證內(nèi)積的結(jié)果不依賴于坐標(biāo)系的選擇——無(wú)論在笛卡爾系還是柱坐標(biāo)系下a·b的值應(yīng)該完全相同因?yàn)樗菐缀螌?shí)體之間的不變量。這個(gè)思想在后來(lái)的廣義相對(duì)論、連續(xù)介質(zhì)力學(xué)中都是基石而即便在普通機(jī)器人學(xué)中當(dāng)你要做關(guān)節(jié)空間與操作空間之間的變換時(shí)度規(guī)在這里表現(xiàn)為慣性矩陣或質(zhì)量矩陣也扮演了核心角色。從操作空間向關(guān)節(jié)空間映射力時(shí)要用到雅可比轉(zhuǎn)置這與度規(guī)的變換規(guī)則一脈相承。3.4 張量變換規(guī)則在工程中的具體體現(xiàn)說(shuō)了這么多抽象的東西舉一個(gè)看得見(jiàn)摸得著的工程例子慣性張量。在剛體動(dòng)力學(xué)里慣性張量是一個(gè)3x3矩陣它描述了剛體在旋轉(zhuǎn)時(shí)的質(zhì)量分布。但這里有個(gè)陷阱同一根連桿在固連坐標(biāo)系下算出來(lái)的慣性張量和在世界坐標(biāo)系下使用的時(shí)候不能直接套用同樣的矩陣數(shù)值而必須乘以旋轉(zhuǎn)矩陣及其轉(zhuǎn)置來(lái)變換。具體公式是 I_world R I_body R^T這個(gè)變換就是張量分量變換規(guī)則的一個(gè)典型案例。很多剛接觸機(jī)器人動(dòng)力學(xué)的人會(huì)習(xí)慣性地把慣性矩陣當(dāng)成普通矩陣去乘結(jié)果算出來(lái)的力矩、角加速度與實(shí)測(cè)對(duì)不上。一旦意識(shí)到慣性張量是一個(gè)二階張量必須遵循張量變換規(guī)則很多問(wèn)題就迎刃而解了。同理協(xié)方差矩陣在坐標(biāo)變換下也遵循類似的規(guī)則Σ_world R Σ_local R^T。這種根據(jù)物理對(duì)象類型來(lái)選擇變換規(guī)則的能力正是張量運(yùn)算訓(xùn)練帶給工程師的核心價(jià)值——它不只是數(shù)學(xué)上的優(yōu)雅包裝而是直接決定計(jì)算結(jié)果正確與否的工具。4. 搭建tf樹(shù)坐標(biāo)變換從數(shù)學(xué)到ROS工程實(shí)踐4.1 tf樹(shù)在ROS中的定位說(shuō)完了原理我們進(jìn)入實(shí)操環(huán)節(jié)。ROS中的tf庫(kù)本質(zhì)上就是一套現(xiàn)成的坐標(biāo)變換管理框架它提供了一張“坐標(biāo)變換樹(shù)”每個(gè)節(jié)點(diǎn)代表一個(gè)坐標(biāo)系每條邊代表父子坐標(biāo)系之間的變換關(guān)系。只要建立了這棵樹(shù)任意兩個(gè)坐標(biāo)系之間的變換都能通過(guò)從葉子走到根再走下來(lái)的方式推導(dǎo)出來(lái)。tf樹(shù)的設(shè)計(jì)思路其實(shí)就是一個(gè)分布式數(shù)據(jù)庫(kù)里面存儲(chǔ)著各坐標(biāo)系之間的相對(duì)位姿包括平移和旋轉(zhuǎn)。它的數(shù)據(jù)來(lái)源可以來(lái)自靜態(tài)發(fā)布比如傳感器在機(jī)身中心的固定安裝位置也可以來(lái)自動(dòng)態(tài)計(jì)算比如從輪式里程計(jì)、IMU積分、視覺(jué)SLAM輸出的位姿估計(jì)。系統(tǒng)運(yùn)行時(shí)tf庫(kù)自動(dòng)維護(hù)所有坐標(biāo)系的實(shí)時(shí)關(guān)系并提供lookupTransform接口來(lái)查詢?nèi)我鈨蓚€(gè)坐標(biāo)系之間的最新變換。4.2 靜態(tài)變換部分搭建地基搭建tf樹(shù)的第一步是理清機(jī)器人本體的坐標(biāo)系結(jié)構(gòu)。拿一臺(tái)典型的差速輪小車舉例至少有這些坐標(biāo)系map地圖系全局參考、odom里程計(jì)系漂移修正、base_footprint/base_link機(jī)器人本體系、laser_link激光雷達(dá)安裝位置、imu_linkIMU安裝位置以及各輪子的旋轉(zhuǎn)坐標(biāo)系。靜態(tài)變換用static_transform_publisher來(lái)發(fā)布。比如激光雷達(dá)裝在底盤中心正上方0.2米旋轉(zhuǎn)角約180度因?yàn)榧す鈷呙璧姆较蛲ǔ：蜋C(jī)器人前進(jìn)方向有安裝偏移那么對(duì)應(yīng)的命令形如rosrun tf2_ros static_transform_publisher x y z yaw pitch roll parent_frame child_frame很多初學(xué)者直接背命令但我的建議是每次寫靜態(tài)變換前先在紙上畫出坐標(biāo)系的朝向關(guān)系標(biāo)清楚三個(gè)軸的指向再填參數(shù)。因?yàn)橄嗖钜粋€(gè)軸、差一個(gè)正負(fù)號(hào)整個(gè)tf樹(shù)看起來(lái)一切正常但數(shù)據(jù)流向完全錯(cuò)了。在靜態(tài)變換中還有一類特殊的細(xì)節(jié)frame_id的命名規(guī)范。整套系統(tǒng)里的坐標(biāo)系命名必須全局唯一建議用小寫字母加下劃線而且最好把所有坐標(biāo)系的名稱維護(hù)在一個(gè)單獨(dú)的約定文件里避免不同模塊之間用不同的昵稱導(dǎo)致查詢失敗。4.3 動(dòng)態(tài)變換部分讓樹(shù)“活”起來(lái)靜態(tài)變換只適合固定安裝關(guān)系的坐標(biāo)系底盤移動(dòng)的時(shí)候map到odom到base_link這幾條邊是動(dòng)態(tài)的。這個(gè)動(dòng)態(tài)更新通常由兩個(gè)模塊產(chǎn)生里程計(jì)源和定位源。底盤輪式里程計(jì)負(fù)責(zé)輸出odom到base_link的變換視覺(jué)/激光定位模塊則負(fù)責(zé)輸出map到odom之間的變換相當(dāng)于對(duì)里程計(jì)漂移做修正。還有一種方式是固定map到odom保持不動(dòng)把累計(jì)漂移全部放到odom到base_link這條邊上這種方案在純輪式里程計(jì)下更簡(jiǎn)單但長(zhǎng)期精度會(huì)下降。在ROS2中動(dòng)態(tài)變換通常用tf2_ros::TransformBroadcaster發(fā)布每次發(fā)布時(shí)給出新的變換矩陣。典型代碼大致長(zhǎng)這樣geometry_msgs::msg::TransformStamped t; t.header.stamp this-now(); t.header.frame_id odom; t.child_frame_id base_link; t.transform.translation.x x; t.transform.translation.y y; t.transform.translation.z 0.0; t.transform.rotation quat; broadcaster_-sendTransform(t);這里有件事必須強(qiáng)調(diào)時(shí)間戳很重要。tf2在設(shè)計(jì)上帶有時(shí)間戳校驗(yàn)機(jī)制如果發(fā)布的變換時(shí)間戳比當(dāng)前時(shí)間慢了超過(guò)緩存窗口lookupTransform會(huì)報(bào)錯(cuò)。而如果時(shí)間戳遠(yuǎn)快于真實(shí)頻率整個(gè)樹(shù)的插值也會(huì)失去意義。對(duì)于動(dòng)態(tài)變換消息頻率應(yīng)該和對(duì)應(yīng)傳感器的更新率一致不要盲目地往高頻發(fā)。4.4 完整搭建步驟與驗(yàn)證手段我建議按下面這套步驟去搭tf樹(shù)比較不容易踩坑第一步畫tf樹(shù)結(jié)構(gòu)圖明確各坐標(biāo)系之間的父子關(guān)系每一對(duì)父子之間想好是靜態(tài)還是動(dòng)態(tài)。第二步選一個(gè)根坐標(biāo)系。本質(zhì)上根坐標(biāo)系的選擇應(yīng)當(dāng)是一個(gè)“參考精度最高、漂移修正能力最優(yōu)”的坐標(biāo)系通常選map或者odom而不是base_link。把根坐標(biāo)系掛在樹(shù)的頂層之后所有坐標(biāo)系都能從根節(jié)點(diǎn)追溯到。第三步把靜態(tài)變換全部發(fā)布出來(lái)用tf2_echo逐個(gè)驗(yàn)證每一對(duì)坐標(biāo)系的數(shù)值是否符合物理安裝參數(shù)。第四步接上動(dòng)態(tài)源比如里程計(jì)和IMU或者定位模塊打開(kāi)rviz同時(shí)顯示odom和map兩條鏈路觀察是否隨時(shí)間持續(xù)對(duì)齊。第五步做退化測(cè)試把小車抬起來(lái)手動(dòng)旋轉(zhuǎn)一下看rviz里base_link和laser_link的相對(duì)位置是否始終保持一致。通常到這一步剛才所有的小問(wèn)題都會(huì)暴露出來(lái)。4.5 tf樹(shù)查詢的注意事項(xiàng)在實(shí)際做導(dǎo)航、做抓取規(guī)劃時(shí)最常用的接口就是lookupTransform。這里有幾個(gè)常被忽略的坑。第一是“父系目標(biāo)”還是“子系目標(biāo)”的顛倒。查詢時(shí)參數(shù)是target_frame和source_frame意思是“把source坐標(biāo)系下的數(shù)據(jù)變換到target坐標(biāo)系下”寫代碼時(shí)我非常建議把兩個(gè)frame_id打印出來(lái)驗(yàn)證而不是靠記憶硬寫。第二是時(shí)間插值的方向。tf2支持查詢一個(gè)歷史時(shí)間點(diǎn)的變換但需要保證兩邊的時(shí)間戳之間有對(duì)應(yīng)的緩存數(shù)據(jù)時(shí)間窗口設(shè)置得過(guò)小會(huì)讓歷史查詢失敗。第三是“靜態(tài)緩存”的誤用。雖然靜態(tài)變換數(shù)據(jù)理論上不變但tf2的緩存機(jī)制仍會(huì)存儲(chǔ)多幀數(shù)據(jù)如果發(fā)布頻率異常高且長(zhǎng)期運(yùn)行內(nèi)存可能緩慢增長(zhǎng)需要在發(fā)布靜態(tài)變換時(shí)選擇低頻或一次性發(fā)布策略。5. 常見(jiàn)問(wèn)題速查與排錯(cuò)實(shí)錄5.1 TF樹(shù)中的超時(shí)與丟幀問(wèn)題跑tf樹(shù)時(shí)最容易看到的錯(cuò)誤就是“Lookup would require extrapolation into the past”或者“Lookup would require extrapolation into the future”。這類錯(cuò)誤的本質(zhì)是樹(shù)里找不到時(shí)間戳匹配的變換數(shù)據(jù)。常見(jiàn)的誘因有幾種動(dòng)態(tài)變換發(fā)布頻率太低緩存窗口覆蓋不到系統(tǒng)啟動(dòng)初期時(shí)間尚未同步各節(jié)點(diǎn)時(shí)間戳相差過(guò)大發(fā)布端和查詢端的時(shí)鐘不一致。排查思路我一般這樣走先檢查發(fā)布端的發(fā)布頻率可以訂閱/tf話題看消息間隔再檢查緩存時(shí)間看參數(shù)里cache_time是否足夠大最后檢查所有傳感器節(jié)點(diǎn)的系統(tǒng)時(shí)間是否同步設(shè)備多的時(shí)候NTP同步非常重要否則每個(gè)節(jié)點(diǎn)的時(shí)間基準(zhǔn)差幾個(gè)毫秒就會(huì)導(dǎo)致查詢失敗。有一個(gè)經(jīng)驗(yàn)值得分享在開(kāi)發(fā)階段把ROS的調(diào)試日志級(jí)別調(diào)到debugtf2會(huì)打印每一組發(fā)布的變換數(shù)據(jù)直接對(duì)比數(shù)值可以很快確認(rèn)是哪一條邊的數(shù)據(jù)不對(duì)。另外用tf2_monitor工具可以查看當(dāng)前樹(shù)的狀態(tài)包括每條邊的更新頻率和平均時(shí)延這個(gè)工具在復(fù)雜系統(tǒng)里簡(jiǎn)直救命。5.2 坐標(biāo)抖動(dòng)與隨機(jī)漂移如果你發(fā)現(xiàn)rviz里某個(gè)坐標(biāo)系在靜止?fàn)顟B(tài)下仍然不停抖動(dòng)問(wèn)題多半出在以下三個(gè)地方。第一旋轉(zhuǎn)四元數(shù)沒(méi)有歸一化。tf2在底層對(duì)四元數(shù)有歸一化處理但如果上游傳入的四元數(shù)數(shù)值嚴(yán)重異常歸一化后仍然會(huì)有微小抖動(dòng)。第二姿態(tài)融合算法輸出的位姿噪聲過(guò)大。常見(jiàn)于IMU沒(méi)有做校準(zhǔn)或者磁力計(jì)干擾嚴(yán)重時(shí)此時(shí)需要優(yōu)先優(yōu)化傳感器融合環(huán)節(jié)而不是懷疑tf樹(shù)發(fā)布邏輯。第三靜態(tài)變換與動(dòng)態(tài)變換之間存在沖突。比如base_link到laser_link明明發(fā)布了兩套不同的靜態(tài)關(guān)系同一對(duì)坐標(biāo)系在樹(shù)里出現(xiàn)了環(huán)路就會(huì)造成結(jié)果跳變。遇到這種情況先把問(wèn)題定位到“哪個(gè)坐標(biāo)系的變換在抖”這個(gè)可以用rviz中單獨(dú)顯示該坐標(biāo)系和其父系來(lái)觀察。然后斷開(kāi)上游數(shù)據(jù)源替換成一段固定值發(fā)布器看抖動(dòng)是否消失。如果固定值發(fā)布時(shí)也抖動(dòng)那問(wèn)題出在tf緩存或查詢邏輯如果不抖動(dòng)那問(wèn)題出在上游數(shù)據(jù)源。5.3 旋轉(zhuǎn)方向約定不一致最后是一個(gè)特別隱蔽但很常見(jiàn)的坑坐標(biāo)系的旋轉(zhuǎn)方向約定。有些設(shè)備定義正yaw為逆時(shí)針有些為正順時(shí)針有些廠家把IMU的z軸向上定為正旋轉(zhuǎn)有些卻沒(méi)有在文檔中說(shuō)明清楚。當(dāng)你把多個(gè)傳感器的數(shù)據(jù)統(tǒng)一到同一棵tf樹(shù)上時(shí)如果k各自的旋轉(zhuǎn)方向不一致在模型上看起來(lái)都是“轉(zhuǎn)了同樣的角度”實(shí)際數(shù)據(jù)卻互相矛盾。解決方案是每個(gè)傳感器接入tf樹(shù)之前先單獨(dú)做一次矩形軌跡測(cè)試。例如讓機(jī)器人沿閉合矩形行走檢查rviz中機(jī)器人朝向相對(duì)軌跡的角度變化是否符合預(yù)期。如果發(fā)現(xiàn)某個(gè)數(shù)據(jù)源的旋轉(zhuǎn)方向和預(yù)期相反最好在驅(qū)動(dòng)層做歸一化處理如對(duì)四元數(shù)z取負(fù)而不是在tf樹(shù)里加一個(gè)額外的旋轉(zhuǎn)邊來(lái)“校正”。5.4 坐標(biāo)變換與張量運(yùn)算結(jié)合后的常見(jiàn)認(rèn)知誤區(qū)把坐標(biāo)變換和張量運(yùn)算放到一起后很多人還會(huì)犯一個(gè)認(rèn)知層面的錯(cuò)誤以為旋轉(zhuǎn)矩陣圖是個(gè)張量。嚴(yán)格來(lái)說(shuō)旋轉(zhuǎn)矩陣本身是一個(gè)特殊的二階張量但它的兩個(gè)下標(biāo)在坐標(biāo)變換下并不總是遵循一樣的規(guī)則需要區(qū)分矩陣是作為“變換矩陣”還是“物理量”的表示。比如慣性張量在坐標(biāo)系變換后按R I R^T更新而旋轉(zhuǎn)矩陣R本身則是用于變換向量的算子它并不像普通張量那樣被“操作”它本身就是“操作”本身。這種微妙差別在實(shí)際計(jì)算中直接決定了你用R^T還是R。在機(jī)械臂運(yùn)動(dòng)學(xué)里當(dāng)從關(guān)節(jié)速度計(jì)算末端速度時(shí)用雅可比矩陣而從末端力映射回關(guān)節(jié)力矩時(shí)用雅可比矩陣的轉(zhuǎn)置這背后的數(shù)學(xué)機(jī)制就是協(xié)變與逆變關(guān)系的具體體現(xiàn)。不搞懂這兩個(gè)概念遇到機(jī)器人奇異位形時(shí)很容易不知道雅可比為什么會(huì)退化更不知道如何用奇異值分解去判斷可操作度。這些看似“純數(shù)學(xué)”的知識(shí)在工程中實(shí)實(shí)在在決定著系統(tǒng)的穩(wěn)定性、精度和調(diào)試效率。我也是連續(xù)做了幾個(gè)多傳感器融合項(xiàng)目踩過(guò)不少角度、方向、矩陣順序上的坑之后才真正意識(shí)到張量運(yùn)算和坐標(biāo)變換不是一個(gè)“知道就行”的知識(shí)點(diǎn)而是一種需要肌肉記憶的工程直覺(jué)。最后分享一個(gè)親身試驗(yàn)過(guò)很有效的練習(xí)方法自己拿張紙手動(dòng)把同一個(gè)向量在直角坐標(biāo)和極坐標(biāo)之間變換把協(xié)變和逆變各算一遍再手動(dòng)計(jì)算度規(guī)矩陣完成一輪之后對(duì)張量變換的那些公式會(huì)從“背住”變成“理解”。這套練習(xí)做完之后再回頭處理tf樹(shù)你會(huì)發(fā)現(xiàn)那些報(bào)錯(cuò)不再是隨機(jī)碰運(yùn)氣的事而是完全可預(yù)測(cè)、可推理的。