基于前瞻窗口的單線激光雷達(dá)特征提取方法研究

馬小陸 ,鞏朝光 ,王兵 ,鄭睿

(1.安徽工業(yè)大學(xué) 電氣與信息工程學(xué)院,安徽 馬鞍山,243002;2.安徽師范大學(xué) 物理與電子信息學(xué)院,安徽蕪湖,241002)

單線激光雷達(dá)特征提取對移動(dòng)機(jī)器人的目標(biāo)追蹤、目標(biāo)提取和特征融合等具有重要作用,如何準(zhǔn)確且可靠檢測出點(diǎn)云特征成為研究熱點(diǎn)。國內(nèi)外學(xué)者針對單線激光雷達(dá)特征提取已經(jīng)做出了大量研究。例如,Siadat[1]等在基于點(diǎn)間距離分割(Point-Distance-Based Segmentation,PDBS)算法基礎(chǔ)上提出了以相鄰點(diǎn)之間的差值提取直線的連續(xù)邊沿追蹤(Successive Edge Following,SEF)算法,劉朋[2]等提出使用相鄰點(diǎn)之間的斜率差提取環(huán)境特征。Arras[3]等提出線性回歸(Line-Regression,LR)算法,實(shí)現(xiàn)移動(dòng)機(jī)器人在地圖上的定位。Borges[4]、Zhang[5]等在分割—合并(Split-and-Merge,SM)算法[6-7]基礎(chǔ)上,提出了迭代端點(diǎn)擬合(Iterative End Point Fit,IEPE)方法提取環(huán)境中的直線特征,該方法具有速度快和準(zhǔn)確度高的特點(diǎn),但由于該方法對閾值敏感,所以容易出現(xiàn)分割、合并效果不好的現(xiàn)象,因此滿增光[8]等將IEPE 方法融合端點(diǎn)擬合(End Point Fit,EPF)和總體最小二乘方法,有效降低了發(fā)生欠分割或過分割的可能性,并且將該方法應(yīng)用于室內(nèi)AGV 地圖創(chuàng)建。除此之外,張巖[9]等提出了隨機(jī)采樣一致(Random Sample Consensus,RANSAC)算法,該方法可以較為穩(wěn)健的進(jìn)行特征提取,但是特征提取正確率不高[10]。Fernandes[11]等、李萬莉[12]等將霍夫變換(Hough Transform)應(yīng)用于單線激光雷達(dá)特征提取上,邢亞蒙[13]等提出了改進(jìn)的Hough 變換算法,使用激光掃描信息提取墻面特征,精度較高,但對噪聲比較敏感,穩(wěn)定性差。

綜上,當(dāng)前的特征提取方法存在著對噪聲敏感、穩(wěn)定性差等問題。針對上述問題,本文提出了一種基于前瞻窗口的單線激光雷達(dá)特征提取方法。

1 基于前瞻窗口的單線激光雷達(dá)特征提取方法

本文的單線激光雷達(dá)特征提取方法包括雙邊濾波、Harris 角點(diǎn)提取、基于前瞻窗口的區(qū)域搜索,相交直線拆分。激光點(diǎn)云原始數(shù)據(jù)使用極坐標(biāo)表示為

本方法在笛卡爾坐標(biāo)系下完成,轉(zhuǎn)化到直角坐標(biāo)系:

un為在笛卡爾坐標(biāo)系下的掃描點(diǎn),本文中N取909。

1.1 雙邊濾波

由于激光雷達(dá)在測距過程中受到傳感器噪聲、環(huán)境噪聲以及物體對激光的反射率影響,需要對點(diǎn)云數(shù)據(jù)進(jìn)行預(yù)處理,這有利于提升測量的準(zhǔn)確性。預(yù)處理的方法要求滿足在過濾噪聲的同時(shí)又能很好的保護(hù)圖像的邊緣。激光雷達(dá)噪聲主要變現(xiàn)為非線性噪聲,常用濾波的方法主要有中值濾波、雙邊濾波以及小波變換[14]等方法,其中雙邊濾波[15]具有非線性、噪聲抑制、數(shù)據(jù)平滑等特性以及更好的邊緣保護(hù)特征,所以本文選擇雙邊濾波。

