一种基于时空连续性的矿山可通行区域识别方法和系统

本发明涉及自动驾驶感知，尤其涉及一种基于时空连续性的矿山可通行区域识别方法和系统。

背景技术：

1、得益于自动驾驶技术的不断进步以及对智能化和自动化的日益增长的需求，无人驾驶矿卡在矿山中迅速普及。与城市结构化道路场景相比，矿山道路场景的道路特征存在一定差异，包括激光雷达点云特征稀疏、道路边界模糊不清以及路面平整度不一致。这些差异为矿山场景自动驾驶技术带来了很多挑战。

2、无人驾驶矿卡的高效运行依赖于精确的道路可行驶区域的提取，这不仅是其感知和规划能力的核心，也是确保决策、控制和车辆安全运行的基础。三维激光雷达，凭借其高精度、广泛的感知范围和强大的鲁棒性，在点云分割任务中变得尤为重要。而矿山场景道路崎岖不平，道路边界不清且路面平整度不一，使得基于单一平面拟合的传统的可通行区域识别方法效果较差。加之激光雷达高线束化产生的大量数据，对点云分割算法的实时处理能力提出了更高的要求。因此，开发一种能够准确迅速且鲁棒地从矿山道路环境中分割提取可通行点的方法是具有研究价值及应用的工作。

3、近年来，有关三维激光雷达道路可通行区域识别任务提出了多种方法。基于高斯过程的方法和基于马尔可夫随机场的方法都取得了较好的可通行区域识别效果，但由于此类算法计算复杂度较高，在矿车上搭载的计算平台难以达到实时运行；此外，深度学习也在可通行区域识别任务中得到广泛应用，此类方法将三维点云投影为二维距离图像作为网络的输入，实现了准确高效的可通行区域识别结果，然而，矿山场景由于路面结果不规则，点云语义标签标注难度大，难以构建足以供网络训练的大规模数据集，因此该类算法在此场景下较难应用；基于2.5d高程图的可通行区域识别方法通过设定高程阈值进而判断栅格的可通行性，实现了高精度的可通行区域识别，然而，矿山场景下道路常常存在坡度，会导致栅格中高程差距较大超出高程阈值造成可通行区域识别错误。

技术实现思路

1、本发明的目的是为了解决上述现有技术中存在的缺点,而提供了一种基于时空连续性的矿山可通行区域识别方法和系统，通过“区域连通性-点连通性”两步判断方法以提高方法在非连通路面中的可通行性识别效果，并首次借助高程栅格构建时间稳定权重以实现不稳定点的滤除。

2、一方面，所述基于时空连续性的矿山可通行区域识别方法包括如下步骤：

3、s1：输入三维激光雷达点云，采用同心圆模型并通过对整体点云按照极性进行区域划分，通过自适应平面拟合判定每个划分好的所述区域是否为可通行区域，并对可通行点和非可通行点进行归类，输出为初始可通行点云集合和初始不可通行点云集合；

4、s2：通过连通性地面的区域滤波和点滤波两部分滤除所述初始可通行点云集合中不符合连通性要求的点云，输出为优化后的可通行点云集合和优化后的不可通行点云集合；

5、s3：输入当前帧点云相对于第一帧点云的位姿变换，通过时序地面滤除所述初始可通行点云集合中由于地面崎岖度变化而出现的不稳定的点云，输出为二次优化后的可通行点云集合和二次优化后的不可通行点云集合。

6、进一步地，输入的所述三维激光雷达点云记为p，p＝{p1,p2,p3,…}，考虑到激光雷达的点云密度会随距离增大而降低，所述同心圆区域划分按照距离雷达由远及近逐渐密集，实现所述点云数量在各区域均匀分布。

7、进一步地，在步骤s1中，所述自适应平面拟合具体包括：

8、s11：首先对每个所述区域分别使用主成分分析法你和局部平面，得到平面法向量n以及平面方程：

9、

10、其中，为区域中点云的质心；

11、s12：接着计算所述区域垂直度u及平坦度f:

12、

13、其中，z＝[0,0,1]为车体法向量，λ1、λ2、λ3为区域点云使用主成分分析得到的特征值且λ3≤λ2≤λ1；

14、s13：根据周围栅格情况对垂直度及平坦度阈值进行实时更新，对于m个区域的更新区域内，垂直度集合u和平整度集合f，垂直度阈值uτ和平坦度阈值的fτ更新公式：

15、uτ＝mean(u)+ku·std(u)，

16、fτ＝mean(f)+kf·std(f)，

17、其中，ku和kf均为常数。

18、优选地，由所述垂直度阈值uτ和平坦度阈值的fτ更新公式可知：

19、大于所述垂直度阈值和小于所述平坦度阈值的区域被判定为可通行区域，其中低于所述高程阈值的点归类到可通行点其他点归类到非可通行点

