一種基于語(yǔ)義地圖的激光雷達(dá)定位方法

金泰宇 黃勁松

1武漢大學(xué)測(cè)繪學(xué)院，湖北 武漢，430079

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

語(yǔ)義地圖是一種面向環(huán)境理解的地圖模型，不似傳統(tǒng)的柵格地圖、拓?fù)涞貓D等，它不僅包含周?chē)h(huán)境的空間幾何結(jié)構(gòu)信息，同時(shí)也可提供環(huán)境的語(yǔ)義信息，即環(huán)境中物體的類(lèi)別及位置信息等，能夠使機(jī)器人完成高水平的人機(jī)交互。Vasudevan等［1］首先提出認(rèn)知地圖的思想，在拓?fù)涞貓D上融合環(huán)境目標(biāo)的語(yǔ)義信息，提升機(jī)器人理解、解釋和表示環(huán)境的能力；Nietogranda等［2］基于語(yǔ)義信息對(duì)空間，區(qū)域進(jìn)行分類(lèi)并用于拓?fù)涞貓D的構(gòu)建；Dube等［3］提出Segmap算法，使用區(qū)域增長(zhǎng)法分割點(diǎn)云并用數(shù)據(jù)驅(qū)動(dòng)描述子進(jìn)行語(yǔ)義信息提取，完成定位和地圖重構(gòu)。

目前市場(chǎng)上有豐富成熟的導(dǎo)航電子地圖資源如高德地圖等，這種地圖可提供環(huán)境的語(yǔ)義信息如目標(biāo)的類(lèi)別、位置信息等。假如能把LiDAR單幀掃描到的樹(shù)木、電線桿等目標(biāo)的平面位置信息提取出來(lái)，就能與現(xiàn)成的電子地圖語(yǔ)義配準(zhǔn)，給機(jī)器人平面位置上的約束，幫助機(jī)器人更魯棒的定位［4］。

因此，本文基于使用語(yǔ)義地圖完成LiDAR定位的思想，利用語(yǔ)義提取算法提取出激光點(diǎn)云中靜態(tài)目標(biāo)的語(yǔ)義信息（包含類(lèi)別和平面位置信息），與事先已知的包含同類(lèi)型目標(biāo)的語(yǔ)義信息的全局語(yǔ)義地圖進(jìn)行配準(zhǔn)實(shí)現(xiàn)定位。該方法使用靜態(tài)目標(biāo)語(yǔ)義特征，配準(zhǔn)穩(wěn)定性高，運(yùn)行所需的計(jì)算資源較小，且能給出一個(gè)可靠的絕對(duì)參考位置。

1 基于語(yǔ)義地圖定位

1.1 定位流程

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

圖1 定位流程Fig.1 Positioning Process

1.2 數(shù)據(jù)濾波

針對(duì)3種點(diǎn)云噪聲進(jìn)行處理：①離群點(diǎn)剔除（由遮擋等問(wèn)題造成）；②數(shù)據(jù)下采樣（海量數(shù)據(jù)）；③點(diǎn)云密度不規(guī)則。

采用統(tǒng)計(jì)濾波去除離群點(diǎn)，其方法為：對(duì)查詢點(diǎn)的鄰域進(jìn)行統(tǒng)計(jì)分析，計(jì)算它到近鄰點(diǎn)的距離。距離分布特性符合高斯分布N(u，σ2)，u和σ2決定一個(gè)標(biāo)準(zhǔn)范圍，距離在標(biāo)準(zhǔn)范圍之外的點(diǎn)為離群點(diǎn)，從數(shù)據(jù)中去除。

采用體素柵格下采樣法簡(jiǎn)化海量點(diǎn)云。該方法構(gòu)造m×n×l的三維體素柵格，將點(diǎn)云數(shù)據(jù)填充至對(duì)應(yīng)的小體素柵格，用每個(gè)體素內(nèi)的所有點(diǎn)的重心替代該體素的其他點(diǎn)，減少了數(shù)據(jù)量，提高了程序的運(yùn)行速度。

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

1.3 前景背景信息分離

采集的點(diǎn)云含有大量的地面點(diǎn)（即背景信息，高達(dá)40%），應(yīng)盡可能分離點(diǎn)云中的地面點(diǎn)，保留感興趣的地物點(diǎn)即前景信息。

