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

2021-01-10 手機鳳凰網

本文為 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 庫去實現一些必要物體的圖像。這裡可以下載：

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

相關焦點

  • 卡爾曼濾波器的工作原理(一)
    視頻演示了一個卡爾曼濾波器通過測量一個自由浮動物體的速度計算出它的方向。1.卡爾曼濾波器是什麼？可以在任何有關某些動態系統的不確定信息的地方使用卡爾曼濾波器，並且可以對有關系統下一步做什麼進行有根據的預測。
  • 面向軟體工程師的卡爾曼濾波器
    與我的朋友交談時，我經常聽到：「哦，卡爾曼(Kalman)濾波器……我經常學它，然後我什麼都忘了」。好吧，考慮到卡爾曼濾波器(KF)是世界上應用最廣泛的算法之一(如果環顧四周，你80％的技術可能已經在內部運行某種KF)，讓我們嘗試將其弄清楚。
  • 移動機器人的定位
    然而在很多環境中機器人不能利用全局定位系統進行定位，而且事先獲取機器人工作環境的地圖很困難，甚至是不可能的。這時機器人需要在自身位置不確定的條件下，在完全未知環境中創建地圖,同時利用地圖進行自主定位和導航。這就是移動機器人的同時定位與地圖創建(SLAM) 問題，最先是由SmithSelf 和Cheeseman在1988年提出來的，被認為是實現真正全自主移動機器人的關鍵。
  • 基於卡爾曼濾波器和CAN智能從站技術實現開關磁阻電機調速系統設計
    打開APP 基於卡爾曼濾波器和CAN智能從站技術實現開關磁阻電機調速系統設計 方旭東，孫劍 發表於 2020-05-12 09:50:19
  • 為什麼叫「卡爾曼」?卡爾曼濾波器算法介紹
    我們現在要學習的卡爾曼濾波器，正是源於他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預測問題的新方法）。簡單來說，卡爾曼濾波器是一個「最優化自回歸數據處理算法」。
  • 機器人的雙眸:視覺SLAM是如何實現的?
    到目前為止，視覺SLAM的框架已基本成熟，可以在運動穩定、光照充足的環境中，在釐米級別的誤差範圍內進行定位與建圖。但是，離充滿動態光照和人群密集的現實環境，要應用視覺SLAM技術，仍有很長的路要走。以下，我會介紹SLAM的歷史、理論以及實現的方式，且主要介紹視覺（Kinect）的實現方式。
  • 視頻教程 | 理解卡爾曼濾波器(英語中字)
    卡爾曼濾波器是一種優化估算算法，用於在不確定和間接測量的情況下估算系統狀態。本視頻教程由7個小節組成，系統介紹了卡爾曼濾波器，言簡意賅，適合入門。各個小節的主要內容為：第一節：通過幾個案例了解使用卡爾曼濾波器的常見場景。了解卡爾曼濾波器背後的工作原理。第二節：介紹了解狀態觀測器的工作原理，並解釋其背後的數學原理。在無法直接測量時，使用狀態觀測器估算系統的內部狀態。
  • 開發者說 | 手把手教你寫卡爾曼濾波器
    這兩步完成後，我們就會獲得某一時刻，自車坐標系下的各種傳感器數據。這些數據包括障礙物的位置、速度；車道線的曲線方程、車道線的類型和有效長度；自車的GPS坐標等等。你會發現綠色區域的方差比紅色區域和藍色區域的小。
  • 為什麼叫「卡爾曼」,卡爾曼濾波器算法介紹
    我們現在要學習的卡爾曼濾波器，正是源於他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》(線性濾波與預測問題的新方法)。　　簡單來說，卡爾曼濾波器是一個「optimal recursive data processing algorithm(最優化自回歸數據處理算法)」。
  • 【強基固本】卡爾曼濾波器
    轉載來源：https://longaspire.github.io/blog/卡爾曼濾波/Kalman Filter (KF) 是一個高效的遞歸濾波器，它可以實現從一系列的噪聲觀測中，估計動態系統的狀態。01卡爾曼濾波器以它的發明者Rudolf. Emil. Kalman先生（2016年去世，向這位傳奇的科學家致敬）而命名。
  • 一代宗師卡爾曼
    它的廣泛普及由於數字計算機被有效地用於設計階段和實施階段。從理論的角度來看，它是基於一個共同的過濾和控制概念以及這兩個問題之間的對偶性。 1964年，卡爾曼前往史丹福大學，在電氣工程，力學和運籌學部門從事研究。在這期間，他的研究成果轉向了與實現理論和代數系統理論相聯繫的基本問題。
  • 卡爾曼與卡爾曼濾波
    先後居住於蘇黎世和佛羅裡達。2009年獲美國國家科學獎章。斯坦利·施密特(Stanley Schmidt)首次實現了卡爾曼濾波器。卡爾曼在NASA埃姆斯研究中心訪問時，發現他的方法對於解決阿波羅計劃的軌道預測很有用，後來阿波羅飛船的導航電腦使用了這種濾波器。
  • 擴展卡爾曼濾波器在同步電機無速度矢量控制系統中的應用
    0 引言　　矢量技術是目前應用比較廣泛的電機調速技術[1]，其採用雙閉環控制，且外環需要轉速作為反饋，同時坐標變換需要轉子磁極的位置信號，以實現轉子磁場與電樞電流在空間上的正交，使得在一定條件下產生的電磁轉矩最大[2]。因此，得到準確的轉子磁極的速度和位置信號是實現該控制系統的關鍵。
  • 透徹理解擴展卡爾曼濾波
    卡爾曼濾波是貝葉斯濾波的一種特例，是在線性濾波的前提下，以最小均方誤差為最佳準則的。估計線性高斯模型，是對線性模型和高斯分布的優化方法。邊緣分布和條件分布的模型：卡爾曼濾波器的主要參數卡爾曼濾波器假設x（paths）, z（observations）都為線性高斯的：主要參數：A是在沒有命令的情況下，由於環境因素造成的機器人的位置移動。
  • 卡爾曼濾波應用於自動駕駛
    下圖顯示了車輛中使用的少數重要部件的位置。自主車輛的組成部分卡爾曼濾波器使用的數據來自LIDAR和RADAR 。所以現在只關注這兩個方面。為何使用卡爾曼濾波器？我們可以使用卡爾曼濾波器進行有根據的猜測，在我們對某些動態系統有不確定信息的任何地方，系統將要做什麼。在自主車輛的情況下，卡爾曼濾波器可用於根據我們的車輛接收的數據預測我們的自動駕駛車輛前方的車輛將採取的下一組動作。這是一個使用兩步預測和更新的迭代過程。
  • 卡爾曼濾波是怎麼回事?
    維納濾波就是一個典型的降噪濾波器。卡爾曼濾波Kalman Filter 算法，是一種遞推預測濾波算法。Kalman Filter 提供了一種高效可計算的方法來估計過程的狀態，並使估計均方誤差最小。卡爾曼濾波器應用廣泛且功能強大：它可以估計信號的過去和當前狀態，甚至能估計將來的狀態，即使並不知道模型的確切性質。
  • 車載毫米波雷達之大話卡爾曼濾波
    所以，卡爾曼濾波在毫米波雷達的應用上，就扮演著至關重要的角色。卡爾曼&卡爾曼濾波什麼是卡爾曼濾波？卡爾曼濾波和傅立葉變換、泰勒級數一樣，都是以其發明人命名。稍微不同的是，卡爾曼是個現代人！卡爾曼是匈牙利數學家，1930年出生於匈牙利首都布達佩斯。
  • 卡爾曼濾波在電容觸控螢幕坐標定位中的應用
    觸控螢幕作為一種新的智能設備輸入裝置，其技術已經比較成熟，常見的類型可分為紅外線、表面聲波、電阻式和電容式等[1-2]。其中電容觸控螢幕中使用較廣泛的是投射電容式觸控螢幕，其根據製作、布線工藝和掃描方式不同又分為互電容和自電容。
  • 卡爾曼濾波的原理
    我們現在要學習的卡爾曼濾波器，正是源於他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預測問題的新方法）。簡單來說，卡爾曼濾波器是一個「optimal recursive data processing algorithm（最優化自回歸數據處理算法）」。