馬小陸，甄文雪，鞏朝光

(安徽工業(yè)大學(xué) 電氣與信息工程學(xué)院，安徽 馬鞍山 243000)

激光雷達(dá)(LiDAR)因其測距精度高、實(shí)時(shí)掃描速度快、穩(wěn)定性等特點(diǎn)，現(xiàn)被廣泛應(yīng)用在機(jī)器人等領(lǐng)域中，激光雷達(dá)獲取的點(diǎn)云信息反映的是其與周圍物體之間的距離信息。因此對(duì)原始點(diǎn)云數(shù)據(jù)的處理非常重要，它是獲取高精度點(diǎn)云數(shù)據(jù)信息的基礎(chǔ)[1]。

點(diǎn)云數(shù)據(jù)噪聲表現(xiàn)為物體表面(包含物體邊緣)一些隨機(jī)分布的點(diǎn)，為了平滑這些噪聲點(diǎn)，當(dāng)前對(duì)原始點(diǎn)云數(shù)據(jù)的處理主要采用濾波方法，如萬劍華等[2-3]提出基于曲面進(jìn)行二次約束的方法，該方法能夠?qū)ξ矬w表面點(diǎn)云進(jìn)行濾波；隋立春等[4-5]提出一種提取形態(tài)學(xué)點(diǎn)云濾波的處理方法；以雙邊濾波思想為核心，鄭麗娟等[6-8]提出新的噪聲檢測過濾方法，該方法能夠有效濾除數(shù)據(jù)和圖像中的噪聲；楊宏飛等[9]提出基于加權(quán)因子的雙邊濾波方法，該方法能夠?yàn)V除離散波動(dòng)較小的噪聲；除此之外，Alexa等[10]提出基于多項(xiàng)式局部逼進(jìn)曲面移動(dòng)最小二乘(Moving Least-Squares，MLS)算法，得到較平滑的物體表面；Chen等[11]提出隱藏式移動(dòng)最小二乘方法(Improved Moving Least-Squares，IMLS)，構(gòu)建了接近隱式曲面；Zhang等[12]提出加權(quán)最小二乘方法加快了MLS，這些方法具有速度快的優(yōu)點(diǎn)，但都對(duì)噪聲敏感，穩(wěn)定性差。戴士杰等[13]提出的張量投票算法和朱廣堂等[14]提出直接使用曲率結(jié)合閾值的方法對(duì)特征點(diǎn)進(jìn)行濾波會(huì)導(dǎo)致準(zhǔn)確度下降。

綜上可以看出，當(dāng)前的點(diǎn)云濾波方法存在著穩(wěn)定性差、準(zhǔn)確度下降等問題。本文結(jié)合傳統(tǒng)高斯濾波的思想，提出一種加權(quán)的LiDAR點(diǎn)云濾波方法。該方法在雷達(dá)掃描點(diǎn)云數(shù)據(jù)過程中，根據(jù)數(shù)據(jù)點(diǎn)之間夾角的余弦值設(shè)置合理權(quán)重，從而較好地克服了上述問題。

1 改進(jìn)的雷達(dá)點(diǎn)云濾波方法

本文使用的單線激光雷達(dá)如圖1所示，在工作模式時(shí)，激光發(fā)射器在高速旋轉(zhuǎn)的同時(shí)向外界發(fā)射激光，激光在碰到障礙物后返回[15]，接收器將接收到的光信號(hào)化為電信號(hào)并實(shí)時(shí)傳送給雷達(dá)內(nèi)部。該雷達(dá)是基于三角測距的原理，其掃描速度快、測距誤差小、穩(wěn)定性高、技術(shù)成熟。

圖1 激光雷達(dá)結(jié)構(gòu)示意圖Fig.1 Structural diagram of lidar

文中配置的激光雷達(dá)角度增量為0.003 456 rad，激光雷達(dá)的掃描范圍是[-π/2, π/2]，每一幀掃描可獲取909個(gè)激光掃描點(diǎn)，且每一個(gè)角度的激光掃描點(diǎn)上都有一個(gè)掃描距離與之對(duì)應(yīng)。這些掃描數(shù)據(jù)點(diǎn)以極坐標(biāo)形式返回測量數(shù)據(jù)，極坐標(biāo)表示為

