劉重男,楊 洋,盧清心,張 藝,汪 沛

(北京林業(yè)大學(xué) 理學(xué)院,北京 100083)

0 引 言

近年來,隨著計算機技術(shù)和自動化技術(shù)的發(fā)展,機器人越來越多地被應(yīng)用于工業(yè)生產(chǎn)[1]、物流[2]、家政服務(wù)[3]等領(lǐng)域。工作中的機器人需要自我定位,感知周圍環(huán)境,并構(gòu)建環(huán)境地圖,以輔助導(dǎo)航和規(guī)劃路徑。因此機器人在未知環(huán)境中的自動控制和定位成為當前研究的熱點問題。SLAM(Simultaneous Localization and Mapping)技術(shù)通過搭載多種傳感器,可以在對機器人定位的同時完成環(huán)境地圖的構(gòu)建,從而為后續(xù)的機器人導(dǎo)航和路徑規(guī)劃提供重要的基礎(chǔ)[4]。

SLAM技術(shù)可以基于不同種類的傳感器實現(xiàn),比如:視覺傳感器、毫米波傳感器和激光傳感器?；谝曈X的SLAM技術(shù),由于需要從二維圖像反演三維信息,運算量較大、算法較復(fù)雜[5]?；诤撩撞ɡ走_的SLAM技術(shù),當前的成本較高,技術(shù)還不成熟[6]。基于激光傳感器的SLAM技術(shù)具有獲取三維信息直接、運算量小的特點,在光照影響不大時能夠保證較好的精度。目前激光傳感器在三維感知和自動駕駛等應(yīng)用領(lǐng)域已具有較大的優(yōu)勢[7]。對于室內(nèi)SLAM技術(shù),由于室內(nèi)環(huán)境比較復(fù)雜,傳感器與室內(nèi)障礙物距離一般較近,對于精度和實時響應(yīng)要求高,因此與其他傳感器相比,二維激光傳感器以其成本低、易集成的優(yōu)點,在室內(nèi)SLAM產(chǎn)品中占據(jù)更多的市場[8]。

當前基于二維激光雷達的室內(nèi)SLAM方法主要有兩大類。第一類是基于粒子濾波的算法,主要通過概率的方法,將SLAM中定位與建圖兩個過程分開,先定位再建圖[9]。2007年Griseti等在Rao-Blackwellized Particle Filters(RBPF)粒子濾波器的基礎(chǔ)上提出了GMapping算法,通過改進提議分布(Proposal Distribution)和引入自適應(yīng)重采樣這兩個機制提高了執(zhí)行效率[10]。這類方法的優(yōu)點是計算量小,在構(gòu)建小場景地圖時精度更高;缺點是算法依賴于里程計,構(gòu)建大場景時會產(chǎn)生地圖錯位[11-12]。第二類是基于圖優(yōu)化的算法,通過計算幀間預(yù)測值與觀測值的誤差,利用非線性最小二乘作為誤差函數(shù),求解誤差函數(shù)的最小值,來找到每幀的最優(yōu)位姿(Pose),使地圖預(yù)測值與觀測值的誤差最小[13-14]。2016年谷歌公司提出的Cartograppher算法,通過引入回環(huán)檢測,實現(xiàn)地圖的全局約束和局部約束[15]。這類方法的優(yōu)點是對激光雷達硬件條件要求低;缺點是計算量大、運算時間長[16]。

目前主流的基于視覺的室內(nèi)SLAM方法主要有兩大類。第一類是基于圖像像素點的算法[17];第二類是基于圖像特征點的算法[18]。

該文采用低成本二維激光雷達獲取數(shù)據(jù),并基于圖優(yōu)化算法進行幀間數(shù)據(jù)配準,再結(jié)合Density-Based Spatial Clustering of Applications with Noise(DBSCAN)算法和K-Nearest Neighbors(KNN)算法繪制地圖?；诩す饫走_的硬件特性,使用DBSCAN算法預(yù)處理點云數(shù)據(jù),先采用KNN算法與均方根誤差對點云聚類,再利用最小線性二乘法擬合點云數(shù)據(jù),實現(xiàn)對室內(nèi)環(huán)境的制圖。

1 試驗設(shè)備及數(shù)據(jù)采集

1.1 硬件設(shè)備平臺

該文采用“塔克機器人”作為試驗平臺采集數(shù)據(jù),其硬件構(gòu)成包括:A1M8型激光雷達、樹莓派4B、STM32和四個JGA37直流減速電機,實物圖如圖1所示。用樹莓派4B,搭載Raspbian系統(tǒng),安裝Kinetic Kame版本的ROS系統(tǒng)。運動控制器選擇STM32 F103作為主控制單元。

