基于EKF 算法的小地塊SLAM 技術研究與分析*

曹鵬鋮 ， 陸 靜 ， 高巧明

（1.廣西科技大學機械與交通工程學院，廣西 柳州 545616；2.廣西科技大學廣西汽車零部件與整車技術重點實驗室，廣西 柳州 545616）

0 引言

隨著《中國制造2025》的提出和機器人技術的不斷發展，自主機器人廣泛應用于家庭生活、醫學、農業和軍事等眾多領域，農用智能機械的自主作業也逐漸成為研究熱點[1-2]。其中，精確的地圖與定位是自主機器人工作的關鍵技術之一。在GPS 和高精度地圖技術的支持下，在大面積的農田內實現農業機械的智能化作業已不再是問題，但是，在一些偏遠地區的零碎、大小不一的小地塊中，GPS 信號往往不穩定，且民用衛星的精度無法保證小地塊建圖的精度，嚴重影響農用智能機械的自主作業。在這種情況下，同步定位與建圖（Simultaneous Localization and Mapping，SLAM）技術就顯得尤為重要[3]。

SLAM 是指機器人處于未知環境中時，通過自身攜帶的傳感器獲得位姿信息和環境信息，實現作業過程的自身定位和地圖創建[4-7]，它是自主機器人在未知環境中實現自主作業的關鍵技術。目前，關于機器人的SLAM 技術學術界提出了多種解決方案，有基于擴展卡爾曼濾波（EKF）的SLAM 方法[8-10]，基于粒子濾波的Fast SLAM方法[11-12]，基于全局優化的最大似然算法[13]，基于RGB-D 的視覺SLAM 方法[14]，以及基于深度學習的SLAM 方法[15-16]。基于擴展卡爾曼濾波的SLAM 算法直接提供了導航問題的遞歸解，以及計算機器人和路標不確定性的估計方法，因其數學的嚴謹性和結構的適用性，在SLAM 問題中得到了最廣泛的應用。Smith 等人最早提出了基于EKF 的SLAM 解決方案，隨后許多學者對其進行深入研究，并進行了延伸，在不同的場景中提出了基于EKF 的SLAM 算法[17-18]。但是，在計算資源受限，路標點單一，待估計量比較簡單的小地塊場景中的SLAM技術尚未有人涉及。因此，課題組提出將基于EKF 算法的SLAM技術應用于小地塊環境。

1 SLAM問題的提出

SLAM 是機器人在一個未知的環境里，搭載可獲取環境信息的傳感器，從某一個位置出發，在行進的過程中估計自身位置，同時建立環境地圖，其實現模型圖如圖1 所示。其中，x、y、z、u分別表示位姿、路標、觀測數據和運動傳感器數據。SLAM 過程可以由運動方程和觀測方程兩個方程來解釋，假設在t=0 到t=N的時間內，機器人的位姿由x0到xN，路標由y1到yM，則運動方程和觀測方程分別為：

圖1 SLAM模型圖

其中，uk是運動傳感器的輸入，zk, j是產生的觀測數據，wk、vk, j是兩個噪聲項，并假設滿足零均值的高斯分布。

通過數據z和u來推斷位姿x和路標y，由此可見，SLAM 問題是一個運動過程的狀態估計問題。但是，在噪聲的影響下，這時的位姿和地圖是有誤差的。而且，隨著時間的移動，誤差逐漸累積，對位置方差的估計將越來越大。圖2 中的左圖是只有運動數據的情況，右圖是考慮了觀測數據的情況，圖中的虛線圓表示協方差矩陣。由圖2 左圖可以看出，由于只有運動數據，虛線圓隨著物體運動越來越大，機器人不確定自身的位置，越走越“迷路”；在圖2 右圖中，利用合理的位姿和觀測數據，虛線圓就會縮小到一定的大小，保持穩定。因此，需要綜合利用運動方程和觀測方程的結果以減小狀態估計的不確定性，提高SLAM的精度。

圖2 狀態估計的不確定性

2 基于EKF的SLAM算法

目前，關于SLAM 問題的研究都是采用概率密度的方法。SLAM 問題通過將機器人的每個位姿和地圖位置表示為概率密度函數，按照貝葉斯法則，k時刻的狀態分布為：

這時，k時刻的狀態是根據0 到k時刻的數據估計來的。但是，若假設該過程滿足一階馬爾可夫性，也就是k時刻的狀態只與k－1 時刻的狀態相關，式（2）中的先驗部分以k－1 時刻為條件展開：

由以上方程可以看出，技術的關鍵在于如何精確地將機器人k－1時刻的狀態分布推導至k時刻。

為了提高計算的精度，EKF-SLAM 算法通過預測和更新兩個階段來求解機器人的狀態估計。在觀測到新的路標點時，將其添加到系統狀態向量中繼續進行更新，示意圖如圖3所示。

圖3 EKF-SLAM算法示意圖

2.1 預測階段

在基于EKF 的SLAM 算法中，采用聯合狀態向量表示機器人的位姿與路標點，k時刻系統的聯合狀態向量為：

為了估計k時刻系統的狀態，首先要根據式（1）中的運動方程對機器人的位姿進行預測，在預測階段狀態向量的坐標值不變，則有：

