滿恂鈺 劉元盛 齊含 嚴(yán)超 楊茹錦

（1.北京聯(lián)合大學(xué)，北京市信息服務(wù)工程重點(diǎn)實(shí)驗(yàn)室，北京 100101；2.北京聯(lián)合大學(xué)，機(jī)器人學(xué)院，北京 100101；3.北京聯(lián)合大學(xué)，智慧城市學(xué)院，北京 100101）

主題詞：自主導(dǎo)航探索 長短期記憶網(wǎng)絡(luò) 深度強(qiáng)化學(xué)習(xí) 地圖構(gòu)建

1 前言

具備自主移動功能的無人車[1]可用于災(zāi)害救援[2]、地下勘探[3-4]、行星探索[5]及建筑作業(yè)[6]等場景探索未知環(huán)境。自主導(dǎo)航探索與地圖構(gòu)建的任務(wù)涉及環(huán)境感知、同步定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）和路徑規(guī)劃等多個領(lǐng)域的結(jié)合應(yīng)用[7]。盡管自主導(dǎo)航探索技術(shù)已經(jīng)取得長足發(fā)展，但面對長時段、大規(guī)模以及復(fù)雜環(huán)境等任務(wù)場景時依然面臨導(dǎo)航定位丟失、探索區(qū)域不全、計算效率過低等挑戰(zhàn)。常見自主導(dǎo)航探索問題的解決方法可分為基于邊界的探索方法、基于采樣的探索方法以及基于學(xué)習(xí)的探索方法。邊界法的思想是尋找已知空間和未知空間之間的所有邊界，隨機(jī)選擇其中一個邊界作為下一個需要探索的目標(biāo)區(qū)域。Yamauchi等[8]開創(chuàng)了邊界法，使無人車不斷朝距離當(dāng)前位置最近的邊界移動，隨著無人車的移動，邊界不斷更新。基于邊界的方法采用貪婪策略引導(dǎo)無人車探索，在大規(guī)模復(fù)雜環(huán)境中會導(dǎo)致探索效率低下。

基于采樣的方法通過隨機(jī)采樣一些可視點(diǎn)，設(shè)計增益函數(shù)選擇能探索到更多未知空間的視點(diǎn)組并執(zhí)行導(dǎo)航探索。常見算法包括快速擴(kuò)展隨機(jī)樹（Rapidlyexploring Random Tree，RRT）算法[9]、快速擴(kuò)展隨機(jī)圖（Rapidly-exploring Random Graph，RRG）算法[10]、下一步最佳視圖規(guī)劃（Next Best View Planning，NBVP）算法[11]、分層和圖的探索規(guī)劃器（Graph-Based Path planning，GBP）[12]、運(yùn)動圖元的探索規(guī)劃器（Motion primitives-Based Path planning，MBP）[13]和無人機(jī)自主探索（Technologies for Autonomous Robot Exploration，TARE）算法[14]等。然而，基于采樣的算法本質(zhì)屬于貪心算法，易導(dǎo)致無人車陷入局部區(qū)域而無法完成完整環(huán)境探索。

基于學(xué)習(xí)的方法可直接從環(huán)境中學(xué)習(xí)而無需人工干預(yù)。Wang 等[15]提出改進(jìn)TARE局部探索算法，引入注意力機(jī)制學(xué)習(xí)局部地圖區(qū)域間多尺度依賴關(guān)系，但該方法未考慮長期增益，易導(dǎo)致無人車重復(fù)進(jìn)入已探索空間。

本文對自主導(dǎo)航探索任務(wù)進(jìn)行重新思考，設(shè)計考慮歷史信息的深度強(qiáng)化學(xué)習(xí)框架，選擇最優(yōu)目標(biāo)點(diǎn)以解決無人車探索過程中陷入局部區(qū)域的問題，將無人車水平移動因素作為約束條件以獲得一條最短全局探索路徑。最終，將本文方法在仿真和真實(shí)環(huán)境中進(jìn)行驗(yàn)證。

2 融合采樣與深度強(qiáng)化學(xué)習(xí)的探索算法