圖1 試驗流程

二維激光雷達選用思嵐公司開發(fā)的RPLIDAR A1M8傳感器,成本低于500元。該傳感器采用激光三角測距技術(shù)[19]。該傳感器可以360度全方位獲取二維點云數(shù)據(jù),最遠測量距離為12米,在掃描頻率為5.5 Hz時360度采樣可獲取1 450個采樣點。

1.2 試驗數(shù)據(jù)采集

2020年10月至2021年4月之間,項目組選擇北京林業(yè)大學(xué)校內(nèi)四個不同室內(nèi)場景,采用移動平臺展開了多次數(shù)據(jù)采集試驗。場景一位于新食堂四層北側(cè)樓道內(nèi),樓道內(nèi)存在直角拐彎的地形特征;場景二位于新食堂四層?xùn)|側(cè)樓道內(nèi),樓道內(nèi)存在長側(cè)面的邊界;場景三位于學(xué)研中心A座14層南側(cè)樓道內(nèi),樓道內(nèi)存在多個短側(cè)面與玻璃裝飾;場景四位于基礎(chǔ)樓一層西側(cè)樓道,樓道內(nèi)結(jié)構(gòu)整齊,側(cè)面長度均勻。

試驗中,項目組通過筆記本電腦遙控小車平臺運動,同時控制小車搭載的激光雷達傳感器采集點云數(shù)據(jù),從而持續(xù)獲取室內(nèi)結(jié)構(gòu)信息,并將數(shù)據(jù)存儲在樹莓派4B中,原始點云數(shù)據(jù)的格式為.bag文件。

2 方法設(shè)計

該文提出的室內(nèi)移動建圖方法可以分為三個階段,分別是:幀間匹配及降噪、聚類分割和自動制圖,流程如圖1所示。

2.1 幀間匹配及降噪

通過激光傳感器采集,獲得單幀的點云數(shù)據(jù),而室內(nèi)自主建圖需要將單幀點云數(shù)據(jù)進行幀間的融合匹配,即通過一組平移和旋轉(zhuǎn)參數(shù),使相鄰的兩幀點云數(shù)據(jù)能夠最大程度的重合,從而將連續(xù)掃描的多幀激光點云在同一個坐標系中疊加顯示[20]。

由于傳感器測量誤差、數(shù)據(jù)處理精度、平臺運動時的震動等干擾都會增加點云數(shù)據(jù)的噪聲,從而增加測量誤差,降低測量精度[21]。因此,需要對點云降噪以降低數(shù)據(jù)誤差,提高處理速度和結(jié)果精度。該文采用兩次降噪處理,第一次對每幀的細節(jié)處進行降噪處理,第二次從全局的角度出發(fā),對幀間匹配后的點云數(shù)據(jù)進行降噪處理,這樣分別從整體和局部兩個方向降低了誤差。

幀間配準及降噪的流程如下所示:

(1)激光雷達采集單幀數(shù)據(jù),并將極坐標系轉(zhuǎn)換為直角坐標系;

(2)進行第一次DBSCAN降噪處理。

定義每幀點云數(shù)據(jù),距離激光雷達中心點直線距離5米以內(nèi)的點為有效數(shù)據(jù)。激光雷達在掃描時,由于傳感器的誤差和平臺運動帶來的顛簸,造成每幀的數(shù)據(jù)點存在誤差。同時,為了降低后續(xù)在配準時,由于單幀中的異常點對全局地圖所產(chǎn)生的誤差,對坐標轉(zhuǎn)換后的每幀數(shù)據(jù)進行第一次DBSCAN降噪處理。

DBSCAN是一種基于密度的聚類算法,這是一種很典型的密度聚類算法,既可以適用于凸樣本集,也可以適用于非凸樣本集。DBSCAN算法的顯著優(yōu)點是聚類速度快且能夠有效處理噪聲點和發(fā)現(xiàn)任意形狀的空間聚類。該算法利用基于密度的聚類的概念,要求聚類空間中的一定區(qū)域內(nèi)所包含對象(點或其他空間對象)的數(shù)目不小于某一給定的閾值。過濾低密度區(qū)域,發(fā)現(xiàn)稠密度樣本點。在該類別任意樣本周圍不遠處一定有同類別的樣本存在。