運動方程中的f由所采用的機器人模型決定，在不同情況下具有不同的形式，確定f的過程稱為參數化。在一般情況下，f往往不是線性的，傳遞協方差時，需要對f進行線性化，以保證經傳遞后的協方差依然服從高斯分布。其計算式如下：

其中，Q是觀測方程中噪聲的協方差矩陣，F是狀態轉移函數，該函數是對整體狀態量進行操作，而不是僅考慮機器人的位姿變換，是對k－1 時刻狀態量求導所得的雅可比矩陣。使用下式對協方差的先驗估計進行計算：

其中，

2.2 更新階段

機器人運動之后，可使用自身攜帶的傳感器對環境進行測量，并通過式（1）中的觀測方程計算觀測數據，并在更新階段結合觀測數據和預測階段的計算數據，優化SLAM算法中狀態估計的不確定性。

記已觀測路標點在當前時刻的測量值為Zk, j，由于當前觀測到的路標點數量可能不唯一，故用下標i來區分不同的路標點。另外，由于這些路標在之前的過程中已經被觀測到，所以其系統世界坐標下的坐標值（系統中的絕對坐標）已知。

根據當前機器人位姿的預測值和上述路標點的坐標值，可用觀測模型計算出機器人與路標點間的預計測量值：

對于同一路標點，傳感器測量值與預測值之間存在一定的誤差，用新息γ來表示該誤差：

計算新息與新息協方差之后，即可對系統狀態量進行更新，完成EKF-SLAM的算法流程。

3 仿真實驗及分析

本研究中的模擬環境以某地果園為環境模型。結構性的田壟整齊劃分，每道壟上按一定距離間隔種有果樹，果樹可看為路標，激光雷達放置在小車上層，用于掃描、匹配、預測位置和建圖，如圖4所示。

圖4 某果園地形模型及實驗小車

3.1 室外果園環境模擬

為了驗證基于EKF 算法的SLAM 適用于室外果園場景，并具有良好的操作性能，在MATLAB R2018b 平臺上進行仿真實驗，建立仿真環境。模擬的硬件環境是英特爾酷睿i7-8700CPU，3.2GHz，Ubuntu 18.04 操作系統。設定機器人的主要參數為：輪距L=1 m，行駛速度V=1.5 m/s，最大舵角30o，線速度的誤差是0.02 m/s，角速度誤差為1 rad/s，傳感器測得的距離噪聲為0.03 m，角度噪聲為1o。俯視來看，小車的作業環境如圖5 所示，是一個寬8 m、長16 m 的小地塊，三角形表示小車。實驗中的路標點為田壟上每間隔2 m 設置一個路標點（果樹）。

圖5 作業環境示意圖

3.2 模擬結果及分析

在模擬環境中，小車從設定位置（果園田頭）出發，在田壟間作業經過整個果園，最終的地圖模型和作業軌跡如圖6 所示，地圖模型中白色表示田壟邊界，黑色表示小車的可通行域，作業軌跡中虛線表示實際位置，實線表示估計位置。

圖6 EKF-SLAM地圖和位置估計

模擬結果顯示，EKF-SLAM 算法基本實現了小地塊的地圖建立和定位，證明了算法的可行性。為了進一步分析對比算法的性能和精度，設置轉彎路標點稠密實驗組，分析路標點的數量對EKF-SLAM 算法結果的影響，并根據激光雷達掃描匹配的估計位置和姿態存在偏差，定義以下誤差為：

通過比較位置的估計和實際偏差，位置偏差的標準偏差曲線和x、y的狀態量，如圖7 所示。從圖中可以看出，小車的縱向誤差穩定，橫向誤差有較大波動，EKF-SLAM 算法的整體位置偏差小于0.5 m，但是在150 s 和480 s 左右有較大的偏差，這個時間是小車轉彎的前后階段。由此可見，該算法在轉彎處的仿真結果具有較大的誤差。小地塊的地形通常轉彎較多，因誤差累積的原因，小地塊轉彎處的計算偏差對整體建模精度的影響遠大于常規的大塊平整地塊，因此，在基于EKF-SLAM 算法的小地塊地圖建立和定位中，重點考慮轉彎處的建模精度。

圖7 EKF-SLAM位置誤差和標準偏差

在轉彎處加密路標點，模擬結果如圖8 所示，由圖可知，地圖效果和小車位置估計有了明顯的改進。小車位置偏差曲線和x、y標準差如圖9 所示，對比圖7 可以看出，轉彎稠密路標點實驗組的位置偏差的數值小于原路標點實驗組，并且變化趨勢較平緩；同時，轉彎稠密路標點實驗組的標準差也小于原實驗組。但是，在150 s、300 s 和480 s 左右位置偏差還是波動異常，表明在小車轉彎時的偏差控制還需要進一步的研究。

圖8 轉彎路標點稠密實驗地圖和位置估計

圖9 轉彎稠密路標點實驗EKF-SLAM位置偏差和標準差

4 結論

課題組基于EKF-SLAM 原理研究分析了在小地塊果園中，小車作業過程的同步定位與建圖技術，這將有助于解決小地塊果園GPS 信號弱且精度不足時導致難以實現自主作業的問題。通過模擬實驗證明了該算法在小地塊果園場景中的適用性及有效性。分析仿真結果發現，由于地形的特點，轉彎處的模擬精度是影響小地塊EKF-SLAM 技術的關鍵性因素，通過在轉彎處設置稠密路標點（果樹），有效提高了算法的位置估計精度。