基于改進柵格圖的道路和障礙物檢測算法研究*

岳元琛 王東生

（南京理工大學計算機科學與工程學院 南京 210094）

1 引言

近年來，傳感器和人工智能相關技術高速發展，無人駕駛技術作為人工智能的一個重要應用也成為了熱門的研究內容。其中，高精度實時的環境感知是無人車進行有效路徑規劃和控制的重要前提。多線激光雷達作為一種重要的車載設備，是無人車進行環境感知任務中最重要傳感器之一，可以直接得到周圍環境的三維點云信息，同時可以很好地適應夜間、雨天等較復雜條件下的行駛任務。因此，多線激光雷達點云數據相關的研究工作具有重要的研究價值和應用前景。

點云數據最常見的處理方法主要分為直接對無序點云的處理［1～3］、體素化處理［4］、多視角圖像［5～6］、柵格化［7～12］。柵格化處理是將原始點云數據投影到一個鳥瞰視角下2D柵格圖中，簡單而高效，在無人駕駛系統中應用廣泛。基于高度差的柵格圖［9］存在很多問題，無法正確區分懸浮障礙物和正障礙物，同時極易受到噪聲點的影響。針對這一問題，本文提出一種改進的柵格圖構建方法，通過統計柵格內點云高度分布的方法進行柵格分類，有效區分噪聲點、懸浮障礙物點、正障礙點和地面點，同時通過膨脹處理得到緊密相連的占據柵格。

道路檢測過程中，通常采用先確定地面點再進行道邊點檢測。地面點提取過程中使用線段分割的方法進行提取，線段分割方法主要有PDBS［13］、SEF［14］、LT［15］和IEPF［16］等。這些方法直接對原始點云進行處理，閾值控制比較困難，并且計算量較大難以滿足實時性需求。針對這些問題，本文采用直接從柵格圖中搜索候選道邊的方法，使用自適應半圓弧搜索進行道邊候選點的搜索，同時采用改進RANSAC算法進行道邊擬合，得到最終的道路檢測結果。算法簡單高效，提高了檢測精度并滿足實時性需求。

2 基于改進柵格圖的障礙物檢測

2.1 點云數據柵格化

原始點云數據具有海量性、稀疏性、無序性等特點，在實際使用的過程中會存在很多困難。常見的Velodyne32線激光雷達每一幀數據量可以達到50K，在處理和數據傳輸的過程中有很大的時間和內存開銷；點云數據具有稀疏性，不同于圖像中緊密連接的像素點，點云數據散落在三維空間中，并沒有固定的分布區域并且密度也不相同；點云數據具有無序性，一般的圖像都可以表示為一個C×W×H的3維矩陣，但是點云數據無法直接使用一個結構化矩陣來表示。

柵格法是一種針對點云數據進行障礙物檢測的一種應用廣泛的算法，它可以將3D空間內散亂分布的點云數據結構化到一個鳥瞰視角下的2D柵格地圖。該方法主要問題是柵格化處理會損失一定的空間信息，同時柵格大小的選擇也將對實驗精度產生較大的影響，因此需要設置恰當的統計量來有效描述柵格內點云的空間信息，同時應設置恰當的柵格大小。

本文改進了經典的基于高程差的柵格圖［8～9］，使用了一種新穎的柵格化處理方法。如圖1所示，首先設置一個大小為W×H的柵格地圖，柵格大小為g×g，將該柵格圖看作一個二維空間內的哈希表，Δy代表坐標偏移量，點云坐標(x,y)與柵格坐標(w,h)的映射關系滿足哈希函數：

圖1 點云柵格化過程示意圖

映射過程中，柵格圖范圍外的點將去除掉不用于檢測任務中。設置每個柵格內最少點個數閾值Tmin，當柵格內點的個數Ng，Ng＜Tmin時認為柵格是一個未被占據的柵格，設置最小點閾值可以有效避免一些空間內孤立的噪聲點對實驗結果的影響，Ng≥Tmin認為是一個占據的柵格。由于三維空間內點云分布不均勻，同時柵格內所有的點是無序的，不需要進行隨機讀取，因此使用鏈表來記錄同一個柵格內點的信息，這樣可以極大提高存儲效率。

2.2 基于點云高度分布的柵格分類

