室內(nèi)環(huán)境下基于邊際約束的快速路徑自主探索算法

徐曉蘇，梁紫依，楊 博，王 迪

（1.微慣性儀表與先進導航技術教育部重點實驗室，南京 210096； 2.東南大學 儀器科學與工程學院，南京 210096）

目前移動機器人已經(jīng)在工業(yè)、農(nóng)業(yè)和軍事等各領域取得了廣泛的應用，減輕或者取代了人類繁重的勞動。從搜索和救援到行星探測和監(jiān)視，自主探索技術也發(fā)揮著越來越重要的作用[1]。移動機器人與環(huán)境交互，根據(jù)傳感器數(shù)據(jù)構建完整可靠的環(huán)境地圖即移動機器人自主探索[2]。大規(guī)模環(huán)境下，人為引導進行環(huán)境探索，工作量繁瑣且巨大；在地震、火災等危險環(huán)境中，人類無法直接進入，只能依賴機器人攜帶的傳感器實現(xiàn)對環(huán)境的自主認知和建模。因此，移動機器人對未知環(huán)境的自主探索就變得尤為重要。如何在短時間和路程完成移動機器人所在環(huán)境的遍歷，從而得到足夠空間環(huán)境用于全局地圖創(chuàng)建，是自主探索的核心問題。自主探索是機器人實現(xiàn)真正自主的關鍵性一步，也是機器人技術的一個重要研究方向[3]。

移動機器人自主探索算法的研究一直受到國內(nèi)外眾多學者的關注。在自主探索的過程中，機器人需感知環(huán)境來獲得一系列的局部目標點，指導機器人進行環(huán)境的全局遍歷。1997年，Yamauchi[4]提出了前沿探測（frontier exploration）理論，將已知區(qū)域與未知區(qū)域的交界定義為前沿，探索過程中使機器人盡可能駛向未知區(qū)域，但是該算法忽略了其他環(huán)境因素的影響，不能保證每一步都能獲得新息。2015年，P.G.C.N.Senarathne 和Danwei Wang[5]提出了一種計算高效的增量式邊界檢測方法安全可達邊界檢測，降低了邊界生成算法的計算量，同時提高了執(zhí)行效率。2016年，坦佩雷理工大學 Lauri[6]等人提出了基于地圖信息采樣的正演探索算法，在以激光測距儀（Laser Range Finder,LRF）觀測環(huán)境信息構建的地圖的基礎上，根據(jù)移動機器人運動速度、觀測范圍等約束，將探索過程描述成部分可觀測馬爾可夫決策過程（Partially Observable Markov Decision Process,POMDP），通過對局部環(huán)境信息進行采樣，從而推演當前目標點，并結合基于邊界的啟發(fā)式探索策略，有效規(guī)避了局部最優(yōu)問題。2017年，南開大學的于寧波[7]等人提出了一種基于RGB-D 信息的移動機器人自主探索與地圖構建方法，將探索過程描述成部分可觀測馬爾可夫決策過程，結合局部地圖推演策略與全局邊界搜索策略,建立了移動機器人的自主探索方法，但是該算法對于全局的路徑規(guī)劃仍沒有具體的說明。

自主探索策略幾乎可以簡化為尋找和移動到下一步最佳探索目標點[8]。因此，路徑規(guī)劃是機器人技術的重要研究方向，也是實現(xiàn)移動機器人在探索目標點之間移動的關鍵技術。傳統(tǒng)的路徑規(guī)劃算法有人工勢場法、A*算法、D*算法等。近年來，越來越多的現(xiàn)代智能算法被提出，例如模糊邏輯算法、神經(jīng)網(wǎng)絡算法、群智能啟發(fā)式優(yōu)化算法[9]等。由于快速擴展隨機樹算法具有隨機性和高維度適用的特點，基于該算法的各種改進方法逐漸應用到高維復雜環(huán)境的路徑規(guī)劃中[10-12]。

