智能掃地機器人的全覆蓋路徑規(guī)劃

黃月琴，羅兵，鄧輔秦,，李偉科，楊勇

（1.五邑大學(xué) 智能制造學(xué)部，廣東 江門 529020；2.深圳市杉川機器人有限公司，廣東 深圳 518000）

掃地機器人是目前應(yīng)用較廣泛的機器人產(chǎn)品，對全部清掃區(qū)域的全覆蓋路徑規(guī)劃，是掃地機智能化研究的內(nèi)容之一.該優(yōu)化問題在遙感拍攝、大面積對象自動檢測、自動噴涂等自動控制技術(shù)應(yīng)用中是共性問題，統(tǒng)稱為全覆蓋路徑規(guī)劃（Complete Coverage Path Planning，CCPP）[1-2].對CCPP問題的解決方案主要分為3類：柵格地圖法、單元分解法和兩者相結(jié)合的方法[3-6].由于柵格地圖法還有待解決移動死區(qū)問題，單元分解方法是目前解決CCPP問題效果最好的方法[7-9].

單元分解方法先將一個有界單元分解為一組互不重疊且不包含障礙物的單元，再進(jìn)行單元內(nèi)路徑優(yōu)化，最后進(jìn)行單元間轉(zhuǎn)移路徑優(yōu)化.其中單元分解有梯形分解法、布氏單元分解法（Boustrophedon Cellular Decomposition，BCD）和莫爾斯分解方法等，BCD能有效減少分解后的單元數(shù)目[10-13].文獻(xiàn)[7]使用 BCD在礦區(qū)檢測應(yīng)用中分解礦區(qū)地圖后再利用神經(jīng)網(wǎng)絡(luò)進(jìn)行路徑規(guī)劃，得到優(yōu)化的CCPP路徑.

在單元內(nèi)的路徑規(guī)劃中，往復(fù)式路徑（Back and Forth Path，BFP）能適應(yīng)任意凸多邊形且計算簡單而被廣泛應(yīng)用.為了尋找單元內(nèi)最優(yōu)BFP路徑，Huang[3]提出了最小傾向和方法（Minimal Sum of Attitudes，MSA），通過尋找平行于某一條邊界的掃描線，使得 BFP路徑轉(zhuǎn)彎次數(shù)更少.最近，Vasquez-Gomez等人[5]提出了旋轉(zhuǎn)卡殼路徑規(guī)劃方法（Rotating Calipers Path Planner，RCPP），可以快速找到凸多邊形的最佳BFP，比其他方法更為高效.RCPP方法算法復(fù)雜度低，適合在嵌入式設(shè)備中使用.

在單元間的轉(zhuǎn)移路徑規(guī)劃方面，文獻(xiàn)[14]基于蟻群算法和遺傳算法的優(yōu)化方法被采用，設(shè)計了玻璃幕墻清潔機器人的CCPP.但現(xiàn)有的單元間轉(zhuǎn)移路徑優(yōu)化方法沒有考慮單元內(nèi)路徑優(yōu)化，而且單元內(nèi)BFP路徑有多個較優(yōu)解時未考慮不同單元起始點，這些缺點降低了全局優(yōu)化效果.

基于單元分解的 CCPP在單元內(nèi)、單元間路徑規(guī)劃時已取得了較好的效果.但對于復(fù)雜形狀的凹區(qū)域劃分過于瑣碎，未考慮相鄰區(qū)域的總體規(guī)劃[13].另一方面，在掃地機器人的實際應(yīng)用中，機器人的轉(zhuǎn)彎和調(diào)頭都會影響其移動速度，需要額外能耗和時間[15].為此，本文將在3個方面進(jìn)行改進(jìn)：首先在路徑規(guī)劃代價函數(shù)中加入轉(zhuǎn)彎和調(diào)頭的代價；其次在單元分解時，對于可以和相鄰區(qū)域統(tǒng)一規(guī)劃的凹區(qū)域不再分解；單元內(nèi)路徑規(guī)劃兼顧單元間的轉(zhuǎn)移路徑規(guī)劃，全局優(yōu)化結(jié)合全搜索和蟻群算法.