經典的基于高程差的柵格化處理方法存在很多的問題，只統計柵格內點的最大高度和最小高度，會忽略很多空間分布信息，無法實現柵格的有效分類；少量具有較大高度的噪聲點會影響高程差的統計，造成柵格的誤標記，因而魯棒性不夠高；無法有效區分懸浮障礙物和普通正障礙物，基于高程差的柵格圖會將懸浮障礙物當作普通障礙物，在一些城市道路的場景中，伸入路面的樹枝等懸浮物會被誤檢為障礙物導致道路可通行區域被封死，影響無人車的行駛。

針對這些問題，本文提出一種統計柵格內點云高度分布的方法進行柵格分類。常見的柵格類型可以分為正障礙柵格、懸浮障礙柵格、地面柵格、噪聲柵格等。去除噪聲柵格，并將柵格圖中每一個障礙物的“占據”柵格標記為對應類別，非障礙物的“占據”柵格標記為地面。

柵格內點云的高度分布滿足一定的分布規律。將點云根據高度信息進行聚類后，噪聲點簇滿足點簇中點的個數少；地面點簇滿足點簇內高程差很小并且高度均值小于一定閾值；懸浮障礙點簇滿足點數大于一定閾值，并且最低高度要高于安全高度閾值；正障礙點簇滿足點的個數大一定的閾值，并且高程差也大于一定的閾值。

根據高度分布的規律，考慮可能的兩種解決方法，K-means算法和雙指針法。K-means算法是一種經典的聚類算法，應用在此場景下可以有效實現指定數量點云簇的聚類，進而將懸浮點、正障礙點、地面點等區分開，但是由于柵格類型的多樣性，每個柵格內的點的數量以及高度分布情況不同，很難選擇適合的K值，同時在計算過程中需要進行多次迭代過程，將產生較大的時間開銷；雙指針法實現原理簡單，時間開銷較小，不需要設置K值，可以很好適應此場景下多種類型柵格的點云分布情況，同時也可以很好應對不同柵格內點云數量差距太大的情況。

本文使用雙指針法進行處理，有效地將懸浮障礙點、地面點、正障礙點區分，并去除噪聲點。圖2所示在不同柵格類型的條件下運用雙指針法的結果，算法執行流程如下：

圖2 柵格內點云高度分布規律

1）輸入柵格內的按照高度排序后的點云集P，初始化高指針ptr1為點云最大高度，低指針ptr2為點云最小高度，并設置聚類閾值T1，最小間隔閾值T2，點簇最少點個數閾值N1，間隔內最大點個數N2，障礙物高程差閾值Th。

2）高指針ptr1與相鄰點高度差小于T1時，指針下移；低指針ptr2與相鄰點高度差小于T1時，指針上移。直到滿足終止條件，ptr1與ptr2相遇時或者兩個指針與相鄰的點高度差大于T1，無法繼續移動。其中一個點簇中點的個數小于N1，如圖2（a）所示，則將這個點簇視作噪聲點簇，去除該點簇內的點，重新執行2）。

3）雙指針終止移動后，可能的結果如圖2所示，根據高低指針的位置，各種情況處理方法如下：

（1）如圖2（b）所示，終止條件下ptr1與ptr2之間點個數小于N2，ptr1超過了安全高度，且ptr2對應點簇高程差Δh小于Th，此時標記柵格為懸浮障礙柵格。

（2）如圖2（c）所示，終止條件下ptr1與ptr2之間點個數小于N2，ptr1超過了安全高度，且ptr2對應點簇高程差Δh大于Th，此時標記柵格為正障礙柵格。

（3）如圖2（d）所示，終止條件下，ptr1與ptr2中間點個數大于N2，且中間的點在安全高度之下，合并中間的點簇和ptr2對應點簇，計算對應的高程差Δh，并標記柵格為正障礙柵格。

（4）如圖2（e）所示，如果ptr1與ptr2相遇，則柵格內的點視作一個整體，高程差Δh大于Th標記柵格為正障礙柵格，如圖2（f）所示，高程差Δh小于Th標記為地面柵格。

4）重復1）～3）的過程，直到所有的柵格都被成功標記結束算法流程。