(1)

式中，ρn表示以角度增量為間隔第n個(gè)角度增量的方向上掃描到的物體的距離。

本文方法的測量數(shù)據(jù)是在笛卡爾坐標(biāo)系下得到的，兩者的轉(zhuǎn)換關(guān)系如下。

(2)

式中，xn,yn為笛卡爾坐標(biāo)系下雷達(dá)掃描點(diǎn)的坐標(biāo)，文中n取909。

1.1 高斯濾波

當(dāng)LiDAR 開始工作時(shí)，由周圍環(huán)境反射激光點(diǎn)云產(chǎn)生點(diǎn)云信息，在獲得的點(diǎn)云信息中含有大量的噪聲，這些噪聲總體為以下兩個(gè)方面。第一個(gè)為LiDAR 傳感器噪聲，當(dāng) LiDAR 工作時(shí)激光旋轉(zhuǎn)時(shí)產(chǎn)生的振動(dòng)和出廠時(shí)的精度，影響了LiDAR 的測量效果；第二個(gè)為 LiDAR 工作時(shí)周圍環(huán)境噪聲。受到這些噪聲影響，需要對(duì)點(diǎn)云進(jìn)行處理。第二個(gè)噪聲為本文的研究重點(diǎn)。

高斯濾波本質(zhì)上是一種通過正態(tài)分布確定權(quán)重的線性平滑濾波，能夠有效地抑制噪聲，平滑數(shù)據(jù)，其實(shí)高斯濾波就是對(duì)LiDAR 掃描到的每一個(gè)點(diǎn)云數(shù)據(jù)進(jìn)行加權(quán)平均之后再進(jìn)行卷積的過程。傳統(tǒng)的高斯濾波模板W定義為

(3)

式中f(x)定義為

(4)

高斯濾波結(jié)果表達(dá)式為

Ifiltered=W·I(xi)

(5)

掃描窗口的寬度定義為

x=lin space(-d,d,2·d+1)

(6)

式(5)與式(6)中，Ifiltered為當(dāng)前濾波掃描點(diǎn)的距離值，x為當(dāng)前窗口的寬度，d為窗口的半徑，xi為窗口內(nèi)排除極大值位置和無效點(diǎn)位置數(shù)據(jù)之后雷達(dá)掃描到的數(shù)據(jù)點(diǎn)位置，I(xi)為每個(gè)數(shù)據(jù)點(diǎn)對(duì)應(yīng)的距離值。

本文傳統(tǒng)高斯濾波方法中掃描點(diǎn)的高斯模板計(jì)算公式為式(3)和式(4)，公式中x為掃描窗口寬度，文中分別取σ=3，d=1。傳統(tǒng)的高斯濾波方法中，默認(rèn)每個(gè)數(shù)據(jù)點(diǎn)沒有設(shè)置權(quán)值，以模板確定窗口寬度為3的數(shù)據(jù)點(diǎn)經(jīng)高斯濾波后計(jì)算得到一個(gè)新的數(shù)據(jù)點(diǎn)，過程見式(5)。

高斯濾波的核心思想其實(shí)就是卷積過程，也就是將雷達(dá)掃描到的窗口中心點(diǎn)和左右數(shù)據(jù)與一個(gè)高斯模板進(jìn)行卷積操作。本文設(shè)計(jì)中選擇1×3的高斯模板。整個(gè)高斯濾波實(shí)現(xiàn)的過程就是隨著選定的高斯模板的移動(dòng)對(duì)雷達(dá)掃描到的環(huán)境中909個(gè)數(shù)據(jù)點(diǎn)(實(shí)際情況下可用的數(shù)據(jù)點(diǎn)一定小于909個(gè))進(jìn)行卷積處理，并將卷積之后某個(gè)數(shù)據(jù)點(diǎn)的高斯濾波值放到定義好的高斯模板中間位置，隨后高斯模板繼續(xù)基于中心點(diǎn)向后移動(dòng)一個(gè)數(shù)據(jù)點(diǎn)位置。

