面向復(fù)合地圖的移動機(jī)器人分層路徑規(guī)劃

武 星 楊俊杰 湯 凱 翟晶晶

樓佩煌南京航空航天大學(xué)機(jī)電學(xué)院，南京，210016

0 引言

路徑規(guī)劃是移動機(jī)器人研究的重點(diǎn)領(lǐng)域，其主要目的是在給定環(huán)境下搜索出一條從起點(diǎn)到終點(diǎn)的優(yōu)化路徑，該路徑在不與障礙物碰撞的前提下，盡量保證行駛距離最短、安全性最高、行駛時間最優(yōu)等一個或多個性能指標(biāo)[1-2]。根據(jù)對環(huán)境信息的認(rèn)知程度，路徑規(guī)劃可分為兩類：對環(huán)境有完整認(rèn)知的全局路徑規(guī)劃、環(huán)境未知或動態(tài)變化的局部路徑規(guī)劃[3-4]。前者旨在利用全局視野尋找一條全局優(yōu)化路徑，后者旨在躲避障礙物，修正全局路徑。環(huán)境建模是機(jī)器人理解周圍環(huán)境的重要途徑，也是全局路徑規(guī)劃的前提，常用的環(huán)境地圖有柵格地圖[5-6]、可視圖[7]、拓?fù)涞貓D[8]。

隨著移動機(jī)器人工作空間的不斷擴(kuò)大以及作業(yè)場景的日益復(fù)雜化，跨車間配送、非預(yù)期障礙物等因素增加了路徑規(guī)劃的難度?，F(xiàn)有的全局路徑規(guī)劃方法如A*算法[9-10]、蟻群算法[11-12]、粒子群算法[13]、遺傳算法[14-15]等常因搜索空間的膨脹導(dǎo)致搜索效率急劇降低，甚至無法搜索到最優(yōu)解。傳統(tǒng)局部路徑規(guī)劃方法如人工勢場法(artificial potential field, APF)[16]、動態(tài)窗口法(dynamic window approach, DWA)[17]等存在易于陷入局部極值點(diǎn)、避障實(shí)時性及成功率不高等問題。

針對部分未知的復(fù)雜大場景環(huán)境，余翀等[18]提出一種分層次多步驟的路徑規(guī)劃算法，首先在拓?fù)鋵右?guī)劃，采用起泡算法生成Voronoi圖來描述全局拓?fù)潢P(guān)系，然后利用廣義水平集實(shí)現(xiàn)拓?fù)鋵幼顑?yōu)路徑規(guī)劃，最后在柵格層利用快速行進(jìn)法(fast marching method，F(xiàn)MM)實(shí)現(xiàn)路徑再規(guī)劃。該方法平衡了單一路徑規(guī)劃算法的缺陷，發(fā)揮了融合算法優(yōu)勢，但未充分考慮機(jī)器人運(yùn)動特性，生成的路徑較長且拐點(diǎn)較多。MARTINS等[19]提出一種IMOA-star(improved multi-objective a-star)方法，通過重復(fù)利用存儲為pickle格式的障礙圖，避免了頻繁計(jì)算柵格的啟發(fā)函數(shù)從而縮短算法的執(zhí)行時間，同時引入路徑問題感知執(zhí)行器，減小了路徑長度和提高了路徑平滑度，然而該方法未充分考慮動態(tài)障礙物對障礙圖更新的影響。徐開放[20]提出一種基于度量-拓?fù)浞謱咏Y(jié)構(gòu)的復(fù)合地圖的D*Lite和BP神經(jīng)網(wǎng)絡(luò)融合路徑規(guī)劃方法，全局采用低分辨率地圖，并使用D*Lite算法規(guī)劃出一組路徑節(jié)點(diǎn)序列，路徑節(jié)點(diǎn)間利用增強(qiáng)學(xué)習(xí)實(shí)現(xiàn)了相鄰兩個節(jié)點(diǎn)的抵達(dá)問題。該方法將任務(wù)拆解，減少搜索空間，是一種較好的解決方案，但只能輸出離散的動作，不利于機(jī)器人連續(xù)控制。深度強(qiáng)化學(xué)習(xí)(deep reinforcement learning，DRL)兼具深度學(xué)習(xí)的感知能力和強(qiáng)化學(xué)習(xí)的決策能力，有望改善現(xiàn)有路徑規(guī)劃方法在未知環(huán)境下的缺陷，這類端到端的方法能夠?qū)χ車h(huán)境迅速作出決策，靈活性高、適應(yīng)能力強(qiáng)，可以更好地處理突發(fā)情況。然而，DRL還有些亟待優(yōu)化的問題，如訓(xùn)練耗時長、樣本利用率低等問題。

為了克服上述方法的不足且更好適應(yīng)部分未知的復(fù)雜大場景環(huán)境，本文提出一種面向復(fù)合地圖的移動機(jī)器人分層路徑規(guī)劃方法，引入層次圖思想，建立了拓?fù)?柵格-度量復(fù)合地圖，實(shí)現(xiàn)了不同抽象程度和精細(xì)程度的環(huán)境表示。在此基礎(chǔ)上進(jìn)行路徑規(guī)劃，先針對拓?fù)?柵格地圖生成全局優(yōu)化的初始路徑，機(jī)器人沿初始路徑行駛時再探測周圍局部環(huán)境，并使用深度強(qiáng)化學(xué)習(xí)算法進(jìn)行避障路徑規(guī)劃，通過分層分區(qū)域方式提高路徑規(guī)劃算法的效率和成功率。

1 復(fù)合地圖分層路徑規(guī)劃框架

1.1 復(fù)合地圖描述

1.1.1層次圖定義

目前多數(shù)路徑規(guī)劃方法均基于某種環(huán)境地圖在全局范圍內(nèi)開展搜索，搜索范圍大導(dǎo)致搜索效率不盡人意。為減小路徑規(guī)劃算法的搜索范圍，通常會對大規(guī)模場景進(jìn)行抽象描述和層次分解，層次圖非常適合這種情況。

