基于動態(tài)矩陣的未知環(huán)境地圖構(gòu)建與路徑規(guī)劃*

張志遠(yuǎn)，陳海進(jìn)

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

1 引言

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

本文提出的基于動態(tài)矩陣的未知環(huán)境地圖構(gòu)建算法，利用機(jī)器人測距傳感器[12]的數(shù)值與當(dāng)前位置坐標(biāo)的數(shù)學(xué)關(guān)系，建立不斷擴(kuò)張的二維動態(tài)矩陣，借助標(biāo)記回溯法[13，14]實(shí)現(xiàn)全覆蓋地圖探測。完成地圖構(gòu)建后，本文提出基于封閉區(qū)域檢測的路徑規(guī)劃算法，在路徑規(guī)劃中優(yōu)先處理檢測到的封閉區(qū)域，在封閉區(qū)域內(nèi)采用沿邊循跡與牛耕式運(yùn)動相結(jié)合的子區(qū)域路徑規(guī)劃方式。通過仿真驗(yàn)證,本文提出的基于封閉區(qū)域檢測的路徑規(guī)劃算法與傳統(tǒng)的局部覆蓋算法相比，在減少子區(qū)域數(shù)目、回溯路徑總長和路徑重復(fù)率等指標(biāo)上有明顯提高。

2 相關(guān)算法

2.1 A*算法

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

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

(1)

(2)

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

(3)

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

2.2 Boustrophedon局部覆蓋

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

Figure 1 Boustrophedon local coverage algorithm

通過上述分析不難看出，Boustrophedon局部覆蓋算法容易實(shí)現(xiàn)，路徑規(guī)劃簡單，但是分割出的子區(qū)域較多，分割出子區(qū)域但并不優(yōu)先處理，區(qū)域內(nèi)路徑規(guī)劃方式單一，種種問題造成了子區(qū)域之間連通往返的重復(fù)路徑較多，清掃效率較低。

3 基于動態(tài)矩陣的未知環(huán)境地圖構(gòu)建算法

3.1 機(jī)器人基本運(yùn)動機(jī)制

本文規(guī)定機(jī)器人以牛耕式運(yùn)動軌跡交替往復(fù)清掃，設(shè)置運(yùn)動方向優(yōu)先級為:東>西>北>南。每當(dāng)清掃過程中遇見障礙物時(shí)，機(jī)器人根據(jù)運(yùn)動方向優(yōu)先級，原地旋轉(zhuǎn)90°轉(zhuǎn)向下一個(gè)優(yōu)先級方向。

3.2 回溯機(jī)制

3.2.1 回溯點(diǎn)列表的建立

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

Figure 2 Schematic diagram of mark backtracking algorithm

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

3.2.2 回溯點(diǎn)選取原則

標(biāo)記回溯算法規(guī)定回溯點(diǎn)選取優(yōu)先級:北回溯點(diǎn)>南回溯點(diǎn)。每當(dāng)機(jī)器人按基本運(yùn)動機(jī)制運(yùn)動至死點(diǎn)時(shí)，立即檢測回溯點(diǎn)列表中是否存在北回溯點(diǎn)，若存在，計(jì)算各北回溯點(diǎn)與死點(diǎn)之間的歐氏距離，如式(4)和式(5)所示:

i∈{A,B,…,N}

(4)

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

(5)

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

在實(shí)際探測過程中，回溯列表中的每個(gè)回溯點(diǎn)不可能都被回溯，一旦有回溯點(diǎn)被遍歷，將其自動刪除，釋放內(nèi)存空間，提高程序運(yùn)行效率。

如圖3所示為部分探測路徑圖，起點(diǎn)為原點(diǎn)(0,0)，黑色實(shí)線為探測路徑軌跡，A點(diǎn)和B點(diǎn)均為機(jī)器人運(yùn)動過程中標(biāo)記的北回溯點(diǎn)，C點(diǎn)為標(biāo)記的南回溯點(diǎn)，當(dāng)機(jī)器人第一次進(jìn)入死點(diǎn)1時(shí):

S=min(SA,SB)=SA

(6)

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

Figure 3 Path map using mark backtracking algorithm

3.3 動態(tài)矩陣地圖構(gòu)建

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

機(jī)器人接收東、西、南、北4個(gè)方向傳感器的環(huán)境數(shù)據(jù)，得到機(jī)器人與周圍障礙或邊界的4個(gè)方向距離，分別記為Ey、Wy、Sx、Nx。以初始矩陣的西北角為坐標(biāo)原點(diǎn)，設(shè)X為生成的二維動態(tài)矩陣寬度，Y為生成的二維動態(tài)矩陣長度，定義正東方向?yàn)閅軸正方向，正南方向?yàn)閄軸正方向，可以得到:

X=Nx+Sx-1

(7)

Y=Ey+Wy-1

(8)

基于動態(tài)矩陣的未知環(huán)境地圖構(gòu)建算法流程如下所示:

(1)矩陣的建立。

以Y和X為長和寬生成二維柵格矩陣，并以機(jī)器人直徑為單位進(jìn)行單元格劃分，記錄機(jī)器人位置在當(dāng)前矩陣中的坐標(biāo)，機(jī)器人每移動1個(gè)柵格，坐標(biāo)相應(yīng)改變，記機(jī)器人動態(tài)坐標(biāo)為(y,x)。

(2)動態(tài)矩陣參數(shù)記錄。

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

(3)動態(tài)矩陣的擴(kuò)展。