1 路徑代價函數(shù)和地圖單元分解的改進(jìn)

本文以深圳市杉川掃地機器人為研究對象，實驗場地為室內(nèi)封閉環(huán)境.機器人直徑34.5 cm，形狀為圓形.在此應(yīng)用條件下設(shè)計了符合掃地機場景的實際代價函數(shù)，評估覆蓋路徑開銷.另外，在仿真地圖環(huán)境中，單元分解數(shù)目過多時采用合并相鄰且尺寸小的單元.

1.1 路徑代價函數(shù)改進(jìn)

傳統(tǒng)路徑優(yōu)化目標(biāo)只考慮路徑長度最短，規(guī)劃時代價函數(shù)是起點和終點之間歐氏距離[5-6].但在掃地機實際應(yīng)用中，相同距離的直線移動和帶轉(zhuǎn)彎的移動所花時間和功耗均不相同，轉(zhuǎn)彎前須減速，轉(zhuǎn)彎后重新加速，轉(zhuǎn)彎會帶來更多的能耗和時間開銷[15].Theresa[4]提出綜合轉(zhuǎn)彎次數(shù)、機器人半徑、路徑偏移量和區(qū)域邊界線角度的距離代價函數(shù).這一方法更符合實際情況，準(zhǔn)確評估路徑的總代價.不同于無人機轉(zhuǎn)彎時可以超越環(huán)境邊界，掃地機工作空間是封閉環(huán)境，轉(zhuǎn)彎時被限制在距離邊界機器人半徑的距離內(nèi).因此，本文基于文獻(xiàn)[4]的代價函數(shù)設(shè)計新的代價函數(shù).

通過實驗測試發(fā)現(xiàn)，保持均速移動時，掃地機每次轉(zhuǎn)彎的時間開銷是大致相當(dāng)?shù)?杉川掃地機移動速度為0.8 m/s，同時移動兩個掃地機器人，其中一個有一次直角轉(zhuǎn)彎累計移動距離10 m，另一個無轉(zhuǎn)彎直線移動同樣時間內(nèi)移動了10.45 m.類似的大量實驗表明：對于0.8 m/s移動速度的掃地機器人，每次轉(zhuǎn)彎的開銷可以折合為0.45 m.類似條件下的測試也顯示，調(diào)頭的開銷大約是0.49 m.本文將一次轉(zhuǎn)彎、調(diào)頭的開銷，線性折合為路徑長度，折合系數(shù)分別是ct=0.45 m/次、cr=0.49 m/次.系數(shù)值可根據(jù)不同掃地機器人進(jìn)行調(diào)整.

通過將轉(zhuǎn)彎次數(shù)、調(diào)頭次數(shù)折合為移動距離的方法，將多目標(biāo)優(yōu)化方法轉(zhuǎn)化為單目標(biāo)優(yōu)化方法.代價函數(shù)如下：

其中ct為轉(zhuǎn)彎代價系數(shù)，Nt表示轉(zhuǎn)彎次數(shù)，cr為調(diào)頭代價系數(shù)，Nr表示調(diào)頭次數(shù)，Lin是分解的n個單元內(nèi)的路徑長度之和.

每個單元的路徑長度Lin(i)按歐氏距離計算：

Llink表示n個單元之間的轉(zhuǎn)移路徑長度：

通過代價函數(shù)的改進(jìn)，在路徑優(yōu)化中，不僅追求路徑長度最短，還會尋找轉(zhuǎn)彎、調(diào)頭次數(shù)更少的路徑.

1.2 地圖單元分解的改進(jìn)