本文基于激光雷達(dá)和慣性導(dǎo)航單元融合的優(yōu)化正態(tài)分布變換（OPtimization Normal Distributions Transform，OP-NDT）[16]算法提供實(shí)時點(diǎn)云和狀態(tài)估計，提出基于采樣與深度強(qiáng)化學(xué)習(xí)融合的探索（Exploration based on Sampling and Deep Reinforcement Learning，ESDRL）算法完成自主導(dǎo)航探索任務(wù)，設(shè)計含歷史信息的視點(diǎn)選擇軟演員評論家網(wǎng)絡(luò)（Viewpoint Selection network with Historical information Soft Actor-Critic，VSH-SAC）解決傳統(tǒng)方法局部探索時無法存儲和利用歷史導(dǎo)航信息問題，引入長短期記憶（Long Short-Term Memory，LSTM）網(wǎng)絡(luò)優(yōu)化下一步視點(diǎn)選擇，并采用軟演員-評論家（Soft Actor-Critic，SAC）算法[17]對選擇的目標(biāo)點(diǎn)進(jìn)行評判以輸出最優(yōu)策略。將當(dāng)前車輛所在局部空間之外的歷史未探索區(qū)域記錄為多個全局未探索空間，利用采樣算法得到全局未探索空間質(zhì)心，由A*算法生成全局子空間距離矩陣，并結(jié)合無人車當(dāng)前位姿計算水平移動成本，通過解非對稱旅行商問題（Asymmetric Traveling Salesman Problem，ATSP）得到一條串聯(lián)所有未探索空間的最短路徑。圖1所示為ESDRL算法的整體框架。

定義S?R3為全局需探索區(qū)域，隨著探索的進(jìn)行，全局區(qū)域可分為已探索子空間集合Sk、未探索子空間集合Su和正在探索的子空間Sv，當(dāng)不存在正在探索的子空間時探索結(jié)束，此時S=Sk∪Su。設(shè)當(dāng)前位置激光雷達(dá)掃描的范圍為F，以固定大小20 m×20 m 作為可覆蓋面Fc，其余范圍為不可覆蓋面。定義激光雷達(dá)傳感器采集視點(diǎn)由集合V=[Pv,Qv]表示，其中Pv和Qv分別為視點(diǎn)位置信息和姿態(tài)信息，可覆蓋面內(nèi)視點(diǎn)Vc設(shè)置為已訪問視點(diǎn)。局部子空間內(nèi)無人車的目標(biāo)是根據(jù)采樣視點(diǎn)集合實(shí)時找到最短的無碰撞軌跡并覆蓋，當(dāng)前正在探索的子空間不存在時即完成局部區(qū)域探索：

式中，Гt為所有采樣視點(diǎn)構(gòu)成的路徑矩陣；C函數(shù)計算路徑采樣點(diǎn)間歐式距離并求和，從而得到每條路徑長度。

以構(gòu)成最短路徑的視點(diǎn)集作為控制點(diǎn)，經(jīng)過B樣條曲線（B-Spline Curve）平滑后處理獲得最終可行駛局部路徑。

無人車探索過程中遇到分叉路口，未被選擇的分叉路段記為未探索空間，如圖2所示。定義所有未探索子空間質(zhì)心集合為={g0,g1,g2,...,gn}，其中g(shù)0為初始無人車所在的未探索子空間質(zhì)心，n為未探索子空間的數(shù)量，根據(jù)探索任務(wù)的進(jìn)行，不斷更新。全局探索問題定義為找到一條全局路徑，該路徑穿過當(dāng)前無人車所在視點(diǎn)位置中的每個子空間的質(zhì)心。

圖2 全局探索子空間

2.1 VSH-SAC局部探索算法

本文根據(jù)無人車在t時刻激光雷達(dá)采集的視點(diǎn)集兩兩相連，構(gòu)建無碰撞圖Gt=(Vt,Et)，其中Pvkt、Qvtk分別為t時刻下第k個視點(diǎn)的位置和姿態(tài)，Et為一組可遍歷的邊。之后將Gt作為VSH-SAC算法模型的輸入。VSH-SAC算法模型由1個編碼器和1個解碼器組成：編碼器利用自注意力機(jī)制從當(dāng)前局部子空間采樣視點(diǎn)構(gòu)成的圖中提取特征；基于提取的特征、無人車當(dāng)前位置以及通過LSTM存儲的上一時刻無人車位姿信息，解碼器輸出關(guān)于相鄰視點(diǎn)的策略，該策略可用于決定下一步訪問的視點(diǎn)。本文使用SAC 算法來訓(xùn)練策略網(wǎng)絡(luò)，其中評論家（Critic）網(wǎng)絡(luò)用于預(yù)測狀態(tài)-行動值。圖3 所示為VSH-SAC 模型整體結(jié)構(gòu)。

圖3 VSH-SAC模型整體結(jié)構(gòu)

