面向無人駕駛礦車的露天礦山道路坡度實時檢測方法

孟德將，田 濱，蔡 峰，高義軍，陳 龍

1. 北京慧拓無限科技有限公司，北京 100190； 2. 中國科學院自動化研究所復雜系統管理與控制國家重點實驗室，北京 100190； 3. 中國科學院大學人工智能學院，北京 100190； 4. 中國中煤能源集團有限公司，北京 100120； 5. 安徽馬鋼礦業資源集團南山礦業有限公司，馬鞍山 243031； 6. 中山大學數據科學與計算機學院，廣州 510006

智慧礦山已經成為礦山發展的方向。露天礦山道路坡度變化范圍大，無人駕駛礦車容易發生因下坡急減速導致的物料外撒或因上坡導致的溜車等危險。無人駕駛礦車需要精確檢測車輛前方一定范圍內的道路坡度，輔助速度規劃合理規劃速度，才能避免危險的發生。無人駕駛礦車依據道路坡度合理規劃速度后，還可以提升運輸的效率。

目前可以用于無人駕駛礦車實時檢測露天礦山道路坡度的方法主要分為4類。

(1) 基于INS或GNSS，直接從車載高精度的INS讀取俯仰角、基于GNSS單天線計算水平與豎直方向的速度比或基于GNSS雙天線提取信號的低頻部分等作為道路的坡度[1-5]。這一類方法在道路坡度小且平整的結構化場景中精確度較高，但是在檢測露天礦山道路坡度時有兩個挑戰：①礦山道路坡度大且不平整，礦車在行駛過程中的俯仰和彈跳運動會降低檢測精確度；②無法實時檢測車輛前方的道路坡度。

(2) 基于SLAM算法，首先提取周圍環境的線、面等特征；然后利用幀間匹配算法或地圖匹配算法等構建精確的三維環境地圖；最后可以檢測道路坡度[6-12]。這一類方法在幾何特征明顯的結構化場景中的精確度比較高，但是在檢測露天礦山道路坡度時有一個挑戰：露天礦山幾乎沒有樹木和建筑，幾何特征缺失，在一些特征缺失嚴重的路段容易匹配錯誤，導致道路坡度檢測精確度較低。同時，這一類方法測出的也是車輛的俯仰角，而不是道路的坡度。

(3) 基于卡爾曼濾波器或龍伯格(Luenberger)觀測器等，通過分析車輛的受外力情況建立車輛模型，進而利用動力學的方法估計道路的坡度[13-19]。這一類方法無須借助昂貴的激光雷達或INS，但是該方法檢測道路坡度會受到車輛動力學性能和道路條件的影響，露天礦山道路不平整，同時獲取的礦車動力學參數也存在誤差，所以很難獲得精確的道路坡度。

(4) 基于激光雷達的方法。激光雷達可以實時精確地獲得周圍環境的三維位置信息，基于激光雷達的這個特點，已經提出了很多地形檢測的方法。研究比較多的地形包括路沿(路面)、凹坑等。路沿(路面)的研究方法主要分為兩類：一類基于傳統方法，首先使用高度差、ILP(integral laser points)等特征提取路沿候選點，然后使用霍夫變換、B樣條等方法對路沿候選點進行曲線擬合，提取最終的道路邊界[20-21]；另外一類基于機器學習或深度學習的方法，使用CNN(convolutional neural network)或DCNN(deep convolutional neural network)等對路面進行分割，從而提取路面[22-23]。凹坑主要采用傳統方法，一種方法是首先通過投影獲得點云深度圖，然后在點云深度圖的基礎上采用激光雷達直方圖及點云分割的方法檢測凹坑[24]；另一種方法是直接利用原始的激光點云之間的幾何關系檢測凹坑[25]。雖然基于激光雷達檢測地形的方法比較多，但是公開的基于激光雷達檢測道路坡度的方法還比較少。目前基于激光雷達檢測道路坡度的方法首先通過提取打在斜坡上的點云，然后使用PROSAC(progressive sample consensus)、RANSAC(random sample consensus)等平面擬合的方法得到斜坡坡面方程，進而獲得道路坡度[26-27]。因為這一類方法沒有融合INS姿態角信息，無法檢測道路相對于水平面的坡度，所以，在坡度小且平整的結構化場景中的道路坡度檢測精確度還比較高，而在露天礦山場景下，則對道路坡度的檢測精確度就比較差。

