基于激光SLAM的AGV卡爾曼濾波算法

王亞偉

摘要：AGV自動導航問題研究的是AGV在工作環(huán)境中尋找從起始點位置到指定目標點位置的各種可能路徑，在AGV安全、快速運動到目標位置點的前提下，用激光SLAM進行導航，通過路徑算法決策出一條最優(yōu)的運動路徑，再對AGV進行運動分析及建模，從而準確的確定AGV定位信息。本文從激光SLAM著手，通過擴展卡爾曼濾波和無跡卡爾曼濾波兩種算法的仿真分析，選擇出最優(yōu)算法。

關鍵詞：AGV;激光SLAM;擴展卡爾曼濾波;無跡卡爾曼濾波;

1、SLAM的定義

SLAM（Simultaneous Localization and Mapping），也稱為CML（Concurrent Mapping and Localization），即定位與地圖構建。SLAM最早由Smith、Self和Cheeseman于1988年提出。是指機器人在自身位置不確定的條件下，在完全未知環(huán)境中創(chuàng)建地圖，同時利用地圖進行自主定位和導航。SLAM問題可描述為：機器人在未知環(huán)境中從一個未知位置開始移動，在移動過程中根據(jù)位置預測和傳感器數(shù)據(jù)進行自身定位，同時建造增量式地圖。

2、卡爾曼濾波的定義

1960年，卡爾曼發(fā)表了用遞歸方法解決離散數(shù)據(jù)線性濾波問題的論文（A New Approach to Linear Filtering and Prediction Problems）。在這篇文章里，一種克服了維納濾波缺點的新方法被提出來，這就是我們今天稱之為卡爾曼濾波的方法?？柭鼮V波可以預測信號的過去和當前狀態(tài)，甚至能預測將來的狀態(tài)，即使不知道模型的確切性質。本質上來講，濾波就是一個信號處理與變換的過程，這個過程可以通過硬件來實現(xiàn)，也可以通過軟件來實現(xiàn)?？柭鼮V波屬于一種軟件濾波方法，基本思想是：以最小均方誤差為最佳估計準則，采用信號與噪聲的狀態(tài)空間模型，利用前一時刻的預測值和當前時刻的觀測值來更新對狀態(tài)變量的預測，求出當前時刻的預測值，算法根據(jù)建立的系統(tǒng)方程和觀測方程對需要處理的信號做出滿足最小均方誤差的估計。

3、基于激光SLAM的AGV導航模型與卡爾曼濾波算法

激光SLAM的AGV導航如圖2所示。設為AGV瞬時狀態(tài)時候和障礙物位置的系統(tǒng)狀態(tài)矩陣，其中表示AGV所處狀態(tài)，為障礙物的坐標。

設為激光雷達檢測到的障礙物位置數(shù)據(jù)，根據(jù)卡爾曼濾波算法，AGV狀態(tài)轉移方程可表示為：

在式中，各量說明如下：

根據(jù)t-1時刻預測的系統(tǒng)狀態(tài)可推算出t時刻的系統(tǒng)先驗狀態(tài)預測，根據(jù)系統(tǒng)測量矩陣Zt推算出t時刻的系統(tǒng)后驗狀態(tài)預測。由此可獲得基于t-1時刻對t時刻狀態(tài)的預測值與真實值之間的先驗狀態(tài)預測誤差，其協(xié)方差矩陣為

同時也得到系統(tǒng)后驗狀態(tài)預測與系統(tǒng)真實值的后驗狀態(tài)預測誤差，其協(xié)方差矩陣與測量矩陣的預測分別

其中是預測值與測量值之間的誤差，若等于0，說明預測值與測量值完全吻合。Kt表示卡爾曼增益，其求取原則是令式（5）的系統(tǒng)后驗狀態(tài)誤差趨于零，即將系統(tǒng)后驗狀態(tài)預測帶入后驗狀態(tài)預測誤差，可解得Kt矩陣值為

可見，測量噪聲協(xié)方差R越小，卡爾曼增益Kt越大。先驗預測協(xié)方差越小，Kt越小。也可以說Kt表示預測值與測量值在狀態(tài)預測時的權重，Kt越大表示測量值Zt的權重越大，Zt預測值的權重越小;反之亦然。

卡爾曼濾波框架包含兩個部分：時間更新和測量更新。時間更新推算出當前時間狀態(tài)變量和誤差協(xié)方差矩陣，以便為下一個時間狀態(tài)構造先驗預測。測量更新將先驗狀態(tài)預測與新的測量狀態(tài)結合以構造改進的后驗狀態(tài)預測。

時間更新和測量更新過程循序迭代，這種遞歸實現(xiàn)的卡爾曼濾波框架相比于粒子濾波算法更加簡單。