得到的原始柵格圖存在一定的孤立性，由于多線激光雷達本身具有一定稀疏性，并且檢測出來的障礙物柵格也會存在一定漏檢或者斷裂的情況，很多的柵格可能沒有成為一個連貫的整體，這將不利于后續的處理過程。此時，考慮對柵格進行膨脹處理，選用四近鄰膨脹模板。注意不可重復膨脹。在膨脹過程中，出于安全性的考慮，設置優先級依次為正障礙柵格、懸浮障礙柵格、地面柵格，在膨脹過程中，首先將所有的正障礙柵格進行膨脹，膨脹過程中如果高優先級柵格近鄰存在低優先級的柵格，即膨脹過程中近鄰遇到懸浮障礙柵格和地面柵格，將使用正障礙柵格替換對應柵格。緊接著分別膨脹懸浮障礙柵格和地面柵格，得到最終的柵格圖。

3 基于改進柵格圖的道路檢測

3.1 候選道邊點搜索

道路邊界點提取主要可以分為兩大類方法：使用雷達掃描線上的點進行線段分割，進而區分路面點和候選道路邊界點；直接從柵格圖中搜索出候選道邊點。

本文采用一種新穎的自適應半圓弧搜索法，可以直接運用之前得到的改進柵格圖來搜索候選道標，不用保留多余的雷達掃描線信息。如圖3所示，算法流程如下：

圖3 自適應半圓弧搜索過程

1）以柵格圖底部中心位置（對應車體坐標系的原點）為圓心起點，作半徑為r的半圓弧，其中左搜索使用左半圓弧，右搜索使用右半圓弧。

2）右搜索過程中，從90°～-90°間隔為Δθ依次進行檢驗圓弧所在柵格的占據情況，如果所在柵格為正障礙柵格，則記錄下圓心位置并將此柵格作為右道邊候選柵格，結束本次行搜索過程；如果直到-90°位置仍沒有搜索到正障礙柵格，那么將圓心向右平移Δw的距離再重新進行搜索；如果一直到柵格圖邊界仍沒有搜索到，認為本次行搜索沒有發現候選道邊點，圓心點重置為起始點，并結束本次行搜索過程。

3）行搜索結束后，根據上一次搜索的圓心位置，向上平移Δh的距離，并結合右道邊點的橫向變化趨勢做一次橫向偏移，進而確定新的圓心起始位置，再次執行2）中的行搜索過程。

4）重復執行2）、3）直到搜索點到搜索起始點的縱向距離超過閾值H，停止搜索過程，此時得到了完整的右候選道邊點。

5）重復執行以上過程，確定左候選道邊的點。

3.2 基于最小二乘法的直線擬合

最小二乘法是一種經典的數學擬合算法，可以通過最小化誤差的平方和來得到數據的最佳函數匹配。在進行道路邊界擬合的過程中，期望的目標函數是一條直線，這里選擇使用左右候選道邊點進行左右道邊擬合。

設右候選道邊點集為PR，左候選道邊點集為PL，其中點的坐標為車體坐標系下的坐標。分別進行左右道邊的擬合，期望的直線方程為

根據左（右）候選道邊點的坐標和期望直線方程，可以得到矩陣形式的超定方程組：

推導的最小二乘解公式，得到方程組的最小二乘解為

在實際算法執行過程中，以左（右）候選道邊點作為輸入，分別帶入到上述矩陣方程中，得到左（右）道邊模型的各個參數值。同時注意到，為了滿足超定方程組的前置條件，用于擬合的候選道邊點個數應該大于3，點的個數過少將停止擬合過程。

3.3 基于改進RANSAC算法的道邊擬合

RANSAC即隨機抽樣一致性算法，是一種能夠從一組包含一定量異常樣本的樣本集中計算出數學模型參數的方法，可以有效解決異常數據對模型的影響。RANSAC算法假設數據集中既有正確的樣本集，稱作內點集，也有異常的樣本的樣本集，稱作外點集。內點集中的樣本數據與擬合模型偏差較小，可以被模型有效描述；外點集中的樣本數據與擬合模型偏差較大。同時RANSAC算法還假設，給定一組正確的數據，存在一種模型構建方法可以有效描述這組數據。基本的RANSAC算法流程為