層次圖采用多叉樹來表達(dá)，樹中包含多個抽象層次，抽象程度自上而下逐層降低。每一層由一個或多個關(guān)鍵節(jié)點(diǎn)構(gòu)成，節(jié)點(diǎn)之間通過邊連接，因此每個抽象層都是一幅圖。構(gòu)建層次圖是一個迭代的過程，某一層中的關(guān)鍵節(jié)點(diǎn)是其下一層圖的抽象描述，逐層分解直至對環(huán)境做出詳細(xì)的描述。每個關(guān)鍵節(jié)點(diǎn)都代表了某一局部環(huán)境，故層次圖不宜過深，否則導(dǎo)致關(guān)鍵點(diǎn)間隔過近，抽象程度降低，進(jìn)而會增加算法的復(fù)雜度。層次圖序列L可表示為

L={L0,L1,L2,…,Ld}

其中，Li為第i層抽象。值得注意的是，L0為根層次，是對環(huán)境地圖的最高層抽象；Ld為最深層，即最小的子地圖。層次圖中每一層Li都有圖Gi對應(yīng)，Gi由關(guān)鍵節(jié)點(diǎn)、普通節(jié)點(diǎn)、連接各節(jié)點(diǎn)的邊及其權(quán)重等信息組成，本文將其演化為拓?fù)涞貓D。普通節(jié)點(diǎn)僅用于輔助路徑搜索，而關(guān)鍵節(jié)點(diǎn)負(fù)責(zé)連接子地圖與其父地圖，在層次圖的同一層次中通過離線先驗(yàn)路徑連接相鄰子地圖。以制造業(yè)公司場景為例，為每個子區(qū)域設(shè)置關(guān)鍵節(jié)點(diǎn)，層次圖的表達(dá)方式如圖1所示。

圖1 層次圖示例Fig.1 Example of hierarchical graph

在圖1中，紅色方點(diǎn)和藍(lán)色圓點(diǎn)分別代表關(guān)鍵節(jié)點(diǎn)和普通節(jié)點(diǎn)，連接節(jié)點(diǎn)的黑色線條為先驗(yàn)路徑(邊)。其中，兩個帶有五角星標(biāo)記的關(guān)鍵節(jié)點(diǎn)是同一個點(diǎn)，只不過顯示在不同層級上，可實(shí)現(xiàn)相鄰層級間的訪問。根層次L0描述了公司的組織架構(gòu)，包含行政室、研發(fā)室、制造室和休息室，L1則進(jìn)一步描述制造室的功能布局。

1.1.2復(fù)合地圖架構(gòu)

首先，通過機(jī)器人操作系統(tǒng)(robot operating system，ROS)軟件gmapping功能包將移動機(jī)器人作業(yè)環(huán)境描述為柵格地圖形式。然后，利用層次圖將全局柵格地圖劃分為若干個不同層次的子地圖，且每個子地圖均設(shè)置關(guān)鍵節(jié)點(diǎn)，同一層次的關(guān)鍵節(jié)點(diǎn)間通過離線路徑(邊)連接，具有明顯拓?fù)涮卣?，進(jìn)而構(gòu)建拓?fù)涞貓D。柵格地圖中，某物體的坐標(biāo)通常由它占據(jù)柵格的中心點(diǎn)坐標(biāo)替代，這種表示方式的精度有限，因此，移動機(jī)器人在運(yùn)行過程中利用激光雷達(dá)和慣導(dǎo)系統(tǒng)等傳感器維護(hù)局部度量地圖，實(shí)現(xiàn)更高精度的位置表示。由此建立了一種拓?fù)?柵格-度量復(fù)合地圖，如圖2所示。

圖2 拓?fù)?柵格-度量復(fù)合地圖Fig.2 Hybrid topological-grid-metric map

圖3展示了面向復(fù)合地圖的分層路徑規(guī)劃架構(gòu)。拓?fù)涞貓D反映了關(guān)鍵節(jié)點(diǎn)之間的空間位置關(guān)系，利用Floyd算法規(guī)劃最優(yōu)點(diǎn)集序列，確保起點(diǎn)所在子地圖的關(guān)鍵節(jié)點(diǎn)到終點(diǎn)所在子地圖的關(guān)鍵節(jié)點(diǎn)之間的區(qū)間路徑是離線最優(yōu)的。將關(guān)鍵節(jié)點(diǎn)代表的子地圖內(nèi)部展開為柵格地圖，提出改進(jìn)A*算法進(jìn)行內(nèi)部路徑的局部規(guī)劃。度量地圖利于移動機(jī)器人精細(xì)化描述周圍環(huán)境，提出改進(jìn)深度確定性策略梯度(deep deterministic policy gradient, DDPG)算法進(jìn)行機(jī)器人的避障規(guī)劃。

圖3 面向復(fù)合地圖的分層路徑規(guī)劃架構(gòu)Fig.3 Hierarchical path planning architecture forhybrid map

1.2 分層路徑規(guī)劃方案

分層路徑規(guī)劃方案的流程如圖4所示，主要分為兩個步驟：首先在拓?fù)?柵格地圖上規(guī)劃出一條全局優(yōu)化初始路徑;然后移動機(jī)器人沿初始路徑在行駛過程中實(shí)時檢測其周圍環(huán)境并維護(hù)局部度量地圖，若離機(jī)器人最近障礙物距離小于避障閾值，則運(yùn)行針對度量地圖的改進(jìn)DDPG算法進(jìn)行避障路徑規(guī)劃。

圖4 復(fù)合地圖分層路徑規(guī)劃流程Fig.4 Procedure of hierarchical path planning forhybrid map

分層路徑規(guī)劃的處理流程如下：

(1)在柵格地圖上采用細(xì)化算法生成離線先驗(yàn)路徑，提取拓?fù)涮卣魍戤吅螅瑯?gòu)造拓?fù)涞貓D并完善層次圖。

