基于路牙特征提取和匹配的车辆定位方法及系统与流程

本发明涉及智能驾驶定位，具体地，涉及一种基于路牙特征提取和匹配的车辆定位方法及系统。

背景技术：

1、基于激光雷达的特征定位已经成为自动驾驶中高精定位中非常重要的一部分，尤其是当车辆需要进行高精度定位或者gps信号不好时，特征定位可以起到提高定位精度和稳定性的作用。特征定位方法常使用环境的线、面特征以及人工放置的固定特征。特征的鲁棒性和复杂度会直接影响定位的性能，路牙作为环境中常见的特征且其提取相对简单，可以提高车辆的横向高精度定位。

2、专利文献cn111323802a公开了一种车辆定位方法、装置及设备，涉及智能驾驶技术领域，尤其涉及定位技术领域。本技术公开的技术方案包括：获取车辆中的激光传感器采集得到的道路的点云数据，以及获取车辆的第一定位信息；对点云数据进行检测得到第一路沿信息；根据第一路沿信息，对第一定位信息进行修正，得到车辆的第二定位信息。但是该技术方案对激光雷达获取的路沿点云信度不高，因此后续修正结果的误差也会较大，其定位的精度和准确性还有提高的空间。

技术实现思路

1、针对现有技术中的缺陷，本发明的目的是提供一种基于路牙特征提取和匹配的车辆定位方法及系统。

2、第一方面，根据本发明提供的一种基于路牙特征提取和匹配的车辆定位方法，包括：

3、步骤s1：实时获取车辆激光雷达采集的点云数据，并获取最优路牙特征；

4、步骤s2：根据采集的点云数据构建路牙点云地图，并获取地图路牙特征；

5、步骤s3：将所述最优路牙特征和所述地图路牙特征进行特征匹配和非线性优化，最终获取优化后的车辆位姿。

6、优选地，所述步骤s1包括：

7、步骤s1.1：实时获取车辆激光雷达采集的点云数据，并对所述点云数据进行预处理选取感兴趣区域；

8、步骤s1.2：对所述感兴趣区域点云的每个线束点云分别进行粗候选点云提取并保存；

9、步骤s1.3：对所述粗候选点过滤，获取候选点云；

10、步骤s1.4：对候选点云进行多次拟合以及分析置信度，得到最优路牙特征。

11、优选地，所述步骤s1.1包括：

12、步骤s1.1.1：采集路牙，实时提取单帧点云并判断是否有采集处的地图输入，若否，则人工设置感兴趣区域；若是，则执行步骤s1.1.2；

13、步骤s1.1.2：通过所述实时提取的单帧点云和所述地图计算路牙的方向和距离车辆的距离；

14、步骤s1.1.3：根据所述路牙的方向和距离车辆的距离，将输入的地图通过定位初值转换到车体坐标系，并设置预设阈值筛选距离地图中路牙最近的一段点云，进而选出感兴趣区域，其中地图转换到车体坐标系的计算公式如下：

15、xbase＝r*xutm+t

16、式中，xbase为车体坐标系，xutm为横墨卡托格网系统坐标系，r和t分别表示定位初值的旋转矩阵和平移矩阵；

17、所述地图输入为输入地图世界坐标系，根据车辆位姿投影到车体坐标系；

18、所述地图包括高精地图或拓扑地图。

19、优选地，所述步骤s1.2包括：

20、步骤s1.2.1：将感兴趣区域点云的每个线束点云进行排序，并从车体坐标系开始逐渐滑窗进行栅格划分；

21、步骤s1.2.2：计算当前点坐标与车体坐标下点距离，并将所述点距离的倒数作为滑窗大小；

22、步骤s1.2.3：判断当前线束点云上两点之间的角度是否大于所述滑动窗口大小，若是，则触发步骤s1.2.3；若否，则从滑窗的预设位置再次进行所述判断；

23、步骤s1.2.3：将所述两点之间点作为一个栅格点，并根据预设栅格的高度阈值判断当前栅格是否为有效栅格，若是，则触发步骤s1.2.4；若否，则将所述栅格标记为废弃栅格；

24、步骤s1.2.4：对所述有效栅格内的相邻点进行高程变化计算生成粗候选点，计算公式如下：

25、angle＝tan-1(dz/dy)

26、式中，angle表示相邻点的高程变化，dz和dy分别表示相邻点之间的高度差和横向差；

