一種基于語義地圖的激光雷達定位方法

金泰宇 黃勁松

1武漢大學測繪學院，湖北 武漢，430079

SLAM（simultaneous localization and mapping）技術能夠解決機器人在未知環境下的定位、導航和建圖問題，但是SLAM算法是基于馬爾科夫性質的，即后一時刻的位姿由前一時刻的位姿推算而來，那么多次位置推算后，定位誤差會越來越大，不能滿足機器人的定位需求。此外，SLAM屬于局部定位的方式，只能提供一個相對位置，在沒有位姿初值的情況下無法確定自身在真實空間的絕對坐標，而當有額外的地圖輔助時，利用地圖絕對位置信息進行全局匹配定位能夠幫助機器人獲得更精確的位置。

語義地圖是一種面向環境理解的地圖模型，不似傳統的柵格地圖、拓撲地圖等，它不僅包含周圍環境的空間幾何結構信息，同時也可提供環境的語義信息，即環境中物體的類別及位置信息等，能夠使機器人完成高水平的人機交互。Vasudevan等［1］首先提出認知地圖的思想，在拓撲地圖上融合環境目標的語義信息，提升機器人理解、解釋和表示環境的能力；Nietogranda等［2］基于語義信息對空間，區域進行分類并用于拓撲地圖的構建；Dube等［3］提出Segmap算法，使用區域增長法分割點云并用數據驅動描述子進行語義信息提取，完成定位和地圖重構。

目前市場上有豐富成熟的導航電子地圖資源如高德地圖等，這種地圖可提供環境的語義信息如目標的類別、位置信息等。假如能把LiDAR單幀掃描到的樹木、電線桿等目標的平面位置信息提取出來，就能與現成的電子地圖語義配準，給機器人平面位置上的約束，幫助機器人更魯棒的定位［4］。

因此，本文基于使用語義地圖完成LiDAR定位的思想，利用語義提取算法提取出激光點云中靜態目標的語義信息（包含類別和平面位置信息），與事先已知的包含同類型目標的語義信息的全局語義地圖進行配準實現定位。該方法使用靜態目標語義特征，配準穩定性高，運行所需的計算資源較小，且能給出一個可靠的絕對參考位置。

1 基于語義地圖定位

1.1 定位流程

語義地圖定位流程如圖1所示。單幀點云處理部分，采用多線激光雷達作為數據輸入端，利用濾波算法［5-9］去除點云的離群點與空洞，利用形態學濾波算法［7，10］剔除背景信息，然后利用語義提取算法［8，11］提取目標語義信息，并確定語義參考點，生成語義幀；語義地圖定位部分，將單幀語義點云和事先已知的全局語義地圖進行配準［12-15］完成定位，確定Li-DAR在全局坐標系下的位置。

圖1 定位流程Fig.1 Positioning Process

1.2 數據濾波

針對3種點云噪聲進行處理：①離群點剔除（由遮擋等問題造成）；②數據下采樣（海量數據）；③點云密度不規則。

采用統計濾波去除離群點，其方法為：對查詢點的鄰域進行統計分析，計算它到近鄰點的距離。距離分布特性符合高斯分布N(u，σ2)，u和σ2決定一個標準范圍，距離在標準范圍之外的點為離群點，從數據中去除。

采用體素柵格下采樣法簡化海量點云。該方法構造m×n×l的三維體素柵格，將點云數據填充至對應的小體素柵格，用每個體素內的所有點的重心替代該體素的其他點，減少了數據量，提高了程序的運行速度。

激光雷達的點云密度有距離中心點越遠的地方越稀疏的特點，本文采用直通濾波剔除遠處的、稀疏的部分，保留密集的、包含絕大部分特征的點云，節省計算資源。

1.3 前景背景信息分離

采集的點云含有大量的地面點（即背景信息，高達40%），應盡可能分離點云中的地面點，保留感興趣的地物點即前景信息。

本文采用形態學濾波算法剔除地面點云。首先將LiDAR點云內插成二維網格數據，然后用形態學算子即腐蝕算子和膨脹算子依次處理網格數據。

腐蝕算子ep和膨脹算子d p滿足關系：