20、而不符合垂直度和平坦度要求的区域被判定为不可通行区域，其中所有点均归类到非可通行点

21、所有所述可通行点输出至所述初始可通行点云集合g，所有所述非可通行点输出至所述初始不可通行点云集合n。

22、进一步地，在步骤s2中，在所述区域滤波部分，定义所述可通行区域集合z＝{z1,z2,z3,…}，对于所述区域集合z中的每一个可通行区域元素，通过区域生长算法获取所述区域与车辆之间的连通性，所述区域滤波部分包括如下步骤：

23、s211：初始化阶段，任取所述区域集合z中的一个可通行区域zi，查询当前区域上下左右四个邻域的可通行性，将其中可通行的所述邻域假如区域集合l中，并继续迭代遍历所述集合l中的元素，在遍历全部区域元素后，若所述集合内存在车辆所在区域，则将全部区域标记为连通区域，否则标记为非连通区域；

24、s212：继续任选所述区域集合z中未被连通标记过的可通行区域zj，并重复步骤s211的初始化操作，直到遍历所述区域集合z中的全部元素；

25、s213：对于所述不符合连通性要求的所有所述非连通区域，将其更改为不可通行区域，所述区域中所有点标记为不可通行点。

26、更优地，在步骤s2中，所述点滤波部分具体包括如下步骤：

27、s221：将所述区域滤波得到的全部所述连通区域中的点构建为点集合c＝{c1,c2,c3,…}；

28、s222：使用基于密度的噪声应用空间聚类算法遍历点集合c中的所有点，划分出不符合密度要求的离群点，所述离群点由于周围相邻点密度较小、且连通性较低，因此将所述离群点归类为不可通行点，其余中心点仍保存为可通行点。

29、其中，通过连通性地面的所述区域滤波和所述点滤波后得到的所有所述可通行点输出至所述优化后的可通行点云集合g'，所有所述不可通行点输出至所述优化后的不可通行点云集合n'。

30、进一步地，步骤s3的所述时序地面滤波具体包括：

31、s31：通过输入的所述位姿及点云使用正态分布变换算法构建高程可通行栅格地图，表达式如下：

32、

33、其中，为时刻以来第i个栅格的累计点云数量，为t-1时刻以来第i个栅格的累计点云数量，为t时刻第i个栅格的点云数量,和为t时刻点云中第i个栅格的高程均值和方差，和为t-1时刻第i个栅格更新后的高程均值和方差，和为t时刻第个栅格估计的高程均值和方差；

34、s32：根据时间衰减公式计算每个栅格的稳定性将小于稳定性阈的占据状态；

35、s33：通过区域栅格投影获取该区域中所述稳定性栅格占据的比例k，将小于比例阈值kτ的区域视为不稳定区域，即将该区域中的所有点标记为不可通行点。

36、优选地，通过所述时许地面滤波得到的所有所述可通行点输出至所述二次优化后的可通行点云集合g”，所有所述二次优化后的不可通行点输出至所述不可通行点云集合n”。

37、其中，符合通过所述连通性地面滤波和所述时序地面滤波后得到的共同的可通行区域，以及最终输出的所述可通行点云集合为gfinal＝g'∩g”，最终输出的所述不可通行点云集合为nfinal＝p\gfinal。

38、另一方面，所述基于时空连续性的矿山可通行区域识别系统，包括：

39、自适应平面拟合模块，输入三维激光雷达点云，采用同心圆模型并通过对整体点云按照极性进行区域划分，通过所述自适应平面拟合模块判定每个划分好的所述区域是否为可通行区域，并对可通行点和非可通行点进行归类，输出为初始可通行点云集合和初始不可通行点云集合；

40、连通性地面滤波模块，通过所述连通性地面滤波模块的区域滤波和点滤波两部分滤除所述初始可通行点云集合中不符合连通性要求的点云，输出为优化后的可通行点云集合和优化后的不可通行点云集合；

41、时序地面滤波模块，输入当前帧点云相对于第一帧点云的位姿变换，通过所述时序地面滤波模块滤除所述初始可通行点云集合中由于地面崎岖度变化而出现的不稳定的点云，输出为二次优化后的可通行点云集合和二次优化后的不可通行点云集合。

42、与现有技术相比，本发明的有益效果是：

43、(1)本发明通过自适应平面拟合，针对矿山道路场景路面崎岖和难以使用单一平面拟合整体点云的问题，以实现点云数量在各区域的均匀分布；

44、(2)本发明通过连通性地面滤波中区域连通性和点连通性两步判断方法以提高方法在非连通路面中的可通行性识别效果，并借助高程栅格构建时间稳定权重以实现不稳定点的滤除；

45、(3)本发明通过时序地面滤波方法滤除初始可通行区域中由于地面崎岖度变化而出现的不稳定的点云。