基于路徑規(guī)劃特點(diǎn)的語義目標(biāo)導(dǎo)航方法

高宇，霍靜，李文斌，伍靜，來煜坤，高陽

（1.南京大學(xué) 計(jì)算機(jī)軟件新技術(shù)全國重點(diǎn)實(shí)驗(yàn)室, 江蘇 南京 210023; 2.卡迪夫大學(xué) 計(jì)算機(jī)科學(xué)與信息學(xué)院, 英國 威爾士卡迪夫 CF10 3XQ）

語義目標(biāo)導(dǎo)航任務(wù)[1]要求一個(gè)裝有紅綠藍(lán)-深度（RGB-depth，RGBD）相機(jī)的機(jī)器人（智能體）在未知地圖環(huán)境中自主探索以找到特定類別的物體，其對安防、智能巡檢具有重要意義。智能體為完成語義目標(biāo)導(dǎo)航任務(wù)所需要的能力可概括為場景建圖、去哪里和如何去3類。場景建圖是指針對傳感器觀測數(shù)據(jù)的記憶和整合能力，除顯式構(gòu)建地圖外也有使用循環(huán)神經(jīng)網(wǎng)絡(luò)[2]等方法；去哪里是指分析目標(biāo)類別物體最有可能出現(xiàn)位置的能力，以實(shí)現(xiàn)高效率探索；如何去是指路徑規(guī)劃和軌跡跟蹤能力，以找到快速抵達(dá)目標(biāo)位置的可行控制動作序列。去哪里是語義目標(biāo)導(dǎo)航區(qū)別于其他導(dǎo)航任務(wù)的鮮明特點(diǎn)。

解決語義目標(biāo)導(dǎo)航任務(wù)需要搭建涉及眾多組件的復(fù)雜系統(tǒng)，每個(gè)組件的設(shè)定都會對最終性能產(chǎn)生很大影響，導(dǎo)致對現(xiàn)有方法的對比分析和對改進(jìn)之處的探索極為困難。為此，本文創(chuàng)建了一個(gè)解決語義目標(biāo)導(dǎo)航任務(wù)的標(biāo)準(zhǔn)框架，并針對測試中發(fā)現(xiàn)的深度不精準(zhǔn)、語義分割效果差、探索效率低、路徑規(guī)劃沒有考慮任務(wù)特點(diǎn)等問題提出了新的解決方案從而進(jìn)一步提升了性能。具體來說：1)針對探索效率低問題，在探索模塊中引入了基于路徑規(guī)劃特點(diǎn)的覆蓋最大化算法；2)針對深度不精準(zhǔn)和語義分割效果差問題，在語義建圖模塊中引入了深度圖邊緣處理與地圖糾錯機(jī)制；3)針對路徑規(guī)劃沒有考慮語義目標(biāo)導(dǎo)航任務(wù)特點(diǎn)的問題，引入了替代點(diǎn)機(jī)制和障礙物概率地圖。

使用提出的框架在一個(gè)逼真的交互式3D室內(nèi)語義數(shù)據(jù)集（habitat-matterport 3D semantics dataset，HM3DSem）[3-4]下進(jìn)行了實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果顯示，本文提出的3個(gè)改進(jìn)明顯提高了語義目標(biāo)導(dǎo)航任務(wù)的性能指標(biāo)。本文所提框架成功應(yīng)用到了四足機(jī)器人上，從而驗(yàn)證了該框架對現(xiàn)實(shí)世界具有泛化性。

1 語義目標(biāo)導(dǎo)航相關(guān)工作

1.1 語義SLAM

同時(shí)定位與地圖構(gòu)建（simultaneous localization and mapping ，SLAM）算法根據(jù)傳感器數(shù)據(jù)來源的不同可以分為激光SLAM[5-7]和視覺SLAM[8-11]。激光SLAM根據(jù)激光測距結(jié)果直接構(gòu)造障礙物地圖，視覺SLAM則尋找圖片上的關(guān)鍵特征點(diǎn)并通過多個(gè)視角來確定相機(jī)位姿。SLAM解決方案一般都集中在尋找像素級別的特征點(diǎn)，即在單幅圖像中提取特征點(diǎn)然后在多幅圖像中對特征點(diǎn)進(jìn)行匹配。這與人類通過判斷物體在眼睛中的移動來進(jìn)行定位極為不同，因?yàn)槿祟惗ㄎ会槍φ麄€(gè)物體而SLAM解決方案針對特征點(diǎn)。為此可以引申出使用語義信息來幫助尋找多幅圖像中的關(guān)聯(lián)或?yàn)镾LAM的回環(huán)檢測等引入更多信息，語義信息為SLAM提供了更多判斷相機(jī)移動的條件從而更好地定位。語義信息的引入也使得SLAM不僅能夠構(gòu)建點(diǎn)云地圖、障礙物地圖，而且可以構(gòu)建一個(gè)有語義標(biāo)簽的地圖，從而為下游任務(wù)提供了更廣闊的空間。

語義目標(biāo)導(dǎo)航任務(wù)主要用到語義SLAM提供的帶有語義標(biāo)簽的地圖。有了帶有語義信息的二維地圖或者三維地圖，智能體能夠更加結(jié)構(gòu)化地去分析不同類別物體的分布關(guān)系，更好地去理解場景從而推理出目標(biāo)類別物體在地圖中各個(gè)位置的出現(xiàn)概率。對目標(biāo)類別物體出現(xiàn)位置的推理是語義目標(biāo)導(dǎo)航任務(wù)的核心。但是現(xiàn)有方法沒有針對語義目標(biāo)導(dǎo)航中存在的深度不精準(zhǔn)和語義分割效果差問題進(jìn)行優(yōu)化，這導(dǎo)致連續(xù)多幀的語義建圖出現(xiàn)較大誤差而無法完成語義導(dǎo)航任務(wù)。本文為此引入了深度圖邊緣處理及語義點(diǎn)云地圖和二維地圖糾錯機(jī)制。

