概率機器人：定位問題的學習6——蒙特卡羅定位演算法

來自專欄概率機器人專欄7 人贊了文章

繼續更新。

一、蒙特卡羅定位演算法

定位問題的最後一個用粒子表示置信度 的定位方法——蒙特卡羅定位演算法。其實就是粒子濾波定位演算法。同樣用於局部定位或者全局定位。目前做的很多項目大多數都採用了這種方法，比較容易實現，或者說比較直觀，容易理解。

1、演算法思路

蒙特卡羅定位的初始化就是通過隨機抽取的位姿粒子集合取得，一般來說初始化的時候，我們認為這個粒子均勻分布在整個位姿空間中。當機器人感知到一個路標時，它會給每個粒子分配係數，或者說權重，就是說會對初始化的粒子重新改變每個粒子的權重。

整個演算法的思路同樣用遵循測量-更新的思路。

1.對粒子進行初始化，使其均勻分布在整個位姿空間中

2.機器人進行移動感知路標位置，獲得測量模型，此時對粒子權重進行更新，注意此時粒子的分布沒有改變，更新的只是每個位姿粒子的權重，獲得了機器人可能所處的位置。

3.機器人開始下一次移動，此時進行重採樣，此時粒子集合分布改變為新的權重相同，但分布位置改變的粒子集合，改變之處在於，對機器人可能所處的位置增加了新的粒子。

4.機器人再次進行測量，獲得測量模型，綜合後，此時粒子重新分配為非均勻分布且權重不同的粒子集合。機器人大概率所處的位置就是粒子分布密集並且粒子所佔權重較大的部分。下一步就是機器人再次進行移動，並根據運動模型產生新的粒子分布集合。然後重複的過程。

註：粒子的權重用測量模型確定。

2.演算法步驟和實例下次更新時給出

3.蒙特卡羅定位的特點

蒙特卡羅定位優勢在於增加粒子總數的情況下總是可以提高近似的精度，那麼粒子的數目就是你使用這個演算法不能不考慮的參數，因為在定位需求中，任何一個演算法人員都需要去平衡定位時計算的精度和計算資源的問題。

4.綁架問題的解決

之前提到定位問題的時候，我們就著重提到了機器人的綁架問題，或者加上定位丟失時的恢復問題。

在蒙特卡羅定位演算法中，我們會對粒子分布和權重進行更新，那麼你會從演算法中發現，在整個位姿空間中，通過定位演算法，不處於可能的位置處的粒子會在更新中逐漸消失，那麼在定位問題中就會遇到一個很現實的問題，演算法在進行重採樣的步驟中可能會丟失正確位置處的粒子，從而帶來定位的失效，並且是不可恢復的，最終的結果就是定位演算法的失敗。

然而一個很簡單的思路就解決了這個問題。就是在演算法的迭代過程中不停的加入隨機粒子到粒子集合中，這樣不怎麼會影響到最終的定位結果，並且由於加入了隨機粒子，機器人的運動模型中產生了一些隨機的情況，從而避免了上述情況的發生。

當然提到這個就會說到兩種問題，每次迭代中加入多少隨機粒子和這些隨機分布的粒子如何產生？

問題一：評估定位性能增加粒子

問題二：由均勻分布在位姿空間產生粒子，用觀察值加權這些粒子，特殊情況例如：檢測地標模型無法由測量分布直接產生粒子，就使用似然的分布，直接確定附加粒子的位置。

特殊需要提到的問題：

對於蒙特卡羅定位來說，若是一個毫無雜訊的感測器，總能測量出機器人的正確位姿，那麼反而會帶來蒙特卡羅演算法定位的失效。總的來說就是蒙特卡羅定位在某個特定的情況下，利用不精確的感測器定位反而優於精確的感測器，EKF就不是這種情況。（EKF考慮在計算新的均值時，考慮了測量值，而蒙特卡羅演算法只從運動模型中產生均值）

方案：利用測量模型認為加入測量雜訊，從而避免這種情況。如此不僅適應了測量的不確定性，並且適應了粒子濾波演算法的近似特性帶來的不確定性。

另外的一些討論例如動態環境下帶來的新的定位問題以及缺失圖文說明下次更新補上。