編碼器由多個堆疊注意層組成，首先將Gt中視點(diǎn)嵌入d維視點(diǎn)特征中，并計算所有相鄰視點(diǎn)邊緣掩碼，其中vi、vj為選擇的2 個視點(diǎn)，i、j為選擇的視點(diǎn)編號，所有邊緣掩碼組成邊緣掩碼矩陣M。將視點(diǎn)特征傳遞給注意層，每個注意層都以前一個注意層的輸出作為輸入。應(yīng)用邊緣掩碼矩陣M限制每個視點(diǎn)只能訪問其相鄰視點(diǎn)的特征。解碼器引入LSTM 結(jié)構(gòu)實(shí)現(xiàn)歷史視點(diǎn)信息利用，以選擇出符合無人車運(yùn)動學(xué)約束的下一目標(biāo)點(diǎn)。設(shè)當(dāng)前為t時刻，上一時刻的視點(diǎn)與當(dāng)前無人車所在位置Pt和姿態(tài)Qt的變化共同影響對下一視點(diǎn)的選擇，使SAC能選擇出在一條相對平滑路徑上且轉(zhuǎn)角較小的下一目標(biāo)視點(diǎn)。無人車在t時刻的觀察狀態(tài)表示為ot=(Gt,Pt)，VSH-SAC 模型輸出一個隨機(jī)策略：

式中，θ為神經(jīng)網(wǎng)絡(luò)的權(quán)值集合；at為輸出的動作。

式中，ws、wp、wa為子獎勵函數(shù)對總獎勵函數(shù)的影響系數(shù)。

綜上，可得SAC最優(yōu)策略公式為：

式中，T為決策步驟數(shù)量，從t=0開始進(jìn)行訓(xùn)練；γ為折扣因子；α為正則化系數(shù)，用于控制策略熵H的重要程度；π為VSH-SAC模型輸出的隨機(jī)策略；E(ot,at)為求期望，旨在考慮動作分布的不確定性。

2.2 引入ATSP的全局探索算法

待探索子空間序列的生成對探索任務(wù)最終能覆蓋的最大區(qū)域以及探索效率至關(guān)重要。多數(shù)方法采用傳統(tǒng)的最近鄰點(diǎn)策略生成探索序列，每次取當(dāng)前位置與未探索空間質(zhì)心的歐氏距離最小的空間作為下一個待探索空間。旅行商問題（Traveling Salesman Problem，TSP）的傳統(tǒng)求解方法雖然算法簡單，但本質(zhì)上是一種貪心算法，在大規(guī)模環(huán)境探索時效率低下。本文采用解ATSP方法生成待探索子空間序列，同時考慮與無人車當(dāng)前坐標(biāo)位置距離最短、與無人車當(dāng)前車身朝向最接近以及需要無人車橫、縱向速度變化最小的未探索空間作為下一個待探索子空間。具體地說，首先利用A*算法計算未探索子空間質(zhì)心間的最短路徑并生成距離矩陣D，然后將無人車水平移動因素，包括歐氏距離、偏航角變化、待探索空間與無人車當(dāng)前所在位置之間的速度方向變化作為成本，最后通過解ATSP生成更好的全局探索序列：

式中，ca(gi)為當(dāng)前無人車所在子空間質(zhì)心位置go與第i個未探索子空間質(zhì)心位置gi之間位置的成本函數(shù)；Dmin(gi)為由A*算法生成的與第i個未探索子空間之間的最短路徑。

同時考慮無人車當(dāng)前速度vo和偏航方向?qū)ο乱粋€需探索區(qū)域的影響，設(shè)計成本函數(shù)cb(gi)和cc(go,gi)：

式中，Po為當(dāng)前無人車所在位置；L(gi,Po)為gi、Po之間的歐氏距離；Vmax為無人車可達(dá)到的最大速度；Qi、Qo分別為第i個未探索子空間質(zhì)心偏航方向（生成未探索子空間時的無人車朝向）和當(dāng)前無人車偏航方向；Qmax為無人車最大偏航角度。

根據(jù)式（6）計算得出無人車當(dāng)前所在位置與下一個待探索子空間位置受無人車速度約束的成本函數(shù)，使得規(guī)劃的下一個待探索子空間橫、縱向速度變化最小。最終，得到解ATSP的成本矩陣：

式中，wb、wa為2個成本權(quán)重系數(shù)。

根據(jù)成本矩陣可得到一條串連所有未探索子空間的全局路徑。

3 仿真與試驗(yàn)驗(yàn)證結(jié)果與分析

3.1 驗(yàn)證設(shè)施及環(huán)境

本文通過Gazebo 仿真平臺進(jìn)行仿真測試并利用搭載激光雷達(dá)及慣性導(dǎo)航設(shè)備的履帶式差速運(yùn)動無人車進(jìn)行實(shí)車測試，仿真和試驗(yàn)平臺如圖4所示。仿真環(huán)境采用文獻(xiàn)[14]中搭建的礦道模型，長約為280 m，寬約200 m，可行駛道路總面積約為16 000 m2。