1.2 經(jīng)典導(dǎo)航方法

導(dǎo)航能力是無人車、無人機(jī)、無人船等眾多移動機(jī)器人所需要的基本能力之一。經(jīng)典導(dǎo)航方法通常在SLAM構(gòu)建的地圖上使用包括全局和局部路徑規(guī)劃在內(nèi)的路徑規(guī)劃模塊產(chǎn)生路徑，然后使用控制算法對路徑規(guī)劃模塊生成的路徑進(jìn)行跟蹤和實(shí)際行進(jìn)。路徑規(guī)劃的經(jīng)典算法有很多，如迪克斯特拉算法（Dijkstra algorithm）、A星搜索算法（A* search algorithm，A*）[12]、概率路線圖算法（probabilistic road maps，PRM）[13]、快速探索隨機(jī)樹算法（rapidly exploring random trees，RRT）[14]、人工勢場法（artificial potential fields，APF）[15]等。Dijkstra算法是基于圖結(jié)構(gòu)的能夠保證最優(yōu)解的路徑規(guī)劃算法，但處理大地圖時(shí)效率低下；AStar算法在Dijkstra算法的基礎(chǔ)上引入衡量任何一個(gè)位置到目標(biāo)位置距離的啟發(fā)式函數(shù)從而使得探索方向具有一定的目的性；PRM算法對地圖進(jìn)行稀疏采樣從而將柵格地圖轉(zhuǎn)換為由少量采樣點(diǎn)和可達(dá)邊組成的圖結(jié)構(gòu)，然后在圖結(jié)構(gòu)中再使用AStar等算法尋找路徑；RRT和PRM同是基于隨機(jī)采樣的規(guī)劃算法，基本思想是從一個(gè)點(diǎn)出發(fā)向外探索擴(kuò)展；與前面幾種算法不同的是，APF更適用于動態(tài)環(huán)境和局部路徑規(guī)劃。但是現(xiàn)有算法都沒有針對語義目標(biāo)導(dǎo)航任務(wù)特點(diǎn)進(jìn)行改進(jìn)從而導(dǎo)致路徑規(guī)劃模塊的失敗率較高，為此本文引入了替代點(diǎn)機(jī)制以提高路徑規(guī)劃算法對語義目標(biāo)導(dǎo)航任務(wù)的魯棒性。

1.3 基于學(xué)習(xí)的導(dǎo)航方法

經(jīng)典導(dǎo)航算法所使用的基于網(wǎng)格的地圖表示在精度和內(nèi)存需求上具有天然的矛盾，在動態(tài)環(huán)境中實(shí)時(shí)重新規(guī)劃路徑也需要大量計(jì)算。此外，經(jīng)典導(dǎo)航框架中從SLAM到路徑規(guī)劃，計(jì)算誤差會逐漸積累。為此，有研究將深度強(qiáng)化學(xué)習(xí)等引入導(dǎo)航中，從而得到了基于學(xué)習(xí)的導(dǎo)航方法[16]。大多數(shù)研究均使用深度Q網(wǎng)絡(luò)[17]、異步優(yōu)勢演員評論家算法[18]、近端策略優(yōu)化算法[19]、深度確定性策略梯度算法[20]等通用強(qiáng)化學(xué)習(xí)算法，但它們在狀態(tài)設(shè)計(jì)、獎勵函數(shù)設(shè)計(jì)、強(qiáng)化學(xué)習(xí)的使用方式等方面做了很多改進(jìn)。狀態(tài)設(shè)計(jì)涵蓋了起點(diǎn)、目標(biāo)點(diǎn)、障礙物位置等關(guān)鍵信息，與測試環(huán)境息息相關(guān)。若只考慮導(dǎo)航的核心任務(wù)（如避障和到達(dá)目標(biāo)點(diǎn)），獎勵會非常稀疏從而導(dǎo)致強(qiáng)化學(xué)習(xí)難以訓(xùn)練；所以多數(shù)研究引入了手工設(shè)計(jì)的中間獎勵函數(shù)，包括碰撞、與最近障礙物的距離變化、與目標(biāo)點(diǎn)的距離變化、時(shí)間步懲罰等。如何在導(dǎo)航中使用強(qiáng)化學(xué)習(xí)是基于學(xué)習(xí)的方法的核心問題，本文將使用方式分為3種。第1種是在導(dǎo)航中直接使用深度強(qiáng)化學(xué)習(xí)[21]，即將完整的導(dǎo)航過程描述為馬爾科夫過程，以傳感器的觀測數(shù)據(jù)作為狀態(tài)，直接學(xué)習(xí)到路徑或控制動作序列的映射；這種方法不再需要SLAM、全局地圖等，但在復(fù)雜環(huán)境中容易落入局部陷阱。第2種是與經(jīng)典導(dǎo)航技術(shù)相結(jié)合[22]，如先使用PRM對地圖進(jìn)行稀疏化，再在局部使用深度強(qiáng)化學(xué)習(xí)進(jìn)行路徑規(guī)劃。第3種是分層深度強(qiáng)化學(xué)習(xí)，即將路徑規(guī)劃劃分為靜態(tài)避障、動態(tài)避障、趨向目標(biāo)點(diǎn)等不同層次上的子任務(wù)，這主要是考慮到環(huán)境復(fù)雜度與狀態(tài)空間巨大。但現(xiàn)有算法面對語義目標(biāo)導(dǎo)航任務(wù)均存在探索效率低的問題，為此本文提出了基于路徑規(guī)劃特點(diǎn)的覆蓋范圍最大化算法。

2 基于路徑規(guī)劃特點(diǎn)的解決方案

2.1 語義目標(biāo)導(dǎo)航任務(wù)定義