雙邊濾波定義為

式(3)中權(quán)重Wp定義為

式(4)中,Ifiltered(x)表示當(dāng)前濾波的掃描點(diǎn)深度距離值,Ω表示當(dāng)前窗口區(qū)域,xi表示窗口內(nèi)非窗口中心的其他掃描點(diǎn)位置,I(xi)為其對應(yīng)的深度距離值,fr表示平滑值域中強(qiáng)度差異的核函數(shù),gs表示平滑空間域差異的核函數(shù)。本文中使用高斯函數(shù)作為值域、空間域核函數(shù)

每個(gè)掃描點(diǎn)權(quán)重

式(7)中,σd為空間域核函數(shù)方差,σr為值域核函數(shù)方差,本文分別取3、0.1,對式(7)進(jìn)行歸一化后得

式(8)中Id為濾波后的掃描點(diǎn),重復(fù)以上過程對掃描點(diǎn)進(jìn)行濾波。

使用激光雷達(dá)真實(shí)掃描到的數(shù)據(jù)進(jìn)行測試,結(jié)果如圖1 所示。

圖1 橙色散點(diǎn)表示濾波后的點(diǎn)云,藍(lán)色點(diǎn)為原始點(diǎn)云,原始點(diǎn)云受到反射率或噪聲影響后,物體輪廓點(diǎn)云出現(xiàn)突變、波動(dòng),經(jīng)過雙邊濾波后波動(dòng)得到了抑制,獲得了平滑的點(diǎn)云。驗(yàn)證了雙邊濾波對激光雷達(dá)濾波的有效性。

圖1 點(diǎn)云雙邊濾波效果圖

1.2 Harris 角點(diǎn)檢測

環(huán)境中的特征點(diǎn)主要由角點(diǎn)、直線、圓弧、斷點(diǎn)等組成,首先對角點(diǎn)進(jìn)行定位,再進(jìn)行特征線的提取,有利于提升檢測的速度與準(zhǔn)確性。Harris 角點(diǎn)檢測方法通過計(jì)算掃描點(diǎn)的曲率與梯度來檢測角點(diǎn),具有良好的噪聲魯棒性、穩(wěn)定性[16],下面給出計(jì)算詳細(xì)過程。首先計(jì)算掃描距離點(diǎn)u(x,y),在X和Y方向上的2 個(gè)梯度

計(jì)算梯度乘積

對ux2、ux2、uxy使用σ取1的高斯函數(shù)加權(quán)得

式(11)中w為高斯加權(quán)函數(shù)。圖像u(x,y)在點(diǎn)(x,y)處平移(Δx,Δy)后的自相關(guān)函數(shù)可以近似為

將自相關(guān)函數(shù)表示為橢圓方程:

橢圓的扁率與尺寸由M(x,y)的特征值決定,與物理特征對應(yīng)關(guān)系如表1 所示。

表1 環(huán)境特征與特征值λ1、λ2的對應(yīng)關(guān)系

將激光點(diǎn)云的投影點(diǎn)作為待處理點(diǎn),這時(shí)方法沿著投影點(diǎn)軌跡進(jìn)行計(jì)算,而非對整個(gè)二維平面進(jìn)行計(jì)算(圖2),提升準(zhǔn)確度的同時(shí),提升了檢測效率。

圖2 沿序列位置角點(diǎn)提取示意圖

計(jì)算每一個(gè)投影點(diǎn)的角點(diǎn)響應(yīng)值

