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

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

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

單元分解方法先將一個(gè)有界單元分解為一組互不重疊且不包含障礙物的單元，再進(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ū)檢測(cè)應(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)任意凸多邊形且計(jì)算簡(jiǎn)單而被廣泛應(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è)計(jì)了玻璃幕墻清潔機(jī)器人的CCPP.但現(xiàn)有的單元間轉(zhuǎn)移路徑優(yōu)化方法沒有考慮單元內(nèi)路徑優(yōu)化，而且單元內(nèi)BFP路徑有多個(gè)較優(yōu)解時(shí)未考慮不同單元起始點(diǎn)，這些缺點(diǎn)降低了全局優(yōu)化效果.

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

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

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

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

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

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

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

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

每個(gè)單元的路徑長(zhǎng)度Lin(i)按歐氏距離計(jì)算：

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

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

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

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

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

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

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

2 路徑優(yōu)化

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

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

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

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

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

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

圖2 凸多邊形對(duì)踵點(diǎn)下的各種路徑情況

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

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

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

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

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

4）重復(fù)步驟2）直至L到達(dá)單元的最右端xright；5）若為向右運(yùn)動(dòng)，路徑為ρ.若向左，逆序排列路徑點(diǎn)；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ù)目較多時(shí)單元間的轉(zhuǎn)移路徑長(zhǎng)度對(duì)全覆蓋的路徑總長(zhǎng)度影響更大.圖3展示了不同的路徑組合產(chǎn)生不同的路徑代價(jià)，圖3-d的路徑代價(jià)最小，與代價(jià)最大的圖3-a相差141.99 m，超過路徑總長(zhǎng)度的 10%.當(dāng)單元間的轉(zhuǎn)移路徑選擇最短的路徑組合時(shí)，必然會(huì)縮短整體區(qū)域的覆蓋路徑長(zhǎng)度.

圖3 一個(gè)單元的4種起始點(diǎn)組合

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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