使用卡爾曼濾波器和路標(biāo)實(shí)現(xiàn)機(jī)器人定位

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

Robot localization with Kalman-Filters and landmarks

作者 | Jannik Zürn

翻譯 | 郭乃嶠、ThomasGui              

校對(duì) | Disillusion        審核 | 醬番梨       整理 | 立魚(yú)王

原文鏈接：

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

讓我來(lái)介紹一下——Robby 是個(gè)機(jī)器人。技術(shù)上說(shuō)他是個(gè)過(guò)于簡(jiǎn)單的機(jī)器人虛擬模型， 但對(duì)我們的目的來(lái)說(shuō)足夠了。Robby 迷失在它的虛擬世界，這個(gè)世界由一個(gè)2維平面構(gòu)成，里面有許多地標(biāo)。他有一張周?chē)h(huán)境的地圖（其實(shí)不需要地圖也行），但是他不知道他在環(huán)境中的確切位置。

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

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

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

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

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

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

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

我不會(huì)深入探討卡爾曼濾波器的數(shù)學(xué)計(jì)算細(xì)節(jié)，因?yàn)楹芏嗦斆魅艘呀?jīng)做過(guò)了。如想要了解更深層次的解釋?zhuān)铱梢酝扑]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)容我沒(méi)有告訴你的一個(gè)隱含的假設(shè)：當(dāng)使用卡爾曼濾波器時(shí)，狀態(tài)轉(zhuǎn)移和測(cè)量必須是線性模型。從數(shù)學(xué)觀點(diǎn)，這意味著我們可以采用這個(gè)假設(shè)和線性代數(shù)的優(yōu)雅來(lái)更新機(jī)器人狀態(tài)和機(jī)器人測(cè)量。實(shí)際上，這意味著狀態(tài)變量和測(cè)量值隨著時(shí)間線性改變。舉個(gè)例子，如果我們測(cè)量機(jī)器人的X 方向位置。 我們假設(shè)機(jī)器人在時(shí)刻t1 位于x1, 它在t2時(shí)刻必定位于x2位置。 變量v表示機(jī)器人在x 方向的速度。假設(shè)機(jī)器人實(shí)際上在加速， 或任意非線性運(yùn)動(dòng)（例如 沿著圓周運(yùn)動(dòng)），狀態(tài)轉(zhuǎn)移模型有點(diǎn)錯(cuò)誤。在大多數(shù)情形下，并沒(méi)有多大的錯(cuò)誤。但是在某些邊界情形，這個(gè)線性假設(shè)就錯(cuò)的離譜。

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

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

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

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

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

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

根據(jù)面向?qū)ο缶幊蹋覍?shí)現(xiàn)了下面的類(lèi)：

• Robert類(lèi)

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

• KalmanFilter類(lèi)

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

• Landmark類(lèi)

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

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

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

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

愿你享受這個(gè)過(guò)程!   ?

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

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

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

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

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

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