本文采用形態(tài)學(xué)濾波算法剔除地面點(diǎn)云。首先將LiDAR點(diǎn)云內(nèi)插成二維網(wǎng)格數(shù)據(jù)，然后用形態(tài)學(xué)算子即腐蝕算子和膨脹算子依次處理網(wǎng)格數(shù)據(jù)。

腐蝕算子ep和膨脹算子d p滿足關(guān)系：

式中，w為形態(tài)學(xué)算子的尺寸；(x p，y p，zp)為網(wǎng)格化后的數(shù)據(jù)。

利用形態(tài)學(xué)算子過(guò)濾地面點(diǎn)時(shí)，首先必須合理設(shè)置算子的尺寸，如尺寸過(guò)大則建筑物點(diǎn)云和突起的地形點(diǎn)云可能同時(shí)被濾除；其次，濾除后的點(diǎn)云仍可能含部分的地物點(diǎn)，給定合適的高程閾值避免樹(shù)木等地物點(diǎn)被判為地面點(diǎn)。

圖2為利用形態(tài)學(xué)算子濾波操作濾除點(diǎn)云的效果。圖中紅色部分為地面點(diǎn)，占42%；灰色部分為地物點(diǎn)，占58%。

圖2 地面提取Fig.2 Ground Extraction

1.4 點(diǎn)云語(yǔ)義提取

本文基于實(shí)驗(yàn)環(huán)境的關(guān)系，選取樹(shù)木作為語(yǔ)義提取對(duì)象，需要用到點(diǎn)云分割算法。

首先，采用歐氏聚類(lèi)算法基于歐氏距離將點(diǎn)云分割成多個(gè)子簇。歐式距離滿足的關(guān)系式為：

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

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

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

提取點(diǎn)云中的樹(shù)木之后，還需設(shè)置語(yǔ)義參考點(diǎn)，確定其位置信息。假設(shè)軸線與水平方向的夾角無(wú)限接近90°時(shí)，可統(tǒng)一選取軸線上z=0的點(diǎn)（即樹(shù)木中心）作為語(yǔ)義參考點(diǎn)，其平面坐標(biāo)(x，y）為：

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

語(yǔ)義提取出的參考點(diǎn)具有精確的位置信息，完成了從三維的、無(wú)序的、不規(guī)則的稀疏點(diǎn)云到二維的位置信息的樹(shù)木中心的語(yǔ)義抽象。

三維稀疏點(diǎn)云轉(zhuǎn)換成二維的位置信息，從信息多樣性角度來(lái)看會(huì)造成信息的少量損失，但從配準(zhǔn)的角度看，本文的靜態(tài)目標(biāo)特征穩(wěn)定，配準(zhǔn)效率能大幅提升，同時(shí)保持可靠的位置精度。因?yàn)長(zhǎng)iDAR數(shù)據(jù)采集技術(shù)的局限性造成LiDAR不會(huì)在連續(xù)兩個(gè)時(shí)刻掃描到同一點(diǎn)，但LiDAR的測(cè)程大及垂直方向的視場(chǎng)角寬，相鄰時(shí)刻掃描到同一顆靜止的樹(shù)的概率遠(yuǎn)高于掃描到單個(gè)點(diǎn)的概率，因此基于靜態(tài)的樹(shù)木中心的配準(zhǔn)穩(wěn)定性非常高。點(diǎn)云語(yǔ)義提取處理結(jié)果如圖3所示，樹(shù)木提取準(zhǔn)確率可達(dá)90%。

圖3 語(yǔ)義提取結(jié)果Fig.3 Semantic Extraction Results

1.5 語(yǔ)義地圖定位

基于語(yǔ)義地圖的定位方法屬于全局語(yǔ)義定位方式。語(yǔ)義地圖中存儲(chǔ)著樹(shù)木（路標(biāo)）中心的平面位置信息，并對(duì)LiDAR當(dāng)前位置上掃描的點(diǎn)云進(jìn)行語(yǔ)義提取得到語(yǔ)義幀，語(yǔ)義幀中存儲(chǔ)的也是樹(shù)木中心平面位置。將全局語(yǔ)義地圖和當(dāng)前位置生成的語(yǔ)義幀進(jìn)行配準(zhǔn)，得出兩者之間的旋轉(zhuǎn)平移關(guān)系，即可確定當(dāng)前時(shí)刻LiDAR在全局坐標(biāo)系下的位置。

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

