基于動態矩陣的未知環境地圖構建與路徑規劃*

張志遠，陳海進

(南通大學信息科學技術學院，江蘇 南通 226019)

1 引言

目前，掃地機器人路徑覆蓋算法有很多，早期的隨機覆蓋式[1]算法容易實現并且對于機器人硬件設備要求低，但在復雜的環境中不一定能實現全覆蓋，路徑重復率相當高。后來研究人員提出了啟發式規劃算法[2，3]和螺旋生成樹覆蓋算法[4-6]，雖然實現了路徑的全覆蓋，但是柵格地圖的構建[7-10]和區域劃分的過程繁瑣，路徑重復率較高。為解決這個問題研究人員又提出了Boustrophedon局部覆蓋算法[11]，該算法通過牛耕式運動將整個地圖動態劃分為若干子區域，每個子區域內部通過牛耕式往返完成簡單覆蓋，再利用深度優先搜索遍歷各個子區域，從而實現全覆蓋。該算法劃分出的子區域數量較多，回溯路徑重復率較高。上述算法都是在地圖信息完全已知或部分已知的前提下(需向機器人輸入地圖的邊界大小)再規劃覆蓋路線的。

本文提出的基于動態矩陣的未知環境地圖構建算法，利用機器人測距傳感器[12]的數值與當前位置坐標的數學關系，建立不斷擴張的二維動態矩陣，借助標記回溯法[13，14]實現全覆蓋地圖探測。完成地圖構建后，本文提出基于封閉區域檢測的路徑規劃算法，在路徑規劃中優先處理檢測到的封閉區域，在封閉區域內采用沿邊循跡與牛耕式運動相結合的子區域路徑規劃方式。通過仿真驗證,本文提出的基于封閉區域檢測的路徑規劃算法與傳統的局部覆蓋算法相比，在減少子區域數目、回溯路徑總長和路徑重復率等指標上有明顯提高。

2 相關算法

2.1 A*算法

A*算法[15-18]是在二維靜態網格中規劃最短路徑的較為有效的算法。算法規定，從起點開始每移動1個柵格都需要對當前位置周圍8個柵格進行計算篩選，依次計算8個柵格相對于當前柵格位置的估計函數值f(s)，其計算如式(1)～式(3)所示:

f(s)=g(s)+h(s)

(1)

(2)

h(s)=|xs-xg|+|ys-yg|

(3)

其中,c為起點柵格，s為當前選中柵格，g為終點柵格，(xc,yc)為起點柵格的坐標，(xs,ys)為當前選中柵格的坐標，(xg,yg)為終點柵格的坐標，g(s)為當前選中柵格到起點柵格的歐氏距離，h(s)為當前選中柵格到終點柵格的曼哈頓距離。通過依次計算，選取8個相鄰柵格中f(s)值最小的柵格作為下一個目標柵格，因此可以通過A*算法規劃出一條死點(四周柵格均為障礙或已清掃點)到回溯點[12]的最短回溯路徑。

2.2 Boustrophedon局部覆蓋

Boustrophedon局部覆蓋是一種基于回溯的覆蓋算法，該算法要求提前給機器人輸入需要清掃的地圖模型邊界條件，用固定的運動模式(比如牛耕式)讓機器人在清掃的過程中劃分區域，將當前地圖模型中已清掃路徑和邊界與未清掃區域構成的封閉區域篩選出來作為劃分的子區域，劃分出子區域后并不立即處理，繼續執行清掃命令，不斷劃分出新的子區域，直至機器人運動至死點后才開始處理剩余的各個子區域，通過深度優先搜索DFS，規劃出一條遍歷所有子區域的回溯路徑，直至每個子區域都被清掃完畢。如圖1所示，斜線填充區域即為機器人通過已清掃路徑分割出的子區域。黑色箭頭實線為機器人清掃遍歷路徑，灰色箭頭實線為機器人往返各個子區域之間的回溯重復路徑。

Figure 1 Boustrophedon local coverage algorithm

通過上述分析不難看出，Boustrophedon局部覆蓋算法容易實現，路徑規劃簡單，但是分割出的子區域較多，分割出子區域但并不優先處理，區域內路徑規劃方式單一，種種問題造成了子區域之間連通往返的重復路徑較多，清掃效率較低。

3 基于動態矩陣的未知環境地圖構建算法

3.1 機器人基本運動機制

本文規定機器人以牛耕式運動軌跡交替往復清掃，設置運動方向優先級為:東>西>北>南。每當清掃過程中遇見障礙物時，機器人根據運動方向優先級，原地旋轉90°轉向下一個優先級方向。

3.2 回溯機制

3.2.1 回溯點列表的建立

