權利要求

1.礦山路沿檢測及建圖方法，其特征在于，所述方法包含以下步驟：

步驟一：通過激光雷達采集得到目標區(qū)域的激光點云數據，對采集得到的所述激光點云數據進行柵格化處理，得到柵格化數據；

步驟二：基于所述柵格化數據，結合海拔高度信息進行篩選，得到地面點云數據；

步驟三：針對所述地面點云數據進行路沿檢測，得到左側路沿點以及右側路沿點；

步驟四：將激光雷達坐標系下的所述左側路沿點以及所述右側路沿點進行坐標系轉換，得到全局坐標系下的路沿點數據；

步驟五：基于所述路沿點數據，利用八叉樹模型更新得到全局坐標系下的路沿地圖。

2.如權利要求1所述的方法，其特征在于，所述步驟一中具體包含以下步驟：

以垂直路面向上的方向為z軸，車輛的行進方向指向x軸，按照右手系建立三維坐標系；

基于所述三維坐標系，以激光雷達中心為原點，依據所述激光雷達的環(huán)信息劃分扇形區(qū)域，將所述點云數據按扇形網格區(qū)域進行劃分；

計算所述三維坐標系下每個單元格內部的所述點云數據的平均半徑、平均高度、平均水平坐標、高度最大值以及高度最小值。

3.如權利要求2所述的方法，其特征在于，所述步驟二中具體包含以下步驟：

基于每個單元格內部的高度差以及相鄰單元格的高度差來篩選得到地面候選單元格，具體來說，若當前單元格內部的高度差小于第一閾值，并且當前單元格與臨近單元格的高度差小于第二閾值，則將當前單元格記錄為所述地面候選單元格。

4.如權利要求3所述的方法，其特征在于，所述步驟二還包含以下步驟：若某個單元格被所述地面候選單元格包圍，則將該單元格也標記為地面候選單元格。

5.如權利要求3或4中任一項所述的方法，其特征在于，所述步驟三中具體包含以下步驟：通過環(huán)壓縮、垂直高度差以及水平波動差三個特征進行路沿檢測。

6.如權利要求5所述的方法，其特征在于，所述步驟三中具體包含以下步驟：

依據激光雷達光束照射在水平面上的半徑獲取相鄰環(huán)之間的距離，若環(huán)間距的壓縮比例落在預設范圍內，則將該單元格標記為路沿候選單元格；

沿扇形向外尋找高度突變單元格標記為路沿候選單元格；

尋找預設鄰域內水平波動在第三閾值內的變化單元標記為路沿候選單元格。

7.如權利要求6所述的方法，其特征在于，通過以下公式得到所述路沿候選單元格：

Δri＝ri+1-ri

＝h(cotθi+1-cotθi)

Ii＝[αΔri,βΔri)

其中，ri表示第i環(huán)的半徑，ri+1表示第i+1環(huán)的半徑，θi表示第i環(huán)相應的照射角，θi+1表示第i+1環(huán)相應的照射角，h表示傳感器高度，Δri表示第i環(huán)與第i+1環(huán)之間的距離，Ii表示環(huán)間距的壓縮比例預設范圍，α和β表示第一系數以及第二系數。

8.如權利要求6或7中任一項所述的方法，其特征在于，所述步驟三還包含以下步驟：

將標記的所述路沿候選單元格根據點的分布規(guī)律得到左、右側路沿的分割角度；

遍歷所有路沿候選單元格，根據所述柵格化數據中的扇形位置，對不存在路沿點的扇形進行排序，尋找非路沿扇形位置的中位數，選其為路沿分割線。

9.如權利要求1所述的方法，其特征在于，所述步驟五中具體包含以下步驟：使用相同尺寸的小立方體離散化待建圖區(qū)域，通過所述八叉樹模型進行路沿點更新。

10.一種礦山路沿檢測及建圖裝置，其特征在于，所述裝置包含：

第一模塊，其用于通過激光雷達采集得到目標區(qū)域的激光點云數據，對采集得到的所述激光點云數據進行柵格化處理，得到柵格化數據；

第二模塊，其用于基于所述柵格化數據，結合海拔高度信息進行篩選，得到地面點云數據；

