№ 008互動
一邊畫地圖，一邊找自己：從零寫一個 SLAM
一台沒有 GPS、沒有地圖的小車，怎麼一邊畫地圖、一邊知道自己在地圖上的哪裡。開著它繞一圈，看地圖慢慢歪掉，再在回到起點的那一刻整張彈正。
- 發布
- 更新
- 閱讀時間
- 6 分鐘
上一篇的機器狗會走路了，但牠不知道自己走到哪裡。牠沒有 GPS（室內收不到），也沒有人給牠地圖。
這件事麻煩在它是一個雞生蛋的問題：要知道自己在哪，得先有地圖；要畫地圖，得先知道自己在哪。 兩件事只能同時做。這個問題叫 SLAM，同時定位與建圖。
先開車。標著「真實世界」的那一格，車子自己看不到；另一格是車子自己畫出來的地圖，從上面往下看。按「自動駕駛」（前半圈會快轉），或用搖桿自己開，沿著走廊繞一圈回到起點。
車子估的路徑 只靠輪子算的路徑 認出舊地方時拉上的線
拖曳搖桿開車；或先點一下搖桿，再用方向鍵／WASD。放開就停。
快繞完一圈時，車子以為的位置和真正的位置已經差了兩公尺左右。回到起點的那一步，整張圖啪地彈正，誤差只剩一兩公分。
這篇文章要講的就是那一下是怎麼發生的。核心的程式大約 500 行 TypeScript，沒有用任何函式庫。
車子只有兩種感覺
輪子。 車子知道輪子轉了多少，所以知道「我剛剛往前 10 公分、左轉 1 度」。這個感覺永遠都有，但每一步都有一點誤差，而誤差只會累積、不會抵銷。
一圈測距。 車頂有一個旋轉的雷射測距儀（光達），每一圈量出 180 個方向上離牆多遠，誤差只有 2 公分。它很準，但它只告訴你「現在周圍長什麼樣子」，不告訴你「你在哪裡」。
如果只相信輪子會怎樣？下面是同一條路線繞兩圈，白線是真正走的路，粉紅線是把輪子的回報一步一步加起來得到的路。
預設的不準程度是每公尺多轉 0.006 弧度，大約三分之一度，用眼睛看不出車子在偏。兩圈之後卻差了 4.3 公尺。更麻煩的是，這條粉紅線沒有任何一段「看起來錯」。只靠輪子，你連自己錯了都不會知道。
把兩張掃描對起來
光達幫得上忙的地方在這裡：兩個時刻各掃一圈，只要看到的是同一批牆，把兩張掃描疊到剛好重合，需要移動多少、轉多少，就是車子在這兩個時刻之間的相對位置，準到公分。
試試看用手把青色的掃描拖去對白色的，再按「交給 ICP」。
拖曳青色的掃描去對白色的掃描（或先點一下畫面，用方向鍵移動）；滑桿可以轉它。
在轉角，一開始差了兩公尺、快三十度，它十步之內就對回來，最後的誤差不到一公分。按「打亂」多試幾次：大部分的起點都救得回來；救不回來的時候，它自己也知道，因為對得上的點只剩一兩成。
然後切到走廊中段再按一次。這次青色的掃描只是沿著走廊往後退了兩公尺，它卻停在離正確答案快一公尺的地方，而且看起來很有把握：七成多的點對上了，離牆平均只有 3.5 公分。兩面平行的牆只告訴你「離牆多遠」，沒有告訴你「沿著牆走了多遠」；往前滑一公尺，掃描看起來幾乎一樣。
好消息是，這種不確定算得出來，所以車子可以直接不採用這種配對。
所有的「我覺得」，放進同一張圖
現在車子手上有兩種資訊：
- 每走一小段，輪子說：「我覺得我從上一個點到這個點，移動了這麼多。」不太準，但每一段都有。
- 偶爾，光達說：「我覺得我現在的位置，和很久以前那個點的相對位置是這樣。」很準，但只有回到舊地方才有。
把車子沿路記下的每個位置畫成一個點，每一句「我覺得」畫成兩點之間的一根彈簧，彈簧的自然長度就是那句話的內容，越有把握的話，彈簧越硬。這張圖叫位姿圖。
只有輪子的彈簧時，整條鏈子是鬆的，怎麼擺都行，所以它就照輪子說的歪著。按下「加上閉環」，多出一根又硬又短的彈簧，把鏈子的頭和尾拉在一起：
重點是被修正的不只是最後那個點。新彈簧的拉力會沿著整條鏈子分攤下去，每一段都讓一點，整條路徑一起變直。而地圖是掛在這些點上的（每個點記著當時那一圈掃描），點動了，地圖就跟著動。這就是開場那一下「啪」。
數學上，「讓總張力最小」是一個最小平方問題，解法叫 Gauss–Newton：每一輪把問題當成線性的解一次，走一步，再來一輪。通常三四輪就不動了。
它會怎麼壞
同一條路線、同一台車，換四種設定各繞兩圈。下面是它最後畫出來的地圖。
同一條路線繞兩圈之後，車子畫出來的地圖。數字是最後的位置誤差。這四張圖是你的瀏覽器剛剛算出來的，沒有預先錄好。
不認舊地方。 沒有那根把頭尾拉在一起的彈簧，地圖一路歪下去，第二圈畫在第一圈的旁邊，變成兩條走廊。
輪子很不準。 它有在找舊地方，但一次都找不到。它是用「自己估計的位置」去翻附近的舊記錄，而估計已經漂到十幾公尺外，它根本不覺得自己回來過。
同樣的輪子，加一台相機。 每面牆漆上自己的條紋，車子多了一個只有一列像素的環景相機，改用「這個畫面我看過」來提名舊地方，再交給 ICP 確認。兩圈裡它認出舊地方二十一次，沒有一次認錯。靠位置找，漂遠了就找不回來；靠長相找，漂多遠都沒差。
認錯一次。 地圖本來是好的，只是硬加了一根錯的彈簧，宣稱兩個相距很遠的地方是同一點。我另外量過：一百零四根對的彈簧也救不回一根錯的，平均誤差從 0.10 公尺變成 3.9 公尺，因為最小平方對離群的資料沒有抵抗力。所以真的系統寧可漏掉十個舊地方，也不要認錯一個。按這張卡的按鈕，被弄壞的會是你自己在最上面繞出來的那張地圖。
機器狗身上的，差在哪裡
我們的機器狗用的是 3D 光達加相機。和這篇的小車比，前半段差很多，後半段幾乎一樣。
光達從一圈變成一團。 16 條雷射上下排開，轉一圈得到五千多個點的點雲。位置也不再是平面上的三個數字（x、y、朝向），而是六個：多了高度、俯仰和側傾，因為狗走路時身體會晃。但「把兩次掃描對起來」的做法完全一樣，只是小方程式從三個未知數變成六個。
白：已經對齊的舊點雲。青：現在這一圈。青線：估計的路徑；粉紅線：真正的路徑。
單次比對的誤差不到一公分、四分之一度，每一圈只要十到三十毫秒。但繞房間三十公尺之後，它還是漂了六十幾公分、六度多。誤差小，不等於誤差不累積。 補救的方法就是前面那條鏈子，只是每個點從三個數字換成六個。
相機負責認地方。 就是上一節那台相機的真實版本：不是一列像素，而是整張影像裡的特徵點，用途相同。
真的系統還要處理這裡完全沒碰的事：光達轉一圈要十分之一秒，這段時間狗在動，點雲本身就是歪的，所以通常會和慣性感測器綁在一起；還有走動的人、玻璃、長得一模一樣的走廊。
但核心就是你剛剛玩過的三件事：把兩次觀測對起來，把所有的「我覺得」放進一張圖，然後在認出舊地方的那一刻，讓整張圖一起鬆開。
來源
- 位姿圖與 Gauss–Newton：Grisetti, Kümmerle, Stachniss, Burgard, A Tutorial on Graph-Based SLAM, IEEE Intelligent Transportation Systems Magazine, 2010。
- 真實的 2D 光達 SLAM 怎麼在大範圍裡找舊地方：Hess, Kohler, Rapp, Andor, Real-Time Loop Closure in 2D LIDAR SLAM, ICRA 2016（Cartographer）。