綜上，上述自主探索方法都側重于探索目標點的確定，解決了機器人探索目標點的確定問題，但是忽略了具體的路徑規(guī)劃部分，或者耗費高成本尋求最優(yōu)的路徑。本文針對上述問題，從機器人移動范圍和運行時間考慮，提出一種基于邊際條件約束的啟發(fā)式路徑自主探索算法。首先將機器人探索問題描述為POMDP 過程，然后結合邊際約束條件的快速擴展隨機樹（Rapidly-exploration Random Trees,RRT）算法尋找路徑，再采用啟發(fā)式算法優(yōu)化路徑，進而提出一種基于邊際條件約束的啟發(fā)式快速路徑自主探索算法。

1 移動機器人自主探索問題建模

基于移動機器人當前位姿與構建的地圖信息進行推演，衡量探索過程中的信息量，確定探索目標點，是探索算法的研究重點。由于實際環(huán)境通常是部分可觀測和時變的，移動機器人通過激光測距儀、深度相機等傳感器觀測到的環(huán)境具有不確定性，因此機器人在未知環(huán)境中進行自主探索需要對這些因素進行考慮。根據(jù)傳感器觀測范圍和機器人移動速度、動力學等條件，在傳感器構建的局部地圖上，我們可以把移動機器人自主探索問題描述為POMDP 過程，并用 〈τ,S,U,Z,Ts,O,R〉進行描述，其中：

●τ= ｛0 ,1,… ,k,…｝ 表示一次探索任務確定目標點經(jīng)歷的決策階段。

●S=X×M表示機器人的隱藏狀態(tài)，包括地圖狀態(tài)M和機器人的內(nèi)部狀態(tài)X。在實際環(huán)境中，M和X都是可觀測的。

●U表示機器人的動作集合，即按照動力學、運動學等約束，機器人可以執(zhí)行的動作。

●Z表示狀態(tài)空間的觀測集合。

●TS表示狀態(tài)轉移模型，由機器人狀態(tài)轉移TX和地圖狀態(tài)更新TM共同組成。

●O表示觀測模型，采用光線追蹤模型對柵格占據(jù)地圖進行采樣，得到當前狀態(tài)的觀測值。

●R表示獎勵函數(shù)，由狀態(tài)信息S，觀測信息Z和動作集U共同構成。

獎勵函數(shù)R是評價路徑好壞的準則，本文采用互信息[8]作為對路徑信息增益的度量。表達式如下：

其中，b表示當前的置信狀態(tài)，u表示當前的動作序列，x′、m′分別表示從機器人內(nèi)部狀態(tài)和地圖狀態(tài)采集的樣本。

若假設機器人內(nèi)部狀態(tài)和地圖狀態(tài)的變化過程是 獨立的，并且其中N是 采樣數(shù)，則可將式(1)近似為：

根據(jù)互信息的鏈式規(guī)則證明，當N→∞，其收斂于I(X,M;Z|B,U)。

式(2)中，第一部分為移動機器人位姿的期望信息增益。考慮室內(nèi)環(huán)境的地形平整性，機器人能平穩(wěn)地進行移動。因此，可以假設機器人的運動模型不存在噪聲，表述為

式(4)所示為機器人速度運動模型的具體形式，其中以機器人位姿狀態(tài)與觀測值表示的互相關函數(shù)值為0。

第二部分表示的是地圖狀態(tài)的期望信息增益，為地圖和觀測值的互相關函數(shù)值，通過對當前地圖推演前后指定范圍內(nèi)柵格的熵值求和實現(xiàn)積分運算。在2D的柵格占據(jù)地圖中，每個網(wǎng)格具有兩個隱藏的狀態(tài)：占據(jù)和空閑。柵格的具體狀態(tài)具有不確定性，用p表示柵格的占據(jù)概率，則p∈[ 0,1]，其中1 表示占據(jù)點即存在障礙物，0 表示空閑點[13]。采用二元信息熵的形式來表示每個柵格的信息量：

