融合泊松重建的激光語義SLAM系統(tǒng)

曾圣堯，張 雷，徐 方，杜振軍，劉明敏

(1.中國科學院沈陽自動化研究所機器人學國家重點實驗室，遼寧 沈陽 110016； 2.中國科學院機器人與智能制造創(chuàng)新研究院，遼寧 沈陽 110169；3.中國科學院大學，北京 100049；4.沈陽新松機器人自動化股份有限公司，遼寧 沈陽 110168)

0 引 言

當前，依賴激光雷達的激光SLAM和依賴圖像的視覺SLAM是當下SLAM領域的兩大主要分支[1,2]，多傳感器融合的SLAM技術(shù)也日趨成熟[3]。在激光SLAM領域，以SuMa[4]和LiTAMIN[5]為代表的激光SLAM系統(tǒng)使用各種改進的ICP作為幀間匹配的方法；以LOAM、LeGO-LOAM[6]為代表的激光SLAM系統(tǒng)則在匹配前首先對點云中的點進行基于幾何信息的特征提取，篩選出一系列特征點進行處理。在地圖表示方面，大部分激光SLAM系統(tǒng)都使用點云地圖[5,6]，Cartographer[7]中使用占據(jù)柵格地圖，SuMa使用基于surfel的面元地圖。Vizzo等[8]提出在地圖建立中使用泊松重建建立面元地圖同時配合點面ICP的激光SLAM方法，但是由于泊松三維重建的時間耗散過大，系統(tǒng)無法實時運行，僅在理論上論證了泊松重建的面元地圖在恢復場景的三維細節(jié)，節(jié)省存儲空間等多方面的優(yōu)越性。近些年來，隨著深度學習的崛起，計算機對數(shù)據(jù)的理解能力的加深使得語義SLAM成為近來研究的熱點問題[9]。語義SLAM加深了機器人對于環(huán)境的理解能力。在激光SLAM領域，SuMa++[10]利用Rangnet++網(wǎng)絡建立基于surfel面元的語義地圖，同時利用語義信息加強了在動態(tài)環(huán)境中的定位精度。但是surfel面元仍是較為稀疏的面元表示方式。本文所提出的框架使用全局點云語義地圖和局部基于泊松重建的面元地圖，可以在保證實時性的基礎上，一方面保證建圖精度，另一方面增加面元地圖的表示能力。

1 總體架構(gòu)設計

系統(tǒng)的總體框架設計如圖1所示。系統(tǒng)接受6自由度的3D雷達數(shù)據(jù)，輸出6自由度的位姿估計與語義地圖。系統(tǒng)主體可以分為定位與語義地圖的建立兩大部分。在定位部分中，系統(tǒng)采用了基于LeGO-LOAM的設計思路——點云幾何信息的特征提取方案。在建圖模塊中，雷達點云數(shù)據(jù)中的每一個點基于神經(jīng)網(wǎng)絡進行語義標注，結(jié)合定位模塊中給出的位姿信息，進行局部語義點云地圖的構(gòu)建，通過泊松重建最終獲得含有語義信息的面元地圖。在此架構(gòu)中，主要依賴于CPU解算的定位模塊與主要依賴于GPU解算的語義模塊并發(fā)執(zhí)行，保證系統(tǒng)算力的有效使用。

圖1 系統(tǒng)總體架構(gòu)

2 具體實現(xiàn)

2.1 定位模塊的實現(xiàn)

定位模塊由基于幾何特征提取的前端與回環(huán)檢測以及圖優(yōu)化3部分組成。

在幾何特征提取中，所有點按照平滑度大小排列，選取平滑度較大的點集合為平面點，平滑度較小的點集合為角點。其中，對于點云P中的第i個點pi平滑度Cpi定義為

(1)

其中，n表示選取近鄰點的個數(shù)，S表示由近鄰點構(gòu)成的集合。

對于平面點而言，進一步被區(qū)分為地面點P和其它平面點E。其判定依據(jù)為該點與相鄰線束的激光點的連線在其雷達坐標系中的俯仰角θ。點pi的俯仰角θ計算為

(2)

其中， (xi,yi,zi) 為pi在雷達坐標系下的位置， (xj,yj,zj) 為點pi的相鄰線束的激光點pj在雷達坐標系下的位置。當θ足夠小，即 |θ|<θthreshold時，判定為地面點。

(3)

當前時刻的總位姿變換為

(4)

2.2 語義地圖的建立

對于激光SLAM而言，通過深度學習對雷達點云中各點所屬物體進行分類，通過SLAM算法為點云提供其在世界坐標系下的三維位姿，建立的地圖被歸為語義地圖。

