羅 磊 談 彬 姚 劍 李 禮

1武漢大學(xué)遙感信息工程學(xué)院，湖北 武漢，430079

同步定位與構(gòu)圖（simultaneous location and mapping，SLAM）是機(jī)器人利用傳感器感知周圍環(huán)境，同時(shí)構(gòu)建周圍地圖和確定自身位置的技術(shù)，廣泛應(yīng)用于智能家居、會(huì)場(chǎng)服務(wù)、快遞物流、VR、AR、文物保護(hù)等領(lǐng)域［1］。激光SLAM是利用激光傳感器作為數(shù)據(jù)源的SLAM系統(tǒng)，具有精度高、測(cè)距遠(yuǎn)、不受光照影響、直接獲取坐標(biāo)等特點(diǎn)，可快速進(jìn)行場(chǎng)景識(shí)別定位和構(gòu)建高精度環(huán)境地圖［2］。對(duì)于室內(nèi)激光SLAM，2D-SLAM系統(tǒng)相對(duì)成熟，出現(xiàn)了一些經(jīng)典算法，如：Gmapping算法［3］、Hectormap算法［4］、Cartographer算法［5］等。但由于2D地圖缺乏3D信息表達(dá)，不能滿足樓梯、不同樓層等場(chǎng)景表達(dá)需求，需要一個(gè)3D激光SLAM解決方案。

Trevor等［6］利用2D激光傳感器和RGBD相機(jī)組合采集場(chǎng)景點(diǎn)云數(shù)據(jù)，在2D-SLAM基礎(chǔ)上結(jié)合平面約束進(jìn)行3D-SLAM，但由于RGBD相機(jī)測(cè)距短，僅適用小尺度場(chǎng)景。Geneva等［7］利用多線激光傳感器和慣性測(cè)量單元（inertial measurement unit，IMU）提出了一種無歧義平面表達(dá)方式的平面SLAM系統(tǒng)；文獻(xiàn)［8，9］提出的室內(nèi)背包系統(tǒng)，采用多線激光與慣導(dǎo)組合的背包式設(shè)備，可快速進(jìn)行數(shù)據(jù)獲取。這兩種解決方案由于采用價(jià)格昂貴的多線激光傳感器，在很多應(yīng)用場(chǎng)景中存在局限性，因此一些學(xué)者開始研究使用低成本的2D激光傳感器進(jìn)行3D-SLAM的研究。經(jīng)典激光SLAM算法LOAM（LiDAR odometry and mapping）［10］，采用IMU預(yù)積分，與旋轉(zhuǎn)2D激光傳感器松耦合，提取場(chǎng)景平面點(diǎn)和角點(diǎn)作為特征進(jìn)行室內(nèi)外SLAM。但在室內(nèi)場(chǎng)景中，LOAM較依賴IMU，存在對(duì)較大變形敏感、易跟蹤丟失的問題。本文探索僅使用一個(gè)2D激光傳感器，不借助其他傳感器，進(jìn)行穩(wěn)定魯棒的室內(nèi)3DSLAM。相比于3D傳感器，采集的數(shù)據(jù)少，場(chǎng)景采集頻率低，幀內(nèi)易存在較大變形。

1 低成本室內(nèi)3D-SLAM系統(tǒng)

數(shù)據(jù)采集硬件是由電機(jī)帶動(dòng)2D激光傳感器繞前向軸旋轉(zhuǎn)的低成本數(shù)據(jù)采集裝置，如圖1所示。2D激光傳感器采用Hokuyo UTM-30LX，視場(chǎng)角達(dá)270°，點(diǎn)位分辨率0.25°，一條掃描線可獲取1 081個(gè)場(chǎng)景點(diǎn)，采集頻率40 Hz。電機(jī)繞前向軸以180°/s的速度帶動(dòng)激光傳感器旋轉(zhuǎn)，每秒鐘40條掃描線（稱為一幀）可覆蓋整個(gè)3D場(chǎng)景。

圖1 硬件設(shè)備示意圖Fig.1 Hardware Equipment Diagram

基于此采集設(shè)備，本文提出了一種利用平面和面交線特征魯棒抗形變的室內(nèi)3D-SLAM算法，流程如圖2所示，包括特征提取、特征匹配、位姿優(yōu)化、地圖表達(dá)等部分。

圖2 本文算法流程圖Fig.2 Flow Chart of Algorithm

1.1 特征提取與表達(dá)