2 基于邊際條件約束的啟發(fā)式自主探索算法

RRT（Rapidly-exploration random tree）算法最早在1998年的美國愛荷華州立大學被提出。該方法通過隨機搜索自由空間，逐漸構建一棵從初始狀態(tài)到目標狀態(tài)的可達路徑的隨機樹。在迭代過程中，隨機生成采樣點并搜索距離該點最近的樹的頂點，然后以確定步長生長，得到該方向上的新節(jié)點，并檢查其是否屬于安全環(huán)境，直到到達目標點[14]。RRT 算法是概率完備的，且在搜索全局性、隨機性等方面具有優(yōu)勢[15]。

借助RRT 算法的快速性，參考文獻[15]的動態(tài)窗口形式，本文提出一種基于邊際條件約束的啟發(fā)式自主探索算法。一方面限制隨機樹采樣點的生成范圍，弱化隨機性；另一方面，將隨機樹的節(jié)點類比二維坐標系的原點，平行于地圖的長和寬方向，構造四個象限，根據(jù)節(jié)點和目標點的象限方位，確定隨機點采樣的象限。同時，采用啟發(fā)式評估函數(shù)[16]對移動路徑進行優(yōu)化，避開障礙物的同時，盡可能地減少機器人的旋轉動作和縮短移動路徑的長度。

在室內(nèi)環(huán)境中，每一次探索目標點的確定和傳感器的選取及其觀測范圍、機器人的速度運動模型等有關系。現(xiàn)有的自主探索大多是通過激光雷達等距離傳感器對環(huán)境進行感知，獲取障礙物的位置。因此探索給出的目標點一定是在同一個空間，即起點和終點之間的邊界內(nèi)始終存在可連通的區(qū)域。

基于該情況，基于邊際條件約束的啟發(fā)式自主探索算法改進步驟如下：

首先，基于傳感器觀測到的已知地圖，由于起點q_start和終點q_goal位于圖中，并且在大多數(shù)情況下，起點q_start和終點q_goal圍成的邊際窗口遠小于實際探索的全局地圖，因此將采樣的范圍限制在該邊際窗口中。

其次，將隨機樹上距離終點q_goal最近鄰節(jié)點q_gnear設置為起點q_start。同時，保持邊際窗口的大小不變，將其移動至新起點處。

然后，根據(jù)二維坐標系的四個象限的概念，以起點q_start為原點，平行于初始地圖的長和寬為相互垂直的兩坐標軸。根據(jù)終點q_goal的方位，確定邊際窗口的相應象限。

接著，在邊際窗口中進行概率采樣。給每一個采樣點賦予隨機的概率值p_rand，設置一個概率閾值p_thre。當大于閾值時，在邊框窗口中產(chǎn)生采樣點，否則以終點q_goal為采樣點。

最后，當找到起點q_start和終點q_goal的可達路徑后，采用啟發(fā)式評估函數(shù)對路徑進行優(yōu)化[16]，尋找起點q_start到第一個無碰撞連接的路徑節(jié)點。循環(huán)如此，直至終點q_goal。

圖1是構造邊際窗口的示意圖。其中，黑色的五角星為起點，紅色的五角星為終點，黑色細實線即為初始設置的采樣邊際窗口。

圖1 邊際窗口示意圖 Fig.1 Marginal window diagram

若q_start= [xs,ys]，q_goal= [x g,yg]，則邊際窗口的四條邊應滿足以下條件：

圖2中的紅色三角形為隨機采樣點，黃色線條是隨機樹節(jié)點的連接線。在圖2中，由于起點和終點圍成的邊際窗口在起點的第四象限，因此在起點的第四象限的邊際窗口中根據(jù)隨機賦予的概率值進行采樣。圖2為一隨機采樣結果。根據(jù)隨機樹節(jié)點位置，重新設置紅色三角形為起點，終點仍然在起點的第四象限，因此將邊際窗口移動，但保持窗口大小不變。采樣點和終點的象限關系可以根據(jù)式(7)進行判斷。