27、重复触发步骤s1.2.3至步骤s1.2.4直到搜索完整线束点云上的点，并将所有的粗候选点保存

28、优选地，所述步骤s1.3中对所述粗候选点过滤的方式包括密度滤波和去离群点滤波。

29、优选地，所述步骤s1.4包括：

30、步骤s1.4.1：对过滤后的候选点进行ransac曲线拟合或直线拟合；

31、步骤s1.4.2：对拟合后的曲线或直线进行过滤，并筛选出当前拟合后的曲线或直线中置信度最高的线作为最优路牙点，进而完成最优路牙特征的提取；

32、所述过滤包括对拟合后的曲线或直线的长度、线束点云数量和曲率进行过滤，所述筛选包括根据拟合后的曲线或直线与车体的距离、曲线长度进行筛选。

33、优选地，所述构建路牙点云地图包括：

34、步骤s2.1：根据步骤s1中实时采集的点云，提取路牙点；

35、步骤s2.2：计算所述路牙点位姿；

36、步骤s2.3：通过slam构建路牙点云地图。

37、优选地，所述特征匹配和非线性优化包括：

38、步骤s3.1：获取路牙点云地图，并构建kd-tree存放；

39、步骤s3.2：通过初始位姿在kd-tree中以预设范围进行半径搜索，查找预设范围内所述地图路牙点；

40、步骤s3.3：将步骤s1获取的最优路牙特征点与所述地图路牙点做欧式距离最近临匹配，得到候选匹配点对集合如下：

41、

42、其中，n表示候选匹配点对数量，x表示路牙点前向坐标，dy表示路牙点到地图点之间的距离；

43、步骤s3.4：对所述最近候选匹配点对集合进行直线拟合，去除错误候选匹配点对；

44、步骤s3.5：对过滤后的匹配点对和路牙点云地图，通过当前位姿再次进行欧式距离最近临匹配，设置距离阈值，剔除大于距离阈值的匹配点对，剩余匹配点对作为匹配点对集合；

45、步骤s3.6：通过所述匹配点对集合中所有匹配点对和位姿构造残差，公式如下：

46、

47、式中，e为构造的残差；n为匹配点对数量；p′map_i表示utm坐标系下第i个带误差的位姿投影到车体坐标系下的坐标；pbase_i表示车体坐标系下第i个点云坐标；r和t表示待优化量；

48、步骤s3.7：对所述位姿进行优化，最小化所述残差；

49、重复触发步骤s3.5至步骤s3.7直至满足预设的迭代次数和终止条件，得到优化后的位姿以及协方差矩阵。

50、第二方面，根据本发明提供的一种基于路牙特征提取和匹配的车辆定位系统，包括：

51、模块m1：实时获取车辆激光雷达采集的点云数据，并获取最优路牙特征；

52、模块m2：根据采集的点云数据构建路牙点云地图，并获取地图路牙特征；

53、模块m3：将所述最优路牙特征和所述地图路牙特征进行特征匹配和非线性优化，最终获取优化后的车辆位姿。

54、优选地，所述模块m1包括：

55、模块m1.1：实时获取车辆激光雷达采集的点云数据，并对所述点云数据进行预处理选取感兴趣区域；

56、模块m1.2：对所述感兴趣区域点云的每个线束点云分别进行粗候选点云提取并保存；

57、模块m1.3：对所述粗候选点过滤，获取候选点云；

58、模块m1.4：对候选点云进行多次拟合以及分析置信度，得到最优路牙特征。

59、优选地，所述模块m1.1包括：

60、模块m1.1.1：采集路牙，实时提取单帧点云并判断是否有采集处的地图输入，若否，则人工设置感兴趣区域；若是，则触发模块m1.1.2；

61、模块m1.1.2：通过所述实时提取的单帧点云和所述地图计算路牙的方向和距离车辆的距离；

62、模块m1.1.3：根据所述路牙的方向和距离车辆的距离，将输入的地图通过定位初值转换到车体坐标系，并设置预设阈值筛选距离地图中路牙最近的一段点云，进而选出感兴趣区域，其中地图转换到车体坐标系的计算公式如下：

63、xbase＝r*xutm+t

64、式中，xbase为车体坐标系，xutm为横墨卡托格网系统坐标系，r和t分别表示定位初值的旋转矩阵和平移矩阵；

