若無法得到車輛在地圖上的準(zhǔn)確位置及周圍障礙物的位置，那么自動(dòng)駕駛便無從談起。因此在自動(dòng)駕駛領(lǐng)域，對車輛自身及周圍物體的精確定位至關(guān)重要。然而現(xiàn)實(shí)環(huán)境中存在各類噪聲，無論是激光雷達(dá)、慣性導(dǎo)航器件甚至衛(wèi)星定位系統(tǒng)都無法得到完全真實(shí)的車輛位置信息。為了應(yīng)對此類情況，便需要采用濾波算法來過濾觀測噪聲，以得到更加精確的定位結(jié)果。

1

何為卡爾曼濾波

在各類濾波定位算法中，卡爾曼濾波是最為知名的一種。為了解決阿波羅登月計(jì)劃中的航天器定位及噪聲干擾問題，這種算法最初在1963年由匈牙利數(shù)學(xué)家魯?shù)婪颉た柭岢鯷1]?？柭鼮V波不僅將過去類似的基于系統(tǒng)可觀性與可控性的控制方法做出了系統(tǒng)化的整理，并且在嚴(yán)格的數(shù)學(xué)意義上證明這種方法在線性系統(tǒng)的高斯隨機(jī)噪聲過濾方法中具有最優(yōu)性。

卡爾曼濾波的簡單舉例

為了更好的理解卡爾曼濾波的基本原理，本文首先對卡爾曼濾波的原理進(jìn)行舉例說明。

試想一下，假設(shè)你要憑借地圖從A地前往B地，你要如何知道自己的位置呢？

首先，你可以通過自己的腳程判斷，“A地到B地大約10公里，憑我的腳程大約需要步行3小時(shí)左右”。

其次，還可以通過路牌和手里的地圖判斷，“這個(gè)路牌上寫著到了D鎮(zhèn)，地圖上顯示D鎮(zhèn)應(yīng)該在距離B地還有3公里的地方”。

然而不論是地圖還是腳程估算都存在的較大誤差，此時(shí)我們?yōu)榱说玫礁鼫?zhǔn)確的所處位置，可以結(jié)合地圖與腳程來對當(dāng)前位置進(jìn)行估算。比如：A地到B地大約10公里，憑我的腳程大約需要步行3小時(shí)左右，目前已經(jīng)走了2小時(shí)，那么我推測再過不久就可以到達(dá)D鎮(zhèn)，此時(shí)再對照地圖上D鎮(zhèn)的位置，就可以得到更加精確的位置信息，同時(shí)通過對比實(shí)際到達(dá)D鎮(zhèn)的時(shí)間，還可以反過來修正對于自己腳程的估計(jì)誤差。

圖1 卡爾曼濾波原理示意圖

2

卡爾曼濾波的基本公式

根據(jù)上一節(jié)的例子，我們就可以引出卡爾曼濾波的基本公式：

前兩步是根據(jù)上一個(gè)最優(yōu)估計(jì)值得出此刻的估計(jì)值和估計(jì)值的協(xié)方差，緊接著就可以得到此刻的最優(yōu)估計(jì)值和最優(yōu)估計(jì)值的協(xié)方差，然后利用此刻的最優(yōu)估計(jì)值和最優(yōu)估計(jì)值的協(xié)方差進(jìn)行下一個(gè)迭代。

公式詳解

看起來有點(diǎn)多，但是實(shí)際上非常緊湊，推理非常嚴(yán)謹(jǐn)，接下來將對上述六個(gè)公式進(jìn)行詳細(xì)解釋。

式（1）為系統(tǒng)的狀態(tài)預(yù)測公式，通過上一時(shí)刻的位置對當(dāng)前時(shí)刻的位置進(jìn)行了推測，但是由于過程噪聲的存在，這個(gè)推測出的位置不完全符合真實(shí)情況。

式（2）為系統(tǒng)的狀態(tài)誤差傳遞，類似式（1），是基于上一時(shí)刻位置可能分布的范圍對當(dāng)前時(shí)刻的可能分布范圍做出預(yù)測，Q是可能的估計(jì)誤差。

式（3）則是當(dāng)前時(shí)刻觀測量的預(yù)測值，代表我們根據(jù)預(yù)測出的狀態(tài)，推測出我們將會(huì)觀測到的讀數(shù)，并非真正的讀數(shù)。

式（4）為卡爾曼增益的計(jì)算方法，其物理意義為狀態(tài)預(yù)測誤差與觀測誤差的比例關(guān)系。

式（5）則為對系統(tǒng)狀態(tài)的更新，這個(gè)公式可以分為兩方面看，后半段的代表實(shí)際讀數(shù)與推測讀數(shù)之間的差值，被稱為殘差。使用式（4）求得的卡爾曼增益與殘差相乘，可以得到對狀態(tài)預(yù)測的修正量，最終式（5）可以得到圖1中的卡爾曼位置估計(jì)更新。

式（6）則為對位置估計(jì)的分布估計(jì)，可以作為下一時(shí)刻式（2）的初值使用。

實(shí)例說明

假設(shè)車輛在進(jìn)行勻速運(yùn)動(dòng)，則可以設(shè)置其遞推運(yùn)動(dòng)模型為：

最后，按順序?qū)⑸鲜鲎兞繋胧剑?）、（5）、（6）即可實(shí)現(xiàn)對位置坐標(biāo)的更新，同時(shí)得到的結(jié)果還可以作為下一步迭代的初值。

3

非線性卡爾曼濾波

