穆莉莉,史 程,王 超,單卓佳,劉帥帥

(安徽理工大學(xué) 機(jī)械工程學(xué)院,安徽 淮南 232000)

掃地機(jī)器人是一種智能化的機(jī)器人產(chǎn)品,全覆蓋路徑規(guī)劃是機(jī)器人中最為重要的內(nèi)容之一,決定著清潔效果和工作效率[1],常用的全覆蓋路徑規(guī)劃算法有柵格法、內(nèi)螺旋法、模板規(guī)劃法等,總體來說,掃地機(jī)器人覆蓋率低、重復(fù)率高、工作效率低[2]。掃地機(jī)器人工作場景復(fù)雜,靜動(dòng)態(tài)障礙物多,面對(duì)多障礙物或突然出現(xiàn)的動(dòng)態(tài)障礙物,如何獲取最佳清掃路徑,是目前國內(nèi)外研究機(jī)構(gòu)的研究熱點(diǎn)[3-4]。對(duì)全覆蓋路徑規(guī)劃算法的進(jìn)一步優(yōu)化提升是當(dāng)前掃地機(jī)器人中的重要研究方向。

對(duì)全覆蓋路徑規(guī)劃的優(yōu)化有兩種途徑,一是可以從路徑生成方法上優(yōu)化,二是可通過地圖優(yōu)化及地圖分割方式去規(guī)劃出更加合理的路徑。目前,基于內(nèi)螺旋搜索的生物激勵(lì)路徑規(guī)劃算法已經(jīng)能夠取得較好的效果,其重復(fù)率低、拐點(diǎn)數(shù)較少且適應(yīng)性強(qiáng)[5]。另外,使用全局路徑規(guī)劃與局部實(shí)時(shí)避障路徑規(guī)劃相結(jié)合的導(dǎo)航技術(shù)能夠提高路徑規(guī)劃的合理性[6]。

通過更加精細(xì)的地圖分割和優(yōu)化,也可提升路徑規(guī)劃的合理性,使掃地機(jī)器人的清掃工作更加高效。目前基于激光導(dǎo)航的掃地機(jī)器人已經(jīng)可以建立較為精細(xì)的室內(nèi)地圖,這為全局路徑規(guī)劃提供了良好的基礎(chǔ)[7]。如elek等[8]針對(duì)已知環(huán)境開發(fā)了一種覆蓋算法,其想法來源于生成樹覆蓋算法(STC)。然而,該算法的線路含有過多的拐點(diǎn),這給作業(yè)效率帶來了影響;Le[9]等設(shè)計(jì)了一套全覆蓋路徑規(guī)劃算法,該算法基于螺旋生成樹原理并可降低覆蓋率重復(fù)率。然而,該算法的規(guī)劃路徑長度較長,拐彎次數(shù)顯著增多。當(dāng)面臨密集零散障礙區(qū)域時(shí),機(jī)器人通常會(huì)陷入死區(qū),導(dǎo)致效率顯著下降。許倫輝等[10]基于分治思想對(duì)二維柵格地圖進(jìn)行分割,在分割后的區(qū)域分別進(jìn)行路徑規(guī)劃,單一環(huán)境下可通過分區(qū)域規(guī)劃路徑的方式,降低全覆蓋算法的復(fù)雜度,但該方法分割方式相對(duì)簡單,對(duì)于不規(guī)則形狀的工作環(huán)境很難做出有效分割;王紅星[11]等針對(duì)凹多邊形區(qū)域,提出一種區(qū)域覆蓋算法,把凹多邊形轉(zhuǎn)化為凸多邊形,進(jìn)行區(qū)域劃分后對(duì)每一個(gè)區(qū)域進(jìn)行路徑規(guī)劃,在空曠的大場景下可規(guī)劃出復(fù)雜度低的路徑,但該算法不適用于有復(fù)雜障礙物的室內(nèi)環(huán)境,容易受室內(nèi)障礙物的影響從而無法進(jìn)行地圖分割。

本文提出一種基于骨架抽取的地圖優(yōu)化方法,通過降低全覆蓋路徑的復(fù)雜程度,提高掃地機(jī)器人的清潔率,降低掃地機(jī)器人的清潔重復(fù)率。