式(15)中α為經(jīng)驗(yàn)常數(shù)。角點(diǎn)位置響應(yīng)值閾值TR=pr*Rmax,pr為角點(diǎn)響應(yīng)比例系數(shù),Rmax為遍歷一幀點(diǎn)云后計(jì)算得到的最大角點(diǎn)響應(yīng)值。

對角點(diǎn)響應(yīng)值進(jìn)行局部非極大值抑制,取半徑為d的窗口,滿足條件

取α=0.06、pr=0.000 2、d=2,獲取單線激光雷達(dá)在室內(nèi)掃描的點(diǎn)云,使用經(jīng)過2.1 節(jié)濾波處理后,提取點(diǎn)云中的角點(diǎn),結(jié)果如圖3 所示。圖3中紅圈處為角點(diǎn)位置,藍(lán)色為初始點(diǎn)云。

由圖3 可知,該方法可以準(zhǔn)確地提取點(diǎn)云中的特征角點(diǎn)。

圖3 角點(diǎn)提取結(jié)果

1.3 基于前瞻窗口的區(qū)域搜索

為提取到完整的特征,使用基于前瞻窗口的搜索方法,以提取到的角點(diǎn)位置為起點(diǎn),計(jì)算當(dāng)前點(diǎn)Pcurrent(x0,y0)與下一點(diǎn)Pnext(x1,y1)組成直線的斜率ki,若|ki|小于閾值,將Pnext歸為當(dāng)前類,同時(shí)將Pnext更新為Pcurrent,若|ki|大于閾值,則進(jìn)入前瞻窗口搜索模式。前瞻窗口搜索原理如圖4 所示,圖中P0～P9屬于同一特征類,P5、P6為跳躍斷點(diǎn)或噪點(diǎn),假設(shè)搜索窗口寬度為3,以P0為起點(diǎn)搜索,P0至P4處的|ki|均小于閾值,當(dāng)Pcurrent為P4,Pnext為P5時(shí),|ki|大于閾值,進(jìn)入前瞻窗口搜索模式,取進(jìn)入窗口搜索前的點(diǎn)為Pcurrent,即將P4設(shè)為Pcurrent,在窗口內(nèi)依次取點(diǎn)作為Pnext計(jì)算|ki|值。圖中P5、P6與P4點(diǎn)計(jì)算的|ki|均大于閾值,將下一點(diǎn)設(shè)為Pnext,且P7與Pcurrent所在直線的斜率小于閾值,此時(shí)退出窗口搜索模式,并更新Pcurrent為P7,Pnext等于P8,繼續(xù)根據(jù)斜率繼續(xù)搜索。圖4 搜索完成后,歸為此類的點(diǎn)集為{P0,P1,P2,P3,P4,P7,P8,P9},完成了特征點(diǎn)聚類。

圖4 前瞻窗口搜索、聚類示意圖

若搜索完整個(gè)前瞻窗口,均未出現(xiàn)斜率小于設(shè)定閾值的點(diǎn),將Pcurrent點(diǎn)標(biāo)記為特征終止端點(diǎn),示意圖如5 所示,圖5搜索后對應(yīng)的點(diǎn)集為{P0,P1,P2,P3,P4,P5}。按照同種方式向前繼續(xù)搜索,獲得特征點(diǎn)云的起始點(diǎn)。對每一個(gè)角點(diǎn)進(jìn)行前后方向搜索,若角點(diǎn)處于之前搜索得到的點(diǎn)云中,則跳過此角點(diǎn)的搜索,遍歷完角點(diǎn)后,獲得各類特征點(diǎn)云。

圖5 特征終止位置前瞻窗口搜索示意圖

完整的特征提取流程圖如圖6 所示。圖6 中,i為點(diǎn)云投影點(diǎn)數(shù)組ranges 下標(biāo),num_max 表示點(diǎn)云總個(gè)數(shù)。變量ranges_last 用于暫存符合條件的距離參考點(diǎn),搜索過程中對計(jì)算當(dāng)前點(diǎn)Pcurrent(x0,y0)與下一點(diǎn)Pnext(x1,y1)所在直線斜率為

