基于改進單元分解法的全覆蓋路徑規劃

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

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

0 引 言

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

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

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

1 單元初步合并生成

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

1.1 柵格地圖初始化

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

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

(1)

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

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

(2)

1.2 生成路徑片段

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

(3)

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

(4)

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

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

1.3 確定遍歷起始單元

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

(5)

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

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

(6)

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

L′=L+PM

(7)

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

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

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

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

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

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

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

步驟 4對調整后的集合Cm重復步驟2和步驟3,直至Cm不滿足步驟2中的要求。

步驟 5將Cm從Cpath中移除,并添加至CMer,以防止出現重復規劃。

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

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

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

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

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

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

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

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

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

2.1 生成拓撲地圖

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

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

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

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

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

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

2.2 第一次求解

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

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

具體步驟如下:

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

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

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

(2) 單元CNear未遍歷。

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

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

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

圖8 優先遍歷效果示意Fig.8 Schematic diagram of priority traversal effect

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

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

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

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

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

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

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

步驟 9重復步驟8,但每次CNext內所有柵格的插入位置在上次位置的基礎上右移一位,直至Nmax+1處。

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

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

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

2.3 第二次求解

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

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

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

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

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

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

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

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

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

2.4 第三次求解

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

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

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

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

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

3 仿真實驗與分析

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

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

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

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

表1 場景1下不同算法的路徑規劃結果比較

表2 場景2下不同算法的路徑規劃結果比較

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

4 結 論

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