使用卡爾曼濾波器和路標實現機器人定位

本文為 AI 研習社編譯的技術博客，原標題 ：

Robot localization with Kalman-Filters and landmarks

作者 | Jannik Zürn

翻譯 | 郭乃嶠、ThomasGui

校對 | Disillusion 審核 | 醬番梨 整理 | 立魚王

原文鏈接：

https://medium.com/@jannik.zuern/robot-localization-with-kalman-filters-and-landmarks-cf97fa44e80b

讓我來介紹一下——Robby 是個機器人。技術上說他是個過於簡單的機器人虛擬模型， 但對我們的目的來說足夠了。Robby 迷失在它的虛擬世界，這個世界由一個2維平面構成，裡面有許多地標。他有一張周圍環境的地圖（其實不需要地圖也行），但是他不知道他在環境中的確切位置。

Robby(紅色大圓圈)和2個地標（紅色小圓圈）

這個文章的目的是教你用地標檢測和擴展卡爾曼濾波器一步一步實現機器人定位。

卡爾曼濾波器可以理解為一種感知充滿雜訊的世界的方式。當我們要定位機器人在哪裡，依賴兩個條件：我們知道機器人如何從一個時刻移動到下個時刻，因為我們以某種確定的方式命令它移動。這稱為狀態轉移（即機器人如何從一個狀態轉移到另一個）而且我們能用各種感測器如相機，激光雷達或回波探測器（德語：毫米波雷達）測量機器人的環境。問題是這2類信息都受到雜訊影響。我們不能精確地知道機器人從一個狀態轉移到下一個狀態的精確程度，因為執行部件不完美。而且我們不能無限精確地測量物體間的距離。這就是卡爾曼濾波器發揮作用的場合。

卡爾曼濾波器允許我們結合當前狀態的不確定和它的感測器測量的不確定來理想地降低機器人的總體不確定程度。這兩類不確定通常用高斯概率分布或正態分布來描述。高斯分布有2個參數：均值和方差。均值表示最高概率的值，方差表示我們認為這個均值有多大的不確定性。

卡爾曼濾波器運行2個步驟。在預測步驟，卡爾曼濾波器以當前狀態變數值生成預測和不確定度。當觀測到下一次測量結果（必然有一定的誤差，包含雜訊），就能以加權平均的方式更新這些預測，確定程度高的預測給予更高的權重。演算法是遞歸的。它可以實時運行，僅需要當前測量輸入和前個計算的狀態和不確定矩陣；不需要更多的過去信息。

因為Wikipedia 關於卡爾曼濾波器的信息流圖太好了，我這裡就直接用它了：

卡爾曼濾波器圖片 來自：https://upload.wikimedia.org/wikipedia/commons/a/a5/Basic_concept_of_Kalman_filtering.svg

我不會深入探討卡爾曼濾波器的數學計算細節，因為很多聰明人已經做過了。如想要了解更深層次的解釋，我可以推薦Tim Babb的博客：

How a Kalman filter works, in picturesI have to tell you about the Kalman filter, because what it does is pretty damn amazing. Surprisingly few software…

第二部分-擴展卡爾曼濾波器

擴展卡爾曼濾波器（如名字所示）是「標準」卡爾曼濾波器的擴展。在上節內容我沒有告訴你的一個隱含的假設：當使用卡爾曼濾波器時，狀態轉移和測量必須是線性模型。從數學觀點，這意味著我們可以採用這個假設和線性代數的優雅來更新機器人狀態和機器人測量。實際上，這意味著狀態變數和測量值隨著時間線性改變。舉個例子，如果我們測量機器人的X 方向位置。 我們假設機器人在時刻t1 位於x1, 它在t2時刻必定位於x2位置。 變數v表示機器人在x 方向的速度。假設機器人實際上在加速， 或任意非線性運動（例如 沿著圓周運動），狀態轉移模型有點錯誤。在大多數情形下，並沒有多大的錯誤。但是在某些邊界情形，這個線性假設就錯的離譜。

