1 引 言

　　同時定位與建 圖 (Simultaneous Localization andMapping，SLAM) 技術自 1988 年被提出以來，主要用于研究移動機器人的智能化。目前配備了激光雷達、攝像頭、慣性導航模塊 (Inertial Measurement Unit，IMU) 等關鍵傳感設備的智能載體借助 SLAM 技術已經能滿足室內場景的自主導航任務。SLAM 技術是在未知環境中實現自主定位的關鍵技術， 而基于激光雷達同時定位與地圖構建 ( 激光 SLAM) 的技術廣泛應用在無人駕駛、室內外機器人導航及三維重建等領域。激光SLAM 之所以能作為無人駕駛定位方案中不可或缺的技術，其主要原因在于：(1) 激光 SLAM 無需對定位場景進行預先布置， 即能在未知的環境中進行定位服務;(2) 激光能準確測量障礙點的角度與距離;(3) 能在光線較差、光照變化較大的環境下工作;(4) 能夠生成便于導航的柵格地圖與直觀的點云地圖。

　　前谷歌無人車領導者 Sebastian Thrun 在其 2005 年出版的經典著作《概率機器人》一書中介紹了如何基于概率濾波的方法使用二維激光雷達進行地圖構建和定位。該書詳細闡述了從基于 RB 粒子濾波 (Rao-Blackwellised Particle Filter， RBPF) 的經典 FastSLAM 算法，逐步發展成為基于激光雷達建圖標準 GMapping 算法的過程 [4]。2016 年，Google 開源其優化的激光 SLAM 算法 Cartographer，該算法改進了GMapping 計算耗時且無法有效處理閉環的缺點，利用幀間點云地圖匹配與子地圖構建思想有效地彌補了 GMapping 的缺陷。

　　盡管激光 SLAM 技術可以在一定場景下滿足定位精度的要求，但是難以在全工況的自動駕駛場景中提供持續、穩定的高精度定位輸出。另外，由于定位場景的結構相似性且激光點云對環境的描述能力不足，容易發生 SLAM 的閉環檢測誤報(False Positives) 現象，從而降低系統定位與建圖性能。本研究旨在通過將地磁信號與激光 SLAM 的閉環檢測環節融合， 提出激光 SLAM 在應對結構相似性及激光強干擾場景下的定位與建圖任務時的創新解決方法。同時，本研究將此技術應用到無人駕駛的定位與建圖領域，實現了無人駕駛汽車全天候、全地域的精準基于矩陣補全的無人車定位，提升了無人駕駛汽車定位算法的精準性與穩定性。

　　2 相關研究進展

　　2.1 激光 SLAM 閉環檢測

　　近年來一些國外學者為解決閉環檢測誤報現象對 SLAM 系統的影響，提出了閉環驗證算法。Bosse 與 Roberts 的工作提出，利用熵序列之間的最大相關性之和與投影直方圖的驗證度量來估計閉環檢測的正確性，并在此基礎上提升了閉環檢測的正確率。Granstrom 等提出了基于變換后掃描并在最后配準的閉環檢測驗證標準。Corso 提出在室內環境中嚴格處理閉環驗證時，由于閉環的現象可以從多種傳感器的數據源中被檢測到，因此應將閉環驗證算法與閉環檢測算法分成兩個方向進行研究。此外，Gao 與 Harle 在已有報道的基礎上，提出將地磁序列信號的匹配定位方法用于矯正基于 PDR(Pedestrian Dead Reckoning) 的 SLAM 閉環檢測環節，以此來提高行人步態檢測與定位的精度。

　　(a)

　　(c) 閉環誤檢與錯誤定位 (d) 環境干擾中的錯誤建圖圖 1 常見閉環誤檢干擾及錯誤定位與建圖

　　2.2 閉環檢測模型

　　閉環檢測是指無人車經過一段較長行程并再次回到曾經歷過的位置時，對地圖中曾觀測到的特征進行二次觀測，同時通過兩次觀測的差異對全局地圖進行調整，以減少定位與建圖過程中的誤差累計。目前主要的閉環檢測方法包括：(1) 幀與幀閉環檢測;(2) 幀與子圖閉環檢測;(3) 子圖與子圖閉環檢測。以上閉環檢測算法都是采用相關性掃描匹配，通過旋轉矩陣 R 與平移向量 T 判斷兩幀激光點云的相似性。當激光雷達獲得新的掃描幀時，在其附近一定范圍搜索最優匹配幀，若該最優匹配幀符合要求，則認為是一個回環。該匹配問題可以描述為如下公式：

                                                                         (1)