紅外、激光雷達(dá)和超聲波等傳感器均可作為構(gòu)建動態(tài)矩陣時(shí)所用的傳感設(shè)備?？紤]到不同種類傳感器測距范圍的差異，部分情況下某一方向障礙物或邊界與機(jī)器人的距離可能超出傳感器的測距范圍，此時(shí)傳感器的返回值為其最大測距值。因此，基于動態(tài)矩陣的未知環(huán)境地圖構(gòu)建算法規(guī)定，在機(jī)器人遇到障礙物或運(yùn)動至矩陣邊界位置時(shí)進(jìn)行擴(kuò)展檢測，以應(yīng)對傳感器測距范圍受限的情形。

①矩陣寬度擴(kuò)展。

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

②矩陣長度擴(kuò)展。

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

若機(jī)器人運(yùn)動至當(dāng)前動態(tài)矩陣邊界處(當(dāng)前位置坐標(biāo)y=Y或y=1)，當(dāng)機(jī)器人運(yùn)動至矩陣東邊界(y=Y)時(shí),若Eyn>0,動態(tài)矩陣需在原基礎(chǔ)上擴(kuò)展Eyn個(gè)單位，動態(tài)矩陣長度Y相應(yīng)改變。當(dāng)機(jī)器人運(yùn)動至矩陣西邊界(y=1)時(shí)，若Wyn>0,動態(tài)矩陣需在原基礎(chǔ)上擴(kuò)展Wyn個(gè)單位，動態(tài)矩陣長度Y相應(yīng)改變。

(4)回溯。

當(dāng)機(jī)器人按基本運(yùn)動機(jī)制運(yùn)動至死點(diǎn)位置時(shí)，從回溯點(diǎn)列表中選取最近回溯點(diǎn)回溯，返回步驟(1)執(zhí)行操作命令，若回溯點(diǎn)列表中回溯點(diǎn)個(gè)數(shù)為0且當(dāng)前動態(tài)矩陣中不存在未被遍歷的柵格，則表示地圖信息已全覆蓋采集，結(jié)束循環(huán)?；趧討B(tài)矩陣的未知環(huán)境地圖構(gòu)建算法流程圖如圖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

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

4 路徑規(guī)劃

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

Figure 6 Path planning algorithm based on closed area detection

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

5 算法評估

5.1 路徑規(guī)劃算法仿真結(jié)果對比

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

本節(jié)對室內(nèi)環(huán)境模型按照由簡單到復(fù)雜的順序，利用Matlab仿真平臺進(jìn)行對比實(shí)驗(yàn)。圖7是本文基于封閉區(qū)域檢測的路徑規(guī)劃算法和Boustrophedon局部覆蓋算法對室內(nèi)環(huán)境模型的仿真結(jié)果對比。

對應(yīng)圖7右側(cè)圖注所示，斜條紋柵格表示機(jī)器人第1次遍歷的區(qū)域，灰色柵格表示機(jī)器人第2次遍歷的區(qū)域，斑馬點(diǎn)紋柵格示機(jī)器人第3次遍歷的區(qū)域。算法性能的4個(gè)評價(jià)指標(biāo)為：子區(qū)域個(gè)數(shù)、回溯路徑總長、初始覆蓋率和路徑重復(fù)率，2種算法的仿真結(jié)果統(tǒng)計(jì)如表1所示。

Figure 7 Simulation results comparison

Table 1 Simulation results analysis of path planning

5.2 基于動態(tài)矩陣的未知環(huán)境地圖構(gòu)建算法仿真結(jié)果

為了驗(yàn)證基于動態(tài)矩陣的未知環(huán)境地圖構(gòu)建算法的可行性，本節(jié)使用Webots平臺進(jìn)行仿真實(shí)驗(yàn)。在理想三維空間模型中，設(shè)置地圖尺寸為400 cm*400 cm的規(guī)則矩形，機(jī)器人直徑為33 cm，默認(rèn)其為1個(gè)柵格大小。機(jī)器人配備4個(gè)紅外測距傳感器，測距范圍為0～200 cm，誤差范圍為0～2 cm。機(jī)器人運(yùn)動執(zhí)行機(jī)構(gòu)為4輪馬達(dá)旋轉(zhuǎn)電機(jī)，可通過差速控制各個(gè)電機(jī)實(shí)現(xiàn)原地轉(zhuǎn)彎的基本功能。

在室內(nèi)環(huán)境中以本文基于動態(tài)矩陣的未知環(huán)境地圖構(gòu)建算法構(gòu)圖和清掃的過程如圖8所示，黑色實(shí)線為機(jī)器人牛耕式運(yùn)動軌跡，灰色實(shí)線為機(jī)器人回溯軌跡。圖中A、B分別為探測過程中標(biāo)記的回溯點(diǎn)，①、②、③、④標(biāo)號點(diǎn)為機(jī)器人遇障礙物進(jìn)行地圖擴(kuò)展檢測的位置，其探測獲得的矩陣分別為：5*7矩陣，11*7矩陣，11*12矩陣，12*12矩陣。機(jī)器人以地圖左上角位置為起點(diǎn)，自北向南以牛耕式運(yùn)動清掃探測，運(yùn)動至地圖右下角的死點(diǎn)處選取標(biāo)記的回溯點(diǎn)A進(jìn)行回溯，到達(dá)A點(diǎn)完成子區(qū)域的清掃探測后，檢測矩陣中已無回溯點(diǎn)和未清掃點(diǎn)，探測結(jié)束，在構(gòu)建地圖的同時(shí)完成全覆蓋清掃。

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

6 結(jié)束語

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