1 引 言

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

　　前谷歌無(wú)人車(chē)領(lǐng)導(dǎo)者 Sebastian Thrun 在其 2005 年出版的經(jīng)典著作《概率機(jī)器人》一書(shū)中介紹了如何基于概率濾波的方法使用二維激光雷達(dá)進(jìn)行地圖構(gòu)建和定位。該書(shū)詳細(xì)闡述了從基于 RB 粒子濾波 (Rao-Blackwellised Particle Filter， RBPF) 的經(jīng)典 FastSLAM 算法，逐步發(fā)展成為基于激光雷達(dá)建圖標(biāo)準(zhǔn) GMapping 算法的過(guò)程 [4]。2016 年，Google 開(kāi)源其優(yōu)化的激光 SLAM 算法 Cartographer，該算法改進(jìn)了GMapping 計(jì)算耗時(shí)且無(wú)法有效處理閉環(huán)的缺點(diǎn)，利用幀間點(diǎn)云地圖匹配與子地圖構(gòu)建思想有效地彌補(bǔ)了 GMapping 的缺陷。

　　盡管激光 SLAM 技術(shù)可以在一定場(chǎng)景下滿足定位精度的要求，但是難以在全工況的自動(dòng)駕駛場(chǎng)景中提供持續(xù)、穩(wěn)定的高精度定位輸出。另外，由于定位場(chǎng)景的結(jié)構(gòu)相似性且激光點(diǎn)云對(duì)環(huán)境的描述能力不足，容易發(fā)生 SLAM 的閉環(huán)檢測(cè)誤報(bào)(False Positives) 現(xiàn)象，從而降低系統(tǒng)定位與建圖性能。本研究旨在通過(guò)將地磁信號(hào)與激光 SLAM 的閉環(huán)檢測(cè)環(huán)節(jié)融合， 提出激光 SLAM 在應(yīng)對(duì)結(jié)構(gòu)相似性及激光強(qiáng)干擾場(chǎng)景下的定位與建圖任務(wù)時(shí)的創(chuàng)新解決方法。同時(shí)，本研究將此技術(shù)應(yīng)用到無(wú)人駕駛的定位與建圖領(lǐng)域，實(shí)現(xiàn)了無(wú)人駕駛汽車(chē)全天候、全地域的精準(zhǔn)基于矩陣補(bǔ)全的無(wú)人車(chē)定位，提升了無(wú)人駕駛汽車(chē)定位算法的精準(zhǔn)性與穩(wěn)定性。

　　2 相關(guān)研究進(jìn)展

　　2.1 激光 SLAM 閉環(huán)檢測(cè)

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

　　(a)

　　(c) 閉環(huán)誤檢與錯(cuò)誤定位 (d) 環(huán)境干擾中的錯(cuò)誤建圖圖 1 常見(jiàn)閉環(huán)誤檢干擾及錯(cuò)誤定位與建圖

　　2.2 閉環(huán)檢測(cè)模型

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

                                                                         (1)

　　其中，ξ 為匹配解算的剛性變換，ω 為搜索空間，k 為搜索空間的位姿數(shù)量，Mnearest 表示觀測(cè)激光點(diǎn)云 hk 和位置變換矩陣 Tξ 的乘積與地圖激光柵格點(diǎn)云最接近重合的置信度。對(duì)于每一幀激光點(diǎn)云插入子地圖上時(shí)的信度和來(lái)說(shuō)，信度越高則認(rèn)為越相似。在 ω 空間中尋找出該信度和最大的匹配幀，需要在 ω 空間中尋找出匹配結(jié)果的最優(yōu)解。一種常見(jiàn)的方法是暴力匹配法，即在搜索空間范圍內(nèi)，對(duì)每一幀與當(dāng)前幀進(jìn)行上式非線性最小二乘計(jì)算。匹配算法解算的剛性變換 ξ 與智能車(chē)初始的位姿相乘后便得出此時(shí)的位姿 (xi，yi，θi)。當(dāng)匹配的兩幀之間存在對(duì)激光光束產(chǎn)生干擾的材質(zhì) ( 反射率過(guò)低的物體或?qū)馐型干渥饔玫牟ＡУ?) 時(shí)，場(chǎng)景中的這些干擾因素是激光點(diǎn)云匹配階段誤差累積的主要成分。其中，該累積誤差可以通過(guò)閉環(huán)檢測(cè)的方法進(jìn)行消除。以下場(chǎng)景容易引起激光 SLAM 的閉環(huán)檢測(cè)誤檢現(xiàn)象：(1) 室內(nèi)的長(zhǎng)廊、空曠的室內(nèi)大堂、結(jié)構(gòu)布局相似的房間，如圖 1(a) 所示;(2) 場(chǎng)景中反射率過(guò)低的物體，如黑色吸光墻面、家具等;(3) 場(chǎng)景中存在玻璃或?qū)す夤馐a(chǎn)生透射現(xiàn)象的物體，如圖 1(b) 所示;(4) 場(chǎng)景中存在鏡子、不銹鋼等對(duì)激光光束產(chǎn)生鏡面反射的物體。