65、所述地图输入为输入地图世界坐标系，根据车辆位姿投影到车体坐标系；

66、所述地图包括高精地图或拓扑地图。

67、优选地，所述模块m1.2包括：

68、模块m1.2.1：将感兴趣区域点云的每个线束点云进行排序，并从车体坐标系开始逐渐滑窗进行栅格划分；

69、模块m1.2.2：计算当前点坐标与车体坐标下点距离，并将所述点距离的倒数作为滑窗大小；

70、模块m1.2.3：判断当前线束点云上两点之间的角度是否大于所述滑动窗口大小，若是，则触发模块m1.2.3；若否，则从滑窗的预设位置再次进行所述判断；

71、模块m1.2.3：将所述两点之间点作为一个栅格点，并根据预设栅格的高度阈值判断当前栅格是否为有效栅格，若是，则触发模块m1.2.4；若否，则将所述栅格标记为废弃栅格；

72、模块1.2.4：对所述有效栅格内的相邻点进行高程变化计算生成粗候选点，计算公式如下：

73、angle＝tan-1(dz/dy)

74、式中，angle表示相邻点的高程变化，dz和dy分别表示相邻点之间的高度差和横向差；

75、重复触发模块m1.2.3至模块m1.2.4直到搜索完整线束点云上的点，并将所有的粗候选点保存。

76、优选地，所述模块m1.3中对所述粗候选点过滤的方式包括密度滤波和去离群点滤波。

77、优选地，所述模块m1.4包括：

78、模块m1.4.1：对过滤后的候选点进行ransac曲线拟合或直线拟合；

79、模块m1.4.2：对拟合后的曲线或直线进行过滤，并筛选出当前拟合后的曲线或直线中置信度最高的线作为最优路牙点，进而完成最优路牙特征的提取；

80、所述过滤包括对拟合后的曲线或直线的长度、线束点云数量和曲率进行过滤，所述筛选包括根据拟合后的曲线或直线与车体的距离、曲线长度进行筛选。

81、优选地，所述构建路牙点云地图包括：

82、模块m2.1：根据模块m1中实时采集的点云，提取路牙点；

83、模块m2.2：计算所述路牙点位姿；

84、模块m2.3：通过slam构建路牙点云地图。

85、优选地，所述特征匹配和非线性优化包括：

86、模块m3.1：获取路牙点云地图，并构建kd-tree存放；

87、模块m3.2：通过初始位姿在kd-tree中以预设范围进行半径搜索，查找预设范围内所述地图路牙点；

88、模块m3.3：将模块m1获取的最优路牙特征点与所述地图路牙点做欧式距离最近临匹配，得到候选匹配点对集合如下：

89、

90、其中，n表示候选匹配点对数量，x表示路牙点前向坐标，dy表示路牙点到地图点之间的距离；

91、模块m3.4：对所述最近候选匹配点对集合进行直线拟合，去除错误候选匹配点对；

92、模块m3.5：对过滤后的匹配点对和路牙点云地图，通过当前位姿再次进行欧式距离最近临匹配，设置距离阈值，剔除大于距离阈值的匹配点对，剩余匹配点对作为匹配点对集合；

93、模块m3.6：通过所述匹配点对集合中所有匹配点对和位姿构造残差，公式如下：

94、

95、式中，e为构造的残差；n为匹配点对数量；p′map_i表示utm坐标系下第i个带误差的位姿投影到车体坐标系下的坐标；pbase_i表示车体坐标系下第i个点云坐标；r和t表示待优化量；

96、模块m3.7：对所述位姿进行优化，最小化所述残差；

97、重复触发模块m3.5至模块m3.7直至满足预设的迭代次数和终止条件，得到优化后的位姿以及协方差矩阵。

98、与现有技术相比，本发明具有如下的有益效果：

99、1、本发明通过实时帧提取路牙特征，使用路牙特征点云进行车辆定位，能够减少定位横向误差，提高定位的稳定性和精度。

100、2、本发明通过使用激光雷达点云的ring特性，从局部出发寻找路牙候选点，再从全局拟合最优路牙。

101、3、本发明将点云有序化，使用滑动窗口高度变化动态寻找候选点，通过点云密度，离群点剔除等方法来剔除错误候选点，并多次对候选点进行拟合，设置不同约束条件作为拟合线置信度，得到最优拟合。

102、4、本发明能够兼容拓扑地图和点云地图。