若無法得到車輛在地圖上的準(zhǔn)確位置及周圍障礙物的位置,那么自動駕駛便無從談起。因此在自動駕駛領(lǐng)域,對車輛自身及周圍物體的精確定位至關(guān)重要。然而現(xiàn)實環(huán)境中存在各類噪聲,無論是激光雷達(dá)、慣性導(dǎo)航器件甚至衛(wèi)星定位系統(tǒng)都無法得到完全真實的車輛位置信息。為了應(yīng)對此類情況,便需要采用濾波算法來過濾觀測噪聲,以得到更加精確的定位結(jié)果。
前言
1
何為卡爾曼濾波
在各類濾波定位算法中,卡爾曼濾波是最為知名的一種。為了解決阿波羅登月計劃中的航天器定位及噪聲干擾問題,這種算法最初在1963年由匈牙利數(shù)學(xué)家魯?shù)婪颉た柭岢鯷1]。卡爾曼濾波不僅將過去類似的基于系統(tǒng)可觀性與可控性的控制方法做出了系統(tǒng)化的整理,并且在嚴(yán)格的數(shù)學(xué)意義上證明這種方法在線性系統(tǒng)的高斯隨機(jī)噪聲過濾方法中具有最優(yōu)性。
卡爾曼濾波的簡單舉例
為了更好的理解卡爾曼濾波的基本原理,本文首先對卡爾曼濾波的原理進(jìn)行舉例說明。
試想一下,假設(shè)你要憑借地圖從A地前往B地,你要如何知道自己的位置呢?
首先,你可以通過自己的腳程判斷,“A地到B地大約10公里,憑我的腳程大約需要步行3小時左右”。
其次,還可以通過路牌和手里的地圖判斷,“這個路牌上寫著到了D鎮(zhèn),地圖上顯示D鎮(zhèn)應(yīng)該在距離B地還有3公里的地方”。
然而不論是地圖還是腳程估算都存在的較大誤差,此時我們?yōu)榱说玫礁鼫?zhǔn)確的所處位置,可以結(jié)合地圖與腳程來對當(dāng)前位置進(jìn)行估算。比如:A地到B地大約10公里,憑我的腳程大約需要步行3小時左右,目前已經(jīng)走了2小時,那么我推測再過不久就可以到達(dá)D鎮(zhèn),此時再對照地圖上D鎮(zhèn)的位置,就可以得到更加精確的位置信息,同時通過對比實際到達(dá)D鎮(zhèn)的時間,還可以反過來修正對于自己腳程的估計誤差。
在這個例子里,通過自己腳程估計的位置,在卡爾曼濾波里就被稱為狀態(tài)預(yù)測,其誤差則為估計誤差,估計方法為預(yù)測矩陣,地圖上自己的位置則為狀態(tài)觀測,從地圖換算到當(dāng)前位置的方法則被稱為觀測矩陣。卡爾曼的關(guān)鍵就在于對即將到來的觀測進(jìn)行預(yù)測,并用其對比最后實際得到的觀測,通過這兩者之間的差值進(jìn)行修正。假設(shè)用一個笛卡爾坐標(biāo)系來對這種方法進(jìn)行表示,則可參考圖1。
圖1 卡爾曼濾波原理示意圖
2
卡爾曼濾波的基本公式
根據(jù)上一節(jié)的例子,我們就可以引出卡爾曼濾波的基本公式:
(1) (2) (3) (4) (5) (6)
前兩步是根據(jù)上一個最優(yōu)估計值得出此刻的估計值和估計值的協(xié)方差,緊接著就可以得到此刻的最優(yōu)估計值和最優(yōu)估計值的協(xié)方差,然后利用此刻的最優(yōu)估計值和最優(yōu)估計值的協(xié)方差進(jìn)行下一個迭代。
公式詳解
看起來有點多,但是實際上非常緊湊,推理非常嚴(yán)謹(jǐn),接下來將對上述六個公式進(jìn)行詳細(xì)解釋。
式(1)為系統(tǒng)的狀態(tài)預(yù)測公式,通過上一時刻的位置對當(dāng)前時刻的位置進(jìn)行了推測,但是由于過程噪聲的存在,這個推測出的位置不完全符合真實情況。
式(2)為系統(tǒng)的狀態(tài)誤差傳遞,類似式(1),是基于上一時刻位置可能分布的范圍對當(dāng)前時刻的可能分布范圍做出預(yù)測,Q是可能的估計誤差。
式(3)則是當(dāng)前時刻觀測量的預(yù)測值,代表我們根據(jù)預(yù)測出的狀態(tài),推測出我們將會觀測到的讀數(shù),并非真正的讀數(shù)。
式(4)為卡爾曼增益的計算方法,其物理意義為狀態(tài)預(yù)測誤差與觀測誤差的比例關(guān)系。
式(5)則為對系統(tǒng)狀態(tài)的更新,這個公式可以分為兩方面看,后半段的代表實際讀數(shù)與推測讀數(shù)之間的差值,被稱為殘差。使用式(4)求得的卡爾曼增益與殘差相乘,可以得到對狀態(tài)預(yù)測的修正量,最終式(5)可以得到圖1中的卡爾曼位置估計更新。
式(6)則為對位置估計的分布估計,可以作為下一時刻式(2)的初值使用。
實例說明
假設(shè)車輛在進(jìn)行勻速運(yùn)動,則可以設(shè)置其遞推運(yùn)動模型為:
(7) (8)
其中式(7)代表k時刻的車輛狀態(tài),包含車輛的坐標(biāo)和在橫縱坐標(biāo)軸的速度,式(8)則為勻速運(yùn)動狀態(tài)轉(zhuǎn)換矩陣,可以通過將式(7)、(8)帶入式(1)求取下一時刻的狀態(tài)預(yù)測。
之后我們給出狀態(tài)誤差矩陣的迭代初值,通常設(shè)置為一個與狀態(tài)同階的單位陣,帶入式(2)即可求得下一時刻的誤差矩陣預(yù)測,這里Q需要根據(jù)估計模型的誤差來人為給定。
下一步,我們假設(shè)運(yùn)動系統(tǒng)使用GPS作為觀測器,經(jīng)過預(yù)處理的GPS信號可以直接輸出車輛的位置坐標(biāo),那么GPS的觀測模型可以簡化為:
(9) (10)
將式(9)與式(1)的結(jié)果帶入式(3),可以得到基于狀態(tài)預(yù)測的觀測值預(yù)測,系統(tǒng)觀測誤差的方差矩陣R可以根據(jù)傳感器的技術(shù)說明書確定。
最后,按順序?qū)⑸鲜鲎兞繋胧剑?)、(5)、(6)即可實現(xiàn)對位置坐標(biāo)的更新,同時得到的結(jié)果還可以作為下一步迭代的初值。
3
非線性卡爾曼濾波
雖然卡爾曼濾波已經(jīng)在數(shù)學(xué)上被嚴(yán)格證明具有最優(yōu)性,然而卻無法處理非線性系統(tǒng)。針對這種情況,現(xiàn)有的解決方法分為兩種,分別是基于泰勒展開進(jìn)行線性化近似的擴(kuò)展卡爾曼濾波[2]和基于多點采樣進(jìn)行均值方差近似的無跡卡爾曼濾波。
擴(kuò)展卡爾曼濾波
擴(kuò)展卡爾曼濾波通過一組式(11)所示的n階泰勒線性展開式近似表達(dá)非線性函數(shù):
(11)
如圖2所示,其階數(shù)越高,近似程度越高。
圖2 各級泰勒展開線性化效果
這種方法具有計算復(fù)雜度低、技術(shù)成熟的優(yōu)點,因此,目前在工程中被廣泛使用。然而由于泰勒展開式的復(fù)雜度隨著階數(shù)呈指數(shù)增長,往往在實際應(yīng)用中很難實現(xiàn)三階以上的泰勒展開,這導(dǎo)致了這種線性化方法存在較大的截斷誤差,因此限制了算法的估計精度,并且在非線性程度較高的系統(tǒng)中容易發(fā)散。
無跡卡爾曼濾波
無跡卡爾曼濾波則通過對非線性函數(shù)在目標(biāo)時刻的均值與方法進(jìn)行相似擬合來實現(xiàn)線性化近似的(如圖3所示),這種方法被稱為無跡變換(UT變換)。
圖3 無跡變換原理
UT變換已被Julier和Uhlmann[3]證明具有不低于三階泰勒展開的線性化精度,因此基于UT變換的無跡卡爾曼濾波具有相對于擴(kuò)展卡爾曼濾波更高的精度,同時其均值與方差的擬合不會受函數(shù)非線性化程度的影響,因此可以適用于更為非線性的系統(tǒng)。然而,由于使用無跡卡爾曼濾波需要設(shè)置較多參數(shù),并且計算負(fù)擔(dān)較重,因此在實際工程中仍然不能完全取代擴(kuò)展卡爾曼濾波。
基于上述原因,目前在非線性系統(tǒng)上需要根據(jù)系統(tǒng)特征與實際需求對濾波器的性能做出取舍,選擇最合適的方法進(jìn)行計算。
4
SLAM中的卡爾曼濾波與圖優(yōu)化
由于SLAM(Simultaneous Localization and Mapping,同步定位與建圖)中對于特征點、路徑與移動體的定位同卡爾曼濾波的適用范圍十分吻合,因此,早期SLAM中通常采用卡爾曼濾波在后端對輸出進(jìn)行優(yōu)化。隨著計算能力的不斷加強(qiáng),近年來基于圖優(yōu)化的SLAM方法也逐漸占據(jù)了一席之地。
從應(yīng)用角度分析,基于卡爾曼濾波和基于圖優(yōu)化的SLAM方法各有優(yōu)勢。
卡爾曼濾波具有計算速度快、系統(tǒng)簡單的優(yōu)點,但由于其對后端輸出的優(yōu)化僅基于相鄰兩個馬爾可夫鏈的信息進(jìn)行一次迭代,因此在精度上有所欠缺,更適用于高動態(tài)且對精度要求不高的場合。
相對的,因子圖優(yōu)化可以統(tǒng)籌全局信息以對各位姿輸出進(jìn)行優(yōu)化,并且優(yōu)化迭代次數(shù)可以人為設(shè)置,可以得到更加精確的后端輸出。正因如此,其每次迭代及出現(xiàn)新的輸出時都需要對全局進(jìn)行計算,使得計算負(fù)擔(dān)大幅增加。所以一般情況下,基于圖優(yōu)化的SLAM僅適用于對于精度要求極高的特殊場合。
隨著技術(shù)的發(fā)展,這兩種路線也在分別補(bǔ)齊自身的固有短板。
如基于濾波的SLAM中引入了迭代擴(kuò)展卡爾曼濾波,將最小二乘法與卡爾曼濾波相結(jié)合,對誤差而不是位姿進(jìn)行優(yōu)化,實現(xiàn)了更高的定位精度,并且可以實現(xiàn)動態(tài)設(shè)置迭代次數(shù),代表算法有LINS和Fast-Lio等。
反之,為了應(yīng)對圖優(yōu)化“牽一發(fā)而動全身”的問題,最近比較熱門的LIOM和LIO-SAM等算法引入了動態(tài)滑窗和關(guān)鍵幀機(jī)制,僅對少數(shù)相關(guān)的時刻進(jìn)行聯(lián)系,有效降低了冗余計算,并且引入了IMU預(yù)積分等技術(shù),進(jìn)一步削減系統(tǒng)的計算負(fù)擔(dān),獲得了更快的動態(tài)響應(yīng)時間。
總體來看,基于圖優(yōu)化的SLAM是未來的發(fā)展趨勢,而目前產(chǎn)品化的SLAM設(shè)備仍以濾波為主流。
審核編輯 :李倩
-
gps
+關(guān)注
關(guān)注
22文章
2981瀏覽量
169147 -
算法
+關(guān)注
關(guān)注
23文章
4708瀏覽量
95245 -
卡爾曼濾波
+關(guān)注
關(guān)注
3文章
166瀏覽量
25065
發(fā)布評論請先 登錄
卡爾曼濾波算法
卡爾曼濾波簡介
卡爾曼濾波算法的改進(jìn)方案

基于擴(kuò)展卡爾曼濾波算法的無源定位效果分析

評論