Learning Objectives
- 說明定位問題：有地圖、有感測器，要估計自己的位置
- 說出蒙地卡羅定位的三個步驟：移動、權重、重新取樣
- 解釋粒子數的取捨，以及「粒子都擠在一起」為什麼不代表定位正確
- 說明對稱環境與綁架問題，以及隨機粒子怎麼幫忙恢復
有地圖，但不知道自己在哪裡
開機的時候，機器人手上有一張圖書館的地圖，光達看到左邊有牆、前面三公尺有書架。這個描述符合地圖上好幾個地方，它到底在哪裡？
里程計幫不上忙：它只知道「從開機以來走了多少」，不知道開機時在哪裡，而且誤差會一路累積（第 8 課）。第 12 課的卡爾曼濾波也不夠用：它的估計只有一個高斯分布，也就是「一個最可能的位置加上一個範圍」，沒辦法同時說「我可能在東邊走廊，也可能在西邊走廊」。
粒子濾波（particle filter）換一個表示法：不寫公式，直接放幾百個猜測。每一顆粒子都是一個完整的假設「我在 (x, y)，朝向 θ」。哪裡的粒子多，就代表機器人覺得自己比較可能在那裡。這樣任何形狀的分布都表示得出來，包括同時有兩群。
用在定位上就叫蒙地卡羅定位（Monte Carlo Localization，MCL），Dellaert、Fox、Burgard 與 Thrun 在 1999 年提出。每一步只做三件事：
| 步驟 | 做什麼 | 粒子雲的變化 |
|---|---|---|
| 移動 | 機器人動了多少，每顆粒子就跟著動多少，再多加一點隨機誤差 | 散開一點 |
| 權重 | 每顆粒子算「如果我在這裡，光達應該量到多少」，和實際量到的比，越接近權重越大 | 位置不動，大小不同 |
| 重新取樣 | 依權重重新抽一批粒子，權重高的被抽到很多次，權重低的消失 | 集中 |
移動時為什麼要故意加誤差？因為真實的里程計本來就不準；而且如果重新取樣之後大家都是同一顆的複製品、之後又完全一樣地移動，粒子就永遠分不開，失去探索附近位置的能力。
一步一步看一輪粒子濾波
為了看得清楚，這裡只有一維的走廊和 4 顆粒子：走廊在位置 2 和位置 7 有門。機器人往前走 1 格，感測器回報「旁邊有門」。注意看權重怎麼決定下一批粒子。
動手寫：一維走廊的粒子濾波
機器人在長度 20 的走廊往前走，門的位置不規則。感測器回報旁邊有沒有門，有 10% 的機率講錯。移動、感測與起點的程式都寫好了，亂數種子固定，所以每次結果相同。補上權重和重新取樣兩步，估計值要慢慢貼近真實位置，分散程度要變小。
random.choices(particles, weights=w, k=N) 會依權重抽 N 次。
粒子濾波會在哪裡騙你
粒子全部擠成一團，看起來很有把握。但集中不等於正確，分散程度只說明粒子彼此同意，不代表它們和世界一致。下面三種情況都會遇到：
對稱的環境。 兩條長得一模一樣的走廊，光達看到的完全相同，光靠觀測分不出來。此時最誠實的答案就是兩群粒子。如果程式只取平均，平均值會落在兩群中間，甚至落在牆裡面，所以取估計值之前要先看分布的形狀。
粒子耗盡（particle deprivation）。 就算兩個位置機率相等，重新取樣每次都有隨機性，其中一群可能因為運氣不好慢慢變小，最後消失。消失之後就回不來了，而且系統不會有任何警告。
綁架問題（kidnapped robot）。 有人把機器人抱到別的地方（搬動、電梯、充電站）。粒子全部集中在舊位置，附近已經沒有任何粒子對得上新的觀測，權重只是在一群全錯的粒子之間比較誰比較不錯，濾波器不會自己發現。
常見的補救方法是每一輪換掉一小部分粒子，改成隨機放在地圖上。平常這些隨機粒子權重低，很快被淘汰，代價只是少了幾顆粒子；一旦真實位置和粒子雲對不上，落在真實位置附近的隨機粒子就會勝出。
ROS 2 Nav2 使用的定位套件 AMCL（Adaptive Monte Carlo Localization）就是這一類方法的實作：它會依粒子的分散程度調整粒子數；開啟恢復參數（recovery_alpha_slow、recovery_alpha_fast，預設是 0，也就是關閉）之後，定位變差時也會加入隨機粒子。它處理的是局部定位加上有限度的恢復，真實部署通常還是要給一個初始位置，例如在 RViz 上點一下。
確認一下
重新取樣之後，有些粒子是同一顆的複製品。接下來如果「移動」階段完全不加雜訊，會發生什麼事？
兩條對稱的走廊各有一群粒子，每群占一半。程式直接把所有粒子取平均當作機器人的位置，會得到什麼？
把感測模型的標準差設得比實際雜訊小很多（非常相信感測器），最可能發生什麼事？
機器人在餐廳裡定位很好，午餐時間人潮湧入，光達看到的大多是人。下面哪個現象最可能出現？
重點整理
- 定位是「有地圖，估計自己在哪裡」。粒子濾波用一堆猜測表示機率分布，可以同時有好幾群。
- 每一步：移動（加雜訊）、權重（比較預測與實際觀測）、重新取樣（權重高的被複製）。
- 粒子多比較可靠，每步要算的量也成比例增加。
- 集中不等於正確。粒子可能全部擠在錯的地方，要另外檢查觀測和地圖吻不吻合。
- 對稱環境要靠不對稱的線索；綁架問題要靠隨機粒子。AMCL 是這類方法的實作。
現在機器人有地圖，也知道自己在哪裡了。下一課處理：要去的地方怎麼走。