激光室內定位的混合式高效點云地圖構建方法

史鵬程，李加元，張永軍

(1. 武漢大學計算機學院, 武漢430072；2.武漢大學遙感信息工程學院,武漢 430079)

0 引言

隨著自動化及無人駕駛的不斷發展，導航定位技術已廣泛應用于物流配送、軍事偵察、環衛清潔等眾多領域。高精度定位是無人駕駛和移動機器人系統的核心組成部分。近年來，隨著定位技術的不斷迭代，新興的地圖定位方法可有效解決車輛或機器人的綁架(Kidnapping)問題。目前，眾多科研機構和商業公司正致力于打造適用于自動駕駛的高精度地圖(High Definition Map，HD Map)。室內環境下,由于無法接收全球定位系統(Global Positioning System，GPS)信號，機器人的自主定位與地圖創建受到一定的限制，常采用高精度三維激光掃描儀采集并拼接點云地圖。這導致地圖數據十分龐大，不利于數據的存儲與管理，因此研究一種高效且豐富的地圖創建管理方法對于室內機器人的定位研究至關重要。

目前,眾多學者已對點云地圖的創建和管理進行了大量研究,主要有四種代表性地圖：點云地圖、語義地圖、柵格地圖及拓撲地圖。點云地圖中包含海量的點云數據，存儲開銷較大。LOAM(LiDAR Odometry and Mapping)方法利用曲率計算出機械式激光雷達數據中的特征點，通過最小化點到線和點到面之間的距離估計出位姿轉換關系，采用低頻的方式構建了稠密的點云地圖。LOAM-Livox對LOAM進行了改進，在特征提取中加入了反射強度值作為判斷依據，利用固態激光雷達完成地圖構建任務。但以上定位與建圖方案若缺少后端優化，在長時間運行下位姿會出現明顯的漂移，導致地圖誤差較大。使用傳統的點云配準方法如迭代最近點 (Iterative Closest Point，ICP)，對地面三維激光掃描儀的多站數據進行配準，也可得到稠密的點云地圖。但是涉及的配準方法對數據初始位置要求較高，容易失敗。此外，海量的點云也會造成配準效率較低。

語義地圖可有效提高機器人對周圍環境的理解能力。文獻[13]將條件隨機場(Conditional Random Field, CRF)-循環神經網絡(Recurrent Neural Net-work, RNN)與基于ORB特征識別的同時定位與建圖系統(Oriented FAST and Rotated BRIEF, ORB-Simultaneous Localization and Mapping, SLAM)相結合，實現語義信息和三維點云的關聯，形成語義地圖。SuMa++利用RangeNet++網絡對LiDAR點云進行語義分割，構建了高精度的語義地圖。通常語義地圖的信息提取較為復雜，大范圍自動駕駛場景需要繁瑣的人工標注步驟，代價較高。柵格地圖可在二維空間中描述周圍地物的分布狀態，由于信息復雜度降低，定位效率顯著提高。Cartographer將機器人當前掃描數據與局部地圖進行匹配，快速地計算出機器人位姿，并構建了柵格地圖。但是該方法在位姿融合中采用無跡卡爾曼濾波(Unscented Kalman Filter,UKF)，容易出現位姿融合不準確，存在延遲等問題。文獻[16]利用Kinect相機中的彩色圖和點云，結合視覺SLAM技術構建了占據格網地圖(Occu-pancy Grid Map，OGM)。拓撲地圖關注場景中地物之間的連通關系，這使得其十分有利于機器人的控制規劃任務。但是拓撲地圖中缺少表達真實地物的特征信息，使得機器人對目標的感知和理解能力不佳。