機器人按基本運動機制清掃時，很容易進入運動死點。這時需要建立一套完整的回溯[10]機制幫助機器人脫離死點。在Boustrophedon局部覆蓋算法中，機器人的運動回溯點通常設置為未清掃區域的邊角點，而邊角點的位置信息需要在地圖的邊界條件已知的前提下才能獲取。

Figure 2 Schematic diagram of mark backtracking algorithm

本文基于動態矩陣的未知環境地圖構建算法在地圖環境完全未知的前提下采用標記回溯法設置回溯點，規定如下:若前進方向與北方向均無障礙且未被遍歷，根據運動方向優先級(東>西>北>南)，機器人優先向前進方向(東或西)運動探測，所以北方向存在尚未被遍歷探測的空間，需要一個北回溯點來回溯遍歷探測這些柵格空間。如圖2a所示，當前進方向與北方向一格均無障礙且未被遍歷，而東北或西北方向一格存在障礙時，本文選取當前位置北方向一格的點作為標記的北回溯點，依據這個原則選取的回溯點通常為北方向未被探測空間的邊角點，南回溯點選取原則與北回溯點相似，如圖2b所示。這樣，即使未知地圖邊界條件，也可以設置未清掃區域邊角點作為回溯點，從而建立回溯點列表。

3.2.2 回溯點選取原則

標記回溯算法規定回溯點選取優先級:北回溯點>南回溯點。每當機器人按基本運動機制運動至死點時，立即檢測回溯點列表中是否存在北回溯點，若存在，計算各北回溯點與死點之間的歐氏距離，如式(4)和式(5)所示:

i∈{A,B,…,N}

(4)

S=min(SA,SB,…,SN)

(5)

其中，(Xi,Yi)為由點A到點N組成的回溯點列表中的某個待選回溯點，(Xdp,Ydp)為死點。遍歷回溯點列表，選取歐氏距離最小的點回溯,并利用A*算法規劃回溯路線[8-12]。若回溯點列表中不存在北回溯點進而選擇南回溯點回溯。若列表中南、北回溯點個數均為0，則機器人移動至最近未清掃柵格處。

在實際探測過程中，回溯列表中的每個回溯點不可能都被回溯，一旦有回溯點被遍歷，將其自動刪除，釋放內存空間，提高程序運行效率。

如圖3所示為部分探測路徑圖，起點為原點(0,0)，黑色實線為探測路徑軌跡，A點和B點均為機器人運動過程中標記的北回溯點，C點為標記的南回溯點，當機器人第一次進入死點1時:

S=min(SA,SB)=SA

(6)

故選取A點回溯，并利用A*算法規劃回溯路徑，到達A點后從回溯列表中刪除該點。B點在到達A點之后的探測路徑中會被遍歷，故B點被刪除。利用A*算法規劃的回溯路徑如圖3中灰色實線箭頭所示，進入死點2后由于北回溯點列表中已不存在回溯點，故選取南回溯點列表中的節點進行回溯(圖3中的C點)，加粗黑色實線為尋路軌跡。

Figure 3 Path map using mark backtracking algorithm

3.3 動態矩陣地圖構建

主流的基于激光即時定位與地圖構建SLAM(Simultaneous Localization And Mapping)的地圖構建方法在初次獲取地圖信息時并不能滿足掃地機器人全覆蓋清掃的要求，同時需要通過人機交互平臺完成對機器人的移動控制。由于機器人的移動軌跡和位姿的不確定性，因此需要在機器人的工作環境中布置人工信標，以達到定位和精準構圖的目的，雖然構建地圖清晰精準，但對于多變陌生環境，需要投入的人工也隨之上升。本文基于動態矩陣的未知環境地圖構建算法在二維柵格建模的基礎上采用動態矩陣的模型，利用機器人移動軌跡始終為南北垂直的牛耕式路徑這一特點，較容易獲取機器人在當前動態矩陣中的坐標，利用機器人當前位置坐標與傳感器返回動態數據的數學關系完成動態矩陣的擴展，進而實現對陌生環境的地圖構建。本文基于動態矩陣的未知環境地圖構建算法在陌生多變的環境中盡可能降低了人工成本，在構建地圖的同時，實現了初次清掃的全覆蓋。

機器人接收東、西、南、北4個方向傳感器的環境數據，得到機器人與周圍障礙或邊界的4個方向距離，分別記為Ey、Wy、Sx、Nx。以初始矩陣的西北角為坐標原點，設X為生成的二維動態矩陣寬度，Y為生成的二維動態矩陣長度，定義正東方向為Y軸正方向，正南方向為X軸正方向，可以得到:

X=Nx+Sx-1

(7)

Y=Ey+Wy-1

(8)

基于動態矩陣的未知環境地圖構建算法流程如下所示:

(1)矩陣的建立。

以Y和X為長和寬生成二維柵格矩陣，并以機器人直徑為單位進行單元格劃分，記錄機器人位置在當前矩陣中的坐標，機器人每移動1個柵格，坐標相應改變，記機器人動態坐標為(y,x)。