4、基于擴展的卡爾曼濾波算法

經(jīng)典的卡爾曼濾波算法要求系統(tǒng)是線性的，而在現(xiàn)實中，系統(tǒng)往往是非線性的。因此，如果利用泰勒級數(shù)將非線性系統(tǒng)展開，之后取一次項，這樣非線性系統(tǒng)就近似成了線性系統(tǒng)，這樣就可以根據(jù)卡爾曼濾波框架遞推地推算出系統(tǒng)狀態(tài)預測信息，這就是基于擴展卡爾曼濾波算法的SLAM（EKF-SLAM）。

EKF-SLAM算法實現(xiàn)過程中，AGV的運動模型與測量模型可表示為

其中，與為非線性函數(shù)，Xt、Ut、Zt含義同上Wt和Vt表示符合高斯分布的噪聲，Wt～N（0，Qt）和Vt～N（0，Rt）。

由于AGV的運動模型和測量模型是非線性的，需要對其進行泰勒級數(shù)展開并取一次項進行近視線性化。

5、基于無跡卡爾曼濾波算法

無跡卡爾曼濾波又稱無損卡爾曼濾波（Unscented Kalman Filter，UKF），是無損變換（Unscented Transform，UT）與標準卡爾曼濾波體系的結合，通過無損變換使非線性系統(tǒng)方程適用于線性假設下的標準卡爾曼體系。UKF使用的是統(tǒng)計線性化技術，這一技術主要通過n個在先驗分布中采集的點的線性回歸來線性化隨機變量的非線性函數(shù)，由于我們考慮的是隨機變量的擴展，所以這種線性化要比泰勒級數(shù)線性化更準確。

UKF-SLAM算法的復雜性與EKF-SLAM近似，但是UKF-SLAM不用求雅可比矩陣，某些情況下，雅可比矩陣是很難確定的，所以EKF-SLAM不能適用于所有系統(tǒng)。但是如果系統(tǒng)是非高斯的，UKF-SLAM就會有較大的誤差，會導致濾波失敗。

6、SLAM算法仿真分析

比較EKF-SLAM與UKF-SLAM算法的差別，利用MATLAB對兩種算法進行了模擬仿真實驗，通過實驗進行比較分析。

實驗條件：AGV車體上安裝激光雷達，最大測量距離為80m，測量分辨率為0.01m，最大測量角度為180°，分辨率為1°。測量結果均加入高斯噪聲中。測量采樣周期為25ms。AGV速度0.5m/s，輪間距為0.8m，最大轉角為30°。

模擬仿真實驗分兩部分：在約100m長的直線路徑附近設置150個路標點，AGV沿該直線路徑行走。分別使用EKF-SLAM和UKF-SLAM進行模擬仿真，所得到的AGV實際軌跡和估計軌跡如圖3;估計軌跡與實際軌跡的誤差如圖4。

由仿真結果可知，兩種算法的誤差最大均是發(fā)生在Y軸方向，EKF-SLAM耗時約19分鐘，最大誤差約為1.57m，而UKF-SLAM耗時約21分鐘，最大誤差約為0.8m。這兩種算法在運行過程中，由于誤差的不斷累計，AGV預測路徑逐漸偏離了真實路徑。但是UKF-SLAM在直線行走路徑預測比EKF-SLAM誤差值小。由此可見UKF-SLAM相比于EKF-SLAM有更好的準確性。

7、總結

基于激光SLAM的運動仿真，對比EKF-SLAM和UKE-SLAM兩種算法在直線路徑的仿真結果，可知，當檢測數(shù)據(jù)中包含已知信息時，定位構建會更加精確，即如果在定位構建時加入閉環(huán)檢測環(huán)節(jié)，SLAM誤差會降低。而且UKF-SLAM算法精確度高于EKF-SLAM。當路標密度增加時，兩種算法處理時間都會增加，且UKF-SLAM耗時會更多;而當路標密度較低時，UKF-SLAM與EKF-SLAM耗時相差不大。由此可見，UKF-SLAM適用于小范圍環(huán)境導航，在環(huán)境尺寸較大時，導航多采用EKF-SLAM算法。

參考文獻：

[1]于洋.AGV及其控制器設計與開發(fā)[D].杭州：浙江大學，2008.

[2]白彪才，馬殷元.組合導航的AGV定位精度的改善[J].測控技術，2018，37（4）：138.

[3]蘇永華.全方位移動式AGV技術研究[J].制造業(yè)自動化，2014，36（15）：10.

[4]孫宇，張世琪等.AGV自動導航技術的研究[J].中國機械過程，1996，（6）：31-32.

[5]任永益，饒聃，潘孟春.自動導向車控制系統(tǒng)的數(shù)學模型及應用.國防科技大學學報，1997，19（6）：42-47.