本發明涉及機器人避障檢測方法，特別是涉及面向無人車導航的一種基于激光雷達點云局部結構特征的負障礙物檢測方法。

背景技術：

傳統的用于負障礙物檢測的方法有基于熱紅外圖像、基于彩色圖像和基于雙目視覺的檢測方法。基于熱紅外圖像的負障礙物檢測方法使用熱紅外相機采集圖像，根據負障礙物與環境溫度存在溫差的特點檢測環境中的負障礙物。這種檢測方法的缺點是對環境溫度變化十分敏感。基于彩色圖像的負障礙物檢測方法使用彩色相機采集圖像，在圖像序列中提取顏色模型和幾何模型檢測負障礙物，這種方法受光照條件影響強烈。基于雙目視覺的方法使用雙目圖像并利用種子生長法檢測負障礙物，這種方法同樣受到光照條件影響。除了受到光照條件影響，傳統使用單相機圖像的負障礙物檢測方法無法確定負障礙物的實際距離，使用雙目視覺的負障礙物檢測方法雖然能計算距離，但也存在精度不高的缺點。

為解決使用傳統圖像檢測負障礙物易受到光照條件影響以及無法準確定位的問題，在無人車導航領域一類方法是使用三維激光雷達作為導航傳感器。

三維激光雷達能夠獲取環境的點云數據，探測范圍廣，精度高，且不受光照條件影響，能夠精確描述環境結構信息。

目前基于激光雷達檢測負障礙物有基于多幀融合的檢測方法和基于SVM分類的方法。

多幀融合的負障礙物檢測方法通過融合多幀點云得到高密度局部地圖，進而通過遮擋關系確定負障礙物位置。這種方法對靜態環境中的負障礙檢測效果較好，但計算復雜度高且不適用于環境中存在大量運動物體的情況。

基于SVM的負障礙物檢測方法通過計算點云的局部特征，例如高度-長度-密度特征，再通過大量樣本訓練得到負障礙物分類器，進而用于對候選負障礙物點云的分類與位置檢測。這種方法缺點在于需要預先進行大量樣本訓練，并且對未訓練過的負障礙物種類的檢測效果較差。

技術實現要素：

在無人車越野行駛環境中，存在溝壑、坑洞等負障礙物，本發明的目的在于提供了一種基于激光雷達點云局部結構特征的負障礙物檢測方法，即利用垂直掃描激光雷達點云的局部結構特征提取負障礙物候選點對，進而聚類形成負障礙物候選區域，最后進行過濾得到負障礙物結果。

本發明所述的負障礙物檢測方法采取先在單線激光點云中提取候選點對，再將多線激光提取的候選點對集合進行聚類的方案。首先是計算了點云中負障礙物的三類結構特征，即局部點云距離跳變、局部點云分布密集和局部點云高度下降。根據三類結構特征提取出負障礙物候選點對，通過空間位置一致性和點對長度一致性聚類得到負障礙物候選區域，通過面積過濾、點對數量過濾得到負障礙物結果區域。

本發明采用的具體技術方案包括如下步驟：

1)已采集的激光雷達點云數據，針對單線激光點云，逐線檢測局部點云距離跳變、局部點云分布密集和局部點云高度下降的三個結構特征，依據三個結構特征在單線激光點云中提取篩選獲得可能屬于負障礙物的候選點對；

2)由各線激光的激光點云獲得的所有候選點對，依據點對長度一致性和空間位置一致性進行聚類，得到一系列負障礙物候選區域，再經過面積過濾得到負障礙物區域。

本發明的負障礙物一般指的是地面上的凹坑、壕溝等障礙物。

本發明涉及的點云數據由激光雷達采集得到，激光雷達為多線激光旋轉掃描的工作方式，一根激光線對應一段連續分布的點云。

所述步驟1)具體是針對單線激光點云，采用以下方式進行處理：

1.1)逐激光點計算鄰近點間距與局部平滑度，并根據局部平滑度判斷獲得平滑點；

1.2)逐激光點通過檢測局部點云距離跳變特征選取鄰近點間距局部極大值的候選點對，作為初始候選點對；

1.3)在初始候選點對中，通過檢測局部點云分布密集特征選取初始候選點對中后沿點所在位置點云局部密集的點對，獲得篩選后的初始候選點對；

1.4)通過局部點云高度下降特征進行進一步篩選，獲得最終的候選點對。