圖6 特征線區(qū)域搜索流程圖

若斜率|ki|不小于設(shè)定閾值T_k,進(jìn)入前瞻窗口掃描模式,否則繼續(xù)搜索,搜索過程中|ki|計(jì)算方式為

式(18)中,Ps(xs,ys)為進(jìn)入窗口掃描模式前的一個(gè)掃描點(diǎn),依次計(jì)算窗口內(nèi)掃描點(diǎn)與Ps(xs,ys)點(diǎn)所在直線的斜率絕對值,若在前瞻窗口寬度Tw內(nèi)|ki|大于T_k的點(diǎn)個(gè)數(shù)(cnt)等于前瞻窗口寬度,則Ps(xs,ys)為特征的邊界點(diǎn),若在前瞻窗口范圍內(nèi)|ki|小于T_k,則退出前瞻窗口搜索模式,并將i處的點(diǎn)坐標(biāo)更新為Ps(xs,ys),且將i點(diǎn)聚類到當(dāng)前特征點(diǎn)集內(nèi)。對提取到的角點(diǎn)重復(fù)以上搜索步驟,獲取點(diǎn)云中存在的特征點(diǎn)集。沿著投影點(diǎn)軌跡上下搜索,找出點(diǎn)云特征起始位置,搜索判定條件總結(jié)如表2 所示。

表2 前瞻窗口搜索判定條件

向下搜索與向上搜索原理一致,重復(fù)此過程對每一個(gè)角點(diǎn)進(jìn)行上下區(qū)域搜索。若搜索角點(diǎn)處于已搜索到的特征點(diǎn)云中,則放棄對當(dāng)前角點(diǎn)搜索,并選取下一角點(diǎn)進(jìn)行處理。使用上述1.2 節(jié)提取到的角點(diǎn)為中心進(jìn)行搜索,結(jié)果如圖7 所示。

圖7 中使用方框圈出區(qū)域搜索后的角點(diǎn)位置,經(jīng)前瞻窗口搜索后,特征的邊界點(diǎn)被準(zhǔn)確的提取了出來,且對噪聲不敏感,穩(wěn)定性較好。

圖7 方法角點(diǎn)提取結(jié)果

實(shí)際的物理環(huán)境中,存在著弧線、直線、折線等特征,需要將這些特征有效的區(qū)分開。對于折線,需要將兩相交直線進(jìn)行分割,拆分出兩條獨(dú)立的直線,從而有利于后續(xù)的擬合處理。首先創(chuàng)建一個(gè)長度為2m+1的窗口(文中m取3),分別在前后兩半窗口中取值,并計(jì)算相對于窗口中心點(diǎn)的斜率

則兩段線段的夾角

式(20)中,k1=(y0-ym)/(x0-xm),k2=(ym-y2m+1)/(xm-x2m+1)。按照此方法遍歷完整個(gè)區(qū)域,計(jì)算窗口內(nèi)線段的夾角,若夾角大于設(shè)定閾值,則窗口中心即為交點(diǎn)所在位置,此時(shí)的θ近似等于兩條直線的夾角。以交點(diǎn)位置為分界點(diǎn),將相交的特征進(jìn)行拆分,最終提取結(jié)果如圖8 所示。結(jié)果表明,該方法可以將特征有效、穩(wěn)定地提取出來,且對于相交的直線也可以準(zhǔn)確進(jìn)行分割。

圖8 特征拆分后提取結(jié)

2 實(shí)驗(yàn)驗(yàn)證

2.1 實(shí)驗(yàn)平臺(tái)介紹

