基于2D旋轉激光的室內3D-SLAM

羅 磊 談 彬 姚 劍 李 禮

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

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

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

1 低成本室內3D-SLAM系統

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

圖1 硬件設備示意圖Fig.1 Hardware Equipment Diagram

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

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

1.1 特征提取與表達

在室內場景中，廣泛存在大量平面結構，如墻面、地面、屋頂等，可作為特征約束。但當遇到長走廊等易退化的場景，平面約束往往不足，可通過面交線進行有效場景約束，如門框邊線、柱子角線等。本文中采用有界平面和相鄰平面相交線段（面交線）作為特征，具有較強的結構和語義信息，能夠穩定有效表達場景特征，一定程度上克服場景約束不足、幀內變形大的情況，如圖3所示。

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

對于一條掃描線，首先利用種子點增長算法快速提取直線段；然后計算相鄰直線交點作為角點。利用掃描線間的相鄰關系，直線段區域增長，快速生成場景平面，得到平面特征［11，12］。該平面提取方法依據掃描線特有鄰接結構，可快速提取場景平面，滿足SLAM系統實時性要求。平面特征表達為P=[p，n，E]，其中p表示平面質點，n表示指向背離激光傳感器方向的平面法向量，E表示平面邊界點，構建有邊界的平面特征。

平面特征提取后，可利用角點快速提取面交線特征。在一條掃描線中，利用構成角點的兩條相鄰直線所屬平面描述角點屬性，即該角點與那些平面相連接。將具有相同屬性的角點進行直線擬合即可得到面交線特征，表示為：L=[pstart，pend，iP1，iP2]，其中pstart，pend表示線段兩端點，iP1，iP2索引面交線連接的平面。該角點提取方式相較于LOAM，具有更強的結構和語義信息，可有效表征場景結構骨架信息，有利于特征匹配和單幀形變去除。

1.2 特征匹配

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

在滿足后3個條件的前提下，選擇代價最小的匹配作為最終匹配。其中，依據經驗設為0.5m，設為π/6，設為0.4。

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

式中，依據經驗設為0.8 m；和設為π/5；設為0.1。

本文中的特征表達具有較強的結構信息，不同結構間差異大，可有效減少錯誤匹配，即使單幀內有較大變形，仍可以進行穩健的特征匹配。

1.3 分級位姿優化

本文位姿優化采用由粗到精，逐步優化的分級位姿優化策略，可在保證精度的前提下，減少計算量，保證算法的實時性。分級位姿優化策略分為3部分，分別為單幀粗略估計運動變形、幀內線性位姿優化、地圖B樣條精細優化。

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

粗略糾正運動變形后，利用當前幀特征與局部地圖的匹配關系，在當前幀內部采用勻速運動模型進行位姿優化。每條掃描線位姿可表示為：

式中，Tstart、Tend是李代數表達的幀內首尾位姿；⊕表示位姿變換操作；t∈[0，1)表示當前掃描線在幀內的時間。

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

勻速假設和三次B樣條位姿優化能量方程同為：

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

1.4 地圖表達與構建

激光SLAM常規地圖表達形式有格網地圖、點云地圖等，存在表達冗余、無結構鄰接信息等問題。本文結合自身特征描述特點，提出了一種結構化場景地圖表達形式。結構化場景地圖是指由平面和面交線特征及其鄰接關系構建的場景骨架地圖，可以簡潔有效地表達場景結構，具有很強的結構和語義信息，可方便快速進行特征關聯和地圖更新，適用于室內場景。

局部地圖采用寬度為8幀的滑動窗口保存環境地圖，通過結構化特征地圖表達，可有效簡潔表達局部環境［13］。并且平面和面交線等具有全局特性，可有效降低位姿漂移現象。

2 實驗分析

2.1 定性對比實驗

選取LOAM作為對比試驗，去除LOAM中的IMU預積分部分，便可與本文算法形成對比。本文采集的一棟寫字樓1～5樓的樓梯數據，該場景數據具有較大旋轉變形。具體實驗結果如圖4所示。

圖4 定性對比實驗結果Fig.4 Results of Qualitative Comparison Experiment

圖4（a）所示為LOAM重建結果，在樓梯轉角平臺處沒有有效去除旋轉變形，地圖變形明顯。由于LOAM角點提取在室內環境中不魯棒，且較依賴IMU提供初始位姿，對較大的旋轉變形不能有效去除。圖4（b）所示為本文算法結果，地圖重建完好，結構清晰，表明算法具有較強的抗形變能力，可以穩定進行定位與構圖。

2.2 定量消融實驗

本部分驗證面交線結構約束和多級位姿優化的有效性。實驗數據仍采用樓梯數據。由于沒有絕對位姿真值，本文對比指標參考Razlaw等［14］提出的評價點云地圖的平均地圖熵（mean map entropy，MME）和 平 均 平 面 變 量（mean plane variance，MPV），以及采用平面點云到對應平面特征距離的均方根誤差（root mean square error，RMSE）作為定量指標，這3項指標越小，地圖質量越好，定位精度越高。MME和MPV計算公式為：

式中，Q表示點云總數；qk表示計算點；Σ(qk)表示qk鄰域內點云的協方差；v(qk)表示在qk鄰域內點到擬合平面的上四分位距離。

實驗結果如表1所示，由表1可知，面交線作為額外約束，與平面約束相互提升，可提升地圖質量。尤其在平面易退化場景，面交線約束可有效減弱退化現象，保證定位精度。通過B樣條優化，可有效提升定位精度，優化地圖質量，驗證了多級位姿優化的有效性。

表1 定量消融對比實驗結果Tab.1 Results of Quantitative Ablation Comparison Experimental

2.3 多場景數據集驗證

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

圖5 室內場景數據集Fig.5 Indoor Scene Dataset

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

表2 多場景數據集驗證結果Tab.2 Results of Multi-scene Dataset Validation

3 結束語

針對當下室內激光SLAM系統存在的硬件成本高、室內場景定位構圖不魯棒的情況，本文設計實現了一種僅依靠低成本2D激光傳感器的數據采集設備，基于該設備提出了一種基于平面和面交線特征的室內3D-SLAM算法，通過魯棒的特征設計、匹配和分級位姿優化策略，可以穩定有效地進行室內定位與構圖。通過對比實驗和多場景數據集驗證，證明本文算法的有效性、魯棒性，說明低成本采集硬件的可行性。