第三模塊，其用于針對所述地面點云數據進行路沿檢測，得到左側路沿點以及右側路沿點；

第四模塊，其用于將激光雷達坐標系下的所述左側路沿點以及所述右側路沿點進行坐標系轉換，得到全局坐標系下的路沿點數據；

第五模塊，其用于基于所述路沿點數據，利用八叉樹模型更新得到全局坐標系下的路沿地圖。

說明書

技術領域

本發(fā)明涉及傳感器感知技術與智能交通技術領域，具體地說，涉及一種礦山路沿檢測及建圖方法及裝置。

背景技術

隨著智能車輛的高速發(fā)展，礦山環(huán)境下的車輛自動化越來越受到人們的關注，其中感知路沿是礦山高精地圖構建中的重要環(huán)節(jié)。通過使用精準的路沿圖，可以獲取車輛的有效感知范圍，減少計算資源的消耗；同時，路沿圖能夠提供地圖的先驗信息，幫助進行路徑規(guī)劃；此外，還可以根據車輛與路沿的相對位置的比較進行定位，完成多傳感器融合的車輛狀態(tài)估計。

與攝像頭相比，LiDAR(激光雷達)受天氣和光照的影響較小，在大部分場景下正常工作，并且滿足自動駕駛的實時性。根據環(huán)境的差異，路沿感知策略也千差萬別。道路一般可分為結構化與非結構化兩種，高速路和城市道路就是典型的結構化道路，非結構化道路一般缺乏人工標志，本發(fā)明所涉及的礦山道路可以歸為此類。

近年來，與城市道路路沿檢測相關的研究不少。根據所識別的特征，LiDAR的路沿檢測的方法可以分為基于障礙物檢測和基于路面提取兩種思路?；谡系K物檢測的方法適用于具有護欄結構的結構化道路，基于車道線提取的方法不適用于礦山這種無人工繪制車道線的環(huán)境。傳統(tǒng)基于LiDAR的路沿檢測方法，是通過進行二次曲線進行擬合完成的。但是，在礦山環(huán)境下，路沿具有高度的不規(guī)則形狀，強行擬合則會失去很多真正的路沿點?，F有的路沿檢測方法多假設環(huán)境為二維平面，用于起伏多變的礦山環(huán)境進行路沿檢測時，會忽視重要的海拔高度信息。

為了解決上述問題，本發(fā)明給出一種礦山路沿檢測及建圖方法及裝置。

發(fā)明內容

為解決上述問題，本發(fā)明提供了一種礦山路沿檢測及建圖方法，所述方法包含以下步驟：

步驟一：通過激光雷達采集得到目標區(qū)域的激光點云數據，對采集得到的所述激光點云數據進行柵格化處理，得到柵格化數據；

步驟二：基于所述柵格化數據，結合海拔高度信息進行篩選，得到地面點云數據；

步驟三：針對所述地面點云數據進行路沿檢測，得到左側路沿點以及右側路沿點；

步驟四：將激光雷達坐標系下的所述左側路沿點以及所述右側路沿點進行坐標系轉換，得到全局坐標系下的路沿點數據；

步驟五：基于所述路沿點數據，利用八叉樹模型更新得到全局坐標系下的路沿地圖。

根據本發(fā)明的一個實施例，所述步驟一中具體包含以下步驟：

以垂直路面向上的方向為z軸，車輛的行進方向指向x軸，按照右手系建立三維坐標系；

基于所述三維坐標系，以激光雷達中心為原點，依據所述激光雷達的環(huán)信息劃分扇形區(qū)域，將所述點云數據按扇形網格區(qū)域進行劃分；

計算所述三維坐標系下每個單元格內部的所述點云數據的平均半徑、平均高度、平均水平坐標、高度最大值以及高度最小值。

根據本發(fā)明的一個實施例，所述步驟二中具體包含以下步驟：

基于每個單元格內部的高度差以及相鄰單元格的高度差來篩選得到地面候選單元格，具體來說，若當前單元格內部的高度差小于第一閾值，并且當前單元格與臨近單元格的高度差小于第二閾值，則將當前單元格記錄為所述地面候選單元格。

根據本發(fā)明的一個實施例，所述步驟二還包含以下步驟：若某個單元格被所述地面候選單元格包圍，則將該單元格也標記為地面候選單元格。