為驗(yàn)證算法的實(shí)用性與可靠性,將算法移植到自制的機(jī)器人平臺(tái),結(jié)構(gòu)示意圖如圖9 所示。機(jī)器人平臺(tái)包括主體支撐、執(zhí)行機(jī)構(gòu)、處理單元、電源管理以及傳感器部分。機(jī)器人平臺(tái)搭載了ROS Kinect 版本系統(tǒng),配備了光電碼盤、基于三角測量的激光雷達(dá)[17]、RGBD 攝像機(jī)等傳感器。平臺(tái)前輪平衡結(jié)構(gòu)保證了機(jī)器人平臺(tái)的平穩(wěn),可減少地面環(huán)境對激光雷達(dá)等傳感器的干擾。相關(guān)軟硬件配置如表3所示。

圖9 機(jī)器人結(jié)構(gòu)示意圖

表3 機(jī)器人軟硬件配置

實(shí)驗(yàn)選取了實(shí)驗(yàn)室環(huán)境進(jìn)行測試,環(huán)境中存在著桌子、墻壁等結(jié)構(gòu)化特征,也存在著廢舊電纜、凳子腿等非結(jié)構(gòu)化特征。實(shí)驗(yàn)從距離測量、長度測量、提取成功次數(shù)角度對方法特征提取效果進(jìn)行分析,并與準(zhǔn)確度較高的Hough 變換方法進(jìn)行對比,驗(yàn)證本方法在特征提取上的準(zhǔn)確性與魯棒性。

2.2 實(shí)驗(yàn)過程

首先對方法的濾波效果進(jìn)行驗(yàn)證。提取真實(shí)環(huán)境中的特征,環(huán)境如圖10(a)所示,方法處理完成后,將提取到的特征以點(diǎn)云形式發(fā)布出去,如圖10(b)所示。

圖10 特征提取實(shí)驗(yàn)環(huán)境及結(jié)果

圖10(b)中,取一段特征直線“LA-LB”,計(jì)算特征區(qū)域內(nèi)雷達(dá)中心到掃描平面的距離,并在同一時(shí)間戳下計(jì)算使用相同區(qū)域內(nèi)初始點(diǎn)云的掃描距離值。重復(fù)上述測試過程100 次,記錄每次獲取的距離值,統(tǒng)計(jì)結(jié)果如圖11 所示。圖11 中,藍(lán)色線為每次使用原始點(diǎn)云數(shù)據(jù)獲取的距離值,紅色線為使用該方法處理后的測距結(jié)果。由圖11 可知,使用該方法對點(diǎn)云數(shù)據(jù)處理后,與原始點(diǎn)云測距相比測距結(jié)果穩(wěn)定,數(shù)據(jù)更為準(zhǔn)確。對比2 次實(shí)驗(yàn)結(jié)果,統(tǒng)計(jì)兩者的測量均值、標(biāo)準(zhǔn)差,并以均值為參考值統(tǒng)計(jì)絕對平均誤差,如表4 所示。由表4 可知,使用該方法處理后的點(diǎn)云,在測距方面結(jié)果更為穩(wěn)定,方法有效提升了點(diǎn)云距離掃描值的精確性。

表4 點(diǎn)云測距結(jié)果統(tǒng)計(jì)

圖11 該方法輸出點(diǎn)云數(shù)據(jù)與原始點(diǎn)云數(shù)據(jù)測距結(jié)果對比圖

為驗(yàn)證方法的穩(wěn)定性與準(zhǔn)確性,首先對點(diǎn)云進(jìn)行濾波,再分別使用本方法與Hough 變換方法對同一場景下的環(huán)境提取特征,結(jié)果如圖12(a)～(b)所示。

圖12 使用本方法與Hough 變換方法特征提取結(jié)果對比

圖12(a)中,橙色圓點(diǎn)為提取到的特征起始端點(diǎn),圖12(b)中Hough變換方法提取到特征的起始點(diǎn)使用黑色圓點(diǎn)進(jìn)行標(biāo)記,為便于統(tǒng)計(jì)使用“L_A”、“L_B”、“L_C”…“L_Q”分別對兩種方法提取到的特征進(jìn)行編號。