(2)判斷起始柵格S和目標(biāo)柵格D是否處于同一子地圖中，即判斷是否隸屬于層次圖的同一節(jié)點(diǎn)，若是則直接利用改進(jìn)A*算法規(guī)劃出S到D的優(yōu)化路徑即為全局優(yōu)化初始路徑，轉(zhuǎn)至步驟(6)；否則轉(zhuǎn)至步驟(3)。

(3)分別獲取S和D在層次圖中的深度LS、LD及它們所在子地圖的關(guān)鍵節(jié)點(diǎn)CS、CD，若LS

(4)以S為起點(diǎn)，CS為終點(diǎn)，利用改進(jìn)A*算法在S所在子區(qū)域的柵格地圖上搜索出一條局部路徑并保存。同理，將CD設(shè)為起點(diǎn)，D設(shè)為終點(diǎn)，規(guī)劃出另一條局部路徑并保存。若無法搜索出可行解，則路徑規(guī)劃失敗，算法結(jié)束。

(5)將步驟(3)生成的起始關(guān)鍵節(jié)點(diǎn)和目標(biāo)關(guān)鍵節(jié)點(diǎn)之間的離線先驗(yàn)路徑和步驟(4)搜索到的S和D所在子地圖的局部路徑合并，得到全局優(yōu)化初始路徑。

(6)機(jī)器人沿著初始路徑行進(jìn)時，一旦機(jī)器人離最近障礙物的距離小于避障閾值，利用改進(jìn)DDPG算法完成避障后重新回到初始路徑；直到機(jī)器人到達(dá)目標(biāo)位置D，算法終止。

2 拓?fù)?柵格分層全局路徑規(guī)劃

2.1 拓?fù)涞貓D最優(yōu)先驗(yàn)離線路徑規(guī)劃

拓?fù)涞貓D不易構(gòu)建，原因在于拓?fù)涮卣?節(jié)點(diǎn))的定義和識別難度較大。本節(jié)將利用細(xì)化算法生成離線先驗(yàn)路徑，以選取拓?fù)涮卣?，?gòu)建拓?fù)涞貓D。在此基礎(chǔ)上還研究了離線路徑點(diǎn)集序列的優(yōu)化方法，最終生成最優(yōu)先驗(yàn)路徑。

2.1.1離線先驗(yàn)路徑生成

細(xì)化算法[21]是圖像處理中常用的方法，能夠有效提取原圖像形狀的拓?fù)浣Y(jié)構(gòu)。二維柵格地圖由“0”和“1”組成，可以視為一幅二值化圖像，通過細(xì)化算法將自由柵格區(qū)域細(xì)化，最終生成安全路徑。需要注意的是，細(xì)化算法處理的是值為1的像素，故處理時需把自由柵格視為“1”，障礙柵格視為“0”。

細(xì)化算法的主要思想是：檢查圖像中某一像素是否符合刪除的條件，一旦滿足立即刪除該像素(像素值由1變?yōu)?)，重復(fù)迭代上述過程直到無法繼續(xù)刪除為止。檢查像素p1時建立圖5所示的3×3窗口，將p1作為中心單元，并判斷周圍的8鄰域是否滿足細(xì)化條件。每次迭代分為如下兩個步驟：

(1)2≤B(p1)≤6；A(p1)=1；p2×p4×p6=0；p4×p6×p8=0；

(2)2≤B(p1)≤6；A(p1)=1；p2×p4×p8=0；p2×p6×p8=0。

其中，B(p1)表示p1周圍8鄰域中數(shù)值為1的個數(shù)；A(p1)表示以順時針方向統(tǒng)計(jì)p2,p3,p4,…,p8,p9,p2數(shù)值從0變?yōu)?的次數(shù)。步驟(1)和步驟(2)交替進(jìn)行，只有步驟(1)或步驟(2)中所有條件同時滿足，p1才能被刪除。

圖5 細(xì)化算法窗口Fig.5 Window of thinning algorithm

2.1.2拓?fù)涮卣魈崛?/p>

柵格地圖經(jīng)過細(xì)化算法后會生成較為簡潔的離線先驗(yàn)路徑，該路徑存在明顯拓?fù)涮卣鳎跇?gòu)建拓?fù)涞貓D。首先，人工選取區(qū)域入口柵格或其他具有代表性的柵格作為子地圖的關(guān)鍵節(jié)點(diǎn)；其次，提取路徑的分岔點(diǎn)作為拓?fù)鋱D中的普通節(jié)點(diǎn)；最后，統(tǒng)計(jì)連接相鄰節(jié)點(diǎn)的邊所占據(jù)的柵格數(shù)作為邊的權(quán)。每個節(jié)點(diǎn)存儲自身編號和所有與之相連的節(jié)點(diǎn)編號及該拓?fù)溥叺臋?quán)重，拓?fù)涮卣魈崛⊥戤吅髽?gòu)建各層次對應(yīng)的拓?fù)涞貓D。

圖6 拓?fù)涮卣魈崛∈纠鼺ig.6 Example of topological feature extraction

圖6展示了按以上規(guī)則提取出來的拓?fù)涮卣?，圖中黑色柵格為障礙物，白色柵格為自由空間，灰色柵格為細(xì)化算法生成的離線先驗(yàn)路徑，紅色方形為關(guān)鍵節(jié)點(diǎn)，藍(lán)色圓點(diǎn)為普通節(jié)點(diǎn)。

2.1.3優(yōu)化離線路徑點(diǎn)集序列

拓?fù)涞貓D包含關(guān)鍵節(jié)點(diǎn)、普通節(jié)點(diǎn)和連接節(jié)點(diǎn)的邊，是一張帶權(quán)的路徑網(wǎng)絡(luò)圖，從起始節(jié)點(diǎn)出發(fā)可以有多條路徑到達(dá)目標(biāo)節(jié)點(diǎn)。機(jī)器人路徑規(guī)劃期望路徑長度盡可能地短，因此拓?fù)涞貓D上的路徑規(guī)劃問題等價于在帶權(quán)圖中尋找起點(diǎn)到終點(diǎn)的最優(yōu)點(diǎn)集序列問題。本文選用Floyd算法在帶權(quán)圖中求取多源點(diǎn)之間的最短距離。由于Floyd算法作用于拓?fù)涞貓D，而拓?fù)涞貓D又是對環(huán)境的高度抽象，局部細(xì)微的變動對拓?fù)浣Y(jié)構(gòu)影響不大，因此真實(shí)環(huán)境的細(xì)微變化不必在拓?fù)涞貓D上頻繁調(diào)用Floyd算法。