高斯模板對(duì)整個(gè)數(shù)據(jù)進(jìn)行卷積操作的過程中還需要考慮的地方有：909個(gè)點(diǎn)云數(shù)據(jù)的邊緣情況，保證能取到1×3大小的窗口數(shù)據(jù)點(diǎn)；雷達(dá)掃描到的點(diǎn)云數(shù)據(jù)中存在的極大數(shù)據(jù)點(diǎn)和0值的點(diǎn)，這些數(shù)據(jù)點(diǎn)對(duì)高斯濾波是無效的，在與高斯模板進(jìn)行卷積的過程中，注意窗口中排除不需要的數(shù)據(jù)點(diǎn)。為改進(jìn)后高斯模板能掃描到完整的點(diǎn)云數(shù)據(jù)，以提取到的高斯模板中心位置不為零的數(shù)據(jù)點(diǎn)為中心起點(diǎn)，采用中心點(diǎn)前后位置以相同步長同時(shí)開始搜索的方法，判斷同一時(shí)刻下前后位置數(shù)據(jù)是否為零，若同時(shí)不為零，模板掃描到的數(shù)據(jù)有效，若有一側(cè)數(shù)據(jù)等于零，繼續(xù)搜索，直到前后數(shù)據(jù)都不為零結(jié)束。高斯模板向后移動(dòng)，以提取到的中心位置不為零的數(shù)據(jù)點(diǎn)為中心起點(diǎn)，重復(fù)此過程對(duì)每一個(gè)點(diǎn)云數(shù)據(jù)進(jìn)行前后區(qū)域搜索。

對(duì)雷達(dá)掃描到的有效數(shù)據(jù)點(diǎn)在文中規(guī)定的窗口范圍內(nèi)重復(fù)上述過程，得到傳統(tǒng)高斯濾波方法之后的掃描點(diǎn)數(shù)據(jù)，最后通過濾波得到的真實(shí)掃描數(shù)據(jù)結(jié)果如圖2所示。

圖2 點(diǎn)云高斯濾波Fig.2 Point cloud Gaussian filtering

圖2中，藍(lán)色點(diǎn)為激光雷達(dá)掃描到的室內(nèi)原始數(shù)據(jù)，紅色點(diǎn)為通過傳統(tǒng)濾波之后得到的點(diǎn)云數(shù)據(jù)。通過濾波結(jié)果可以發(fā)現(xiàn)，初次濾波之后，離散程度較大的噪聲點(diǎn)得到了一定程度的平滑處理，但是平滑效果還沒有達(dá)到預(yù)期結(jié)果，需要進(jìn)一步濾波。本文把傳統(tǒng)的高斯濾波思想作為核心，提出一種新的濾波方法。

1.2 加權(quán)高斯濾波

根據(jù)傳統(tǒng)的高斯濾波思想，為了調(diào)整窗口中心數(shù)據(jù)點(diǎn)的影響力，本文采用對(duì)雷達(dá)掃描到的點(diǎn)云進(jìn)行加權(quán)濾波的處理方式，即對(duì)點(diǎn)云采用在傳統(tǒng)高斯濾波的基礎(chǔ)上再加一次權(quán)重的方法，減小雷達(dá)掃描到物體輪廓的波動(dòng)，提高濾波方法準(zhǔn)確性。雷達(dá)點(diǎn)云濾波方法流程如圖3。

圖3 點(diǎn)云濾波方法流程Fig.3 Gaussian filtering method flow

直接使用傳統(tǒng)高斯濾波方法對(duì)雷達(dá)掃描到的數(shù)據(jù)點(diǎn)進(jìn)行濾波，可以處理雷達(dá)點(diǎn)云中較明顯的噪聲，但是還存在離散波動(dòng)較小的噪聲。從matlab中導(dǎo)出的點(diǎn)云效果如圖2所示的離散點(diǎn)分布可以看出，當(dāng)離散數(shù)據(jù)點(diǎn)兩兩之間的夾角逐漸趨于180°時(shí)，點(diǎn)云的效果會(huì)更加平滑，對(duì)后續(xù)進(jìn)行的使用雷達(dá)建圖等效果也會(huì)更好。因此本文考慮從相鄰數(shù)據(jù)點(diǎn)之間的夾角入手，對(duì)雷達(dá)掃描到的點(diǎn)云利用加權(quán)高斯濾波方法進(jìn)行濾波處理，窗口內(nèi)數(shù)據(jù)點(diǎn)之間夾角的大小決定了窗口中心數(shù)據(jù)點(diǎn)所賦的權(quán)值大小，并且以窗口中心數(shù)據(jù)點(diǎn)為起點(diǎn)，分別對(duì)左右兩邊數(shù)據(jù)點(diǎn)賦權(quán)值，保證窗口內(nèi)各相鄰數(shù)據(jù)點(diǎn)之間權(quán)值之和為1。

