基于參數(shù)動態(tài)調(diào)整的雙激光掃描頭SLAM方法

萬樂民，張云生， 余 天

(中南大學 地球科學與信息物理學院，長沙 410083)

隨著社會經(jīng)濟的發(fā)展，傳統(tǒng)的二維地圖已經(jīng)不能滿足社會生產(chǎn)發(fā)展的需要。三維點云地圖能夠更加立體真實的還原建筑物的結(jié)構(gòu)信息，在生產(chǎn)建設中能夠發(fā)揮重要的數(shù)字底圖作用。因此，三維空間點云地圖的制圖技術，成為目前測繪領域的重要研究方向[1]。激光掃描(lidar detection and ranging, LiDAR)技術[2]，能夠快速獲取物體表面的高精度三維點云信息，被廣泛應用于三維測繪領域?；贚iDAR的同步定位與地圖構(gòu)建技術(simultaneous localization and mapping，SLAM)[3]，融合了LiDAR的高精度和快速掃描的優(yōu)勢以及SLAM技術高效而低成本的特點，能夠在移動測量平臺運動的過程中近乎實時地獲取場景的三維點云地圖[4]，提升了三維空間點云數(shù)據(jù)的獲取效率。

SLAM技術最初起源于機器人領域，隨著硬件系統(tǒng)的更新以及學科的交叉發(fā)展，SLAM技術的應用領域也逐漸擴大，測繪領域也應用該技術來實現(xiàn)三維環(huán)境地圖的構(gòu)建[5-6]。近年來，學者們提出了一些三維激光SLAM方法，比如Google公司推出的Cartographer[7]算法，該框架可以實現(xiàn)二維和三維條件下的建圖，主要針對的是計算資源相對有限條件下的室內(nèi)二維建圖。不同于Cartographer算法，卡內(nèi)基梅隆大學Zhang等人提出的LOAM[8]算法，可以直接面向三維LiDAR點云數(shù)據(jù)，僅通過激光里程計實現(xiàn)場景的三維點云地圖構(gòu)建。Shan等人在LOAM算法框架中加入回環(huán)檢測以及圖優(yōu)化模塊，提出了一種改進的三維SLAM方法 LeGO-LOAM[9]，該方法通過預先剔除地面點，提高前端激光里程計的效率，然后通過回環(huán)檢測算法，探測存在的回環(huán)路徑，最后通過因子圖的形式構(gòu)建圖優(yōu)化模型進行全局位姿圖優(yōu)化，獲取全局一致的優(yōu)化點云地圖和軌跡位姿數(shù)據(jù)。

雖然以上方法可以較好實現(xiàn)場景重建，但都是以處理單個水平放置的激光掃描頭為主，強調(diào)實時性，并且對點云數(shù)據(jù)做了一定抽稀。面向城市高建筑物場景時，單個水平激光掃描頭由于其視場角的限制，常常只能掃描到建筑物靠近地面部分的立面信息，難以獲取建筑物上部的立面信息。且因為多線掃描頭的線束具有稀疏特性，單個掃描頭獲取的點云幀信息也不夠稠密，導致建筑物的細節(jié)信息丟失過多，重建結(jié)果不全。而面向測繪領域高精度建模時，對點云的密度、覆蓋度等提出了更高的需求。因此，搭載多個激光雷達傳感器進行融合建圖成為一種可行的方案。香港大學提出的Decentralized_LOAM[10]算法框架和香港科技大學提出的M-LOAM[11]算法框架均能夠?qū)崿F(xiàn)多個激光雷達的標定以及同步建圖工作，但是這些方案主要是面向自動駕駛進行設計的，且系統(tǒng)設計復雜、設備研制成本較高。在雙激光頭的外參標定方法上，文獻[12]依賴環(huán)境中存在3個線性獨立平面的場景來實現(xiàn)標定。而文獻[13]提出的一種通過提取幾何特征點、線、面特征進行多激光雷達進行自動標定的方法，該標定方法需要掃描數(shù)據(jù)中具備足夠多的點(交通錐)、線(交通桿)等幾何特征。

針對以上問題，文中提出一種融合雙激光掃描頭的SLAM方法，該方法通過固化兩個具有一定角度的掃描頭，進行不同角度的掃描數(shù)據(jù)獲取，并借助地面激光掃描獲取的高精度稠密點云數(shù)據(jù)作為媒介，實現(xiàn)雙激光掃描頭的標定。然后利用兩者之間的標定參數(shù)，實現(xiàn)初步融合，在此基礎上，提出點云幀-子圖的ICP(iterative closest point)[14]配準的動態(tài)優(yōu)化方法，實現(xiàn)雙激光掃描頭點云數(shù)據(jù)的高精度融合，獲得高覆蓋度與高精度的三維點云。

1 方 法

方法流程如圖1所示，輸入的水平和豎直方向點云幀經(jīng)過激光掃描儀的信號控制實現(xiàn)嚴格的幀同步。本方法主要包括以下3部分：

圖1 雙掃描頭的數(shù)據(jù)融合方法

1) 雙激光雷達外參標定：利用密集點云為媒介，進行水平方向和豎直方向雙激光雷達的外參標定。