2.2 基于柵格地圖的改進(jìn)A*算法

A*算法通過包含啟發(fā)信息的代價函數(shù)來搜索最優(yōu)路徑，代價函數(shù)f(n)由兩部分組成：起點(diǎn)沿著已生成的路徑到達(dá)當(dāng)前節(jié)點(diǎn)的開銷g(n)和當(dāng)前節(jié)點(diǎn)到終點(diǎn)的預(yù)估開銷h(n)，可表示為

f(n)=g(n)+h(n)

(1)

A*算法建立并維護(hù)兩個列表：存放已經(jīng)探測到但還未訪問節(jié)點(diǎn)的Open列表和存放已經(jīng)訪問過節(jié)點(diǎn)的Close列表。初始化時只包含起點(diǎn)，Close初始化為空。從起點(diǎn)開始，遍歷周圍8鄰域節(jié)點(diǎn)，若該節(jié)點(diǎn)在Open表中不存在，則加入至Open，否則判斷是否需要更新Open表中該節(jié)點(diǎn)的代價。然后從Open表中彈出代價值最小的節(jié)點(diǎn)，同時移入Close表，以該節(jié)點(diǎn)為路徑當(dāng)前節(jié)點(diǎn)繼續(xù)向目標(biāo)點(diǎn)拓展，重復(fù)以上過程直至Open表為空或搜索到終點(diǎn)。路徑上每個節(jié)點(diǎn)都有一個指向父節(jié)點(diǎn)的指針，通過跟蹤指針向前回溯即可找到最佳路徑。

標(biāo)準(zhǔn)A*算法在擴(kuò)展節(jié)點(diǎn)時比較盲目，可能將多余節(jié)點(diǎn)加入Open表，增加了維護(hù)Open表代價的同時擴(kuò)大了搜索空間，導(dǎo)致算法在大范圍空間下的搜索效率不夠理想。此外，規(guī)劃出來的路徑拐點(diǎn)較多，不利于移動機(jī)器人跟蹤。針對以上問題，對A*算法進(jìn)行改進(jìn)，如圖7所示。本文采用雙向搜索機(jī)制，同時開展從起點(diǎn)向終點(diǎn)的正向搜索和從終點(diǎn)向起點(diǎn)的反向搜索，逐步生成路徑節(jié)點(diǎn)向中間靠攏；在擴(kuò)展過程中依據(jù)方向信息對候選的節(jié)點(diǎn)進(jìn)行篩選，過濾掉一些無效節(jié)點(diǎn)，提高搜索效率；最后對生成的路徑進(jìn)行修飾，刪除冗余點(diǎn)，提高路徑質(zhì)量。

圖7 改進(jìn)A*流程圖Fig.7 Flow chart of improved A* algorithm

2.2.1篩選擴(kuò)展點(diǎn)

圖8展示了A*算法擴(kuò)展過程，其中S為起始位置，m為被擴(kuò)展到的節(jié)點(diǎn)，D為目標(biāo)位置。路徑規(guī)劃期望路徑長度盡可能地短，即路徑方向盡可能貼合起點(diǎn)指向終點(diǎn)的方向SD。θ為當(dāng)前節(jié)點(diǎn)到終點(diǎn)的方向mD與期望方向SD的角度偏差，可表示為

(2)

|θ|越小，節(jié)點(diǎn)m被加入Open表的可能性越大。

圖8 A*算法擴(kuò)展過程Fig.8 Extension process of A* algorithm

式(2)中涉及反三角函數(shù)運(yùn)算，在實(shí)際運(yùn)用過程中會占用較多算力。為了簡化擴(kuò)展節(jié)點(diǎn)和預(yù)期路徑符合程度的表示，同時放大各擴(kuò)展節(jié)點(diǎn)之間的這種差異，本文選用圖8中綠色矩形面積來引導(dǎo)路徑規(guī)劃過程。以m為原點(diǎn)分別向標(biāo)準(zhǔn)坐標(biāo)系的X軸和Y軸延伸，并與邊SD、DT相交于兩點(diǎn)，可構(gòu)成一個矩形，矩形面積φ(m)可表示為

(3)

φ(m)越小，當(dāng)前被擴(kuò)展到的節(jié)點(diǎn)和預(yù)期路徑符合程度越高，越有機(jī)會加入Open表中繼續(xù)擴(kuò)展。設(shè)置面積閾值φ0，當(dāng)φ(m)>φ0且滿足p(m)>p0時(其中p(m)為隨機(jī)概率，p0為信任閾值概率)，忽略當(dāng)前被擴(kuò)展的點(diǎn)m，重新選擇新的擴(kuò)展點(diǎn)。其余情況對m的處理方式和標(biāo)準(zhǔn)A*算法保持一致。通過引入方向指引信息，有一定概率忽略方向性不強(qiáng)的節(jié)點(diǎn)，進(jìn)而減少了Open表中候選點(diǎn)的個數(shù)，縮小了算法的搜索空間。

2.2.2剔除冗余點(diǎn)

雖然上述改進(jìn)步驟能夠有效規(guī)劃出一條路徑，但依舊遵從8鄰域擴(kuò)展，即以45°為分辨率選擇下個柵格，該方式生成路徑會存在較多拐點(diǎn)，且并不一定是最優(yōu)路徑，故改進(jìn)路徑生成策略。首先在前文獲得的初始路徑中每對相鄰節(jié)點(diǎn)距離二等分處插入新節(jié)點(diǎn)。然后將所有節(jié)點(diǎn)按起點(diǎn)指向終點(diǎn)方向從小到大編號，從終點(diǎn)向前搜索是否有可以略過的冗余點(diǎn)。當(dāng)兩節(jié)點(diǎn)連線經(jīng)過障礙物時，則說明編號小的節(jié)點(diǎn)無法被刪除，否則即可刪除。重復(fù)以上過程直到搜索到起點(diǎn)，最后按順序連接剩余的節(jié)點(diǎn)，如圖9所示，其中虛線即刪除冗余點(diǎn)后的路徑。

