基于SLAM算法的清潔設(shè)備研究*

王 琦，崔志鵬，曾祥明，李佳睿，薛培延，喬俊福

(太原工業(yè)學(xué)院 自動(dòng)化系，山西 太原 030008)

0 引言

隨著社會(huì)的發(fā)展以及人們生活節(jié)奏的加快,越來(lái)越多的人想要把自己從繁瑣的家務(wù)中解脫出來(lái),市場(chǎng)對(duì)于掃地機(jī)器人的需求也越來(lái)越大。而當(dāng)下市面上的掃地機(jī)器人大多為隨機(jī)式、LDS激光式和視覺(jué)導(dǎo)航式,這些掃地機(jī)器人路徑規(guī)劃方式不僅低效,還有著掃描不全面、抗干擾能力弱、功能較為單一等缺陷,無(wú)法滿足一些特殊的需求,為此研發(fā)了一種掃描更全面、路徑規(guī)劃更精準(zhǔn)、功能更加齊全的清潔機(jī)器人。

1 設(shè)備介紹

我們?cè)O(shè)計(jì)的基于SLAM算法的多功能清潔機(jī)器人由底盤、爬樓機(jī)構(gòu)、清潔機(jī)構(gòu)、激光雷達(dá)及控制系統(tǒng)等組成。通過(guò)激光雷達(dá)及時(shí)檢測(cè)機(jī)器人周圍環(huán)境,從而實(shí)現(xiàn)清潔機(jī)器人的防跌落以及障礙物的檢測(cè);由吸塵裝置結(jié)合毛刷反復(fù)清潔以增強(qiáng)清潔能力,可以帶走墻體死角等處難以清除的灰塵細(xì)毛;通過(guò)合理地設(shè)計(jì)機(jī)器人的機(jī)械結(jié)構(gòu),使機(jī)器人更加多功能化,可以清潔大教室、會(huì)議室等難以清潔的地方。

2 結(jié)構(gòu)設(shè)計(jì)

2.1 爬樓機(jī)構(gòu)

爬樓機(jī)構(gòu)如圖1所示,主要由機(jī)械臂、舵機(jī)、滑軌、齒條等組成。

圖1 爬樓機(jī)構(gòu)

(1) 當(dāng)機(jī)器人檢測(cè)到前方有樓梯時(shí),上端機(jī)械臂通過(guò)舵機(jī)控制逆時(shí)針旋轉(zhuǎn),同時(shí)下端機(jī)械臂通過(guò)舵機(jī)控制順時(shí)針旋轉(zhuǎn)將機(jī)器人支撐起來(lái)。

(2) 將機(jī)器人支撐起來(lái)后,通過(guò)齒輪齒條傳動(dòng)機(jī)構(gòu)將機(jī)器人前半部分伸出;通過(guò)齒輪反向旋轉(zhuǎn),使機(jī)器人后半部分向前運(yùn)動(dòng)。

(3) 后半部分向前運(yùn)動(dòng)之后,機(jī)器人重心前傾,通過(guò)后輪摩擦力的作用使機(jī)器人成功攀爬上臺(tái)階,完成爬樓。

清潔機(jī)器人爬樓過(guò)程如圖2所示。

圖2 清潔機(jī)器人爬樓過(guò)程

2.2 清潔機(jī)構(gòu)

清潔機(jī)構(gòu)(如圖3所示)的掃帚葉片由電機(jī)驅(qū)動(dòng),兩個(gè)電機(jī)相對(duì)轉(zhuǎn)動(dòng)將灰塵等垃圾掃至中間灰塵吸入口處,通過(guò)抽風(fēng)機(jī)將垃圾吸入到塵盒中,完成對(duì)地面的清潔。

圖3 清潔機(jī)構(gòu)

3 控制部分及工作原理

清潔機(jī)器人控制部分的底層架構(gòu)由單片機(jī)控制中心、電機(jī)調(diào)速系統(tǒng)、PID算法控制器等構(gòu)成,下面依次介紹各個(gè)部分原理及其作用。