1）輸入一個樣本集P，并定義最小抽樣集S，S中樣本的數目m為模型參數計算所需的最小樣本數，樣本集P中的樣本數應該大于m。設置算法迭代次數K以及誤差閾值δ。

2）從樣本集P中隨機抽取m個樣本構成最小抽樣集S，并使用S中的樣本進行模型參數計算，得到擬合模型M。

3）計算樣本集中其他的數據與擬合模型M之間的誤差，當樣本的誤差小于閾值δ的時候，將該樣本加入到S中構成新的內點集S*。

4）迭代執行2）、3）過程K次，選擇樣本數最多的內點集S*所對應的模型M作為最優的模型，并根據內點集S*重新計算模型，得到最終的擬合模型M*。

在道邊擬合的使用場景下考慮使用直線模型來描述候選道邊點，執行過程如圖4所示，圖中展示了兩次迭代過程的內點和外點分布情況。在迭代過程結束，得到最優擬合結果之后，使用最小二乘法重新計算模型參數，得到最終的模型。

圖4 RANSAC算法道邊擬合過程

實際運用過程中，通常設定固定大小的m，此時在保持置信概率p一定的情況下，可以適當提高內點率w來降低迭代次數K；通過增加迭代次數K來提高置信概率p。

在道邊擬合的使用場景下，對RANSAC算法進行三種優化：

1）在得到最終的擬合模型M*之后，重新計算內點集并進行模型擬合，迭代執行該過程直到達到指定的迭代次數N或者沒有新的樣本加入內點集的時候停止迭代過程。

2）可以預設內點率閾值w，當一次迭代過程中內點率達到了預設閾值則提前結束迭代過程。

3）在抽取最小抽樣集S的過程中，結合一定的先驗信息來確定實際的抽取方案，代替完全隨機的選擇，提高算法效率。實際的道路邊界點通常是高度較為一致的點，在執行算法流程之前可以預先將候選道邊點按高度進行歸類，抽取的過程中優先抽取高度較為一致的道邊點作為最小抽樣集。

4 實驗結果與分析

4.1 主要參數設定

實驗過程中選用Velodyne 32線激光雷達在校園內進行實驗驗證，根據該雷達的屬性和實際功能需求，按照表1設定參數。

表1 主要參數設定

4.2 實驗結果展示與分析

圖5（a）中，無人車直線行駛過程中，伸入路面的樹枝被成功檢測為懸浮障礙，同時道邊檢測正常，不會影響車輛的正常行駛。圖5（b）中，在行駛過程中道路右側遇到了一輛車，成功將其檢測為正障礙，同時也成功檢測出了懸浮障礙物。圖5（c）中，道路左側存在一定的噪聲點，成功將噪聲濾除，沒有影響道邊擬合的結果。圖5（d）中，在岔路口轉彎的過程中，也能夠擬合出道邊。

圖5 校園道路典型場景中障礙物和道路檢測結果

實驗過程中，總共采集了2000幀的數據，算法運行時間如圖6所示，平均算法運行時間為25.3ms，達到了無人駕駛場景下實時性的需求。在實驗過程中，在所有的場景下成功檢測出來了全部正障礙物并成功消除了噪聲點的影響，同時在99.5%的場景下成功檢測出了全部懸浮障礙，證明了本算法實現了改進柵格的魯棒性需求。

圖6 算法運行時間分析

在道邊擬合過程中，在直線行駛且道邊清晰的場景下都可以正確檢測出道邊，但是在交叉路口以及道邊不明顯的道路中檢測效果不佳。

5 結語

本文針對無人駕駛中多線激光雷達障礙物和道路檢測過程中遇到的一些問題，提出的基于改進柵格圖的障礙物和道路檢測算法。運用基于點云高度分布的柵格分類方法，有效解決了經典的基于高程差的柵格圖無法避免噪聲點影響以及無法有效區分正障礙物和懸浮障礙物的缺陷，建立了更加魯棒的柵格圖。同時針對道邊檢測過程中，精度和實時性不足的問題，提出了直接在柵格圖內進行候選道邊點搜索并運用改進RANSAC算法進行道邊擬合的方法，有效提高了道邊檢測的精度和效率。最終在實驗結果中，證明了算法的有效性。在未來的研究過程中，將考慮進一步提高算法的性能，并考慮實現對行人、車輛等目標實現識別。