機器人在實時運行過程中，魯棒的定位和控制規劃算法可以有效保證系統的安全性。復雜的外界環境對機器人的上述兩種模塊均具有較大挑戰性。若存在魯棒的地圖表達，則在相同的硬件條件下，定位和規劃算法的魯棒性將得到有效提高。為此，本文針對目前室內環境下點云地圖構建方法中形式單一、存儲空間大、特征信息匱乏等不足之處，提出了一種融合特征地圖、通行地圖和精簡地圖的點云地圖構建新方法。該方法將原始單一的點云地圖組織為具有三種地圖形式的混合地圖，在為機器人定位提供特征信息的同時，還可為其控制規劃提供邊緣和通行信息，最后在保留場景主體信息的前提下，實現地圖數據的精簡，降低了點云地圖的存儲空間，為點云地圖的創建與管理提供了一種新方法。

1 算法框架

本文地圖構建方法的技術路線如圖1所示，主要包括：A特征地圖構建，B通行地圖構建，C精簡地圖構建。在構建特征地圖時，本文首先考慮將法線和曲率突變點選取為特征點，并利用特征值間相互關系及非極大值抑制選取出局部顯著點。在構建通行地圖時，本文認為室內布局符合曼哈頓假設，以平面分割為基礎，通過平面之間的空間關系分割出地面，進一步利用障礙物高度構建2D范圍的可通行區域,并將三維空間中聚類目標的輪廓作為3D輔助信息,以指示機器人通行區域。在構建精簡地圖時，本文將點云按照空間格網劃分，利用特征值關系將格網分為線性、平面和球面三類。針對不同類型的格網點云，采用與其相適應的點云精簡方法，實現點云地圖的精簡。

圖1 地圖構建技術路線Fig.1 Technical route of map construction

2 特征地圖表達

2.1 曲率和法線特征點

曲率可以間接地反映局部凹凸程度，是一種典型的特征點篩選依據。通常大曲率值的位置彎曲程度較大。本文首先采用KD樹在點云地圖中建立拓撲結構。對于任意三維點，其鄰域點集為={,,…,}，構建如下協方差矩陣

(1)

(2)

式中，是鄰域集合的重心點坐標。協方差矩陣為半正定對稱矩陣，采用特征值分解得到特征值,,(>>)和特征向量,,。描述了某曲面沿法向量的變化程度，和則表示鄰域內各點在切平面的分布情況。計算任一點的曲率如下

(3)

選取作為當前點法線的估計值。根據式(4)、式(5)分別挑選出曲率特征點和法線特征點

(4)

(5)

式中，[,]表示計算法線和的夾角；和為兩閾值。

2.2 局部顯著點

本文通過鄰域點之間的特征值關系,挑選出在局部范圍內具有代表性的點，稱為局部顯著點。主要包括以下步驟：

1)采用Alpha Shape算法提取整個點云地圖中的邊緣點。邊緣點的位置遠離內部特征區域，不可作為局部顯著點。

2)在每一個非邊緣點的鄰域點集中尋找是否存在邊緣點，若存在，則將當前非邊緣點定義為邊緣臨界點，該類點同樣不可作為局部顯著點。

3)由于2.1節中已計算每一個地圖點的特征值、曲率及法線，在同時滿足非邊緣和非邊緣臨界點的點中，判斷其特征值是否滿足如下關系

(6)

式中，和為兩個比例閾值。

4)對于任意滿足式(6)的地圖點，判斷其特征值是否為鄰域點集中最大的第三特征值。若為局部最大值，則該點為局部顯著點。

3 通行地圖表達

3.1 場景邊緣點

邊緣信息是對目標輪廓的直接描述。在點云地圖中獲取目標的輪廓先驗信息，有利于保證機器人系統的安全性。在點云地圖中，本文首先采用歐式聚類，將點云地圖分成多個尺寸不同的點云聚類，并利用包圍盒體積描述每一個聚類的尺寸。部分小聚類目標對于機器人實時運行影響較小，剔除盒子體積較小的點云聚類。在剩余的點云聚類中，使用Alpha Shape找出每個聚類的邊緣點，并將邊緣點作為場景邊緣信息保存。在這里，地圖中各聚類的邊緣作為靜態目標的3D輪廓先驗信息，若機器人附近某時刻出現較多邊緣點，應及時進行路徑規劃。

