一种融合点云强度以及地面约束的无人车建图方法及相关装置

本发明涉及一种融合点云强度以及地面约束的无人车建图方法及相关装置，属于无人车激光同时定位与建图。

背景技术：

1、对于移动机器人的自主导航以及三维重建领域中，最重要的是知道自身的位置以及构建周围的环境地图。移动机器人的发展，可以极大的解放传统生产制造业的人力物力成本，可以代替人类在高危环境中完成危险复杂任务，同时对于物流、出行领域的自动化、智能化变革也具有重大意义。在未知环境下，环境建模与定位是一个“鸡生蛋蛋生鸡”的问题：为了实现精准的定位，机器人需要知道环境地图(或特征)来不断修正自身的位姿。而为了建立环境地图，机器人又需要实时确定自身位姿。所以这类问题通常又被称为即时定位与建图(simultaneous localization and mapping，slam)技术。

2、slam技术可以分为激光和视觉两大类。视觉传感器获取的信息丰富且成本较低，但是受光照影响会导致图像质量下降且计算量较大，所以视觉slam一般只能构建相对稀疏的点云；激光雷达传感器因其高精度和高速扫描的特性被广泛使用，并且能够测量每个点的深度信息，测量距离也较远，受周围环境影响小，但是价格相对较高，线数越多价格越高，所以对于用相对低成本的16线激光雷达构建出精确鲁棒的点云地图的研究较多，也一直是个较大的难点。

3、目前对于激光slam的建图技术已经有了很多的研究。主流的建图方法基于点云的几何特征提取特征点，再进行匹配，后端优化，但是对于复杂的室外环境，如果不融合别的传感器或者加入别的约束就会导致匹配的效果很差，优化的效果也会大打折扣，造成很大的漂移。如中国专利“cn111583369a”一种基于面线角点特征提取的激光slam方法，仅提取了点云的面线角点特征，在小范围相对规则的环境下可能效果较好，但是室外道路凹凸不平，特征也相对复杂较多，对于特征的提取和匹配就会造成很大的困难，精度肯定也会受影响。中国专利“cn115639570a”融合点云强度特征和点云几何特征的机器人定位和建图方法，在几何特征的基础上融合了激光强度，减少了机器人的漂移问题，但是运用了点云机器学习分类模型，残差的计算量较大，影响系统的实时性，也仅适合在平稳的环境中进行建图，当运动较快或者路面崎岖就导致定位和建图的不准确，因此迫切需要突出新的方法加以解决。

技术实现思路

1、无人车在室外环境中难免会经过凹凸不平的道路，当入射角较大时，激光雷达测量的深度信息将会产生偏差。因此当无人车在道路上移动时，从道路上观测到的点云略有弯曲，将会导致激光雷达里程计估计的轨迹沿垂直方向漂移；传统的slam算法又忽视了点云强度信息，强度信息与材料的反射率有关，并且对于不同的物体是不同的，这可以用于定位和物体识别。

2、目的：鉴于以上技术问题中的至少一项，本发明提供一种融合点云强度以及地面约束的无人车建图方法及相关装置,在提取特征时融合了强度信息，提出了计算量更小，计算更准确的强度残差构建方式，与地面约束和几何约束共同构建残差来进行位姿的求解，获得更精确的位姿，最终提高大范围室外环境下无人车建图与定位的精度和鲁棒性。

3、本发明采用的技术方案为：

4、第一方面，本发明提供一种融合点云强度以及地面约束的无人车建图方法，包括：

5、获取当前帧激光雷达点云数据；其中所述点云数据包括每个点的三维坐标以及强度值；

6、利用体素滤波方法对所述点云数据进行下采样；

7、对下采样后的点云数据进行地平面点云提取，其中所述地平面点云提取通过地面测量模型提取；

8、根据提取到的地平面点云构建地面约束，其中所述地面约束采用局部条件性地面约束，选择性地加入地面约束；

9、使用强度加权欧式聚类方法对非地平面点云进行聚类；其中所述非地平面点云为下采样后的点云数据去除地平面点云后剩下的点云；

10、针对聚类后的非地平面点云，计算点的局部平滑度，根据局部平滑度从聚类后的非地平面点云中提取边缘特征和平面特征；根据构建的强度图获取点的强度推算值；

11、基于地面约束、边缘特征、平面特征和点的强度推算值，构建无人车姿态估计优化模型，其中所述无人车姿态估计优化模型以最小化边缘残差、平面残差、强度残差以及地面约束残差为目标；利用所述无人车姿态估计优化模型估计得到当前帧无人车的姿态；

12、使用滑动窗口优化当前帧和前一帧无人车的姿态得到当前帧无人车的位姿，根据当前帧无人车的位姿利用当前帧中边缘特征和平面特征更新全局地图；

13、依次处理所有帧激光雷达点云数据，得到最终稠密点云地图。

14、在一些实施例中，所述点云数据通过激光雷达传感器获取，由无人车搭载16线三维激光雷达来实现。

15、在一些实施例中，利用体素滤波方法对所述点云数据进行下采样，包括：

16、创建一个三维体素格，将基于所述点云数据的点云空间划分为一系列立方体，每个立方体为一个体素；