　　其中，ξ 為匹配解算的剛性變換，ω 為搜索空間，k 為搜索空間的位姿數量，Mnearest 表示觀測激光點云 hk 和位置變換矩陣 Tξ 的乘積與地圖激光柵格點云最接近重合的置信度。對于每一幀激光點云插入子地圖上時的信度和來說，信度越高則認為越相似。在 ω 空間中尋找出該信度和最大的匹配幀，需要在 ω 空間中尋找出匹配結果的最優解。一種常見的方法是暴力匹配法，即在搜索空間范圍內，對每一幀與當前幀進行上式非線性最小二乘計算。匹配算法解算的剛性變換 ξ 與智能車初始的位姿相乘后便得出此時的位姿 (xi，yi，θi)。當匹配的兩幀之間存在對激光光束產生干擾的材質 ( 反射率過低的物體或對光束有透射作用的玻璃等 ) 時，場景中的這些干擾因素是激光點云匹配階段誤差累積的主要成分。其中，該累積誤差可以通過閉環檢測的方法進行消除。以下場景容易引起激光 SLAM 的閉環檢測誤檢現象：(1) 室內的長廊、空曠的室內大堂、結構布局相似的房間，如圖 1(a) 所示;(2) 場景中反射率過低的物體，如黑色吸光墻面、家具等;(3) 場景中存在玻璃或對激光光束產生透射現象的物體，如圖 1(b) 所示;(4) 場景中存在鏡子、不銹鋼等對激光光束產生鏡面反射的物體。

　　隨著地圖擴大，匹配搜索效率決定了閉環檢測的實時性。

　　當 SLAM 在應對較大場景和復雜干擾的環境時，實時定位與建圖對閉環檢測算法的效率提出了更高要求。此外，對于空間相似性較高的環境，由于低線束激光雷達對環境特征的表達能力不足，導致閉環檢測誤報現象十分常見。對激光光束存在干擾的場景 ( 玻璃、鏡子、不銹鋼等 )，無人車運行定位與建圖算法時，會出現一些未完全探明的區域，此處區域顯示為灰色， 如圖 1(d) 紅圈處所示。對于這些干擾，傳統激光雷達定位主要是在干擾激光掃描平面處貼上反射片，或是通過人工校驗的方法進行解決。而傳統的人工后處理方法是通過使用機器人仿真軟件 RoboStudio 中的畫圖工具將建圖結果中的誤判區域進行擦除，使其變為已探明區域，因此該方法既不智能也費時耗力。而本文通過對地磁序列信號的研究，提出了地磁序列信號與激光雷達融合的閉環檢測方法，“一站式”地解決了上述干擾對整個定位與建圖系統的影響。

　　3 地磁信號與激光 SLAM 融合算法設計

　　本文基于地磁導航技術與激光 SLAM 算法，針對無人駕駛載體定位與建圖的閉環檢測環節，設計了地磁序列搜索與激光點云匹配融合的閉環檢測算法。算法框架如圖 2 所示，其中紅色虛線框中是本文提出的閉環粗匹配與地磁特征篩選閉環檢測算法。

圖 2 地磁信號與激光雷達融合定位建圖算法框架

　　3.1 基于地磁信號與激光雷達融合定位的算法設計

　　地磁信號與激光雷達融合定位建圖算法是在激光 SLAM 算法框架基礎上設計的，因此和激光 SLAM 框架一樣擁有前端掃描匹配、后端優化和閉環檢測等環節。本文提出的閉環粗匹配與地磁特征篩選閉環檢測算法是將地磁序列匹配技術引入激光 SLAM 閉環檢測環節。整個地磁信號與激光雷達融合定位建圖的算法流程如圖 3 所示。閉環粗匹配與地磁特征篩選閉環檢測算法位于整個系統框右側紅色虛線框中。算法總體框架參數設置分為傳感器參數設置與位姿地圖參數設置。其中傳感器參數設置分為來自激光雷達的輸入與來自 IMU 的輸入。激光雷達的參數設置包括：掃描角度的范圍、掃描角度間隔、掃描點數、掃描時間、一次掃描的線束數等。

