robot-notes /核心/導航/座標轉換與 TF

座標轉換與 TF

機器人身上每個感測器裝在不同位置、朝不同方向;地圖、里程、車體又各有原點。一筆「LiDAR 看到前方 2m 有牆」要變成「這牆在地圖的哪裡」,中間隔著好幾次座標換算。這篇從這個根本問題出發,講 ROS 的座標慣例、2D 剛體變換的數學(為什麼用齊次矩陣),以及 tf2 怎麼用一棵樹管理所有座標系。

差速車本身的運動學(輪速 ↔ v/ω)見底盤 §1.1、下位機運動控制;本篇專講「座標系之間怎麼換算」。延伸閱讀:定位、SLAM、路徑規劃。

1. 根本問題:每個量測都「只對某個座標系」有意義

LiDAR 回報「正前方 2m 有牆」,這個「2m、正前方」是相對 LiDAR 自己的。但 LiDAR 鎖在車身某個位置、相機在另一個位置、IMU 又在別處;而「我在地圖哪裡」是相對地圖的。要回答「這牆在地圖哪裡、要不要閃」,就得把這筆資料從感測器座標系,一路換算到車體、再到地圖。

座標轉換就是這個換算;TF(transform) 是 ROS 用來集中管理「所有座標系彼此關係、且隨時間更新」的系統,tf2 是其第二代實作。REP-103 開宗明義:單位與慣例不一致是整合 bug 的常見來源——統一慣例 + 集中管理變換,就是為了消除這類風險。

2. ROS 座標慣例(REP-103)

要大家接得起來,先約定座標軸與單位:

軸向(相對車體):x 朝前、y 朝左、z 朝上,全部右手系。
單位:SI——長度公尺、角度弧度(rad)、時間秒。角度逆時針為正(右手定則)。
旋轉表示優先序:四元數(quaternion,緊湊、無奇異點)> 旋轉矩陣 > 固定軸 roll/pitch/yaw;一般 Euler 角不建議(24 種衝突慣例)。
特殊後綴:相機光學 frame 加 _optical(z 朝前、x 朝右、y 朝下,跟車體慣例不同,因為光軸朝前);地理用 _ned 等。

roll / pitch / yaw 就是繞這三根軸的轉動,名字的排列順序跟 x / y / z 一一對應——軸向定了,這三個名字不必另外背。(要合成一個姿態時,ROS 用的是固定軸 X→Y→Z 的順序,等價於內旋 ZYX。)

名稱	繞哪根軸	直覺	對輪式車	對足式
roll(側傾)	x(前後軸)	車身左右歪	幾乎恆為 0(橫坡、單邊過門檻、有懸吊的車轉彎側傾才變)	會變,要估
pitch(俯仰)	y(左右軸)	車頭上下點	幾乎恆為 0(上下坡、急加減速才變)	會變,要估
yaw(偏航)	z(垂直軸)	原地轉向	唯一會變的角度	會變,而且光靠本體感測在數學上不可觀測

方向的正負最容易踩坑:在這個右手系裡,正的 roll 是左側抬起,正的 pitch 是車頭往下(不是往上),正的 yaw 是向左轉。pitch 那個特別反直覺——「抬頭」是負的。這一節開頭講的慣例衝突,實務上有一大半就是在這種地方翻車。

最右邊那兩欄是輪式與足式在狀態估計上的分水嶺:在室內平整地面上,輪式車只需要 (x, y, yaw) 三個量,roll 與 pitch 當常數處理不會出大錯——「yaw 是唯一會變的角度」是這個假設的結果,不是量出來的事實。地面一不平就會咬人:2D 光達在車身傾斜時掃描面會打到地板或天花板。足式的軀幹六個自由度(3 個位置 + 3 個姿態)全都在動,一個都不能省。

3. REP-105:frame 鏈,以及為什麼分 map 與 odom 兩層

REP-105 規範移動平台的座標鏈:map → odom → base_link(再往下接各感測器)。

base_link:剛性固定在車身上的座標系,跟著車走。
odom(里程):世界固定 frame,連續、平滑,但會無界漂移——它由輪速/視覺/IMU 累積推算,誤差隨時間累積,但不會突然跳。
map:世界固定 frame,長期不漂,但會離散跳變——定位元件(如 AMCL)不斷依感測重算位姿來消除漂移,修正的瞬間位姿會「跳一下」。

為什麼要分這兩層?(第一性原理)因為導航同時要兩種互斥的特性:

控制迴路要「平滑」:控制器吃的位姿不能突然跳,跳一下控制就抖。→ 用 odom(連續)。
全域定位要「長期準」:長距離規劃不能讓誤差無限漂。→ 用 map(長期準)。

把兩種需求拆成兩層,就能各取所需:近程平滑用 odom、全域準確用 map。

TF tree:map→odom→base_link→感測器;map 跳變但準、odom 連續但漂

一個關鍵權責:定位元件不直接發 map→base_link,而是先收 odom→base_link(里程發的),再反推、廣播 map→odom。這樣才能維持「每個 frame 只有一個 parent」(base_link 的 parent 只能是 odom)。

4. 座標轉換數學:旋轉 + 平移,合成一個矩陣