式中，w為形態學算子的尺寸；(x p，y p，zp)為網格化后的數據。

利用形態學算子過濾地面點時，首先必須合理設置算子的尺寸，如尺寸過大則建筑物點云和突起的地形點云可能同時被濾除；其次，濾除后的點云仍可能含部分的地物點，給定合適的高程閾值避免樹木等地物點被判為地面點。

圖2為利用形態學算子濾波操作濾除點云的效果。圖中紅色部分為地面點，占42%；灰色部分為地物點，占58%。

圖2 地面提取Fig.2 Ground Extraction

1.4 點云語義提取

本文基于實驗環境的關系，選取樹木作為語義提取對象，需要用到點云分割算法。

首先，采用歐氏聚類算法基于歐氏距離將點云分割成多個子簇。歐式距離滿足的關系式為：

式 中，p的 位 置 為(X p，Y p，Z p)；q的 位 置 為(X q，Y q，Z q)；X pq、Y pq、Zpq分 別 為p、q兩 點 的 坐 標差值。

然后，采用隨機采樣一致性分割算法［16］的圓柱面模型遍歷所有子簇，找出含有樹木的子簇。圓柱的模型表達式為：

式中，(a，b，c）是軸線上的一點；(d?，e?，f?）為軸線的方向向量；r為半徑；(x，y，z）為圓柱面上任一點。

提取點云中的樹木之后，還需設置語義參考點，確定其位置信息。假設軸線與水平方向的夾角無限接近90°時，可統一選取軸線上z=0的點（即樹木中心）作為語義參考點，其平面坐標(x，y）為：

式（4）中各量的意義與式（3）相同。

語義提取出的參考點具有精確的位置信息，完成了從三維的、無序的、不規則的稀疏點云到二維的位置信息的樹木中心的語義抽象。

三維稀疏點云轉換成二維的位置信息，從信息多樣性角度來看會造成信息的少量損失，但從配準的角度看，本文的靜態目標特征穩定，配準效率能大幅提升，同時保持可靠的位置精度。因為LiDAR數據采集技術的局限性造成LiDAR不會在連續兩個時刻掃描到同一點，但LiDAR的測程大及垂直方向的視場角寬，相鄰時刻掃描到同一顆靜止的樹的概率遠高于掃描到單個點的概率，因此基于靜態的樹木中心的配準穩定性非常高。點云語義提取處理結果如圖3所示，樹木提取準確率可達90%。

圖3 語義提取結果Fig.3 Semantic Extraction Results

1.5 語義地圖定位

基于語義地圖的定位方法屬于全局語義定位方式。語義地圖中存儲著樹木（路標）中心的平面位置信息，并對LiDAR當前位置上掃描的點云進行語義提取得到語義幀，語義幀中存儲的也是樹木中心平面位置。將全局語義地圖和當前位置生成的語義幀進行配準，得出兩者之間的旋轉平移關系，即可確定當前時刻LiDAR在全局坐標系下的位置。

配準算法是語義地圖定位的關鍵一步?？紤]到語義幀中語義目標特征比較稀疏，本文在配準時采用迭代最近點（iterative closset point，ICP）算法進行配準定位［17］。ICP算法根據已知的全局語義地圖和當前時刻的語義幀，基于最小二乘法及距離誤差最小的收斂準則，重復選擇兩者的對應關系點對，直到找出滿足收斂準則的最優旋轉矩陣R和平移矩陣t。誤差函數E(R，t)為：

式中，mi、ni為全局語義地圖和語義幀的對應點對；k為對應點對的個數。

根據ICP配準得到兩者的旋轉和平移關系，能將語義幀的位姿轉換到全局語義地圖的坐標基準下，數學關系為：

式中，X s為當前時刻的位姿；X m為LiDAR在全局坐標系下的位姿。

選取兩幀點云，本文做了對比實驗：①基于原始點云的ICP配準（見圖4）；②基于二維樹中心的ICP配準（見圖5）。比較其配準的（平移）結果和耗時，結果如表1、表2所示。

圖4 基于原始點云的ICP配準Fig.4 ICP Registration Based on Original Point Cloud

圖5 基于二維樹中心的ICP配準Fig.5 ICP Registration Based on Two-Dimensional Tree Center

表1 配準結果/mTab.1 Registration Results/m

表2 耗時結果/sTab.2 Time-Consuming Results/s

由實驗結果可知，實驗②相比于實驗①在靜態環境下的配準耗時減少了46.1%；配準結果方面，可看出兩者在X和Y兩個維度的平移（定位）誤差控制在0.2 m之內。由此可知，本文提出的定位方法在算法耗時上有顯著提升，這對數據的實時處理有著較大的優勢，同時也能給出可靠的定位結果。

2 實驗結果及分析

本文使用的語義地圖是在開源SLAM框架-hdl_graph_slam［4］基礎上，編寫語義提取模塊，構建的全局語義地圖。本文選取的實驗場景為武漢大學信息學部求是一路旁田徑場四周的林蔭大道。數據采集設備為鐳神16線混合固態激光雷達，數據采集路程全長共544.62 m。圖6為實驗場景，紅色箭頭方向代表數據采集方向。

圖6 實驗場景及路徑Fig.6 Experimental Scene and Path

本文基于語義地圖定位的思想和方法，選取3個實驗算例進行定位結果分析。3個算例對應的位置分別為：①求是一路體育場旁；②求是二路學生宿舍二舍旁；③一號教學樓正面靠近求是二路旁。定位結果如圖7所示。

圖7 3個算例定位結果Fig.7 Positioning Results of Three Examples

圖7中，黑色點為地圖點，每個點對應相應樹木中心的平面坐標，共計142個點。

算例①，在該掃描位置上進行語義提取，共提取到8個點（圖7中紅色點），其中7個點為樹木的平面坐標，且與地圖點一一對應。在全局坐標系下，該位置坐標的真值為（61.843，-53.776）m，本文的定位結果為（61.544，-53.685）m。

算例②，共提取到11個點（圖7中綠色點），其中9個點為樹木的平面目標并與地圖點對應。在全局坐標系下，該位置坐標真值為（121.958，60.712）m，本文的定位結果為（122.646，60.476）m。

算例③，共提取到7個點（圖7中洋紅色點），這7個點為樹木的平面目標并與地圖點對應。在全局坐標系下，該位置坐標真值為（-24.793，154.33）m，本文的定位結果為（-24.848，154.431）m。

3個算例的語義提取結果如表3所示，定位結果與真值的誤差如表4所示。

表3 3個算例語義提取結果Tab.3 Semantic Extraction Results ofThree Examples

表4 3個算例定位誤差/mTab.4 Positioning Errors of Three Examples/m

由實驗結果可知，本文的方法是可行的，且使用到的靜態特征是穩定的，能夠提供一個穩定的全局絕對位置。未來若要提高定位精度，可從兩個方向進行優化：

1）使用更高精度的并且包含更豐富語義信息的地圖?，F階段使用的語義地圖，是借助語義激光SLAM構建的，它存在誤差積累問題，長時間會影響環境地圖的精度。另外，地圖包含的語義信息更豐富，那么一定程度上語義定位精度也會增加。

2）優化語義提取算法。本文雖已成功實現提取樹木的中心的平面位置，但此種方法還有一定的局限性，例如當樹木的軸線與水平面的夾角是銳角時，提取到的樹心平面位置就會存在誤差，未來還需要找出方法進行約束。此外，語義提取算法的結果對閾值（角度閾值、距離閾值等）比較敏感，閾值的波動也會影響語義提取出的樹木的數量和中心坐標的數據質量，進而影響定位結果的精度。

3 結束語

本文提出利用全局語義地圖進行LiDAR定位，利用形態學濾波算法進行地面點云分離，利用歐式分割算法和采樣一致性分割算法提取出穩定的靜態目標語義信息，與已知的全局語義地圖配準，得到LiDAR的絕對位置信息，同時也可以發揮傳統地圖資源的使用價值。實驗結果表明，該方法是可行的并且能夠提供可靠的全局定位結果。本文是語義地圖應用于智能移動機器人自主定位領域的一個探索，未來隨著點云算法的優化，語義地圖將表達更豐富多樣的場景語義信息，智能移動機器人也就能應對更具挑戰化的自主定位和導航任務。