語義分割是建立語義地圖的第一步。在語義分割中，首先將3D點云數(shù)據(jù)進行球面投影，處理為2維的深度圖像，在深度圖像上進行基于全卷積網(wǎng)絡的語義分割，最后對深度圖像上帶有語義標簽的像素點轉(zhuǎn)換回3D點云數(shù)據(jù)中。在語義地圖的建立過程中，系統(tǒng)維護一個以點云方式存儲的全局語義地圖以及以面元形式存儲的局部面元語義地圖。語義地圖的整體處理流程如圖2所示。其中圖2(a)表示雷達獲取的原始點云數(shù)據(jù)，圖2(b)表示系統(tǒng)的全卷積神經(jīng)網(wǎng)絡架構(gòu)，圖2(c)表示具有語義信息的點云，圖2(d)表示表示泊松重建后具有語義信息的局部面元地圖。

圖2 語義地圖的處理流程

2.2.1 語義點云的獲取

語義分割神經(jīng)網(wǎng)絡選用深度圖像作為輸入。對點云數(shù)據(jù)進行球面投影，獲得對應的深度圖像是語義分割的第一步。在球面投影的過程中，對于點云中的點pi=(xj,yj,zj)， 先計算其在雷達坐標系中的俯仰角與偏轉(zhuǎn)角θpitch、θyaw

(5)

當雷達傳感器垂直分辨率(線數(shù))為h，水平分辨率(一圈掃描點數(shù))為w，上下測量角度范圍分別為θup、θdown時，對于創(chuàng)建的分辨率為[h×w]的圖像，利用式(6)計算其在深度圖像中的坐標(u,v)

(6)

(7)

語義分割完成后的深度圖像反向解算將語義映射回原始的三維點云中去，完成對于點云的語義分割。具有語義信息的點云一方面將融合進關鍵幀，參與全局語義點云的構(gòu)建；另外一方面將融合進局部子圖，參與局部面元地圖的構(gòu)建。

2.2.2 泊松重建的處理

在局部面元地圖的處理過程中，關鍵步驟是將點云轉(zhuǎn)化為面元。泊松重建是一種將三維離散點轉(zhuǎn)換為連續(xù)表面的先進三維重建方法，重建后生成具有水密性質(zhì)的三角面元[13]。與傳統(tǒng)的三維重建方法相比，泊松重建將點云中的所有點作為一個整體考慮，不進行局部分割與拼接，最大程度保留了原始點云的局部細節(jié)與空間信息。使用泊松重建可以構(gòu)建一個高質(zhì)量的局部面元地圖，面片光滑細膩，提供更多環(huán)境的細節(jié)信息。這樣的局部地圖可以為人機交互提供一個更清晰、明了的局部環(huán)境感知。

泊松表面重建算法的本質(zhì)是根據(jù)點云建立的指示函數(shù)，提取出由三角形面片拼接成的光滑曲面，屬于一種隱式曲面重建算法。其具體的計算流程如圖3所示。

圖3 泊松重建基本流程

(8)

(9)

其中，*n表示進行n次相同的卷積操作。在對點云進行八叉樹劃分后，每個八叉樹的節(jié)點o中的點q使用如下平滑函數(shù)進行處理

(10)

其中，o.w表示節(jié)點o的寬度，o.c表示節(jié)點o的中心。同時對于梯度空間采用如下近似

(11)

(12)

泊松重建生成具有水密性的網(wǎng)格地圖，網(wǎng)格地圖在數(shù)據(jù)構(gòu)成上由各個頂點的三維坐標集合v以及每一個三角形面元所包含的3個頂點索引 (n1,n2,n3) 所組成。泊松重建的水密性質(zhì)決定了生成的網(wǎng)格地圖具有封閉性，與室外環(huán)境地圖自然帶有的非封閉特性結(jié)合，會在重建過程中生成大量多余面元，從而影響對與環(huán)境信息的直接觀測。所以在對局部子圖的泊松重建中，去除多余的面元是十分重要的。針對道路環(huán)境的結(jié)構(gòu)特點，選取面元平均距離作為移除多余面元的判定依據(jù)。在距離原始點云過遠處生成的面片是十分不可靠的面片，因為這表示原始的雷達沒有掃描到此處的空間信息。同時，當掃描到一處的點數(shù)量過少時，此處的信息仍是不夠準確的，所以在評價指標中選取面片到原始點云中多個點的平均距離進行測量，增加面片信息的準確性。頂點pi平均距離tpi的定義為