(2)動態矩陣參數記錄。

機器人按基本運動機制運動，同時記錄機器人當前所在行每一個傳感器探測到的Nx和Sx的數值生成line列表，機器人清掃過多少行就生成相應個數的line列表，并把每一個line列表中的Nx和Sx最大值分別記為Nxmax和Sxmax。

(3)動態矩陣的擴展。

紅外、激光雷達和超聲波等傳感器均可作為構建動態矩陣時所用的傳感設備。考慮到不同種類傳感器測距范圍的差異，部分情況下某一方向障礙物或邊界與機器人的距離可能超出傳感器的測距范圍，此時傳感器的返回值為其最大測距值。因此，基于動態矩陣的未知環境地圖構建算法規定，在機器人遇到障礙物或運動至矩陣邊界位置時進行擴展檢測，以應對傳感器測距范圍受限的情形。

①矩陣寬度擴展。

當機器人遇障礙(Ey=0或Wy=0)或運動至當前動態矩陣邊界處(當前位置坐標x=1或x=X)時均需進行擴展檢測，檢測記錄機器人傳感器數據的line列表(假設為第n個),若第n個line列表的Nxmax>x(x為機器人當前縱坐標的值)，說明動態矩陣需在原矩陣的基礎上向北擴展Nxmax-X(X為動態矩陣當前寬度)個單位，動態矩陣寬度X相應改變；若Sxmax>(X-x),說明進入下一行后動態矩陣還需在原矩陣的基礎上向南擴展Sxmax-(X-x)個單位，X值再次相應改變。機器人當前位置坐標也隨矩陣擴展相應改變。

②矩陣長度擴展。

若機器人遇障礙(Ey=0或Wy=0),進入下一行清掃后(假設從第n行進入第n+1行)讀取當前所在位置數據Wyn+1和Eyn+1。若Wyn+1>y,說明進入下一行后動態矩陣需在原矩陣的基礎上向西擴展Wyn+1-y個單位，動態矩陣長度Y相應改變;若當前位置Eyn+1>Y-y,說明進入下一行后動態矩陣需在原矩陣基礎上向東擴展Eyn+1-(Y-y)個單位，Y值再次相應改變，機器人當前位置坐標也隨矩陣擴展相應改變。

若機器人運動至當前動態矩陣邊界處(當前位置坐標y=Y或y=1)，當機器人運動至矩陣東邊界(y=Y)時,若Eyn>0,動態矩陣需在原基礎上擴展Eyn個單位，動態矩陣長度Y相應改變。當機器人運動至矩陣西邊界(y=1)時，若Wyn>0,動態矩陣需在原基礎上擴展Wyn個單位，動態矩陣長度Y相應改變。

(4)回溯。

當機器人按基本運動機制運動至死點位置時，從回溯點列表中選取最近回溯點回溯，返回步驟(1)執行操作命令，若回溯點列表中回溯點個數為0且當前動態矩陣中不存在未被遍歷的柵格，則表示地圖信息已全覆蓋采集，結束循環。基于動態矩陣的未知環境地圖構建算法流程圖如圖4所示。

Figure 4 Flow chart of unknown environment map construction algorithm based on dynamic matrix

Figure 5 Extended schematic diagram of unknown environment map construction algorithm based on dynamic matrix

動態矩陣擴展實例如圖5所示，黑色實心圓點為機器人起始點，在該點處根據傳感器數值建立初始4*4矩陣(如圖5中灰色實線框所示)，機器人按牛耕式軌跡運動至坐標為(1,3)的柵格處，西側遇障礙物(Wy=0)，原地旋轉90°運動至下一行的(1,4)柵格,至黑色空心圓點進行擴展檢測。獲取當前機器人傳感器數據Ey4(第4行),Ey4=12，判定Ey4>(Y-y)，故矩陣向東擴展Ey4-(Y-y)=8個單位，生成12*4矩陣(如圖5中黑色點虛線框所示),同時矩陣長度Y更新為12。機器人運動至(4,12)黑色陰影底紋圓點所在柵格處時，東側遇障礙物(Ey=0)，此時判定第4個line列表的Sxmax>X-x,故矩陣需向南方向擴展Sxmax-(X-x)=5個單位，生成12*9矩陣(如圖5中黑色短橫虛線框所示)，同時矩陣寬度更新為9。

4 路徑規劃

