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

曹鵬鋮 ， 陸 靜 ， 高巧明

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

0 引言

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

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

1 SLAM問題的提出

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

圖1 SLAM模型圖

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

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

圖2 狀態(tài)估計的不確定性

2 基于EKF的SLAM算法

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

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

由以上方程可以看出，技術(shù)的關(guān)鍵在于如何精確地將機器人k－1時刻的狀態(tài)分布推導(dǎo)至k時刻。

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

圖3 EKF-SLAM算法示意圖

2.1 預(yù)測階段

在基于EKF 的SLAM 算法中，采用聯(lián)合狀態(tài)向量表示機器人的位姿與路標(biāo)點，k時刻系統(tǒng)的聯(lián)合狀態(tài)向量為：

為了估計k時刻系統(tǒng)的狀態(tài)，首先要根據(jù)式（1）中的運動方程對機器人的位姿進行預(yù)測，在預(yù)測階段狀態(tài)向量的坐標(biāo)值不變，則有：

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

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

其中，

2.2 更新階段

機器人運動之后，可使用自身攜帶的傳感器對環(huán)境進行測量，并通過式（1）中的觀測方程計算觀測數(shù)據(jù)，并在更新階段結(jié)合觀測數(shù)據(jù)和預(yù)測階段的計算數(shù)據(jù)，優(yōu)化SLAM算法中狀態(tài)估計的不確定性。

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

根據(jù)當(dāng)前機器人位姿的預(yù)測值和上述路標(biāo)點的坐標(biāo)值，可用觀測模型計算出機器人與路標(biāo)點間的預(yù)計測量值：

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

計算新息與新息協(xié)方差之后，即可對系統(tǒng)狀態(tài)量進行更新，完成EKF-SLAM的算法流程。

3 仿真實驗及分析

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

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

3.1 室外果園環(huán)境模擬

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

圖5 作業(yè)環(huán)境示意圖

3.2 模擬結(jié)果及分析

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

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

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

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

圖7 EKF-SLAM位置誤差和標(biāo)準(zhǔn)偏差

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

圖8 轉(zhuǎn)彎路標(biāo)點稠密實驗地圖和位置估計

圖9 轉(zhuǎn)彎稠密路標(biāo)點實驗EKF-SLAM位置偏差和標(biāo)準(zhǔn)差

4 結(jié)論

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