傳統(tǒng)單元分解原則是將凹多邊形分解為多個凸多邊形，然后在凸多邊形內(nèi)分別進(jìn)行路徑規(guī)劃，最后在單元間規(guī)劃轉(zhuǎn)移連接路徑[3,5].這樣雖然可以實現(xiàn)路徑長度優(yōu)化，但會導(dǎo)致分解過于細(xì)碎、路徑轉(zhuǎn)彎多、調(diào)頭多，難以達(dá)到總體最優(yōu)，如圖1-a.實際上，很多凹多邊形仍然是可以進(jìn)行路徑規(guī)劃尋優(yōu)的.為此，我們改進(jìn)為：對于存在一組平行線路徑可以全覆蓋的凹多邊形，不再分解，這樣將減少分解單元數(shù)量，原多個相鄰單元總體進(jìn)行路徑規(guī)劃將減少路徑轉(zhuǎn)彎、調(diào)頭次數(shù)，連接路徑也縮短，如圖1-b.

圖1 不同單元分解方法對比及路徑規(guī)劃效果

圖1中的虛線是單元分解線，傳統(tǒng)方法將圖1的凹多邊形分解為3個矩形，各單元采用式（1）代價函數(shù)進(jìn)行路徑規(guī)劃，如圖1-a，路徑多了一段兩個單元間的連接線，轉(zhuǎn)彎次數(shù)為21次.本文方案不再分解該凹多邊形，路徑如圖1-b所示，有一段重復(fù)路徑，轉(zhuǎn)彎次數(shù)為14，調(diào)頭1次.兩種方案的代價計算結(jié)果見表1.可見，避免過于細(xì)碎的單元分解可以減少轉(zhuǎn)彎、調(diào)頭次數(shù)，路徑規(guī)劃效果更優(yōu).

表1 不同單元分解方案的路徑規(guī)劃結(jié)果對比

2 路徑優(yōu)化

2.1 凸多邊形區(qū)域的路徑規(guī)劃

對于凸區(qū)域路徑規(guī)劃，文獻(xiàn)[5]中的RCPP方法可以快速求得無人機場景中的最佳BFP，但是不適用于掃地機場景，而且尋優(yōu)時路徑代價未計算轉(zhuǎn)彎次數(shù).故本文采用 RCPP時，將路徑點限制在邊界內(nèi)，并使用公式（1）作為代價函數(shù)選擇最優(yōu)路徑.

被分解后的區(qū)域中包含凹、凸單元，記多邊形Q=｛V,E｝，V=｛ 1,2,…,n｝是順時針的凸多邊形頂點集合，E=｛(1,2),(2,3),…,(n-1,n),(n,1)｝是由V中元素組成的邊.多邊形的區(qū)域為A(Q)，設(shè)掃地機的一條路徑s的覆蓋區(qū)域為C(s).對于 2D凸多邊形的一條覆蓋路徑ρ需滿足C(ρ)?A(Q).具體算法步驟如下：

1）計算凸區(qū)域的頂點集合Q=｛V｝的對踵點集合A=｛(i1,j1),…,(in,jn)｝；2）對于步驟1）中A中的每對對踵點，使用順時針卡殼和逆時針卡殼分別計算出兩條 BFP，返回總長度最短的路徑；3）針對A中對踵點下的路徑，根據(jù)式（1）選擇長度最短的路徑作為凸區(qū)域的BFP；4）獲得全覆蓋凸區(qū)域的路徑ρ.

步驟 1）中的對踵點對是凸多邊形上的一對頂點，過這對點的一組平行線可以將凸多邊形夾在這兩條平行線中間，或落在平行線上.圖2展示了該凸多邊形的6對對踵點和對應(yīng)BFP.圖中的虛線表示穿過對踵點的平行線，圖2-a中路徑起始于標(biāo)號（0,4）的邊，向垂直于（0,4）邊往2頂點的方向移動.

