基于改進(jìn)單元分解法的全覆蓋路徑規(guī)劃

吳靖宇, 朱世強(qiáng), 宋 偉,*, 施浩磊, 吳澤南

(1. 浙江大學(xué)海洋學(xué)院, 浙江 舟山316021; 2. 浙江大學(xué)機(jī)器人研究院, 浙江 寧波 315400;3. 之江實(shí)驗(yàn)室, 浙江 杭州 311121; 4. 舟山市質(zhì)量技術(shù)監(jiān)督檢測研究院, 浙江 舟山 316013)

0 引 言

當(dāng)在靜態(tài)已知環(huán)境中進(jìn)行全覆蓋路徑的離線規(guī)劃時(shí),單元分解法是一種常用的方法[16-17]。該方法的實(shí)現(xiàn)過程主要分為兩個(gè)步驟,分別為劃分單元和求解單元間遍歷順序。其中劃分單元是指將待遍歷的區(qū)域劃分為若干個(gè)互不重疊的子區(qū)域。傳統(tǒng)的單元分解法在此步驟采用的是根據(jù)障礙物的形狀和位置劃分單元的方式,包括:Trapezoidal分解[18]、Boustrophedon分解[19-21]和Morse分解[22-23]。而求解單元間遍歷順序則是一個(gè)旅行商問題,常用的方法包括深度優(yōu)先搜索[24]、模擬退火法[25]、蟻群算法[26]、粒子群優(yōu)化算法[27]、遺傳算法[28-29]、強(qiáng)化學(xué)習(xí)[30]和深度強(qiáng)化學(xué)習(xí)[31]等。

但傳統(tǒng)的單元分解法在障礙物分布不規(guī)則和凹形障礙物較多的場景下卻易出現(xiàn)較多的冗余路徑和轉(zhuǎn)向次數(shù)。其中分布不規(guī)則是指在柵格地圖上,各個(gè)障礙物的幾何中心一般位于不同的行和列。日常生活中椅子、凳子等物件即會(huì)因人們的使用而成為分布不規(guī)則的障礙物。凹形障礙物則可見于辦公區(qū)域等場景,例如只有一扇門進(jìn)出的房間,其四周墻壁即可視為凹形障礙物。在這兩種場景中,待遍歷區(qū)域的形狀變得不規(guī)則,使得傳統(tǒng)的單元分解法所得單元的數(shù)量較多。而單元數(shù)量的增加會(huì)使得每個(gè)單元的平均面積減少,造成傳統(tǒng)的單元分解法難以在更大的范圍內(nèi)應(yīng)用并優(yōu)化局部路徑,故易產(chǎn)生較多的冗余路徑和轉(zhuǎn)向次數(shù)。對此,本文提出一種改進(jìn)的規(guī)劃算法,目的是減少最終路徑的冗余和轉(zhuǎn)向次數(shù)。改進(jìn)后的單元?jiǎng)澐滞ㄟ^在柵格地圖上合并路徑片段來實(shí)現(xiàn)(路徑片段由位于同一行且左右相鄰的柵格組成)。這樣,單元的劃分不再完全依賴于障礙物的形狀和位置,而是兼顧了單元內(nèi)的路徑規(guī)劃。同時(shí),為了減少單元的數(shù)量,在求解單元間遍歷順序時(shí),結(jié)合貪心算法和拓?fù)涞貓D進(jìn)行三次求解,不斷合并單元,并對局部路徑進(jìn)行了優(yōu)化。

本文的結(jié)構(gòu)安排如下:引言部分介紹了路徑規(guī)劃的研究現(xiàn)狀和傳統(tǒng)單元分解法存在的一些不足;第1節(jié)闡述了路徑片段初步合并生成單元的過程;第2節(jié)闡述了基于貪心算法和拓?fù)涞貓D的單元間遍歷順序的求解過程;第3節(jié)進(jìn)行了仿真實(shí)驗(yàn),其結(jié)果驗(yàn)證了本文算法的有效性;第4節(jié)對全文進(jìn)行了總結(jié),指出當(dāng)前本文算法存在的不足。本文的主要?jiǎng)?chuàng)新點(diǎn)在于:改進(jìn)了傳統(tǒng)單元分解法劃分單元的方式,并通過對單元的合并,降低了在障礙物分布不規(guī)則和凹形障礙物較多的場景下單元?jiǎng)澐值臄?shù)量,減少了冗余路徑和轉(zhuǎn)向次數(shù)。