圖9 刪除冗余點(diǎn)后的路徑Fig.9 Path after removing redundant points

3 度量地圖局部路徑規(guī)劃

當(dāng)移動機(jī)器人沿全局優(yōu)化初始路徑運(yùn)行時，針對部分未知場景中的動態(tài)障礙物，研究了基于深度強(qiáng)化學(xué)習(xí)的避障路徑規(guī)劃方法，其研究方案如圖10所示。移動機(jī)器人負(fù)責(zé)將自身感知到的狀態(tài)數(shù)據(jù)輸送至Actor網(wǎng)絡(luò)中，經(jīng)過基于價值分類經(jīng)驗(yàn)回放機(jī)制的深度確定性策略梯度(deep deterministic policy gradient based on value-classified experience replay, VCER-DDPG)算法決策后，控制器輸出信號指導(dǎo)機(jī)器人做出相應(yīng)動作。

圖10 基于VCER-DDPG算法的局部路徑規(guī)劃框架Fig.10 Local path planning framework based on VCER-DDPG algorithm

VCER-DDPG算法的核心由兩部分組成：價值分類經(jīng)驗(yàn)回放池和Actor-Critic網(wǎng)絡(luò)架構(gòu)。價值經(jīng)驗(yàn)回放池主要負(fù)責(zé)存儲訓(xùn)練過程中產(chǎn)生的經(jīng)驗(yàn)樣本，并按一定的采樣策略抽取部分樣本用于訓(xùn)練。Actor稱為策略網(wǎng)絡(luò)，完成狀態(tài)空間到動作空間的映射；Critic稱為價值網(wǎng)絡(luò)，使用價值函數(shù)對Actor輸出的動作進(jìn)行評價，指導(dǎo)Actor改進(jìn)策略。為降低損失函數(shù)震蕩發(fā)散概率，提高算法的穩(wěn)定性，Actor和Critic均采用雙重神經(jīng)網(wǎng)絡(luò)架構(gòu)，即在線網(wǎng)絡(luò)(online)和目標(biāo)網(wǎng)絡(luò)(target)。VCER-DDPG算法共包含4個深層神經(jīng)網(wǎng)絡(luò)，即

(4)

(5)

其中，μ、μ′分別為online動作策略函數(shù)和target動作策略函數(shù)；θμ、θμ′分別為online Actor和target Actor網(wǎng)絡(luò)參數(shù)；Q、Q′分別為online價值函數(shù)和target價值函數(shù)，θQ、θQ′分別為online Critic和target Critic網(wǎng)絡(luò)參數(shù)；S為狀態(tài)，時間步為t時的狀態(tài)表示為St。VCER-DDPG訓(xùn)練的本質(zhì)是利用深度學(xué)習(xí)更新優(yōu)化Actor和Critic的網(wǎng)絡(luò)參數(shù)，最終獲得能應(yīng)對復(fù)雜避障問題的online Actor網(wǎng)絡(luò)。

完整的局部路徑規(guī)劃過程如下：首先，激光雷達(dá)的距離點(diǎn)云數(shù)據(jù)、機(jī)器人當(dāng)前位姿、目標(biāo)位姿被合并成一維狀態(tài)數(shù)組St，在線策略網(wǎng)絡(luò)依據(jù)狀態(tài)St生成動作μ(St)，疊加行為策略β后得到機(jī)器人最終執(zhí)行的動作at。動作at由線速度和角速度組成，機(jī)器人控制器將at轉(zhuǎn)化為實(shí)際控制信號，執(zhí)行完畢后到達(dá)新的狀態(tài)St+1。環(huán)境判斷當(dāng)前回合是否結(jié)束，同時利用獎勵函數(shù)對動作at進(jìn)行評價，給出獎勵值rt。這一過程產(chǎn)生的經(jīng)驗(yàn)樣本(St，at，rt，St+1，done)由價值分類經(jīng)驗(yàn)回放池進(jìn)行存儲。模型訓(xùn)練時，根據(jù)分層采樣策略從經(jīng)驗(yàn)回放池抽取小批量樣本送入Actor和Critic網(wǎng)絡(luò)進(jìn)行訓(xùn)練，重復(fù)上述過程直到模型網(wǎng)絡(luò)收斂至最佳避障策略。

3.1 價值分類經(jīng)驗(yàn)回放池

原始經(jīng)驗(yàn)回放機(jī)制采用等概率采樣，未區(qū)分經(jīng)驗(yàn)樣本的價值，重復(fù)學(xué)習(xí)簡單樣本對網(wǎng)絡(luò)指導(dǎo)效率有限，網(wǎng)絡(luò)收斂速度較緩慢，因此，在訓(xùn)練過程中根據(jù)樣本價值，將經(jīng)驗(yàn)樣本分類存放至不同的子經(jīng)驗(yàn)回放池中。制定合適的樣本價值衡量標(biāo)準(zhǔn)是對經(jīng)驗(yàn)樣本進(jìn)行分類的前提，大多數(shù)強(qiáng)化學(xué)習(xí)算法采用時間差分誤差δ作為狀態(tài)值函數(shù)的估計(jì)，|δ|越大，向預(yù)期動作值的校正就越積極，因此δ很大程度上與樣本的學(xué)習(xí)價值相關(guān)聯(lián)。十分成功和失敗的經(jīng)驗(yàn)樣本通常擁有較高的|δ|，重復(fù)回放這類樣本能夠及時審視當(dāng)前策略的學(xué)習(xí)效果，加快網(wǎng)絡(luò)收斂，提高訓(xùn)練效率。然而訓(xùn)練初期，對環(huán)境的探索有限，策略還未成熟，在狀態(tài)空間的邊緣也可能出現(xiàn)較大的|δ|，可能改變網(wǎng)絡(luò)的收斂方向，僅僅依據(jù)時間差分誤差δ無法準(zhǔn)確衡量樣本價值。從短期來看，獎勵值r直接表現(xiàn)了動作的好壞程度，高獎勵值樣本在訓(xùn)練初期具有學(xué)習(xí)價值。本文將時間差分誤差δ和獎勵值r結(jié)合起來，制定了價值衡量方法，其表達(dá)式為