在室內(nèi)場(chǎng)景中，廣泛存在大量平面結(jié)構(gòu)，如墻面、地面、屋頂?shù)龋勺鳛樘卣骷s束。但當(dāng)遇到長(zhǎng)走廊等易退化的場(chǎng)景，平面約束往往不足，可通過面交線進(jìn)行有效場(chǎng)景約束，如門框邊線、柱子角線等。本文中采用有界平面和相鄰平面相交線段（面交線）作為特征，具有較強(qiáng)的結(jié)構(gòu)和語義信息，能夠穩(wěn)定有效表達(dá)場(chǎng)景特征，一定程度上克服場(chǎng)景約束不足、幀內(nèi)變形大的情況，如圖3所示。

圖3 特征提取示意圖Fig.3 Feature Extraction Diagram

對(duì)于一條掃描線，首先利用種子點(diǎn)增長(zhǎng)算法快速提取直線段；然后計(jì)算相鄰直線交點(diǎn)作為角點(diǎn)。利用掃描線間的相鄰關(guān)系，直線段區(qū)域增長(zhǎng)，快速生成場(chǎng)景平面，得到平面特征［11，12］。該平面提取方法依據(jù)掃描線特有鄰接結(jié)構(gòu)，可快速提取場(chǎng)景平面，滿足SLAM系統(tǒng)實(shí)時(shí)性要求。平面特征表達(dá)為P=[p，n，E]，其中p表示平面質(zhì)點(diǎn)，n表示指向背離激光傳感器方向的平面法向量，E表示平面邊界點(diǎn)，構(gòu)建有邊界的平面特征。

平面特征提取后，可利用角點(diǎn)快速提取面交線特征。在一條掃描線中，利用構(gòu)成角點(diǎn)的兩條相鄰直線所屬平面描述角點(diǎn)屬性，即該角點(diǎn)與那些平面相連接。將具有相同屬性的角點(diǎn)進(jìn)行直線擬合即可得到面交線特征，表示為：L=[pstart，pend，iP1，iP2]，其中pstart，pend表示線段兩端點(diǎn)，iP1，iP2索引面交線連接的平面。該角點(diǎn)提取方式相較于LOAM，具有更強(qiáng)的結(jié)構(gòu)和語義信息，可有效表征場(chǎng)景結(jié)構(gòu)骨架信息，有利于特征匹配和單幀形變?nèi)コ?/p>

1.2 特征匹配

平面特征與面交線特征匹配采用先剔除錯(cuò)誤匹配，再挑選最優(yōu)匹配的策略。對(duì)于平面特征匹配，記當(dāng)前平面為Pi，地圖平面為質(zhì)心到的距離設(shè)為；Pi與的法向量夾角為；Pi與的面積和Pi投影到的重疊面積分別表示為Si、Sj和Sij，則兩平面的重疊率。則平面匹配策略可表示為：

在滿足后3個(gè)條件的前提下，選擇代價(jià)最小的匹配作為最終匹配。其中，依據(jù)經(jīng)驗(yàn)設(shè)為0.5m，設(shè)為π/6，設(shè)為0.4。

面交線特征匹配策略與平面匹配類似。記當(dāng)前面交線為L(zhǎng) i，地圖面交線特征為質(zhì)心到的距離設(shè)為；L i與的法向量夾角；設(shè)面交線長(zhǎng)度為M，則兩面交線重疊率為；面交線對(duì)應(yīng)平面的法向量之和單位化后表示為N LP，則用表征面交線的面朝向相似性。則面交線匹配策略可表示為：

式中，依據(jù)經(jīng)驗(yàn)設(shè)為0.8 m；和設(shè)為π/5；設(shè)為0.1。

本文中的特征表達(dá)具有較強(qiáng)的結(jié)構(gòu)信息，不同結(jié)構(gòu)間差異大，可有效減少錯(cuò)誤匹配，即使單幀內(nèi)有較大變形，仍可以進(jìn)行穩(wěn)健的特征匹配。

1.3 分級(jí)位姿優(yōu)化

本文位姿優(yōu)化采用由粗到精，逐步優(yōu)化的分級(jí)位姿優(yōu)化策略，可在保證精度的前提下，減少計(jì)算量，保證算法的實(shí)時(shí)性。分級(jí)位姿優(yōu)化策略分為3部分，分別為單幀粗略估計(jì)運(yùn)動(dòng)變形、幀內(nèi)線性位姿優(yōu)化、地圖B樣條精細(xì)優(yōu)化。

由于數(shù)據(jù)采集頻率為1 Hz，在采集數(shù)據(jù)時(shí)，平移速度不超過0.7 m/s、旋轉(zhuǎn)不超過40°/s，因此單幀內(nèi)可能具有較大變形。首先將局部地圖轉(zhuǎn)換到當(dāng)前幀坐標(biāo)系下，通過提取當(dāng)前幀后0.5 s的角點(diǎn)特征（不超過100個(gè)）與局部地圖角點(diǎn)快速ICP（iterative closest point）配準(zhǔn)［13］，即可獲得當(dāng)前幀的粗略變形，糾正幀內(nèi)較大形變。