3.2 2D通行地圖

本文中的2D通行地圖即在平面內給出機器人可通行的位置。室內通常為多平面環境，這種環境布局滿足曼哈頓假設。同時，室內坡度起伏較小，地面可以近似為一個平面特征?；谝陨咸卣鞣治?，該方法通常適用于室內環境。本文采用圖2中技術路線，利用點云地圖構建2D通行地圖。假定點云地圖的坐標系未進行水平方向校正，即地圖坐標可在任意坐標系下。主要包括以下步驟：

圖2 2D通行地圖構建流程Fig.2 Construction pipeline of 2D passable maps

1)平面分組與合并。采用區域生長方法分割出平面點云，將近似平行的平面分為同組，得到={},其中為第個平面分組，表示該組內第個平面。每組選取點數最多的平面。

2)確定地面所在分組。曼哈頓假設表明室內環境主要有3個平面方向，即1個水平和2個豎直方向，地面隸屬于水平方向。選出包含平面數量最多的3個平面分組={,,}。在每組平面中，按式(7)～式(9)計算出變換矩陣,對點云地圖進行旋轉和平移變換。其中式(8)為羅德里格斯旋轉矩陣計算公式。計算轉換后的地圖在平面的投影面積，本文采用平面內外接矩形的面積近似表達投影面積。室內環境下地面的面積通常會大于墻面的面積。因此，投影面積最大的平面分組則是包含地面的分組。

(7)

=+sin+(1-cos)

(8)

=[]

(9)

式中，=(×)，×為叉積運算，為每組所挑選的代表性平面的單位法向量，=[0 0 1]；為旋轉矩陣，使在旋轉后與平行；為與之間的夾角；＾表示反對稱矩陣運算；(,,)為平面的重心坐標。

3)地平面檢測。在平面分組中，地面具有兩個顯著的特性：(a)地面位于該組平面的兩側，屬于最外層的平面；(b)當該組平面中同時存在地面和棚頂平面時，室內地物距離地面的距離明顯小于棚頂。據此，提取出中的地平面。

4)通行圖構建。采用柵格化手段，在地平面點云中，沿平面方向生成二值圖。若柵格內包含地面點，則為目標像素，否則為背景像素。柵格化的同時保留其坐標。采用Canny算子在轉為灰度圖的上檢測其輪廓，填充最大輪廓內的所有孔洞元素,得到填充圖。預先設定障礙物高度范圍，將地圖中位于高度范圍內的地物點及3.1節中所提取的邊緣點，映射到中作為背景要素。按式(10)計算出可通行柵格圖，并反算回三維空間中。

(10)

4 精簡地圖表達

在機器人的全局定位中，常利用實時數據與地圖進行精匹配,進而解算位姿。然而，特征地圖的原始信息過少，點云地圖的維護與動態加載等需要消耗巨大資源。為此，本文提出了一種點云地圖精簡方法，旨在降低點云存儲空間的同時，具備以下特性：1)為傳感器的精匹配定位提供數據支持；2)在保留場景主體信息的前提下，實現地圖數據的快速預覽和可視化，以便于地圖進行檢查和更新等操作。

4.1 空間格網劃分

首先,在地圖點云中劃分三維體素網格如下

(11)

(12)

式中，和為比例閾值。參考文獻[21]中參數設置，選取==01。對網格內點云精簡時，預先設定初始數據精簡比值=01，網格內原始點云總數為。

4.2 線性網格精簡

當第一特征值遠大于第二特征值時，網格內點云在某主方向的分布顯著大于其他方向，整體呈線狀。為此，提出了基于主方向權重的點云精簡方法，權重計算方式如式(13)所示。根據格網內各點的權重，保留網格內權重位于前(1-)的點。

(13)

4.3 面類網格精簡

