萬樂民，張云生， 余 天

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

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

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

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

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

1 方 法

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

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

1) 雙激光雷達(dá)外參標(biāo)定：利用密集點(diǎn)云為媒介，進(jìn)行水平方向和豎直方向雙激光雷達(dá)的外參標(biāo)定。

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

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

1.1 雙激光雷達(dá)掃描頭的外參標(biāo)定

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

(1)

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

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

(2)

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

(3)

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

(4)

1.2 水平激光雷達(dá)SLAM建圖

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

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

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

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

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

1.4 精度評價指標(biāo)

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

(5)

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

2 實(shí)驗(yàn)與分析

2.1 實(shí)驗(yàn)系統(tǒng)搭建

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

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

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

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

圖2 實(shí)驗(yàn)原型系統(tǒng)

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

2.2 實(shí)驗(yàn)數(shù)據(jù)

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

表1 實(shí)驗(yàn)數(shù)據(jù)概況

圖3 標(biāo)定場地

圖4 融合前后對比

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

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

對于數(shù)據(jù)樓A，基于單個激光雷達(dá)(水平安置的激光雷達(dá))進(jìn)行LeGO-LOAM的建圖結(jié)果如圖6(a)所示，使用文中提出的基于雙激光掃描頭數(shù)據(jù)融合建圖結(jié)果如圖6(c)所示。從圖中可以看出，基于單個水平激光雷達(dá)LeGO-LOAM的建圖結(jié)果受到其視場角的限制，導(dǎo)致目標(biāo)建筑的立面信息缺失，無法對建筑外表面進(jìn)行完整的三維重建。而使用融合雙激光掃描頭建圖方法，能夠彌補(bǔ)單個水平激光掃描頭的視場角受限的缺點(diǎn)，獲取更豐富的建筑上部立面信息，單個激光掃描頭和雙激光掃描頭數(shù)據(jù)融合前后點(diǎn)云地圖局部區(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)化前后的融合點(diǎn)云對比結(jié)果

對于數(shù)據(jù)樓B，基于單個激光雷達(dá)(水平安置的激光雷達(dá))進(jìn)行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)化前后的點(diǎn)云對比結(jié)果如圖9所示。

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

2.4 定量精度評定

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

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

3 結(jié)束語

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