濾波流程如下:對點云集合中的每個點,在其半徑鄰域內(nèi)搜索,若該點的鄰域內(nèi)包含的點少于設(shè)定的最小樣本點數(shù),則標記為異常點并濾除。

(3)采用Cartographer的配準思想,利用非線性優(yōu)化的方法,實現(xiàn)單幀掃描點云產(chǎn)生的局部地圖與全局地圖間的轉(zhuǎn)變。

局部地圖通過每幀點云數(shù)據(jù)掃描產(chǎn)生,在直接匹配子局部地圖時,誤差會慢慢累計。因此采用稀疏姿態(tài)調(diào)整(Sparse Pose Adjustment)的思想優(yōu)化局部地圖的位姿,這個過程可以視為一個非線性最小二乘問題。其中由全局地圖下的局部地圖位姿和掃描位姿在相對位姿的約束下與協(xié)方差矩陣關(guān)聯(lián)估算特性,如文獻[22]中提出的方法,計算并保存其最小參差時的位姿為最優(yōu)解。

為了更好地完成精準點間匹配,在局部地圖中框出一個搜索窗口,通過選擇窗口的移動角度步長和移動坐標步長得到一個有限的搜索集合,遍歷集合中的每個元素,改變窗口的角度與坐標位置,求得相鄰幀間點云的最佳位姿。采用分支定界方法(Branch and Bound Approach),提高窗口搜索的效率,其基本原理見文獻[23]。

(4)將每幀點云數(shù)據(jù),按照相對于全局地圖的位置和角度信息轉(zhuǎn)換后,進行第二次DBSCAN降噪處理。

將激光雷達獲得的極坐標數(shù)據(jù)轉(zhuǎn)換到笛卡爾坐標系下,然后根據(jù)幀間匹配得到每幀的位姿信息,由每幀的局部地圖構(gòu)建全局地圖。坐標轉(zhuǎn)換公式如(1)所示:

Do=ρ×(cosθsinθ)

(1)

(2)

其中,ρ和θ分別為通過傳感器獲得的極坐標信息,Do為笛卡爾坐標系下的原始坐標,px、py和pθ分別為幀間匹配獲得的位姿信息,Dt為笛卡爾坐標系下的真實坐標。

因為配準中存在著誤差,為降低對后續(xù)聚類結(jié)果的影響,當幀間配準完成后,對全局地圖進行第二次DBSCAN降噪處理。對全局中每個點,按照設(shè)定的半徑鄰域與最小樣本點數(shù)逐點迭代,標記并濾除掉不符合要求的點。兩次降噪結(jié)束后,將全局地圖轉(zhuǎn)換為正射影像圖方便對結(jié)果進行觀察。

2.2 聚類分割

為繪制室內(nèi)墻面的環(huán)境地圖,需要先按照線性關(guān)系對原始點云數(shù)據(jù)進行分類。該文采用基于KNN原理的方法對點云數(shù)據(jù)進行聚類分析,將聚類點云通過線性最小二乘法擬合成線段,并利用RMSE值判斷擬合效果。RMSE是用來衡量觀測值與真實值之間的誤差,一般指擬合后線段數(shù)據(jù)與原始點云數(shù)據(jù)之間方差的平方根[24],RMSE越小,代表擬合效果越好,其計算公式如下所示:

(3)

其中,m是觀測過程中使用的數(shù)據(jù)總數(shù),f(xi)是擬合值,yi是原始值。

聚類擬合方法流程如圖2所示。

圖2 基于KNN的聚類算法流程

聚類具體流程如下所示:

(1)判斷點云數(shù)據(jù)集合中是否為空,若為空則聚類結(jié)束,否則隨機選擇未被分類的點作為聚類中心點Pc,并定義為一個類別。

(2)對Pc點采用KNN處理,KNN(K-Nearest Neighbor)是采用測量不同特征值之間的距離方法進行分類,優(yōu)點有以下幾點:思想簡單、理論成熟,既可以用來做分類也可以用來做回歸;可用于非線性分類;訓(xùn)練時間復(fù)雜度比支持向量機之類的算法低;和樸素貝葉斯之類的算法比,精度高、對異常值不敏感且無數(shù)據(jù)輸入假定。由于KNN方法主要靠周圍有限的鄰近的樣本,而不是靠判別類域的方法來確定所屬的類別,因此對于類域交叉或重疊較多的待分類樣本集來說,KNN方法較其他方法更為適合。且KNN方法比較適用于樣本容量較大的類域的自動分類,而那些樣本容量較小的類域采用KNN方法比較容易產(chǎn)生誤分類的情況。

