一種改進的LiDAR點云濾波方法

馬小陸，甄文雪，鞏朝光

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

激光雷達(LiDAR)因其測距精度高、實時掃描速度快、穩定性等特點，現被廣泛應用在機器人等領域中，激光雷達獲取的點云信息反映的是其與周圍物體之間的距離信息。因此對原始點云數據的處理非常重要，它是獲取高精度點云數據信息的基礎[1]。

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

綜上可以看出，當前的點云濾波方法存在著穩定性差、準確度下降等問題。本文結合傳統高斯濾波的思想，提出一種加權的LiDAR點云濾波方法。該方法在雷達掃描點云數據過程中，根據數據點之間夾角的余弦值設置合理權重，從而較好地克服了上述問題。

1 改進的雷達點云濾波方法

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

圖1 激光雷達結構示意圖Fig.1 Structural diagram of lidar

文中配置的激光雷達角度增量為0.003 456 rad，激光雷達的掃描范圍是[-π/2, π/2]，每一幀掃描可獲取909個激光掃描點，且每一個角度的激光掃描點上都有一個掃描距離與之對應。這些掃描數據點以極坐標形式返回測量數據，極坐標表示為

(1)

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

本文方法的測量數據是在笛卡爾坐標系下得到的，兩者的轉換關系如下。

(2)

式中，xn,yn為笛卡爾坐標系下雷達掃描點的坐標，文中n取909。

1.1 高斯濾波

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

高斯濾波本質上是一種通過正態分布確定權重的線性平滑濾波，能夠有效地抑制噪聲，平滑數據，其實高斯濾波就是對LiDAR 掃描到的每一個點云數據進行加權平均之后再進行卷積的過程。傳統的高斯濾波模板W定義為

(3)

式中f(x)定義為

(4)

高斯濾波結果表達式為

Ifiltered=W·I(xi)

(5)

掃描窗口的寬度定義為

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

(6)

式(5)與式(6)中，Ifiltered為當前濾波掃描點的距離值，x為當前窗口的寬度，d為窗口的半徑，xi為窗口內排除極大值位置和無效點位置數據之后雷達掃描到的數據點位置，I(xi)為每個數據點對應的距離值。

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

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

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

對雷達掃描到的有效數據點在文中規定的窗口范圍內重復上述過程，得到傳統高斯濾波方法之后的掃描點數據，最后通過濾波得到的真實掃描數據結果如圖2所示。

圖2 點云高斯濾波Fig.2 Point cloud Gaussian filtering

圖2中，藍色點為激光雷達掃描到的室內原始數據，紅色點為通過傳統濾波之后得到的點云數據。通過濾波結果可以發現，初次濾波之后，離散程度較大的噪聲點得到了一定程度的平滑處理，但是平滑效果還沒有達到預期結果，需要進一步濾波。本文把傳統的高斯濾波思想作為核心，提出一種新的濾波方法。

1.2 加權高斯濾波

根據傳統的高斯濾波思想，為了調整窗口中心數據點的影響力，本文采用對雷達掃描到的點云進行加權濾波的處理方式，即對點云采用在傳統高斯濾波的基礎上再加一次權重的方法，減小雷達掃描到物體輪廓的波動，提高濾波方法準確性。雷達點云濾波方法流程如圖3。

圖3 點云濾波方法流程Fig.3 Gaussian filtering method flow

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

1.2.1 權值選取方法

由于在LiDAR 掃描到點云數據發生突變的數據點，數據特征變化隨之較大，相鄰數據點之間的夾角變化劇烈；相反的，在平滑區域的數據點，其數據特征變化比較平緩，所對應的數據點之間的夾角變化幅度較小。因此從相鄰數據點間的夾角入手進行權值的選取是正確合理的。

由正態分布曲線可知，越接近中心取值越大，越遠離中心取值越小。當數據點與相鄰點之間的夾角在0°～90°范圍時，夾角的余弦值是單調遞減函數，即余弦值隨著角度的增大而減小，從而使窗口中心位置數據點所占權重減??；當數據點與相鄰點之間的夾角在90°～180°范圍時，夾角余弦值的絕對值是單調遞增函數，即余弦值的絕對值隨著角度的增大而增大，窗口中心位置的數據點所占的權重就越大。