1.2.1 權(quán)值選取方法

由于在LiDAR 掃描到點(diǎn)云數(shù)據(jù)發(fā)生突變的數(shù)據(jù)點(diǎn)，數(shù)據(jù)特征變化隨之較大，相鄰數(shù)據(jù)點(diǎn)之間的夾角變化劇烈；相反的，在平滑區(qū)域的數(shù)據(jù)點(diǎn)，其數(shù)據(jù)特征變化比較平緩，所對(duì)應(yīng)的數(shù)據(jù)點(diǎn)之間的夾角變化幅度較小。因此從相鄰數(shù)據(jù)點(diǎn)間的夾角入手進(jìn)行權(quán)值的選取是正確合理的。

由正態(tài)分布曲線可知，越接近中心取值越大，越遠(yuǎn)離中心取值越小。當(dāng)數(shù)據(jù)點(diǎn)與相鄰點(diǎn)之間的夾角在0°～90°范圍時(shí)，夾角的余弦值是單調(diào)遞減函數(shù)，即余弦值隨著角度的增大而減小，從而使窗口中心位置數(shù)據(jù)點(diǎn)所占權(quán)重減??；當(dāng)數(shù)據(jù)點(diǎn)與相鄰點(diǎn)之間的夾角在90°～180°范圍時(shí)，夾角余弦值的絕對(duì)值是單調(diào)遞增函數(shù)，即余弦值的絕對(duì)值隨著角度的增大而增大，窗口中心位置的數(shù)據(jù)點(diǎn)所占的權(quán)重就越大。

因此依據(jù)權(quán)重值參照正態(tài)分布選取的原則，并結(jié)合夾角余弦函數(shù)分布規(guī)律，采用窗口內(nèi)相鄰數(shù)據(jù)點(diǎn)之間夾角余弦值來作為濾波時(shí)窗口中心數(shù)據(jù)點(diǎn)的權(quán)值是合理的。

1.2.2 點(diǎn)云數(shù)據(jù)的權(quán)值計(jì)算

首先創(chuàng)建一個(gè)2m+1的窗口(文中m=1)，分別在窗口的中心數(shù)據(jù)點(diǎn)位置和左右兩邊數(shù)據(jù)點(diǎn)取值，取值為激光雷達(dá)在真實(shí)實(shí)驗(yàn)環(huán)境中掃描到的每個(gè)點(diǎn)云數(shù)據(jù)的角度與其對(duì)應(yīng)的距離值，需要注意的是距離值必須為非0的有效值。并計(jì)算出相對(duì)于中心位置在窗口左右兩端的斜率值k1,k2：

(7)

式(7)中x,y分別為極坐標(biāo)系下的雷達(dá)掃描數(shù)據(jù)點(diǎn)對(duì)應(yīng)直角坐標(biāo)系下的x,y軸上的坐標(biāo)。兩段線段之間的夾角為

(8)

方法中加權(quán)高斯模板定為

(9)

式(9)中：

B=[(1+cosθ·c)·D(2)(a+

1)·D(1)(b+1)·D(3)]

(10)

∑B=(1+cosθ·c)·D(2)+(a+

1)·D(1)+(b+1)·D(3)

(11)

x=lin space(-d,d,2·d+1)

(12)

(13)

將改進(jìn)后的窗口中間位置的數(shù)據(jù)點(diǎn)定義為

Ifiltered=W·I(xi)

(14)

式中，σ=3、d=1、a=-0.5、b=-0.5、c=-1，I(xi)是半徑為d的窗口內(nèi)每個(gè)數(shù)據(jù)點(diǎn)位置分別對(duì)應(yīng)的距離值，Ifiltered是改進(jìn)濾波方法之后的窗口中間位置的數(shù)據(jù)點(diǎn)對(duì)應(yīng)的距離值。

