一种基于改进RANSAC的多线程地面特征提取方法

本发明属于基于激光雷达的同时定位与建图(light detection andranging simultaneous localization and mapping，lidar slam)，具体是一种基于改进随机一致性采样(random sample consensus，ransac)的多线程地面特征提取方法。

背景技术：

1、随着时代的发展，同步定位与建图(simultaneous localization and mapping，slam)技术逐渐成为室内外定位的关键技术之一。slam技术根据传感器类型可分为：激光雷达(light detection and ranging，lidar)slam和视觉slam两种。相比于视觉传感器，lidar传感器能够获取丰富的、具有深度信息的点云数据，具有抗干扰能力强、不受光照变化影响等优点，现在已经成为实现室内外定位技术的关键传感器之一。按照适用场景，lidar slam可分为二维(two dimensional，2d)lidar slam和三维(three dimensional，3d)lidar slam。2d lidar slam可获取平台在2d平面的三自由度位姿和2d地图。到目前为止2d lidar slam经过了十几年的发展，现包括fastslam、hector-slam、gmapping等算法。相比于2d lidar slam，虽然3d lidar slam成本较高，并且数据处理量大，但是其在2dlidar slam通常无法工作的大型室外场景中更加可靠，因此3d lidar slam算法已成为室外复杂场景下定位与建图的首选方案之一。常见的3d lidar slam算法在前端多使用基于地面特征和聚类特征的配准方法来获得lidar里程计数据。其中，地面特征在室外场景中相对稳定，同时点云数量也多于聚类特征，因此提高地面特征提取的质量和精度，可以从根本上提高lidar slam特征配准精度。

2、目前，3d lidar slam算法多采用角度判断的方式从原始点云数据中进行地面特征提取，但是当lidar传感器(如16线)分辨率扫描线数较低的更小，利用lidar slam获取到的数据中地面点会大量减少。同时由于lidar slam算法会对原始点云进行过轻量级处理，导致在室外场景地面点分割过程中会出现过分割或欠分割情况，而影响定位精度。

3、在室外复杂场景下，目前常用的点云分割算法包括基于ransac的点云分割算法、基于区域增长(region growing，rg)的点云分割算法以及基于霍夫变换(houghtransform，ht)的点云分割算法。基于rg的点云分割算法原理简单、容易实现，但是对噪声非常敏感并且分割精度较低。基于ht的点云分割算法在复杂环境中的形状检测效果很好，但是存在耗时长、内存占用量大的缺点。与前两种算法相比，ransac算法可以鲁棒地获取模型参数，不但用于基础3d点云分割以及3d点云检测，而且可以和其他算法相结合来解决室外复杂场景下的点云分割问题。文献将ransac算法与深度学习算法相结合，可以对建筑物屋顶进行分割，并且屋顶点云完整度较高。因此利用ransac算法进行地面分割可以鲁棒地获取完整度较高的地面点云。但是由于传统ransac算法种子点筛选流程过于繁琐，并且无法保证种子点质量，将导致平面拟合精度和算法效率大幅度降低。

4、本发明针对lidar slam算法地面特征提取中存在特征提取准确度不高，以及ransac算法的缺点，提出了一种基于改进ransac的多线程地面分割算法，通过提高lidarslam地面特征提取质量和精度，进而提高lidar slam特征匹配精度。

技术实现思路

1、针对现有技术的不足，本发明提出一种基于改进ransac的多线程地面特征提取方法，利用点云高程阈值、曲率阈值等筛选种子点集合，确定初始地面模型，根据种子点数量确定处理模式，为确保数据处理的实时性，选择合适的线程处理模式利用改进ransac算法进行地面拟合。

2、一种基于改进ransac的多线程地面特征提取方法包括以下步骤：

3、步骤1、采集lidar传感器数据，搜索激光点云的近邻点集合；

4、步骤2、基于步骤1得到的近邻点集合，计算激光点云点曲率和夹角；

5、步骤3、基于步骤2得到的激光点云曲率、夹角以及激光点云高程，筛选种子点集合；

6、步骤4、基于步骤3得到的种子点集合，根据种子点数量，选择合适线程计算平面法向量；