經過步驟1.4)得到的點對即是滿足負障礙物三個局部結構特征，即鄰接點距離跳變、后沿點密集分布、后沿點高度下降的候選點對集合。

所述步驟1.1)具體為：

1.1.1)計算鄰近點間距

針對單線激光點云，采用以下公式計算每個激光點與其鄰近激光點之間的空間距離，激光點的坐標為鄰近激光點的坐標為

其中，表示激光點及其鄰接點之間的鄰近點間距，l表示單線激光點云中的激光點序號；

1.1.2)計算局部平滑度

除鄰近點間距外，還需要計算單線激光點云的局部平滑度。對于單線激光而言，其掃描方式為平面掃描，因而單線激光掃描局部地形所形成的點云可看做該掃描平面與地面的交線。在平坦的地面上，該交線呈直線分布，在地形起伏劇烈的地面上，該交線則呈現出蜿蜒曲折的折線形狀。基于此，采用以下公式計算單線激光點云中每個激光點的局部平滑度s_l：

式中，N_neighbor表示預設的鄰近點個數，一般取n＝5，l表示單線激光點云中的激光點序號，k表示該激光點的鄰近點的序號；

上述的s_l大小體現了該激光點局部區域的平滑度，s_l值非常小表明該激光點處于平滑的近似平面上，s_l值非常大表明該激光點所處的位置地形起伏劇烈。

預設平滑閾值Ths_smooth和非平滑閾值Ths_sharp，通過將激光點的局部平滑度s_l分別與平滑閾值Ths_smooth和非平滑閾值Ths_sharp進行比較獲得激光點是否為平滑點的結果：若滿足s_l≤Ths_smooth，該激光點為平滑點；若滿足s_l≥Ths_sharp，該激光點為非平滑點。

所述步驟1.2)具體是：對于單線激光點云上的各激光點，按以下公式處理并提取獲得鄰近點間距跳變點集合P_jump：

式中，N_{jump_nbr}表示預設的鄰近點個數，Ths_dis為鄰近點間距跳變閾值，l表示單線激光點云中的激光點序號，k表示該激光點的鄰近點的序號；

鄰近點間距跳變點集合P_jump中的點是作為候選點對中的后沿點，候選點對中包括前沿點和后沿點，前沿點是序號靠前的激光點，后沿點是序號靠后的激光點，判斷負障礙往往需要前后沿的點對，因此再取鄰近點間距跳變點集合P_jump中的每個激光點作為后沿點，其在單線激光點云中之前的一個激光點作為前沿點，形成鄰近點間距跳變點對并獲得鄰近點間距跳變點對集合P_{neg_pair}，具體是采用以下公式：

式中，l表示單線激光點云中的激光點序號。

以鄰近點間距跳變點對集合P_{neg_pair}中的鄰近點間距跳變點對作為初始候選點對。

所述的步驟1.3)具體為：將初始候選點對投影到激光雷達點云空間所對應的柵格屬性地圖上，每個柵格統計落在該柵格內激光點的數目，針對初始候選點對的后沿點判斷是否滿足以下公式：

PtNum_(k，l)-PtNum_(m,n)≥Ths_dense

m∈[k-N_grid,k+N_grid],m≠k

n∈[l-N_grid,l+N_grid],n≠l

式中，N_grid表示預設柵格鄰近距離，PtNum_(k,l)表示當前柵格中包含的所有激光點總數，PtNum_(m,n)表示當前柵格在預設柵格鄰近距離N_grid鄰域內的一柵格內包含的所有激光點總數，Ths_dense是預設的點云密集閾值，當前柵格是指初始候選點對的后沿點所在的柵格，m、n分別表示當前柵格在預設柵格鄰近距離N_grid鄰域內的柵格行號與列號。

初始候選點對的后沿點滿足公式，則認為滿足局部點云分布密集特征，將該初始候選點對保留，繼續下一步的處理；否則，將該初始候選點對舍棄，不繼續下一步的處理。

所述步驟1.4)具體為：

對于步驟1.3)篩選后的每個初始候選點對，選取后沿點前方的N_line個平滑點形成平滑點集P_{local_smooth}，N_line表示選取的平滑點總數，為預設值，將滑點集P_{local_smooth}中的所有平滑點通過最小二乘法進行直線擬合獲得直線方程；