在語義目標(biāo)導(dǎo)航任務(wù)中，智能體以隨機(jī)的位置和方向被初始化在一個(gè)未知地圖環(huán)境中，其目標(biāo)是找到特定類別的物體，如床、廁所等。智能體需要依靠提供的RGBD相機(jī)、深度相機(jī)、位置信息（GPS）和羅盤等傳感器實(shí)現(xiàn)導(dǎo)航，也就是說視覺觀察包括第一人稱的RGB圖和深度圖。動作空間是離散的，由行進(jìn)、左轉(zhuǎn)、右轉(zhuǎn)、停止共4個(gè)動作組成，行進(jìn)意味著向前移動0.25 m，左轉(zhuǎn)和右轉(zhuǎn)的幅度為30°。當(dāng)智能體認(rèn)為它已經(jīng)接近目標(biāo)對象時(shí)，需要采取“停止”操作；如果智能體采取“停止”操作且停止時(shí)與目標(biāo)物體的距離小于閾值1 m，則認(rèn)為該回合是導(dǎo)航成功的?？梢姵晒π枰瑫r(shí)滿足發(fā)出“停止”操作和與目標(biāo)物體足夠近2個(gè)條件。

2.2 基于語義建圖的探索框架

本文構(gòu)建了基于語義建圖的探索框架，將探索模塊、語義SLAM模塊、路徑規(guī)劃模塊等進(jìn)行解耦合，為不同模塊下不同算法的性能對比及模塊之間的重要性對比提供了方便公平的對比框架。語義SLAM模塊使用GPS、羅盤、RGBD圖像等傳感器數(shù)據(jù)構(gòu)建語義、障礙物等地圖；探索模塊根據(jù)構(gòu)建好的地圖預(yù)測目標(biāo)類別物體最有可能出現(xiàn)的位置從而得到長期目標(biāo)；路徑規(guī)劃模塊依據(jù)障礙物地圖、智能體位置和長期目標(biāo)規(guī)劃出一條可行路徑；軌跡跟蹤模塊依據(jù)路徑等輸出下一步智能體需要采取的動作。本文針對探索效率低問題，引入了基于路徑規(guī)劃特點(diǎn)的覆蓋范圍最大化算法；針對深度不精準(zhǔn)和語義分割效果差問題，引入了深度圖邊緣處理及語義點(diǎn)云地圖和二維地圖糾錯機(jī)制；針對沒有考慮語義目標(biāo)導(dǎo)航任務(wù)特點(diǎn)的問題，在路徑規(guī)劃模塊中引入了替代點(diǎn)機(jī)制和障礙物概率地圖。下面將詳細(xì)介紹各個(gè)模塊及對應(yīng)改進(jìn)。

2.3 基于路徑規(guī)劃特點(diǎn)的探索算法

探索模塊負(fù)責(zé)選取路徑規(guī)劃模塊的目標(biāo)點(diǎn)，該目標(biāo)點(diǎn)稱為長期目標(biāo)。長期目標(biāo)的選擇通常依據(jù)2點(diǎn)：1)到達(dá)長期目標(biāo)所需經(jīng)過區(qū)域多為未知以盡可能擴(kuò)大探索面積；2)長期目標(biāo)附近很可能存在目標(biāo)類別物體以盡快完成任務(wù)。以往工作[23-26]中長期目標(biāo)點(diǎn)的選擇通常使用強(qiáng)化學(xué)習(xí)來完成。以面向目標(biāo)的語義探索算法（goal oriented semantic exploration，SemExp）[23]為例，其將障礙物地圖、語義地圖作為狀態(tài)輸入，將與目標(biāo)類別距離變化和探索面積變化的加權(quán)和作為獎勵函數(shù)，將近端策略優(yōu)化（proximal policy optimization，PPO）[19]作為策略網(wǎng)絡(luò)進(jìn)行強(qiáng)化學(xué)習(xí)訓(xùn)練。

本文通過實(shí)驗(yàn)發(fā)現(xiàn)該方法存在探索效率低問題，結(jié)果如表1所示，數(shù)據(jù)集描述以及成功率、帶有路徑長度加權(quán)的成功率（success weighted by path length，SPL）等指標(biāo)說明詳見第3.2節(jié)。使用SemExp論文公布的訓(xùn)練好的網(wǎng)絡(luò)參數(shù)的成功率為25.8%，SPL為12.8%；隨機(jī)初始化的網(wǎng)絡(luò)參數(shù)成功率為25.3%，SPL為11.6%。二者指標(biāo)接近，說明強(qiáng)化學(xué)習(xí)訓(xùn)練對探索效率的提高幫助有限。將強(qiáng)化學(xué)習(xí)狀態(tài)輸入的語義地圖全部置零，成功率甚至達(dá)到了26.4%，SPL為12.2%。這更加突出了探索效率低的問題。

表1 SemExp不同設(shè)置在HM3DSem數(shù)據(jù)集下的指標(biāo)對比Table 1 Comparison in different settings of SemExp under HM3DSem test dataset%

為此，本文提出了基于路徑規(guī)劃特點(diǎn)的覆蓋范圍最大化算法。該算法將長期目標(biāo)設(shè)置在地圖的邊界處：

式中：psubgoal為長期目標(biāo)，(x,y,θ)為當(dāng)前智能體的位置和朝向，K為地圖尺寸。未探索區(qū)域被設(shè)置為無障礙物，路徑規(guī)劃會設(shè)計(jì)出一條涵蓋無障礙物和未探索區(qū)域的路線并在行進(jìn)中不斷更新地圖。當(dāng)前路徑在更新后地圖中不再可通行時(shí)，會重新規(guī)劃路線。

使用替代點(diǎn)的覆蓋范圍最大化算法：

輸入仿真器env，強(qiáng)制更新步數(shù)LMAX，局部規(guī)

劃器LPF，目標(biāo)檢測OD；

1) lstep=0，haveseen=false，subgoal=(0, 0)

2) obs=env.reset()

3) While not env.done:

4) lstep+=1

5) If lstep＞LMAX or LPF(subgoal)失敗:

6) 按2.3節(jié)公式計(jì)算subgoal

7) lstep=0

8) If not haveseen:

9) goal=OD(obs)

10) If goal is not None:

11) 按2.5節(jié)根據(jù)goal計(jì)算subgoal

12) haveseen=true

13) action=LPF(subgoal)

14) obs=env.step(action)

智能體不會再去訪問已經(jīng)探索過且沒有其他支路的區(qū)域，因?yàn)檫@些區(qū)域不可能再涵蓋到達(dá)目標(biāo)點(diǎn)的路線。也就是說，智能體進(jìn)入死胡同后會自動改變朝向從而發(fā)現(xiàn)其他未探索區(qū)域。這避免了在某個(gè)房間反復(fù)探索或是在2點(diǎn)間反復(fù)來回等低探索效率行為。本文設(shè)計(jì)的探索模塊利用路徑重新規(guī)劃的特點(diǎn)使智能體覆蓋范圍最大化，從而提高了語義目標(biāo)導(dǎo)航任務(wù)的探索效率。

2.4 邊緣處理與地圖糾錯

語義SLAM模塊負(fù)責(zé)地圖構(gòu)建，包括點(diǎn)云地圖、障礙物地圖、已探索區(qū)域地圖和語義地圖，具體描述如表2所示。點(diǎn)云地圖通過對RGBD圖像使用主動神經(jīng)SLAM（active neural SLAM，ANS）[27]方法得到。

表2 語義建圖模塊所建地圖含義描述Table 2 Description of maps created by semantic mapping module

語義地圖在語義目標(biāo)導(dǎo)航任務(wù)中起著重要作用，語義地圖是探索模塊的主要輸入。語義地圖的質(zhì)量直接決定了下游模塊的性能。以往語義目標(biāo)導(dǎo)航研究所使用的語義地圖建立方法一般是先對RGB圖像進(jìn)行語義分割得到分割圖，根據(jù)深度圖像得到三維點(diǎn)云，然后根據(jù)分割圖和深度圖像的對應(yīng)關(guān)系為每一個(gè)點(diǎn)標(biāo)注一個(gè)語義標(biāo)簽，最后對得到的帶有語義信息的三維點(diǎn)云在垂直維度上進(jìn)行求和得到二維語義地圖。使用每一幀的RGB圖像和深度圖像都能得到一幅二維語義地圖。不同時(shí)間下得到的二維語義地圖根據(jù)當(dāng)時(shí)智能體的GPS和羅盤信息被拼接起來，這樣就構(gòu)建出了供下游模塊使用的保存了全部所見幀信息的二維語義地圖。

深度不精準(zhǔn)和語義分割效果差為上述語義地圖構(gòu)建方法造成了嚴(yán)重問題。RGB圖像語義分割的些許差錯可能會導(dǎo)致該幀生成的二維語義地圖存在巨大錯誤，更嚴(yán)重的是二維語義地圖錯誤會隨著幀數(shù)增多而積累，最終導(dǎo)致拼接而成的二維語義地圖出現(xiàn)嚴(yán)重偏差甚至失效。

造成RGB圖像語義分割些許差錯會導(dǎo)致二維語義地圖產(chǎn)生巨大錯誤的根本來源是物體實(shí)例邊緣處深度值的不精準(zhǔn)。一方面，語義分割容易在物體實(shí)例邊緣處的像素點(diǎn)上出錯，將部分屬于其他物體的像素點(diǎn)標(biāo)注為該物體實(shí)例；另一方面，物體邊緣的深度值會發(fā)生跳變，在二維圖像中相鄰的2個(gè)點(diǎn)在實(shí)際的三維空間中可能相距很遠(yuǎn)。

圖1給出了一組深度圖像、RGB圖像及對RGB圖像進(jìn)行語義分割，其中語義分割算法實(shí)際使用的是掩膜區(qū)域卷積神經(jīng)網(wǎng)絡(luò)（mask regionbased convolutional neural network，Mask R-CNN）[28]實(shí)例分割算法。深度圖像的像素值表示與相機(jī)的遠(yuǎn)近，椅子和桌子后面的大片同像素區(qū)域超過了相機(jī)的最大測量距離；分割圖給出了椅子和桌子的語義信息。上段提到的物體實(shí)例邊緣處的2個(gè)不利方面均在該組圖中得到體現(xiàn)。以椅子腿為例，一方面，因?yàn)镽GB圖像的噪聲和實(shí)例分割算法的性能限制，部分屬于地面、桌子等的像素點(diǎn)被劃分為椅子腿，這些像素點(diǎn)將進(jìn)入椅子所對應(yīng)的語義地圖層；另一方面，椅子與后方墻壁雖然在RGB圖像和深度圖像上像素點(diǎn)相鄰，但其在三維空間中實(shí)際距離相差很遠(yuǎn)。這導(dǎo)致如果將部分墻壁像素點(diǎn)標(biāo)注為椅子的語義信息，那么三維點(diǎn)云地圖中與椅子相離很遠(yuǎn)的部分墻壁也將會被標(biāo)注為椅子，而智能體到達(dá)該部分墻壁時(shí)會認(rèn)為已找到椅子而采取停止動作?？梢?，在物體實(shí)例邊緣處對深度圖像和分割圖進(jìn)行處理是非常必要且重要的，但以往語義目標(biāo)導(dǎo)航研究沒有針對這一點(diǎn)進(jìn)行優(yōu)化。

圖1 一組深度圖像、RGB圖像與對應(yīng)的部分分割圖Fig.1 Depth image, RGB image, and corresponding partial segmentation image

為此，本文針對上述問題提出了3種基于形態(tài)學(xué)的方法以改進(jìn)語義SLAM模塊建圖的質(zhì)量。這3種方法分別應(yīng)用于語義SLAM模塊的3個(gè)步驟中，所以可以同時(shí)應(yīng)用。