7、步骤5、基于步骤4得到的平面法向量，计算激光点云中各激光点与平面之间的距离，判断激光点是否属于地面点，根据地面点数量得到最优地面模型以及地面点云。

8、步骤1所述在获得当前激光点云数据后，可利用多维二叉树(k-dimensionaltrees，kd-trees)对当前激光点云中各激光点的近邻点，按照距离值进行搜索，最终获取激光点云中各激光点的近邻点集合。

9、步骤2所述的利用当前激光点的近邻点集合计算当前激光点曲率和夹角；

10、具体步骤如下：

11、步骤2-1、假设当前激光点近邻点集合中包含n个点，各近邻点p(neighbor,i)坐标为(xi,yi,zi)(i＝1,2,…,n)；

12、计算当前点近邻点集合各坐标均值：

13、

14、其中分别为近邻点集合pneighbor中所有点的x、y、z三个坐标均值；

15、步骤2-2、基于步骤2-1计算得到的x、y、z三个坐标均值，构建计算近邻点集合pneighbor的协方差矩阵cneighbor：

16、

17、步骤2-3、基于步骤2-2计算得到的协方差矩阵，计算矩阵特征值λ及其对应的特征向量：

18、|cneighbor-λe|＝0         (3)

19、计算得到的特征值中，最大特征值λmax为当前激光点p(l,cur)所对应的曲率ccur；最小特征值λmin所对应的特征向量为当前激光点p(l,cur)的近似法向量

20、步骤2-4、基于步骤2-3得到的当前激光点法向量，计算当前激光点点p(l,cur)与lidar传感器坐标原点ol连线形成的向量与法向量的夹角值

21、

22、步骤3所述的将当前激光点的曲率、夹角以及当前激光点高程，分别与曲率阈值cth、夹角阈值和高程阈值hth进行比较，如果当前激光点三种数值均小于阈值，则将当前激光点加入种子点集合pseeds。

23、步骤4所述的利用种子点集合pseeds，根据种子点数量，选择合适线程计算平面法向量；

24、具体步骤如下：

25、步骤4-1、判断种子点集合中的种子点数量nseeds是否大于三倍线程数nthread，如果大于就将种子点集合分成nthread个子集，分nthread线程进行处理，各线程的处理方式相同，否则单线程处理；

26、步骤4-2、假设按照多线程进行处理并假设当前为ti(i＝1,2,…,nthread)线程，从ti线程种子点集合中随机选取三个点作为ti线程地面拟合的种子点；

27、根据公式(4)计算得到利用上述三个种子点拟合平面的平面法向量

28、

29、其中、为和两点构成的向量；为和两点构成的向量，法向量垂直于两个相交向量。

30、步骤5所述的利用平面法向量，计算当前激光点云中各激光点与平面之间的距离，判断激光点是否属于地面点，根据地面点数量得到最优地面模型以及地面点云；

31、具体步骤如下：

32、步骤5-1、假设当前激光点云中，除种子点外的剩余激光点为p(l,other)，根据公式(5)求得点到平面距离dother：

33、

34、其中，为当前平面外一点与平面内一点所构成的向量，用于求解当前剩余点到拟合平面的距离；

35、步骤5-2、当点到平面距离小于距离阈值dth时，则认为当前剩余点p(l,other)为地面点。遍历全部剩余点判断来筛选地面点并保存地面点数量，然后从种子点集合中不重复地选择三个种子点重新拟合平面。重复上述步骤，统计属于当前地面模型的地面点数量，最后将地面点数量最多的平面参数作为地面参数进行保存，对应的模型为最优地面模型，对应点云为地面点云。

36、本发明优点：

37、1、利用改进ransac地面特征提取算法对地面特征进行提取，与现有lidar slam算法的地面特征提取模块地面特征提取结果进行对比，本发明提取到的地面特征精度更高，完整度更好。

38、2、将本发明处理后的点云数据输入lego-loam进行定位与建图，相较于基于传统lego-loam算法输出结果，定位误差降低至3.73m，平面均方根误差降低了20.8％，定位精度更高。