未知環境下無人車自主導航探索與地圖構建*

滿恂鈺 劉元盛 齊含 嚴超 楊茹錦

（1.北京聯合大學，北京市信息服務工程重點實驗室，北京 100101；2.北京聯合大學，機器人學院，北京 100101；3.北京聯合大學，智慧城市學院，北京 100101）

主題詞：自主導航探索 長短期記憶網絡 深度強化學習 地圖構建

1 前言

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

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

基于學習的方法可直接從環境中學習而無需人工干預。Wang 等[15]提出改進TARE局部探索算法，引入注意力機制學習局部地圖區域間多尺度依賴關系，但該方法未考慮長期增益，易導致無人車重復進入已探索空間。

本文對自主導航探索任務進行重新思考，設計考慮歷史信息的深度強化學習框架，選擇最優目標點以解決無人車探索過程中陷入局部區域的問題，將無人車水平移動因素作為約束條件以獲得一條最短全局探索路徑。最終，將本文方法在仿真和真實環境中進行驗證。

2 融合采樣與深度強化學習的探索算法

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

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

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

以構成最短路徑的視點集作為控制點，經過B樣條曲線（B-Spline Curve）平滑后處理獲得最終可行駛局部路徑。

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

圖2 全局探索子空間

2.1 VSH-SAC局部探索算法

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

圖3 VSH-SAC模型整體結構

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

式中，θ為神經網絡的權值集合；at為輸出的動作。

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

綜上，可得SAC最優策略公式為：

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

2.2 引入ATSP的全局探索算法

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

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

同時考慮無人車當前速度vo和偏航方向對下一個需探索區域的影響，設計成本函數cb(gi)和cc(go,gi)：

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

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

式中，wb、wa為2個成本權重系數。

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

3 仿真與試驗驗證結果與分析

3.1 驗證設施及環境

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

圖4 仿真和試驗平臺

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

3.2 評價指標

自主導航探索與建圖關注探索的效率和覆蓋范圍，本文采用文獻[14]中提出的探索時間和探索區域面積曲線、探索時間和無人車行駛距離作為基本指標，同時，針對不同算法對比單次運行時間和探索效率。在探索時間越短且無人車行駛路程越短的情況下能夠獲得更多的探索區域是自主導航探索領域追求的更優結果。

3.3 對比仿真與結果分析

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

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

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

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

在礦道仿真環境探索中，ESDRL 方法成功探索礦道未知區域并建立了三維點云鳥瞰地圖，如圖6 所示。此外，將ESDRL方法建立的三維點云鳥瞰地圖與TARE方法建立的三維點云鳥瞰地圖進行部分區域對比，如圖7所示：在對比區域1中，本文方法在探索路徑上相較于TARE 方法更加平滑且沒有掉頭探索，而TARE 方法則折返從第2 條礦道進行探索，增加了探索路程；對比區域2 中，本文方法對歷史探索信息進行選擇，不進入窄道繼續探索，引導無人車駛入其他未探索區域，而TARE方法重復2次進入窄道探索；對比區域3中，本文方法考慮歷史和當前車身位姿，相對TARE方法較少出現大角度轉彎的情況，使得探索路徑更為平滑。

圖6 ESDRL算法探索仿真礦道環境建立三維點云鳥瞰圖及探索軌跡

圖7 ESDRL與TARE算法三維點云鳥瞰圖部分區域對比

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

3.4 真實場景試驗

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

圖8 北京聯合大學北四環校區南門地下車庫場景

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

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

4 結束語

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

本文方法中采用了深度強化學習算法，該算法計算時間較長，導致性能過低的車載工控機設備在探索過程中出現了設備死機的情況。未來的工作將側重于將局部探索方法輕量化，在保證探索精度的同時提升探索效率。