李健平，楊必勝

（武漢大學(xué)測繪遙感信息工程國家重點(diǎn)實(shí)驗(yàn)室，湖北武漢 430079）

建設(shè)智慧城市需依靠室內(nèi)外三維全空間地理信息數(shù)據(jù)為橋梁，從而搭建現(xiàn)實(shí)物理世界與虛擬網(wǎng)絡(luò)世界之間雙向映射，形成虛實(shí)結(jié)合的城市治理模式，對(duì)提升我國城市可持續(xù)發(fā)展能力具有重要意義［1］。隨著以攝影測量、激光掃描為代表的地理信息設(shè)備獲取技術(shù)的發(fā)展，點(diǎn)云已成為繼二維影像和矢量地圖之后的第三類時(shí)空數(shù)據(jù)，是構(gòu)建智慧城市重要基礎(chǔ)［2，3］。利用車載、機(jī)載、地面三維激光掃描為代表的移動(dòng)測量技術(shù)，可以快速獲取大范圍城市場景三維點(diǎn)云數(shù)據(jù)，構(gòu)建室外環(huán)境的數(shù)字三維模型［4］。而相對(duì)于室外環(huán)境，室內(nèi)環(huán)境由于GNSS（Global Navigation Satellite System，全球?qū)Ш叫l(wèi)星系統(tǒng)）信號(hào)拒止，光照條件差導(dǎo)致環(huán)境特征分布復(fù)雜，給現(xiàn)有LiDAR（Light Detection and Ranging，激光掃描）系統(tǒng)準(zhǔn)確、高效獲取三維數(shù)據(jù)帶來了巨大的挑戰(zhàn)［5］。如何克服現(xiàn)有移動(dòng)測量系統(tǒng)對(duì)導(dǎo)航信號(hào)的依賴，提高設(shè)備在室內(nèi)復(fù)雜環(huán)境中的數(shù)據(jù)獲取精度與效率，是學(xué)術(shù)界與工業(yè)界共同關(guān)注的熱點(diǎn)［6-7］。

隨著眾源地理信息獲取裝備［8］以及LiDAR SLAM（Simultaneous Localization and Mapping，同時(shí)定位測圖）技術(shù)的快速發(fā)展［9］，以穿戴式為代表的輕小型激光掃描系統(tǒng)具有成本低、體積小、易操作的特點(diǎn)，可以便捷在通行受限區(qū)域獲取三維點(diǎn)云數(shù)據(jù)，為室內(nèi)外一體化無縫三維地理信息獲取提供了可行方案，是傳統(tǒng)移動(dòng)測量系統(tǒng)的有效補(bǔ)充?，F(xiàn)有穿戴式激光掃描設(shè)備，如手持式［10］、背包式［11］等，需要用戶在作業(yè)過程中依據(jù)經(jīng)驗(yàn)或設(shè)備反饋判斷掃描范圍，難以保證三維點(diǎn)云的完整性，導(dǎo)致作業(yè)效率較低。尤其在應(yīng)急救災(zāi)過程中，施救人員需攜帶并操作救援裝備，難以再利用手持或背包設(shè)備對(duì)未知復(fù)雜三維環(huán)境進(jìn)行探測，導(dǎo)致實(shí)時(shí)三維空間信息缺失，嚴(yán)重制約了救災(zāi)的效率和安全性?；陬^盔的觀測平臺(tái)與用戶視線保持方向一致，具有“所見即所得”的優(yōu)勢，解放了用戶的雙手，已在單兵作戰(zhàn)、應(yīng)急搜救、AR、VR等領(lǐng)域有了成功運(yùn)用［12］。但目前尚基于無頭盔平臺(tái)的激光掃描系統(tǒng)在室內(nèi)外三維數(shù)據(jù)獲取方面的案例。因此，本文以自主研發(fā)的頭盔式激光掃描系統(tǒng)“WHU-Helmet”為研究對(duì)象，設(shè)計(jì)基于多尺度正態(tài)分布變換的LiDAR-IMU SLAM算法，并在典型室內(nèi)外場景進(jìn)行三維點(diǎn)云獲取實(shí)驗(yàn)，以驗(yàn)證頭盔式激光掃描在室內(nèi)外一體化三維測圖中的精度與可行性。