(13)

其中，n為選取近鄰點的個數(shù)，S為點Pi在重建前的原始點云中的近鄰點的集合。面元m的平均距離定義為其3個頂點p1,p2,p3平均距離的最小值，即tm=min(tp1,tp2,tp3)。 圖4展示了重建后面元的平均距離的分布情況，圖中面元顏色反應了面元的平均距離變化。為了便于顯示，圖4中對于平均距離采用對數(shù)拉伸處理，顏色深度T與平均距離tpi的對應關系為T=70log(tm+1)。 從圖中可以看到，需要移除的多余面片向穹廬一樣環(huán)繞包裹，多余面片顏色較深，主要集中在遠離點云的上部區(qū)域，單塊三角形面片面積較大。而下部靠近道路部分的面片呈現(xiàn)淺色，這部分面片反應的是雷達采集到數(shù)據(jù)的真實環(huán)境，面片十分密集，單塊面片面積細小，能夠反應局部細節(jié)。通過選取合適的門限值，移除多余面片，獲得重建后的局部子圖。

圖4 泊松重建后的全部面元及面元平均距離分布

在移除多余面片時，如果對移除的面片的頂點進行刪除，則會影響其余所有面片的頂點排序，需要對其余所有面片的頂點索引進行更新，這要無疑在計算復雜度上是不可接受的。為了減小計算量，在移除多余面片時，只移除面片頂點的索引，而保留頂點坐標，這樣在渲染面元地圖時便不會顯示需要被去除的面片。具體計算流程如算法1所示。其中近鄰點查找使用KDTree[14]算法對進行加速。算法1中，移除面片頂點索引表達為將面片索引置為非法索引值-1。在本系統(tǒng)中，選取近鄰點個數(shù)K為10，面元平均距離門限值tmin為0.5。

泊松重建的過程中不會對語義信息進行處理，重建后的面元地圖中不包含語義信息。所以在獲得重建完成后的面元地圖后還需要對頂點進行語義信息的關聯(lián)。在語義渲染中，面元地圖的每個頂點在數(shù)據(jù)構(gòu)成上包含6個維度：空間坐標 (X,Y,Z) 與顏色 (R,G,B)。 在關聯(lián)語義信息時對于頂點坐標v在重建前的語義點云P中尋找最近鄰點，以其語義所對應的顏色作為該頂點的顏色數(shù)據(jù)?；诂F(xiàn)實環(huán)境中同一垂直空間的物體歸屬同一語義標簽的假設，在關聯(lián)語義信息時可以將所有待渲染頂點和語義點云投影到XOY平面上，在二維平面上進行近鄰關系的尋找。在具體實現(xiàn)上，可以先對于包含語義信息的原始點云在的XOY平面上構(gòu)建KDTree，然后對于待渲染的面片頂點進行基于KDTree的查找,找到距離其最近的語義點進行關聯(lián)，加快渲染速度。

算法1:多余面片移除

輸入：原始點云:RawPoint[n1][3];

面元頂點:Vertex[n2][3];

面元頂點索引:Mesh[n3][3];

近鄰點個數(shù):K;

面元平均距離門限值:tmin;

輸出：移除多余面片以后的面元頂點索引Mesh[n3][3]

(1)KdTree=MakeKdTree(RawPoint) //利用原始點云建立KdTree

(2)fori=1,2,…,n2do

(3)Nearest=KdTreeSearch(KdTree,Vretex,K)

(4)t[i]=0

(5)forj=1,2,…,Kdo

(6)t[i]=t[i]+||RawPoint[Nearest[i] ]-Vertx[i]||

(7)endfor

(8)t[i]=t[i]/K

(9)endfor

(10)fori=1,2,…,n3do

(11)t1=t[Mesh[i][1]]

(12)t2=t[Mesh[i][2]]

(13)t3=t[Mesh[i][3]]

(14)tm=min(t1,t2,t3)

(15)iftm>tminthen

(16)Mesh[k]={-1,-1,-1}

(17)endif

(18)endfor

3 實 驗