當第一、二特征值相近，且第二特征值顯著大于第三特征值時，網格內點云在前兩個主方向的分布相近，第三方向分量權重顯著小于其他兩方向，網格內點云呈面狀分布。面狀點云通常無明顯幾何特征，本文采用隨機采樣的方式精簡網格內的點云。固定輸出后的點數為(1-)，對網格內點云隨機采樣。

4.4 球類網格精簡

當3個特征值相差不大時，網格內點云不在任一方向上存在顯著分布，整體呈球面狀。本文采用K均值聚類的方法對網格內的點云進行精簡。主要包括以下步驟：

5 實驗與分析

5.1 實驗數據與環境

為驗證本文方法的可行性，采用圖3中3組室內稠密的點云地圖進行測試，實驗場景分別為臥室、辦公室和會議室。其中臥室和會議室數據來自文獻[22]中的實驗數據，辦公室數據是利用FARO掃描儀在真實場景下多站掃描拼接得到。其中臥室與會議室數據坐標系統存在任意性，不便于渲染顯示；辦公室數據坐標系統在采集之前對設備進行嚴格的整平，因此根據高度對辦公室數據進行渲染顯示，以提高可讀性。3個場景均包含顯著的地平面，符合本文方法中所述的地面特點。此外，辦公室場景還存在天花板。實驗環境為Intel Core i9-10850K，Ubuntu20.04LTS系統，采用C++編程實現。

(a) 臥室

(b)辦公室

(c)會議室

5.2 實驗結果

1)參數設置。對特征地圖中涉及的3個關鍵參數進行實驗，分析在不同取值情況下的特征點數量分布。圖4所示為臥室場景下曲率閾值、局部顯著點中特征值比例閾值(=)、法線偏差閾值與特征點數量分布的曲線圖。為提高顯示效果，縱坐標采用對數表示法?？v坐標為0時，表示未檢測到特征點。綜合各類特征點的數量，本文分別選取=06，==07，=50進行后續實驗。點云地圖通常數據海量，在計算特征值、曲率和法線時，具有豐富的鄰域信息，因此該參數閾值可應用于一般性的點云地圖。

(a) εc和ν1

(b) εn圖4 參數設置實驗Fig.4 Experiments of parameters setup

2)地圖構建效果。(a)特征地圖：圖5所示為各場景中構建的特征地圖，3個場景中分別提取了9089、14422和12172個特征點。(b) 通行地圖：圖6所示為在不同高度范圍內的2D通行圖，其中紅色表示該位置附近存在障礙物不可通行，綠色為可通行區域。圖中構建了5個高度范圍內的2D通行地圖，分別為0～0.4m，0～0.8m，0～1.2m，0～1.6m和0～2.0m。圖7所示為3D空間中的邊緣圖，可直接在三維空間反映出目標的邊緣細節。(c)精簡地圖：圖8所示為初始精簡比率設置為0.9時的精簡地圖。從圖8中可以看出，地圖點云的精簡程度較大時，本文方法仍可較好地保持整個場景的主體信息和細節特征。其中高度閾值范圍是根據室內環境下的移動平臺高度而設定的，本文實驗選取0.4m，可用于一般室內環境下的機器人平臺，若使用某種大型機器人，則相應地調整高度閾值即可。

3)地圖內存分析。表1記錄了本文方法構建的三種地圖的內存。其中辦公室場景由于使用高精度掃描儀采集，原始內存達到了1.6Gb，地圖的存儲和加載十分浪費資源。本文構建的特征地圖內存均小于1Mb。由于會議室的地面面積顯著大于其他兩場景，導致其通行地圖和精簡地圖內存相對較大，但仍僅需2Mb。同時為了實現以上內存統計的統一，均是以ASCII格式存儲的，若使用二進制格式存儲，內存將進一步減少。

表1 地圖內存