　　隨著地圖擴(kuò)大，匹配搜索效率決定了閉環(huán)檢測(cè)的實(shí)時(shí)性。

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

　　3 地磁信號(hào)與激光 SLAM 融合算法設(shè)計(jì)

　　本文基于地磁導(dǎo)航技術(shù)與激光 SLAM 算法，針對(duì)無(wú)人駕駛載體定位與建圖的閉環(huán)檢測(cè)環(huán)節(jié)，設(shè)計(jì)了地磁序列搜索與激光點(diǎn)云匹配融合的閉環(huán)檢測(cè)算法。算法框架如圖 2 所示，其中紅色虛線框中是本文提出的閉環(huán)粗匹配與地磁特征篩選閉環(huán)檢測(cè)算法。

圖 2 地磁信號(hào)與激光雷達(dá)融合定位建圖算法框架

　　3.1 基于地磁信號(hào)與激光雷達(dá)融合定位的算法設(shè)計(jì)

　　地磁信號(hào)與激光雷達(dá)融合定位建圖算法是在激光 SLAM 算法框架基礎(chǔ)上設(shè)計(jì)的，因此和激光 SLAM 框架一樣擁有前端掃描匹配、后端優(yōu)化和閉環(huán)檢測(cè)等環(huán)節(jié)。本文提出的閉環(huán)粗匹配與地磁特征篩選閉環(huán)檢測(cè)算法是將地磁序列匹配技術(shù)引入激光 SLAM 閉環(huán)檢測(cè)環(huán)節(jié)。整個(gè)地磁信號(hào)與激光雷達(dá)融合定位建圖的算法流程如圖 3 所示。閉環(huán)粗匹配與地磁特征篩選閉環(huán)檢測(cè)算法位于整個(gè)系統(tǒng)框右側(cè)紅色虛線框中。算法總體框架參數(shù)設(shè)置分為傳感器參數(shù)設(shè)置與位姿地圖參數(shù)設(shè)置。其中傳感器參數(shù)設(shè)置分為來(lái)自激光雷達(dá)的輸入與來(lái)自 IMU 的輸入。激光雷達(dá)的參數(shù)設(shè)置包括：掃描角度的范圍、掃描角度間隔、掃描點(diǎn)數(shù)、掃描時(shí)間、一次掃描的線束數(shù)等。

圖 3 基于地磁信號(hào)與激光雷達(dá)融合定位與建圖算法流程

　　本文算法首先初始化上述參數(shù)，以運(yùn)動(dòng)載體的初始位置為原點(diǎn)，將 IMU 估計(jì)的載體坐標(biāo)附近劃定為激光點(diǎn)云匹配的搜素區(qū)域。在該區(qū)域半徑內(nèi)調(diào)用激光 SLAM 前端點(diǎn)云匹配算法估計(jì)載體更精細(xì)的坐標(biāo)，載體的姿態(tài)由當(dāng)前時(shí)刻的 IMU 提供， 此時(shí)已完成前端位姿的估計(jì)。但是在激光點(diǎn)云的匹配過(guò)程中， 由于受到環(huán)境的干擾，不可避免地出現(xiàn)累計(jì)誤差，這些誤差中一些由后端的優(yōu)化環(huán)節(jié)進(jìn)行消除，另一些則由閉環(huán)檢測(cè)環(huán)節(jié)進(jìn)行消除。本文提出的閉環(huán)粗匹配與地磁特征篩選閉環(huán)檢測(cè)算法利用地磁序列快速搜索與匹配的特點(diǎn)，能為激光 SLAM 提供穩(wěn)定精準(zhǔn)的閉環(huán)候選集合。