式中，mi、ni為全局語(yǔ)義地圖和語(yǔ)義幀的對(duì)應(yīng)點(diǎn)對(duì)；k為對(duì)應(yīng)點(diǎn)對(duì)的個(gè)數(shù)。

根據(jù)ICP配準(zhǔn)得到兩者的旋轉(zhuǎn)和平移關(guān)系，能將語(yǔ)義幀的位姿轉(zhuǎn)換到全局語(yǔ)義地圖的坐標(biāo)基準(zhǔn)下，數(shù)學(xué)關(guān)系為：

式中，X s為當(dāng)前時(shí)刻的位姿；X m為L(zhǎng)iDAR在全局坐標(biāo)系下的位姿。

選取兩幀點(diǎn)云，本文做了對(duì)比實(shí)驗(yàn)：①基于原始點(diǎn)云的ICP配準(zhǔn)（見(jiàn)圖4）；②基于二維樹(shù)中心的ICP配準(zhǔn)（見(jiàn)圖5）。比較其配準(zhǔn)的（平移）結(jié)果和耗時(shí)，結(jié)果如表1、表2所示。

圖4 基于原始點(diǎn)云的ICP配準(zhǔn)Fig.4 ICP Registration Based on Original Point Cloud

圖5 基于二維樹(shù)中心的ICP配準(zhǔn)Fig.5 ICP Registration Based on Two-Dimensional Tree Center

表1 配準(zhǔn)結(jié)果/mTab.1 Registration Results/m

表2 耗時(shí)結(jié)果/sTab.2 Time-Consuming Results/s

由實(shí)驗(yàn)結(jié)果可知，實(shí)驗(yàn)②相比于實(shí)驗(yàn)①在靜態(tài)環(huán)境下的配準(zhǔn)耗時(shí)減少了46.1%；配準(zhǔn)結(jié)果方面，可看出兩者在X和Y兩個(gè)維度的平移（定位）誤差控制在0.2 m之內(nèi)。由此可知，本文提出的定位方法在算法耗時(shí)上有顯著提升，這對(duì)數(shù)據(jù)的實(shí)時(shí)處理有著較大的優(yōu)勢(shì)，同時(shí)也能給出可靠的定位結(jié)果。

2 實(shí)驗(yàn)結(jié)果及分析

本文使用的語(yǔ)義地圖是在開(kāi)源SLAM框架-hdl_graph_slam［4］基礎(chǔ)上，編寫(xiě)語(yǔ)義提取模塊，構(gòu)建的全局語(yǔ)義地圖。本文選取的實(shí)驗(yàn)場(chǎng)景為武漢大學(xué)信息學(xué)部求是一路旁田徑場(chǎng)四周的林蔭大道。數(shù)據(jù)采集設(shè)備為鐳神16線混合固態(tài)激光雷達(dá)，數(shù)據(jù)采集路程全長(zhǎng)共544.62 m。圖6為實(shí)驗(yàn)場(chǎng)景，紅色箭頭方向代表數(shù)據(jù)采集方向。

圖6 實(shí)驗(yàn)場(chǎng)景及路徑Fig.6 Experimental Scene and Path

本文基于語(yǔ)義地圖定位的思想和方法，選取3個(gè)實(shí)驗(yàn)算例進(jìn)行定位結(jié)果分析。3個(gè)算例對(duì)應(yīng)的位置分別為：①求是一路體育場(chǎng)旁；②求是二路學(xué)生宿舍二舍旁；③一號(hào)教學(xué)樓正面靠近求是二路旁。定位結(jié)果如圖7所示。

圖7 3個(gè)算例定位結(jié)果Fig.7 Positioning Results of Three Examples

圖7中，黑色點(diǎn)為地圖點(diǎn)，每個(gè)點(diǎn)對(duì)應(yīng)相應(yīng)樹(shù)木中心的平面坐標(biāo)，共計(jì)142個(gè)點(diǎn)。