1 WHU-Helmet 硬件組成與坐標(biāo)系定義

本文研發(fā)的頭盔式激光掃描系統(tǒng)硬件組成如圖1 所示，系統(tǒng)中主要由四種傳感組成：GNSS 接收機(jī)與天線，IMU（慣性測量單元），固態(tài)激光雷達(dá)，以及全局曝光相機(jī)。GNSS 接收機(jī)用于獲取絕對(duì)地理位置以及時(shí)間基準(zhǔn)信息，當(dāng)設(shè)備從室外移動(dòng)到室內(nèi)環(huán)境時(shí)，系統(tǒng)將根據(jù)GNSS 接收機(jī)播報(bào)的衛(wèi)星顆數(shù)自動(dòng)忽略定位信息，僅利用參考時(shí)間信息保持系統(tǒng)高精度時(shí)間基準(zhǔn)。系統(tǒng)中搭載的MEMS（microelectro-mechanical system）IMU，用于積分短時(shí)間系統(tǒng)相對(duì)姿態(tài)與位置變化，降低作業(yè)過程中劇烈運(yùn)動(dòng)對(duì)系統(tǒng)的影響。固態(tài)激光雷達(dá)和光學(xué)相機(jī)用于采集環(huán)境的幾何與紋理信息。所有傳感器都根據(jù)接收電信號(hào)觸發(fā)信號(hào)進(jìn)行嚴(yán)格時(shí)間同步［13］。

圖1 頭盔式激光掃描系統(tǒng)硬件描述Fig.1 Hardware description of the helmet-based laser scanning system

2 基于多尺度正態(tài)分布變換的實(shí)時(shí)LiDAR-IMU SLAM

本文提出的基于多尺度正態(tài)分布變換的實(shí)時(shí)LiDAR-IMU SLAM 流程如圖2所示。在獲取到時(shí)間同步的原始LiDAR及IMU數(shù)據(jù)后，本文提出基于多尺度正態(tài)分布變換的實(shí)時(shí)LiDAR-IMU SLAM，主要分為畸變矯正，特征匹配與地圖維護(hù)，以及后端優(yōu)化三個(gè)部分組成，詳細(xì)步驟見圖2。

圖2 多尺度正態(tài)分布（NDT）變換的實(shí)時(shí)LiDAR-IMU SLAM流程Fig.2 Flowchart for the multi-scale real-time nor?mal distributions transform（NDT）based Li?DAR-IMU SLAM

2.1 IMU預(yù)積分與LiDAR運(yùn)動(dòng)畸變矯正

由于LiDAR連續(xù)掃描特性，第k幀（一般為0.1s內(nèi)的掃描數(shù)據(jù)）LiDAR 數(shù)據(jù)存在由于劇烈運(yùn)動(dòng)導(dǎo)致的運(yùn)動(dòng)畸變效應(yīng)，采用可利用單幀激光掃描時(shí)間內(nèi)的IMU 數(shù)據(jù)對(duì)系統(tǒng)運(yùn)動(dòng)進(jìn)行補(bǔ)償，將單幀內(nèi)的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換至初始掃描位置，從而消除單幀激光掃描運(yùn)動(dòng)畸變。利用IMU獲取運(yùn)動(dòng)補(bǔ)償姿態(tài)采用預(yù)積分法［15］，對(duì)第k幀激光采集時(shí)間△t的所有IMU 數(shù)據(jù)進(jìn)行建模。所有IMU數(shù)據(jù)按照式（2）～（4）進(jìn)行建模：

式中：a?(t)、ω?(t)分別為加速度和角速度觀測；gM為重力加速度；Ω(?)為軸角到純虛四元數(shù)的轉(zhuǎn)換。分別為位置、速度和角度預(yù)積分項(xiàng)，即第k幀激光掃描時(shí)間內(nèi)載體的相對(duì)運(yùn)動(dòng)，可直接用于補(bǔ)償激光掃描運(yùn)動(dòng)畸變。

2.2 多尺度正態(tài)分布變換的特征匹配與三維地圖管理