　　3.2 閉環(huán)粗匹配與地磁特征篩選閉環(huán)檢測(cè)算法

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

                                                                                 (2)

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

　　圖 4 閉環(huán)粗匹配 與地磁特征篩選閉環(huán)檢測(cè)算法

　　圖 4 為閉環(huán)粗匹配與地磁特征篩選閉環(huán)檢測(cè)算法流程圖。本文算法可在激光閉環(huán)檢測(cè)到的候選子圖中，用地磁匹配的DTW 和軌跡匹配的 ICP 算法再次篩選出準(zhǔn)確的閉環(huán)檢測(cè)點(diǎn)， 從而解決了閉環(huán)檢測(cè)誤檢問(wèn)題 ( 室內(nèi)建筑結(jié)構(gòu)相似性引起 ) 和定位建圖擾動(dòng)問(wèn)題( 室內(nèi)玻璃、光滑鏡面材質(zhì)對(duì)激光光束透射、反射現(xiàn)象引起)。其中，ICP 迭代至收斂步數(shù)作為軌跡相似評(píng)價(jià)， 并自適應(yīng)調(diào)節(jié)閉環(huán)檢測(cè)點(diǎn)處匹配的地磁序列閾值，從而保證閉環(huán)檢測(cè)的準(zhǔn)確性與穩(wěn)定性。

　　當(dāng)新的地磁序列 Snew 生成時(shí)，用 DTW 算法計(jì)算已生成的地磁序列 Si 與 Snew 的歐式距離。當(dāng) DTW(Snew, Si) 的距離小于閉環(huán)檢測(cè)的閾值 DLC 時(shí)，將此閉環(huán)檢測(cè)的可能位姿節(jié)點(diǎn)加入閉環(huán)檢測(cè)候選集。表 1 給出了地磁信號(hào)與激光雷達(dá)融合閉環(huán)檢測(cè)算法的偽代碼。圖 5 所示為地磁序列與激光 SLAM 融合原理圖。三軸地磁信號(hào)通過(guò)求平方和的形式形成序列 Si，與相同時(shí)戳位姿對(duì)應(yīng)的示意圖形成新位姿節(jié)點(diǎn) Xi，體現(xiàn)地磁序列信號(hào)與激光解算位姿的融合原理。

　　表 1 地磁信號(hào)與激光雷達(dá)融合的閉環(huán)檢測(cè)的算法偽代碼

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

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

　　4.1 無(wú)人駕駛硬件測(cè)試平臺(tái)設(shè)計(jì)

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

圖 6 地磁與激光融合 SLAM 實(shí)驗(yàn)平臺(tái)總框圖

　　4.2 定位與建圖效果對(duì)照實(shí)驗(yàn)

　　為對(duì)比說(shuō)明 Google 的 Cartographer 激光 SLAM 算法 [5] 與本文的地磁與激光融合 SLAM 算法在建圖與定位效果上的優(yōu)劣，選取對(duì)激光雷達(dá)產(chǎn)生大量干擾的場(chǎng)景，如圖 1(b)( 中國(guó)科學(xué)院深圳先進(jìn)技術(shù)研究院 D 棟 2 樓 ) 實(shí)驗(yàn)場(chǎng)景中存在大片的玻璃落地窗，進(jìn)行對(duì)比實(shí)驗(yàn)。圖 7 為實(shí)驗(yàn)場(chǎng)景平面圖。

圖 7 實(shí)驗(yàn)場(chǎng)景平面圖