4)運行效率分析。 表2記錄了本文方法構建的三種地圖的運行時間。臥室場景相比其他兩場景，布局和地物較簡單，因此其地圖構建的速率也相對較快，其中特征地圖需要約43s，通行地圖大約23s，精簡地圖大約42s。辦公室場景由于點云密度最大，因此其特征地圖和精簡地圖的構建耗時最長，分別需要約74s和79s。會議場景由于顯著地面的原因，使得在通行圖構建時最為耗時，約36s。由于點云地圖數據量較大，構建KD樹等結構較為耗時，同時構建地圖時需要進行大量的計算，在實時運行效果方面仍有待提高。對于室內激光定位等任務，通常會預先采集點云地圖，地圖的構建任務也可離線完成，因此本文的地圖構建管理方法仍可有效應用于室內激光定位中。

表2 運行時間

(a) 臥室

(b) 辦公室

(c)會議室

(a) 臥室

(b) 辦公室

(c) 會議室圖6 2D可通行圖Fig.6 2D passable maps

(a) 臥室

(b) 辦公室

(c)會議室

(a) 臥室

(b) 辦公室

(c)會議室

5.3 精度評價

1)地面分割精度。本文中的2D通行圖是基于地面分割結果所構建的。為此，首先對地面分割的精度進行評估。利用CloudCompare軟件的語義標注功能，人工標記出室內場景的地面點云作為真值數據，采用式(14)和式(15)計算出地面分割的準確率Precision和召回率Recall

(14)

(15)

式中，TP為正確分割的地面點數；FP為錯誤分割的地面點數；FN為漏分割的地面點數。表3詳細記錄了各場景地面分割的準確率和召回率。表中為真實地面點數，為本文方法分割的地面點數。3個場景的地面分割準確率均達到95%以上，分割的召回率在85%～90%之間。本文假定室內地面為平面，并從區域生長所分割的平面中選擇出地平面。區域生長在分割平面時利用鄰域點的法線角度實現種子點的擴充。在平面邊緣附近處法線的偏差開始逐漸增加，區域生長在到達真實邊緣之前，提前停止生長造成欠分割。

表3 地面分割精度

2)精簡性能。實驗選取隨機采樣和體素采樣作為對比方法，評估點云地圖的精簡效果，在精簡點云中采用滾球法(Ball Pivoting)重建曲面，并采用文獻[24]中的模型誤差評估精簡質量。

(16)

式中，Δ為模型誤差均值，為原始點云地圖，′為重建后的曲面；(,′)表示上的一點到重建曲面′上的投影距離。圖9描述了各方法在不同精簡程度下的模型誤差均值和標準差。從圖9中可以看出，本文方法和體素格網方法在模型誤差方面優于隨機采樣方法。即使當點云地圖減少到原數據的5%時，仍能取得0.8mm的最佳模型精度。由于精簡方法中綜合了主方向加權、隨機采樣和K均值聚類方法，多種采樣方法使得模型誤差的標準差稍大于體素采樣。

(a) 模型誤差均值

(b) 模型誤差標準差圖9 精簡質量評估Fig.9 Simplification performance evaluations

6 結論

1)針對機器人室內激光定位中的地圖創建管理問題，提出了一種新的混合形式的地圖構建方法。將稠密的點云地圖組織為特征地圖、通行地圖和精簡地圖三種，克服了傳統地圖形式單一的局限性，強化了地圖在機器人系統的作用。

2)選取部分曲率、法線特征點和局部特征點作為顯著特征信息，構建場景特征地圖，可為移動平臺提供魯棒的點特征信息。

3)基于曼哈頓假設，以室內地平面檢測為基礎，構建2D通行地圖和3D邊緣地圖，可以為機器人在室內環境下的控制規劃提供良好的先驗信息。

4)融合主方向權重、隨機采樣和K均值聚類等方法，在保留數據主體信息的前提下，顯著降低地圖的存儲開銷。既可為傳感器精匹配定位提供數據支持，還利于實現地圖數據的快速可視化。

文中選取三種典型的室內場景進行實驗，結果表明，本文地圖構建方法在保留場景特征的同時，可提供魯棒的通行信息，并可有效降低地圖存儲空間。后續將研究基于深度學習的目標實例分割方法，為通行圖提供更準確的邊緣信息。