基于激光雷達的移動機器人三維建圖與定位

殷江，林建德，孔令華，鄒誠，游通飛，易定容

(1.福建工程學院 設計學院 福建 福州 350118;2.數(shù)字福建工業(yè)制造物聯(lián)網(wǎng)實驗室 福建 福州 350118;3.福建工程學院 機械與汽車工程學院 福建 福州 350118;4.華僑大學 機電及自動化學院 福建 廈門 362021)

在進行太空探索、軍事考察與礦洞探測等室外復雜非結(jié)構(gòu)化環(huán)境探索時，三維地圖構(gòu)建和定位技術(shù)是實現(xiàn)移動機器人進行自主導航的前提與基礎。三維SLAM系統(tǒng)所使用的外部傳感器主要有兩種：一種是視覺傳感器，另一種是專用3D掃描儀或結(jié)構(gòu)光設備。這種主動傳感器有著測距精確與測量范圍廣的優(yōu)勢，能夠獲取精確的三維點云信息，但開發(fā)成本昂貴，缺少較好的三維激光回環(huán)檢測方法。

目前三維激光實時建圖與定位的研究已經(jīng)成為熱點，如伯克利人工智能實驗室的Erik Nelson[1]使用GICP算法計算點云的初始變換，之后獲取當前幀對應子地圖的最近鄰點，最后再次使用算法配準獲得較為精確的轉(zhuǎn)換關系，他們的開源代碼是BLAM SLAM。HESS W 等[2]研究了利用回環(huán)檢測來消除建圖中出現(xiàn)的累計誤差問題。Zhang等[3]提出了雷達測距與實時建圖系統(tǒng)loam，先消除運動畸變，然后進行快速激光配準，得到準確的里程計信息。

本研究采用大疆公司旗下生產(chǎn)的Livox棱鏡旋轉(zhuǎn)式雷達作為數(shù)據(jù)采集模塊，引入Renaud等人[4]提出的點云匹配約束作為回環(huán)檢測模塊；最后使用GSTAM(georgia tech smoothing and mapping)[5]中的iSAM庫[6]來完成圖優(yōu)化工作，并將立體占用地圖轉(zhuǎn)換為點云地圖輸出，助力機器人導航與定位工作。

1 Livox系統(tǒng)框架

1.1 Livox Lidar傳感器

三維激光點云SLAM整體系統(tǒng)包括數(shù)據(jù)獲取、回環(huán)檢測、激光里程計和圖優(yōu)化4個模塊。

移動機器人利用外部傳感器采集環(huán)境信息，通過對感知到的點云進行點云配準、圖優(yōu)化等處理，構(gòu)建所經(jīng)過環(huán)境的地圖模型和自主定位。研究采用的外部傳感器是大疆公司生產(chǎn)的Livox Mid-40激光雷達，Livox激光雷達是一種非機械非固態(tài)的棱鏡旋轉(zhuǎn)式激光雷達，采用獨特的非重復掃描方式，視場覆蓋率隨積分時間推移而顯著提升，能夠減少視場內(nèi)物體被漏檢的概率。

1.2 里程計模塊

里程計模塊的目的是對移動機器人位姿進行估計，良好的估計有助于之后的優(yōu)化工作，能夠減少誤差帶來的計算量增大問題，減少匹配的搜索空間等。

采用LOAM算法通過幀與幀、幀與地圖匹配結(jié)合的方式輸出10 Hz粗糙里程計，在估計出較為精準的位姿信息后輸出給后端圖優(yōu)化模塊。激光里程計流程圖如下所示：

圖1 激光里程計流程圖

實現(xiàn)三維點云相對精準的幀間匹配，再使用點云片段匹配方法實現(xiàn)回環(huán)檢測。

1.3 回環(huán)檢測模塊

1.3.1 點云分割

考慮到三維點云數(shù)據(jù)量龐大計算資源有限，首先對全局參考系的每個點云P通過以機器人為中心R為半徑的圓柱形領域提取獲得局部點云，將體素網(wǎng)格應用于局部點云，以濾除重復點和噪聲點；再通過基于垂直平局值和方差的點簇相鄰體素“Cluster-All”方法[7]先將地面剔除，然后使用歐幾里得聚類增長區(qū)域，最后計算每個點簇Ci中點的平均值得到質(zhì)心ci。

