李 揚,蘇 海,余松森,曹一波,劉好新

(1.華南師范大學(xué)計算機學(xué)院,廣東 廣州 510631;2.華南師范大學(xué)軟件學(xué)院,廣東 佛山 528225;3.海南大學(xué)信息與通信工程學(xué)院,海南 ?？?570228)

1 引 言

激光頭發(fā)出的紅外激光打到物體表面時發(fā)生漫反射,旋轉(zhuǎn)激光掃描儀接收漫反射回的激光信號,利用激光測距算法,如三角測距法[1],實現(xiàn)距離測量,具有成本低、計算快以及精度高的特點。而這正好滿足移動機器人對環(huán)境感知類傳感器實時性和高精度的要求,因此被廣泛應(yīng)用于移動機器人測距領(lǐng)域,用于獲取環(huán)境障礙物的分布[2]。

但是基于漫反射原理的測距儀,常受環(huán)境影響而產(chǎn)生誤差。尤其是產(chǎn)生的粗大誤差嚴重偏離了被測量物體的真值,在后續(xù)的機器人任務(wù)中影響最大,必須被過濾[3-4]。粗大誤差產(chǎn)生的原因有很多,分為主觀原因與客觀原因兩類[5]。文獻[6]-[8]詳細分析了激光測距的誤差來源,其中環(huán)境溫度、物體材質(zhì)、顏色和傾斜度等外界環(huán)境都會影響漫反射回的信號,進而影響測距精度。此外,粗大誤差相對于系統(tǒng)誤差和偶然誤差,擁有更鮮明的特征,較容易在數(shù)據(jù)預(yù)處理階段實現(xiàn)直接過濾。因此,進行粗大誤差過濾是很有收益且必不可少的過程。

目前國內(nèi)外針對粗大誤差過濾的算法已有較深入的研究?？傮w分為兩大類,一類是鄰域比較法,如中值濾波[9]、弦夾角、弦高比[10]等通過和鄰域的數(shù)據(jù)進行比較來進行誤差判斷,算法簡單易實現(xiàn),但是比較時使用的閾值難以定義,因此無法應(yīng)對多變的環(huán)境,魯棒性不強。文獻[11]中提到了一種移動最小二乘中位數(shù)法,通過預(yù)先估計出測量數(shù)據(jù)集的標準差,然后以標準差作為判斷依據(jù),它能在一定程度上減輕過濾效果對閾值的依賴,但存在計算量大的問題。另外一類是分段法,將數(shù)據(jù)分段后過濾成員數(shù)少的段。文獻[12]采用基于改進的連續(xù)邊緣提取方法(Improved Successive Edge Following,ISEF)進行快速分段及濾波。文獻[13]將掃描點進行區(qū)域轉(zhuǎn)換后,計算相鄰點間的距離,通過分析得出了不同測量距離下的閾值,從而分段過濾。然而這些算法除了本身的缺陷外均存在不適合應(yīng)用于移動機器人領(lǐng)域的問題。裝載到移動機器人上的旋轉(zhuǎn)激光掃描儀,其測量數(shù)據(jù)具有缺失性(某些角度有數(shù)據(jù)缺失)、不確定性(測量長度隨著環(huán)境不斷變化)、數(shù)據(jù)量大(旋轉(zhuǎn)激光測距儀隨機器移動的同時以一定的角速度不斷旋轉(zhuǎn)采集數(shù)據(jù))、粗大誤差量少等特點。并且機器人在移動測量的同時進行環(huán)境地圖構(gòu)建,這要求數(shù)據(jù)過濾需要具備一定的實時性[14-16]。因此,本文研究了一種旋轉(zhuǎn)激光掃描數(shù)據(jù)的實時過濾方法,解決如何在上述特性下實現(xiàn)粗大誤差的實時過濾。

本文采用動態(tài)閾值分段,根據(jù)上一個相鄰的有效采樣點設(shè)定動態(tài)閾值,對當(dāng)前采樣數(shù)據(jù)進行同段判斷,將數(shù)據(jù)分段后,過濾掉包含成員少的分段。文中給出了動態(tài)閾值的設(shè)定方法及原理推導(dǎo),并建立了多組環(huán)境進行算法測試,同時為了量化分析算法的效果,通過構(gòu)建實驗環(huán)境的等比例平面圖對測試數(shù)據(jù)進行了人工標注,且對特殊環(huán)境做了具體分析,最后的實驗結(jié)果表明了算法的可行性。

