Apollo開發者社區-開發者說 | 手把手教你寫卡爾曼濾波器

 

https://mp.weixin.qq.com/s?__biz=MzI1NjkxOTMyNQ==&mid=2247486367&idx=1&sn=809b181e9cb54d3f268e065fe31b8071&chksm=ea1e19eddd6990fba657edbcc7545aa1119b7043c046af830f19046f64ae0ce4e162a5d3129c&mpshare=1&scene=1&srcid=#rd

 

無人駕駛技術入門軟件篇已經介紹了傳感器數據的解析和傳感器信息的坐標轉換。

這兩步完成后，我們就會獲得某一時刻，自車坐標系下的各種傳感器數據。

這些數據包括障礙物的位置、速度；

車道線的曲線方程、車道線的類型和有效長度；

自車的GPS坐標等等。

這些信號的組合，表示了無人車當前時刻的環境信息。

由於傳感器本身的特性，任何測量結果都是有誤差的。

以障礙物檢測為例，如果直接使用傳感器的測量結果，在車輛顛簸時，可能會造成障礙物測量結果的突變，這對無人車的感知來說是不可接受的。

因此需要在傳感器測量結果的基礎上，進行跟蹤，以此來保證障礙物的位置、速度等信息不會發生突變。

最經典的跟蹤算法莫過於卡爾曼在1960年提出的卡爾曼濾波器。

在無人車領域，卡爾曼濾波器除了應用於障礙物跟蹤外，也在車道線跟蹤、障礙物預測以及定位等領域大展身手。

在介紹卡爾曼濾波器數學原理之前，先從感性上看一下它的工作原理。

簡單來講，卡爾曼濾波器就是根據上一時刻的狀態，預測當前時刻的狀態，將預測的狀態與當前時刻的測量值進行加權，加權后的結果才認為是當前的實際狀態，而不是僅僅聽信當前的測量值。

假設有個小車在道路上向右側勻速運動，我們在左側安裝了一個測量小車距離和速度傳感器，傳感器每1秒測一次小車的位置s和速度v，如下圖所示。

我們用向量xt來表示當前小車的狀態，該向量也是最終的輸出結果，被稱作狀態向量（state vector）：

由於測量誤差的存在，傳感器無法直接獲取小車位置的真值，只能獲取在真值附近的一個近似值，可以假設測量值在真值附近服從高斯分布。

如下圖所示，測量值分布在紅色區域的左側或右側，真值則在紅色區域的波峰處。

由於是第一次測量，沒有小車的歷史信息，我們認為小車在1秒時的狀態x與測量值z相等，表示如下：

公式中的1表示第1秒。

預測是卡爾曼濾波器中很重要的一步，這一步相當於使用歷史信息對未來的位置進行推測。

根據第1秒小車的位置和速度，我們可以推測第2秒時，小車所在的位置應該如下圖所示。

會發現，圖中紅色區域的范圍變大了，這是因為預測時加入了速度估計的噪聲，是一個放大不確定性的過程。

根據小車第一秒的狀態進行預測，得到預測的狀態xpre：

其中，Pre是Prediction的簡稱；時間間隔為1秒，所以預測位置為距離+速度*1；由於小車做的是勻速運動，因此速度保持不變。

在第2秒時，傳感器對小車的位置做了一次觀測，我們認為小車在第2秒時觀測值為z2，用向量表示第2秒時的觀測結果為：

很顯然，第二次觀測的結果也是存在誤差的，我們將預測的小車位置與實際觀測到的小車位置放到一個圖上，即可看到：

圖中紅色區域為預測的小車位置，藍色區域為第2秒的觀測結果。

很顯然，這兩個結果都在真值附近。為了得到盡可能接近真值的結果，我們將這兩個區域的結果進行加權，取加權后的值作為第二秒的狀態向量。

為了方便理解，可以將第2秒的狀態向量寫成：

其中，w1為預測結果的權值，w2為觀測結果的權值。

兩個權值的計算是根據預測結果和觀測結果的不確定性來的，這個不確定性就是高斯分布中的方差的大小，方差越大，波形分布越廣，不確定性越高，這樣一來給的權值就會越低。

加權后的狀態向量的分布，可以用下圖中綠色區域表示：

你會發現綠色區域的方差比紅色區域和藍色區域的小。

這是因為進行加權運算時，需要將兩個高斯分布進行乘法運算，得到的新的高斯分布的方差比兩個做乘法的高斯分布都小。

兩個不那么確定的分布，最終得到了一個相對確定的分布，這是卡爾曼濾波的一直被推崇的原因。

第1秒的初始化以及第2秒的預測、觀測，實現卡爾曼濾波的一個周期。

同樣的，我們根據第2秒的狀態向量做第3秒的預測，再與第3秒的觀測結果進行加權，就得到了第3秒的狀態向量；

再根據第3秒的狀態向量做第4秒的預測，再與第4秒的觀測結果進行加權，就得到了第4秒的狀態向量。以此往復，就實現了一個真正意義上的卡爾曼濾波器。

以上就是卡爾曼濾波器的感性分析過程，下面我們回歸理性，談談如何將以上過程寫成代碼。

前文用了一個簡答的例子對卡爾曼濾波器的整個流程進行了介紹，下面我們根據卡爾曼濾波器的原理，編寫代碼，跟蹤連續的激光雷達點。

在這里就要祭出卡爾曼老先生給我們留下的寶貴財富了，下面7個公式就是卡爾曼濾波器的理性描述，使用下面7個公式，就能夠實現一個完整的卡爾曼濾波器。