　　定位與建圖算法對(duì)比實(shí)驗(yàn)步驟：(1) 對(duì)照實(shí)驗(yàn)采用 AGV 實(shí)驗(yàn)平臺(tái)，控制實(shí)驗(yàn)無(wú)關(guān)變量進(jìn)行實(shí)驗(yàn)( 在相同場(chǎng)景、時(shí)間段、控制實(shí)驗(yàn)車(chē)運(yùn)行大致相同的軌跡 );(2) 啟動(dòng)實(shí)驗(yàn)小車(chē)同時(shí)運(yùn)行Cartographer SLAM 算法，實(shí)時(shí)觀察 SLAM 算法輸出的定位軌跡與建立的柵格地圖;(3) 讓實(shí)驗(yàn)小車(chē)完整地運(yùn)行一周，運(yùn)行結(jié)束后保存運(yùn)行軌跡與建立的柵格地圖;(4) 運(yùn)行本文提出的地磁與激光融合 SLAM 算法，重復(fù) (2)、(3) 步驟。

　　4.3 實(shí)驗(yàn)結(jié)果與分析

　　4.3.1 定位與建圖效果對(duì)比實(shí)驗(yàn)

　　搭載了激光雷達(dá)的 AGV 在室內(nèi)場(chǎng)景運(yùn)行一周后構(gòu)建的柵格地圖與推算的運(yùn)動(dòng)軌跡如圖 8(a) 所示。結(jié)果顯示，與小車(chē)實(shí)際行駛軌跡的最大出入是剛開(kāi)始的一段錯(cuò)誤定位軌跡，這是由于場(chǎng)景中結(jié)構(gòu)相似性導(dǎo)致的。因?yàn)闊o(wú)論是從左到右還是從右到左行駛，激光雷達(dá)的觀測(cè)量都存在極大相似性。在經(jīng)過(guò)該結(jié)構(gòu)相識(shí)性較高的環(huán)境后，Cartographer 的定位才與實(shí)際情況相符合。而本文算法在相同干擾環(huán)境中定位的軌跡如圖 8(b) 所示，符合無(wú)人車(chē)的實(shí)際運(yùn)動(dòng)。

　　在進(jìn)行定位與建圖效果對(duì)照實(shí)驗(yàn)中，傳統(tǒng) SLAM 算法在應(yīng)對(duì)環(huán)境中的玻璃干擾時(shí)，常常錯(cuò)誤建圖與定位，具體如圖8(c) 所示。原因是激光雷達(dá)對(duì)場(chǎng)景的描述能力不足，幀間匹配特征不夠明顯，閉環(huán)檢測(cè)環(huán)節(jié)在應(yīng)對(duì)上述干擾時(shí)不起作用。因?yàn)椴ＡУ裙饣R面材質(zhì)對(duì)激光光束透射、反射，所以此算法容易構(gòu)建出實(shí)際中不存在的可行使區(qū)域 ( 被玻璃材質(zhì)隔斷的區(qū)域 )，這對(duì)于使用該地圖進(jìn)行導(dǎo)航的任務(wù)來(lái)說(shuō)將會(huì)產(chǎn)生碰撞后果。針對(duì)這些問(wèn)題，基于地磁信號(hào)在空間分布的差異性與穩(wěn)定性，本文提出的地磁與激光融合的解決方案在應(yīng)對(duì)上述干擾時(shí)有更好的建圖與定位效果，如圖 8(c)、8(d) 中的圓圈區(qū)域?qū)?yīng)圖 1(b) 中的玻璃干擾區(qū)域。

圖 8 定位與建圖對(duì)比實(shí)驗(yàn)

　　4.3.2 算法運(yùn)行效率實(shí)驗(yàn)與結(jié)果分析

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

　　圖 9 三種閉環(huán)檢測(cè)算法耗時(shí)比較

　　　　4.3.3 算法穩(wěn)定性實(shí)驗(yàn)與結(jié)果分析