粗略糾正運(yùn)動(dòng)變形后，利用當(dāng)前幀特征與局部地圖的匹配關(guān)系，在當(dāng)前幀內(nèi)部采用勻速運(yùn)動(dòng)模型進(jìn)行位姿優(yōu)化。每條掃描線位姿可表示為：

式中，Tstart、Tend是李代數(shù)表達(dá)的幀內(nèi)首尾位姿；⊕表示位姿變換操作；t∈[0，1)表示當(dāng)前掃描線在幀內(nèi)的時(shí)間。

當(dāng)局部地圖積累一些線性優(yōu)化幀后，將局部地圖內(nèi)部采用3次B樣條進(jìn)行光滑精確位姿估計(jì)。為平衡精度和優(yōu)化效率，間隔0.75 s設(shè)置B樣條控制點(diǎn)，以0.2 Hz優(yōu)化局部地圖，可獲得光滑連續(xù)軌跡，優(yōu)化局部地圖精度，進(jìn)而減少漂移，提升算法性能。

勻速假設(shè)和三次B樣條位姿優(yōu)化能量方程同為：

式中，d(a，b)表示a到b的距離；p s表示平面抽稀點(diǎn)；p m表示面交線兩端點(diǎn)。

1.4 地圖表達(dá)與構(gòu)建

激光SLAM常規(guī)地圖表達(dá)形式有格網(wǎng)地圖、點(diǎn)云地圖等，存在表達(dá)冗余、無結(jié)構(gòu)鄰接信息等問題。本文結(jié)合自身特征描述特點(diǎn)，提出了一種結(jié)構(gòu)化場(chǎng)景地圖表達(dá)形式。結(jié)構(gòu)化場(chǎng)景地圖是指由平面和面交線特征及其鄰接關(guān)系構(gòu)建的場(chǎng)景骨架地圖，可以簡(jiǎn)潔有效地表達(dá)場(chǎng)景結(jié)構(gòu)，具有很強(qiáng)的結(jié)構(gòu)和語義信息，可方便快速進(jìn)行特征關(guān)聯(lián)和地圖更新，適用于室內(nèi)場(chǎng)景。

局部地圖采用寬度為8幀的滑動(dòng)窗口保存環(huán)境地圖，通過結(jié)構(gòu)化特征地圖表達(dá)，可有效簡(jiǎn)潔表達(dá)局部環(huán)境［13］。并且平面和面交線等具有全局特性，可有效降低位姿漂移現(xiàn)象。

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

2.1 定性對(duì)比實(shí)驗(yàn)

選取LOAM作為對(duì)比試驗(yàn)，去除LOAM中的IMU預(yù)積分部分，便可與本文算法形成對(duì)比。本文采集的一棟寫字樓1～5樓的樓梯數(shù)據(jù)，該場(chǎng)景數(shù)據(jù)具有較大旋轉(zhuǎn)變形。具體實(shí)驗(yàn)結(jié)果如圖4所示。

圖4 定性對(duì)比實(shí)驗(yàn)結(jié)果Fig.4 Results of Qualitative Comparison Experiment

圖4（a）所示為L(zhǎng)OAM重建結(jié)果，在樓梯轉(zhuǎn)角平臺(tái)處沒有有效去除旋轉(zhuǎn)變形，地圖變形明顯。由于LOAM角點(diǎn)提取在室內(nèi)環(huán)境中不魯棒，且較依賴IMU提供初始位姿，對(duì)較大的旋轉(zhuǎn)變形不能有效去除。圖4（b）所示為本文算法結(jié)果，地圖重建完好，結(jié)構(gòu)清晰，表明算法具有較強(qiáng)的抗形變能力，可以穩(wěn)定進(jìn)行定位與構(gòu)圖。

2.2 定量消融實(shí)驗(yàn)

本部分驗(yàn)證面交線結(jié)構(gòu)約束和多級(jí)位姿優(yōu)化的有效性。實(shí)驗(yàn)數(shù)據(jù)仍采用樓梯數(shù)據(jù)。由于沒有絕對(duì)位姿真值，本文對(duì)比指標(biāo)參考Razlaw等［14］提出的評(píng)價(jià)點(diǎn)云地圖的平均地圖熵（mean map entropy，MME）和 平 均 平 面 變 量（mean plane variance，MPV），以及采用平面點(diǎn)云到對(duì)應(yīng)平面特征距離的均方根誤差（root mean square error，RMSE）作為定量指標(biāo)，這3項(xiàng)指標(biāo)越小，地圖質(zhì)量越好，定位精度越高。MME和MPV計(jì)算公式為：