17、对于每个体素，选择离体素中心最近的点作为采样点，将采样点作为该体素代表点；

18、将所有体素中的体素代表点集合起来，得到下采样后的点云数据p。

19、在一些实施例中，构建地面约束，包括：

20、设上一个地平面点云的平面方程为：

21、ax+by+cz＝d   (1)

22、其中，a2+b2+c2＝1，d＞0为平面的距离，(a,b,c)为平面法向量；x,y,z为地平面上点的三维坐标；

23、当前地平面点云中的一个点pi(xi,yi,zi)到上个平面的距离为di，得到一个代价函数：

24、

25、其中，fg(pi)为地面约束残差，由此就构建好了地面约束，选择性地加入地面约束，称为条件性地面约束，一旦满足(1)-(2)中任意一个条件就不添加，条件如下：

26、(1)计算地平面的法线与无人车坐标系的z轴之间的夹角角度；如果夹角角度大于角度阈值，则认为地平面与运动平面不匹配；

27、(2)计算在地平面点云中最远的点和最近的点相对于无人车的距离；如果最远的点相对于无人车的距离低于第一阈值，最近的点相对于无人车的距离高于第二阈值，则认为地平面与运动平面不匹配。

28、在一些实施例中，使用强度加权欧式聚类方法对非地平面点云进行聚类，包括：

29、将距离小于设定阈值的一定数量的点云聚类于集合q，然后对于这些点，根据每个点和它周围最近4个点强度值计算这个点的权重，剔除权重较大的设定数量点，得到聚类后的非地平面点云；

30、权重公式如下:

31、

32、其中，p1为根据距离聚类得到的15个点中的一个，wp1为点p1的权重，ip1为点p1的强度值，q1、q2、q3、q4为p1周围最近的四个点。

33、在一些实施例中，针对聚类后的非地平面点云，计算所有点的局部平滑度ci，根据局部平滑度从聚类后的非地平面点云中提取边缘特征fie和平面特征fip，包括：

34、点的局部平滑度ci的计算方法包括：

35、

36、其中，分别为点pi的深度因子、强度因子；p为点pi的邻点集合q，ri、rj分别为点pi以及其相邻点pj的距离；

37、深度因子公式为：

38、

39、强度因子公式为：

40、

41、其中，g为增益参数，r0为临界距离，ii为点pi的强度值，i0为临界强度；根据点的局部平滑度ci进行排序，将局部平滑度ci较大的点作为边缘特征fie，将局部平滑度ci较小的点作为平面特征fip。

42、在一些实施例中，根据构建的强度图获取点的强度推算值，包括：

43、步骤4.3：针对聚类后的非地平面点云，在点pi周围寻找6个三维栅格单元构建一个球形强度图，每个栅格单元都是该区域的平均强度，每个栅格单元的强度ik计算公式如下：

44、

45、其中，ij表示每个区域内包含点的强度，i0表示最近点的强度，s表示6个区域中的一个，n表示该区域内的点数；

46、假设第1个栅格点坐标为p1(x1,y1,z1)，第6个栅格点的坐标为p6(x6,y6,z6)，利用均值法获得点pi的强度推算值

47、

48、在一些实施例中，其中所述无人车姿态估计优化模型，包括：

49、

50、其中，t*为无人车姿态估计结果，fie、fip分别为边缘特征和平面特征，fe(pi)为边缘残差、fp(pi)为平面残差、fτ(pi)为强度残差，p为下采样后的点云数据，fg(pi)为地面约束残差，为地平面点云。

51、进一步地，边缘残差fe(pi)公式如下：

52、

53、其中，pi为给定的边缘特征点，为从全局地图中搜索最近的两个边缘特征点；

54、平面残差fp(pi)公式如下：

55、

56、其中，pi为给定的平面特征点，为从全局地图中搜索最近的三个平面特征点；

57、强度残差fτ(pi)公式如下：

58、

59、其中，为点pi的强度测量值，为点pi的强度推算值。

60、第二方面，本发明提供了一种融合点云强度以及地面约束的无人车建图装置，包括处理器及存储介质；

61、所述存储介质用于存储指令；

62、所述处理器用于根据所述指令进行操作以执行根据第一方面所述的方法。

63、第三方面，本发明提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现第一方面所述的方法。

64、第四方面，本发明提供了一种设备，包括，

65、存储器；

66、处理器；

67、以及

68、计算机程序；

69、其中，所述计算机程序存储在所述存储器中，并被配置为由所述处理器执行以实现上述第一方面所述的方法。

70、有益效果：本发明提供的融合点云强度以及地面约束的无人车建图方法及相关装置，具有以下优点：提供的融合建图的方法能够解决在室外大范围环境下因地面凹凸不平导致的点云图往z轴方向漂移的问题，并且通过改进强度图构建的方法，计算效率更高，系统实时性更好，并得到高精度的稠密点云地图。

71、本发明能够应用在测绘、三维重建和自动驾驶行业，通过无人车搭载此系统的激光雷达，通过控制车辆，可以在相对复杂的环境，比如工业园区、校园、小区等环境实时构建出周围环境的高精地图，也可以在道路上建图为自动驾驶行业提供高精度地图。