圖2 節(jié)點象限示意圖 Fig.2 Node quadrant diagram

在基于邊際條件約束的啟發(fā)式自主探索算法中，若在邊際窗口的移動過程中，碰到障礙物或地圖邊界時，用該邊界限制邊際窗口。在基于該算法使用受到限制時，放松邊際條件的約束，在全局空間進行采樣。這樣不僅一定程度限制了采樣空間的大小，減少采樣點的數(shù)量。而且隨著隨機樹T的生長，邊際窗口進行窗口式的滑動，增加了路徑生成的可能性，縮短了程序運行的時間，提高了算法的效率。

最后采用啟發(fā)式評估函數(shù)尋求最優(yōu)的路徑節(jié)點。將算法找到的路徑節(jié)點記為

其中，h為啟發(fā)式評估函數(shù)。

基于邊際條件約束的啟發(fā)式自主探索算法的具體操作步驟如下：

步驟1：初始化起點q_start和終點q_goal，并將起始節(jié)點q_start作為快速擴展隨機樹T的根節(jié)點；

步驟2：判斷起點q_start和終點q_goal是否相同，且起點q_start和終點q_goal的連接線是否不和障礙物發(fā)生碰撞，若是則轉入步驟8；

步驟3：根據(jù)起點q_start和終點q_goal構成的邊際窗口重新設置采樣空間。初始時，最近鄰節(jié)點q_gnear為起點q_start；

步驟4：根據(jù)節(jié)點q_gnear和終點q_goal的方位關系，即確定終點q_goal位于最近鄰節(jié)點q_gnear的哪一個象限。在該象限中以邊際窗口為范圍生成隨機的采樣點q_rand。

步驟 5：給每一采樣點隨機賦予一個概率值p_rand，當p_rand小于設定的概率閾值p_thre時，設置采樣點q_rand為終點q_goal，否則為隨機采樣點q_rand；

步驟6：根據(jù)設定的步長q_step，沿著節(jié)點q_near和采樣點q_rand的連線方向生成一個新節(jié)點q_new，若新節(jié)點q_new和節(jié)點q_near的路徑不與障礙物碰撞，則新節(jié)點q_new添加到RRT 樹T中，并將其設置為q_near，否則舍棄，重新進入步驟4；

步驟7：判斷新節(jié)點q_new是否為終點q_goal，或者終點q_goal是否在RRT 樹T的新舊節(jié)點連接線上。如果條件成立，則將最后一個樹節(jié)點設置為終點q_goal，否則重新進入步驟3，直至找到終點q_goal；

步驟8：返回起點q_start和終點q_goal的路徑，并記錄為path；

步驟9：找到路徑初始點p_start和path上不發(fā)生碰撞的最后一個節(jié)點p_free。然后重新設置其后的一個節(jié)點為起始點p_start，直至終點q_goal；

步驟10：返回最終起點q_start和終點q_goal優(yōu)化后的路徑；

步驟11：程序結束。

具體的算法流程圖如圖3所示。

圖3 基于邊際條件約束的啟發(fā)式自主探索算法流程圖 Fig.3 Flow chart of heuristic autonomous exploration algorithm based on marginal condition constraint

3 仿真實驗

本文的仿真實驗分為兩個部分:POMDP 探索算法仿真實驗和本文提出的基于邊際條件約束的啟發(fā)式自主探索算法的驗證。

3.1 POMDP 探索算法仿真實驗

在以LRF 觀測環(huán)境信息構建的地圖基礎上，根據(jù)移動機器人運動速度、觀測范圍等約束進行預測，衡量探索過程中的信息量進而生成目標點是POMDP 探索模型的主要思想。具體的推演過程包括粒子采樣、權重計算與重采樣。