根據(jù)上述公式可以知道，改進(jìn)后的高斯濾波方法根據(jù)窗口內(nèi)鄰近區(qū)域點(diǎn)云的夾角余弦值來確定濾波之后點(diǎn)云位置的優(yōu)化。因此，通過測試多次數(shù)值調(diào)整之后，由a，b，c的值對(duì)窗口內(nèi)每個(gè)數(shù)據(jù)點(diǎn)的權(quán)重值進(jìn)行了更加合理的分配，在對(duì)含有較小波動(dòng)點(diǎn)云邊緣信息的離散噪聲進(jìn)行濾波時(shí)盡可能使邊緣信息得到平滑，提高了高斯濾波的準(zhǔn)確性。

1.3 Harris角點(diǎn)檢測

上文所述高斯濾波方法表明，掃描到的室內(nèi)環(huán)境不平滑信息得到了一定程度的改進(jìn)，為驗(yàn)證方法的可靠性，使用Harris角點(diǎn)檢測的方法。Harris角點(diǎn)檢測就是尋找掃描點(diǎn)中的極大值點(diǎn)來確定角點(diǎn)，具有一定的穩(wěn)定性。Harris角點(diǎn)檢測算法實(shí)現(xiàn)的步驟如下：

以單線激光雷達(dá)掃描到的點(diǎn)云數(shù)據(jù)作為輸入值，計(jì)算掃描到的數(shù)據(jù)點(diǎn)u在X和Y方向兩個(gè)上的梯度ux,uy：

(15)

計(jì)算數(shù)據(jù)點(diǎn)在兩個(gè)方向上梯度的乘積：

(16)

(17)

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

c(x,y;Δx,Δy)≈AΔx2+2CΔxΔy+BΔy2

(18)

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

(19)

式(19)中計(jì)算每一個(gè)數(shù)據(jù)點(diǎn)的矩陣M：

(20)

計(jì)算每個(gè)數(shù)據(jù)點(diǎn)的Harris角點(diǎn)響應(yīng)值：

R=detM-α(traceM)2,α∈[0.04,0.06]

(21)

式中α為經(jīng)驗(yàn)常數(shù)，文中α=0.06。每個(gè)數(shù)據(jù)點(diǎn)的角點(diǎn)響應(yīng)值首先要大于閾值TR，計(jì)算公式為

TR=pr·Rmax

(22)

式中pr為角點(diǎn)響應(yīng)比例系數(shù)，取pr=0.000 2，Rmax為遍歷一幀點(diǎn)云后得到的最大角點(diǎn)響應(yīng)值。

在點(diǎn)pi及其某一鄰域D，D={pi|i∈(k-2,k+2)}，若點(diǎn)pk所對(duì)應(yīng)的值在其鄰域中為最大值且大于閾值TR，則保留pk的值作為提取到的角點(diǎn)。

2 實(shí)驗(yàn)結(jié)果與分析

2.1 實(shí)驗(yàn)數(shù)據(jù)采集

為了驗(yàn)證方法的適用性和可靠性，本文實(shí)驗(yàn)數(shù)據(jù)均來源于搭載在機(jī)器人上的單線激光雷達(dá)掃描儀，為得到真實(shí)實(shí)驗(yàn)室室內(nèi)環(huán)境點(diǎn)云數(shù)據(jù)，通過真實(shí)的機(jī)器人平臺(tái)得到雷達(dá)掃描到的點(diǎn)云數(shù)據(jù)，對(duì)所處實(shí)驗(yàn)室內(nèi)掃描的點(diǎn)云進(jìn)行濾波處理。機(jī)器人使用兩輪差速運(yùn)動(dòng)底盤，搭載了激光雷達(dá)、工控機(jī)等設(shè)備，實(shí)驗(yàn)環(huán)境是Windows10，64位操作系統(tǒng)，實(shí)驗(yàn)軟硬件參數(shù)配置見表1。

表1 實(shí)驗(yàn)軟硬件參數(shù)配置

圖4 實(shí)驗(yàn)環(huán)境Fig.4 Experimental environment