2 移動旋轉(zhuǎn)激光掃描儀數(shù)據(jù)分析

在對移動旋轉(zhuǎn)激光掃描數(shù)據(jù)進行過濾之前,分析其數(shù)據(jù)來源及數(shù)據(jù)特點。

2.1 旋轉(zhuǎn)激光掃描儀工作原理

三角測距法是常用的激光測距方法。通常旋轉(zhuǎn)激光掃描儀使用三角測距法得到環(huán)境距離數(shù)據(jù),而后經(jīng)由機器人平臺傳輸給上位機或后臺等待處理。如圖1所示為三角測距原理,激光頭與攝像頭處在同一基準線上,兩者間距離為s,攝像頭焦距為f,激光頭與基準線呈固定夾角β。激光頭發(fā)射的激光打在物體表面,經(jīng)由漫反射被攝像頭捕捉到,經(jīng)過小孔成像,顯示到成像面上。

圖1 三角測距原理

根據(jù)三角形相似和余弦定理可得:

(1)

以常見的傳感器設(shè)備為例,參數(shù)為f=16 mm,s=41 mm,β=82.145°,每個像元的大小為5.5 μm,則其像元偏移X與測距長度D的對應(yīng)關(guān)系如圖2所示。

圖2 X-D曲線

由于像元偏移X是離散值所以測距長度D也是離散的。這意味著對于300 cm處的障礙物,激光測距所得到的測距距離只能是301 cm或者漫反射的激光點投影錯誤時可能會得到293.7 cm的次等結(jié)果,此時的誤差大概為2.1 %。由圖2易知誤差值隨著測距距離的增大而增大,當(dāng)測距距離為最大有效值Dmax時,誤差最大。另一方面,隨著X增大,距離D的變化逐漸放緩,且由于成像面有限,所以實際的X并不會無限增大,致使最終D會趨近于一個極小值Dmin。因此,系統(tǒng)會結(jié)合傳感器的參數(shù),在誤差可接受范圍內(nèi),定義一個有效測距范圍[Dmin,Dmax],只處理有效范圍內(nèi)的數(shù)據(jù)。

2.2 旋轉(zhuǎn)激光掃描儀數(shù)據(jù)分析

上位機接收到的數(shù)據(jù)經(jīng)過奇偶校驗以及范圍篩選后才進行誤差過濾,可以定義這樣的數(shù)據(jù)為待過濾數(shù)據(jù)。如圖3所示為在某室內(nèi)環(huán)境下旋轉(zhuǎn)激光測距儀旋轉(zhuǎn)一圈所得到的待過濾數(shù)據(jù),橫軸為采樣時間,縱軸為測距距離,測距儀每旋轉(zhuǎn)一度進行一次采樣,為方便顯示,將缺失的數(shù)據(jù)用零填充。

圖3 待過濾數(shù)據(jù)

結(jié)合圖3以及移動機器人的工作特性可知,待過濾數(shù)據(jù)具有如下特點:

(1)缺失性。部分角度對應(yīng)的測距數(shù)據(jù)有缺失,主要的原因可能是對應(yīng)角度在有效范圍內(nèi)并沒有障礙物存在,或者該角度所在的數(shù)據(jù)幀,因為傳輸錯誤而丟失。

(2)不確定性。角度所對應(yīng)的長度變化完全由所處的環(huán)境決定,測距長度的走勢具有不確定性。

(3)分段性。數(shù)據(jù)的不確定性和缺失性使得連續(xù)的角度所對應(yīng)的測量數(shù)據(jù)被分割成了多個段,但是正確的數(shù)據(jù)在獨立成段時往往會擁有足夠多的段成員。

(4)數(shù)據(jù)量大。在機器人移動過程中,旋轉(zhuǎn)激光測距儀一直在進行測距,新的數(shù)據(jù)一直在不斷的傳回給上位機等待處理,因此所提的算法應(yīng)該具備實時性以及計算量小的特點。

(5)粗大誤差量少。旋轉(zhuǎn)一圈所采樣數(shù)據(jù)中包含的粗大誤差并不多,且不會多個連續(xù)出現(xiàn)。

3 基于動態(tài)閾值分段的實時過濾算法

搭載在移動機器人上的旋轉(zhuǎn)激光掃描儀所產(chǎn)生的掃描數(shù)據(jù)具備其獨有的特性,加上對實時性和計算量的要求,導(dǎo)致現(xiàn)有的一些過濾算法都不太適用。本文針對粗大誤差與相鄰數(shù)據(jù)差異較大,且數(shù)量較少的特點,通過設(shè)定動態(tài)閾值實現(xiàn)數(shù)據(jù)分段,最后定義成員數(shù)較少的段為粗大誤差,進行過濾。算法總流程如圖4所示。