針對目前無人駕駛礦車實時檢測露天礦山道路坡度研究中存在的問題，利用激光雷達可以實時精確地獲得環境三維位置信息及INS可以實時精確獲得INS坐標系相對于水平面姿態角的特點，本文提出了柵格卡爾曼道路坡度實時檢測(grid Kalman road slope real-time detection，GKSRD)方法。該方法以三維激光雷達點云和INS俯仰角信息作為輸入，并采用二維柵格地圖、感興趣矩形區域迭代優化算法和卡爾曼濾波器。相比于基于INS或GNSS的方法，該方法減小了無人駕駛礦車行駛過程中由于道路坡度大且不平整對道路坡度實時檢測帶來的誤差。相比于基于SLAM的方法，因為該方法不依賴周圍環境的幾何特征，所以其不會受到露天礦山幾何特征缺失的影響。同時，相比于基于激光雷達的方法，該方法融合了INS俯仰角信息，可以更加精確地檢測露天礦山的道路坡度。經過試驗驗證，該方法在露天礦山復雜的道路條件下可以實時、精確地檢測車輛前方道路某一區域的坡度。

1 柵格卡爾曼道路坡度實時檢測(GKSRD)方法

露天礦山道路坡度變化范圍大且不平整，無人駕駛礦車會有比較大的俯仰和彈跳運動。同時，露天礦山幾何特征缺失。為了精確地提取露天礦山道路的坡度，本文提出了柵格卡爾曼道路坡度實時檢測方法。激光雷達可以提供精確的環境三維信息。INS可以提供INS坐標系相對于水平面的精確姿態信息。本文方法融合激光雷達和INS作為輸入，可以在很大程度上降低無人駕駛礦車在運動過程中的俯仰和彈跳運動對道路坡度提取的影響。傳感器融合必須保證不同傳感器有統一的坐標表示，本文采用文獻[28]的方法對INS和激光雷達進行聯合標定，獲得INS坐標系與激光雷達坐標系之間的相對位姿關系。該方法適用于機械式旋轉雷達和固態雷達，如果是前者，輸出的點云會發生運動畸變，本文基于勻速運動模型[29]對點云進行運動畸變矯正。

本文方法輸出的道路坡度是實時檢測的，即只用到了當前幀和歷史幀的數據，沒有用到未來幀的數據。根據露天礦山道路的實際情況，方法忽略無人駕駛礦車的側傾運動，只融合INS俯仰角，從而獲得點云在INS水平坐標系下的三維坐標，INS水平坐標系原點與INS坐標系原點重合。無人駕駛礦車主要關心車輛行駛方向的道路坡度，不關心另外兩個維度的道路坡度，因此，本文方法基于二維柵格地圖計算道路坡度，將INS水平坐標系下的點云投影到二維柵格地圖，柵格的值為點云的高度，從而獲得二維高度柵格地圖[30]。相比于實際的道路坡度變化情況，由于本文方法采用的二維柵格的分辨率比較高，所以基于二維柵格地圖計算車輛行駛方向的道路坡度不僅對檢測的精確度影響不大，還能在很大程度上減少時間成本。在二維高度柵格地圖中定義一個用于計算道路坡度的感興趣矩形區域，矩形區域相對于INS水平坐標系原點的位置以及矩形區域的大小是固定的。將感興趣矩形區域劃分為遠近兩部分，使用感興趣矩形區域迭代優化算法分別計算每一部分的高度值，進而計算整個感興趣矩形區域的坡度初始值。基于坡度初始值，融合卡爾曼濾波器，即可提取精確的實時道路坡度。

1.1 二維高度柵格地圖構建

本文方法基于二維高度柵格地圖檢測道路坡度，首先需要構建二維高度柵格地圖。如圖1所示，x0y0z0為激光雷達坐標系，xyz為車體坐標系，車體坐標系的xy平面與車底板平行，x1y1z1為INS坐標系，x2y2z2為INS水平坐標系，INS水平坐標系的x2y2平面與水平面平行。

(1)

(2)

式中，λ1為INS相對于車底板的角度安裝誤差；λ2為由于車輛俯仰運動產生的車底板與路面的夾角；λ3為車輛所在道路的坡度；λ1+λ2+λ3為INS的俯仰角。

根據式(3)將PL投影到二維柵格地圖，可以獲得二維高度柵格地圖