初始時，根據(jù)機器人底盤轉速范圍進行均勻采樣， 生成可能的粒子動作序列，并為每個粒子分配權重。當粒子多樣性小于設定值時，進行重采樣。然后依據(jù)粒子具體數(shù)目和迭代的次數(shù)進行濾波，算法逐漸收斂于最優(yōu)粒子的動作序列。

圖4是在Ubuntu16.04 下機器人操作系統(tǒng)（Robot Operating System,ROS）中運行的實驗結果圖。圖中黑色線條是已知區(qū)域和未知區(qū)域的邊界，黑色的圓是移動機器人模型，灰色區(qū)域是已知區(qū)域，深灰色區(qū)域是未知區(qū)域。綠色線條即是移動機器人可能的粒子動作序列，經(jīng)過多次迭代計算后，逐漸收斂于子圖(c)中的最優(yōu)粒子的動作序列。

圖4 POMDP 探索算法仿真實驗 Fig.4 Simulation experiment of POMDP exploration algorithm

3.2 基于邊際條件約束的啟發(fā)式自主探索算法

本算法的仿真實驗是在 Windows 10 下的MATLAB-R2015b 上進行。圖5是RRT 算法的結果圖。

圖5 RRT 算法 Fig.5 Rapidly-exploring Random Trees algorithm

在圖5的(a)(b)(c)(d)四幅子圖中，黃色線條表示RRT 算法產(chǎn)生的路徑，黑色線條表示經(jīng)過啟發(fā)式優(yōu)化算法優(yōu)化之后的路徑。從圖中可以看出，由于路徑是在全局空間基于隨機采樣生成的，所以隨機樹的節(jié)點數(shù)量存在明顯的差異。復雜情況下，樹節(jié)點數(shù)量幾乎遍布全局，如圖5(a)所示；簡單情況下，樹節(jié)點數(shù)量較為適量，如圖5的(b)(c)(d)子圖。圖5的(a)(b)(c)(d)子圖是傳統(tǒng)RRT 算法隨機產(chǎn)生的四個結果圖，在大量的實驗中，結果相差更為明顯。

將本文提出的算法在相同的環(huán)境中進行驗證。

圖6 基于邊際條件約束的啟發(fā)式自主探索算法 Fig.6 Heuristic autonomous exploration algorithm based on marginal constraint

在圖6中，黃色線條表示基于邊際條件約束的啟 發(fā)式自主探索算法產(chǎn)生的路徑，黑色線條表示經(jīng)過啟發(fā)式優(yōu)化算法優(yōu)化的路徑。與圖5相比，圖6的樹節(jié)點數(shù)量明顯減少。圖6的(a)(b)(c)(d)子圖的節(jié)點數(shù)量分別為22、19、26、20，比圖5中最少樹節(jié)點子圖(c)還少6 個節(jié)點。對比8 個子圖規(guī)劃的路徑可以明顯看出，經(jīng)過啟發(fā)式算法優(yōu)化之后，機器人的移動路徑節(jié)點減少，即轉角變少，路徑移動距離縮短。

在圖5和圖6的仿真環(huán)境中，設置起點、終點和障礙物等環(huán)境條件相同，分別運行傳統(tǒng)RRT 算法和本文算法，將運行時間結果列于表1中。

表1 RRT 算法和本文算法的時間對比表 Tab.1 Time comparison of RRT algorithm and the proposed algorithm

根據(jù)表1的數(shù)據(jù)可知，啟發(fā)式優(yōu)化算法的優(yōu)化時間很短，對于本文算法和RRT 算法的運行時間幾乎不會產(chǎn)生影響。在子圖6(c)中，雖然本文算法運行時間稍長于RRT 算法，但和圖6的(a)(b)(d)子圖相比，本文算法仍具有時間優(yōu)勢。從4 幅子圖的運行時間分析，本文算法的時間穩(wěn)定性優(yōu)于RRT 算法。