1 二維激光SLAM地圖構(gòu)建與路徑規(guī)劃

1.1 二維激光SLAM地圖構(gòu)建

結(jié)合實(shí)際工作環(huán)境以及機(jī)器人的算力,首先選擇Gmapping算法來進(jìn)行室內(nèi)地圖構(gòu)建,在此基礎(chǔ)上對(duì)其進(jìn)行路徑規(guī)劃研究。Gmapping算法以RBPF方法為基礎(chǔ),對(duì)提議分布進(jìn)行了改進(jìn),并且提出了選擇性采樣的方法。

1.1.1 RBPF

機(jī)器人通過觀測信息和里程計(jì)數(shù)據(jù)來獲得其位姿和地圖,RBPF算法對(duì)概率密度函數(shù)進(jìn)行因式分解,首先估計(jì)位姿,然后計(jì)算柵格圖。即拆開先求其中某一個(gè)變量,在解決另一個(gè)變量之前,先解決此變量,計(jì)算方法如式(1)所示。

P(X1:t,m|Z1:t,u1:t-1)

=P(m|X1:t,Z1:t)P(X1:t|Z1:t,u1:t-1)

(1)

式(1)中,P(X1:t,m|Z1:t,u1:t-1)是定位部分,P(m|X1:t,Z1:t)P(X1:t|Z1:t,u1:t-1)是建圖部分,用前者作為后者的條件從而可以得到柵格地圖。

1.1.2 提議分布

為了獲取下一個(gè)時(shí)刻掃地機(jī)器人的姿態(tài),從提議分布中進(jìn)行采樣。當(dāng)建議分布與目標(biāo)分布非常接近時(shí),可以直接在目標(biāo)分布中進(jìn)行采樣,這樣只需要使用較少的粒子就能獲取機(jī)器人的位姿。具體方法為:首先通過機(jī)器人的運(yùn)動(dòng)模型得到粒子數(shù)量;然后通過激光雷達(dá)的觀測信息給粒子加權(quán),從而得最佳粒子;最后用較大權(quán)重的粒子對(duì)擬議分布進(jìn)行模擬改進(jìn)。通過高斯函數(shù)來建立提議分布,使用K個(gè)數(shù)據(jù)模擬峰值并生成高斯函數(shù)作為提議分布,如式(2)所示。

(2)

(3)

式(3)中,用有限K個(gè)數(shù)據(jù)積分近似轉(zhuǎn)變成求和,然后進(jìn)行權(quán)重計(jì)算。

1.1.3 選擇性重采樣

針對(duì)頻繁采樣過程導(dǎo)致粒子退化和粒子多樣性減少問題,通過設(shè)置閾值控制重采樣的次數(shù)來解決,當(dāng)粒子權(quán)重比閾值大時(shí),則進(jìn)行重采樣否則不執(zhí)行重采樣。

使用Gmapping算法對(duì)自主搭建的仿真環(huán)境進(jìn)行建圖,建圖效果如圖1所示。

1.2 路徑規(guī)劃

室內(nèi)掃地機(jī)器人工作空間封閉,且區(qū)域多網(wǎng)格化,首先選用傳統(tǒng)的內(nèi)螺旋式路徑規(guī)劃方法進(jìn)行效果測試,如圖2所示。

圖2 螺旋算法示意圖

此處使用的地圖為柵格地圖,把遍歷過的柵格定義為已知柵格,未遍歷的柵格為自由柵格。為了讓掃地機(jī)器人在運(yùn)動(dòng)中更加高效,將其行走過程分成三種行為向左旋轉(zhuǎn)90°、直行、向右旋轉(zhuǎn)90°,并優(yōu)先級(jí)遞減。根據(jù)這個(gè)優(yōu)先級(jí),掃地機(jī)器人會(huì)選擇下一個(gè)要遍歷的柵格,并標(biāo)記已經(jīng)遍歷過的柵格。在掃地機(jī)器人工作時(shí),如果它在前方遇到了邊界,并且在左側(cè)或右側(cè)的柵格中,出現(xiàn)了障礙物或該區(qū)域已被清掃,則機(jī)器人會(huì)進(jìn)入一個(gè)死角狀態(tài)。在這種情況下,掃地機(jī)器人會(huì)倒退沿著原始路徑,并按照預(yù)設(shè)的行走優(yōu)先級(jí)進(jìn)行搜索,直到所有的柵格都被完全清掃為止。這個(gè)過程類似于回溯法,以確保每個(gè)區(qū)域都被覆蓋到,從而實(shí)現(xiàn)全覆蓋清掃。