(3)

圖1 道路坡度實時檢測示意Fig.1 A diagram of road slope real-time detection

式中，Zindex為索引index的柵格的高度；其中index與i滿足

i∈[0,n]

(4)

1.2 感興趣矩形區域迭代優化算法

建立了二維高度柵格地圖后，即可進行道路坡度計算。首先需要確定柵格地圖中的感興趣矩形區域。為了使感興趣矩形區域中有盡量多的值不為0的柵格，感興趣矩形區域選擇在車輛前方道路點云比較密集的區域。根據實際情況，道路的寬度必然大于無人駕駛礦車的寬度，所以感興趣矩形區域的寬一般略大于車寬。如圖2所示，A與B分別為近處和遠處的感興趣矩形區域，組成了選定的感興趣矩形區域。A與B的大小相同，位置相鄰，且在二維柵格地圖坐標系中的位置固定。

確定了二維柵格地圖中的感興趣矩形區域后，根據二維柵格地圖矩形區域迭代優化算法可以分別計算A與B的幾何中心位置(XA,YA)與(XB,YB)處的高度ZA和ZB。算法的輸入包括二維高度柵格地圖、矩形區域邊界、初始迭代次數、迭代次數閾值、初始偏移量閾值、偏移量步長及標準差閾值。初始偏移量閾值、偏移量步長與標準差閾值配合篩選每次迭代時偏離均值較遠的柵格。然后基于直角三角形模型，由式(5)可確定感興趣區域A+B的幾何中心位置的初始坡度

(5)

圖2 二維柵格地圖Fig.2 2D grid map

二維柵格地圖矩形區域迭代優化算法如下。

輸入：ogm二維高度柵格地圖；minrow矩形區域下邊界；maxrow矩形區域上邊界；mincol矩形區域左邊界；maxcol矩形區域右邊界；count初始迭代次數；counthre迭代次數閾值；offsethre初始偏移量閾值；s偏移量步長；stdevthre標準差閾值

輸出：mean矩形區域幾何中心位置高度

1: function Interation(ogm, minrow, maxrow, mincol, maxcol, count, counthre, offsethre, stdevthre,s)

2: offsethre←offsethre-s

3: count++

4: totalvalue←0 矩形區域內部柵格值的總和

5: totalcell←0 矩形區域內部不為0的柵格的數量

6: array 保存矩形區域內部不為零的柵格的值的數組

7: width 柵格地圖的寬，即列數

8: fori←minrow to maxrow step 1 do

9: forj←mincol to maxcol step 1 do

10: if ogm[i·width+j]>0 then

11: totalvalue←totalvalue+ogm[i·width+j]

12: array[totalcell]←ogm[i·width +j]

13: totalcell←totalcell+1

14: end if

15: end for

16: end for

17: mean←totalvalue / totalell

19: if stdev>stdevthre and count

offsethre>1 then

20: fori←minrow to maxrow step 1 do

21: forj←mincol to maxcol step 1 do

22: offset←|ogm[i·width+j] - mean|

23: if ogm[i·width+j]≠0 and offset>

offsethre then

24: ogm[i·width+j]←0

25: end if

26: end for

27: end for

28: return

29: function Interation(ogm, minrow, maxrow,

mincol, maxcol, count, counthre, offsethre, stdevthre,s)

30: else

31: return mean

32: end if

33: end function

1.3 基于卡爾曼濾波器的結果優化

GKSRD方法的前兩步已經求出了當前幀感興趣矩形區域幾何中心位置的道路初始坡度。根據實際情況，基于相鄰幀道路坡度均勻變化模型，融合卡爾曼濾波器，GKSRD方法可以進一步優化當前幀感興趣矩形區域幾何中心位置的道路坡度，優化的結果即為GKSRD方法最終的道路坡度實時提取結果。相鄰幀道路坡度均勻變化模型是指相鄰幀的感興趣矩形區域幾何中心位置的道路坡度均勻變化。

卡爾曼濾波器由兩個步驟描述：估計和測量更新。

第1步：估計。當前幀的狀態估計向量xk和誤差協方差矩陣Pk根據式(6)和式(7)求得

xk=Fkxk-1

(6)

(7)