再將后沿點空間坐標[x_l,y_l,z_l]中位于水平平面的x,y坐標代入直線方程，并與后沿點的z坐標相比較，具體是采用以下公式進行判斷篩選：

式中，d_zl是篩選后的初始候選點對前方的平滑點擬合直線在后沿點處的估計高度與實際高度之差，Ths_z為預設的候選點對后沿點高度下降閾值，l表示候選點對序號；

若滿足上述公式，則表明該后沿點位于直線的下方，滿足局部點云高度下降特征，保留該后沿點所在的初始候選點對，繼續下一步的處理；否則，將該后沿點所在的初始候選點對舍棄，不繼續下一步的處理。

本發明的激光點云空間的xyz坐標中，x是沿朝車輛的右方向，y是沿朝車輛的正前方向，z是沿垂直于底面朝向正上方向。

所述的直線方程采用以下方式擬合計算得到：

設空間直線經過點[x₀,y₀,0]，方向向量為[a,b,1]，則空間直線方程表示為：

其中，a、b分別表示空間直線的歸一化的方向向量在x、y坐標軸的兩個分量，x₀、y₀分別表示空間直線經過點的坐標的x、y坐標軸的兩個分量。

上述方程寫成矩陣形式為：

將這N_line個平滑點代入上述矩陣，得到：

其中，x₁到y₁到z₁到分別表示第1個到第N_line個平滑點的坐標的x、y、z坐標軸分量；

化簡得到：

上式中，l是平滑點序號；

解出上述a、b、x₀、y₀即求出直線方程。

所述的步驟2)中聚類具體為：

2.1)將步驟1)獲得各線激光點云中的候選點對進行合并，然后采用以下方式進行聚類，聚類過程在柵格屬性地圖上進行，柵格屬性地圖中的坐標具有行u與列v，將候選點對投影到柵格屬性地圖上獲得對應的柵格點對，柵格點對由前沿柵格點p_front和后沿柵格點p_back構成；

2.2)每一對候選點對均使用向量表示；

取前沿柵格點p_front和后沿柵格點p_back之間的中點表示該候選點對在柵格屬性地圖上的空間位置：

其中，u、v分別表示該候選點對在柵格屬性地圖上的行坐標和列坐標，u_front、v_front分別表示前沿柵格點p_front在柵格屬性地圖上的行坐標和列坐標，u_back、v_back分別表示后沿柵格點p_back在柵格屬性地圖上的行坐標和列坐標；

采用以下公式取前沿柵格點p_front和后沿柵格點p_back之間的距離表示候選點對的點對長度w：

2.3)由所有候選點對的向量形成候選點對向量集合G_pars：

其中，N_pars表示步驟1)獲得所有線激光上的候選點對總數；

2.3)使用均值偏移的方法進行聚類：

對每一個候選點對的向量均初始構造一個質心向量使得從而形成質心向量集合G_centers：

初始的質心向量集合G_centers滿足G_centers＝G_pars；

對質心向量集合G_centers中的每個質心向量，采用公式遍歷計算其對應的加權質心向量

其中，為高斯核函數，l、k均為質心向量的序號，h為預先設定的核函數帶寬；

所述的高斯核函數采用以下公式計算：

其中，h為核函數帶寬；

從而得到加權質心向量集合G_shfts：

2.4)對加權質心向量集合G_shfts中的每個加權質心向量，采用以下公式計算其與其對應的質心向量之間的歐氏距離Dshft_l：

式中，l表示加權質心向量的序號。將歐氏距離Dshft_l與誤差閾值Epsilon采用以下公式相比較，判斷獲得質心向量是否達到局部概率密度極大條件：

Dshft_l≤Epsilon

若滿足條件，則保持不變，否則采用以下方式更新

2.5)重復步驟2.2)-2.4)，直到所有候選點對的向量對應的所有質心向量均達到局部概率密度極大條件，即對任意i∈{0,1,…,N_pairs-1}，均滿足Dshift_i≤Epsilon；

2.6)每一個候選點對對應一個質心向量遍歷這些質心向量，將相同質心向量對應的候選點對歸為同一類，完成候選點對集合的聚類。

所述步驟1.3)或者步驟2.1)中的柵格屬性地圖具體是：柵格屬性地圖為基于大地平面假設，將激光雷達點云空間所在的x-y水平平面分割成大小相等的柵格，每個柵格尺寸為gridsize×gridsize，激光雷達點云空間的三維激光點坐標投影到柵格屬性地圖所在的柵格坐標(row,col)為：