圖4 算法總流程

3.1 動態(tài)閾值分段

分段的依據(jù)是判斷新的采樣數(shù)據(jù)是否存在突變,即采樣值是否大于動態(tài)閾值,若是,則需要分段,否則,定義為同段數(shù)據(jù)。現(xiàn)實環(huán)境中,對相對較平緩的物體表面進行采樣所構(gòu)成的點集通常會組成同段數(shù)據(jù)。另一方面,基于漫反射原理的測距傳感器,在入射角θ較大時,傳感器所能接收到的漫反射信號較弱,會影響到像元偏移量的計算。由前述可知,在測距很遠時,較小的像元偏移計算錯誤也會導(dǎo)致較大的測量誤差[17]。因此,本文通過分析平緩物體表面采樣數(shù)據(jù)的分布推導(dǎo)動態(tài)閾值的設(shè)定方法,同時考慮入射角作為約束條件,構(gòu)建了如圖所示的約束模型,定義掃描儀逆時針旋轉(zhuǎn),lt-k為采樣時刻t-k所對應(yīng)的測距長度,α為入射角的余角。k既是角度也代表采樣間隔(每隔一度進行一次采樣)。

如圖5所示,相鄰的采樣點間存在數(shù)據(jù)約束。對于lt時刻的數(shù)據(jù),記其最近的上一個有效數(shù)據(jù)為lt-k,則lt相對于lt-k可能是遞增的或是遞減的。在限定了入射角的前提下,我們可以根據(jù)t-k時刻的數(shù)據(jù)lt-k計算在分別進行最大遞增和最大遞減變化的k個時刻后的數(shù)據(jù)值,作為t時刻數(shù)據(jù)lt的合理取值范圍。

圖5 約束模型

(2)

則合理的lt應(yīng)滿足:

(3)

(4)

lt應(yīng)滿足:

(5)

綜上,得到lt的合理取值范圍:

(k

(6)

式中,lt-k為最近的一個先前時刻的有效數(shù)據(jù),即Dmin≤lt-k≤Dmax；α和N為預(yù)設(shè)參數(shù),當(dāng)且僅當(dāng)lt滿足上式且k

3.2 段判斷

每當(dāng)建立新段時,檢查舊段的成員數(shù),若小于N,則將舊段中數(shù)據(jù)均標記為粗大誤差,進行過濾。在分段過程中,當(dāng)檢測到當(dāng)前段成員數(shù)大于等于N時,段內(nèi)的數(shù)據(jù)均能直接標記為正確數(shù)據(jù),因此本文所提算法能在N個采樣時間間隔內(nèi)實現(xiàn)數(shù)據(jù)過濾。

4 實 驗

4.1 實驗環(huán)境

如圖6所示為實驗所用的旋轉(zhuǎn)激光掃描儀,以及裝載有旋轉(zhuǎn)激光掃描儀的機器人平臺。旋轉(zhuǎn)激光掃描儀使用三角測距法得到環(huán)境距離信息,后經(jīng)由機器人通過ZigBee通信傳輸給上位機。

圖6 旋轉(zhuǎn)激光掃描儀與機器人

所用旋轉(zhuǎn)激光掃描儀的參數(shù)為:f=16 mm,s=41 mm,β=82.145°,每個像元的大小為5.5 μm。由此,定義15～300 cm為激光掃描儀的有效測距范圍,即Dmin=15 cm,Dmax=300 cm。

經(jīng)過測試,發(fā)現(xiàn)所使用的移動機器人主要是在受環(huán)境影響時會產(chǎn)生粗大誤差,尤其是在如圖7所示的兩種環(huán)境下。

圖7 粗大誤差環(huán)境

如圖7(a)所示,激光頭正對著的方向,若存在黑色且表面光滑的障礙物,則易產(chǎn)生粗大誤差。其次,如圖7(b)所示,對于由兩個光滑平面所組成的夾角,被激光頭直射到時,也容易產(chǎn)生粗大誤差?；诖?我們構(gòu)造了四組不同的能產(chǎn)生粗大誤差的測試環(huán)境,并采集該環(huán)境下的測量數(shù)據(jù)進行實驗,以驗證算法的可行性。如圖8所示,為構(gòu)建環(huán)境所用到的障礙物,包括:兩個邊長為6 cm的六面體,一個邊長為4.5 cm的八面體,一個直徑為11.5 cm的圓柱及多個長為15 cm、寬為4 cm的長方體。圖中瓷磚大小為50 cm×50 cm。

圖8 障礙物集合

結(jié)合粗大誤差產(chǎn)生條件,我們布置了如圖9(a)所示的實測環(huán)境。除了用到了部分障礙物,實際環(huán)境中也包含了門及黑色墻磚。如圖9(b)所示為由旋轉(zhuǎn)激光掃描儀在該環(huán)境下所得到的激光點圖。由激光點圖可知,在某些夾角處產(chǎn)生了粗大誤差。

圖9 采樣環(huán)境

4.2 實驗分析

測量現(xiàn)實環(huán)境能得到環(huán)境的測量集(一圈的有效掃描數(shù)據(jù)),為了方便分析所提算法的優(yōu)缺點,我們對測量集的數(shù)據(jù)進行人工標注,標注每一個數(shù)據(jù)點的好壞。本文使用Auto CAD(Autodesk Computer Aided Design)按1∶10的比例重建了環(huán)境圖,如圖10所示為環(huán)境1的等比例平面圖,并模擬激光束發(fā)射,每隔一度數(shù)據(jù)進行一次采樣,得到每個角度對應(yīng)的測距長度,構(gòu)成環(huán)境的標準集。

圖10 環(huán)境1等比例平面圖

根據(jù)標準集標注測量集的方法為:對于測量集中每一個點數(shù)據(jù),找到其在標準集中的值,若兩者差值的絕對值小于閾值,則定義為好點,否則為壞點。本文取對應(yīng)標準采樣點的20 %作為閾值。如圖11所示,為標準集和測量集的混合圖,橫軸代表采樣時間,縱軸代表測距長度,為了顯示方便,對于某些角度是無效數(shù)據(jù)的情況做了數(shù)據(jù)填充,無效點的縱坐標值填充為零,圖中方點為測量集,圓點代表標準集。

圖11 標準集與測量集

使用所提算法對測量集進行過濾,圖11中“*”點即為算法認為需要被過濾的點。因為所提過濾算法將包含成員數(shù)較少的段定義為粗大誤差,進行濾掉,因此會出現(xiàn)誤過濾的情況。

根據(jù)多次實驗測試,本文將使用參數(shù)α=20,N=4,如下表所示為過濾結(jié)果,錯誤數(shù)據(jù)標記為0,正確數(shù)據(jù)標記為1。以標準集對測量集的標記為準對比所提過濾算法對測量集的標記,可以得到過濾算法的過濾率等評價指標。對于環(huán)境1,正確率為98.12 %,有1.94 %的數(shù)據(jù)被誤過濾,不過算法的未過濾率為0 %,這說明算法能準確的識別粗大誤差數(shù)據(jù)并實現(xiàn)過濾,具體過濾結(jié)果如表1所示。

表1 過濾結(jié)果

同一個環(huán)境下,機器人運動時的測量數(shù)據(jù)與靜止時的測量數(shù)據(jù)有很大差別,但是很難為移動中得到的測量集制定標準集,即難以人工標注,因此本文借助了相對運動的概念,使機器人靜止,但布置了多種障礙物環(huán)境,在能產(chǎn)生粗大誤差的同時讓環(huán)境盡量復(fù)雜,如此既能得到和機器人移動時相近的測量集,也能進行人工標注。本文設(shè)置了多組實驗,如圖12所示為另外三組實驗的環(huán)境圖和激光點圖。

(a)環(huán)境2

(b)環(huán)境3

(c)環(huán)境4

以上所述的四種環(huán)境的算法過濾效果匯總?cè)绫?所示。結(jié)合環(huán)境圖和過濾結(jié)果可知,所提算法在不同的環(huán)境下均能實現(xiàn)粗大誤差數(shù)據(jù)的過濾(未過濾率均為0 %)。在較為復(fù)雜的環(huán)境4中,誤過率的數(shù)據(jù)最多,達到了5.05 %,這主要是因為,環(huán)境4中存在較多離散的障礙物。這些障礙物在離激光源較遠的時候,其數(shù)據(jù)點集所對應(yīng)的段成員數(shù)會很少,因此被誤過濾。但因為數(shù)據(jù)量大,所以誤過濾部分數(shù)據(jù)是可以接受的,又因為測距是在移動中進行的,所以不會出現(xiàn)某一障礙物所對應(yīng)的測量數(shù)據(jù)一直被誤過濾的情況。