2) 水平激光雷達SLAM建圖：利用LeGO-LOAM算法進行水平LiDAR點云的SLAM，獲得水平點云幀建立的點云地圖和全局軌跡位姿數(shù)據(jù)KeyPoses6D。

3) 雙掃描頭點云數(shù)據(jù)融合：通過KeyPoses6D數(shù)據(jù)和標定的外參數(shù)T，將豎直LiDAR點云幀初步變換到水平點云地圖的坐標系下，然后通過點云幀-子圖ICP算法進一步優(yōu)化轉(zhuǎn)換參數(shù)，實現(xiàn)豎直點云幀數(shù)據(jù)到水平點云地圖的精確配準。

1.1 雙激光雷達掃描頭的外參標定

為實現(xiàn)激光傳感器之間數(shù)據(jù)的融合，首先必須確定雙掃描頭之間的外部變換參數(shù)T，其表示如式1所示。對于角度不同的兩個掃描頭獲取的掃描數(shù)據(jù)，點云之間重疊率較小，難以通過直接配準單幀點云數(shù)據(jù)獲取二者之間的轉(zhuǎn)換參數(shù)。因此使用地面激光掃描儀獲取標定場景中稠密點云數(shù)據(jù)作為參考點云，將水平和豎直方向上激光雷達同時掃描的單幀點云數(shù)據(jù)與參考點云匹配，分別得到水平與豎直兩個激光雷達相對于參考點云的坐標轉(zhuǎn)換參數(shù)T1、T2。

(1)

式中：R表示旋轉(zhuǎn)矩陣；t表示平移關系。

標定時取出待標定水平點云和豎直點云各1幀。以水平點云處理過程為例，用單幀水平點云先與參考點云進行粗配準，再使用ICP算法進行動態(tài)優(yōu)化精配準，獲得水平點云與參考點云之間的變換矩陣T1。采用類似方法，獲得豎直激光雷達相對于參考點云變換矩陣為T2。設參考點云所在坐標系為Tw，水平點云的原始坐標系為Tl，待標定豎直點云的坐標系為Tv，則3個坐標系之間存在如下變換關系：

(2)

由式(2)可以獲得水平和豎直點云之間的轉(zhuǎn)換關系為：

(3)

因此雙掃描頭間外參數(shù)T如式(4)所示，由式(4)可見，分別獲得T1、T2后，即可獲得兩個掃描頭點云間的轉(zhuǎn)換關系，為后續(xù)的點云融合提供基礎。

(4)

1.2 水平激光雷達SLAM建圖

LeGO-LOAM算法是基于LOAM算法的一種三維激光點云SLAM建圖算法，不依賴慣性導航定位裝置，就能夠進行同步定位和建圖，且在后端處理中融入了回環(huán)檢測以及圖優(yōu)化模塊，有效保證了激光SLAM建圖結(jié)果的精度。因此文中采用LeGO-LOAM算法處理水平激光掃描頭的數(shù)據(jù)。

利用LeGO-LOAM算法對水平安置的激光掃描頭點云數(shù)據(jù)進行處理后，可以獲取水平點云地圖和對應的軌跡位姿數(shù)據(jù)KeyPoses6D。KeyPoses6D包含了每幀水平點云建圖時對應的三維坐標以及姿態(tài)等信息，該數(shù)據(jù)為豎直點云數(shù)據(jù)的融合提供了基礎。

1.3 雙掃描頭點云數(shù)據(jù)融合

在導出水平點云地圖和軌跡位姿數(shù)據(jù)基礎上，使用標定獲取的外參數(shù)T，根據(jù)兩個掃描頭的同步時間戳，對同時刻下的豎直激光雷達掃描數(shù)據(jù)進行位姿變換，將單幀豎直點云初步轉(zhuǎn)換到水平點云地圖坐標系下。但采集過程中，可能因為傳感器松動等原因，使得初步轉(zhuǎn)換的豎直和水平方向點云之間存在一定的誤差，因此需要進一步優(yōu)化兩者之間的變換參數(shù)。由于掃描線稀疏，且豎直方向和水平方向角度差異較大，文中提出一種參數(shù)動態(tài)調(diào)整的單幀豎直點云與水平點云子圖匹配的方式優(yōu)化單幀豎直方向點云的轉(zhuǎn)換參數(shù)。

首先計算出待配準單幀豎直點云的外包圍盒，然后用該包圍盒裁剪出水平點云地圖的對應區(qū)域獲得水平點云子圖，該子圖經(jīng)過水平連續(xù)多幀點云拼接而成，相對于原始單幀數(shù)據(jù)密集度有大幅提高，因此可以采用ICP進行優(yōu)化配準。以裁剪的水平點云子圖為參考點云，將單幀豎直點云精確配準至水平點云中。經(jīng)過動態(tài)調(diào)整優(yōu)化后，點云地圖內(nèi)部不一致性得到了很大程度上的消除，保證了點云三維重建的精度。

1.4 精度評價指標

