曹鵬鋮 ， 陸 靜 ， 高巧明

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

0 引言

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

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

1 SLAM問(wèn)題的提出

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

圖1 SLAM模型圖

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

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

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

2 基于EKF的SLAM算法

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

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

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

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

圖3 EKF-SLAM算法示意圖

2.1 預(yù)測(cè)階段

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

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

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

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

其中，

2.2 更新階段

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

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

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

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

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

3 仿真實(shí)驗(yàn)及分析

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

圖4 某果園地形模型及實(shí)驗(yàn)小車

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

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

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

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

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

圖6 EKF-SLAM地圖和位置估計(jì)

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

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

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

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

圖8 轉(zhuǎn)彎路標(biāo)點(diǎn)稠密實(shí)驗(yàn)地圖和位置估計(jì)

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

4 結(jié)論

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