同樣假設線性測量模型也會有問題。假設你正沿著直路行駛，在你前方的路旁有一個燈塔。而你離的比較遠，你測量到離燈塔的距離和它位於你視野的角度接近線性地改變（距離大致以你的車輛的速度來減少，而且角度基本不變）。但是當你越來越靠近，尤其當你行駛過它的時候，角度則急劇地改變。這就是為什麼當Robby在它的2-D 世界採用散落在它的2-D 平面的地標導航的時候，我不能再用線性卡爾曼濾波器。

擴展卡爾曼濾波器是拯救者，它解除了線性狀態轉移和測量模型的線性限制。而它允許使用任何非線性函數對你的機器人狀態轉移和測量建模。為了還能在我們的濾波器中使用有效而且簡單的線性代數的魔力，我們採取了一個技巧：我們在當前機器人狀態鄰域採取線性化。這意味著我們假設測量模型和狀態轉移模型在我們當前的狀態附近接近線性（再次引用路/燈塔的例子）。但在每個步驟之後，我們在新狀態的臨域線性化更新。而這個方法迫使我們對非線性函數採取線性化。

這就是結果。擴展卡爾曼濾波基本上是「正常」卡爾曼濾波，只是對現有的非線性狀態轉移模型和測量模型進行了額外的線性化。

在我們的例子中，Robby迷路了，想要在這個(有爭議的)敵對環境中進行本地化，擴展卡爾曼濾波使Robby能夠感知地標並相應地更新其狀態信念。如果狀態估計值和測量估計值的方差足夠低，羅比很快就能非常確定他所處的位置相對於地標的位置因為他知道地標的確切位置，他知道自己在哪裡!

他的快樂指數飆升!

第三部分-實現

實現的代碼是非常直接的。為了直觀，我選擇使用SDL2 庫去實現一些必要物體的圖像。這裡可以下載：

根據面向對象編程，我實現了下面的類：

• Robert類

這個類最重要的部分是Pose(x 的位置, y的位置, 方向) 和 Velocity (線速度和角速度)。它可以向前，向後，向右和想左旋轉。為了測量路標的位置，它有measureLandmarks方法，這個方法可以獲取真實的路標，並且考慮路標的位置和觀測噪音，從而得到觀測過的路標的列表。

• KalmanFilter類

這個類毫無疑問非常複雜。他的成員是矩陣。矩陣可以用來狀態轉換，觀測，計算協方差。我會掠過大部分細節，因為代碼注釋已經提供了提示關於代碼的目的。過濾在localization_landmarks函數里實現。

• Landmark類

這個類是最簡單的。他有位置，ID, 一個把自己呈現在屏幕上的方法。這就是全部了。

在主函數里，我們初始化所有並且開始無限循環，同時機器人的位置一直更新根據鍵盤的輸入。機器人估測他的環境，Kalman濾波預測和更新下一步。

所有的代碼，可以發現在github:

https://github.com/jzuern/robot-localization

願你享受這個過程! ?

想要繼續查看該篇文章相關鏈接和參考文獻？

點擊【使用卡爾曼濾波器和路標實現機器人定位】或長按下方地址：

https://ai.yanxishe.com/page/TextTranslation/1437

AI研習社今日推薦：雷鋒網雷鋒網雷鋒網

李飛飛主講王牌課程，計算機視覺的深化課程，神經網路在計算機視覺領域的應用，涵蓋圖像分類、定位、檢測等視覺識別任務，以及其在搜索、圖像理解、應用、地圖繪製、醫學、無人駕駛飛機和自動駕駛汽車領域的前沿應用。

加入小組免費觀看視頻：https://ai.yanxishe.com/page/groupDetail/19

喜歡這篇文章嗎？立刻分享出去讓更多人知道吧！