NDT 是常用于LiDAR SLAM 的匹配與三維地圖表達(dá)方式，相比于基于線、面特征的SLAM 方法，無需提取幾何特征，在復(fù)雜退化環(huán)境或沒有規(guī)則幾何結(jié)構(gòu)的環(huán)境中具有較好的表現(xiàn)，且整體計(jì)算量可控［16］。其核心思想是將一定三維體素大小內(nèi)的點(diǎn)云p轉(zhuǎn)換為正態(tài)分布，其均值為μ，協(xié)方差矩陣為Σ，滿足概率密度函數(shù)f(p)，寫作式（5）。利用NDT 進(jìn)行特征匹配時(shí)，以點(diǎn)到分布的概率為代價(jià)函數(shù)，優(yōu)化相對(duì)位置與姿態(tài)參數(shù)。

對(duì)于搭載在頭盔式激光掃描系統(tǒng)中的固態(tài)激光雷達(dá)，其掃描獲取的單幀點(diǎn)云內(nèi)部點(diǎn)密度存在較大差異，距離掃描中心較近處點(diǎn)密度大，相反距掃描中心較遠(yuǎn)處點(diǎn)密度小，如圖3所示。如果為了保證遠(yuǎn)處的體素內(nèi)有足夠多的點(diǎn)用于準(zhǔn)確計(jì)算協(xié)方差，則需要將格網(wǎng)設(shè)置大，導(dǎo)致近處格網(wǎng)分辨率太低，損失了近處的細(xì)節(jié)信息。如果為了保證近處細(xì)節(jié)信息，設(shè)置較小的格網(wǎng)尺寸，會(huì)導(dǎo)致遠(yuǎn)處格網(wǎng)內(nèi)點(diǎn)數(shù)較少，導(dǎo)致遠(yuǎn)處的體素協(xié)方差計(jì)算錯(cuò)誤。因此使用相同的體素對(duì)單幀點(diǎn)云進(jìn)行體素化處理，難以選取合適的體素尺寸以平衡近處和遠(yuǎn)處體素的細(xì)節(jié)信息。

圖3 固態(tài)激光雷達(dá)掃描點(diǎn)云示意圖Fig.3 Scanning pattern of the solid-state LiDAR

本文采取多尺度正態(tài)分布變換以克服上述體素尺寸設(shè)置難題，步驟如下：

（1）設(shè)置體素尺寸為Svoxel（實(shí)驗(yàn)中為0.5 m），對(duì)單幀點(diǎn)云進(jìn)行均勻體素化，并計(jì)算每一個(gè)體素中的均值、方差、點(diǎn)數(shù)、特征向量、空間范圍，依據(jù)特征向量判斷體素幾何屬性（線性、面性、不規(guī)則）［17］，舍棄無點(diǎn)的體素；

（2）對(duì)體素進(jìn)行迭代合并，合并條件見表1；

表1 體素合并條件Tab.1 The merging conditions of voxels

（3）將合并后的體素內(nèi)所有參數(shù)更新。

以面性體素合并為例，如圖4 所示。圖4a 二者都為面性體素，合并后依舊為面性，則合并成功；圖4b 分別為面性體素和不規(guī)則體素，合并后為面性體素，合并成功；圖4c 二者都為面性，合并后為不規(guī)則，合并失敗；圖4c 二者都為不規(guī)則，合并后為面性。通過上述方式合并生成多尺度體素，可以有效克服NDT算法中體素大小難以設(shè)定難題。

圖4 多尺度NDT合并條件示意俯視圖Fig.4 Illustration of the multi-scale NDT merging conditions

2.3 多模態(tài)融合的位置姿態(tài)優(yōu)化

在實(shí)時(shí)估計(jì)頭盔式激光掃描系統(tǒng)的狀態(tài)參數(shù)時(shí)，需考慮兩種模態(tài)信息，即IMU對(duì)運(yùn)動(dòng)的高頻觀測約束ek，k+1Preinte以及激光與地圖的匹配約束ek，k+1LiDAR?？紤]到系統(tǒng)運(yùn)行的實(shí)時(shí)性需求，采用滑動(dòng)窗口［18］，僅實(shí)時(shí)估計(jì)當(dāng)前幀激光的位置與姿態(tài)，將歷史激光幀和歷史IMU 信息邊緣化，轉(zhuǎn)化為先驗(yàn)約束eMargin。三類約束條件的范數(shù)累加共同組成能量函數(shù)E。