兩個座標系之間的剛體變換 = 旋轉 + 平移。一個點在 B 系的座標 p_B,換算到 A 系:

p_A = R(θ) · p_B + t          R(θ) = | cos θ  −sin θ |   (逆時針為正)
                                      | sin θ   cos θ |

這裡 θ = B 系 x 軸相對 A 系 x 軸的夾角(B 在 A 中的朝向),t 是 B 原點在 A 中的位置——θ 與 t 合起來就是「B 在 A 中的位姿」。

R(θ) 不必背,兩行就能自己寫出來。 B 系的 x 軸,在 A 系裡看是 (cos θ, sin θ);B 系的 y 軸,在 A 系裡看是 (−sin θ, cos θ)(x 軸逆時針轉 90°)。把這兩個向量當成矩陣的兩個直行,就是 R(θ)。 這也解釋了它在做什麼:p_B = (2, 0) 代表「沿 B 的 x 軸走 2 格」,乘上 R 之後就變成「沿 A 系裡那個方向走 2 格」。

記住這個來歷,就不會在該寫 R(θ) 還是 R(−θ) 時猜——問自己「我要把誰的軸換算成誰的」,而不是試兩次看哪個結果比較合理。齊次變換(homogeneous transformation) 把「先轉再移」合併成單一矩陣乘法:點補一維成 (x, y, 1),變換寫成 3×3 矩陣 T。

2D 座標轉換:p_A = R(θ)p_B + t,齊次矩陣 T 合成旋轉與平移

為什麼用齊次座標?(第一性原理,三個都很實用)

統一:旋轉本是乘法、平移本是加法;補一維後平移也變乘法,兩者用同一種運算。
可串接:多段變換直接連乘——T_A←C = T_A←B · T_B←C。整條 frame 鏈(感測器→車體→里程→地圖)就是一串矩陣相乘。
可逆:反方向換算只要取逆矩陣 T_B←A = (T_A←B)⁻¹。

tf2 在做的正是這件事:把樹上各段變換連乘(必要時取逆),算出任兩 frame 之間的總變換。

4.1 算一次:LiDAR 說前方 2 m 有牆,那面牆在地圖的哪裡

開頭那個問題,現在可以直接算完。

已知:

LiDAR 裝在車體正前方 0.3 m(所以 T_base←lidar 的平移是 (0.3, 0)、旋轉 0°)
車在 map 上的位姿是 (4.0, 1.0, 90°)(在 (4, 1),車頭朝 map 的 +y)
LiDAR 回報:正前方 2.0 m 有一個點

第一步,量測值先變成座標。 LiDAR 的「正前方 2 m」在 LiDAR 自己的座標系裡就是 p_lidar = (2.0, 0)。

第二步,換到車體。 只有平移,沒有旋轉:

p_base = (2.0, 0) + (0.3, 0) = (2.3, 0)

第三步,換到 map。 車的朝向是 90°,代進去:

          | cos 90°  −sin 90° |     | 0  −1 |
R(90°) =  |                   |  =  |       |
          | sin 90°   cos 90° |     | 1   0 |

p_map = R(90°) · (2.3, 0) + (4.0, 1.0)
      = (0, 2.3)          + (4.0, 1.0)
      = (4.0, 3.3)

R(90°) 把 (2.3, 0) 轉成 (0, 2.3)——原本是「沿車頭方向 2.3 m」,在 map 上就是「沿 +y 方向 2.3 m」,因為車頭正朝著 +y。

那面牆在地圖的 (4.0, 3.3)。

三步各對應一段 frame:lidar → base_link → map。實際系統裡這串是 tf2 幫你連乘的,你只要問它「這個點在 map 裡是多少」。但知道它在算什麼,除錯時差很多——位置整個歪掉通常是某一段的 θ 弄反了,而不是感測器壞了。

5. tf2:用一棵「樹」管理所有座標系

為什麼是樹(每個 frame 只有一個 parent)? 若一個 frame 有兩個 parent,從不同路徑算到它會得到互相衝突的變換,系統無法決定哪個對。樹結構保證任兩 frame 之間路徑唯一,變換可確定地連乘得到、不會出現閉環矛盾。
查詢:lookupTransform(target, source, time) 沿樹找唯一路徑、連乘各段、回傳總變換。
時間戳(為什麼要):感測資料有延遲。LiDAR 在 t 時刻掃到的點,要用 t 時刻的車體位姿換算,不是「現在」的——車一直在動,用錯時刻就算錯位置。所以 tf2 為每段變換存帶時間戳的快照(預設緩衝 ~10 秒),查詢時內插到指定時刻;time=0 代表「最新可用」。
static transform:不隨時間變的關係(LiDAR 螺絲鎖死在車上)用靜態變換廣播,省儲存與發布開銷。

6. 與筆記其他部分的連結

odometry(§4.1) 算出來的就是 odom→base_link 這段,持續發布給 tf2;它連續、平滑、會漂——正好對應 REP-105 的 odom。
AMCL(§22) 估的是 map→odom 的修正量;它依雷射在已知地圖上重定位,讓 map→base_link 長期不漂——正好對應 REP-105「定位元件發 map→odom 而非 map→base_link」的權責。

核心

本頁