式中，Q表示點(diǎn)云總數(shù)；qk表示計(jì)算點(diǎn)；Σ(qk)表示qk鄰域內(nèi)點(diǎn)云的協(xié)方差；v(qk)表示在qk鄰域內(nèi)點(diǎn)到擬合平面的上四分位距離。

實(shí)驗(yàn)結(jié)果如表1所示，由表1可知，面交線作為額外約束，與平面約束相互提升，可提升地圖質(zhì)量。尤其在平面易退化場(chǎng)景，面交線約束可有效減弱退化現(xiàn)象，保證定位精度。通過B樣條優(yōu)化，可有效提升定位精度，優(yōu)化地圖質(zhì)量，驗(yàn)證了多級(jí)位姿優(yōu)化的有效性。

表1 定量消融對(duì)比實(shí)驗(yàn)結(jié)果Tab.1 Results of Quantitative Ablation Comparison Experimental

2.3 多場(chǎng)景數(shù)據(jù)集驗(yàn)證

為驗(yàn)證本文方法在不同室內(nèi)場(chǎng)景進(jìn)行SLAM的有效性和魯棒性，本文采集了9個(gè)不同尺度功能的數(shù)據(jù)構(gòu)建室內(nèi)場(chǎng)景數(shù)據(jù)集，包括多層樓梯、長(zhǎng)廳、回環(huán)走廊、教學(xué)樓、圖書館、寫字樓、辦公樓、地下車庫、居民住宅等多種場(chǎng)景，算法構(gòu)建場(chǎng)景地圖如圖5所示。每個(gè)數(shù)據(jù)的軌跡長(zhǎng)度、幀數(shù)及評(píng)價(jià)結(jié)果如表2所示。評(píng)價(jià)指標(biāo)增加平移誤差和旋轉(zhuǎn)誤差，用于評(píng)價(jià)算法的抗漂移能力，誤差越小抗漂移能力越強(qiáng)。數(shù)據(jù)集中部分有閉環(huán)結(jié)構(gòu)，但由于無絕對(duì)真值位姿，采用起始3幀與最后3幀的點(diǎn)云地圖進(jìn)行ICP配準(zhǔn)，獲取閉環(huán)漂移量。

圖5 室內(nèi)場(chǎng)景數(shù)據(jù)集Fig.5 Indoor Scene Dataset

由表2和圖5可知，本文算法對(duì)9組室內(nèi)場(chǎng)景均可魯棒地實(shí)現(xiàn)位姿估計(jì)和構(gòu)建精確環(huán)境地圖。通過雙線程運(yùn)行（局部地圖B樣條優(yōu)化單獨(dú)線程），可滿足實(shí)時(shí)性需求。其中6組數(shù)據(jù)有閉環(huán)結(jié)構(gòu)，平均相對(duì)平移誤差為1.095%，平均相對(duì)旋轉(zhuǎn)誤差為0.086°/m，本算法具有全局性的特征表達(dá)和由粗到精的分級(jí)位姿優(yōu)化策略，因而有較強(qiáng)的抗漂移能力。而對(duì)比算法LOAM，在數(shù)據(jù)集場(chǎng)景極易跟蹤丟失，不能構(gòu)建地圖，無法通過評(píng)價(jià)指標(biāo)衡量，本文算法明顯優(yōu)于LOAM。通過多場(chǎng)景證明了即使采用低成本采集硬件，本文算法仍可取得不錯(cuò)的效果。

表2 多場(chǎng)景數(shù)據(jù)集驗(yàn)證結(jié)果Tab.2 Results of Multi-scene Dataset Validation

3 結(jié)束語

針對(duì)當(dāng)下室內(nèi)激光SLAM系統(tǒng)存在的硬件成本高、室內(nèi)場(chǎng)景定位構(gòu)圖不魯棒的情況，本文設(shè)計(jì)實(shí)現(xiàn)了一種僅依靠低成本2D激光傳感器的數(shù)據(jù)采集設(shè)備，基于該設(shè)備提出了一種基于平面和面交線特征的室內(nèi)3D-SLAM算法，通過魯棒的特征設(shè)計(jì)、匹配和分級(jí)位姿優(yōu)化策略，可以穩(wěn)定有效地進(jìn)行室內(nèi)定位與構(gòu)圖。通過對(duì)比實(shí)驗(yàn)和多場(chǎng)景數(shù)據(jù)集驗(yàn)證，證明本文算法的有效性、魯棒性，說明低成本采集硬件的可行性。