因此依據權重值參照正態分布選取的原則，并結合夾角余弦函數分布規律，采用窗口內相鄰數據點之間夾角余弦值來作為濾波時窗口中心數據點的權值是合理的。

1.2.2 點云數據的權值計算

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

(7)

式(7)中x,y分別為極坐標系下的雷達掃描數據點對應直角坐標系下的x,y軸上的坐標。兩段線段之間的夾角為

(8)

方法中加權高斯模板定為

(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)

將改進后的窗口中間位置的數據點定義為

Ifiltered=W·I(xi)

(14)

式中，σ=3、d=1、a=-0.5、b=-0.5、c=-1，I(xi)是半徑為d的窗口內每個數據點位置分別對應的距離值，Ifiltered是改進濾波方法之后的窗口中間位置的數據點對應的距離值。

根據上述公式可以知道，改進后的高斯濾波方法根據窗口內鄰近區域點云的夾角余弦值來確定濾波之后點云位置的優化。因此，通過測試多次數值調整之后，由a，b，c的值對窗口內每個數據點的權重值進行了更加合理的分配，在對含有較小波動點云邊緣信息的離散噪聲進行濾波時盡可能使邊緣信息得到平滑，提高了高斯濾波的準確性。

1.3 Harris角點檢測

上文所述高斯濾波方法表明，掃描到的室內環境不平滑信息得到了一定程度的改進，為驗證方法的可靠性，使用Harris角點檢測的方法。Harris角點檢測就是尋找掃描點中的極大值點來確定角點，具有一定的穩定性。Harris角點檢測算法實現的步驟如下：

以單線激光雷達掃描到的點云數據作為輸入值，計算掃描到的數據點u在X和Y方向兩個上的梯度ux,uy：

(15)

計算數據點在兩個方向上梯度的乘積：

(16)

(17)

式中w為高斯加權函數。圖像u(x,y)在點(x,y)處平移(△x,△y)后的自相關函數可以近似為

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

(18)

將自相關函數表示為橢圓方程：

(19)

式(19)中計算每一個數據點的矩陣M：

(20)

計算每個數據點的Harris角點響應值：

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

(21)

式中α為經驗常數，文中α=0.06。每個數據點的角點響應值首先要大于閾值TR，計算公式為

TR=pr·Rmax

(22)

式中pr為角點響應比例系數，取pr=0.000 2，Rmax為遍歷一幀點云后得到的最大角點響應值。

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

2 實驗結果與分析

2.1 實驗數據采集

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

表1 實驗軟硬件參數配置

圖4 實驗環境Fig.4 Experimental environment

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

2.2 實驗結果與分析

本文采用matlab2019a進行編程并進行實驗仿真，對激光雷達采集到的點云數據利用經典的高斯濾波方法和加權濾波方法對數據進行濾波處理。兩種方法的濾波效果對比如圖5所示。

圖5 兩種濾波方法的點云效果對比Fig.5 Comparison of point cloud effects between two filtering methods

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

為了驗證本文濾波方法的適用性，在實驗室環境里，對另一種場景進行濾波和角點提取的測試，圖6為實驗室拍攝的真實場景圖。將基于傳統高斯的點云濾波結果、加權點云濾波結果與相同區域內原始點云進行對比分析，統計如圖7所示，圖中藍色散點為原始點云，黑色線段為加權高斯濾波之后點云平滑效果。

圖6 實驗環境及點云分布Fig.6 Experimental environment and point cloud distribution

相對于雷達掃描到的原始點云，從不同場景實驗環境對比中可得改進后點云平滑效果更好，為了驗證改進的高斯濾波方法的穩定性，在本次實驗場景下進行角點提取測試。角點檢測如圖8所示。

圖7 濾波效果對比圖Fig.7 Comparison diagram of filtering effect