1)對語義分割得到的分割圖進(jìn)行腐蝕。具體地，使用5×5的全1卷積核對分割圖進(jìn)行卷積，分割圖上像素值表示語義概率，如果一個(gè)像素點(diǎn)為概率1的特定語義點(diǎn)且位于語義物體內(nèi)部，則經(jīng)過卷積操作后的該點(diǎn)數(shù)值為25。同時(shí)考慮語義點(diǎn)位置和概率，保留卷積后數(shù)值大于23.5的點(diǎn)作為最終分割圖中的特定語義區(qū)域，用公式表示為

式中：MSemNew表示處理后的分割圖，MSem表示原始分割圖，15表示5×5的全1矩陣，*表示卷積運(yùn)算?？梢娺@會縮小分割圖中語義物體的面積從而減少語義分割在物體邊緣處出錯的概率，但不會影響語義目標(biāo)導(dǎo)航任務(wù)的性能，因?yàn)榻鉀Q語義目標(biāo)導(dǎo)航任務(wù)只需要知道有一個(gè)特定語義物體在該區(qū)域而無需知道物體的擺放細(xì)節(jié)。這也有助于去除語義分割模型輸出的異常孤立點(diǎn)。

2)對語義三維點(diǎn)云地圖進(jìn)行濾波，去除那些距同類語義物體距離遠(yuǎn)大于平均距離的語義點(diǎn)，從而確保去除掉由物體邊緣深度值跳變導(dǎo)致的部分線段狀點(diǎn)云，因?yàn)槲矬w邊緣形成的錯誤點(diǎn)云與其主體點(diǎn)云的距離很大。具體地，使用K維樹（K-dimensional tree，KD Tree）來計(jì)算每個(gè)語義點(diǎn)周圍0.1 m范圍內(nèi)相同語義點(diǎn)的數(shù)量，若數(shù)量大于5則保留該點(diǎn)，否則舍棄：

式中：MPcdNew表示處理后的點(diǎn)云，MPcd表示原始點(diǎn)云，dis是2點(diǎn)之間的距離函數(shù)，label是點(diǎn)對應(yīng)的語義標(biāo)簽函數(shù)，I是指示函數(shù)。

3)對拼接而成的語義二維地圖進(jìn)行開運(yùn)算。腐蝕操作類似于對分割圖的處理，會去除掉地圖中的孤立點(diǎn)，而膨脹操作恢復(fù)物體實(shí)例本來的尺寸并填充由于體素化而缺失的部分語義點(diǎn)。二維語義地圖開運(yùn)算保證了供下游模塊使用的語義地圖質(zhì)量以及與原地圖之間物體尺寸的匹配。

2.5 語義目標(biāo)替代點(diǎn)

以往語義目標(biāo)導(dǎo)航研究中使用的路徑規(guī)劃算法可分為2類：1)基于幾何的經(jīng)典路徑規(guī)劃算法，如Sethian等[29]所使用的快速行進(jìn)算法（fast marching method，F(xiàn)MM）；2)基于學(xué)習(xí)的路徑規(guī)劃算法，如Wijmans等[30]所使用的去中心化分布式近端策略優(yōu)化算法（decentralized distributed proximal policy optimization，DDPPO）。這些框架使用的均是標(biāo)準(zhǔn)路徑規(guī)劃算法，沒有將語義目標(biāo)導(dǎo)航的任務(wù)特點(diǎn)考慮進(jìn)去。為此，本文以語義目標(biāo)導(dǎo)航的任務(wù)特點(diǎn)為驅(qū)動對路徑規(guī)劃模塊作了以下幾點(diǎn)設(shè)計(jì)。

語義目標(biāo)導(dǎo)航任務(wù)的一個(gè)顯著特點(diǎn)是其長期目標(biāo)（也即路徑規(guī)劃的終點(diǎn)）可能設(shè)置在障礙物上。這是合理的。如床在智能體所構(gòu)建的障礙物地圖中屬于無法通過的障礙物，當(dāng)語義目標(biāo)類別設(shè)置為床時(shí)，標(biāo)準(zhǔn)的路徑規(guī)劃算法無法規(guī)劃出一條無障礙路徑，因?yàn)榻K點(diǎn)處于障礙物的包圍之中。以往語義目標(biāo)導(dǎo)航框架沒有顯式地考慮這一問題，而是簡單地把所有目標(biāo)語義點(diǎn)作為路徑規(guī)劃終點(diǎn)。這種方法能夠解決同一語義物體被作為障礙物的問題，但對于被其他障礙物所包圍的語義目標(biāo)而言是無效的。如放置在桌子上的盆栽和嵌入墻壁的電視，二維障礙物地圖中盆栽被桌子圍繞，電視被墻壁圍繞，導(dǎo)致從機(jī)器人當(dāng)前位置無法規(guī)劃出到任意一個(gè)語義目標(biāo)點(diǎn)的路徑。為解決該問題，本文提出了將語義目標(biāo)附近可通行點(diǎn)作為替代點(diǎn)的方法，即將位于語義目標(biāo)點(diǎn)周圍且能夠從智能體當(dāng)前位置規(guī)劃出一條路徑的點(diǎn)作為長期目標(biāo)。該方法解決了語義目標(biāo)被障礙物包圍而無法規(guī)劃路徑的問題。

求解替代點(diǎn)存在從最近替代點(diǎn)看不到目標(biāo)物體的問題。以放置在桌子上的盆栽為例，按照尋找替代點(diǎn)的思路，替代點(diǎn)將被設(shè)置在桌子的4條邊上，若桌子靠墻，則有一條邊將被設(shè)置在墻的另一側(cè)即另一間房間（因?yàn)闊o法從障礙物地圖中判斷從另一間房間是否能夠看到盆栽）。這就導(dǎo)致長期目標(biāo)有可能被設(shè)置在另一間房間而造成任務(wù)失敗。本文使用下述方法來尋找替代點(diǎn)以規(guī)避該問題：將發(fā)現(xiàn)語義目標(biāo)物體時(shí)的智能體位置與隨機(jī)一個(gè)語義目標(biāo)點(diǎn)進(jìn)行連線，將連線上最靠近語義目標(biāo)點(diǎn)的無障礙物點(diǎn)作為長期目標(biāo)：