Vj=α|rj|+(1-α)|δj|

(6)

δj=rj+γQ′(Sj+1,aj+1;θQ′)-Q(Sj,aj;θQ)

(7)

其中，Vj為樣本j的價值，|rj|為樣本j的獎勵值的絕對值，γ∈(0,1]為獎勵值折扣因子，α為|rj|的權(quán)重，|δj|為樣本j的時間差分誤差的絕對值，(1-α)為|δj|的權(quán)重，α隨著訓(xùn)練過程動態(tài)更新。訓(xùn)練初期α取1，主要利用獎勵值引導(dǎo)智能體快速學(xué)習(xí)策略，隨著訓(xùn)練深入，時間差分誤差δ能更好地指導(dǎo)策略更新，α逐漸減小以提高δ的權(quán)重。

價值分類經(jīng)驗(yàn)回放池的結(jié)構(gòu)如圖11所示，分別由存放高價值樣本的子回放池B1、存放近期樣本的子回放池B2、存放普通樣本的子回放池B3組成。子回放池均采用隊(duì)列作為存儲容器，新樣本從隊(duì)尾插入，容量達(dá)到上限時從隊(duì)頭彈出樣本。

圖11 價值分類經(jīng)驗(yàn)回放池結(jié)構(gòu)Fig.11 Structure of value-classified experience replay pool

價值分類經(jīng)驗(yàn)回放池的構(gòu)建步驟為：初始化時設(shè)置B1、B3兩個子回放池中所有樣本價值的平均值Vm為0，同時將獎勵值的權(quán)重系數(shù)α設(shè)為1，訓(xùn)練過程中每產(chǎn)生一條樣本先加入B2隊(duì)尾，若B2達(dá)容量上限則從隊(duì)頭彈出一個樣本，并計(jì)算該樣本的價值，若高于Vm則將該樣本添加至B1，反之存入B3。存儲完畢后，更新Vm和α，重復(fù)以上過程直至訓(xùn)練結(jié)束。

為了保證采樣所得樣本的時效性和多樣性，本文設(shè)計(jì)了分層采樣策略，分別從B1、B2、B3子回放池中隨機(jī)抽取適量樣本。記小批量采樣總數(shù)為N，包括子回放池B1采樣數(shù)N1、子回放池B2采樣數(shù)N2、子回放池B3采樣數(shù)N3，即

N=N1+N2+N3

(8)

其中，N2為固定值，確保每次小批量采樣均有近期樣本，該類樣本具有時效性，有助于及時調(diào)整網(wǎng)絡(luò)收斂方向。訓(xùn)練初期，N1取較大值，通過選取大量高質(zhì)量樣本來促進(jìn)網(wǎng)絡(luò)朝正確方向收斂。隨著訓(xùn)練推進(jìn)，策略網(wǎng)絡(luò)的成熟，機(jī)器人表現(xiàn)越發(fā)優(yōu)異，高價值樣本出現(xiàn)頻率越來越高，為避免過擬合，應(yīng)適當(dāng)降低N1、提高N3。

3.2 VCER-DDPG算法流程

VCER-DDPG的算法偽代碼如下：

4 實(shí)驗(yàn)分析

為了驗(yàn)證本文所提方法的有效性，將采用計(jì)算機(jī)軟件仿真與機(jī)器人樣機(jī)實(shí)驗(yàn)相結(jié)合的手段，分別在C++開發(fā)環(huán)境下進(jìn)行全局路徑規(guī)劃仿真實(shí)驗(yàn)；然后在Gazebo仿真引擎中驗(yàn)證所提局部路徑規(guī)劃算法的避障性能；最后在真實(shí)場景中驗(yàn)證本文所提復(fù)合地圖分層路徑規(guī)劃方法的有效性。

4.1 全局路徑規(guī)劃仿真實(shí)驗(yàn)

本次實(shí)驗(yàn)的環(huán)境由100×100個網(wǎng)格組成，每個網(wǎng)格邊長為10像素，黑色柵格為障礙物，白色柵格可自由通行。實(shí)驗(yàn)選擇標(biāo)準(zhǔn)A*算法、蟻群算法作為對比算法，以驗(yàn)證拓?fù)?柵格地圖分層路徑規(guī)劃方法的性能，實(shí)驗(yàn)參數(shù)設(shè)置如表1所示。

表1 全局路徑規(guī)劃仿真實(shí)驗(yàn)參數(shù)

每種方法分別進(jìn)行了3次路徑規(guī)劃任務(wù)，圖12所示為采用標(biāo)準(zhǔn)A*算法進(jìn)行路徑規(guī)劃的結(jié)果，圖13所示為由蟻群算法規(guī)劃獲得的路徑，圖14所示為本文分層規(guī)劃方法生成的路徑。圖12～圖14中紅色線條是算法生成的最終路徑，灰色部分為初始路徑或離線路徑，算法搜索過的柵格(可反映規(guī)劃算法的計(jì)算量)會被標(biāo)記為綠色。

從圖12、圖13中可以看出，標(biāo)準(zhǔn)A*算法和蟻群算法生成的路徑安全性不高，部分路段長距離貼合障礙物，機(jī)器人沿路徑行走時較易發(fā)生碰撞。此外，算法在搜索過程中缺少方向性引導(dǎo)，搜索空間較大，路徑擺動較為明顯，不利于機(jī)器人運(yùn)動控制。由圖14可知，基于拓?fù)?柵格地圖的分層路徑規(guī)劃方法利用細(xì)化算法可生成安全度較高的離線先驗(yàn)路徑，同時在子地圖內(nèi)部運(yùn)用改進(jìn)A*算法時，對目標(biāo)點(diǎn)的搜索更具有方向性，路徑拐點(diǎn)更少，利于機(jī)器人控制。