為了驗證基于邊際條件約束的啟發(fā)式自主探索算法的實用性，從不同的環(huán)境進行驗證。

圖7 不同環(huán)境的基于邊際條件約束的啟發(fā)式自主探索算法 Fig.7 Heuristic autonomous exploration algorithm based on marginal condition constraint in different environments

從圖7的結果中可以得出，不管是對于規(guī)則還是不 規(guī)則的障礙物，改變起點或者終點的位置都不影響本文算法的效果。圖7中隨機樹節(jié)點和運行時間都有明顯的效果，尤其對于局部較為空曠的空間，效果更為好。

由于采樣算法具有一定的隨機性，為了驗證本文算法的效果不是隨機產(chǎn)生的。在相同的環(huán)境、相同的起點和終點等環(huán)境條件下，分別連續(xù)運行文本算法和RRT 算法各30 次，記錄運行時間和隨機樹節(jié)點數(shù)量，將其統(tǒng)計于圖8中。

圖8 RRT 算法和本文算法的對比圖 Fig.8 Comparison on RRT algorithm and the proposed algorithm

圖8的(a)(b)子圖中，紅色的均是RRT 算法的數(shù)據(jù)，藍色的是基于邊際條件約束的啟發(fā)式自主探索算法的數(shù)據(jù)。從圖8(a)和8(b)的對比中可以看出，基于邊際條件約束的啟發(fā)式自主探索算法在運行時間和采樣點方面有較好的效果，在隨機樹節(jié)點的對比中尤為明顯。根據(jù)30 次實驗數(shù)據(jù)統(tǒng)計，RRT 算法的運行時間的均值為2.0289 s，標準差為4.5574，節(jié)點數(shù)量均值約110 個，標準差為135.6839。而本文算法的運行時間均值為0.4625 s，標準差為0.8447，節(jié)點數(shù)量約為23 個，標準差約為3.1156。在本次試驗中，節(jié)點數(shù)量減少約80%，時間效果縮短約75%。然而，由于RRT算法的標準差過大，時間和節(jié)點數(shù)量的效果不能簡單用均值代替，多次實驗產(chǎn)生的實際效果可能會有些許波動。但是從圖形中仍可明顯看出，本文算法在時間和節(jié)點數(shù)量上的性能都有提高，同時，算法的穩(wěn)定性也強于RRT 算法。

圖9是基于邊際條件約束的啟發(fā)式自主探索算法在ROS 下的仿真實驗結果。紅色的線段是經(jīng)過基于邊際條件約束的啟發(fā)式自主探索算法給出的路徑。從圖中可知，本文算法能夠適用于探索算法，使得機器人在室內(nèi)未知環(huán)境中實現(xiàn)安全、快速的移動。

將機器人探索的POMDP 模型和基于邊際條件約束的啟發(fā)式自主探索算法結合即可實現(xiàn)機器人的自主探索。

圖9 基于邊際條件約束的啟發(fā)式自主探索算法ROS 仿真實驗圖 Fig.9 ROS simulation experiment of heuristic autonomous exploration algorithm based on marginal condition constraint

4 結 論

室內(nèi)未知環(huán)境下的移動機器人自主探索是實現(xiàn)機器人智能化和自主化的重要部分。本文提出一種基于邊際條件約束的啟發(fā)式機器人快速路徑自主路徑探索算法，能實現(xiàn)在未知環(huán)境下自主探索室內(nèi)環(huán)境。同時，在啟發(fā)式算法的指導下，優(yōu)化機器人移動的路徑，使得機器人能快速、安全地移動到探索算法確定的下一目標點。采用本文提出的算法能優(yōu)化多轉角路徑對機器人帶來的不利影響，并且一定程度上節(jié)省了程序運行時間，這對于探索任務來說是有重要作用的。