式中：subgoal表示語義目標(biāo)替代點(diǎn)求解后的長期目標(biāo)，curpos表示智能體發(fā)現(xiàn)語義目標(biāo)時(shí)的位置，goal表示發(fā)現(xiàn)的語義目標(biāo)點(diǎn)位置。智能體在發(fā)現(xiàn)語義目標(biāo)物體時(shí)的位置肯定能夠看到語義目標(biāo)，所以二者連線是智能體能夠完全觀察的區(qū)域，能為其后路徑規(guī)劃提供更多圖像，并且從當(dāng)前位置到連線上點(diǎn)的可通行概率更大。

除了引入語義目標(biāo)驅(qū)動的替代點(diǎn)機(jī)制，本文還將更適用于實(shí)際的障礙物地圖表示方法帶入了語義目標(biāo)導(dǎo)航框架中。以往的語義目標(biāo)導(dǎo)航研究使用非0即1的障礙物地圖表示方式，這種表示方式可能導(dǎo)致智能體在空曠處選擇貼近障礙物的路徑而這些路徑實(shí)際無法通行，也可能導(dǎo)致在狹窄處穿越密集障礙物時(shí)路徑規(guī)劃失靈。以往的語義目標(biāo)導(dǎo)航研究也沒有考慮智能體的尺寸問題，只是把智能體作為一個(gè)質(zhì)點(diǎn)。按照質(zhì)點(diǎn)規(guī)劃出來的路徑往往難以通行。以往工作一般是構(gòu)建所謂的碰撞傳感器[23,27,31-34]以記錄實(shí)際不可通行的區(qū)域來解決智能體尺寸問題，但該方法在現(xiàn)實(shí)中是不可行的。為此，本文將障礙物概率地圖引入到了語義目標(biāo)導(dǎo)航任務(wù)中。本文使用9×9的全1矩陣作為卷積核對二維障礙物地圖進(jìn)行卷積，卷積后地圖中每個(gè)大于0的像素點(diǎn)均當(dāng)作障礙物像素點(diǎn)，從而得到障礙物概率地圖。這樣不僅將智能體尺寸引入到了障礙物地圖中，也顯式地表示了障礙物的密集程度。數(shù)值較大的區(qū)域表示位于障礙物中心無法通行，數(shù)值較小的區(qū)域由于深度圖像的誤差則有可能可以通行。這就給了路徑規(guī)劃模塊更大的選擇空間，同時(shí)也可以去除掉不符合實(shí)際的所謂“碰撞傳感器”。圖2給出了2種地圖表示形式的可視化效果。右圖表示概率地圖，每處障礙物的中心區(qū)域數(shù)值較大不可能通行，邊緣區(qū)域數(shù)值較小則有可能通行。智能體在無法找到全部數(shù)值為0的路徑時(shí)則會考慮數(shù)值較小的那些點(diǎn)。

圖2 障礙物地圖處理示例Fig.2 Example of obstacle map processing

最后，軌跡跟蹤模塊依據(jù)規(guī)劃出的路徑計(jì)算智能體下一步需要采取的動作。因?yàn)樵撊蝿?wù)中行進(jìn)、左轉(zhuǎn)、右轉(zhuǎn)的幅度都是固定的，所以只需要將智能體轉(zhuǎn)向下一個(gè)路徑中間點(diǎn)并朝其前進(jìn)即可。

3 實(shí)驗(yàn)設(shè)計(jì)與結(jié)果分析

3.1 數(shù)據(jù)集與仿真器

本文使用HM3DSem (habitat-matterport 3D semantics dataset)數(shù)據(jù)集。HM3DSem是迄今為止最大的三維真實(shí)世界和室內(nèi)空間數(shù)據(jù)集，具有密集的語義注釋。它包含142 646個(gè)對象實(shí)例注釋，216個(gè)3D空間及這些空間中的3 100個(gè)房間。場景使用142 646個(gè)原始對象名稱進(jìn)行注釋并被映射到40個(gè)Matterport類別中。每個(gè)場景平均包含106個(gè)類別和661件物品。圖3給出了HM3DSem某個(gè)場景的預(yù)覽圖。

圖3 HM3DSem某個(gè)3D空間的預(yù)覽Fig.3 Preview image of one 3D space in HM3DSem

本文選用Habitat[35-36]作為仿真器。Habitat仿真器會加載HM3Dsem數(shù)據(jù)集及語義目標(biāo)導(dǎo)航任務(wù)數(shù)據(jù)集（包括對智能體初始化位姿、語義目標(biāo)類別等的定義），提供了數(shù)據(jù)集和用戶代碼的交互接口（如對傳感器數(shù)據(jù)的訪問、對智能體運(yùn)動的控制、返回任務(wù)的執(zhí)行結(jié)果等）。在本文的實(shí)驗(yàn)環(huán)境中，智能體能夠獲得的信息有RGBD相機(jī)拍攝的第1人稱RGB圖像和深度圖像、跟蹤相機(jī)位置的里程計(jì)信息；智能體的動作空間為前進(jìn)0.25 m、左轉(zhuǎn)30°、右轉(zhuǎn)30°、停止。

3.2 評價(jià)指標(biāo)及基準(zhǔn)算法

本文使用成功率（success rate，SR）、帶有路徑長度加權(quán)的成功率（success weighted by path length，SPL）、發(fā)出停止動作時(shí)與目標(biāo)的距離（distance to goal，DTG）3個(gè)評價(jià)指標(biāo)。指標(biāo)定義為

式中：N表示回合數(shù)量，Si表示第i回合是否成功，li表示第i回合能夠完成任務(wù)的最短路徑長度，pi表示第i回合實(shí)際采取路徑的長度，di表示第i回合發(fā)出停止動作時(shí)智能體與語義目標(biāo)的距離。可見，SR和SPL越大越好，DTG越小越好。SPL綜合考慮了成功率和效率。按照Anderson等[1]的建議，本文將SPL作為主要指標(biāo)，SR和DTG作為輔助指標(biāo)。