雖然卡爾曼濾波已經(jīng)在數(shù)學(xué)上被嚴(yán)格證明具有最優(yōu)性，然而卻無法處理非線性系統(tǒng)。針對這種情況，現(xiàn)有的解決方法分為兩種，分別是基于泰勒展開進(jìn)行線性化近似的擴(kuò)展卡爾曼濾波[2]和基于多點(diǎn)采樣進(jìn)行均值方差近似的無跡卡爾曼濾波。

擴(kuò)展卡爾曼濾波

擴(kuò)展卡爾曼濾波通過一組式（11）所示的n階泰勒線性展開式近似表達(dá)非線性函數(shù)：

如圖2所示，其階數(shù)越高，近似程度越高。

圖2 各級泰勒展開線性化效果

這種方法具有計(jì)算復(fù)雜度低、技術(shù)成熟的優(yōu)點(diǎn)，因此，目前在工程中被廣泛使用。然而由于泰勒展開式的復(fù)雜度隨著階數(shù)呈指數(shù)增長，往往在實(shí)際應(yīng)用中很難實(shí)現(xiàn)三階以上的泰勒展開，這導(dǎo)致了這種線性化方法存在較大的截?cái)嗾`差，因此限制了算法的估計(jì)精度，并且在非線性程度較高的系統(tǒng)中容易發(fā)散。

無跡卡爾曼濾波

無跡卡爾曼濾波則通過對非線性函數(shù)在目標(biāo)時(shí)刻的均值與方法進(jìn)行相似擬合來實(shí)現(xiàn)線性化近似的（如圖3所示），這種方法被稱為無跡變換（UT變換）。

圖3 無跡變換原理

UT變換已被Julier和Uhlmann[3]證明具有不低于三階泰勒展開的線性化精度，因此基于UT變換的無跡卡爾曼濾波具有相對于擴(kuò)展卡爾曼濾波更高的精度，同時(shí)其均值與方差的擬合不會(huì)受函數(shù)非線性化程度的影響，因此可以適用于更為非線性的系統(tǒng)。然而，由于使用無跡卡爾曼濾波需要設(shè)置較多參數(shù)，并且計(jì)算負(fù)擔(dān)較重，因此在實(shí)際工程中仍然不能完全取代擴(kuò)展卡爾曼濾波。

基于上述原因，目前在非線性系統(tǒng)上需要根據(jù)系統(tǒng)特征與實(shí)際需求對濾波器的性能做出取舍，選擇最合適的方法進(jìn)行計(jì)算。

4

SLAM中的卡爾曼濾波與圖優(yōu)化

由于SLAM（Simultaneous Localization and Mapping，同步定位與建圖）中對于特征點(diǎn)、路徑與移動(dòng)體的定位同卡爾曼濾波的適用范圍十分吻合，因此，早期SLAM中通常采用卡爾曼濾波在后端對輸出進(jìn)行優(yōu)化。隨著計(jì)算能力的不斷加強(qiáng)，近年來基于圖優(yōu)化的SLAM方法也逐漸占據(jù)了一席之地。

從應(yīng)用角度分析，基于卡爾曼濾波和基于圖優(yōu)化的SLAM方法各有優(yōu)勢。

卡爾曼濾波具有計(jì)算速度快、系統(tǒng)簡單的優(yōu)點(diǎn)，但由于其對后端輸出的優(yōu)化僅基于相鄰兩個(gè)馬爾可夫鏈的信息進(jìn)行一次迭代，因此在精度上有所欠缺，更適用于高動(dòng)態(tài)且對精度要求不高的場合。

相對的，因子圖優(yōu)化可以統(tǒng)籌全局信息以對各位姿輸出進(jìn)行優(yōu)化，并且優(yōu)化迭代次數(shù)可以人為設(shè)置，可以得到更加精確的后端輸出。正因如此，其每次迭代及出現(xiàn)新的輸出時(shí)都需要對全局進(jìn)行計(jì)算，使得計(jì)算負(fù)擔(dān)大幅增加。所以一般情況下，基于圖優(yōu)化的SLAM僅適用于對于精度要求極高的特殊場合。

隨著技術(shù)的發(fā)展，這兩種路線也在分別補(bǔ)齊自身的固有短板。

如基于濾波的SLAM中引入了迭代擴(kuò)展卡爾曼濾波，將最小二乘法與卡爾曼濾波相結(jié)合，對誤差而不是位姿進(jìn)行優(yōu)化，實(shí)現(xiàn)了更高的定位精度，并且可以實(shí)現(xiàn)動(dòng)態(tài)設(shè)置迭代次數(shù)，代表算法有LINS和Fast-Lio等。

反之，為了應(yīng)對圖優(yōu)化“牽一發(fā)而動(dòng)全身”的問題，最近比較熱門的LIOM和LIO-SAM等算法引入了動(dòng)態(tài)滑窗和關(guān)鍵幀機(jī)制，僅對少數(shù)相關(guān)的時(shí)刻進(jìn)行聯(lián)系，有效降低了冗余計(jì)算，并且引入了IMU預(yù)積分等技術(shù)，進(jìn)一步削減系統(tǒng)的計(jì)算負(fù)擔(dān)，獲得了更快的動(dòng)態(tài)響應(yīng)時(shí)間。

總體來看，基于圖優(yōu)化的SLAM是未來的發(fā)展趨勢，而目前產(chǎn)品化的SLAM設(shè)備仍以濾波為主流。

審核編輯：劉清