為了檢測系統(tǒng)的運行效果，實驗針對KITTI數(shù)據(jù)集[15]和MaiCity數(shù)據(jù)集[16]進行驗證。其中KITTI數(shù)據(jù)集為真實世界場景下錄制的數(shù)據(jù)集，MaiCity數(shù)據(jù)集為基于仿真世界建立的靜態(tài)場景數(shù)據(jù)集。實驗的硬件條件為i7-1800H，顯卡為RTX3060，操作系統(tǒng)選用Ubuntu16，搭載ROS kinetic。點云處理部分使用PCL庫，語義提取網(wǎng)絡基于CUDA10.0與TensorRT構(gòu)建，ICP優(yōu)化部分使用ceres庫，后端圖優(yōu)化使用gtsam構(gòu)建。系統(tǒng)程序框架基于ROS構(gòu)建，利用ROS自帶的rviz可視化機器人開發(fā)工具包可以方便顯示系統(tǒng)當前的各種狀態(tài)信息。全局點云與當前機器人的位姿信息通過rviz進行顯示。由于rviz中不能方便的顯示面元地圖，局部面元地圖通過調(diào)用PCL點云庫中的可視化模塊進行實時顯示更新。

3.1 基于真實世界的測試實驗

在基于真實世界的測試實驗中，選取KITTI數(shù)據(jù)集。KITTI數(shù)據(jù)集是目前自動駕駛領域的重要測試數(shù)據(jù)集。數(shù)據(jù)集主要針對室外大場景錄制了包含激光雷達，灰度雙目相機、IMU、GPS等多傳感器數(shù)據(jù)。本實驗中選取其中的激光雷達數(shù)據(jù)集odometry進行測試。其中，激光雷達型號為Velodyne HDL-64E，安裝在距離地面1.65 m高的車頂。該64線激光雷達的探測距離為100 m，垂直角分辨率為0.4°，水平角分辨率為0.09°。雷達坐標系的方位為以向上為z軸，向前為x軸建立的右手坐標系。

全局點云語義地圖的建圖效果如圖5所示。在建圖過程中對全局點云使用了0.4m×0.4m的柵格體素濾波?？梢钥吹剑谌贮c云地圖中歸屬道路，樹木、車輛等物體的點標記有對應的標簽，其中小窗內(nèi)圖像是對方框內(nèi)局部區(qū)域的放大效果展示。本系統(tǒng)能良好的實時建立全局點云語義地圖，為后續(xù)機器人在導航等各個方面提供更多環(huán)境信息。

圖5 KITTI 05序列運行生成的全局語義點云地圖

圖6展示了本系統(tǒng)的局部面元地圖與SuMa++中的surfel面元地圖的效果對比，圖(a)、圖(b)來自KITTI數(shù)據(jù)集05序列，圖(c)、圖(d)來自KITTI數(shù)據(jù)集00序列。其中圖(a)、圖(c)為本系統(tǒng)采用方法建立的面元地圖，圖(b)、圖(d)為SuMa++方法所建立的surfel面元地圖。圖中語義標簽與圖5中相同?？梢悦黠@看出，本文通過泊松重建建立的三角面元地圖比surfel面元地圖在地圖的連續(xù)性上效果更優(yōu)，包含更多的環(huán)境細節(jié)信息。觀察圖中的面元地圖的道路部分可以發(fā)現(xiàn)，surfel面元地圖中的道路充滿各種孔洞，而泊松重建后的面元地圖中，則沒有出現(xiàn)細小孔洞，道路保持了連續(xù)性。從圖中可以看出，多余面片裁剪算法有效裁去了泊松重建中所殘留的導致面片閉合的多余部分，邊界清晰。同時，實驗發(fā)現(xiàn)，當場景中存在動態(tài)物體時，泊松重建的面元地圖會出現(xiàn)較差效果，放大點云地圖中動態(tài)物體的拖尾現(xiàn)象。

從圖5的全局點云地圖的局部放大圖與圖6(a)、圖6(b)泊松重建后的面元地圖對比可以看出，泊松重建后渲染出的基于三角形面片的語義地圖對比地圖的點云表示更具有直觀的展示效果，在人機交互中更有利于展示機器人當前所處的環(huán)境狀態(tài)，提升了地圖對于環(huán)境的表達能力。

在系統(tǒng)的實時性方面，一些關鍵步驟的平均運行時間見表1。定位模塊平均解算時間為0.05 s，可以保證定位模塊以10 Hz速度運行不丟幀，保證系統(tǒng)的實時性。全卷積神經(jīng)網(wǎng)絡的點云語義提取模塊的運行時間也小于激光雷達的數(shù)據(jù)發(fā)射時間間隔0.1 s。全局點云地圖以1 Hz的頻率更新。局部面元地圖由于面元重建與語義渲染需要較多的時間，所以需要8 s左右的時長，對于非自動駕駛領域的低速移動機器人而言，仍然是可以接受的。此外，由于本系統(tǒng)在實現(xiàn)中完成的是基于CPU的泊松重建，而目前基于GPU并行解算的泊松重建算法也已經(jīng)趨于成熟。GPU并行的泊松重建相較CPU版本能對算法加速7倍以上[13]，可以進一步提高系統(tǒng)的運行效率。

