面向无人驾驶矿车的露天矿山道路坡度实时检测方法
孟德将1
1. 北京慧拓无限科技有限公司, 北京 100190;
2. 中国科学院自动化研究所复杂系统管理与控制国家重点实验室, 北京 100190;
3. 中国科学院大学人工智能学院, 北京 100190;
4. 中国中煤能源集团有限公司, 北京 100120;
5. 安徽马钢矿业资源集团南山矿业有限公司, 马鞍山 243031;
6. 中山大学数据科学与计算机学院, 广州 510006
基金项目：广东省重点领域研发计划（2020B0909050001）
关键词：无人驾驶矿车 道路坡度实时检测 三维激光雷达 露天矿山
引文格式：孟德将, 田滨, 蔡峰, 等. 面向无人驾驶矿车的露天矿山道路坡度实时检测方法[J]. 测绘学 ，2021，50(11)：1628-1638. DOI:
10.11947/j.AGCS.2021.20210242
MENG Dejiang, TIAN Bin, CAI Feng, et al. Road slope real-time detection for unmanned truck in surface mine[J]. Acta Geodaetica et Cartographica Sinica, 2021, 50(11): 1628-1638. DOI: 10.11947/j.AGCS.2021.20210242
阅读全文：
http://xb.sinomaps.com/article/2021/1001-1595/2021-11-1628.htm
引 言
智慧矿山已经成为矿山发展的方向。露天矿山道路坡度变化范围大，无人驾驶矿车容易发生因下坡急减速导致的物料外撒或因上坡导致的溜车等危险。无人驾驶矿车需要精确检测车辆前方一定范围内的道路坡度，辅助速度规划合理规划速度，才能避免危险的发生。无人驾驶矿车依据道路坡度合理规划速度后，还可以提升运输的效率。
目前可以用于无人驾驶矿车实时检测露天矿山道路坡度的方法主要分为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姿态角信息，无法检测道路相对于水平面的坡度，所以，在坡度小且平整的结构化场景中的道路坡度检测精确度还比较高，而在露天矿山场景下，则对道路坡度的检测精确度就比较差。
1 栅格卡尔曼道路坡度实时检测(GKSRD)方法
1.1 二维高度栅格地图构建
图 1 道路坡度实时检测示意Fig. 1 A diagram of road slope real-time detection
图选项
INS和激光雷达联合标定后，可以获得点云在INS坐标系下的三维坐标，假设INS坐标系下的一帧点云表示为PI={P0I,P1I, …,PiI, …,PnI}，PiI=[XiI,YiI,ZiI]，将该点云在INS水平坐标系下表示为PL={P0L,P1L, …,PiL, …,PnL}，PiL=[XiL,YiL,ZiL]，则有
(2)
式中，λ1为INS相对于车底板的角度安装误差；λ2为由于车辆俯仰运动产生的车底板与路面的夹角；λ3为车辆所在道路的坡度；λ1+λ2+λ3为INS的俯仰角。
根据式(3)将PL投影到二维栅格地图，可以获得二维高度栅格地图
式中，Zindex为索引index的栅格的高度；其中index与i满足
(4)
式中，
1.2 感兴趣矩形区域迭代优化算法
建立了二维高度栅格地图后，即可进行道路坡度计算。首先需要确定栅格地图中的感兴趣矩形区域。为了使感兴趣矩形区域中有尽量多的值不为0的栅格，感兴趣矩形区域选择在车辆前方道路点云比较密集的区域。根据实际情况，道路的宽度必然大于无人驾驶矿车的宽度，所以感兴趣矩形区域的宽一般略大于车宽。如图 2所示，A与B分别为近处和远处的感兴趣矩形区域，组成了选定的感兴趣矩形区域。A与B的大小相同，位置相邻，且在二维栅格地图坐标系中的位置固定。
图 2 二维栅格地图Fig. 2 2D grid map
图选项
确定了二维栅格地图中的感兴趣矩形区域后，根据二维栅格地图矩形区域迭代优化算法可以分别计算A与B的几何中心位置(XA,YA)与(XB,YB)处的高度ZA和ZB。算法的输入包括二维高度栅格地图、矩形区域边界、初始迭代次数、迭代次数阈值、初始偏移量阈值、偏移量步长及标准差阈值。初始偏移量阈值、偏移量步长与标准差阈值配合筛选每次迭代时偏离均值较远的栅格。然后基于直角三角形模型，由式(5)可确定感兴趣区域A+B的几何中心位置的初始坡度
(5)
二维栅格地图矩形区域迭代优化算法如下。
输入：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: for i←minrow to maxrow step 1 do
9: for j←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
18:
19: if stdev>stdevthre and count < countthre and offsethre>1 then
20: for i←minrow to maxrow step 1 do
21: for j←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)求得
第2步：测量更新。融合当前帧感兴趣矩形区域几何中心位置的道路坡度测量结果zk，根据式(8)、式(9)和式(10)可以获得当前帧感兴趣矩形区域几何中心位置的最终道路坡度测量更新结果x′k
2 试验与分析
2.1 试验场景
图 3 试验场景Fig. 3 Experimental scene
图选项
2.2 试验平台
图 4 试验平台Fig. 4 Experimental platform
图选项
表 1 RS-LIDAR-16性能参数Tab. 1 Performance parameters of RS-LIDAR-16
| 型号 | RS-LIDAR-16 |
| 波长/nm | 905 |
| 精度/cm | 2 |
| 测距 | 20 cm~150 m |
| 垂直角/(°) | -15~+15 |
| 水平角/(°) | 360 |
| 功耗/W | 9 |
表选项
表 2 OXTS Inertial+性能参数Tab. 2 Performance parameters of OXTS Inertial+
| 型号 | OXTS Inertial+ |
| 参数 | 使用RTK |
| 位置精确度/m | 0.02 |
| 速度精确度 | 0.05 km/h RMS |
| Roll/Pitch/(°) | 0.03 |
| Heading/(°) | 0.1 |
| 更新频率/Hz | 100 |
表选项
由表 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 试验结果
图 5 道路坡度实时检测结果Fig. 5 Real-time detected result of road slope
图选项
2.4 试验结果分析与精度评价
目前的一些基于激光雷达的方法只能提取相对于车体坐标系的道路坡度，所以对比试验结果分析中没有加入基于激光雷达的方法。同时，基于卡尔曼滤波器或龙伯格(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算法。
图 6 LOAM建图结果Fig. 6 Results of LOAM
图选项
图 7 道路坡度离线提取结果Fig. 7 Extracted off-line result of road slope
图选项
表 3 3种方法在8组试验中的误差Tab. 3 Error of the three methods in eight groups of experiment
| 试验编号 | INS俯仰角平均误差 | LOAM平均误差 | GKSRD平均误差 | INS俯仰角最大误差 | LOAM最大误差 | GKSRD最大误差 |
| 1 | 0.073 | 0.162 | 0.006 | 1.63 | 2.42 | 0.32 |
| 2 | 0.093 | 0.134 | 0.004 | 1.62 | 2.27 | 0.21 |
| 3 | 0.098 | 0.127 | 0.005 | 1.72 | 2.03 | 0.18 |
| 4 | 0.087 | 0.152 | 0.002 | 1.82 | 2.21 | 0.23 |
| 5 | 0.113 | 0.193 | 0.008 | 2.21 | 2.64 | 0.38 |
| 6 | 0.143 | 0.178 | 0.004 | 1.93 | 2.52 | 0.42 |
| 7 | 0.128 | 0.184 | 0.007 | 2.34 | 2.72 | 0.46 |
| 8 | 0.132 | 0.203 | 0.007 | 2.81 | 2.61 | 0.36 |
表选项
3 结论与展望
初审：张艳玲
复审：宋启凡
终审：金 君
往期推荐
资讯
○ 关于请在线填 地理信息产业经济运行监测2021年度数据的函
○ 重磅发布：2021年全国科技奖励获奖项目大全（省部级奖+社会力量奖）
○ 重磅发布：2021年全国科技奖励获奖项目大全（省部级奖+社会力量奖）
○ 武汉市自然资源和规划信息中心2021-2022年招聘15名应届毕业生和相关岗位社会人才公告
会议
○ 第三届中国空间数据智能学术会议SpatialDI 2022 （二号通知）
○ 第七届普适定位、室内导航与基于位置服务（UPINLBS 2022）国际会议（一号通知）
○ 会议延期通知｜教育部高等学校地理科学类专业教学指导委员会2021会议暨全国地理科学类学院院长论坛
○ 国土卫星遥感应用重点实验室第一届学术委员会第三次会议在京顺利召开
《测绘学 》
○《测绘学 》8篇论文入选“领跑者5000——中国精品科技期刊顶尖学术论文（F5000）”
○ 智能驾驶环境感知 | 邓晨：基于深度学习的语义SLAM关键帧图像处理
○ 智能驾驶环境感知 | 孙喜亮：面向高精度城市测绘的激光紧耦合SLAM方法
○ 智能驾驶环境感知 | 傅琛：结合行驶场景语义的轨迹-路 实时匹配方法
《测绘通 》
○ 地市级实景三维城市建设及应用
○ 面向上海城市数字化转型的新型测绘
○ 图像全站仪及图像测量发展与展望
○《测绘通 》2021年第12期目录
《北京测绘》
○《北京测绘》2021年第10期摘要推荐
○《北京测绘》2021年第9期摘要推荐
○《北京测绘》2021年第8期摘要推荐（下）
○《北京测绘》2021年第8期摘要推荐（上）
《测绘科学技术学 》
○ 摘要 |《测绘科学技术学 》2021年第2期摘要推荐
○ 摘要 |《测绘科学技术学 》2021年第4期摘要推荐
○ 摘要 |《测绘科学技术学 》2021年第5期摘要推荐
○ 摘要 |《测绘科学技术学 》2021年第6期摘要推荐
《地球信息科学学 》
○ 《地球信息科学学 》2021年第12期佳文推介
○ 《地球信息科学学 》2021年第11期佳文推介
○ 佳文推介 | 大数据城市通勤交通模型的构建与模拟应用
○ 专刊征稿：社会感知与地理大数据挖掘（征稿中）
《测绘工程》
○ 摘要 |《测绘工程》2021年第5期摘要推荐
○ 摘要 |《测绘工程》2021年第4期摘要推荐
○ 摘要 |《测绘工程》2021年第6期摘要推荐
○ 测绘教学 | 后疫情时代“线上+线下”混合教学模式创新——以测绘类课程视角
《中国空间科学技术》
○《中国空间科学技术》2021年第6期摘要
○《中国空间科学技术》2021年第5期摘要推荐
○ 摘要 |《中国空间科学技术》2021年第3期摘要推荐
○ 火卫二地形地貌探测综述
《卫星应用》
○《卫星应用》2021年第10期摘要
○ 摘要 |《卫星应用》2021年第9期摘要推荐
○ 摘要 |《卫星应用》2021年第8期摘要推荐
○ 综述 | 北斗系统应用趋势分析
《Journal of Geodesy and Geoinformation Science》
○《测绘学 （英文版）》专刊征稿 | 地图学与地球空间信息教育：理论与实践
声明：本站部分文章内容及图片转载于互联 、内容不代表本站观点，如有内容涉及侵权，请您立即联系本站处理，非常感谢！