現在看不懂這7個公式沒關系，繼續往下看，我會一個一個做解釋。

寫代碼（C++）的過程，實際上就是結合上面的公式，一步步完成初始化、預測、觀測的過程。

由於公式中涉及大量的矩陣轉置和求逆運算，我們使用開源的矩陣運算庫——Eigen庫。

在Initialization這一步，需要將各個變量初始化，對於不同的運動模型，其狀態向量肯定是不一樣的，

比如前文小車的例子，只需要一個距離s和一個速度v就可以表示小車的狀態；

再比如在一個2維空間中的點，需要x方向上的距離和速度以及y方向上的距離和速度才能表示，這樣的狀態方程就有4個變量。

因此我們使用Eigen庫中非定長的數據結構，下圖中的VerctorXd表示X維的列矩陣，其中的元素數據類型為double。

在這里，我們新建了一個KalmanFilter類，其中定義了一個叫做x_的變量，表示這個卡爾曼濾波器的狀態向量。

完成初始化后，我們開始寫Prediction部分的代碼。首先是公式

這里的x為狀態向量，通過左乘一個矩陣F，再加上外部的影響u，得到預測的狀態向量x'。這里的F成為狀態轉移矩陣（state transistion matrix）。

以2維的勻速運動為例，這里的x為

對於勻速運動模型，根據中學物理課本中的公式s1 = s0 + vt，經過時間△t后的預測狀態向量應該是

由於假設當前運動為勻速運動，加速度為0，加速度不會對預測造成影響，即

如果換成加速或減速運動模型，就可以引入加速度a_x和a_y，根據s1 = s0 + vt + at^2/2這里的u會變成：

作為入門課程，這里不討論太復雜的模型，因此公式

最終將寫成

由於每次做預測時，△t的大小不固定，因此我們專門寫一個函數SetF()

再看預測模塊的第二個公式

該公式中P表示系統的不確定程度，這個不確定程度，在卡爾曼濾波器初始化時會很大，隨着越來越多的數據注入濾波器中，不確定程度會變小，P的專業術語叫狀態協方差矩陣（state covariance matrix）；

這里的Q表示過程噪聲（process covariance matrix），即無法用x'=Fx+u表示的噪聲，比如車輛運動時突然到了上坡，這個影響是無法用之前的狀態轉移估計的。

以激光雷達為例。激光雷達只能測量點的位置，無法測量點的速度，因此對於激光雷達的協方差矩陣來說，對於位置信息，其測量位置較准，不確定度較低；

對於速度信息，不確定度較高。因此可以認為這里的P為：

由於Q對整個系統存在影響，但又不能太確定對系統的影響有多大。工程上，我們一般將Q設置為單位矩陣參與運算，即

根據以上內容和公式

我們就可以寫出預測模塊的代碼了

實際編程時x'及P'不需要申請新的內存去存儲，使用原有的x和P代替即可。

觀測的第一個公式是

這個公式計算的是實際觀測到的測量值z與預測值x'之間差值y。

不同傳感器的測量值一般不同，比如激光雷達測量的位置信號為x方向和y方向上的距離，毫米波雷達測量的是位置和角度信息。

因此需要將狀態向量左乘一個矩陣H，才能與測量值進行相應的運算，這個H被稱為測量矩陣（Measurement Matrix）。

激光雷達的測量值為

其中xm和ym分別表示x方向上的測量（measurement）值。

由於x'是一個4*1的列向量，如果要與一個2*1的列向量z進行減運算，需要左乘一個2*4的矩陣才行，因此整個公式最終要寫成：

即，對於激光雷達來說，這里的測量矩陣H為

求得y值后，對y值乘以一個加權量，再加到原來的預測量上去，就可以得到一個既考慮了測量值，又考慮了預測模型的位置的狀態向量了。

那么y的這個權值該如何取呢？

再看接下里的兩個公式

這兩個公式求的是卡爾曼濾波器中一個很重要的量——卡爾曼增益K（Kalman Gain），用人話講就是求y值的權值。

第一個公式中的R是測量噪聲矩陣（measurement covariance matrix），這個表示的是測量值與真值之間的差值。一般情況下，傳感器的廠家會提供該值。

S只是為了簡化公式，寫的一個臨時變量，不要太在意。

看最后兩個公式

這兩個公式，實際上完成了卡爾曼濾波器的閉環，第一個公式是完成了當前狀態向量x的更新，不僅考慮了上一時刻的預測值，也考慮了測量值，和整個系統的噪聲，

第二個公式根據卡爾曼增益，更新了系統的不確定度P，用於下一個周期的運算，該公式中的I為與狀態向量同維度的單位矩陣。

將以上五個公式寫成代碼如下：

至此，一個卡爾曼濾波器的雛形就出來了。

包含的變量有：

以激光雷達數據為例，使用以上濾波器，代碼如下：

其中GetLidarData函數除了獲取點的位置信息m_x和m_y外，還獲取了當前時刻的時間戳，用於計算前后兩幀的時間差delta_t。

以上就是卡爾曼濾波器對於勻速運動物體跟蹤的例子。

在這個基礎上，業內還有擴展卡爾曼濾波器和無跡卡爾曼濾波器，它們與經典卡爾曼濾波器的最大區別是狀態轉移矩陣F和測量矩陣H的不同，剩下的跟蹤過程依然需要使用前面介紹的7個公式。

只要你能夠寫出某個模型的F、P、Q、H、R矩陣，任何狀態跟蹤的問題都將迎刃而解。