為了評估模型效果，本文選取了前沿探索（frontier-based exploration，F(xiàn)rontierExp）[37]、DDPPO[30]、SemExp[23]、對人類經(jīng)驗(yàn)進(jìn)行模仿學(xué)習(xí)的算法（HabitatWeb）[38]共4種基準(zhǔn)模型作為對比。下面是對基準(zhǔn)模型的簡要介紹。

FrontierExp選擇離自身位置最近且可通行的frontier（即未訪問區(qū)域和已訪問區(qū)域的交界）作為長期目標(biāo)。

DDPPO不進(jìn)行建圖，使用PPO直接學(xué)習(xí)傳感器數(shù)據(jù)到控制動作的策略。DDPPO在PPO基礎(chǔ)上實(shí)現(xiàn)了一種大規(guī)模強(qiáng)化學(xué)習(xí)訓(xùn)練框架。本文使用DDPPO提供的模型參數(shù)進(jìn)行測試，其使用64塊Tesla V100 GPU進(jìn)行了3 d訓(xùn)練，消耗了25億幀樣本?？梢?，DDPPO所需要的訓(xùn)練樣本、訓(xùn)練時(shí)間和硬件資源都是巨大的。

SemExp由語義建圖模塊、強(qiáng)化學(xué)習(xí)長期策略選擇和基于強(qiáng)化學(xué)習(xí)長期策略選擇和基于快速行進(jìn)法（fast marching method，F(xiàn)MM）的局部路徑規(guī)劃算法組成。本文所提系統(tǒng)框架正是對SemExp各個(gè)模塊進(jìn)行了解耦，依據(jù)測試中發(fā)現(xiàn)的問題進(jìn)行了相應(yīng)改進(jìn)。

HabitatWeb收集了大量人類完成語義目標(biāo)導(dǎo)航任務(wù)的示例并使用行為克隆對人類示例進(jìn)行模仿學(xué)習(xí)。

3.3 與基準(zhǔn)算法比較

圖4以實(shí)驗(yàn)示意圖的形式給出了本文所提的系統(tǒng)框架。本文在HM3DSem測試數(shù)據(jù)集下進(jìn)行測試。HM3DSem 測試集共有2 000個(gè)測試回合，每回合任務(wù)必須在500步內(nèi)完成。語義目標(biāo)類別有椅子、床、植物、馬桶、電視、沙發(fā)共6類。表3給出了本文所提框架與上述基準(zhǔn)算法在HM3DSem 測試集下6個(gè)類別及平均的實(shí)驗(yàn)結(jié)果。

圖4 語義目標(biāo)導(dǎo)航系統(tǒng)框架Fig.4 Object goal navigation system framework diagram

表3 HM3DSem 測試數(shù)據(jù)集下語義目標(biāo)導(dǎo)航結(jié)果Table 3 Object goal navigation results under HM3DSem test dataset

從表3可以看出，在使用SPL指標(biāo)的測評中，本文所提框架取得了優(yōu)于其他所有模型的效果（0.236），比其他最好的HabitatWeb（0.193）高出0.043，提高了22.3%。在椅子、植物、馬桶、電視、沙發(fā)這5個(gè)類上本文所提框架的SPL指標(biāo)均最高，僅床類下HabitatWeb的SPL指標(biāo)高于本文所提框架。這說明本文所提框架著實(shí)提高了語義目標(biāo)導(dǎo)航任務(wù)的性能。在總體成功率方面，Habitat-Web（0.491）高于本文所提框架（0.441）；本文框架在植物、馬桶、電視這3個(gè)類上成功率最高，HabitatWeb在椅子、床、沙發(fā)這3個(gè)類上成功率最高。高成功率低SPL說明HabitatWeb未能兼顧好成功率和效率，在提高成功率的同時(shí)極大地犧牲了效率。因?yàn)槟繕?biāo)導(dǎo)航任務(wù)所涉及的場景復(fù)雜多變，人類示例很難窮盡所有情況，這導(dǎo)致HabitatWeb進(jìn)行模仿學(xué)習(xí)時(shí)會遇到無法處理的狀態(tài)而在同一片區(qū)域反復(fù)游蕩，導(dǎo)致HabitatWeb完成任務(wù)的路徑長度極度增加。而本文框架提出了基于路徑規(guī)劃特點(diǎn)的覆蓋范圍最大化算法，避免了在同一片區(qū)域反復(fù)探索或是在2點(diǎn)間反復(fù)來回等低效率行為，使得智能體覆蓋范圍最大化，從而提高了語義目標(biāo)導(dǎo)航任務(wù)的探索效率，使得本文所提框架成功率在低于HabitatWeb的同時(shí)SPL指標(biāo)卻高于它。從以上分析可以看出，從兼顧成功率和效率的SPL指標(biāo)來看，本文框架超過了以往的各個(gè)方法；從成功率來看，本文框架超過了Frontier-Exp、DDPPO、SemExp，略低于HabitatWeb。不過HabitatWeb需要大量的人類示例，耗費(fèi)大量的時(shí)間和資金。本文框架無需昂貴的數(shù)據(jù)收集過程和巨大的訓(xùn)練時(shí)間。