圖 3 基于地磁信號與激光雷達融合定位與建圖算法流程

　　本文算法首先初始化上述參數，以運動載體的初始位置為原點，將 IMU 估計的載體坐標附近劃定為激光點云匹配的搜素區域。在該區域半徑內調用激光 SLAM 前端點云匹配算法估計載體更精細的坐標，載體的姿態由當前時刻的 IMU 提供， 此時已完成前端位姿的估計。但是在激光點云的匹配過程中， 由于受到環境的干擾，不可避免地出現累計誤差，這些誤差中一些由后端的優化環節進行消除，另一些則由閉環檢測環節進行消除。本文提出的閉環粗匹配與地磁特征篩選閉環檢測算法利用地磁序列快速搜索與匹配的特點，能為激光 SLAM 提供穩定精準的閉環候選集合。

　　3.2 閉環粗匹配與地磁特征篩選閉環檢測算法

　　首先，獲取當前激光點云數據與對應的三軸地磁信號，并將當前幀點云插入子地圖中。由三軸地磁信號組成的三維列向量為 (X,Y,Z )，采集到的 X、Y、Z 三軸方向上的地磁序列幅值分別為 mx、my、mz，計算地磁序列的模值并加入地磁子序列Si，其中 Si 表示第 i 個 10 s 地磁序列的模值。　

                                                                                 (2)

　　接著按照激光點云的匹配算法，匹配新構建的點云子地圖與歷史子地圖。本文將滿足子地圖匹配得分閾值的點云子地圖加入候選子圖集，對應的地磁序列信號加入候選子序列集合。若不存在滿足條件的點云子地圖，則當前位姿不存在閉環，并跳出閉環檢測環節。然后，通過使用動態時間規劃 (Dynamic Time Warping，DTW) 算法計算候選地磁子序列與新加入的地磁子序列的距離。由于考慮到即使在閉環檢測的地方，運動軌跡也不可能完全重合，因此閉環檢測附近區域的運動必定是時而靠近上一次運動的軌跡，時而遠離上一次運動的軌跡。針對這種無約束的運動 ( 無固定運動軌跡的運動 )，通過設置固定 DTW 距離來約束閾值是不適用的。故使用迭代最近鄰(Iterative Closest Point，ICP) 算法計算候選集中的運動軌跡， 并與當前軌跡進行匹配，記錄算法迭代至收斂的步數。最后， 對于滿足迭代步數約束，即運動軌跡相似的候選集地磁子序列， 放寬 DTW 閾值的約束。從而使即使偏離原來軌跡的運動在閉環檢測的地方也能滿足 DTW 地磁匹配的閾值篩選條件。

　　圖 4 閉環粗匹配 與地磁特征篩選閉環檢測算法

　　圖 4 為閉環粗匹配與地磁特征篩選閉環檢測算法流程圖。本文算法可在激光閉環檢測到的候選子圖中，用地磁匹配的DTW 和軌跡匹配的 ICP 算法再次篩選出準確的閉環檢測點， 從而解決了閉環檢測誤檢問題 ( 室內建筑結構相似性引起 ) 和定位建圖擾動問題( 室內玻璃、光滑鏡面材質對激光光束透射、反射現象引起)。其中，ICP 迭代至收斂步數作為軌跡相似評價， 并自適應調節閉環檢測點處匹配的地磁序列閾值，從而保證閉環檢測的準確性與穩定性。

　　當新的地磁序列 Snew 生成時，用 DTW 算法計算已生成的地磁序列 Si 與 Snew 的歐式距離。當 DTW(Snew, Si) 的距離小于閉環檢測的閾值 DLC 時，將此閉環檢測的可能位姿節點加入閉環檢測候選集。表 1 給出了地磁信號與激光雷達融合閉環檢測算法的偽代碼。圖 5 所示為地磁序列與激光 SLAM 融合原理圖。三軸地磁信號通過求平方和的形式形成序列 Si，與相同時戳位姿對應的示意圖形成新位姿節點 Xi，體現地磁序列信號與激光解算位姿的融合原理。

　　表 1 地磁信號與激光雷達融合的閉環檢測的算法偽代碼

　　圖 5 地磁序列與激光 SLAM 融合原理圖　

　　4 實驗

　　4.1 無人駕駛硬件測試平臺設計