本文的實(shí)驗(yàn)環(huán)境如圖4 (b)所示，真實(shí)環(huán)境下雷達(dá)實(shí)時(shí)掃描到的點(diǎn)云圖見圖2。環(huán)境中存在著一些桌子、墻面等簡單特征物體，對(duì)應(yīng)掃描點(diǎn)云圖中的圓弧和線段特征，也存在雜亂電線、形狀較復(fù)雜的椅子腿等復(fù)雜物體。真實(shí)實(shí)驗(yàn)環(huán)境中，使用搭載在機(jī)器人上的激光雷達(dá)實(shí)時(shí)掃描得到室內(nèi)點(diǎn)云數(shù)據(jù)，首先對(duì)原始的室內(nèi)點(diǎn)云數(shù)據(jù)進(jìn)行加權(quán)高斯濾波改進(jìn)，然后再進(jìn)行角點(diǎn)特征提取。實(shí)驗(yàn)在不同場景下從濾波方法優(yōu)化效果對(duì)比、角點(diǎn)提取結(jié)果和距離測量上，驗(yàn)證本文濾波方法的穩(wěn)定性和準(zhǔn)確性。

2.2 實(shí)驗(yàn)結(jié)果與分析

本文采用matlab2019a進(jìn)行編程并進(jìn)行實(shí)驗(yàn)仿真，對(duì)激光雷達(dá)采集到的點(diǎn)云數(shù)據(jù)利用經(jīng)典的高斯濾波方法和加權(quán)濾波方法對(duì)數(shù)據(jù)進(jìn)行濾波處理。兩種方法的濾波效果對(duì)比如圖5所示。

圖5 兩種濾波方法的點(diǎn)云效果對(duì)比Fig.5 Comparison of point cloud effects between two filtering methods

圖5中藍(lán)色散點(diǎn)為原始點(diǎn)云，紅色線段為原始高斯濾波后點(diǎn)云的平滑效果，黑色線段為加權(quán)高斯濾波后點(diǎn)云的平滑效果。原始點(diǎn)云受到噪聲等影響之后，由雷達(dá)掃描到的物體輪廓會(huì)發(fā)生波動(dòng)，經(jīng)過高斯加權(quán)濾波之后，通過黑色線段和紅色線段對(duì)比的平滑效果可以看出，離散點(diǎn)較小的波動(dòng)也得到了更好的抑制，物體邊緣點(diǎn)云更加平滑。

為了驗(yàn)證本文濾波方法的適用性，在實(shí)驗(yàn)室環(huán)境里，對(duì)另一種場景進(jìn)行濾波和角點(diǎn)提取的測試，圖6為實(shí)驗(yàn)室拍攝的真實(shí)場景圖。將基于傳統(tǒng)高斯的點(diǎn)云濾波結(jié)果、加權(quán)點(diǎn)云濾波結(jié)果與相同區(qū)域內(nèi)原始點(diǎn)云進(jìn)行對(duì)比分析，統(tǒng)計(jì)如圖7所示，圖中藍(lán)色散點(diǎn)為原始點(diǎn)云，黑色線段為加權(quán)高斯濾波之后點(diǎn)云平滑效果。

圖6 實(shí)驗(yàn)環(huán)境及點(diǎn)云分布Fig.6 Experimental environment and point cloud distribution

相對(duì)于雷達(dá)掃描到的原始點(diǎn)云，從不同場景實(shí)驗(yàn)環(huán)境對(duì)比中可得改進(jìn)后點(diǎn)云平滑效果更好，為了驗(yàn)證改進(jìn)的高斯濾波方法的穩(wěn)定性，在本次實(shí)驗(yàn)場景下進(jìn)行角點(diǎn)提取測試。角點(diǎn)檢測如圖8所示。

圖7 濾波效果對(duì)比圖Fig.7 Comparison diagram of filtering effect

圖8 點(diǎn)云角點(diǎn)檢測結(jié)果Fig.8 Point cloud corner detection results

由圖8提取結(jié)果可得，從原始點(diǎn)云數(shù)據(jù)中提取的角點(diǎn)數(shù)量為28個(gè)，經(jīng)過加權(quán)濾波方法處理后，角點(diǎn)提取數(shù)量減少到了23個(gè)，改進(jìn)后的濾波方法產(chǎn)生的數(shù)據(jù)點(diǎn)云檢測出的角點(diǎn)數(shù)比初始點(diǎn)云提取出的角點(diǎn)數(shù)要少。原因是我們需要的主要是特征邊界處的角點(diǎn)，但使用原始點(diǎn)云進(jìn)行角點(diǎn)提取時(shí)，把連續(xù)的數(shù)據(jù)點(diǎn)中一些波動(dòng)幅度較大的點(diǎn)也當(dāng)作Harris角點(diǎn)提取出來。使用加權(quán)濾波的方法后，平滑了雷達(dá)掃描到的數(shù)據(jù)點(diǎn)，因此所能提取到的角點(diǎn)數(shù)就變少了，結(jié)果對(duì)噪聲敏感度降低，相對(duì)較準(zhǔn)確。