詳細(xì)實(shí)現(xiàn)如下：

（1）IMU預(yù)積分約束

（2）LiDAR匹配約束

對(duì)于第k幀點(diǎn)云中的一個(gè)三維點(diǎn)根據(jù)當(dāng)前位姿及公式（1）計(jì)算得到該點(diǎn)在制圖坐標(biāo)系中的位置并在多尺度正態(tài)分布地圖中搜索到的最鄰近體素（均值為μ，協(xié)方差為P），可根據(jù)正態(tài)分布定義，構(gòu)造誤差函數(shù)

（3）邊緣化約束

利用滑動(dòng)窗口限制SLAM系統(tǒng)實(shí)時(shí)優(yōu)化的參數(shù)量從而保證系統(tǒng)的實(shí)時(shí)計(jì)算能力。當(dāng)一幀新的激光幀到來時(shí)，SLAM 系統(tǒng)將為其創(chuàng)建一組新的系統(tǒng)狀態(tài)量，并刪除最舊的系統(tǒng)狀態(tài)量及相關(guān)約束條件，從而保證SLAM系統(tǒng)中優(yōu)化的參數(shù)量固定不變。為了在刪除最舊狀態(tài)量與相關(guān)約束條件的過程中保留對(duì)當(dāng)前系統(tǒng)狀態(tài)量的約束，降低信息損失，本文利用舒爾補(bǔ)進(jìn)行先驗(yàn)信息提取［19］。根據(jù)非線性最小二乘原理［20］，本文能量函數(shù)E的求解如下：

式中：H=ATPA為Hessian 矩陣；δx為改正數(shù)，b=ATPl為 誤 差 項(xiàng)。 將δx分 成 兩 部 分δx=[δxmarg?，δxremain?]?，式中δxmarg為需要?jiǎng)h除的最老狀態(tài)量，而δxremain為需要保留的待優(yōu)化狀態(tài)量。式（9）可以改寫為

從式（11）中可以發(fā)現(xiàn)，在不求解δxmarg的情況下，也可將δxremain進(jìn)行求解，且顧及了最老觀測信息的約束，即將最舊約束轉(zhuǎn)化為對(duì)現(xiàn)有狀態(tài)的先驗(yàn)信息，從而獲得邊緣化約束eMargin：

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

3.1 實(shí)驗(yàn)環(huán)境與數(shù)據(jù)采集

本文實(shí)驗(yàn)區(qū)域選取了兩個(gè)典型城市場景，包括辦公樓和地鐵站，用于室內(nèi)外一體化無縫測圖實(shí)驗(yàn)（如圖5）所示。實(shí)驗(yàn)人員攜帶頭盔式激光掃描系統(tǒng)（如圖6 a 所示），以1.3 m·s-1的步速對(duì)室內(nèi)外環(huán)境進(jìn)行掃描。實(shí)時(shí)掃描獲取的數(shù)據(jù)經(jīng)過本文提出方法處理，進(jìn)行三維可視化，可視化的效果如圖6 和圖7所示。為了說明頭盔激光掃描在視角與效率的優(yōu)勢，同時(shí)利用手持激光掃描在辦公樓行走相同的路線進(jìn)行數(shù)據(jù)采集，如圖6 b所示，并采用LOAM算法對(duì)手持掃描數(shù)據(jù)進(jìn)行處理，獲取的點(diǎn)云數(shù)據(jù)如圖9所示。

圖5 試驗(yàn)區(qū)域Fig.5 Study area

圖7 廣埠屯地鐵站室內(nèi)外一體化三維點(diǎn)云地圖Fig.7 Outdoor and indoor 3D point cloud map of computer-town subway station

圖8 星湖樓室內(nèi)外一體化三維點(diǎn)云地圖Fig.8 Outdoor and indoor 3D point cloud map of Xinghu building

圖9 手持激光點(diǎn)云星湖樓室內(nèi)點(diǎn)云地圖Fig.9 Point cloud collected by the handheld laser scanner in Xinghu building