1 單元初步合并生成

本節(jié)通過合并路徑片段初步生成單元,以實(shí)現(xiàn)對待遍歷區(qū)域的單元?jiǎng)澐?。相關(guān)步驟包括:柵格地圖初始化、生成路徑片段、確定遍歷起始單元和合并路徑片段。

1.1 柵格地圖初始化

首先,建立平面直角坐標(biāo)系,其原點(diǎn)O位于地圖左上角,向下為X軸,向右為Y軸,則第a行、第b列柵格可以用坐標(biāo)(a,b)表示。

然后,建立與柵格地圖對應(yīng)的同維度矩陣,并將待遍歷柵格以數(shù)字0表示,將障礙物以數(shù)字1表示。為獲取該矩陣對應(yīng)位置的數(shù)值,定義函數(shù)F(a,b)如下:

(1)

設(shè)地圖上所有的柵格組成集合Call,則其中待遍歷柵格可以表示為

Ccov={(xi,yi)∈Call|F(xi,yi)=0}

(2)

1.2 生成路徑片段

定義路徑片段如下:地圖上位于同一行且未被障礙物阻隔的全部連通柵格的有序排列集合,則第R行的某一路徑片段CR={(xR,y1),(xR,y2),…,(xR,yr)}滿足:

(3)

設(shè)某一地圖的待遍歷區(qū)域通過劃分共生成了n個(gè)路徑片段,則它們組成的集合Cpath={C1,C2,…,Cn}滿足:

(4)

圖1所示為一個(gè)生成路徑片段的示例(圖中黑色柵格為障礙物,白色柵格為待遍歷區(qū)域,下同),其中屬于同一路徑片段的柵格以線段依次相連。顯然,每條線段均可作為機(jī)器人的潛在移動(dòng)路徑,而單元?jiǎng)t可通過合并這些路徑片段來生成。

圖1 生成路徑片段Fig.1 Path fragments generation

1.3 確定遍歷起始單元

假定機(jī)器人的直線運(yùn)動(dòng)平行于坐標(biāo)軸,且平均速度為v;機(jī)器人每轉(zhuǎn)過90°視為一次轉(zhuǎn)向,且所需時(shí)間為t。機(jī)器人在從坐標(biāo)為(a,b)的柵格移動(dòng)到坐標(biāo)為(c,d)的過程中共轉(zhuǎn)向M次,直線運(yùn)動(dòng)的總路程長度為L,則該次移動(dòng)所需的總時(shí)間為

(5)

若在該時(shí)間內(nèi)機(jī)器人始終以平均速度v做直線運(yùn)動(dòng),則可移動(dòng)距離為

L′=v·T[(a,b),(c,d)]=L+vtM

(6)

記P=vt,則機(jī)器人從柵格(a,b)移動(dòng)到(c,d)的等效路徑長度為

L′=L+PM

(7)

由于v和t的值僅與機(jī)器人的自身性能有關(guān),數(shù)值可由實(shí)驗(yàn)測出。當(dāng)機(jī)器人的型號(hào)確定后,參數(shù)P即為一個(gè)常量。因此,由式(7)計(jì)算所得的等效路徑長度L′可作為評價(jià)路徑優(yōu)劣的評價(jià)指標(biāo)。

以式(7)計(jì)算機(jī)器人的初始位置所在柵格到每個(gè)路徑片段首尾兩個(gè)端點(diǎn)柵格的等效路徑長度,根據(jù)最小值確定起始路徑片段和起始柵格。若起始柵格位于起始路徑片段的末端,則對起始路徑片段中的柵格進(jìn)行倒序排列。后續(xù)由起始路徑片段合并生成的單元即是遍歷起始單元,記為CS。

1.4 合并路徑片段

路徑片段的合并流程如圖2所示。

圖2 路徑片段合并流程圖Fig.2 Flowchart of path fragment mergency

具體步驟如下:

步驟 1生成空集合CMer,對合并完成的單元進(jìn)行存儲(chǔ)。

步驟 2判斷Cpath中是否存在某一路徑片段Cm(1≤m≤n)且滿足以下兩個(gè)要求:

(1)Cm在地圖上的相鄰路徑片段Cadj屬于Cpath;

(2)Cadj僅有一個(gè)端點(diǎn)柵格與Cm的某一端點(diǎn)柵格上下相鄰。其中,兩個(gè)路徑片段相鄰的定義為:存在兩個(gè)上下相鄰的柵格分別屬于這兩個(gè)路徑片段,即?(a,b)∈Cm,?(c,d)∈Cadj,滿足:

(8)

此外,為避免起始柵格之前出現(xiàn)路徑,將起始柵格視為與所有柵格均不相鄰。若存在Cm滿足要求,則將Cadj合并至Cm,并跳轉(zhuǎn)至步驟3,否則跳轉(zhuǎn)至步驟7。

圖3所示為本步驟中路徑片段滿足合并要求的兩種情形。其中情形1表示兩個(gè)路徑片段除了一組端點(diǎn)柵格上下相鄰,還存在其他柵格相鄰;情形2則與之相反。

圖3 滿足合并要求的兩種情形Fig.3 Two situations meeting the mergency requirements

步驟 3對合并后的Cm,調(diào)整其柵格排列順序,確保以該順序生成的移動(dòng)路徑連貫,如圖4所示。

圖4 調(diào)整集合內(nèi)柵格排列順序后所生成的路徑Fig.4 Path generated after adjusting grid arrangement order in the column

步驟 4對調(diào)整后的集合Cm重復(fù)步驟2和步驟3,直至Cm不滿足步驟2中的要求。

步驟 5將Cm從Cpath中移除,并添加至CMer,以防止出現(xiàn)重復(fù)規(guī)劃。

步驟 6重復(fù)步驟2～步驟5,直至Cpath中不存在集合滿足步驟2中的要求。

步驟 7將Cpath中剩余的集合記為CMer2。

則CMer和CMer2中的每個(gè)元素即為初次合并后的單元。這些單元具有以下兩個(gè)特點(diǎn):

(1) 以柵格排列順序所生成的路徑可無重復(fù)地遍歷該單元;

(2) 該路徑的起點(diǎn)為排列在首末位置的兩個(gè)柵格之一。

這樣,單元的生成除了考慮障礙物的形狀和位置,也兼顧了單元內(nèi)的路徑規(guī)劃。圖5(a)所示為路徑片段初步合并后的結(jié)果,圖5(b)為合并形成的單元,共有13個(gè)(圖中黑色圓點(diǎn)代表路徑起點(diǎn),下同)。后續(xù)單元的合并以及局部路徑的優(yōu)化需參照單元間的遍歷順序進(jìn)行。

圖5 單元初步合并生成的結(jié)果Fig.5 Result of preliminary cellular mergence

2 基于貪心算法和拓?fù)涞貓D的單元間遍歷順序求解

本節(jié)求解單元間的遍歷順序,以生成最終路徑。相關(guān)步驟包括:生成拓?fù)涞貓D和執(zhí)行3次基于貪心算法的求解。其中,拓?fù)涞貓D根據(jù)單元間的相鄰情況生成,用于求解過程中單元的再次合并。而在3次求解中,后一次求解均基于前一次求解的結(jié)果,以使得局部路徑不斷得到優(yōu)化,從而減少最終總體路徑的冗余和轉(zhuǎn)向次數(shù)。

2.1 生成拓?fù)涞貓D

依據(jù)各單元的相鄰情況生成拓?fù)涞貓D。圖6即是由圖5所得的拓?fù)涞貓D。

圖6 生成拓?fù)涞貓DFig.6 Generation of topology map

其中,每個(gè)單元均可視為一個(gè)節(jié)點(diǎn),而整個(gè)地圖則可視為一個(gè)樹形結(jié)構(gòu)。為便于進(jìn)一步合并單元,進(jìn)行如下定義,從而將所有節(jié)點(diǎn)分為3類:

(1) 根節(jié)點(diǎn):遍歷起始單元CS,以及相鄰節(jié)點(diǎn)數(shù)之和大于2的節(jié)點(diǎn)(一般為樞紐)。

