吳靖宇, 朱世強, 宋 偉,*, 施浩磊, 吳澤南

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

0 引 言

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

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

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

1 單元初步合并生成

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

1.1 柵格地圖初始化

首先,建立平面直角坐標(biāo)系,其原點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個路徑片段,則它們組成的集合Cpath={C1,C2,…,Cn}滿足:

(4)

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

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

1.3 確定遍歷起始單元

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

(5)

若在該時間內(nèi)機器人始終以平均速度v做直線運動,則可移動距離為

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

(6)

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

L′=L+PM

(7)

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

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

1.4 合并路徑片段

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

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

具體步驟如下:

步驟 1生成空集合CMer,對合并完成的單元進行存儲。

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

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

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

(8)

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

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

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

步驟 3對合并后的Cm,調(diào)整其柵格排列順序,確保以該順序生成的移動路徑連貫,如圖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中的每個元素即為初次合并后的單元。這些單元具有以下兩個特點:

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

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

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

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

2 基于貪心算法和拓撲地圖的單元間遍歷順序求解

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

2.1 生成拓撲地圖

依據(jù)各單元的相鄰情況生成拓撲地圖。圖6即是由圖5所得的拓撲地圖。

圖6 生成拓撲地圖Fig.6 Generation of topology map

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

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

(2) 通道節(jié)點:用于連接兩個根節(jié)點的節(jié)點。

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

2.2 第一次求解

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

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

具體步驟如下:

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

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

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

(2) 單元CNear未遍歷。

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

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

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

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

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

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

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

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

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

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

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

2.3 第二次求解

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

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

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

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

重復(fù)以上操作,直至所有分枝節(jié)點和通道節(jié)點均得到判定與處理。這樣合并后的總體規(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é)點5、6、7和分枝節(jié)點3、4、9、10、12、13被合并了,單元總數(shù)為8。之后采用與第一次求解相同的方式獲取第二次求解結(jié)果CNav2,此處不再贅述。圖14所示為第二次求解結(jié)果,其路徑長度為112個柵格,轉(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 第三次求解

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

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

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

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

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

3 仿真實驗與分析

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

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

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

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

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

4 結(jié) 論

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