將本方法與Hough 變換方法提取結(jié)果進(jìn)行對比,在同一幀激光數(shù)據(jù)下,統(tǒng)計(jì)在圖12 中編號位置2種方法提取情況,若提取到相應(yīng)特征則對應(yīng)特征的成功提取次數(shù)加一,并計(jì)算2種方法的特征起始點(diǎn)序列號均值。重復(fù)上述檢測、統(tǒng)計(jì)過程100 次,統(tǒng)計(jì)結(jié)果如表5 所示。

表5 2 種方法統(tǒng)計(jì)結(jié)果

續(xù)表5

表5 中,將成功提取次數(shù)轉(zhuǎn)換成百分比,使用柱狀圖進(jìn)行表示,如圖13 所示。

由圖13和表5可知,兩種方法提取到的特征范圍結(jié)果相近,表明兩種方法提取精確度相近。但是本方法特征提取穩(wěn)定性明顯高于Hough 變換方法,且對于“L_A”、“L_B”、“L_E”等非結(jié)構(gòu)化特征也能夠保證較高的正確率。實(shí)驗(yàn)表明本方法對于單線激光雷達(dá)特征提取具有較高的魯棒性、穩(wěn)定性。

圖13 2 種方法特征提取成功率統(tǒng)計(jì)圖

為對比特征提取的準(zhǔn)確性,分別使用本方法與Hough 變換方法提取到的特征點(diǎn)云測量特征“L_I”的長度,根據(jù)提取到的特征起始點(diǎn)n1、n2查找所對應(yīng)的掃描距離值r1、r2,待測量物體在掃描平面的長度

式(21)中,αi=angle_delta*ni+angle_start 其中angel_start、angel_delta 表示角度起始值與角度增量,實(shí)驗(yàn)中分別取-π/2、π/909。獲取100 幀不同時(shí)刻點(diǎn)云掃描數(shù)據(jù),使用兩種方法計(jì)算每一次特征“L_I”處對應(yīng)的特征長度,并與手工測量結(jié)果(746 mm)進(jìn)行對比,如圖14 所示。

圖14 中,使用紅色線表示使用本方法長度測量結(jié)果,藍(lán)色線表示Hough 變換方法測量結(jié)果,綠色線表示手工測量結(jié)果。統(tǒng)計(jì)測量結(jié)果的均值、標(biāo)準(zhǔn)差,以手工測量值為參考計(jì)算絕對平均誤差以及均值偏差百分比,如表6 所示。

結(jié)合圖14 和表6 可知,本次長度測量實(shí)驗(yàn)中,本方法特征提取精度較高,數(shù)據(jù)波動(dòng)較小,平均長度與真實(shí)長度僅相差1.5 mm,而使用Hough 變換方法提取結(jié)果與真實(shí)值相差5.7 mm。且由表6 可知,本方法測量結(jié)果的標(biāo)準(zhǔn)差、平均誤差均低于Hough變換方法,表明本方法具有更高的穩(wěn)定性、準(zhǔn)確性。

圖14 使用該方法處理后點(diǎn)云長度測量與手工測量對比圖

表6 長度測量對比結(jié)果統(tǒng)計(jì)

3 結(jié)論

目前的單線激光雷達(dá)特征方法,仍存在著對噪聲敏感、穩(wěn)定性不高的問題,導(dǎo)致機(jī)器人在單線激光雷達(dá)特征提取上效果不夠理想。本文針對現(xiàn)有特征提取方法存在的問題,提出了基于前瞻窗口的特征提取方法,方法中使用了前瞻窗口搜索,有效提升了特征提取的穩(wěn)定性、準(zhǔn)確性,且對噪聲不敏感。最后將方法應(yīng)用在真實(shí)機(jī)器人平臺(tái),實(shí)驗(yàn)結(jié)果表明,特征提取穩(wěn)定性、準(zhǔn)確性較高,對環(huán)境適應(yīng)性更好,滿足機(jī)器人特征提取需求。