(2) 通道節(jié)點(diǎn):用于連接兩個(gè)根節(jié)點(diǎn)的節(jié)點(diǎn)。

(3) 分枝節(jié)點(diǎn):除以上節(jié)點(diǎn)的節(jié)點(diǎn)(一般為處于凹形區(qū)域的節(jié)點(diǎn))。

2.2 第一次求解

第一次求解的流程圖如圖7所示。

圖7 第一次求解流程圖Fig.7 Flowchart of the first solution

具體步驟如下:

步驟 2判斷是否存在未遍歷的單元,若不存在,則跳轉(zhuǎn)至步驟11。

步驟 3判斷上一個(gè)遍歷的單元CLast和它的相鄰單元CNear是否滿足優(yōu)先遍歷的兩個(gè)要求:

(1)CLast與CNear均為通道節(jié)點(diǎn);

(2) 單元CNear未遍歷。

若是,則將CNear作為下一個(gè)待遍歷的單元CNext;若否,則選擇最近單元作為CNext(即貪心算法)。

最近單元定義為:以式(7)計(jì)算當(dāng)前排列在集合CNav1末端的柵格,其到某一單元的首端或末端柵格的等效路徑長度即為所有未遍歷單元中的最小值。

執(zhí)行此步驟的目的是,使得處于同一通道的單元優(yōu)先集中完成遍歷,否則遍歷路徑可能發(fā)生割裂,出現(xiàn)如圖8(a)所示的規(guī)劃,導(dǎo)致不必要的轉(zhuǎn)向;而圖8(b)所示則采用了優(yōu)先遍歷的規(guī)劃路徑,其路徑長度和轉(zhuǎn)向次數(shù)得到了降低。

圖8 優(yōu)先遍歷效果示意Fig.8 Schematic diagram of priority traversal effect

步驟 4判斷CNext是否存在已遍歷的相鄰單元。若否,則跳轉(zhuǎn)至步驟5;若是,則跳轉(zhuǎn)至步驟6。

步驟 5以式(7)分別計(jì)算CNav1的當(dāng)前末端柵格,及其到CNext的第一和最后一個(gè)柵格的等效路徑長度,以此確定最佳的單元內(nèi)遍歷路徑起點(diǎn)。調(diào)整CNext的柵格排序,將之添加至CNav1的末端,并將該單元標(biāo)記為已遍歷。之后返回步驟2。

步驟 6與步驟5操作類似,以最優(yōu)方式將CNext中的柵格坐標(biāo)添加至CNav1的末端,并以式(7)計(jì)算此時(shí)由CNav1生成的路徑的等效路徑長度。

步驟 7獲取與CNext相鄰的所有已遍歷的柵格,并尋找這些柵格在CNav1中的排列序號(hào),設(shè)最小序號(hào)為Nmin、最大序號(hào)為Nmax。

步驟 8將CNext內(nèi)所有柵格以正序插入CNav1中的Nmin-1處(如圖9所示,其中箭頭方向代表柵格的遍歷順序),計(jì)算此時(shí)CNav1的等效路徑長度。

圖9 將CNext以正序插入CNav1中的Nmin-1處Fig.9 Inserting CNext into Nmin-1 of CNav1 with positive order

再將CNext內(nèi)所有柵格以倒序插入至CNav1中的Nmin-1處,再次計(jì)算此時(shí)CNav1的等效路徑長度。

步驟 9重復(fù)步驟8,但每次CNext內(nèi)所有柵格的插入位置在上次位置的基礎(chǔ)上右移一位,直至Nmax+1處。

步驟 10尋找步驟8～步驟9中獲得的最短等效路徑長度,將之與步驟6所得的等效路徑長度進(jìn)行比較,選取最小值。將對應(yīng)的添加方式作為CNext的最終添加結(jié)果,并標(biāo)記相應(yīng)的單元為已遍歷,之后返回步驟2。

圖10為第一次求解結(jié)果(圖中黑色三角形代表路徑終點(diǎn),下同)。該路徑的長度為116個(gè)柵格,轉(zhuǎn)向次數(shù)為48。