1.3.2 特征提取

1.3.3 點云匹配

針對涉及多種要素類型很難選擇合適的距離度量和閾值，使用分類器最終決定兩段是否表示相同目標或目標的部分。為了保持高效率，首先在特征空間中使用K-D樹檢索來搜索候選的匹配對，然后將其饋送給隨機森林分類器進行分類和保證時間性能。此分類器的工作機制是通過構(gòu)建大量決策樹投票出獲勝類。

1.3.4 幾何驗證

候選的匹配對被送到幾何驗證模塊，使用隨機抽樣一致性(random sample consensus ,RANSAC)[9]濾除錯誤對。使用片段質(zhì)心估計并獲得一個6自由度的旋轉(zhuǎn)變換和一個表示位姿識別的匹配片段列表。Segmatch流程圖見圖2所示。

本研究將點云信息和里程計估計的位姿信息作為輸入信息送入上述模塊，經(jīng)點云預處理和分割后的點云片段進行索引提取，對當前點云索引與本地存儲的片段點云進行匹配，一旦匹配成功就對回環(huán)區(qū)域進行調(diào)整。

1.4 立體占用地圖

目前常見的三維地圖有高程地圖[10]、點云地圖[11]和立體占用地圖[12-14]等。高程地圖只保存了每一柵格的表面高度，解決了內(nèi)存高消耗問題，但無法表示復雜結(jié)構(gòu)。點云地圖保存了空間點的坐標及反射強度等信息，但對硬盤及內(nèi)存空間需求大。立體占用地圖使用立方體狀態(tài)表征此立方體，解決了內(nèi)存高消耗和無法精確表示復雜結(jié)構(gòu)的問題，因此被采用。

基于八叉樹的地圖表示方法Octomap是一種立體占用地圖，通過建立體素占用概率模型減少了地圖對計算機空間的消耗，并提高了地圖表示精度,代表了目前三維地圖表示方法中較高的水準。

三維地圖Octomap基于OcTree實現(xiàn)對空間的構(gòu)建[15]。OcTree是一種數(shù)據(jù)結(jié)構(gòu)，此結(jié)構(gòu)每個節(jié)點(通常被稱為體素)表示一個立體空間。體素會被八等細分至最小分辨率，圖3表示八叉樹分割原理。

圖3 存儲空間與占用單元的八叉樹

一般情況下，八叉樹的分割層最大為16。坐標軸的尺度由式(1)表示：

(1)

式中l(wèi)evel表示八叉樹的層數(shù)。

將坐標原點位置設置為式(2)以此消除空間坐標中出現(xiàn)的負數(shù)。

(2)

Octomap使用概率方式表征當前節(jié)點狀態(tài)，解決地圖受到噪聲或物體移動的影響。式(3)表示在時間測量范圍z1 : t內(nèi)葉子結(jié)點被占用概率P(n|z1 : t)。

P(n|z1 : t)=

(3)

對P(n|z1 : t)進行l(wèi)og-odds變換，將概率空間解轉(zhuǎn)換到實數(shù)空間中，得到：

(4)

(5)

L(n|z1：T)=L(n|z1：T-1)+L(n|zT)

(6)

因此，將空間的狀態(tài)概率問題轉(zhuǎn)變成實數(shù)空間的加和問題。

實驗中使用移動機器人對室外環(huán)境進行SLAM建圖，獲得包含275 913個點的點云地圖，將其換成八叉樹地圖，對前后兩個地圖進行對比，結(jié)果描述同樣點云數(shù)據(jù)量的點云地圖要占8.8 M，而八叉樹地圖只占用1.4 M，表明八叉樹地圖確實可以優(yōu)化對數(shù)據(jù)空間占用情況。

2 實驗與分析