3.1 單片機(jī)控制中心

清潔機(jī)器人由旭日X3派作為開(kāi)發(fā)板,它兼容了以前的樹(shù)莓派接口,具有更強(qiáng)的算力與處理能力且支持Ubuntu20系統(tǒng)。

3.2 電機(jī)PWM調(diào)速

電機(jī)調(diào)速使用直流電機(jī)脈沖寬度調(diào)制(Pulse Width Modulation),簡(jiǎn)稱PWM。在驅(qū)動(dòng)控制的調(diào)整系統(tǒng)中,可以根據(jù)需要的速度改變一個(gè)周期內(nèi)電平接通和斷開(kāi)的時(shí)間長(zhǎng)短,即通過(guò)改變占空比來(lái)改變平均電壓的大小,從而控制電機(jī)的轉(zhuǎn)速。

3.3 機(jī)器人的移動(dòng)控制

使用開(kāi)環(huán)控制的電機(jī)當(dāng)發(fā)出速度等于0的指令后,由于慣性存在,電機(jī)會(huì)繼續(xù)轉(zhuǎn)動(dòng)一段時(shí)間,所以我們利用PID算法實(shí)現(xiàn)快、準(zhǔn)、穩(wěn)的速度控制。PID(Proportion Integration Differentiation)其實(shí)就是指比例、積分、微分控制,可以利用如下公式根據(jù)誤差值e(t)控制變量和獲取反饋去不斷修正控制函數(shù)U(t):

其中:kP、TI、TD分別為比例增益、積分增益及微分增益。

4 SLAM建圖及路徑規(guī)劃算法

由于攝像頭在室外工作時(shí)受環(huán)境、光照影響較大,而激光雷達(dá)受光照影響較小,即使在黑暗條件下也能很好地工作,所以我們選擇用激光雷達(dá)感知環(huán)境。激光雷達(dá)通過(guò)向目標(biāo)環(huán)境發(fā)射激光數(shù)據(jù),并對(duì)接收反射回來(lái)的數(shù)據(jù)進(jìn)行分析,進(jìn)而完成環(huán)境地圖和位置的計(jì)算。

使用卡爾曼濾波(KF)和人工神經(jīng)網(wǎng)絡(luò)算法對(duì)多種傳感器的數(shù)據(jù)進(jìn)行信息融合,進(jìn)而將處理后的數(shù)據(jù)運(yùn)用 Gmapping 算法和 AMCL 定位系統(tǒng)進(jìn)行SLAM 建圖,最后使用Move_base功能包實(shí)時(shí)規(guī)劃出最優(yōu)的全局與局部路徑,使設(shè)備實(shí)現(xiàn)自主導(dǎo)航與自動(dòng)避障。

4.1 AMCL定位系統(tǒng)

AMCL即自適應(yīng)蒙特卡洛定位,其核心是一種利用粒子表示置信度的自適應(yīng)蒙特卡羅算法,主要原理是:根據(jù)環(huán)境地圖,采樣粒子估計(jì)機(jī)器人位姿的概率分布,其可靠程度由粒子權(quán)重決定。

這種方法會(huì)在機(jī)器人可能的定位位置周圍假設(shè)多個(gè)前進(jìn)方向,然后在機(jī)器人行進(jìn)過(guò)程中,依據(jù)激光雷達(dá)和編碼器等信息對(duì)這些假設(shè)的前進(jìn)方向進(jìn)行篩選,逐步剔除明顯不可信的假設(shè)前進(jìn)方向,留下可信度較高的前進(jìn)方向。在機(jī)器人運(yùn)動(dòng)過(guò)程中,那些假設(shè)前進(jìn)方向會(huì)逐漸收斂,最終匯聚成一個(gè)機(jī)器人最可信的定位位置。

4.2 SLAM建圖