根據本發(fā)明的一個實施例，所述步驟三中具體包含以下步驟：通過環(huán)壓縮、垂直高度差以及水平波動差三個特征進行路沿檢測。

根據本發(fā)明的一個實施例，所述步驟三中具體包含以下步驟：

依據激光雷達光束照射在水平面上的半徑獲取相鄰環(huán)之間的距離，若環(huán)間距的壓縮比例落在預設范圍內，則將該單元格標記為路沿候選單元格；

沿扇形向外尋找高度突變單元格標記為路沿候選單元格；

尋找預設鄰域內水平波動在第三閾值內的變化單元標記為路沿候選單元格。

根據本發(fā)明的一個實施例，通過以下公式得到所述路沿候選單元格：

Δri＝ri+1-ri

＝h(cotθi+1-cotθi)

Ii＝[αΔri,βΔri)

其中，ri表示第i環(huán)的半徑，ri+1表示第i+1環(huán)的半徑，θi表示第i環(huán)相應的照射角，θi+1表示第i+1環(huán)相應的照射角，h表示傳感器高度，Δri表示第i環(huán)與第i+1環(huán)之間的距離，Ii表示環(huán)間距的壓縮比例預設范圍，α和β表示第一系數以及第二系數。

根據本發(fā)明的一個實施例，所述步驟三還包含以下步驟：

將標記的所述路沿候選單元格根據點的分布規(guī)律得到左、右側路沿的分割角度；

遍歷所有路沿候選單元格，根據所述柵格化數據中的扇形位置，對不存在路沿點的扇形進行排序，尋找非路沿扇形位置的中位數，選其為路沿分割線。

根據本發(fā)明的一個實施例，所述步驟五中具體包含以下步驟：使用相同尺寸的小立方體離散化待建圖區(qū)域，通過所述八叉樹模型進行路沿點更新。

根據本發(fā)明的另一個方面，還提供了一種礦山路沿檢測及建圖裝置，所述裝置包含：

第一模塊，其用于通過激光雷達采集得到目標區(qū)域的激光點云數據，對采集得到的所述激光點云數據進行柵格化處理，得到柵格化數據；

第二模塊，其用于基于所述柵格化數據，結合海拔高度信息進行篩選，得到地面點云數據；

第三模塊，其用于針對所述地面點云數據進行路沿檢測，得到左側路沿點以及右側路沿點；

第四模塊，其用于將激光雷達坐標系下的所述左側路沿點以及所述右側路沿點進行坐標系轉換，得到全局坐標系下的路沿點數據；

第五模塊，其用于基于所述路沿點數據，利用八叉樹模型更新得到全局坐標系下的路沿地圖。

本發(fā)明主要針對礦山環(huán)境下路況多變的情況，利用八叉樹模型提供的占據更新方案，實現路沿點的更新與平滑，進一步濾除路面凹凸引入的干擾點。另外，針對礦山路面凹凸不平的特性，提出了以環(huán)壓縮為主的路沿檢測方案，適應非垂直上升的礦山道路邊界，兼顧候選點鄰域內的距離波動差。本發(fā)明對于彎道等特殊與非特殊情況下提出了基于路沿分布的左右路沿劃分方案，克服了單純依靠水平軸正負劃分路沿所帶來的局限性。

本發(fā)明的其它特征和優(yōu)點將在隨后的說明書中闡述，并且，部分地從說明書中變得顯而易見，或者通過實施本發(fā)明而了解。本發(fā)明的目的和其他優(yōu)點可通過在說明書、權利要求書以及附圖中所特別指出的結構來實現和獲得。

附圖說明

附圖用來提供對本發(fā)明的進一步理解，并且構成說明書的一部分，與本發(fā)明的實施例共同用于解釋本發(fā)明，并不構成對本發(fā)明的限制。在附圖中：

圖1顯示了根據本發(fā)明的一個實施例的礦山路沿檢測及建圖方法流程圖；

圖2顯示了根據本發(fā)明的一個實施例的礦山路沿檢測及建圖系統(tǒng)框架示意圖；

圖3顯示了根據本發(fā)明的一個實施例的柵格平面分布示意圖；以及