圖8 點云角點檢測結果Fig.8 Point cloud corner detection results

由圖8提取結果可得，從原始點云數據中提取的角點數量為28個，經過加權濾波方法處理后，角點提取數量減少到了23個，改進后的濾波方法產生的數據點云檢測出的角點數比初始點云提取出的角點數要少。原因是我們需要的主要是特征邊界處的角點，但使用原始點云進行角點提取時，把連續的數據點中一些波動幅度較大的點也當作Harris角點提取出來。使用加權濾波的方法后，平滑了雷達掃描到的數據點，因此所能提取到的角點數就變少了，結果對噪聲敏感度降低，相對較準確。

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

圖9 本文方法與原始點云數據測距對比圖Fig.9 Comparison between this method and original point cloud data

圖9中，藍色線段為使用原始點云得到的距離值，紅色線段為使用本文改進濾波方法之后得到的距離值。這兩次實驗中分別統計測量均值、標準差和平均絕對誤差(以均值為參考值)，如表2所示。由表2可得，通過本文方法處理點云之后，絕對平均誤差更小，說明加權濾波方法有效提升了雷達掃描準確性。

綜上可得：由激光雷達掃描到的原始點云信息，數據點離散程度大，且角點檢測時把一些波動幅度較大的點也當作Harris角點檢測出；通過傳統高斯濾波方法，邊緣信息數據點的離散程度得到了一定的優化，但是還存在一定的波動；而加權高斯濾波方法，相對于傳統濾波方法它使點云數據的波動再一次得到了抑制，較好的角點檢測結果驗證了改進濾波方法的可靠性，點云測距結果驗證了準確性。因此，加權濾波方法有效提升了激光雷達掃描點云濾波過程中的問題，有助于之后激光雷達局部建圖效果。

表2 點云測距結果統計

3 機器人操作系統的加權高斯濾波方法實現

本文在機器人操作系統(Robot Operating System, ROS)平臺上，改進的高斯濾波方法優化了建圖時接收到的激光雷達數據，實現了機器人的增量式地圖構建，在構建實驗室環境地圖時，需要輸入優化之后的激光雷達數據信息和機器人行駛過程中產生的坐標信息，啟動所需的建圖節點之后，就會訂閱這兩個信息產生的話題，訂閱和發布的話題如表3所示。

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

表3 建圖訂閱與發布的話題

雷達發布的激光數據：本文加權高斯濾波方法優化之后的激光數據，是建圖過程中必要的環境信息，由909個激光掃描點中有效的數據點組成，激光雷達節點發布，建圖節點訂閱。

地圖信息：全局地圖坐標系，消息類型為nav_msgs/OccupancyGrid，由建圖節點Gmapping發布。

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

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

(1)無法探測到的區域問題。因為激光雷達能夠掃描到的平面范圍取決于激光雷達在機器人上的安裝高度，圖10(a)的椅子經測量最低處平面高度為50 cm，機器人雷達掃描高度為16.5 cm，相機高度為57.3 cm，椅子支撐部分寬度大于機器人寬度。類似場景中，物體表面在視覺范圍內但高于激光雷達安裝高度，雷達掃描建圖時，只能掃描到椅子的支撐部分如圖10(a)中紅框。會影響機器人的通行效果使機器人產生碰撞。

(2)無法探測到玻璃材質的門窗。因為激光雷達的特性，探測到玻璃材質物體時，發射出的紅外激光無法穿透，不能接收到玻璃上發出的反射激光，從而導致建圖出現差錯。

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

圖10 建圖環境及結果Fig.10 Mapping environment and results

4 結論

本文提出一種以高斯濾波方法為基礎，以角度余弦為加權值的濾波方法，改進后絕對平均誤差較改進前減少到了0.000 2，濾波準確性更高，且點云數據離散程度更加平滑，穩定性更高。最后將本文方法應用到實際機器人操作系統底盤中實驗結果表明，改進后的濾波方法準確性高，適應環境能力較好，滿足機器人雷達點云濾波需求。