基于骨架地圖的全覆蓋路徑優(yōu)化方法

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

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

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

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

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

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

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

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

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

1.1.1 RBPF

機器人通過觀測信息和里程計數(shù)據(jù)來獲得其位姿和地圖,RBPF算法對概率密度函數(shù)進行因式分解,首先估計位姿,然后計算柵格圖。即拆開先求其中某一個變量,在解決另一個變量之前,先解決此變量,計算方法如式(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 提議分布

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

(2)

(3)

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

1.1.3 選擇性重采樣

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

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

1.2 路徑規(guī)劃

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

圖2 螺旋算法示意圖

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

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

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

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

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

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

2.1 地圖開運算處理

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

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

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

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

(4)

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

圖像二值化處理后,進一步對圖像孤立小點和小橋進行濾除,對地圖進行開運算處理。即先對二值圖像做腐蝕運算,再做膨脹運算。

用結(jié)構(gòu)元素B對A做腐蝕運算,表示為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中坐標值(x,y)被(x+z1,y+z2)替代。腐蝕運算式的含義為:將結(jié)構(gòu)元素B相對于A進行平移,只要結(jié)構(gòu)元素都包含在A的前景像素點中,則這些位移Z的像素點的集合為腐蝕結(jié)果。

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

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

(6)

開運算式如式(7)所示。

A0B=(A?B)⊕B

(7)

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

圖4 腐蝕操作

因為是對圖片全部像素進行腐蝕,所以此處使用明克夫斯基差的全局定義形式如式(8)所示。

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

(8)

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

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

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

(9)

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

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

圖5 開運算結(jié)果

2.2 骨架抽取

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

圖6 八鄰域點

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

(10)

(11)

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

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

圖7 骨架抽取

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

圖8 抽取后地圖

路徑規(guī)劃結(jié)果如圖9所示,因為沒有細小黑點的干擾與原始地圖全覆蓋路徑規(guī)劃的效果相比,拐點更少,覆蓋率更高,提高了掃地機器人的清潔效率。

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

3 實驗及結(jié)果分析

3.1 機器人實驗平臺搭建及建圖

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

圖10 實驗機器人及實驗室

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

圖11 實驗室原始地圖

3.2 實驗結(jié)果及分析

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

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

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

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

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

表1 實際清掃指標

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

4 結(jié)論

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