圖4顯示了根據本發(fā)明的一個實施例的礦山路沿檢測及建圖裝置結構框圖。

具體實施方式

為使本發(fā)明的目的、技術方案和優(yōu)點更加清楚，以下結合附圖對本發(fā)明實施例作進一步地詳細說明。

現有技術提出了一種基于RANSAC(采樣一致性)算法的地面分割方案，并根據同一掃描層的坡度差、高度差以及到激光雷達的距離比來提取路沿候選點。但在礦山環(huán)境下，當LiDAR的同一掃描環(huán)遭遇非垂直擋墻時，經常會向車輛行進方向延伸而不會產生明顯的坡度或高度變化特征。

另一現有技術提出，可根據當前點的梯度來選取路沿候選點，并且，根據每一幀二次擬合系數變化來判斷當前檢測結果是否有誤。但是，在礦山環(huán)境下，僅根據梯度特征進行判斷，會引入不平整路面產生的干擾。而且，不規(guī)則的礦山路沿并不是每一幀都可以實現完美的二次擬合。

綜上，現有的技術大多只適用于路面平整且路寬不會頻繁變化的城市道路，而對于礦山環(huán)境下的多變路況及不平整路面卻無能為力，單純使用高度、坡度或者法線等特征都會導致大量的誤檢測，而且基于卡爾曼濾波的路沿平滑方案在路寬多變時效果較差。因此，有必要針對自動駕駛礦車提出一種路沿檢測方法。

本發(fā)明針對高精度地圖構建前期的路沿自動化檢測與建圖問題，結合LiDAR(激光雷達)及組合導航定位傳感器，采用OctoMap(基于八叉樹的存儲結構)融合檢測結果，提出了一種新型的礦山環(huán)境下的路沿檢測與建圖技術方案，以至少解決了現有技術中的上述問題。本發(fā)明能夠基于激光點云的礦山環(huán)境下的道路邊界檢測方法，其能用于礦用車輛所需的高精地圖構建與自動駕駛系統(tǒng)。

圖1顯示了根據本發(fā)明的一個實施例的礦山路沿檢測及建圖方法流程圖。

如圖1，步驟S101中，通過激光雷達采集得到目標區(qū)域的激光點云數據，對采集得到的激光點云數據進行柵格化處理，得到柵格化數據。

具體來說，在步驟S101中，首先，以垂直路面向上的方向為z軸，車輛的行進方向指向x軸，按照右手系建立三維坐標系。

然后，基于三維坐標系，以激光雷達中心為原點，依據激光雷達的環(huán)信息劃分扇形區(qū)域，將點云數據按扇形網格區(qū)域進行劃分。

最后，計算三維坐標系下每個單元格內部的點云數據的平均半徑、平均高度、平均水平坐標、高度最大值以及高度最小值。

如圖1，在步驟S102中，基于柵格化數據，結合海拔高度信息進行篩選，得到地面點云數據。

具體來說，在步驟S102中，基于每個單元格內部的高度差以及相鄰單元格的高度差來篩選得到地面候選單元格，具體來說，若當前單元格內部的高度差小于第一閾值，并且當前單元格與臨近單元格的高度差小于第二閾值，則將當前單元格記錄為地面候選單元格。

另外，在步驟S102中，若某個單元格被所述地面候選單元格包圍，則將該單元格也標記為地面候選單元格。

如圖1，在步驟S103中，針對地面點云數據進行路沿檢測，得到左側路沿點以及右側路沿點。

在一個實施例中，通過環(huán)壓縮、垂直高度差以及水平波動差三個特征進行路沿檢測。

具體來說，依據激光雷達光束照射在水平面上的半徑獲取相鄰環(huán)之間的距離，若環(huán)間距的壓縮比例落在預設范圍內，則將該單元格標記為路沿候選單元格；沿扇形向外尋找高度突變單元格標記為路沿候選單元格；尋找預設鄰域內水平波動在第三閾值內的變化單元標記為路沿候選單元格。

具體來說，通過以下公式得到路沿候選單元格：

Δri＝ri+1-ri

＝h(cotθi+1-cotθi)

Ii＝[αΔri,βΔri)