該文選擇N個近鄰。根據(jù)不同試驗場景中,各線段點云數(shù)量和稀疏程度不同,選擇了不同的近鄰點數(shù)量N,范圍在50～200之間。

(3)用線性最小二乘擬合方法對近鄰點Pn進行擬合。

(4)線段擬合后,計算擬合線段的RMSE值。

(5)比較RMSE值與閾值,若小于閾值則跳轉(zhuǎn)到步驟6繼續(xù)聚類,否則跳轉(zhuǎn)到步驟1。根據(jù)不同試驗場景,整體物理結(jié)構(gòu)和算法運行速度選擇不同的RMSE閾值,范圍在0.17～0.2之間。

(6)保存近鄰點Pn與Pc為同一個類別。

(7)計算近鄰點Pn中每個點與Pc的直線距離。

(8)選擇Pn與Pc距離最大的點,作為新的聚類中心點Pc,跳轉(zhuǎn)至步驟2。

2.3 自動制圖

制圖階段的原則是,符合實際物理環(huán)境結(jié)構(gòu)。因此,該文的標準是要求相鄰線段首尾相連,線段間不允許出現(xiàn)斷開、交叉等現(xiàn)象。具體的操作步驟如下所示:

(1)根據(jù)聚類分割的結(jié)果,以不同類別中點云數(shù)據(jù)的數(shù)量為標準再進行篩選。

該文規(guī)定同一類別的點云數(shù)量,應(yīng)至少大于2N個點。即每一個類別至少經(jīng)過兩次完整的KNN搜索,認為在同一類別中若小于設(shè)定的點云數(shù)量則代表搜索的點云過少,擬合后形成的線段過短,即不能明顯地還原環(huán)境信息的線段特征。

(2)采用線性最小二乘方法,對篩選后剩下的類別分類別擬合線段并繪制于圖中。

該文的數(shù)據(jù)獲取于建筑樓宇中,墻面線性度和平整度均較好。因此,選擇線性最小二乘法擬合點云數(shù)據(jù)。如下式所示,若需要擬合的數(shù)據(jù)點為(xi,yi),即找到函數(shù)f(xi)使得ei的平方和最小:

(4)

f(xi)=kxi+b

(5)

(3)將線段分為兩類對應(yīng)樓道的兩側(cè),并對每側(cè)線段重新排序。

試驗場景為室內(nèi)樓道,其兩側(cè)近似平行,另外兩側(cè)則呈向外開放式。因此,將線段按照其坐標點信息劃分為兩類,然后根據(jù)實際場景的物理順序結(jié)構(gòu)排列。

(4)計算每側(cè)相鄰線段的交點,并將交點設(shè)置為每條線段新的起始或終止坐標點后重新繪制所有線段于圖中。

由于點云數(shù)據(jù)在聚類擬合時存在誤差,導(dǎo)致擬合后的線段沒有形成首尾相連的二維環(huán)境圖。且已知在同一平面內(nèi),兩條不平行的直線之間必存在唯一的交點,因此可以求出每側(cè)相鄰線段的交點坐標,實現(xiàn)環(huán)境地圖的完整性。

3 試驗結(jié)果

3.1 融合點云降噪結(jié)果

試驗采集的四個不同場景原始點云數(shù)據(jù),經(jīng)過融合及降噪處理后,點云數(shù)據(jù)質(zhì)量得到改善。圖3對比了降噪前和降噪后的點云數(shù)據(jù),整體降噪效果明顯。在圖3中,用框線標出了重點對比區(qū)域。顯然,經(jīng)過融合降噪處理,數(shù)據(jù)因為傳感器精度、采集條件、平臺和環(huán)境的穩(wěn)定性等導(dǎo)致噪聲點云被大量濾除。

圖3 四個場景原始點云及降噪結(jié)果

同時在降噪后的數(shù)據(jù)中存在數(shù)據(jù)的損失。部分邊緣數(shù)據(jù)或者狀態(tài)不好的數(shù)據(jù),在融合降噪的過程中被濾除掉,如圖3(g)和(h)左側(cè)所示。由于設(shè)備會在室內(nèi)空間沿路徑一直采集數(shù)據(jù),這部分濾除的邊緣數(shù)據(jù)將在后續(xù)的數(shù)據(jù)采集和處理中被生成,因而并不會影響室內(nèi)建圖的完整性。因此,圖中保留點云的整體輪廓還是較好地還原了實際場景的物理環(huán)境信息。

3.2 點云聚類分割結(jié)果