(a)任務(wù)1 (b)任務(wù)2 (c)任務(wù)3圖12 標(biāo)準(zhǔn)A*算法生成路徑Fig.12 Path generated by the original A* algorithm

(a)任務(wù)1 (b)任務(wù)2 (c)任務(wù)3圖13 蟻群算法生成路徑Fig.13 Path generated by the ACO algorithm

(a)任務(wù)1 (b)任務(wù)2 (c)任務(wù)3圖14 分層路徑規(guī)劃算法生成路徑Fig.14 Path generated by the hierarchical path planning algorithm

為了通過量化性能指標(biāo)對比分析三種路徑規(guī)劃方法的性能，對上述實(shí)驗(yàn)過程的路徑長度、危險柵格數(shù)(經(jīng)過障礙柵格周圍8鄰域)、搜索柵格數(shù)、運(yùn)行時間進(jìn)行統(tǒng)計(jì)，實(shí)驗(yàn)數(shù)據(jù)如表2所示。從表2中數(shù)據(jù)可知，相比標(biāo)準(zhǔn)A*算法，拓?fù)?柵格分層路徑規(guī)劃方法的路徑長度平均增大了4.89%，但危險柵格數(shù)減少了42.99%，搜索柵格數(shù)減少了80.20%，運(yùn)行時間縮短了91.75%；相比蟻群算法，拓?fù)?柵格分層路徑規(guī)劃方法的路徑長度平均增大了11.1%，但危險柵格數(shù)減少了50.5%，搜索柵格數(shù)減少了84.8%，運(yùn)行時間減少了4個數(shù)量級。綜合以上實(shí)驗(yàn)數(shù)據(jù)可以發(fā)現(xiàn)，本文所提的拓?fù)?柵格分層路徑規(guī)劃方法顯著縮小了算法的搜索空間，極大提高了搜索效率，最終生成的路徑兼具安全性和平滑度，能夠有效解決復(fù)雜大場景環(huán)境下的路徑規(guī)劃問題。

表2 全局路徑規(guī)劃實(shí)驗(yàn)數(shù)據(jù)

4.2 避障仿真實(shí)驗(yàn)

為了驗(yàn)證VCER-DDPG避障路徑規(guī)劃方法的有效性，首先結(jié)合ROS軟件和TensorFlow框架進(jìn)行DDPG深度學(xué)習(xí)程序的開發(fā)，并根據(jù)表3中的參數(shù)進(jìn)行訓(xùn)練，訓(xùn)練完畢后，可通過加載模型網(wǎng)絡(luò)參數(shù)以應(yīng)用最終習(xí)得的避障策略。然后，在Gazebo軟件中設(shè)計(jì)一個混合障礙場景：方形立柱充當(dāng)靜態(tài)障礙物，圓柱體以0.4 rad/s速度逆時針旋轉(zhuǎn)充當(dāng)動態(tài)障礙物。移動機(jī)器人在該場景中依次到達(dá)5個指定目標(biāo)點(diǎn)記作成功，否則記為失敗。

表3 訓(xùn)練超參數(shù)

實(shí)驗(yàn)過程中，使用了較為成熟的move_base導(dǎo)航功能包作為對比算法，分別對采用VCER-DDPG算法和move_base算法的機(jī)器人進(jìn)行15次測試，統(tǒng)計(jì)成功率、路徑長度、行駛時間等性能指標(biāo)(其中路徑長度和行駛時間只統(tǒng)計(jì)導(dǎo)航成功的測試數(shù)據(jù))。兩種方法的機(jī)器人運(yùn)行軌跡如圖15所示，實(shí)驗(yàn)數(shù)據(jù)如表4所示。

(a)VCER-DDPG的軌跡 (b) move_base的軌跡圖15 動態(tài)避障的仿真軌跡Fig.15 Simulation trajectories of dynamicobstacle avoidance

表4 動態(tài)避障的實(shí)驗(yàn)數(shù)據(jù)

由實(shí)驗(yàn)結(jié)果可見，move_base功能包由全局路徑規(guī)劃器A*算法和局部路徑規(guī)劃器DWA算法構(gòu)成，不易陷入局部最優(yōu)，故move_base方法規(guī)劃的路徑長度更短。然而，復(fù)雜的決策機(jī)制同時可能會導(dǎo)致短期無可行解時機(jī)器人在原地等待，直至出現(xiàn)可行解。該行為也導(dǎo)致平均行駛時間稍長，甚至路徑規(guī)劃失敗。而本文所提的深度強(qiáng)化學(xué)習(xí)算法是端到端的方法，根據(jù)狀態(tài)直接輸出動作，決策迅速，面對障礙主動進(jìn)行避障，雖然犧牲了一些路徑長度性能，但具有較高實(shí)時性且避障成功率更高，更適應(yīng)動態(tài)未知環(huán)境。

4.3 復(fù)合地圖分層路徑規(guī)劃實(shí)驗(yàn)

真實(shí)場景實(shí)驗(yàn)中采用的移動機(jī)器人平臺如圖16所示。該機(jī)器人采用差速驅(qū)動方式，前面有兩個主動輪，后面兩個萬向輪作為從動輪，并安裝有多種進(jìn)行環(huán)境/機(jī)器人狀態(tài)感知的傳感器，包括用于建圖和定位的倍加福R2000激光雷達(dá)、用于探測周圍障礙物的RPLIDARA2激光雷達(dá)、加速度計(jì)、陀螺儀模塊等。

圖16 移動機(jī)器人平臺Fig.16 Mobile robot platform

