本文為 AI 研習(xí)社編譯的技術(shù)博客，原標(biāo)題 ：

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 是個機(jī)器人。技術(shù)上說他是個過于簡單的機(jī)器人虛擬模型， 但對我們的目的來說足夠了。Robby 迷失在它的虛擬世界，這個世界由一個2維平面構(gòu)成，里面有許多地標(biāo)。他有一張周圍環(huán)境的地圖（其實不需要地圖也行），但是他不知道他在環(huán)境中的確切位置。

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

這個文章的目的是教你用地標(biāo)檢測和擴(kuò)展卡爾曼濾波器一步一步實現(xiàn)機(jī)器人定位。

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

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

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

因為Wikipedia 關(guān)于卡爾曼濾波器的信息流圖太好了，我這里就直接用它了：

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

我不會深入探討卡爾曼濾波器的數(shù)學(xué)計算細(xì)節(jié)，因為很多聰明人已經(jīng)做過了。如想要了解更深層次的解釋，我可以推薦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…

  第二部分-擴(kuò)展卡爾曼濾波器

擴(kuò)展卡爾曼濾波器（如名字所示）是“標(biāo)準(zhǔn)”卡爾曼濾波器的擴(kuò)展。在上節(jié)內(nèi)容我沒有告訴你的一個隱含的假設(shè)：當(dāng)使用卡爾曼濾波器時，狀態(tài)轉(zhuǎn)移和測量必須是線性模型。從數(shù)學(xué)觀點，這意味著我們可以采用這個假設(shè)和線性代數(shù)的優(yōu)雅來更新機(jī)器人狀態(tài)和機(jī)器人測量。實際上，這意味著狀態(tài)變量和測量值隨著時間線性改變。舉個例子，如果我們測量機(jī)器人的X 方向位置。 我們假設(shè)機(jī)器人在時刻t1 位于x1, 它在t2時刻必定位于x2位置。 變量v表示機(jī)器人在x 方向的速度。假設(shè)機(jī)器人實際上在加速， 或任意非線性運動（例如 沿著圓周運動），狀態(tài)轉(zhuǎn)移模型有點錯誤。在大多數(shù)情形下，并沒有多大的錯誤。但是在某些邊界情形，這個線性假設(shè)就錯的離譜。

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

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

這就是結(jié)果。擴(kuò)展卡爾曼濾波基本上是“正?！笨柭鼮V波，只是對現(xiàn)有的非線性狀態(tài)轉(zhuǎn)移模型和測量模型進(jìn)行了額外的線性化。

在我們的例子中，Robby迷路了，想要在這個(有爭議的)敵對環(huán)境中進(jìn)行本地化，擴(kuò)展卡爾曼濾波使Robby能夠感知地標(biāo)并相應(yīng)地更新其狀態(tài)信念。如果狀態(tài)估計值和測量估計值的方差足夠低，羅比很快就能非常確定他所處的位置相對于地標(biāo)的位置因為他知道地標(biāo)的確切位置，他知道自己在哪里!

他的快樂指數(shù)飆升!

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

根據(jù)面向?qū)ο缶幊?，我實現(xiàn)了下面的類：

• Robert類

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

• KalmanFilter類

這個類毫無疑問非常復(fù)雜。他的成員是矩陣。矩陣可以用來狀態(tài)轉(zhuǎn)換，觀測，計算協(xié)方差。我會掠過大部分細(xì)節(jié)，因為代碼注釋已經(jīng)提供了提示關(guān)于代碼的目的。過濾在localization_landmarks函數(shù)里實現(xiàn)。

• Landmark類

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

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

所有的代碼，可以發(fā)現(xiàn)在github:

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

愿你享受這個過程!   ?

想要繼續(xù)查看該篇文章相關(guān)鏈接和參考文獻(xiàn)？

點擊【使用卡爾曼濾波器和路標(biāo)實現(xiàn)機(jī)器人定位】或長按下方地址：

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

AI研習(xí)社今日推薦：雷鋒網(wǎng)雷鋒網(wǎng)雷鋒網(wǎng)

李飛飛主講王牌課程，計算機(jī)視覺的深化課程，神經(jīng)網(wǎng)絡(luò)在計算機(jī)視覺領(lǐng)域的應(yīng)用，涵蓋圖像分類、定位、檢測等視覺識別任務(wù)，以及其在搜索、圖像理解、應(yīng)用、地圖繪制、醫(yī)學(xué)、無人駕駛飛機(jī)和自動駕駛汽車領(lǐng)域的前沿應(yīng)用。

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