從表3還可以看到，幾乎所有方法在椅子、床、沙發(fā)、馬桶這4個(gè)類上的表現(xiàn)優(yōu)于在植物、電視這2個(gè)類上的表現(xiàn)。如DDPPO在椅子類上SPL有0.198，而在植物類上SPL只有0.006；HabitatWeb在床類上SPL有0.255，而在電視類上只有0.078。本文分析認(rèn)為造成這種巨大差異來源于不同類別的實(shí)例分割效果差異。為了驗(yàn)證該想法，本文在HM3DSem測試數(shù)據(jù)集中隨機(jī)采樣了7萬張圖片（其中包含17.7萬個(gè)實(shí)例）并制作了實(shí)例分割數(shù)據(jù)集（如圖5所示），使用Mask R-CNN算法對其進(jìn)行了測試，測試結(jié)果如表4所示。從表4中可以看出，植物和電視的邊界框平均精度指標(biāo)（bounding box average precision，bbox AP）分別為12.10和7.94，遠(yuǎn)低于其他物體類別。語義信息的不準(zhǔn)確限制了植物和電視類的語義目標(biāo)導(dǎo)航性能。另外，雖然本文框架在植物和電視類上的表現(xiàn)不如其他類別，但本文框架不同類別之間的差距低于其他算法。這說明本文框架對語義信息的容錯率更高，更適合應(yīng)用到復(fù)雜的現(xiàn)實(shí)任務(wù)中去。

圖5 實(shí)例分割數(shù)據(jù)集可視化效果Fig.5 Visualization of instance segmentation dataset

表4 Mask R-CNN在HM3DSem各個(gè)物體類別下的實(shí)例分割表現(xiàn)Table 4 Instance segmentation performance of Mask RCNN in various object categories of HM3DSem

3.4 消融實(shí)驗(yàn)

為了驗(yàn)證本文3個(gè)模塊的有效性，本文進(jìn)行了消融實(shí)驗(yàn)，結(jié)果如表5所示。其中去除深度圖處理表示語義建圖模塊不再進(jìn)行深度圖邊緣處理與語義點(diǎn)云地圖和二維地圖糾錯；去除最大化探索表示探索模塊不再使用覆蓋范圍最大化算法，更改成了基于Frontier的選擇方法；去除替代點(diǎn)機(jī)制表示路徑規(guī)劃模塊不再使用替代點(diǎn)機(jī)制和障礙物概率地圖。從表5可以看出，去掉深度圖處理、最大化探索、替代點(diǎn)機(jī)制后，SPL從0.236分別降為0.219、0.198、0.213，SR從0.441分別降為0.407、0.420、0.397。這說明本文所提3個(gè)改進(jìn)都不同程度地提高了語義目標(biāo)導(dǎo)航任務(wù)的性能。另外可以看出最大化探索對SPL指標(biāo)的提升貢獻(xiàn)最大，再次證明了其較高的探索效率；深度圖處理和替代點(diǎn)機(jī)制對成功率的貢獻(xiàn)較大，說明其解決了部分噪音問題。

表5 消融實(shí)驗(yàn)Table 5 Ablation experiment

3.5 現(xiàn)實(shí)世界實(shí)驗(yàn)

本文所提框架成功應(yīng)用到了絕影mini-lite四足機(jī)器人（如圖6所示）上去。該機(jī)器人使用了NVIDIA Xavier NX主機(jī)，裝備了16線激光雷達(dá)、Intel RealSense D435i相機(jī)等傳感器?，F(xiàn)實(shí)實(shí)驗(yàn)使用雷達(dá)進(jìn)行定位，建圖要求與語義目標(biāo)導(dǎo)航任務(wù)一致而未使用雷達(dá)。

圖6 現(xiàn)實(shí)實(shí)驗(yàn)所用的四足機(jī)器人Fig.6 Quadruped robot used in real-world experiments

圖7給出了馬桶為語義目標(biāo)類別的一個(gè)回合所錄制視頻的部分截圖。從左到右依次展示了機(jī)器人開始運(yùn)動、繞開障礙物、探索臥室、發(fā)現(xiàn)臥室沒有目標(biāo)物體后繼續(xù)向前探索、發(fā)現(xiàn)并靠近馬桶后停止。四足機(jī)器人實(shí)驗(yàn)證明了本文框架對現(xiàn)實(shí)世界具有泛化性。

圖7 馬桶為語義目標(biāo)類別的導(dǎo)航視頻截圖Fig.7 Screenshot of navigation video with toilet as semantic goal

4 結(jié)束語

語義目標(biāo)導(dǎo)航任務(wù)研究對于將機(jī)器學(xué)習(xí)成果應(yīng)用到實(shí)際機(jī)器人中有著重要意義。本文構(gòu)建了一個(gè)包含語義SLAM模塊、探索模塊、路徑規(guī)劃模塊的系統(tǒng)框架。針對探索效率低的問題，在探索模塊中引入了基于路徑規(guī)劃特點(diǎn)的覆蓋范圍最大化算法；針對深度不精準(zhǔn)問題，在語義SLAM模塊中引入了深度圖邊緣處理與地圖糾錯機(jī)制；針對路徑規(guī)劃沒有考慮語義目標(biāo)導(dǎo)航任務(wù)特點(diǎn)的問題，在路徑規(guī)劃模塊中引入了替代點(diǎn)機(jī)制和障礙物概率地圖。本文在HM3DSem數(shù)據(jù)集下進(jìn)行了實(shí)驗(yàn)并應(yīng)用到了實(shí)際四足機(jī)器人上。實(shí)驗(yàn)結(jié)果表明本文所提的最大化探索提升了語義目標(biāo)導(dǎo)航的探索效率，深度圖處理和替代點(diǎn)機(jī)制解決了部分噪音問題從而提升了成功率。本文所提3個(gè)改進(jìn)明顯提高了語義目標(biāo)導(dǎo)航任務(wù)的性能并對現(xiàn)實(shí)世界具有良好的泛化性。語義目標(biāo)導(dǎo)航除了應(yīng)用于室內(nèi)場景，還能應(yīng)用于隧道、洞穴、城市等室外場景下的巡檢、勘探、安防等任務(wù)。與室內(nèi)場景中無需考慮機(jī)器人運(yùn)動特性不同，在擁有復(fù)雜地形的室外場景中進(jìn)行語義目標(biāo)導(dǎo)航需要結(jié)合機(jī)器人特點(diǎn)。為此，下一步將研究機(jī)器人操作和語義目標(biāo)導(dǎo)航的融合應(yīng)用。