3.2 基于仿真世界的測試實驗

為了驗證面元地圖對于環(huán)境的還原精度，我們選取MaiCity數(shù)據(jù)集01段作為驗證。MaiCity數(shù)據(jù)集是由恩波大學建立的CAD城市模型仿真生成的激光雷達數(shù)據(jù)集。城市模型中包含建筑、樹木、汽車等城市常見元素。仿真城市模型01段中整體由兩個十字路口與連接十字路口的一條馬路所組成。數(shù)據(jù)集提供16線、64線及128線雷達仿真運行數(shù)據(jù)，通過比對地圖數(shù)據(jù)與城市模型，可以測試SLAM算法的建圖精度。實驗選取64線激光雷達進行驗證，對本文系統(tǒng)和SuMa++中的面元地圖進行比較。由于選用的數(shù)據(jù)集只包含100 m的街道數(shù)據(jù)，所以此實驗中選取維護40m×40m的局部面元地圖。

首先定義如下評價指標，平均誤差距離Et，有效面積S。令P為局部子圖中的所有點的點云集合，共包含N1個點，點p為點云P中的點。Mmodel為原始城市模型中的面元集合。Mbuild為重建模型中的面元集合，共包含N2個面元。

點云的平均誤差距離定義為

(14)

其中， d(p,m1) 為點p到平面m1的距離。面元的平均誤差距離定義為

(15)

其中， d(m1,m2) 為平面m1到平面m2的距離。對于SuMa++中的surfel面元地圖而言，每個面元包含參數(shù)半徑r，定義其有效面積為：S=∑m2∈Mπr2。 對于本文方法中的三角形面元地圖而言，每個面元m包含的3個頂點為pm1,pm2,pm3，定義有效面積為

(16)

實驗對數(shù)據(jù)集中兩十字路口之間路段進行測量，得到圖7所示數(shù)據(jù)。其中橫坐標表示時間，縱坐標表示面元的有效面積。虛線表示本文系統(tǒng)的局部面元地圖的有效面積，實線表示surfel面元地圖的有效面積。為了對齊兩種方法的數(shù)據(jù)，表中的橫坐標時間軸進行了拉伸處理。圖中兩種方法在起始時和終止時均有翹起的局部峰值出現(xiàn)，對應于機器人處于兩個十字路口時的數(shù)據(jù)，中部較為平緩區(qū)的數(shù)據(jù)對應于仿真城市模型中的一字型馬路部分。從表中可以看出，本文基于泊松重建后建立的三角形面元地圖在有效面積上全程都要遠優(yōu)于surfel面元地圖。

圖7 面元地圖有效面積對比

為了更有效對比兩者的平均距離誤差與有效面積。選取數(shù)據(jù)集中兩十字路口中間的一字型馬路部分進行多次測量，得到表2所示的平均測量參數(shù)。從表中的數(shù)據(jù)可以看出，本文方法在平均誤差距離的定義標準下，在此數(shù)據(jù)集的靜態(tài)場景中不論是從全局點云還是從局部面元的兩方面分析，其建圖效果都要優(yōu)于SuMa++方法所建立的surfel面元地圖，平均誤差距離Et更小。同時從面元有效面積上分析，基于泊松重建的三角面元的表示方法對原始模型的還原能力約為surfel面元的1.57倍。考慮到實際surfel面元還存在大量面元之間相互遮疊的情況，兩者之間的差距應該更大。

表2 平均距離誤差與有效面積

4 結(jié)束語

本文針對戶外大場景下移動機器人的定位與建圖，提出一種局部面元與全局點云結(jié)合的激光SLAM系統(tǒng)，能夠在保證系統(tǒng)實時性的基礎上，提升地圖對于環(huán)境的表示能力?；谔卣魈崛〉膸g匹配保證建圖精度，通過泊松重建構(gòu)建的三角形面元對比surfel面元能將地圖有效面積提升1.5倍以上。利用全卷積神經(jīng)網(wǎng)絡基于球面投影的方法能夠有效對點云進行語義分割。在本方法中暫未對動態(tài)物體進行處理，結(jié)合語義信息加強動態(tài)環(huán)境下的定位精度與動態(tài)環(huán)境下面元地圖的處理優(yōu)化是下一步的改進點?；贕PU加速的泊松重建提升局部地圖的刷新速度也是今后的改進方向。