為了解決Boustrophedon局部覆蓋算法子區域間連通產生的重復路徑問題，本文對其進行了優化，優先處理檢測到的封閉區域，并改進了封閉區域內的路徑規劃方式。本文提出的基于封閉區域檢測的路徑規劃算法規定東、西方向清掃優先，按牛耕式運動軌跡遍歷清掃，清掃的同時檢測已經過路徑是否與已知地圖中的障礙物或邊界構成封閉區域，若檢測到區域封閉，設置當前所在位置為起始點，封閉區域底端點為終點，沿區域邊界循跡至區域終點，到達終點后再由該點出發按牛耕式軌跡清掃返回至區域起始點的位置，從而實現對整個封閉區域的全覆蓋清掃。

Figure 6 Path planning algorithm based on closed area detection

圖6是對基于封閉區域檢測的路徑規劃算法的描述。通過上述分析可以看出，不同于Boustrophedon局部覆蓋算法，本文提出的基于封閉區域檢測的路徑規劃算法進行覆蓋時，一旦產生由已知路徑與障礙物或邊界構成的封閉區域便會優先處理該區域，這樣就減少了子區域間連通帶來的區域間往返路徑的重復清掃。傳統子區域內的路徑規劃從子區域起始點出發，按牛耕式遍歷至區域底端，完成區域全覆蓋清掃，之后再回溯至起始點，這就產生了不必要的由底端終點返回至區域起始點的縱向回溯路徑。封閉區域檢測算法采用沿邊循跡與牛耕式運動相結合的路徑規劃方法，讓機器人優先沿區域邊界循跡至區域底端，再由底端出發進行牛耕式遍歷清掃，清掃的同時重新返回至起始點位置，這樣避免了不必要的起終點縱向回溯路徑，減少了回溯次數，縮短了回溯路徑，重復覆蓋率明顯降低，提高了算法的整體性能。

5 算法評估

5.1 路徑規劃算法仿真結果對比

通過前文的對比可知，本文提出的基于封閉區域檢測的路徑規劃算法與Boustrophedon局部覆蓋算法[4,5]都是利用運動軌跡對地圖區域進行分解劃分的，但它們對于分解區域的處理方式以及區域內部路徑規劃方式有所不同。

本節對室內環境模型按照由簡單到復雜的順序，利用Matlab仿真平臺進行對比實驗。圖7是本文基于封閉區域檢測的路徑規劃算法和Boustrophedon局部覆蓋算法對室內環境模型的仿真結果對比。

對應圖7右側圖注所示，斜條紋柵格表示機器人第1次遍歷的區域，灰色柵格表示機器人第2次遍歷的區域，斑馬點紋柵格示機器人第3次遍歷的區域。算法性能的4個評價指標為：子區域個數、回溯路徑總長、初始覆蓋率和路徑重復率，2種算法的仿真結果統計如表1所示。

Figure 7 Simulation results comparison

Table 1 Simulation results analysis of path planning

5.2 基于動態矩陣的未知環境地圖構建算法仿真結果

為了驗證基于動態矩陣的未知環境地圖構建算法的可行性，本節使用Webots平臺進行仿真實驗。在理想三維空間模型中，設置地圖尺寸為400 cm*400 cm的規則矩形，機器人直徑為33 cm，默認其為1個柵格大小。機器人配備4個紅外測距傳感器，測距范圍為0～200 cm，誤差范圍為0～2 cm。機器人運動執行機構為4輪馬達旋轉電機，可通過差速控制各個電機實現原地轉彎的基本功能。

在室內環境中以本文基于動態矩陣的未知環境地圖構建算法構圖和清掃的過程如圖8所示，黑色實線為機器人牛耕式運動軌跡，灰色實線為機器人回溯軌跡。圖中A、B分別為探測過程中標記的回溯點，①、②、③、④標號點為機器人遇障礙物進行地圖擴展檢測的位置，其探測獲得的矩陣分別為：5*7矩陣，11*7矩陣，11*12矩陣，12*12矩陣。機器人以地圖左上角位置為起點，自北向南以牛耕式運動清掃探測，運動至地圖右下角的死點處選取標記的回溯點A進行回溯，到達A點完成子區域的清掃探測后，檢測矩陣中已無回溯點和未清掃點，探測結束，在構建地圖的同時完成全覆蓋清掃。

Figure 8 Simulation diagram of unknown environment map construction algorithm based on dynamic matrix in three-dimensional model

6 結束語

本文基于動態矩陣的未知環境地圖構建算法可以適應復雜陌生環境，有效提高機器人的自主性，降低人工代價。本文提出的基于封閉區域檢測的路徑規劃算法，優先處理由已清掃路徑、障礙物和邊界構成的封閉區域，一定程度上避免了子區域間的回溯路徑，故而不存在3次重復清掃的柵格。算法在封閉區域內采用沿邊循跡和牛耕式運動相結合的路徑規劃方法，減少了子區域內縱向的回溯路徑，有效降低了重復率。由實驗數據可以看出，室內環境越復雜，本文提出的基于封閉區域檢測的路徑規劃算法相對于Boustrophedon算法的優勢就越明顯，應變性能越強。