其中，row、col分別表示該三維激光點投影到柵格屬性地圖所在的柵格坐標的行號與列號，Yoffset、Xoffset分別表示預設的偏移量；

并且每一個柵格包含車體區域標志位isCar、負障礙物標志位isNegObs和柵格內點云數目PtNum_(row,col)的屬性。本發明將負障礙物區域使用柵格屬性地圖來呈現，屬性為負障礙的柵格表示負障礙物所在位置。

所述的步驟2)中過濾具體為：得到負障礙的若干聚類之后，還需進行過濾操作。通常認為負障礙物應當大于一定尺寸以排除點云稀疏造成的誤判。負障礙物區域的面積尺寸由兩個值描述：該類中的候選點對個數Num_par，以及候選點對構成多邊形的面積Area_neg，多邊形為該類中候選點對的所有點的最小外接多邊形。由同一類的所有候選點對所形成的區域作為負障礙物候選區域，遍歷所有負障礙物候選區域，采用以下公式判斷是否滿足：

Num_par≥Ths_num

Area_neg≥Ths_area

式中，Num_par表示負障礙物候選區域中所有候選點對的總對數，Area_neg表示負障礙物候選區域中所有候選點對構成多邊形的面積，多邊形為包圍在負障礙物候選區域中所有候選點對外的最小外接多邊形，Ths_num為區域候選點對數量閾值，Ths_area為負障礙物在柵格屬性地圖中形成多邊形的面積閾值；

若同時滿足以上兩個條件，則負障礙物候選區域保留，為最終的負障礙物區域；

若不同時滿足以上兩個條件，則負障礙物候選區域舍棄，不為最終的負障礙物區域。

與背景技術比較，本發明具有的有益效果是：

1)本發明不依賴彩色相機、紅外相機等二維傳感器，因此檢測結果不受光照條件的影響；

2)本發明使用激光雷達傳感器檢測負障礙物，能夠直接測量負障礙物的距離，檢測結果位置精度高；

3)本發明先將大規模點云提取出小數量的候選點對集合，從小數量的候選點對中進行負障礙物聚類，相比直接使用圖像數據建模的計算復雜度更低，實時性更強；

附圖說明

圖1是本發明方法的流程圖。

圖2是垂直掃描激光雷達的安裝側視圖。

圖3是垂直掃描激光雷達的安裝俯視圖。

圖4是本發明的典型使用場景，其中左上是點云俯視圖，右上是場景實拍圖像，右中為柵格屬性地圖，下方是對應點云三維渲染圖。

圖5是對典型場景點云實施本發明內容(1.1)計算鄰近點間距(左圖)與局部平滑度(右圖)之結果，越亮表示值越大。其中P1、P2、P3、P4、P5是負障礙物候選點對的后沿點，可見在負障礙物候選點對中后沿點的鄰近點間距為局部極大值，負障礙物候選點對前方點云的局部平滑度較小，屬于平滑點。

圖6是對典型場景點云實施本發明內容(1.4)擬合直線的結果，P6、P7、P8、P9、P10為負障礙物候選點對前方的5個平滑點，白色長虛線為這5個平滑點擬合的直線，可見負障礙物候選點對中后沿點相對擬合直線呈現高度下降特征。

圖7是對典型場景點云實施本發明內容(1)、(2)提取負障礙物候選點對與聚類后的結果。左圖為提取獲得的負障礙物候選點對，用白色線段標出，右圖為聚類結果，用序號標識。

圖8是對典型場景點云實施本發明內容(2)過濾負障礙物候選區域的結果。左圖為過濾結果，只有0號和1號兩個負障礙物候選區域保留下來，右上為場景實拍圖像，右下為對應柵格屬性地圖，可見只保留過濾后的兩個負障礙物區域。

圖9是對4個野外場景點云實施本發明負障礙物檢測方法的結果圖。對每一張子圖，左邊是提取的負障礙物候選點對的聚類結果，右上為場景實拍圖像，右下為對應的柵格屬性地圖，其中標識出過濾后輸出的負障礙物區域。

具體實施方式

下面結合附圖和實施例對本發明做進一步說明。

如圖1所示，本發明方法的實施例及其實施過程如下：

以一個典型的包含負障礙的場景例子來更清楚地表示本發明的過程。

