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

前言

1

何為卡爾曼濾波

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

卡爾曼濾波的簡(jiǎn)單舉例

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

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

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

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

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

在這個(gè)例子里，通過(guò)自己腳程估計(jì)的位置，在卡爾曼濾波里就被稱為狀態(tài)預(yù)測(cè)，其誤差則為估計(jì)誤差，估計(jì)方法為預(yù)測(cè)矩陣，地圖上自己的位置則為狀態(tài)觀測(cè)，從地圖換算到當(dāng)前位置的方法則被稱為觀測(cè)矩陣。卡爾曼的關(guān)鍵就在于對(duì)即將到來(lái)的觀測(cè)進(jìn)行預(yù)測(cè)，并用其對(duì)比最后實(shí)際得到的觀測(cè)，通過(guò)這兩者之間的差值進(jìn)行修正。假設(shè)用一個(gè)笛卡爾坐標(biāo)系來(lái)對(duì)這種方法進(jìn)行表示，則可參考圖1。

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

2

卡爾曼濾波的基本公式

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

（1） （2） （3） （4） （5） （6）

前兩步是根據(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è)迭代。

公式詳解

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

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

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

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

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

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

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

實(shí)例說(shuō)明

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

（7） （8）

其中式（7）代表k時(shí)刻的車輛狀態(tài)，包含車輛的坐標(biāo)和在橫縱坐標(biāo)軸的速度，式（8）則為勻速運(yùn)動(dòng)狀態(tài)轉(zhuǎn)換矩陣，可以通過(guò)將式（7）、（8）帶入式（1）求取下一時(shí)刻的狀態(tài)預(yù)測(cè)。

之后我們給出狀態(tài)誤差矩陣的迭代初值，通常設(shè)置為一個(gè)與狀態(tài)同階的單位陣，帶入式（2）即可求得下一時(shí)刻的誤差矩陣預(yù)測(cè)，這里Q需要根據(jù)估計(jì)模型的誤差來(lái)人為給定。

下一步，我們假設(shè)運(yùn)動(dòng)系統(tǒng)使用GPS作為觀測(cè)器，經(jīng)過(guò)預(yù)處理的GPS信號(hào)可以直接輸出車輛的位置坐標(biāo)，那么GPS的觀測(cè)模型可以簡(jiǎn)化為：

（9） （10）

將式（9）與式（1）的結(jié)果帶入式（3），可以得到基于狀態(tài)預(yù)測(cè)的觀測(cè)值預(yù)測(cè)，系統(tǒng)觀測(cè)誤差的方差矩陣R可以根據(jù)傳感器的技術(shù)說(shuō)明書(shū)確定。

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

3

非線性卡爾曼濾波

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

擴(kuò)展卡爾曼濾波

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

（11）

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

圖2 各級(jí)泰勒展開(kāi)線性化效果

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

無(wú)跡卡爾曼濾波

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

圖3 無(wú)跡變換原理

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

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

4

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

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

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

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

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

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

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

反之，為了應(yīng)對(duì)圖優(yōu)化“牽一發(fā)而動(dòng)全身”的問(wèn)題，最近比較熱門(mén)的LIOM和LIO-SAM等算法引入了動(dòng)態(tài)滑窗和關(guān)鍵幀機(jī)制，僅對(duì)少數(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í)間。

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

審核編輯 ：李倩