　　實(shí)驗(yàn)數(shù)據(jù)采用圖 7 場(chǎng)景中采集的一圈激光點(diǎn)云與地磁序列信號(hào)。記實(shí)驗(yàn)數(shù)據(jù)待匹配激光點(diǎn)云總幀數(shù)為 N，閉環(huán)檢測(cè)的得分閾值為 M，經(jīng)算法篩選后的候選集合幀數(shù)為 S。由于實(shí)驗(yàn)的一圈運(yùn)行數(shù)據(jù)中僅存在一個(gè)正確閉環(huán)檢測(cè)結(jié)果，因此閉環(huán)檢測(cè)的召回率可定義為

、誤檢率為，則閉環(huán)檢測(cè)的正確率為。為驗(yàn)證本文算法在閉環(huán)檢測(cè)中的穩(wěn)定性，實(shí)驗(yàn)通過(guò)調(diào)整匹配算法的得分閾值 M，得到不同召回率下的閉環(huán)檢測(cè)誤檢率。然后，分別對(duì)暴力匹配算法、Cartographer 的分支定界加速匹配算法與本文算法進(jìn)行對(duì)比實(shí)驗(yàn)與分析。

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

圖 10 三種閉環(huán)檢測(cè)算法誤檢率與召回率對(duì)比

　　5 總結(jié)與展望

　　本文以激光 SLAM 閉環(huán)檢測(cè)環(huán)節(jié)中的誤檢現(xiàn)象，及在定位與建圖環(huán)境中由干擾引起的閉環(huán)誤檢測(cè)與建圖失真現(xiàn)象為研究課題。首先，在對(duì)激光 SLAM 閉環(huán)檢測(cè)的相關(guān)研究進(jìn)行調(diào)研后，將地磁導(dǎo)航技術(shù)中的地磁匹配與激光 SLAM 中的閉環(huán)檢測(cè)環(huán)節(jié)相結(jié)合。通過(guò)從激光 SLAM 算法中必要的 IMU 傳

　　感設(shè)備里提取地磁信號(hào)作為閉環(huán)的判斷依據(jù)，有效地解決了建圖與定位技術(shù)中閉環(huán)檢測(cè)匹配速度緩慢及閉環(huán)誤檢的問(wèn)題。然后基于本文提出的地磁序列搜索與激光點(diǎn)云匹配融合的閉環(huán)檢測(cè)算法以及地磁與激光融合 SLAM 實(shí)驗(yàn)平臺(tái)設(shè)計(jì)，將傳統(tǒng) SLAM 算法與本文算法進(jìn)行對(duì)比。最后對(duì)傳統(tǒng) SLAM 算法定位與建圖失效的原因進(jìn)行詳細(xì)分析，并提出了在上述 SLAM 失效場(chǎng)景中的解決方案。

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

　　傳統(tǒng)的基于濾波的激光SLAM，如CoreSLAM、GMappin g 與 基 優(yōu) 化 的 HectorSLAM 、 KartoSLAM 、 LagoSLAM 和Cartographer，無(wú)法在激光干擾的場(chǎng)景中確保定位與建圖的準(zhǔn)確性。而本文算法由于使用了地磁匹配技術(shù)改進(jìn)激光閉環(huán)檢測(cè)環(huán)節(jié)，在應(yīng)對(duì)上述干擾場(chǎng)景時(shí)均能穩(wěn)定輸出精確的定位與建圖結(jié)果。經(jīng)過(guò)多次實(shí)驗(yàn)測(cè)試，證明本文算法在閉環(huán)檢測(cè)速度提升 31% 的前提下，定位精度在無(wú)激光干擾場(chǎng)景中仍可與Cartographer 算法持平，從而提高了定位與建圖的效率。此外，在激光干擾的場(chǎng)景中，本文算法的定位精度與建圖效果優(yōu)于上述所有激光 SLAM 算法。

　　室外的復(fù)雜環(huán)境為無(wú)人駕駛精準(zhǔn)定位與建圖帶來(lái)更多的挑戰(zhàn)，同時(shí)室外對(duì)激光雷達(dá)感知與IMU 地磁信號(hào)的測(cè)量存在更多干擾。如何在保證定位精度與建圖效果的前提下，將地磁與激光融合 SLAM 技術(shù)拓展到室外駕駛環(huán)境是我們未來(lái)探索的方向。