研究采用的實驗平臺數(shù)據(jù)采集端為棱鏡旋轉(zhuǎn)式激光雷達Livox Mid-40 ，其參數(shù)見表1。數(shù)據(jù)處理端為一臺基于Ubuntu16.04操作系統(tǒng)的i7處理器1.8 Hz CPU睿頻3.0 GHz ，128 G固態(tài)硬盤，8 G內(nèi)存。為了驗證算法的有效性，首先采用livox雷達完成數(shù)據(jù)獲取，使用LOAM算法對教學樓外進行三維重建。圖4(a)為實驗環(huán)境，圖4(b)是出現(xiàn)漂移現(xiàn)象。

表1 Livox-mid40部分參數(shù)表

圖4 真實環(huán)境重建

圖4(a)是構(gòu)建地圖的對象教學樓，使用基于Livox雷達的LOAM算法在三維建圖中表現(xiàn)有很好的實時性和較高的魯棒性[16]，獨特的非重復性采樣策略令雷達能夠精確的探測視場中每個細節(jié)，距離精度和角度精度分別能夠達到2 cm和0.05°。但是從圖4(b)對應圖4(a)構(gòu)建的地圖上紅色圈的位置看到馬路拐彎處發(fā)生了漂移現(xiàn)象，不能進行教學樓周圍的環(huán)境重建工作。發(fā)生這種情況的主要原因是缺少有效的回環(huán)檢測模塊。LOAM算法的前端是通過點到線以及點到面的方式完成匹配，每次估計都帶有一定誤差，由于里程計的工作方式，先前時刻產(chǎn)生的誤差會傳遞到下一時刻導致估計的軌跡出錯，發(fā)生累計漂移。有效的回環(huán)檢測通過判斷機器人是否回到先前走過的位置。一旦回環(huán)成功就會將信息傳遞給后端優(yōu)化模塊進行優(yōu)化最后獲得全局一致的軌跡和地圖。Segmatch算法不依賴“完美分割”的假設或者環(huán)境中“對象”的存在，通過對點云片段的提取、描述、匹配和驗證4個模塊可以有效實現(xiàn)回環(huán)功能。

首先確定LOAM算法的累積誤差值，通過誤差值確定漂移程度。試驗中將高精度全站測量儀放置于機器人運動起點附近，機器人上粘貼標志點，以標志點位姿變換作為相對變換值GT，累計誤差均方根由估計的位姿精度決定[17]：

en(wxn?wx0)?GT

(7)

(8)

實驗結(jié)果為X方向：0.036 m，Y方向：2.24 m，Z方向：-0.054 m，位置偏差：3.05 m，結(jié)果顯示LOAM算法在當前機器人平臺上的累積誤差較大。

如圖5所示，在142 s左右的時候(圖5圓圈附近)出現(xiàn)漂移現(xiàn)象，而在172 s(圖5矩形附近)的時候發(fā)生回環(huán)檢測調(diào)整。Segmatch-LOAM算法使用了三維片段匹配完成累計誤差消除，有效解決了在室外三維環(huán)境建圖中存在的累積漂移問題。

圖5 回環(huán)檢測的校正結(jié)果

優(yōu)化后的slam系統(tǒng)均方根誤差由原來的27.7降低到17.4，均值由原來的24.18降低到13.6，誤差率由原來的0.71%降低到0.42%。Segmatch-LOAM系統(tǒng)各項參數(shù)指標都小于優(yōu)化前的系統(tǒng)，本研究的優(yōu)化方法極大減少系統(tǒng)前端帶來的累計誤差。

3 結(jié)論

在室外三維建圖中處理大量數(shù)據(jù)時，一旦前端幀間匹配的誤差較大就會導致累計誤差產(chǎn)生，一個好的回環(huán)檢測方法決定了slam建圖的精度與速度。LOAM算法對提取的特征進行點到線和點到面結(jié)合的方式進行匹配，提高了系統(tǒng)計算速度和精度，但受到?jīng)]有回環(huán)檢測模塊完成累計誤差消除的限制。本研究基于小型移動機器人平臺，提出使用Segmatch算法作為回環(huán)檢測模塊，有效解決了在進行室外大面積環(huán)境構(gòu)建中出現(xiàn)的漂移問題。因此，本研究提出的基于Livox的室外3D-SLAM系統(tǒng)解決了建圖中易發(fā)生的累計漂移誤差問題，在移動機器人定位與建圖領域具有一定的參考價值。