圖10 第一次求解結(jié)果Fig.10 Results of the first solution

2.3 第二次求解

第一次求解時(shí)未合并分枝和通道節(jié)點(diǎn),導(dǎo)致單元的數(shù)量較多。而未在初始時(shí)就合并的原因在于,該分枝或通道的遍歷起點(diǎn)柵格還未確定。因此,第一次求解完成后,依托于第一次求解的結(jié)果,將連接兩個(gè)根節(jié)點(diǎn)的位于同一通道中的通道節(jié)點(diǎn)進(jìn)行合并。同時(shí),對每個(gè)分枝,從位于其末端的分枝節(jié)點(diǎn)開始,對分枝內(nèi)的每個(gè)節(jié)點(diǎn)進(jìn)行判定:若某個(gè)分枝節(jié)點(diǎn)的父節(jié)點(diǎn)在第一次規(guī)劃時(shí)先于該節(jié)點(diǎn)遍歷,則將該節(jié)點(diǎn)合并至其父節(jié)點(diǎn)。

合并單元時(shí)需調(diào)整單元內(nèi)柵格的排列順序,方法與第一次求解時(shí)的步驟7～步驟9類似,此處不再贅述。

然后,對完成合并的每個(gè)單元進(jìn)行形狀判定:若形狀為矩形,且單元的首尾兩個(gè)柵格位于同一列,則再次調(diào)整該單元內(nèi)柵格的排序,以預(yù)留出撤離路徑,如圖11(b)所示。這樣相對于圖11(a)所示的調(diào)整前的規(guī)劃路徑,單元內(nèi)的冗余路徑實(shí)現(xiàn)了降低。

圖11 柵格排列順序調(diào)整比較Fig.11 Comparison of grid arrangement order adjustment

重復(fù)以上操作,直至所有分枝節(jié)點(diǎn)和通道節(jié)點(diǎn)均得到判定與處理。這樣合并后的總體規(guī)劃路徑更優(yōu)。如圖12(a)所示,合并單元前規(guī)劃的路徑出現(xiàn)了重復(fù),而在圖12(b)中,合并后再規(guī)劃的路徑則得到了改善。

圖12 合并單元前后的規(guī)劃路徑對比Fig.12 Comparison of paths planned before and after cells mergence

圖13為第二次合并后的結(jié)果。相較于圖5,通道節(jié)點(diǎn)5、6、7和分枝節(jié)點(diǎn)3、4、9、10、12、13被合并了,單元總數(shù)為8。之后采用與第一次求解相同的方式獲取第二次求解結(jié)果CNav2,此處不再贅述。圖14所示為第二次求解結(jié)果,其路徑長度為112個(gè)柵格,轉(zhuǎn)向次數(shù)為46。相較于第一次求解,路徑的長度和轉(zhuǎn)向次數(shù)獲得了降低。

圖13 第二次單元合并后的結(jié)果Fig.13 Result after the second cell mergence

圖14 第二次求解結(jié)果Fig.14 Result of the second solution

2.4 第三次求解

在第二次求解時(shí)可能出現(xiàn)以下兩種情形:同一分枝中某一子節(jié)點(diǎn)先于其父節(jié)點(diǎn)遍歷,以及分枝中某一節(jié)點(diǎn)先于該分枝所在根節(jié)點(diǎn)遍歷。為進(jìn)一步減少單元的數(shù)量,將這些節(jié)點(diǎn)合并后再進(jìn)行規(guī)劃。

節(jié)點(diǎn)的合并方式以及第三次求解獲取CNav3的步驟與第二次類似,此處亦不再贅述。

圖15為單元再次合并后的結(jié)果。相較于圖13,分枝節(jié)點(diǎn)11以及由節(jié)點(diǎn)3和節(jié)點(diǎn)4合并而成的節(jié)點(diǎn)均被合并,單元總數(shù)為6。圖16為第三次求解的結(jié)果,其路徑長度為108個(gè)柵格,轉(zhuǎn)向次數(shù)為46。相較于第二次求解,路徑的長度進(jìn)一步減少。

圖15 第三次單元合并結(jié)果Fig.15 Result after the third cellular mergence

圖16 第三次求解結(jié)果Fig.16 Result of the third solution