為了模擬大范圍的工作空間，本次實(shí)驗(yàn)由一個400 m2的實(shí)驗(yàn)室和50 m長的走廊及若干個房間組成，實(shí)驗(yàn)環(huán)境如圖17所示。R2000激光雷達(dá)安裝高度為60 cm，部分物體未被掃描到，建圖時無法獲取這些物體信息，對全局路徑規(guī)劃可能產(chǎn)生一定影響。然而，RPLIDARA2激光雷達(dá)的安裝高度為46 cm，能夠保證掃描到大多數(shù)障礙物，機(jī)器人運(yùn)行過程中將建圖時未能探測的物體視為障礙物從而采取避障策略。

(a)實(shí)驗(yàn)室

機(jī)器人平臺基于ROS開發(fā)，具體步驟如下：首先配置系統(tǒng)環(huán)境，包括搭建分布式通信網(wǎng)絡(luò)，利用SSH協(xié)議從遠(yuǎn)程端登錄機(jī)器人主機(jī)，運(yùn)行rviz工具；然后運(yùn)用gmapping功能包建立整體環(huán)境的柵格地圖，為后期定位和路徑規(guī)劃做準(zhǔn)備；再者利用ROS插件機(jī)制，將move_base框架中的全局和局部路徑規(guī)劃器分別替換為本文所提的拓?fù)?柵格路徑規(guī)劃方法和VCER-DDPG局部路徑規(guī)劃方法；最后機(jī)器人運(yùn)行基于move_base和amcl的導(dǎo)航算法，并通過筆記本上的rviz工具選取目標(biāo)點(diǎn)，進(jìn)行導(dǎo)航實(shí)驗(yàn)。

本次導(dǎo)航實(shí)驗(yàn)的運(yùn)行軌跡如圖18所示，其中紅色圓圈處是機(jī)器人所處的初始位置，綠色箭頭的末端是目標(biāo)位置，機(jī)器人導(dǎo)航過程中的實(shí)際運(yùn)行軌跡如圖中的藍(lán)色線條所示。在實(shí)驗(yàn)室和房間的門口設(shè)置關(guān)鍵節(jié)點(diǎn)，在它們內(nèi)部主要由改進(jìn)A*算法生成局部路徑，在保證安全的前提下以路徑最短為優(yōu)化目標(biāo)，因此路徑相對徑直。而走廊用于連接實(shí)驗(yàn)室和房間，機(jī)器人由離線路徑引導(dǎo)，故軌跡更靠近區(qū)域中心。由圖18可知，機(jī)器人絕大多數(shù)時間都沿著全局優(yōu)化初始路徑運(yùn)行，當(dāng)激光雷達(dá)探測到建圖時未掃描到的障礙物或突然出現(xiàn)的障礙物時會自主進(jìn)行局部路徑規(guī)劃。

圖18 導(dǎo)航實(shí)驗(yàn)Fig.18 Navigation experiment

以圖18中橙色線段分析主要的避障過程，圖19通過時間序號的形式展現(xiàn)了各時刻機(jī)器人和行人的位置。面向衛(wèi)生間走動的行人和長方體紙箱分別充當(dāng)動態(tài)和靜態(tài)的障礙物。記最近障礙物相對機(jī)器人前進(jìn)方向的角度為ao，位于左側(cè)的障礙物ao∈[-60°,0)，位于右側(cè)的障礙物ao∈(0,60°]；記最近障礙物相對機(jī)器人的距離為do。這兩個指標(biāo)作為狀態(tài)空間的組成部分，很大程度上影響了機(jī)器人的避障策略，故表5列舉了不同時刻下的ao和do，并結(jié)合線速度v和角速度ω的變化情況(圖20)來分析避障過程。

圖19 避障過程Fig.19 Obstacle avoidance process

由圖20可以看出，開始時，機(jī)器人沿初始路徑向前行駛，在t=1.7 s時，探測到右前方行人，為避免發(fā)生碰撞，角速度迅速調(diào)整為負(fù)值，向左前方避障；t=3 s時，行人位于機(jī)器人右前方0.56 m處，由于距離較近，隨即減小線速度，等待行人通過；t=4 s左右，行人位于機(jī)器人左前方，且已穿過走廊大半，基本不再產(chǎn)生干擾，機(jī)器人提高線速度的同時降低角速度幅值，并減小左拐的幅度；t=5.4 s時，紙箱成為主要避障目標(biāo)，角速度變?yōu)檎担蛴覀?cè)避障；t=9 s時，機(jī)器人即將繞過紙箱，同時位于走廊中間，角速度穩(wěn)定在0附近，徑直向前行駛。避障完成，回到初始路徑上，最終安全、準(zhǔn)確地到達(dá)目標(biāo)點(diǎn)。

表5 避障路段的實(shí)驗(yàn)數(shù)據(jù)

圖20 避障線速度和角速度Fig.20 Linear velocity and angular velocity duringobstacle avoidance

真實(shí)場景下的移動機(jī)器人路徑規(guī)劃實(shí)驗(yàn)結(jié)果表明，本文所提的復(fù)合地圖分層路徑規(guī)劃方法在實(shí)際場景應(yīng)用中也具有較好的效果。

5 結(jié)語

針對部分未知復(fù)雜大場景環(huán)境，提出一種基于拓?fù)?柵格-度量復(fù)合地圖的移動機(jī)器人分層路徑規(guī)劃方法。首先采用層次圖思想，將機(jī)器人作業(yè)環(huán)境劃分為多個以柵格表示的子地圖，并建立多個子地圖的拓?fù)浼軜?gòu)及局部區(qū)域的精細(xì)化描述模型。其次，在不同地圖層級上分區(qū)域搜索機(jī)器人路徑，在拓?fù)涞貓D上采用Floyd算法規(guī)劃子區(qū)域之間的區(qū)間路徑，面向柵格地圖提出搜索子區(qū)域內(nèi)部路徑的改進(jìn)A*算法，針對動態(tài)障礙物在度量地圖上提出VCER-DDPG避障路徑規(guī)劃方法。最后，采用計(jì)算機(jī)軟件仿真與機(jī)器人樣機(jī)實(shí)驗(yàn)相結(jié)合的手段，驗(yàn)證了所提分層路徑規(guī)劃方法的有效性，路徑搜索效率和避障成功率有明顯提高，生成的路徑兼具安全性和平滑性。