Learning Objectives
- 說明同一個東西為什麼在不同座標系有不同的數字
- 用旋轉與平移，把感測器量到的點換到地圖座標
- 用齊次轉換矩陣組合與反轉座標轉換
- 說明 REP-103 的右手座標慣例與四元數的用途
- 正確計算兩個角度的差
杯子在哪裡，要看你問誰
桌上有一個杯子。機器人的光達回報「在我前方 1 公尺」；手臂的控制器要的是「離我的底座 0.4 公尺，高 0.1 公尺」；地圖上寫的是「教室裡 (3.2, 1.4) 的位置」。三個都對，杯子也沒有動，只是每個數字的基準不同。
這個基準叫座標系（coordinate frame）：一個原點加上幾個方向。機器人上每個感測器都有自己的座標系，因為它們裝的位置和方向都不一樣。所以機器人程式裡最常做的事之一，就是把一個座標系的數字換到另一個座標系。
ROS 2 用一份叫 REP-103 的文件統一慣例，這條軌道都照它：
| 項目 | 慣例 |
|---|---|
| 機器人身上的座標系 | x 朝前、y 朝左、z 朝上，右手座標系 |
| 角度 | 弧度；繞 z 軸逆時針為正 |
| 長度、時間 | 公尺、秒 |
右手座標系的意思是：右手四指從 x 彎向 y，大拇指指的就是 z。拿到一份感測器資料，先確認它的座標系慣例，這一步沒做好，機器人可能會往反方向轉。例如 REP-103 就規定相機的光學座標系不一樣：z 朝前、x 朝右、y 朝下。
機器人身上的座標系通常叫 base_link，地圖的座標系叫 map。先動手看兩者的差別。
旋轉加平移
從 base_link 換到 map 分兩步：先把機器人的朝向轉回來，再加上機器人的位置。機器人在 (x_r, y_r)、朝向 θ：
旋轉： x' = cos θ × x_b − sin θ × y_b
y' = sin θ × x_b + cos θ × y_b
平移： x_m = x' + x_r
y_m = y' + y_r
用數字驗算一次。 機器人在 (2, 1)、朝向 90°，所以 cos θ ＝ 0、sin θ ＝ 1。杯子在它前方 1 公尺、左邊 0.5 公尺，也就是 (1, 0.5)：
x' = 0 × 1 − 1 × 0.5 = −0.5
y' = 1 × 1 + 0 × 0.5 = 1
map = (−0.5 + 2, 1 + 1) = (1.5, 2)
機器人朝地圖的 +y 方向，它的「前方」在地圖上就是 +y，它的「左邊」在地圖上就是 −x。對照一下剛才的互動畫面。
反過來從 map 換到 base_link，順序也反過來：先減掉機器人的位置，再轉 −θ。
把光達的一個讀數換到地圖座標
光達給的是距離和角度（極座標）。這支程式把它換成 base_link 的 x、y，再換到地圖。注意旋轉和平移是分開的兩步，可以把順序想成「先站在機器人身上轉正，再走到機器人所在的位置」。
把旋轉和平移寫成一個矩陣
光達裝在車頭前面，車子又在地圖上，要把光達的點換到地圖就要做兩次轉換。手臂有六個關節就是六次。一次一次寫公式太容易錯，所以寫成一個矩陣：
⎡ cos θ −sin θ x ⎤
T = ⎢ sin θ cos θ y ⎥ 點寫成 (x, y, 1)，用矩陣乘法就同時完成旋轉和平移
⎣ 0 0 1 ⎦
這叫齊次轉換（homogeneous transform）。多出來的那個 1 讓平移也能用乘法表示。它有兩個好用的性質：
- 組合就是相乘：
T_map_laser = T_map_base × T_base_laser。名字中間相同的兩個座標系接起來，很好檢查有沒有寫對。 - 反轉就是反矩陣：
T_base_map是T_map_base的反矩陣。旋轉部分的反矩陣剛好是轉置，所以不必真的解方程。
矩陣乘法不能交換順序。 「先往前走再左轉」和「先左轉再往前走」會到不同的地方。寫反了不會有任何錯誤訊息，只會得到一個錯的位置。
3D 時矩陣變成 4×4，左上 3×3 是旋轉，想法完全一樣。
組合、順序與反轉
用 numpy 寫一次。@ 是矩陣乘法。特別看第二行輸出：同樣兩個矩陣，順序換了，答案就不同。
角度相減與四元數
角度是繞一圈回來的。 朝向 170° 的機器人要轉到 −170°，直接相減得到 −340°，等於叫它順時針轉快一圈；其實逆時針轉 20° 就到了。解法是把差值放回 −180° 到 180° 之間：
diff = math.atan2(math.sin(goal - current), math.cos(goal - current))
之後的 PID 控制、路徑跟隨都要這一行。少了它，機器人平常都正常，只有轉過 ±180° 的時候突然繞一大圈，這種錯誤很難發現。
3D 的朝向怎麼表示。 直覺的寫法是三個角度（歐拉角），但有兩個問題：順序有很多種寫法，REP-103 也提到光是慣例就有 24 種；另外在某些姿勢兩個軸會重合，少了一個能轉的方向，這叫萬向鎖（gimbal lock）。所以 REP-103 建議優先用四元數（quaternion）：四個數字 x、y、z、w，沒有奇異點，也比矩陣精簡。
四元數不必直觀地想像，記住機器人在平面上轉的情況就夠用了。朝向 θ 時：
x = 0, y = 0, z = sin(θ/2), w = cos(θ/2)
反過來：θ = 2 × atan2(z, w)
ROS 2 裡的朝向幾乎都是四元數，例如 /odom 的 pose.pose.orientation。它不是角度，不可以直接拿 z 來比大小。還有一點：(x, y, z, w) 和 (−x, −y, −z, −w) 代表同一個朝向，所以不能直接比較兩個四元數是否相等。
一步一步看角度與四元數
把剛才兩件事放在一起看：角度差怎麼處理，四元數和角度怎麼互換。
實作：光達的點換到地圖
光達裝在車頭前方 mount 公尺，方向和車頭相同。寫出 laser_to_map：輸入光達量到的距離與角度（度），以及機器人在地圖上的位姿（角度也是度），回傳這個點在地圖上的座標。
三個步驟：極座標換成 laser 座標、加上 mount 換到 base_link、旋轉加平移換到 map。
確認一下
依 REP-103，機器人左手邊 0.5 公尺的東西，在 base_link 的座標是？
機器人在地圖 (3, 2)、朝向 90°。它正前方 1 公尺的點，在地圖上是哪裡？
下面這段程式會印出什麼？
import math
a = math.radians(-179) - math.radians(179)
print(round(math.degrees(math.atan2(math.sin(a), math.cos(a)))))
程式把 T_base_laser × T_map_base 當成 T_map_laser（順序反了）。會發生什麼事？
機器人用 /odom 算角度差，只寫 goal - current，沒有放回 ±180° 之內。什麼時候會出問題？
重點整理
- 同一個東西在不同座標系有不同的數字。問「在哪裡」之前，先確認「相對於誰」。
- REP-103：x 朝前、y 朝左、z 朝上，右手座標系，逆時針為正，單位是公尺與弧度。
- 換座標系就是旋轉加平移；寫成齊次轉換矩陣後，組合是相乘、反向是反矩陣，順序不能交換。
- 角度相減要用
atan2(sin, cos)放回 ±180° 之內。 - ROS 2 的朝向多半是四元數。平面上 z ＝ sin(θ/2)、w ＝ cos(θ/2)，朝向 θ ＝ 2 × atan2(z, w)。
下一課回到 ROS 2：程式怎麼拆成節點，節點之間怎麼用 topic 溝通。