3 仿真實(shí)驗(yàn)與分析

本節(jié)分“障礙物分布不規(guī)則”和“凹形障礙物較多”兩種場景對本文算法進(jìn)行了規(guī)劃測試。其中,圖17和圖18所示地圖的尺寸均為40×40。地圖中的障礙物由計(jì)算機(jī)+人工隨機(jī)生成,以驗(yàn)證本文算法在較復(fù)雜環(huán)境下的有效性。作為對比的單元分解法,在劃分單元部分,選取了應(yīng)用較多的Boustrophedon分解;在單元間遍歷順序的求解部分,則選取了貪心算法和遺傳算法。同時(shí),在單元分解法之外,選取了常見的生物激勵(lì)神經(jīng)網(wǎng)絡(luò)算法作為對比。

圖17 場景1下不同算法的路徑規(guī)劃結(jié)果Fig.17 Path planning results of different algorithms in Scenario 1

圖18 場景2下不同算法的路徑規(guī)劃結(jié)果Fig.18 Path planning results of different algorithms in Scenario 2

為計(jì)算方便,將路徑的長度用柵格數(shù)表示。同時(shí),假定仿真中機(jī)器人的平均移動(dòng)速度v和一次轉(zhuǎn)向用時(shí)t的乘積v·t=2,即式(7)中P=2,物理意義為機(jī)器人一次轉(zhuǎn)向所花的時(shí)間可以移動(dòng)兩個(gè)柵格的距離。而為了直觀地顯示不同算法規(guī)劃出的路徑的優(yōu)劣,以式(7)計(jì)算各路徑的等效路徑長度,作為判斷的標(biāo)準(zhǔn)。此外,本文算法在每次求解時(shí)的單元數(shù)在表1和表2中依次列出。其中,表1為障礙物分布不規(guī)則的地圖(場景1),表2為凸形障礙物較多的地圖(場景2)。

表1 場景1下不同算法的路徑規(guī)劃結(jié)果比較

表2 場景2下不同算法的路徑規(guī)劃結(jié)果比較

由實(shí)驗(yàn)結(jié)果可以看出,當(dāng)在相對復(fù)雜的地圖中進(jìn)行全覆蓋路徑規(guī)劃時(shí),無論是障礙物分布不規(guī)則的場景,還是凹形障礙物較多的場景,本文算法所生成的單元在規(guī)劃求解的過程中逐步被合并,其數(shù)量不斷減少,且最終的單元數(shù)均少于Boustrophedon分解。同時(shí),本文算法所得遍歷路徑的重復(fù)率約為3種對比算法的50%,表明路徑具有更少的冗余,且轉(zhuǎn)向次數(shù)也更低。此外,等效路徑長度也表明,本文算法規(guī)劃的路徑更優(yōu)。

4 結(jié) 論

在障礙物分布不規(guī)則或凹形障礙物較多的靜態(tài)已知環(huán)境中,傳統(tǒng)的單元分解法存在單元?jiǎng)澐謹(jǐn)?shù)量較多的問題。這使得最終路徑易出現(xiàn)較多冗余和不必要的轉(zhuǎn)向。本文對傳統(tǒng)的單元分解法進(jìn)行了改進(jìn):在劃分單元階段,通過在柵格地圖上合并路徑片段來生成初始單元,使得單元的劃分不再完全根據(jù)障礙物的形狀和位置進(jìn)行,而是兼顧了單元內(nèi)的路徑規(guī)劃;在單元間遍歷順序的求解階段,基于貪心算法和拓?fù)涞貓D三次求解,合并減少了單元數(shù)量,并優(yōu)化了局部路徑。仿真實(shí)驗(yàn)結(jié)果表明,相較于Boustrophedon分解方法和生物激勵(lì)神經(jīng)網(wǎng)絡(luò)算法,本文算法能夠有效減少最終遍歷路徑的冗余和轉(zhuǎn)向次數(shù)。但由于單元的合并和局部路徑的優(yōu)化存在較大的計(jì)算量,故本文僅進(jìn)行了三次求解,且算法的實(shí)時(shí)性不夠,目前僅適用于離線規(guī)劃,這也是后續(xù)研究需改進(jìn)的方向。