其中，ri表示第i環(huán)的半徑，ri+1表示第i+1環(huán)的半徑，θi表示第i環(huán)相應的照射角，θi+1表示第i+1環(huán)相應的照射角，h表示傳感器高度，Δri表示第i環(huán)與第i+1環(huán)之間的距離，Ii表示環(huán)間距的壓縮比例預設范圍，α和β表示第一系數以及第二系數。

在一個實施例中，在步驟S103中，將標記的路沿候選單元格根據點的分布規(guī)律得到左、右側路沿的分割角度；遍歷所有路沿候選單元格，根據所述柵格化數據中的扇形位置，對不存在路沿點的扇形進行排序，尋找非路沿扇形位置的中位數，選其為路沿分割線。

如圖1，在步驟S104中，將激光雷達坐標系下的左側路沿點以及右側路沿點進行坐標系轉換，得到全局坐標系下的路沿點數據。

如圖1，在步驟S105中，基于路沿點數據，利用八叉樹模型更新得到全局坐標系下的路沿地圖。

具體來說，使用相同尺寸的小立方體離散化待建圖區(qū)域，通過八叉樹模型進行路沿點更新。

圖2顯示了根據本發(fā)明的一個實施例的礦山路沿檢測及建圖系統(tǒng)框架示意圖。如圖2所示的系統(tǒng)為解決礦山環(huán)境下路況多變，且道路表面凹凸的情形下的路沿檢測問題，提出了一種基于八叉樹占據更新模型的路沿檢測與建圖方案，感知數據來自LiDAR，定位數據來自組合導航定位模塊，利用OctoMap融合路沿檢測結果與全局定位信息，得到更加準確的全局路沿地圖。

首先，對激光點云數據進行柵格化處理：

具體來說，以垂直路面向上為Z軸，按照右手系建立三維坐標系，X軸指向車輛行進方向；以LiDAR中心為原點，將點云數據劃分成扇形網格區(qū)域，并根據激光雷達的環(huán)信息劃分每個扇形(如圖3所示)；計算每個單元格ci,j內部的平均半徑，平均高度，平均水平坐標，高度最大值，高度最小值。

在一個實施例中，LiDAR使用16線激光雷達。

接著，利用海拔高度等依據，獲取地面點云：

具體來說，根據每個單元格ci,j內部的高度差以及與相鄰單元格的高度差來篩選地面候選單元格，當ci,j內部的高度差小于閾值，并且ci,j與臨近單元格的高度差小于閾值時，將對應單元格記錄為地面候選單元格；如果某個單元格被地面候選格包圍，則將該單元格也標記為地面候選單元格。

然后，利用地面點云數據的環(huán)壓縮率等特點，獲取左右側路沿點：采用環(huán)壓縮、垂直高度差、水平波動差三個特征進行路沿檢測。

步驟a，尋找環(huán)壓縮所在的單元格：

當LiDAR發(fā)出的光束遭遇路沿時，相鄰環(huán)之間會產生劇烈的壓縮，根據LiDAR光束照射在水平面上的半徑ri＝hcotθi獲取相鄰環(huán)之間的距離，若環(huán)間距的壓縮比例落在某一范圍內，則將該柵格標記為路沿候選單元格，公式如下：

Δri＝ri+1-ri

＝h(cotθi+1-cotθi)

Ii＝[αΔri,βΔri)

其中，ri代表第i環(huán)的半徑，θi為相應的照射角，h為傳感器高度，Δri是環(huán)i與環(huán)i+1之間的距離，α和β決定了壓縮率區(qū)間的范圍。

通過比較環(huán)壓縮的比例，選出路沿點。

步驟b，沿扇形向外尋找高度突變單元格標記為路沿候選單元格；

步驟c，尋找預設鄰域內水平波動在第三閾值內的變化單元標記為路沿候選單元格；

步驟d，左右側路沿劃分：將每一幀檢測到的路沿點根據點的分布規(guī)律得到左右側路沿的分割角度。遍歷全部路沿點，并根據之前柵格化后的扇形位置對不存在路沿點的扇形進行排序(分為車前和車后兩部分)，尋找非路沿扇形位置的中位數，則選為路沿分割線。

接著，將激光雷達坐標系下的路沿點轉換到全局坐標系下。