步驟2）計算每對對踵點(in,jn)的BFP長度，分別使用順時針RCPP和逆時針RCPP方法，即過該對踵點的平行線對尋找首次與多邊形接觸的邊，這條邊稱為基線.順逆旋轉(zhuǎn)得到的基線分別記為(b1,b1+1)和(b2,b2+1)，而路徑遠(yuǎn)端的頂點為相應(yīng)的對踵點a1和a2.因此一對對踵點有兩種 BFP.盡管是同一個凸區(qū)域，不同方向的RCPP方法得到的規(guī)劃路徑長度會有不同，在保證覆蓋率的情況下，路徑長度短的更適用于能量有限的掃地機.因此，保證每一個分解后的單元內(nèi)路徑最優(yōu)，比較d1和d2的大小.兩者距離點到直線公式和最短路徑?jīng)Q策如式（4）和（5）.計算圖2的每對對踵點的路徑總代價分別為948.48 m、985.09 m、1072.18 m、1072.67 m、995.82 m、949.39 m，選擇代價最小的路徑作為該凸邊形的覆蓋路徑.

圖2 凸多邊形對踵點下的各種路徑情況

2.2 凹多邊形區(qū)域的全覆蓋路徑規(guī)劃

凹多邊形區(qū)域的覆蓋路徑規(guī)劃采用直接法（Direct Method，DM），算法描述如下：

1）初始化凹多邊形的上下邊界點集C={(x1,y1),(x2,y2),…,(xn,yn)}、F={(x1,f1),(x2,f2),…,(xn,fn)}、運動方向向下和掃地機半徑r；

2）單元的最左端線段L向右偏移r，L的橫坐標(biāo)為xcurrent=xleft+r.運動方向向下，此時路徑ρ中加入路徑點(xcurrent,ycurrent-r),(xcurrent,fcurrent+r)，更改運動方向向上.若方向向上路徑ρ中加入路徑點(xcurrent,fcurrent+r),(xcurrent,ycurrent-r)，更改運動方向向下；

3）位于xcurrent的掃描線繼續(xù)向右平移d=2r.若向下運動，路徑ρ加入路徑點集合{(xcurrent,fcurrent+r),(xcurrent+1,fcurrent+1+r),…,(xcurrent+d,fcurrent+d+r)}，更改運動方向向上.若向上，路徑ρ加入 {(xcurrent,ycurrent-r),(xcurrent+1,ycurrent+1-r),…,(xcurrent+d,ycurrent+d-r)}，更改運動方向向下.更新xcurrent；

4）重復(fù)步驟2）直至L到達(dá)單元的最右端xright；5）若為向右運動，路徑為ρ.若向左，逆序排列路徑點；6）獲得覆蓋凹區(qū)域路徑ρ.

2.3 單元間的統(tǒng)一優(yōu)化及轉(zhuǎn)移路徑優(yōu)化

通過以上兩節(jié)可以獲得單元內(nèi)的優(yōu)化路徑，但各單元內(nèi)路徑優(yōu)化沒有考慮單元間的轉(zhuǎn)移連接路徑.特別是當(dāng)單元數(shù)目較多時單元間的轉(zhuǎn)移路徑長度對全覆蓋的路徑總長度影響更大.圖3展示了不同的路徑組合產(chǎn)生不同的路徑代價，圖3-d的路徑代價最小，與代價最大的圖3-a相差141.99 m，超過路徑總長度的 10%.當(dāng)單元間的轉(zhuǎn)移路徑選擇最短的路徑組合時，必然會縮短整體區(qū)域的覆蓋路徑長度.

圖3 一個單元的4種起始點組合

文獻(xiàn)[1]運用蟻群算法較好地解決了水下機器人的CCPP問題中的單元間的連接優(yōu)化，但對于單元內(nèi)的多種優(yōu)化路徑對單元間的影響沒有考慮.為此，本文采取全搜索結(jié)合蟻群算法來進(jìn)行優(yōu)化，具體方法是：

1）保留每個單元內(nèi)不同起止點的多個較優(yōu)解，與相鄰單元統(tǒng)一進(jìn)行組合優(yōu)化；

2）確定每個單元的起點、終點、優(yōu)化路徑；