實施例的點云數據由車輛前部安裝的兩個激光雷達采集得到，兩個激光雷達布置方式和采集方式如圖2和圖3所示，激光雷達為多線激光旋轉掃描的工作方式，一根激光線對應一段連續分布的點云。

場景示意見圖4，其中左上為激光雷達點云俯視圖，右上為場景實拍圖像，右中為對應的柵格屬性地圖，下方位激光雷達點云三維渲染圖。取gridsize為20cm，Yoffset為1500cm，Xoffset為2000cm。

步驟一：對該場景實施本發明之(1.1)，逐線計算鄰近點間距、局部平滑度，其中N_neighbor取值5，Ths_smooth取值50，Ths_sharp取值350。如圖5所示，左邊為鄰近點間距示意圖，越亮表示值越大，可見負障礙物候選點對中后沿點的鄰近點間距數值較大；右邊為局部平滑度示意圖，越亮表示值越大，可見負障礙物候選點對前方點云的局部平滑度較小，為平滑點。

步驟二：對該場景實施本發明之(1.2)，逐激光點通過檢測局部點云距離跳變特征選取點云鄰近點間距局部極大值的候選點對，作為初始候選點對。其中Ths_dis取值40cm，N_{jump_nbr}取值5。

步驟三：對該場景實施本發明之(1.3)，在初始候選點對中，通過檢測局部點云分布密集特征選取初始候選點對中后沿點所在位置點云局部密集的點對，獲得篩選后的初始候選點對。其中Ths_dense取值4，N_grid取值2。

步驟四：對該場景實施本發明之(1.4)，在篩選后的初始候選點對中，通過局部點云高度下降特征進行進一步篩選獲得最終的候選點對。選取候選點對前方N_line個平滑度數值較小的點，其中N_line取值為5，用以擬合直線，繼而選取后沿點高度顯著低于直線位置的負障礙點對。如圖6所示，其中白色長虛線代表候選點對前方5個平滑點擬合的直線，可見負障礙物候選點對中的后沿點相對擬合直線呈現高度下降特征。

步驟五：對該場景實施本發明之(2.1)～(2.6)，將步驟四選取的負障礙物候選點對進行空間位置一致性、點對長度一致性聚類，其中核函數帶寬h取值4.0，誤差閾值Epsilon取值0.0001。如圖7所示，左圖為經過步驟四獲得的最終的候選點對，用白色線段標出，右圖為聚類結果，共聚成6類，用序號標識。

步驟六：對該場景實施本發明之(4)，對聚類結果根據區域面積、點對數量過濾，其中Ths_num的值取3，Ths_area值取20。如圖8所示，左圖為負障礙物候選區域經過濾后的結果，僅0、1號負障礙物候選區域被保留下來；右圖為對應輸出的柵格屬性地圖，其中車體前方兩個白色區域代表過濾后得到的負障礙物區域。

另外測試了4組不同的場景數據來評估本發明的效果，以激光雷達點云作為輸入，檢測輸出的柵格屬性地圖中負障礙物標識個數、位置。如圖9所示，4張子圖表示4個不同場景的檢測結果。對每一張子圖，左邊為激光雷達點云俯視圖，右上為對應場景彩色圖像，右下為輸出的柵格屬性地圖。檢測的具體結果如表1所示。表1中場景1、場景2、場景3、場景4分別對應圖9中的左上、右上、左下和右下四個子圖。表1記錄了4組實驗的場景描述、光照條件、總幀數、成功檢測出負障礙物的幀數、負障礙物首次檢出距離、檢測成功率、平均位置誤差和平均每幀耗時。

表1

從上表1結果可見，在不同光照條件下本方法均能夠達到90％以上的檢測成功率，平均位置誤差約為0.2m。對于直徑為1m，深0.5m的凹坑，本方法的首次檢出距離約為12.4m，而寬度為0.6m，深0.5m的壕溝，本方法的首次檢出距離為12.0m。實驗采用一臺配備4核心主頻為2.6GHz的Intel i5處理器的電腦，實驗中每幀激光雷達點云的平均處理耗時約為10ms，所以本發明方法的計算代價很小。

可以看出，本發明提出的面向無人車導航的基于點云數據局部結構特征的負障礙物檢測方法具有檢測結果不受光照條件影響、檢測結果位置精度高的特點，此外，本方法計算代價小，可以實時運行。