對(duì)仿真環(huán)境的柵格地圖進(jìn)行全覆蓋路徑規(guī)劃,規(guī)劃效果如圖3所示。

圖3 路徑規(guī)劃效果

由圖3可知,全覆蓋路徑因?yàn)闃O個(gè)別的黑點(diǎn)存在,近似的將地圖分為多個(gè)網(wǎng)格區(qū)域,改變了全覆蓋路徑規(guī)劃的方法,產(chǎn)生很多拐點(diǎn),增加了路徑的復(fù)雜程度。

針對(duì)上述情況,本文提出一種地圖優(yōu)化算法,對(duì)激光雷達(dá)采集的地圖首先進(jìn)行濾波處理,根據(jù)掃地機(jī)器人的工作環(huán)境特點(diǎn),設(shè)置閾值,有針對(duì)性地濾掉較小離散點(diǎn),接著對(duì)處理后的地圖進(jìn)行骨架抽取處理,去除模糊邊界,最后在該地圖上進(jìn)行路徑規(guī)劃。

2 地圖優(yōu)化算法

2.1 地圖開運(yùn)算處理

因?yàn)槭覂?nèi)環(huán)境中會(huì)有動(dòng)態(tài)物體存在,所以會(huì)使掃地機(jī)器人構(gòu)建的室內(nèi)二維地圖有噪點(diǎn)存在。對(duì)路徑規(guī)劃有很大的干擾,因此需要首先對(duì)這些噪點(diǎn)進(jìn)行濾波處理。

本文采用圖像處理中的開運(yùn)算方法來進(jìn)行濾除。進(jìn)行開運(yùn)算前首先對(duì)地圖進(jìn)行二值化處理,把地圖邊界輪廓的像素單獨(dú)提取出來。為了能夠完整的提取出所有能反映地圖輪廓的像素,通過OTSU算法確定圖像二值化分割閾值。

設(shè)灰度圖像(x,y)有N個(gè)像素點(diǎn)、L個(gè)灰度級(jí)。選取全局閾值Tg,將f(x,y)分為C0和C1兩個(gè)類型,C0是由f(x,y)在灰度級(jí)[0,Tg]內(nèi)的像素點(diǎn)所組成的,C1是由f(x,y)在灰度級(jí)[Tg+1,L]內(nèi)的像素點(diǎn)所組成,由此C0和C1的類間方差可以表示如式(4)所示。

σ=μ0(m0-m)2+μ1(m1-m)2

(4)

式(4)中,μ0和μ1分別是C0和C1兩類的概率,m0和m1分別是C0和C1兩類的灰度均值,m是整幅圖像的灰度均值,在L級(jí)的灰度范圍內(nèi),采用遍歷的方法,選取出當(dāng)類間方差最大時(shí)所對(duì)應(yīng)的閾值Tg,即為相應(yīng)的全局閾值。

圖像二值化處理后,進(jìn)一步對(duì)圖像孤立小點(diǎn)和小橋進(jìn)行濾除,對(duì)地圖進(jìn)行開運(yùn)算處理。即先對(duì)二值圖像做腐蝕運(yùn)算,再做膨脹運(yùn)算。

用結(jié)構(gòu)元素B對(duì)A做腐蝕運(yùn)算,表示為A?B,其式如(5)所示。

A?B={Z|(B)z?A}

(5)

式(5)中,BZ={c│c=b+z,b∈B},指結(jié)構(gòu)元素按照z=(z1,z2)在圖像上平移,即在圖像A上,B中坐標(biāo)值(x,y)被(x+z1,y+z2)替代。腐蝕運(yùn)算式的含義為:將結(jié)構(gòu)元素B相對(duì)于A進(jìn)行平移,只要結(jié)構(gòu)元素都包含在A的前景像素點(diǎn)中,則這些位移Z的像素點(diǎn)的集合為腐蝕結(jié)果。