3）當(dāng)單元數(shù)較少，或比較選擇的組合數(shù)量較少時，采用全搜索選擇最優(yōu)解，當(dāng)單元數(shù)量較多或組合選擇多難以全搜索時，采用蟻群算法進(jìn)行尋優(yōu).

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

由于掃地機器人主要是針對家庭使用，本實驗選取了8種具代表性的家居布局以及2種特殊設(shè)計的布局。來進(jìn)行CCPP仿真實驗，仿真實驗運行于Python環(huán)境，掃地機直徑設(shè)置為34.5 cm.分別使用平行于垂直方向掃描方向DM方法+遺傳算法[6]、RCPP方法+蟻群算法[5,16]和本文方案對已知地圖進(jìn)行CCPP對比實驗.

本文方案首先使用改進(jìn) BCD方法對二維平面地圖單元分解，其中一種特殊設(shè)計的布局地圖獲得的單元分解結(jié)果如圖4-a所示，黑色表示不可達(dá)障礙區(qū)域，虛線代表單元邊界.然后使用式（1）和相鄰單元間統(tǒng)一優(yōu)化等改進(jìn)方案進(jìn)行路徑優(yōu)化，最后得到的優(yōu)化路徑結(jié)果如圖4-b所示，圖中圓點和交叉分別代表單元內(nèi)路徑起點和止點，虛線表示單元間轉(zhuǎn)移路徑，實線代表掃地機路徑.

圖4 本文方案的單元分解和全覆蓋路徑規(guī)劃

本文改進(jìn)的單元分解法分解為12個單元，傳統(tǒng)方法分解為26個單元.對該布局的 DM方法+遺傳算法、旋轉(zhuǎn)卡殼RCPP方法+蟻群算法進(jìn)行的對比實驗結(jié)果如圖5所示.

圖5 對比的全覆蓋路徑規(guī)劃結(jié)果

3種方案的總覆蓋平均路徑總代價、平均轉(zhuǎn)彎數(shù)目、平均Lin和平均Llink的統(tǒng)計結(jié)果如表2所示.結(jié)果表明DM方法的路徑代價最大，使用本文方案轉(zhuǎn)彎數(shù)最少、路徑總代價最小.

表2 3種方法的全覆蓋路徑規(guī)劃結(jié)果

由于DM方法僅使用垂直掃描方向，故當(dāng)單元的水平跨度大于垂直跨度時，其路徑不是最優(yōu)的，導(dǎo)致單元內(nèi)轉(zhuǎn)彎次數(shù)增大.RCPP方法在凸區(qū)域內(nèi)可以得到最佳BFP，但拆分所有非凸區(qū)域為凸區(qū)域增加了單元間的轉(zhuǎn)移路徑和轉(zhuǎn)彎次數(shù).實驗結(jié)果表明，本文方案更適合復(fù)雜環(huán)境的CCPP.

4 結(jié)論

本文設(shè)計了考慮轉(zhuǎn)彎、調(diào)頭開銷的CCPP代價函數(shù)，并以簡單線性系數(shù)將其轉(zhuǎn)化為路徑長度，有利于優(yōu)化比較.在單元分解時采用了可規(guī)劃凹區(qū)域不再分解方法避免過于細(xì)碎的分解，減少轉(zhuǎn)彎次數(shù)和單元間連接路徑長度.單元內(nèi)優(yōu)化考慮了相鄰單元間的統(tǒng)一優(yōu)化和連接，并采用全搜索和蟻群算法進(jìn)行優(yōu)化.仿真實驗表明，本方案比傳統(tǒng)方案減少了轉(zhuǎn)彎次數(shù)，總體優(yōu)化效果更好.但是由于使用實驗的掃地機器人型號有限，轉(zhuǎn)彎、調(diào)頭代價的實驗規(guī)模均不夠，后期通過更多的實驗，有望建立更合理的轉(zhuǎn)彎、調(diào)頭代價函數(shù).另外，單元間的統(tǒng)一優(yōu)化、轉(zhuǎn)移路徑優(yōu)化算法對于存在多單元時仍有待改進(jìn).