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

岳元琛 王東生

（南京理工大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院 南京 210094）

1 引言

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

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

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

2 基于改進(jìn)柵格圖的障礙物檢測

2.1 點(diǎn)云數(shù)據(jù)柵格化

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

柵格法是一種針對點(diǎn)云數(shù)據(jù)進(jìn)行障礙物檢測的一種應(yīng)用廣泛的算法，它可以將3D空間內(nèi)散亂分布的點(diǎn)云數(shù)據(jù)結(jié)構(gòu)化到一個(gè)鳥瞰視角下的2D柵格地圖。該方法主要問題是柵格化處理會(huì)損失一定的空間信息，同時(shí)柵格大小的選擇也將對實(shí)驗(yàn)精度產(chǎn)生較大的影響，因此需要設(shè)置恰當(dāng)?shù)慕y(tǒng)計(jì)量來有效描述柵格內(nèi)點(diǎn)云的空間信息，同時(shí)應(yīng)設(shè)置恰當(dāng)?shù)臇鸥翊笮　?/p>

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

圖1 點(diǎn)云柵格化過程示意圖

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

2.2 基于點(diǎn)云高度分布的柵格分類

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

針對這些問題，本文提出一種統(tǒng)計(jì)柵格內(nèi)點(diǎn)云高度分布的方法進(jìn)行柵格分類。常見的柵格類型可以分為正障礙柵格、懸浮障礙柵格、地面柵格、噪聲柵格等。去除噪聲柵格，并將柵格圖中每一個(gè)障礙物的“占據(jù)”柵格標(biāo)記為對應(yīng)類別，非障礙物的“占據(jù)”柵格標(biāo)記為地面。

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

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

本文使用雙指針法進(jìn)行處理，有效地將懸浮障礙點(diǎn)、地面點(diǎn)、正障礙點(diǎn)區(qū)分，并去除噪聲點(diǎn)。圖2所示在不同柵格類型的條件下運(yùn)用雙指針法的結(jié)果，算法執(zhí)行流程如下：

圖2 柵格內(nèi)點(diǎn)云高度分布規(guī)律

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

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

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

（1）如圖2（b）所示，終止條件下ptr1與ptr2之間點(diǎn)個(gè)數(shù)小于N2，ptr1超過了安全高度，且ptr2對應(yīng)點(diǎn)簇高程差Δh小于Th，此時(shí)標(biāo)記柵格為懸浮障礙柵格。

（2）如圖2（c）所示，終止條件下ptr1與ptr2之間點(diǎn)個(gè)數(shù)小于N2，ptr1超過了安全高度，且ptr2對應(yīng)點(diǎn)簇高程差Δh大于Th，此時(shí)標(biāo)記柵格為正障礙柵格。

（3）如圖2（d）所示，終止條件下，ptr1與ptr2中間點(diǎn)個(gè)數(shù)大于N2，且中間的點(diǎn)在安全高度之下，合并中間的點(diǎn)簇和ptr2對應(yīng)點(diǎn)簇，計(jì)算對應(yīng)的高程差Δh，并標(biāo)記柵格為正障礙柵格。

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

4）重復(fù)1）～3）的過程，直到所有的柵格都被成功標(biāo)記結(jié)束算法流程。

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

3 基于改進(jìn)柵格圖的道路檢測

3.1 候選道邊點(diǎn)搜索

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

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

圖3 自適應(yīng)半圓弧搜索過程

1）以柵格圖底部中心位置（對應(yīng)車體坐標(biāo)系的原點(diǎn)）為圓心起點(diǎn)，作半徑為r的半圓弧，其中左搜索使用左半圓弧，右搜索使用右半圓弧。

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

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

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

5）重復(fù)執(zhí)行以上過程，確定左候選道邊的點(diǎn)。

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

最小二乘法是一種經(jīng)典的數(shù)學(xué)擬合算法，可以通過最小化誤差的平方和來得到數(shù)據(jù)的最佳函數(shù)匹配。在進(jìn)行道路邊界擬合的過程中，期望的目標(biāo)函數(shù)是一條直線，這里選擇使用左右候選道邊點(diǎn)進(jìn)行左右道邊擬合。