式中，xk-1為前一幀的感興趣矩形區域幾何中心位置的道路坡度測量更新結果，只包含道路坡度一個狀態變量；Pk-1初始化為一行一列的單位矩陣；Fk為狀態轉移矩陣，基于相鄰幀道路坡度均勻變化模型，令Fk=1；Qk為過程噪聲協方差，本文方法中忽略過程噪聲協方差，令Qk=0。

(8)

(9)

(10)

式中，Hk為尺度變換矩陣，令Hk=1；Rk為測量噪聲協方差矩陣，本文方法中忽略測量噪聲協方差，令Rk=0。

2 試驗與分析

2.1 試驗場景

為了驗證本文方法的有效性，在兩個不同的露天礦山進行試驗。如圖3所示，其中，圖3(a)為內蒙古寶利露天煤礦，圖3(b)為馬鞍山和尚橋露天鐵礦。圖3(a)道路坡度大，路面相對平整；圖3(b)道路坡度也很大，同時，路面有很多礦石，路面很不平整。

2.2 試驗平臺

本文采用的試驗平臺如圖4所示，型號為臨工集團CMT96非公路自卸車，車頭左右兩側各安裝一個三維激光雷達，型號為RS-LiDAR-16，部分性能參數信息見表1所示，緊貼車輛前軸中心正上方的車底板上安裝一個慣性導航系統，型號為OXTS Inertial+，部分性能參數信息表2所示。

圖3 試驗場景Fig.3 Experimental scene

圖4 試驗平臺Fig.4 Experimental platform

表1 RS-LIDAR-16性能參數

表2 OXTS Inertial+性能參數

由表1和表2可以看出激光雷達的測距精度可以達到2 cm，INS的Pitch精度可以達到0.03°。計算平臺為工控機(industrial personal computer，IPC)，CPU(intel core i7-6820EQ)為四核八線程，主頻2.8 GHz，運行內存為16 GB，系統環境為Ubuntu18.04系統，基于ROS(robot operating system)軟件框架。

在試驗中，為了使道路坡度真值盡量精確，INS由于安裝誤差，還需要進行安裝誤差矯正，誤差矯正采用高精度的傾角儀，將車輛靜止停放在一個平整的斜坡上，利用傾角儀測出斜坡的角度，同時讀出INS的俯仰角，即可獲得安裝誤差。經過安裝誤差矯正后，理論上，車載INS在靜止狀態下的俯仰角就是所在位置的道路坡度。

2.3 試驗結果

本文在圖3(a)和圖3(b)的場景中各選擇了3段道路作為試驗對象，道路依次標記為a、b、c、d、e、f、g、h。在圖3(a)道路車輛的速度為15 km/h，在圖3(b)道路車輛的速度為10 km/h。柵格地圖大小為40×70 m2，分辨率為0.2 m，坐標原點位于左下角，INS水平坐標系原點的坐標為(20,20)。如圖5所示為部分道路坡度實時提取效果圖，紅色單元格的邊長為10 m，白色矩形框為車輛位置，綠色為點云，粉色矩形框為感興趣矩形區域，橫向寬為5 m，縱向高為4 m，青色表示感興趣矩形區域的坡度為正，紅色表示感興趣矩形區域的坡度為負。

2.4 試驗結果分析與精度評價

為了盡量減小由于路面不平整和坡度大導致的車輛俯仰運動帶來的誤差，真值通過以下方式獲得。記錄每一組試驗的起點，以該起點作為真值獲得的起點，車輛每走大約1 m左右，停下來讀取此時的INS俯仰角作為當前所在位置的道路坡度。這種獲得真值的方式由于在車輛靜止的時候讀取INS俯仰角，所以在極大程度上減小了由于路面不平整和坡度大導致的車輛俯仰運動帶來的誤差。同時，為了驗證GKSRD方法的有效性，本文進行的對比試驗包括：以GKSRD方法的提取頻率實時讀取的INS俯仰角以及融合GNSS的LOAM算法，LOAM是目前主流的SLAM算法。

目前的一些基于激光雷達的方法只能提取相對于車體坐標系的道路坡度，所以對比試驗結果分析中沒有加入基于激光雷達的方法。同時，基于卡爾曼濾波器或龍伯格(Luenberger)觀測器等的方法無法進行實時檢測，所以也沒有加入對比試驗結果分析。

