基于骨架地圖的全覆蓋路徑優化方法

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

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

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

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

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

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

1 二維激光SLAM地圖構建與路徑規劃

1.1 二維激光SLAM地圖構建

結合實際工作環境以及機器人的算力,首先選擇Gmapping算法來進行室內地圖構建,在此基礎上對其進行路徑規劃研究。Gmapping算法以RBPF方法為基礎,對提議分布進行了改進,并且提出了選擇性采樣的方法。

1.1.1 RBPF

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

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

(2)

(3)

式(3)中,用有限K個數據積分近似轉變成求和,然后進行權重計算。

1.1.3 選擇性重采樣

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

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

1.2 路徑規劃

室內掃地機器人工作空間封閉,且區域多網格化,首先選用傳統的內螺旋式路徑規劃方法進行效果測試,如圖2所示。

圖2 螺旋算法示意圖

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

對仿真環境的柵格地圖進行全覆蓋路徑規劃,規劃效果如圖3所示。

圖3 路徑規劃效果

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

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

2 地圖優化算法

2.1 地圖開運算處理

因為室內環境中會有動態物體存在,所以會使掃地機器人構建的室內二維地圖有噪點存在。對路徑規劃有很大的干擾,因此需要首先對這些噪點進行濾波處理。

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

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

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

(4)

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

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

用結構元素B對A做腐蝕運算,表示為A?B,其式如(5)所示。

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

(5)

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

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

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

(6)

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

A0B=(A?B)⊕B

(7)

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

圖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 開運算結果

2.2 骨架抽取

經過濾波處理后,雖然過濾掉了大部分的噪點但是地圖邊界輪廓仍不夠清晰,工作特性仍不夠友好,因此對圖5進行骨架抽取,以提升地圖質量。白色像素定義為0,黑色像素定義為1。設圖片中任一點像素值為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)進行骨架抽取,結構如圖7(b)所示,剝離了最外面一層的像素。

圖7 骨架抽取

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

圖8 抽取后地圖

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

圖9 路徑規劃效果

3 實驗及結果分析

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

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

圖10 實驗機器人及實驗室

首先對實驗室進行地圖構建,如下圖11所示?？砂l現,原始地圖中有較多黑色噪點。

圖11 實驗室原始地圖

3.2 實驗結果及分析

使用傳統的內螺旋式全覆蓋路徑規劃,規劃結果如圖12所示。

圖12 原始地圖規劃結果

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

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

圖13 骨架地圖全覆蓋路徑規劃

表1 實際清掃指標

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

4 結論

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