設(shè)右候選道邊點(diǎn)集為PR，左候選道邊點(diǎn)集為PL，其中點(diǎn)的坐標(biāo)為車體坐標(biāo)系下的坐標(biāo)。分別進(jìn)行左右道邊的擬合，期望的直線方程為

根據(jù)左（右）候選道邊點(diǎn)的坐標(biāo)和期望直線方程，可以得到矩陣形式的超定方程組：

推導(dǎo)的最小二乘解公式，得到方程組的最小二乘解為

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

3.3 基于改進(jìn)RANSAC算法的道邊擬合

RANSAC即隨機(jī)抽樣一致性算法，是一種能夠從一組包含一定量異常樣本的樣本集中計(jì)算出數(shù)學(xué)模型參數(shù)的方法，可以有效解決異常數(shù)據(jù)對模型的影響。RANSAC算法假設(shè)數(shù)據(jù)集中既有正確的樣本集，稱作內(nèi)點(diǎn)集，也有異常的樣本的樣本集，稱作外點(diǎn)集。內(nèi)點(diǎn)集中的樣本數(shù)據(jù)與擬合模型偏差較小，可以被模型有效描述；外點(diǎn)集中的樣本數(shù)據(jù)與擬合模型偏差較大。同時(shí)RANSAC算法還假設(shè)，給定一組正確的數(shù)據(jù)，存在一種模型構(gòu)建方法可以有效描述這組數(shù)據(jù)?；镜腞ANSAC算法流程為

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

2）從樣本集P中隨機(jī)抽取m個(gè)樣本構(gòu)成最小抽樣集S，并使用S中的樣本進(jìn)行模型參數(shù)計(jì)算，得到擬合模型M。

3）計(jì)算樣本集中其他的數(shù)據(jù)與擬合模型M之間的誤差，當(dāng)樣本的誤差小于閾值δ的時(shí)候，將該樣本加入到S中構(gòu)成新的內(nèi)點(diǎn)集S*。

4）迭代執(zhí)行2）、3）過程K次，選擇樣本數(shù)最多的內(nèi)點(diǎn)集S*所對應(yīng)的模型M作為最優(yōu)的模型，并根據(jù)內(nèi)點(diǎn)集S*重新計(jì)算模型，得到最終的擬合模型M*。

在道邊擬合的使用場景下考慮使用直線模型來描述候選道邊點(diǎn)，執(zhí)行過程如圖4所示，圖中展示了兩次迭代過程的內(nèi)點(diǎn)和外點(diǎn)分布情況。在迭代過程結(jié)束，得到最優(yōu)擬合結(jié)果之后，使用最小二乘法重新計(jì)算模型參數(shù)，得到最終的模型。

圖4 RANSAC算法道邊擬合過程

實(shí)際運(yùn)用過程中，通常設(shè)定固定大小的m，此時(shí)在保持置信概率p一定的情況下，可以適當(dāng)提高內(nèi)點(diǎn)率w來降低迭代次數(shù)K；通過增加迭代次數(shù)K來提高置信概率p。

在道邊擬合的使用場景下，對RANSAC算法進(jìn)行三種優(yōu)化：

1）在得到最終的擬合模型M*之后，重新計(jì)算內(nèi)點(diǎn)集并進(jìn)行模型擬合，迭代執(zhí)行該過程直到達(dá)到指定的迭代次數(shù)N或者沒有新的樣本加入內(nèi)點(diǎn)集的時(shí)候停止迭代過程。

2）可以預(yù)設(shè)內(nèi)點(diǎn)率閾值w，當(dāng)一次迭代過程中內(nèi)點(diǎn)率達(dá)到了預(yù)設(shè)閾值則提前結(jié)束迭代過程。

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

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

4.1 主要參數(shù)設(shè)定

實(shí)驗(yàn)過程中選用Velodyne 32線激光雷達(dá)在校園內(nèi)進(jìn)行實(shí)驗(yàn)驗(yàn)證，根據(jù)該雷達(dá)的屬性和實(shí)際功能需求，按照表1設(shè)定參數(shù)。

表1 主要參數(shù)設(shè)定

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

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

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

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

圖6 算法運(yùn)行時(shí)間分析

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

5 結(jié)語

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