為了在 SLAM 建圖過(guò)程中能夠生成最佳效果的二維地圖,我們參考了文獻(xiàn)[1],在原有 AMCL 算法的基礎(chǔ)上,添加了動(dòng)態(tài)參數(shù),使其粒子群收斂速度根據(jù)機(jī)器人速度的改變而改變,使清潔機(jī)器人的實(shí)時(shí)定位更加精確。

我們用Gazebo軟件仿真場(chǎng)景,得到的Gazebo仿真場(chǎng)景如圖4所示。圖4中,有兩個(gè)虛擬墻壁(正方形)、兩個(gè)障礙物(小圓)和激光雷達(dá)(大圓)。激光雷達(dá)掃描環(huán)境,并在Rviz中顯示計(jì)算后的SLAM建圖結(jié)果,如圖5所示。

圖4 Gazebo仿真場(chǎng)景

圖5 Rviz中顯示的SLAM建圖結(jié)果

由此可見(jiàn),使用各種傳感器和激光雷達(dá)能夠成功檢測(cè)到障礙物和周圍環(huán)境,并準(zhǔn)確建出此場(chǎng)景的地圖。

4.3 路徑規(guī)劃

所謂路徑規(guī)劃,是指達(dá)到機(jī)器人行走路線最短、行走時(shí)間最短等目標(biāo)并避開(kāi)障礙物(DWA算法)到目標(biāo)點(diǎn)的最優(yōu)路徑。

關(guān)于全局最優(yōu)路徑規(guī)劃問(wèn)題,我們使用了A*(A-Star)算法[2]。A*是一種靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法,也是解決許多搜索問(wèn)題的有效算法。它結(jié)合了BFS和dijkstra算法的優(yōu)點(diǎn),在進(jìn)行啟發(fā)式搜索提高算法效率的同時(shí),可以基于評(píng)估函數(shù)保證找到一條最優(yōu)路徑。

關(guān)于機(jī)器人需要具備的自動(dòng)避障功能,我們選用DWA算法作為機(jī)器人的局部路徑規(guī)劃算法,其流程為:首先使用激光雷達(dá)掃描來(lái)尋找一切即將到來(lái)的障礙物,并將數(shù)據(jù)傳遞給機(jī)器人,接著對(duì)數(shù)據(jù)進(jìn)行解析,提前規(guī)劃好最優(yōu)路線,最后評(píng)價(jià)每一個(gè)方案后擇優(yōu)實(shí)現(xiàn),一直重復(fù)上述步驟至遍歷完成。

Move_Base是 Navigation 系統(tǒng)里扮演核心中樞的 ROS 包,它將機(jī)器人導(dǎo)航需要用到的地圖、坐標(biāo)、路徑和行為規(guī)劃器連接到了一起,同時(shí)還提供了導(dǎo)航參數(shù)的設(shè)置接口。使用此功能包可以實(shí)時(shí)規(guī)劃出最優(yōu)的全局與局部路徑,清潔機(jī)器人可以實(shí)現(xiàn)自主導(dǎo)航與自動(dòng)避障的功能。

5 樣機(jī)效果

制作的清潔機(jī)器人樣機(jī)如圖6所示,經(jīng)測(cè)試機(jī)器人可實(shí)現(xiàn)運(yùn)動(dòng)控制和SLAM建圖,并給出最優(yōu)路徑規(guī)劃方案。

圖6 制作的清潔機(jī)器人樣機(jī)

6 產(chǎn)品適用范圍及推廣前景

在一些大型工廠中,往往需要清潔一些對(duì)人體不太友好的廢棄物,使用大型設(shè)備極大地浪費(fèi)了資源,并且建造成本極高。在階梯大教室及會(huì)議廳等頻繁使用的大型場(chǎng)所更是如此,因此外形小巧、可自動(dòng)避障、高效率的智能清潔機(jī)器人就顯得頗為合適了。故該機(jī)器人不論在車間、火車站還是大型會(huì)議廳均有很廣闊的應(yīng)用前景。