圖4 仿真和試驗(yàn)平臺

VSH-SAC 模型訓(xùn)練環(huán)境為Ubuntu 20.04 系統(tǒng)，硬件平臺采用AMD Ryzen 7 5800H 處理器、16 GB 內(nèi)存和GeForce RTX 3060 顯卡。仿真訓(xùn)練環(huán)境為采用Gazebo搭建的礦道環(huán)境。由于探索環(huán)境規(guī)模較大，在網(wǎng)絡(luò)訓(xùn)練時，設(shè)置最大決策步驟數(shù)T=1 000 000、折扣因子γ=1。使用Adam優(yōu)化器，設(shè)置學(xué)習(xí)率為0.000 2。真實(shí)場景試驗(yàn)測試設(shè)備為基于Ubuntu 和機(jī)器人操作系統(tǒng)（Robot Operating System，ROS）的車載CPU工控機(jī)設(shè)備，其CPU型號為i5-7260U。

3.2 評價指標(biāo)

自主導(dǎo)航探索與建圖關(guān)注探索的效率和覆蓋范圍，本文采用文獻(xiàn)[14]中提出的探索時間和探索區(qū)域面積曲線、探索時間和無人車行駛距離作為基本指標(biāo)，同時，針對不同算法對比單次運(yùn)行時間和探索效率。在探索時間越短且無人車行駛路程越短的情況下能夠獲得更多的探索區(qū)域是自主導(dǎo)航探索領(lǐng)域追求的更優(yōu)結(jié)果。

3.3 對比仿真與結(jié)果分析

為驗(yàn)證本文算法的效果，人為控制探索時間分別為1 000 s和2 000 s，從而比較本文方法與傳統(tǒng)的采樣方法（NBVP、GBP、TARE）在Gazebo 搭建的礦道仿真場景內(nèi)探索面積和行駛距離等的差異，仿真結(jié)果如表1 所示。在相同探索時間條件下，本文提出的ESDRL 算法能在行駛較少路程的情況下探索更多的未知區(qū)域。盡管深度強(qiáng)化學(xué)習(xí)方法相較于傳統(tǒng)方法少量增加了單步的運(yùn)算時間，但加入視點(diǎn)選擇獎懲機(jī)制使得無人車在局部探索過程中避免了大角度轉(zhuǎn)向和多次向已探索區(qū)域的運(yùn)動，從而提高了整體探索效率。

圖5 展示了不同算法在該礦道仿真環(huán)境下的探索效果，無人車重復(fù)經(jīng)過的區(qū)域探索面積不被統(tǒng)計，即圖5 中曲線水平部分代表無人車此刻處于已探索過的區(qū)域。值得注意的是，由于礦道環(huán)境的復(fù)雜性，存在許多死胡同和道路交叉口，無人車進(jìn)入這些死胡同或交叉口時需折返探索或選擇探索方向。由圖5可知：本文方法和TARE方法的探索面積遠(yuǎn)超GBP和NBVP方法；本文方法在第750～900 s和第1 250～1 600 s處進(jìn)行掉頭折返探索，無效探索時間約500 s，TARE 方法在第500～750 s、第900～1 300 s、第1 750～2 000 s 處進(jìn)行掉頭折返探索，無效探索時間約900 s，因此本文方法相較于TARE方法重復(fù)經(jīng)過的路段更少。此外，本文方法和TARE方法在不同時間點(diǎn)（第500 s、第750 s和第1 500 s）下探索時間與探索面積曲線的差異，體現(xiàn)了不同算法計算得到的下一目標(biāo)點(diǎn)的不同，從而引導(dǎo)無人車在交叉路口處駛?cè)肓瞬煌牡V道區(qū)域。

圖5 不同算法在仿真礦道環(huán)境下探索時間與探索面積對比

綜上，本文所提出的ESDRL 方法相較于傳統(tǒng)方法在相同的探索時間內(nèi)能夠在行駛較少路程的情況下探索更多的未知區(qū)域，具備更高的探索效率和準(zhǔn)確性。