圖6(b)中，取一段激光雷達(dá)掃描到的物體邊緣信息直線段“L1-L2”，計(jì)算此時(shí)雷達(dá)掃描中心到物體平面邊緣信息的距離和雷達(dá)掃描中心到初始點(diǎn)云的距離，重復(fù)測量100次，每次距離值統(tǒng)計(jì)結(jié)果如圖9所示。由圖9可得，相比于原始點(diǎn)云的測距結(jié)果，使用本方法處理之后，結(jié)果更加穩(wěn)定，雷達(dá)掃描之后得到的數(shù)據(jù)更加準(zhǔn)確。

圖9 本文方法與原始點(diǎn)云數(shù)據(jù)測距對(duì)比圖Fig.9 Comparison between this method and original point cloud data

圖9中，藍(lán)色線段為使用原始點(diǎn)云得到的距離值，紅色線段為使用本文改進(jìn)濾波方法之后得到的距離值。這兩次實(shí)驗(yàn)中分別統(tǒng)計(jì)測量均值、標(biāo)準(zhǔn)差和平均絕對(duì)誤差(以均值為參考值)，如表2所示。由表2可得，通過本文方法處理點(diǎn)云之后，絕對(duì)平均誤差更小，說明加權(quán)濾波方法有效提升了雷達(dá)掃描準(zhǔn)確性。

綜上可得：由激光雷達(dá)掃描到的原始點(diǎn)云信息，數(shù)據(jù)點(diǎn)離散程度大，且角點(diǎn)檢測時(shí)把一些波動(dòng)幅度較大的點(diǎn)也當(dāng)作Harris角點(diǎn)檢測出；通過傳統(tǒng)高斯濾波方法，邊緣信息數(shù)據(jù)點(diǎn)的離散程度得到了一定的優(yōu)化，但是還存在一定的波動(dòng)；而加權(quán)高斯濾波方法，相對(duì)于傳統(tǒng)濾波方法它使點(diǎn)云數(shù)據(jù)的波動(dòng)再一次得到了抑制，較好的角點(diǎn)檢測結(jié)果驗(yàn)證了改進(jìn)濾波方法的可靠性，點(diǎn)云測距結(jié)果驗(yàn)證了準(zhǔn)確性。因此，加權(quán)濾波方法有效提升了激光雷達(dá)掃描點(diǎn)云濾波過程中的問題，有助于之后激光雷達(dá)局部建圖效果。

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

3 機(jī)器人操作系統(tǒng)的加權(quán)高斯濾波方法實(shí)現(xiàn)

本文在機(jī)器人操作系統(tǒng)(Robot Operating System, ROS)平臺(tái)上，改進(jìn)的高斯濾波方法優(yōu)化了建圖時(shí)接收到的激光雷達(dá)數(shù)據(jù)，實(shí)現(xiàn)了機(jī)器人的增量式地圖構(gòu)建，在構(gòu)建實(shí)驗(yàn)室環(huán)境地圖時(shí)，需要輸入優(yōu)化之后的激光雷達(dá)數(shù)據(jù)信息和機(jī)器人行駛過程中產(chǎn)生的坐標(biāo)信息，啟動(dòng)所需的建圖節(jié)點(diǎn)之后，就會(huì)訂閱這兩個(gè)信息產(chǎn)生的話題，訂閱和發(fā)布的話題如表3所示。