用結(jié)構(gòu)元素B對(duì)圖像A做膨脹運(yùn)算,表示為A⊕B,其式如(6)所示。

A⊕B={Z|[(B)z∩A]A}

(6)

開運(yùn)算式如式(7)所示。

A0B=(A?B)⊕B

(7)

腐蝕后的圖像變化特征和選取的結(jié)構(gòu)元素及其原點(diǎn)有關(guān),當(dāng)結(jié)構(gòu)元素為3×3大小的矩形且選其中心為原點(diǎn)時(shí),進(jìn)行腐蝕處理后圖像的前景會(huì)縮小一個(gè)像素寬,膨脹處理后圖像背景會(huì)擴(kuò)大一個(gè)像素寬,如圖4所示對(duì)地圖進(jìn)行腐蝕,圖4淺色部分即為選定的正方形結(jié)構(gòu)元素,以此結(jié)構(gòu)元素圖4(b)對(duì)圖4(a)進(jìn)行腐蝕,從圖4(a)的邊界出發(fā),由外向內(nèi)對(duì)該像素群進(jìn)行腐蝕操作,最后得到圖4(c)所示的結(jié)果。

圖4 腐蝕操作

因?yàn)槭菍?duì)圖片全部像素進(jìn)行腐蝕,所以此處使用明克夫斯基差的全局定義形式如式(8)所示。

fΘg=▽{fx+g^(x):x∈D[g^]}

(8)

式(8)中,g^為g對(duì)原點(diǎn)的反射,即先對(duì)g進(jìn)行縱軸反射求得個(gè)g(-x),然后再對(duì)g(-x)進(jìn)行橫軸反射求得-g(-x)。

膨脹操作為腐蝕操作的對(duì)偶運(yùn)算,采用全局膨脹方式進(jìn)行,膨脹全局定義形式如式(9)所示。

f⊕g=Δ{fx+g(x):x∈D[g]}

(9)

式(9)中,g(x)為g對(duì)原點(diǎn)的反射,即先對(duì)g進(jìn)行縱軸反射求得個(gè)g(-x),然后再對(duì)g(-x)進(jìn)行橫軸反射求得-g(-x)。

使用以上方法對(duì)圖1所得到的地圖進(jìn)行腐蝕處理,為了使濾波的效果更好,采用多次腐蝕膨脹處理,迭代三次,得到最終的地圖文件如圖5所示。

圖5 開運(yùn)算結(jié)果

2.2 骨架抽取

經(jīng)過濾波處理后,雖然過濾掉了大部分的噪點(diǎn)但是地圖邊界輪廓仍不夠清晰,工作特性仍不夠友好,因此對(duì)圖5進(jìn)行骨架抽取,以提升地圖質(zhì)量。白色像素定義為0,黑色像素定義為1。設(shè)圖片中任一點(diǎn)像素值為X,首先判斷其是否為黑色像素點(diǎn),若為黑色像素則尋找其八鄰域點(diǎn),分別依次定義為P1～P8,順序如圖6所示。

圖6 八鄰域點(diǎn)

由圖6可知,P2、P4、P6、P8分別為該像素的垂直鄰域點(diǎn),P1、P3、P5、P7分別為交叉鄰域點(diǎn),通過判斷這兩類鄰域點(diǎn)像素值,來確定該像素x是否需要被處理。該像素的垂直鄰域點(diǎn)和交叉鄰域點(diǎn)分別代入式(10)和(11)中。

(10)

(11)

若該像素點(diǎn)滿足2.1和2.2的條件,則被視為非骨骼點(diǎn),被濾除。

圖7中,每一個(gè)方格表示一個(gè)像素,圖7(a)為原圖,對(duì)圖7(a)進(jìn)行骨架抽取,結(jié)構(gòu)如圖7(b)所示,剝離了最外面一層的像素。

圖7 骨架抽取

使用該算法對(duì)經(jīng)過濾波處理的地圖進(jìn)行處理,得到如圖8所示的結(jié)果,可見地圖的輪廓更加清晰簡潔,在骨架抽取的同時(shí)也過濾掉了一些不需要的噪點(diǎn)。