　　本研究主要采用為碼頭倉庫重載自動導引運輸車(Automated Guided Vehicle，AGV) 提供高精度定位與建圖作為研究背景，圖 6 所示為地磁與激光融合 SLAM 實驗平臺總框圖。該實驗平臺的定位與建圖主要使用的傳感設備包括， Hokuyo 單線激光雷達、禾賽 40 線激光雷達和 Xsens IMU。其中 Xsens IMU 作為地磁序列信號的感知設備，通過安裝了 ROS(Robot Operating System) 系統的筆記本電腦采集空間三軸地磁信號。實驗平臺的工控機采用英偉達的 Jetson TX2，筆記本只作為與用戶交互的終端，通過 WIFI 向工控機收發指令。激光雷達與采集地磁信號的 IMU 一同接在 Jetson TX2 上，用以建圖與定位。本實驗平臺不僅能滿足空間地磁序列作為定位匹配信號的驗證性實驗要求，還是基于地磁信號的激光 SLAM 閉環檢測方法的主要實驗平臺。

圖 6 地磁與激光融合 SLAM 實驗平臺總框圖

　　4.2 定位與建圖效果對照實驗

　　為對比說明 Google 的 Cartographer 激光 SLAM 算法 [5] 與本文的地磁與激光融合 SLAM 算法在建圖與定位效果上的優劣，選取對激光雷達產生大量干擾的場景，如圖 1(b)( 中國科學院深圳先進技術研究院 D 棟 2 樓 ) 實驗場景中存在大片的玻璃落地窗，進行對比實驗。圖 7 為實驗場景平面圖。

圖 7 實驗場景平面圖

　　定位與建圖算法對比實驗步驟：(1) 對照實驗采用 AGV 實驗平臺，控制實驗無關變量進行實驗( 在相同場景、時間段、控制實驗車運行大致相同的軌跡 );(2) 啟動實驗小車同時運行Cartographer SLAM 算法，實時觀察 SLAM 算法輸出的定位軌跡與建立的柵格地圖;(3) 讓實驗小車完整地運行一周，運行結束后保存運行軌跡與建立的柵格地圖;(4) 運行本文提出的地磁與激光融合 SLAM 算法，重復 (2)、(3) 步驟。

　　4.3 實驗結果與分析

　　4.3.1 定位與建圖效果對比實驗

　　搭載了激光雷達的 AGV 在室內場景運行一周后構建的柵格地圖與推算的運動軌跡如圖 8(a) 所示。結果顯示，與小車實際行駛軌跡的最大出入是剛開始的一段錯誤定位軌跡，這是由于場景中結構相似性導致的。因為無論是從左到右還是從右到左行駛，激光雷達的觀測量都存在極大相似性。在經過該結構相識性較高的環境后，Cartographer 的定位才與實際情況相符合。而本文算法在相同干擾環境中定位的軌跡如圖 8(b) 所示，符合無人車的實際運動。

　　在進行定位與建圖效果對照實驗中，傳統 SLAM 算法在應對環境中的玻璃干擾時，常常錯誤建圖與定位，具體如圖8(c) 所示。原因是激光雷達對場景的描述能力不足，幀間匹配特征不夠明顯，閉環檢測環節在應對上述干擾時不起作用。因為玻璃等光滑鏡面材質對激光光束透射、反射，所以此算法容易構建出實際中不存在的可行使區域 ( 被玻璃材質隔斷的區域 )，這對于使用該地圖進行導航的任務來說將會產生碰撞后果。針對這些問題，基于地磁信號在空間分布的差異性與穩定性，本文提出的地磁與激光融合的解決方案在應對上述干擾時有更好的建圖與定位效果，如圖 8(c)、8(d) 中的圓圈區域對應圖 1(b) 中的玻璃干擾區域。

圖 8 定位與建圖對比實驗

　　4.3.2 算法運行效率實驗與結果分析

　　為體現地磁信號與激光雷達融合的閉環檢測算法對激光點云匹配速度的提升性，實驗選用 100 幀激光數據進行匹配比較。同時將本文算法、暴力匹配的搜索算法與 Cartographer 的分支定界加速閉環檢測算法進行比較。為確保實驗的公平性， 在 AGV 運行一周時采集的激光點云與地磁數據中，隨機選取100 幀激光點云，測試 3 種閉環檢測算法完成 100 次匹配后的運行時間。從圖 9 可看出，在同樣匹配次數下，暴力匹配算法消耗的時間大約為 13 s/ 次;Cartographer 算法大約為 6.7 s/ 次;而具有地磁匹配檢測算法僅為 0.9 s/ 次。可見地磁匹配篩選能減少產生的候選集，在 100 幀的匹配任務中本文算法匹配速度相比于 Cartographer 的分支定界加速匹配算法提升了 10%。