3.2 精度分析

為了驗(yàn)證頭盔式激光掃描系統(tǒng)在室內(nèi)外三維一體化測圖的精度，本文利用地面激光掃描儀（Riegl VZ-400）在實(shí)驗(yàn)環(huán)境中進(jìn)行掃描并進(jìn)行多站掃描數(shù)據(jù)拼接。拼接后的多站激光掃描數(shù)據(jù)作為基準(zhǔn)。人工在兩個(gè)實(shí)驗(yàn)場景中分別選取30 個(gè)均勻分布的特征點(diǎn)，并建立其在地面激光掃描數(shù)據(jù)與頭盔式激光掃描數(shù)據(jù)中的匹配關(guān)系，從而衡量頭盔式激光掃描系統(tǒng)的精度。兩個(gè)場景中的同名點(diǎn)誤差分布如圖10所示，其統(tǒng)計(jì)值如表2所示。其中，有部分同名點(diǎn)誤差的最大值為0.86 m，主要是由于場景中存在高反的墻面（如地鐵站內(nèi)部的玻璃，樓道中的瓷磚等），激光原始觀測存在鏡面反射，產(chǎn)生了鏡像觀測錯(cuò)誤，導(dǎo)致局部點(diǎn)云誤差變大?？梢园l(fā)現(xiàn)同名點(diǎn)的平均誤差小于0.44 m，均方根誤差小于0.23 m。手持激光掃描獲取的點(diǎn)云同名點(diǎn)誤差的平均誤差為0.57 m，大于頭盔激光掃描獲取的點(diǎn)云誤差。

圖10 特征點(diǎn)誤差分布Fig.10 Error distribution of the selected corre?sponding points

表2 室內(nèi)外三維點(diǎn)云誤差分布Fig.2 The error distribution of indoor and outdoor point clouds

對(duì)比手持激光掃描和頭盔激光掃描在室內(nèi)獲取的點(diǎn)云數(shù)據(jù)的完整度可以發(fā)現(xiàn)，頭盔激光掃描獲取的點(diǎn)云密度在建筑物頂面和地面更加均勻、稠密，這是由于不同激光器掃描的模式造成的。這是由于上述結(jié)果說明了頭盔式激光掃描系統(tǒng)在室內(nèi)外三維一體化無縫測圖中具有巨大的潛力。

4 結(jié)論與展望

輕小型穿戴式激光掃描系統(tǒng)具有成本低、體積小、易操作的特點(diǎn)，是傳統(tǒng)激光掃描系統(tǒng)的有效補(bǔ)充，可在通行受限區(qū)域獲取三維空間數(shù)據(jù)。本文自主設(shè)計(jì)了頭盔式激光掃描系統(tǒng)“WHU-Helmet”，提出基于多尺度正態(tài)分布變換的實(shí)時(shí)LiDAR-IMU SLAM方法，并以典型室內(nèi)外環(huán)境為實(shí)驗(yàn)區(qū)域，以驗(yàn)證頭盔式激光掃描系統(tǒng)在室內(nèi)外一體化三維測圖的精度與可行性。通過將“WHU-Helmet”獲取的三維數(shù)據(jù)與地面式激光掃描系統(tǒng)獲取的三維點(diǎn)云對(duì)比，“WHU-Helmet”獲取的點(diǎn)云平均誤差小于0.44 m，均方根誤差小于0.23 m。頭盔式激光掃描系統(tǒng)對(duì)導(dǎo)航信息無依賴，三維測圖自動(dòng)化程度高，在室內(nèi)外三維一體化測圖中有著巨大潛力。在未來的工作中，將以室內(nèi)外結(jié)構(gòu)化信息自動(dòng)提取為研究點(diǎn)，利用結(jié)構(gòu)約束信息提升穿戴式系統(tǒng)的三維測圖精度。

作者貢獻(xiàn)聲明：

李健平：實(shí)現(xiàn)方法，完成實(shí)驗(yàn)，撰寫論文。

楊必勝：提供方法思路，指導(dǎo)模型構(gòu)建，實(shí)驗(yàn)數(shù)據(jù)處理及論文撰寫。