試驗中四個不同場景的點云數(shù)據(jù)經(jīng)降噪處理后,分別使用文中算法進行聚類,將不同類別的數(shù)據(jù)點集擬合為長直線段,并用不同顏色標識出來。聚類后的邊框線如圖5所示,與圖4相比地圖輪廓更加集中規(guī)整,可以更好地展示環(huán)境的邊界特征,但是在相鄰的不同類別線段接觸處,存在著空隙或交叉,與實際環(huán)境情況不符,因此需要對結(jié)果進行進一步處理。

圖4 四個場景聚類結(jié)果

圖5 四個場景制圖結(jié)果

3.3 自動制圖結(jié)果

根據(jù)聚類擬合后的線段,分別計算四個場景中兩兩相鄰線段的起始與終止點坐標,接下來根據(jù)每個類別線段的斜率與截距制圖,處理結(jié)果如圖5所示?？梢灾庇^觀察到整齊的地圖輪廓,且不同線段的交界處較好地銜接在一起,得到較準確的環(huán)境地圖。

3.4 數(shù)據(jù)誤差分析

對四個試驗場景自動制圖后繪制的線段,按照阿拉伯數(shù)字以順時針的順序標號;對四個試驗場景樓道間的位置,按照英文小寫字母自左往右的順序標號。圖6展示了場景一的各線段標號。分別記錄四種場景在室內(nèi)自主建圖中獲得的數(shù)據(jù),并與實際數(shù)據(jù)相比較,計算出樓道長度平均絕對誤差為3.69厘米,樓道寬度平均絕對誤差為2.39厘米。

圖6 場景1線段長度和樓道寬度分析

4 分析與討論

平臺選擇搭載低成本二維激光傳感器,具有精度較高、速度快、價格低等優(yōu)點,試驗獲得的制圖結(jié)果基本反映了試驗區(qū)域的室內(nèi)空間環(huán)境。但是在試驗中也發(fā)現(xiàn)了一些問題和可以改進的空間。

在硬件方面,設(shè)備和環(huán)境的影響都會增加測量誤差,降低制圖精度。二維激光雷達在運動過程中由于每幀采集的數(shù)據(jù)數(shù)量定,在面對較復(fù)雜的地形時,有限的點云數(shù)據(jù)會導(dǎo)致制圖后的地圖精度降低。如表1中場景1的線段9、線段15和線段16,其本身長度小于50厘米,試驗平臺在經(jīng)過時無法獲得充足的環(huán)境紋理信息,導(dǎo)致擬合后的相對誤差值偏大。這個問題也可以在后期通過增加平臺在該區(qū)域的駐留時間,從而提高激光傳感器的掃描密度,改善測量精度。同時,如表2所示,該方法對樓道寬度的平均誤差較小,均在1%左右。

表1 場景1和場景3線段實測值與擬合值及誤差

在軟件方面,算法策略會影響系統(tǒng)的制圖精度和實時性。試驗選擇室內(nèi)長直樓道區(qū)域,環(huán)境中多為墻面的線性結(jié)構(gòu),因此算法中采用線段擬合。當算法在遇到有弧度的拐角或凸起凹陷等不規(guī)則曲線邊緣時,擬合的精度會有較大的下降,甚至在局部區(qū)域出現(xiàn)擬合錯誤。

現(xiàn)在,激光雷達與視覺傳感器相結(jié)合的多傳感器融合的SLAM方案是未來的發(fā)展趨勢,能夠兼顧弱紋理環(huán)境中的特征獲取和結(jié)構(gòu)信息測量,保障數(shù)據(jù)采集的精度。而在軟件算法上,室內(nèi)制圖也將與深度學(xué)習(xí)融合成新的SLAM技術(shù),從而改善前端幀間匹配和后端閉環(huán)檢測[25]。在未來工作中,也將圍繞這些方面開展相應(yīng)的研究工作。

5 結(jié)束語

設(shè)計了搭載低成本二維激光雷達的自主移動室內(nèi)建圖平臺,并通過掃描試驗場景獲取點云數(shù)據(jù)完成建圖。試驗中提出了通過預(yù)濾波、多幀數(shù)據(jù)融合、聚類分割和自動制圖等一系列處理快速實現(xiàn)室內(nèi)建圖。對比四個試驗場景結(jié)構(gòu)信息,制圖結(jié)果能夠較好地反映場景實際結(jié)構(gòu),說明該方法具有一定的有效性和可行性。在未來的研究工作中,將進一步提升算法的魯棒性和精度,提升算法對室內(nèi)較復(fù)雜場景的建圖能力。