算例①，在該掃描位置上進(jìn)行語(yǔ)義提取，共提取到8個(gè)點(diǎn)（圖7中紅色點(diǎn)），其中7個(gè)點(diǎn)為樹(shù)木的平面坐標(biāo)，且與地圖點(diǎn)一一對(duì)應(yīng)。在全局坐標(biāo)系下，該位置坐標(biāo)的真值為（61.843，-53.776）m，本文的定位結(jié)果為（61.544，-53.685）m。

算例②，共提取到11個(gè)點(diǎn)（圖7中綠色點(diǎn)），其中9個(gè)點(diǎn)為樹(shù)木的平面目標(biāo)并與地圖點(diǎn)對(duì)應(yīng)。在全局坐標(biāo)系下，該位置坐標(biāo)真值為（121.958，60.712）m，本文的定位結(jié)果為（122.646，60.476）m。

算例③，共提取到7個(gè)點(diǎn)（圖7中洋紅色點(diǎn)），這7個(gè)點(diǎn)為樹(shù)木的平面目標(biāo)并與地圖點(diǎn)對(duì)應(yīng)。在全局坐標(biāo)系下，該位置坐標(biāo)真值為（-24.793，154.33）m，本文的定位結(jié)果為（-24.848，154.431）m。

3個(gè)算例的語(yǔ)義提取結(jié)果如表3所示，定位結(jié)果與真值的誤差如表4所示。

表3 3個(gè)算例語(yǔ)義提取結(jié)果Tab.3 Semantic Extraction Results ofThree Examples

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

由實(shí)驗(yàn)結(jié)果可知，本文的方法是可行的，且使用到的靜態(tài)特征是穩(wěn)定的，能夠提供一個(gè)穩(wěn)定的全局絕對(duì)位置。未來(lái)若要提高定位精度，可從兩個(gè)方向進(jìn)行優(yōu)化：

1）使用更高精度的并且包含更豐富語(yǔ)義信息的地圖?，F(xiàn)階段使用的語(yǔ)義地圖，是借助語(yǔ)義激光SLAM構(gòu)建的，它存在誤差積累問(wèn)題，長(zhǎng)時(shí)間會(huì)影響環(huán)境地圖的精度。另外，地圖包含的語(yǔ)義信息更豐富，那么一定程度上語(yǔ)義定位精度也會(huì)增加。

2）優(yōu)化語(yǔ)義提取算法。本文雖已成功實(shí)現(xiàn)提取樹(shù)木的中心的平面位置，但此種方法還有一定的局限性，例如當(dāng)樹(shù)木的軸線與水平面的夾角是銳角時(shí)，提取到的樹(shù)心平面位置就會(huì)存在誤差，未來(lái)還需要找出方法進(jìn)行約束。此外，語(yǔ)義提取算法的結(jié)果對(duì)閾值（角度閾值、距離閾值等）比較敏感，閾值的波動(dòng)也會(huì)影響語(yǔ)義提取出的樹(shù)木的數(shù)量和中心坐標(biāo)的數(shù)據(jù)質(zhì)量，進(jìn)而影響定位結(jié)果的精度。

3 結(jié)束語(yǔ)

本文提出利用全局語(yǔ)義地圖進(jìn)行LiDAR定位，利用形態(tài)學(xué)濾波算法進(jìn)行地面點(diǎn)云分離，利用歐式分割算法和采樣一致性分割算法提取出穩(wěn)定的靜態(tài)目標(biāo)語(yǔ)義信息，與已知的全局語(yǔ)義地圖配準(zhǔn)，得到LiDAR的絕對(duì)位置信息，同時(shí)也可以發(fā)揮傳統(tǒng)地圖資源的使用價(jià)值。實(shí)驗(yàn)結(jié)果表明，該方法是可行的并且能夠提供可靠的全局定位結(jié)果。本文是語(yǔ)義地圖應(yīng)用于智能移動(dòng)機(jī)器人自主定位領(lǐng)域的一個(gè)探索，未來(lái)隨著點(diǎn)云算法的優(yōu)化，語(yǔ)義地圖將表達(dá)更豐富多樣的場(chǎng)景語(yǔ)義信息，智能移動(dòng)機(jī)器人也就能應(yīng)對(duì)更具挑戰(zhàn)化的自主定位和導(dǎo)航任務(wù)。