在礦道仿真環(huán)境探索中，ESDRL 方法成功探索礦道未知區(qū)域并建立了三維點(diǎn)云鳥瞰地圖，如圖6 所示。此外，將ESDRL方法建立的三維點(diǎn)云鳥瞰地圖與TARE方法建立的三維點(diǎn)云鳥瞰地圖進(jìn)行部分區(qū)域?qū)Ρ?，如圖7所示：在對比區(qū)域1中，本文方法在探索路徑上相較于TARE 方法更加平滑且沒有掉頭探索，而TARE 方法則折返從第2 條礦道進(jìn)行探索，增加了探索路程；對比區(qū)域2 中，本文方法對歷史探索信息進(jìn)行選擇，不進(jìn)入窄道繼續(xù)探索，引導(dǎo)無人車駛?cè)肫渌刺剿鲄^(qū)域，而TARE方法重復(fù)2次進(jìn)入窄道探索；對比區(qū)域3中，本文方法考慮歷史和當(dāng)前車身位姿，相對TARE方法較少出現(xiàn)大角度轉(zhuǎn)彎的情況，使得探索路徑更為平滑。

圖6 ESDRL算法探索仿真礦道環(huán)境建立三維點(diǎn)云鳥瞰圖及探索軌跡

圖7 ESDRL與TARE算法三維點(diǎn)云鳥瞰圖部分區(qū)域?qū)Ρ?/p>

通過仿真對比，驗(yàn)證了ESDRL 算法在大規(guī)模未知礦道仿真環(huán)境探索的可行性，證明了ESDRL 算法相較于目前最先進(jìn)的方法在探索效率和探索覆蓋率方面表現(xiàn)更為出色。

3.4 真實(shí)場景試驗(yàn)

真實(shí)試驗(yàn)環(huán)境是北京聯(lián)合大學(xué)北四環(huán)校區(qū)南門地下車庫場景，由于該環(huán)境之前未進(jìn)行建圖，因此可以歸類為未知場景，如圖8 所示。地下車庫面積約為3 444.3 m2。設(shè)定無人車最大速度v=0.5 m/s，局部探索區(qū)域大小設(shè)定為40 m×40 m×15 m，采用基于OP-NDT的激光SLAM 算法為無人車提供實(shí)時定位信息并同步建圖。將本文算法與目前最先進(jìn)的TARE 算法進(jìn)行實(shí)車探索對比，最終獲得的點(diǎn)云及軌跡路線如圖9所示。

圖8 北京聯(lián)合大學(xué)北四環(huán)校區(qū)南門地下車庫場景

圖9 2種算法探索獲得的地下車庫場景點(diǎn)云及探索軌跡

ESDRL 算法在1 014 s 內(nèi)完成3 444.3 m2的地下車庫探索并建立環(huán)境三維點(diǎn)云地圖。由于初始無人車位姿朝向圖9a 中正右方，因此無人車首先進(jìn)行地下車庫右半部分探索，之后折返進(jìn)行左半部分探索，最終回到起點(diǎn)位置。TARE算法總運(yùn)行時長1 500 s，有效探索面積約3 365.1 m2。TARE 算法出現(xiàn)陷入局部環(huán)境并重復(fù)繞圈的情況，最終折返至起點(diǎn)處，TARE 算法認(rèn)為不存在未探索區(qū)域而并未進(jìn)入地下車庫左半部分探索。對比試驗(yàn)結(jié)果表明，本文自主導(dǎo)航探索框架可部署于真實(shí)車輛進(jìn)行未知場景無碰撞探索，并能結(jié)合SLAM算法實(shí)時精確定位，建立未知環(huán)境的場景地圖，同時驗(yàn)證了本文所提出的算法在探索完成度和探索效率方面均優(yōu)于TARE算法。

4 結(jié)束語

本文針對自主導(dǎo)航探索算法易陷入局部區(qū)域的問題，提出了融合采樣與深度強(qiáng)化學(xué)習(xí)的ESDRL 探索算法。局部引入長短期記憶網(wǎng)絡(luò)獲得無人車歷史位姿信息進(jìn)而避免重復(fù)走向已探索區(qū)域。全局導(dǎo)航探索基于改進(jìn)傳統(tǒng)旅行商問題算法，以無人車水平移動因素作為成本，通過解非對稱旅行商問題生成更好的全局探索序列。仿真結(jié)果表明，本文方法有助于無人車在行駛途中盡可能減少大角度轉(zhuǎn)彎和重復(fù)探索已知區(qū)域的情況，對比傳統(tǒng)方法，在相同探索時間內(nèi)行駛的路程更短且能較多地探索未知空間。實(shí)車試驗(yàn)中，本文方法在短時間內(nèi)對地下車庫進(jìn)行了完整探索，證明了其可行性。

本文方法中采用了深度強(qiáng)化學(xué)習(xí)算法，該算法計算時間較長，導(dǎo)致性能過低的車載工控機(jī)設(shè)備在探索過程中出現(xiàn)了設(shè)備死機(jī)的情況。未來的工作將側(cè)重于將局部探索方法輕量化，在保證探索精度的同時提升探索效率。