最后，利用八叉樹模型對路沿點進行更新，從而得到全局坐標系下的路沿地圖：

具體來說，使用相同尺寸的小立方體離散化待建圖區(qū)域，通過OctoMap(基于八叉樹的存儲結構)進行路沿點更新，避免了RANSAC(隨機此采樣一致性)或者基于卡爾曼濾波的路沿平滑策略帶來的擬合失敗或者預測失敗問題。

通過統(tǒng)計hit(光束撞擊到立方格)和miss(光束未撞擊到立方格)來進行概率更新，得到每個立方格的占據概率。達到閾值時認為該單元被占用，即存在路沿，反之為空閑，即不存在路沿。使用如下公式進行占據更新：

L(n|z1:t)

＝max(min(L(n|z1:t-1)+L(n|zt),lmax),lmin)

其中，lmin和lmax代表了log-odds的下限和上限。

以上基于體素的占用柵格方案可以有效的回避卡爾曼濾波中對于路寬不魯棒的問題，而且通過調整hit、miss和占據閾值等參數可以實現不平整路面帶來的大量誤檢測點。

綜上，本發(fā)明主要針對礦山復雜環(huán)境下，路況多變的情形，利用OctoMap(基于八叉樹的存儲結構)融合多幀信息并建圖。針對礦山路面凹凸不平的特性，提出了以環(huán)壓縮為主的路沿檢測方案，兼顧候選點鄰域內的距離波動差。本發(fā)明對于彎道等特殊與非特殊情況下提出了基于路沿分布的左右路沿劃分方案，克服了單純依靠水平軸正負劃分路沿所帶來的局限性。

圖4顯示了根據本發(fā)明的一個實施例的礦山路沿檢測及建圖裝置結構框圖。其中，路沿檢測及建圖裝置400包含第一模塊401、第二模塊402、第三模塊403、第四模塊404以及第五模塊405。

第一模塊401用于通過激光雷達采集得到目標區(qū)域的激光點云數據，對采集得到的激光點云數據進行柵格化處理，得到柵格化數據；

第二模塊402用于基于柵格化數據，結合海拔高度信息進行篩選，得到地面點云數據；

第三模塊403用于針對地面點云數據進行路沿檢測，得到左側路沿點以及右側路沿點；

第四模塊404用于將激光雷達坐標系下的左側路沿點以及右側路沿點進行坐標系轉換，得到全局坐標系下的路沿點數據；

第五模塊405用于基于路沿點數據，利用八叉樹模型更新得到全局坐標系下的路沿地圖。

綜上，本發(fā)明主要針對礦山環(huán)境下路況多變的情況，利用八叉樹模型提供的占據更新方案，實現路沿點的更新與平滑，進一步濾除路面凹凸引入的干擾點。另外，針對礦山路面凹凸不平的特性，提出了以環(huán)壓縮為主的路沿檢測方案，適應非垂直上升的礦山道路邊界，兼顧候選點鄰域內的距離波動差。本發(fā)明對于彎道等特殊與非特殊情況下提出了基于路沿分布的左右路沿劃分方案，克服了單純依靠水平軸正負劃分路沿所帶來的局限性。

應該理解的是，本發(fā)明所公開的實施例不限于這里所公開的特定結構、處理步驟或材料，而應當延伸到相關領域的普通技術人員所理解的這些特征的等同替代。還應當理解的是，在此使用的術語僅用于描述特定實施例的目的，而并不意味著限制。

說明書中提到的“一個實施例”或“實施例”意指結合實施例描述的特定特征、結構或特性包括在本發(fā)明的至少一個實施例中。因此，說明書通篇各個地方出現的短語“一個實施例”或“實施例”并不一定均指同一個實施例。

雖然本發(fā)明所公開的實施方式如上，但所述的內容只是為了便于理解本發(fā)明而采用的實施方式，并非用以限定本發(fā)明。任何本發(fā)明所屬技術領域內的技術人員，在不脫離本發(fā)明所公開的精神和范圍的前提下，可以在實施的形式上及細節(jié)上作任何的修改與變化，但本發(fā)明的專利保護范圍，仍須以所附的權利要求書所界定的范圍為準。