圖6(a)、圖6(b)、圖6(c)和圖6(d)依次為LOAM算法在場景3(a)中的道路a、b、c、d的建圖結果，試驗結果顯示，LOAM算法在圖6(a)中矩形圈出來的路段無法正確匹配，在圖6(b)、圖6(c)和圖6(d)中大部分路段都無法正確匹配。圖6(e)、圖6(f)圖6(g)和圖6(h)依次為LOAM算法在場景3(b)中的道路e、f、g、h的建圖結果，試驗結果顯示，LOAM算法在圖6(e)、圖6(f)和圖6(g)中矩形圈出來的路段無法正確匹配，在圖6(h)中可以正確匹配。由于LOAM算法在場景3(a)中的大部分路段無法正確匹配，導致測量的道路坡度無法輸出正常值，所以場景3(a)的對比試驗結果中不加入LOAM算法，而在場景3(b)中，只有部分路段無法正確匹配，測量的道路坡度可以輸出正常值，所以場景3(b)的對比試驗結果中加入了LOAM算法。

圖7(a)、圖7(b)、圖7(c)和圖7(d)依次為場景3(a)中的道路a、b、c、d的對比試驗結果，圖7(e)、圖7(f)圖7(g)和圖7(h)依次為場景3(b)中的道路e、f、g、h的對比試驗結果。橫軸表示每次試驗的里程，縱軸表示對應里程的道路坡度。其中紅色曲線為在車輛運動過程中以GKSRD方法的提取頻率直接實時讀取的INS的俯仰角，藍色曲線為GKSRD方法提取的道路坡度，青色曲線為LOAM算法測量的道路坡度，綠色曲線為采集的道路坡度真值。分析圖7(a)、圖7(b)、圖7(c)、圖7(d)、圖7(e)、圖7(f)、圖7(g)和圖7(h)可以發現8張圖中的道路坡度變化范圍都比較大，坡度絕對值最小為0°，最大可以達到11°，同時GKSRD方法與真值的擬合程度最高。分析圖7(e)、圖7(f)、圖7(g)和圖7(h)可以發現大部分LOAM的測量結果誤差與直接實時讀取的INS的俯仰角的誤差相近，而前者在部分路段的誤差要比后者更大。對比圖7(a)、圖7(b)、圖7(c)、圖7(d)與圖7(e)、圖7(f)、圖7(g)和圖7(h)，可以發現圖7(e)、圖7(f)、圖7(g)和圖7(h)中的曲線波動更劇烈，也驗證了場景3(b)的道路更加不平整，同時，圖7(e)、圖7(f)、圖7(g)和圖7(h)中通過直接讀取INS的俯仰角作為道路坡度的誤差比圖7(a)、圖7(b)、圖7(c)、圖7(d)更大，但是本文提出的GKSRD方法在兩個場景中的誤差是穩定的，說明GKSRD方法具有較強的環境適應性。

表3列出了INS俯仰角作為道路坡度、LOAM測量道路坡度和利用GKSRD方法提取道路坡度在8組試驗中的平均誤差和最大誤差。觀察表中數據，可以發現INS俯仰角作為道路坡度在所有試驗中的平均誤差都大于0.07°，LOAM測量的道路坡度在所有試驗中的平均誤差都大于0.1°，而GKSRD方法在所有試驗中的平均誤差都小于0.01°，INS俯仰角在所有試驗中的最大誤差都大于1.6°，LOAM測量的道路坡度在所有試驗中的最大誤差都大于2.0°，而GKSRD方法在所有試驗中的最大誤差都小于0.5°，說明本文提出的GKSRD方法精度更高，同時具有較強的穩定性。

表3 3種方法在8組試驗中的誤差

圖6 LOAM建圖結果Fig.6 Results of LOAM

圖7 道路坡度離線提取結果Fig.7 Extracted off-line result of road slope

3 結論與展望

露天礦山道路坡度大，不平整，目前的一些方法在提取露天礦山道路坡度時精度比較低，本文針對目前存在的挑戰提出了GKSRD方法，該方法在很大程度上減小了由于道路坡度大和不平整導致的道路坡度測量誤差。相比于目前的一些方法，GKSRD方法的精度更高，穩定性和環境適應性也更高。但是GKSRD方法存在幾個不足：不適用于拐彎小，左右傾斜程度大的道路；未考慮多傳感器的時間同步；真值獲得效率很低。在未來的研究中，會考慮露天礦山道路的一些極端情況，同時進行多傳感器的時間同步，并且尋找更好的真值獲得方法。