表3中的機(jī)器人坐標(biāo)信息tf樹表示機(jī)器人在實(shí)驗(yàn)室環(huán)境建圖過程中所需要的坐標(biāo)系，它是已經(jīng)定義好的，機(jī)器人在行駛的過程中一定要保證tf坐標(biāo)是連通狀態(tài)，也就是說建圖過程中要訂閱相應(yīng)的tf變換，即訂閱激光雷達(dá)到機(jī)器人底盤坐標(biāo)系、機(jī)器人底盤到里程計(jì)以及里程計(jì)到地圖坐標(biāo)系的tf，由tf static_transform_publisher定期廣播插入到tf樹中，保證tf樹處于連接狀態(tài)，這樣才能完成機(jī)器人在建圖過程中的坐標(biāo)系轉(zhuǎn)換。

表3 建圖訂閱與發(fā)布的話題

雷達(dá)發(fā)布的激光數(shù)據(jù)：本文加權(quán)高斯濾波方法優(yōu)化之后的激光數(shù)據(jù)，是建圖過程中必要的環(huán)境信息，由909個(gè)激光掃描點(diǎn)中有效的數(shù)據(jù)點(diǎn)組成，激光雷達(dá)節(jié)點(diǎn)發(fā)布，建圖節(jié)點(diǎn)訂閱。

地圖信息：全局地圖坐標(biāo)系，消息類型為nav_msgs/OccupancyGrid，由建圖節(jié)點(diǎn)Gmapping發(fā)布。

基于ROS平臺(tái)，本文的實(shí)驗(yàn)場景為電氣樓研究生工作室，傳統(tǒng)的激光雷達(dá)獲取的點(diǎn)云數(shù)據(jù)存在噪聲點(diǎn)問題，噪聲點(diǎn)不處理則會(huì)在機(jī)器人建圖中產(chǎn)生負(fù)面影響，在建圖時(shí)可能會(huì)在沒有物體的柵格上出現(xiàn)障礙物或建圖得到的物體邊緣信息不夠平滑。因此移動(dòng)機(jī)器人需要使用改進(jìn)的激光雷達(dá)點(diǎn)云濾波方法，在實(shí)驗(yàn)室真實(shí)場景下進(jìn)行建圖實(shí)驗(yàn)，最終獲得局部二維柵格地圖。建圖過程如圖10所示。

從實(shí)際建圖效果和二維激光雷達(dá)本身的特性來看，基于Gmapping的建圖方式存在如下幾個(gè)問題：

(1)無法探測到的區(qū)域問題。因?yàn)榧す饫走_(dá)能夠掃描到的平面范圍取決于激光雷達(dá)在機(jī)器人上的安裝高度，圖10(a)的椅子經(jīng)測量最低處平面高度為50 cm，機(jī)器人雷達(dá)掃描高度為16.5 cm，相機(jī)高度為57.3 cm，椅子支撐部分寬度大于機(jī)器人寬度。類似場景中，物體表面在視覺范圍內(nèi)但高于激光雷達(dá)安裝高度，雷達(dá)掃描建圖時(shí)，只能掃描到椅子的支撐部分如圖10(a)中紅框。會(huì)影響機(jī)器人的通行效果使機(jī)器人產(chǎn)生碰撞。

(2)無法探測到玻璃材質(zhì)的門窗。因?yàn)榧す饫走_(dá)的特性，探測到玻璃材質(zhì)物體時(shí)，發(fā)射出的紅外激光無法穿透，不能接收到玻璃上發(fā)出的反射激光，從而導(dǎo)致建圖出現(xiàn)差錯(cuò)。

針對(duì)上述問題，需要結(jié)合視覺來豐富機(jī)器人獲取周圍環(huán)境信息的能力，因此此處只需要獲取局部二維柵格圖即可。

圖10 建圖環(huán)境及結(jié)果Fig.10 Mapping environment and results

4 結(jié)論

本文提出一種以高斯濾波方法為基礎(chǔ)，以角度余弦為加權(quán)值的濾波方法，改進(jìn)后絕對(duì)平均誤差較改進(jìn)前減少到了0.000 2，濾波準(zhǔn)確性更高，且點(diǎn)云數(shù)據(jù)離散程度更加平滑，穩(wěn)定性更高。最后將本文方法應(yīng)用到實(shí)際機(jī)器人操作系統(tǒng)底盤中實(shí)驗(yàn)結(jié)果表明，改進(jìn)后的濾波方法準(zhǔn)確性高，適應(yīng)環(huán)境能力較好，滿足機(jī)器人雷達(dá)點(diǎn)云濾波需求。