　　圖 9 三種閉環檢測算法耗時比較

　　　　4.3.3 算法穩定性實驗與結果分析

　　實驗數據采用圖 7 場景中采集的一圈激光點云與地磁序列信號。記實驗數據待匹配激光點云總幀數為 N，閉環檢測的得分閾值為 M，經算法篩選后的候選集合幀數為 S。由于實驗的一圈運行數據中僅存在一個正確閉環檢測結果，因此閉環檢測的召回率可定義為

、誤檢率為，則閉環檢測的正確率為。為驗證本文算法在閉環檢測中的穩定性，實驗通過調整匹配算法的得分閾值 M，得到不同召回率下的閉環檢測誤檢率。然后，分別對暴力匹配算法、Cartographer 的分支定界加速匹配算法與本文算法進行對比實驗與分析。

        閉環檢測誤檢率和召回率分布情況如圖 10 所示。本文算法通過地磁匹配算法篩選出候選閉環檢測位姿節點方法后，由于地磁匹配算法能快速地過濾掉很多容易造成誤檢的結果，因此本文算法的誤檢率在 3 種算法中一直維持在較低水平。從圖10 可看出，不論激光點云幀與子地圖匹配的得分閾值如何設置，在相同召回率的情況下，本文算法的閉環檢測誤檢率都低于分支定界加速匹配算法與暴力匹配算法;在 0.8 召回率的情況下閉環檢測的誤檢率降低了 23%，明顯提升了閉環檢測算法的穩定性。

圖 10 三種閉環檢測算法誤檢率與召回率對比

　　5 總結與展望

　　本文以激光 SLAM 閉環檢測環節中的誤檢現象，及在定位與建圖環境中由干擾引起的閉環誤檢測與建圖失真現象為研究課題。首先，在對激光 SLAM 閉環檢測的相關研究進行調研后，將地磁導航技術中的地磁匹配與激光 SLAM 中的閉環檢測環節相結合。通過從激光 SLAM 算法中必要的 IMU 傳

　　感設備里提取地磁信號作為閉環的判斷依據，有效地解決了建圖與定位技術中閉環檢測匹配速度緩慢及閉環誤檢的問題。然后基于本文提出的地磁序列搜索與激光點云匹配融合的閉環檢測算法以及地磁與激光融合 SLAM 實驗平臺設計，將傳統 SLAM 算法與本文算法進行對比。最后對傳統 SLAM 算法定位與建圖失效的原因進行詳細分析，并提出了在上述 SLAM 失效場景中的解決方案。

　　實驗結果表明，與激光 SLAM 技術的行業標桿 Carto- grapher 算法相比，本文算法降低了局部相似度較高引起的誤檢情況;同時在定位與建圖環境中，改善了激光光束反射與透射干擾引起的閉環誤檢與建圖失真現象。在穩定性方面，本文算法與 Cartographer 算法相比，激光 SLAM 在匹配速度和閉環檢測穩定性上都有明顯提升。

　　傳統的基于濾波的激光SLAM，如CoreSLAM、GMappin g 與 基 優 化 的 HectorSLAM 、 KartoSLAM 、 LagoSLAM 和Cartographer，無法在激光干擾的場景中確保定位與建圖的準確性。而本文算法由于使用了地磁匹配技術改進激光閉環檢測環節，在應對上述干擾場景時均能穩定輸出精確的定位與建圖結果。經過多次實驗測試，證明本文算法在閉環檢測速度提升 31% 的前提下，定位精度在無激光干擾場景中仍可與Cartographer 算法持平，從而提高了定位與建圖的效率。此外，在激光干擾的場景中，本文算法的定位精度與建圖效果優于上述所有激光 SLAM 算法。

　　室外的復雜環境為無人駕駛精準定位與建圖帶來更多的挑戰，同時室外對激光雷達感知與IMU 地磁信號的測量存在更多干擾。如何在保證定位精度與建圖效果的前提下，將地磁與激光融合 SLAM 技術拓展到室外駕駛環境是我們未來探索的方向。