本方法面向SLAM建圖，采用場景中存在的平面來進行精度評定。選擇一定區(qū)域的平面點，利用最小二乘法擬合平面參數(shù)，然后計算所有點到該平面距離的均值間接評價結(jié)果的精度，算式如式(5)所示。

(5)

式中：n是擬合平面的點數(shù)；di是點到擬合平面的距離。

2 實驗與分析

2.1 實驗系統(tǒng)搭建

2.1.1 硬件原型系統(tǒng)

為驗證本文方法，搭建了如圖2所示的原型系統(tǒng)，水平和豎直方向搭載雙激光雷達掃描頭夾角約為90°，激光掃描頭采用Velodyne_16線激光雷達。

2.1.2 雙激光掃描頭標定結(jié)果

根據(jù)設計的雙激光掃描頭外標定方法，選擇了筆者所在單位的一個大廳(如圖3(a)所示)進行原型系統(tǒng)標定。使用FARO X330地面三維激光掃描儀對該場景進行掃描作為標定的參考點云，標定實驗場地實景照片及掃描的點云如圖3所示。

圖2 實驗原型系統(tǒng)

利用1.1節(jié)方法進行處理，獲得兩個掃描頭之間的參數(shù)T，然后將豎直方向的點云轉(zhuǎn)換到水平方向點云所在坐標系，標定前后結(jié)果如圖4所示。由圖4可見，本標定方法標定的外參數(shù)，可以實現(xiàn)豎直方向至水平方向點云的坐標位姿的轉(zhuǎn)換，為兩個方向的點云融合提供初始位姿。

2.2 實驗數(shù)據(jù)

為驗證本方法，筆者對兩棟建筑物進行數(shù)據(jù)獲取，分布如圖5所示，分別記為樓A、樓B，獲取的原始點云數(shù)據(jù)統(tǒng)計如表1所示。

表1 實驗數(shù)據(jù)概況

圖3 標定場地

圖4 融合前后對比

圖5 研究區(qū)域俯視圖

2.3 實驗結(jié)果與分析

對于數(shù)據(jù)樓A，基于單個激光雷達(水平安置的激光雷達)進行LeGO-LOAM的建圖結(jié)果如圖6(a)所示，使用文中提出的基于雙激光掃描頭數(shù)據(jù)融合建圖結(jié)果如圖6(c)所示。從圖中可以看出，基于單個水平激光雷達LeGO-LOAM的建圖結(jié)果受到其視場角的限制，導致目標建筑的立面信息缺失，無法對建筑外表面進行完整的三維重建。而使用融合雙激光掃描頭建圖方法，能夠彌補單個水平激光掃描頭的視場角受限的缺點，獲取更豐富的建筑上部立面信息，單個激光掃描頭和雙激光掃描頭數(shù)據(jù)融合前后點云地圖局部區(qū)域?qū)Ρ热鐖D6(b)(d)所示。參數(shù)動態(tài)調(diào)整優(yōu)化后，局部區(qū)域的重影得到消除，優(yōu)化前后的局部區(qū)域放大對比如圖7所示。

圖6 數(shù)據(jù)樓A水平單激光掃描頭和雙激光掃描頭建圖結(jié)果對比

圖7 參數(shù)動態(tài)調(diào)整優(yōu)化前后的融合點云對比結(jié)果

對于數(shù)據(jù)樓B，基于單個激光雷達(水平安置的激光雷達)進行LeGO-LOAM的建圖結(jié)果如圖8(a)所示，水平地圖局部放大結(jié)果如圖8(b)所示。使用雙激光掃描頭融合建圖方法結(jié)果如圖8(c)所示，融合地圖局部放大結(jié)果如圖8(d)所示。經(jīng)過ICP算法優(yōu)化后，局部區(qū)域的重影得到消除，參數(shù)動態(tài)調(diào)整優(yōu)化前后的點云對比結(jié)果如圖9所示。

圖8 樓B水平單激光掃描頭和雙激光掃描頭建圖結(jié)果對比

2.4 定量精度評定

為了進一步定量評價本方法三維重建點云精度，在實驗數(shù)據(jù)樓A、樓B上分別選取了如圖10所示的5個區(qū)域進行平面擬合度計算，結(jié)果如表2所示。從結(jié)果可以看出，經(jīng)過動態(tài)調(diào)整優(yōu)化處理后，RMS值與優(yōu)化前相比都有所減小，而且樓A和樓B的平均RMS精度提高33%和24%以上，說明了本方法不僅可以提高覆蓋度，也可以保證重建點云的精度。

表2 ICP優(yōu)化前后平面擬合RMS值 m

3 結(jié)束語

針對單個掃描頭進行激光SLAM點云覆蓋度較低的問題，提出了一種基于雙掃描頭的SLAM方法。針對標定參數(shù)的微小誤差，提出基于點云-子圖匹配的方式優(yōu)化配準方法，將水平和豎直方向兩個方向的掃描數(shù)據(jù)進行高精度融合。通過室外重建實驗結(jié)果分析表明，提出的基于參數(shù)動態(tài)調(diào)整的雙激光掃描頭SLAM方法，與傳統(tǒng)的基于單個激光掃描頭的SLAM算法相比，能夠獲取更加豐富的立面信息。