4.3 特殊環(huán)境分析

粗大誤差與相鄰數(shù)據(jù)存在較大的距離差,為何不能用直接比較的方法進行過濾？是因為直接比較能過濾粗大誤差,但只能做到部分過濾,無法應(yīng)對某些特殊情況。如圖13所示,a、b均為粗大誤差,但是由于a點與b點的距離差較小,不滿足粗大誤差的判定條件,a點會被漏判為好點,同理b點也會被漏判。而且,距離差的閾值不好界定。因此,直接比較的方法在多樣的環(huán)境下會存在漏判,不具備魯棒性。

表2 過濾結(jié)果統(tǒng)計

圖13 特殊粗大誤差

那么基于分段法,應(yīng)用弦夾角或弦高比進行分段的算法是否可行？仍存在閾值無法確定和特殊環(huán)境漏判的問題,如弦夾角法通過比較當(dāng)前點的弦夾角和預(yù)設(shè)閾值來進行突變點判斷,那么在a點所示環(huán)境下,其弦夾角較大,從而產(chǎn)生了漏判。但是本文算法能將a、b分為同一段,進而過濾。因此,本文算法具備一定的魯棒性,且給出了動態(tài)閾值的設(shè)定方法。

理論上,本文算法對于離散環(huán)境會出現(xiàn)所有數(shù)據(jù)均被誤過濾的情況。取N=4,當(dāng)段成員數(shù)小于N時進行過濾,我們分析該條件下環(huán)境的離散程度。如圖13所示為激光視野的極限模型,表示被測物體表面至少有多長才能被四個連續(xù)的激光點擊中。設(shè)圖4中圓的半徑為r,相鄰線段間的夾角為1°。如圖14(a)所示,取r=300 cm,可得四條有效范圍內(nèi)最長的激光光束所擊中的物體,其最小長度約為15.7 cm。這意味著在機器視野最遠處的物體,若長度在15.7 cm以下,則會被誤過濾,該物體對激光傳感器不可見。如圖14(b)所示,取r=30 cm,可得四條有效范圍內(nèi)最短的激光光束所擊中的物體,其最小長度約為1.6 cm,即在機器視野最近處的物體,只要長度大于1.6 cm,激光傳感器均能檢測到。

本文認為,由于環(huán)境過于離散導(dǎo)致好點被誤過濾的情況不需要視為特殊情況區(qū)別對待。因為機器一直在移動,在極限遠時被誤過濾的數(shù)據(jù),會在機器人靠近物體的時候再次被檢測到。如環(huán)境3和環(huán)境4中的兩個圓柱體,環(huán)境3中的圓柱體離機器較近,測量數(shù)據(jù)被保留下來,而環(huán)境4中的因為過遠,生成的段成員數(shù)較少而被過濾。

圖14 激光視野極限模型

另一種離散環(huán)境,如圖13所示,由于粗大誤差的出現(xiàn),將連續(xù)的序列分割成了兩個子序列,由于某段子序列過小而產(chǎn)生了誤過濾。對于該種情況可以通過合并兩個子序列,進而再進行段判斷,決定是否過濾。當(dāng)然也可以不處理該種情況,如前所述,當(dāng)機器人移動到近處時,自然會再次檢測到誤過濾的物體。因此,誤過濾部分數(shù)據(jù)是可以接受的。

5 總 結(jié)

本文通過分析移動旋轉(zhuǎn)測距數(shù)據(jù)的特點,提出了一種基于動態(tài)閾值分段的移動旋轉(zhuǎn)激光掃描數(shù)據(jù)實時過濾算法,可應(yīng)用于移動機器人領(lǐng)域,對機器移動中采集的旋轉(zhuǎn)激光掃描數(shù)據(jù)進行實時過濾。算法通過前序有效采樣值為當(dāng)前采樣點設(shè)定動態(tài)閾值,將滿足閾值的采樣點合并為同一個分段,再將段成員數(shù)量較少的段作為粗大誤差進行過濾,通過考慮入射角作為約束條件,分析平緩物體表面采樣數(shù)據(jù)的分布,推導(dǎo)出了動態(tài)閾值的設(shè)定方法,從而實現(xiàn)了使用動態(tài)閾值進行數(shù)據(jù)分段。算法兼顧了移動機器人對過濾的實時性需求,最后的實驗分析表明,該算法具備一定的魯棒性,能準確過濾粗大誤差,雖然存在誤過濾,但在可接受范圍內(nèi)。