圖8 抽取后地圖

路徑規(guī)劃結(jié)果如圖9所示,因?yàn)闆]有細(xì)小黑點(diǎn)的干擾與原始地圖全覆蓋路徑規(guī)劃的效果相比,拐點(diǎn)更少,覆蓋率更高,提高了掃地機(jī)器人的清潔效率。

圖9 路徑規(guī)劃效果

3 實(shí)驗(yàn)及結(jié)果分析

3.1 機(jī)器人實(shí)驗(yàn)平臺(tái)搭建及建圖

為了驗(yàn)證該方法的可行性和實(shí)際工作效率,自主搭建了機(jī)器人實(shí)驗(yàn)平臺(tái),如圖10所示。機(jī)器人實(shí)驗(yàn)平臺(tái)為差速運(yùn)動(dòng)結(jié)構(gòu),已通過建模的方法對(duì)其路徑偏差進(jìn)行糾正,底層運(yùn)動(dòng)控制芯片為stm32f103rct6,主控處理器選用Jetson TX1,內(nèi)部搭載Ubuntu18.04操作系統(tǒng),激光傳感器使用RPLIDAR A1。

圖10 實(shí)驗(yàn)機(jī)器人及實(shí)驗(yàn)室

首先對(duì)實(shí)驗(yàn)室進(jìn)行地圖構(gòu)建,如下圖11所示?？砂l(fā)現(xiàn),原始地圖中有較多黑色噪點(diǎn)。

圖11 實(shí)驗(yàn)室原始地圖

3.2 實(shí)驗(yàn)結(jié)果及分析

使用傳統(tǒng)的內(nèi)螺旋式全覆蓋路徑規(guī)劃,規(guī)劃結(jié)果如圖12所示。

圖12 原始地圖規(guī)劃結(jié)果

由圖12可知,地圖上有大量離散黑點(diǎn)。在機(jī)器人建圖的時(shí)候,實(shí)驗(yàn)室內(nèi)有人在周圍走動(dòng),當(dāng)障礙物在機(jī)器人經(jīng)過的全過程中一直存在,則會(huì)被認(rèn)定為障礙物,即使機(jī)器人離開之后,該障礙物依然存在于地圖中。

對(duì)本算法處理后的地圖進(jìn)行路徑規(guī)劃,結(jié)果如圖13所示,可見輪廓修整得更加平滑,且濾掉了地圖上影響全局路徑規(guī)劃的細(xì)小噪點(diǎn)。由圖易知,該路徑更加規(guī)整,且覆蓋范圍更廣,使用機(jī)器人按照上圖算法分別運(yùn)行30次,對(duì)測得數(shù)據(jù)取平均值,并整理如表1所示。

圖13 骨架地圖全覆蓋路徑規(guī)劃

表1 實(shí)際清掃指標(biāo)

由表1可知,在本文處理后的地圖上進(jìn)行路徑規(guī)劃,路徑的長度平均減少了13%,而且全局路徑的拐點(diǎn)數(shù)相對(duì)于原始地圖減少了15.64%,因?yàn)槭褂玫膶?dǎo)航算法有局部路徑規(guī)劃實(shí)時(shí)避障,所以碰撞次數(shù)基本不變。本文優(yōu)化后的地圖,對(duì)小型障礙物和移動(dòng)的物體進(jìn)行濾除后,實(shí)際清潔率有了明顯提高,由原有的91%提高至98%,滿足室內(nèi)清潔的需求。

4 結(jié)論

本文針對(duì)掃地機(jī)器人全覆蓋路徑算法路徑拐點(diǎn)多,效率低的問題,提出了一種基于地圖優(yōu)化的掃地機(jī)器人全局路徑規(guī)劃算法,其通過抽取二維柵格地圖輪廓的骨架,及使用迭代腐蝕膨脹對(duì)地圖進(jìn)行濾波處理,獲得了更高效更平滑的覆蓋路線。經(jīng)實(shí)驗(yàn)驗(yàn)證,全局路徑拐點(diǎn)次數(shù)明顯減少了15.64%,實(shí)際清潔率提高至98%。