基于路牙特征提取和匹配的车辆定位方法及系统技术方案

本发明专利技术提供了一种基于路牙特征提取和匹配的车辆定位方法及系统，包括：实时获取车辆激光雷达采集的点云数据，并获取最优路牙特征；根据采集的点云数据构建路牙点云地图，并获取地图路牙特征；将所述最优路牙特征和所述地图路牙特征进行特征匹配和非线性优化，最终获取优化后的车辆位姿。本发明专利技术通过实时帧提取路牙特征，使用路牙特征点云进行车辆定位，能够减少定位横向误差，提高车辆定位的稳定性和精度。精度。精度。

【技术实现步骤摘要】
基于路牙特征提取和匹配的车辆定位方法及系统


[0001]本专利技术涉及智能驾驶定位
，具体地，涉及一种基于路牙特征提取和匹配的车辆定位方法及系统。

技术介绍

[0002]基于激光雷达的特征定位已经成为自动驾驶中高精定位中非常重要的一部分，尤其是当车辆需要进行高精度定位或者GPS信号不好时，特征定位可以起到提高定位精度和稳定性的作用。特征定位方法常使用环境的线、面特征以及人工放置的固定特征。特征的鲁棒性和复杂度会直接影响定位的性能，路牙作为环境中常见的特征且其提取相对简单，可以提高车辆的横向高精度定位。
[0003]专利文献CN111323802A公开了一种车辆定位方法、装置及设备，涉及智能驾驶
，尤其涉及定位
本申请公开的技术方案包括：获取车辆中的激光传感器采集得到的道路的点云数据，以及获取车辆的第一定位信息；对点云数据进行检测得到第一路沿信息；根据第一路沿信息，对第一定位信息进行修正，得到车辆的第二定位信息。但是该技术方案对激光雷达获取的路沿点云信度不高，因此后续修正结果的误差也会较大，其定位的精度和准确性还有提高的空间。

技术实现思路

[0004]针对现有技术中的缺陷，本专利技术的目的是提供一种基于路牙特征提取和匹配的车辆定位方法及系统。
[0005]第一方面，根据本专利技术提供的一种基于路牙特征提取和匹配的车辆定位方法，包括：
[0006]步骤S1：实时获取车辆激光雷达采集的点云数据，并获取最优路牙特征；
[0007]步骤S2：根据采集的点云数据构建路牙点云地图，并获取地图路牙特征；
[0008]步骤S3：将所述最优路牙特征和所述地图路牙特征进行特征匹配和非线性优化，最终获取优化后的车辆位姿。
[0009]优选地，所述步骤S1包括：
[0010]步骤S1.1：实时获取车辆激光雷达采集的点云数据，并对所述点云数据进行预处理选取感兴趣区域；
[0011]步骤S1.2：对所述感兴趣区域点云的每个线束点云分别进行粗候选点云提取并保存；
[0012]步骤S1.3：对所述粗候选点过滤，获取候选点云；
[0013]步骤S1.4：对候选点云进行多次拟合以及分析置信度，得到最优路牙特征。
[0014]优选地，所述步骤S1.1包括：
[0015]步骤S1.1.1：采集路牙，实时提取单帧点云并判断是否有采集处的地图输入，若否，则人工设置感兴趣区域；若是，则执行步骤S1.1.2；
[0016]步骤S1.1.2：通过所述实时提取的单帧点云和所述地图计算路牙的方向和距离车辆的距离；
[0017]步骤S1.1.3：根据所述路牙的方向和距离车辆的距离，将输入的地图通过定位初值转换到车体坐标系，并设置预设阈值筛选距离地图中路牙最近的一段点云，进而选出感兴趣区域，其中地图转换到车体坐标系的计算公式如下：
[0018]X
base
＝R*X
utm
+T
[0019]式中，X
base
为车体坐标系，X
utm
为横墨卡托格网系统坐标系，R和T分别表示定位初值的旋转矩阵和平移矩阵；
[0020]所述地图输入为输入地图世界坐标系，根据车辆位姿投影到车体坐标系；
[0021]所述地图包括高精地图或拓扑地图。
[0022]优选地，所述步骤S1.2包括：
[0023]步骤S1.2.1：将感兴趣区域点云的每个线束点云进行排序，并从车体坐标系开始逐渐滑窗进行栅格划分；
[0024]步骤S1.2.2：计算当前点坐标与车体坐标下点距离，并将所述点距离的倒数作为滑窗大小；
[0025]步骤S1.2.3：判断当前线束点云上两点之间的角度是否大于所述滑动窗口大小，若是，则触发步骤S1.2.3；若否，则从滑窗的预设位置再次进行所述判断；
[0026]步骤S1.2.3：将所述两点之间点作为一个栅格点，并根据预设栅格的高度阈值判断当前栅格是否为有效栅格，若是，则触发步骤S1.2.4；若否，则将所述栅格标记为废弃栅格；
[0027]步骤S1.2.4：对所述有效栅格内的相邻点进行高程变化计算生成粗候选点，计算公式如下：
[0028]Angle＝tan
‑1(dz/dy)
[0029]式中，Angle表示相邻点的高程变化，dz和dy分别表示相邻点之间的高度差和横向差；
[0030]重复触发步骤S1.2.3至步骤S1.2.4直到搜索完整线束点云上的点，并将所有的粗候选点保存
[0031]优选地，所述步骤S1.3中对所述粗候选点过滤的方式包括密度滤波和去离群点滤波。
[0032]优选地，所述步骤S1.4包括：
[0033]步骤S1.4.1：对过滤后的候选点进行R
ansac
曲线拟合或直线拟合；
[0034]步骤S1.4.2：对拟合后的曲线或直线进行过滤，并筛选出当前拟合后的曲线或直线中置信度最高的线作为最优路牙点，进而完成最优路牙特征的提取；
[0035]所述过滤包括对拟合后的曲线或直线的长度、线束点云数量和曲率进行过滤，所述筛选包括根据拟合后的曲线或直线与车体的距离、曲线长度进行筛选。
[0036]优选地，所述构建路牙点云地图包括：
[0037]步骤S2.1：根据步骤S1中实时采集的点云，提取路牙点；
[0038]步骤S2.2：计算所述路牙点位姿；
[0039]步骤S2.3：通过SLAM构建路牙点云地图。
[0040]优选地，所述特征匹配和非线性优化包括：
[0041]步骤S3.1：获取路牙点云地图，并构建Kd
‑
tree存放；
[0042]步骤S3.2：通过初始位姿在Kd
‑
tree中以预设范围进行半径搜索，查找预设范围内所述地图路牙点；
[0043]步骤S3.3：将步骤S1获取的最优路牙特征点与所述地图路牙点做欧式距离最近临匹配，得到候选匹配点对集合如下：
[0044][0045]其中，N表示候选匹配点对数量，x表示路牙点前向坐标，dy表示路牙点到地图点之间的距离；
[0046]步骤S3.4：对所述最近候选匹配点对集合进行直线拟合，去除错误候选匹配点对；
[0047]步骤S3.5：对过滤后的匹配点对和路牙点云地图，通过当前位姿再次进行欧式距离最近临匹配，设置距离阈值，剔除大于距离阈值的匹配点对，剩余匹配点对作为匹配点对集合；
[0048]步骤S3.6：通过所述匹配点对集合中所有匹配点对和位姿构造残差，公式如下：
[0049][0050]式中，e为构造的残差；n为匹配点对数量；p
′
map_i
表示UTM坐标系下第i个带误差的位姿投影到车体坐标系下的坐标；p
base_i
表示车体坐标系下第i个点云坐标；R和t表示待优化量；
[0051]步骤S3.7：对所述位姿进本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于路牙特征提取和匹配的车辆定位方法，其特征在于，包括：步骤S1：实时获取车辆激光雷达采集的点云数据，并获取最优路牙特征；步骤S2：根据采集的点云数据构建路牙点云地图，并获取地图路牙特征；步骤S3：将所述最优路牙特征和所述地图路牙特征进行特征匹配和非线性优化，最终获取优化后的车辆位姿。2.根据权利要求1所述的基于路牙特征提取和匹配的车辆定位方法，其特征在于，所述步骤S1包括：步骤S1.1：实时获取车辆激光雷达采集的点云数据，并对所述点云数据进行预处理选取感兴趣区域；步骤S1.2：对所述感兴趣区域点云的每个线束点云分别进行粗候选点云提取并保存；步骤S1.3：对所述粗候选点过滤，获取候选点云；步骤S1.4：对候选点云进行多次拟合以及分析置信度，得到最优路牙特征。3.根据权利要求2所述的基于路牙特征提取和匹配的车辆定位方法，其特征在于，所述步骤S1.1包括：步骤S1.1.1：采集路牙，实时提取单帧点云并判断是否有采集处的地图输入，若否，则人工设置感兴趣区域；若是，则执行步骤S1.1.2；步骤S1.1.2：通过所述实时提取的单帧点云和所述地图计算路牙的方向和距离车辆的距离；步骤S1.1.3：根据所述路牙的方向和距离车辆的距离，将输入的地图通过定位初值转换到车体坐标系，并设置预设阈值筛选距离地图中路牙最近的一段点云，进而选出感兴趣区域，其中地图转换到车体坐标系的计算公式如下：X
base
＝T*X
utm
+T式中，X
base
为车体坐标系，X
utm
为横墨卡托格网系统坐标系，R和T分别表示定位初值的旋转矩阵和平移矩阵；所述地图输入为输入地图世界坐标系，根据车辆位姿投影到车体坐标系；所述地图包括高精地图或拓扑地图。4.根据权利要求2所述的基于路牙特征提取和匹配的车辆定位方法，其特征在于，所述步骤S1.2包括：步骤S1.2.1：将感兴趣区域点云的每个线束点云进行排序，并从车体坐标系开始逐渐滑窗进行栅格划分；步骤S1.2.2：计算当前点坐标与车体坐标下点距离，并将所述点距离的倒数作为滑窗大小；步骤S1.2.3：判断当前线束点云上两点之间的角度是否大于所述滑动窗口大小，若是，则触发步骤S1.2.3；若否，则从滑窗的预设位置再次进行所述判断；步骤S1.2.3：将所述两点之间点作为一个栅格点，并根据预设栅格的高度阈值判断当前栅格是否为有效栅格，若是，则触发步骤S1.2.4；若否，则将所述栅格标记为废弃栅格；步骤S1.2.4：对所述有效栅格内的相邻点进行高程变化计算生成粗候选点，计算公式如下：Angle＝tan
‑1(dz/dy)
式中，Angle表示相邻点的高程变化，dz和dy分别表示相邻点之间的高度差和横向差；重复触发步骤S1.2.3至步骤S1.2.4直到搜索完整线束点云上的点，并将所有的粗候选点保存。5.根据权利要求2所述的基于路牙特征提取和匹配的车辆定位方法，其特征在于，所述步骤S1.3中对所述粗候选点过滤的方式包括密度滤波和去离群点滤波。6.根据权利要求2所述的基于路牙特征提取和匹配的车辆定位方法，其特征在于，所述步骤S1.4包括：步骤S1.4.1：对过滤后的候选点进行Ransac曲线拟合或直线拟合；步骤S1.4.2：对拟合后的曲线或直线进行过滤，并筛选出当前拟合后的曲线或直线中置信度最高的线作为最优路牙点，进而完成最优路牙特征的提取；所述过滤包括对拟合后的曲线或直线的长度、线束点云数量和曲率进行过滤，所述筛选包括根据拟合后的曲线或直线与车体的距离、曲线长度进行筛选。7.根据权利要求1所述的基于路牙特征提取和匹配的车辆定位方法，其特征在于，所述构建路牙点云地图包括：步骤S2.1：根据步骤S1中实时采集的点云，提取路牙点；步骤S2.2：计算所述路牙点位姿；步骤S2.3：通过SLAM构建路牙点云地图。8.根据权利要求1所述的基于路牙特征提取和匹配的车辆定位方法，其特征在于，所述特征匹配和非线性优化包括：步骤S3.1：获取路牙点云地图，并构建Kd
‑
tree存放；步骤S3.2：通过初始位姿在Kd
‑
tree中以预设范围进行半径搜索，查找预设范围内所述地图路牙点；步骤S3.3：将步骤S1获取的最优路牙特征点与所述地图路牙点做欧式距离最近临匹配，得到候选匹配点对集合如下：其中，N表示候选匹配点对数量，x表示路牙点前向坐标，dy表示路牙点到地图点之间的距离；步骤S3.4：对所述最近候选匹配点对集合进行直线拟合，去除错误候选匹配点对；步骤S3.5：对过滤后的匹配点对和路牙点云地图，通过当前位姿再次进行欧式距离最近临匹配，设置距离阈值，剔除大于距离阈值的匹配点对，剩余匹配点对作为匹配点对集合；步骤S3.6：通过所述匹配点对集合中所有匹配点对和位姿构造残差，公式如下：式中，e为构造的残差；n为匹配点对数量；p'
map_i
表示UTM坐标系下第i个带误差的位姿投影到车体坐标系下的坐标；p
base_i
表示车体坐标系下第i个点云坐标；R和t表示待优化量；步骤S3.7：对所述位姿进行优化，最小化所述残差；
重复触发步骤S3.5至步骤S3.7直至满足预设的迭代次数和终止条件，得到优化后的位姿以及协方差矩阵。9.一种基于路牙...

【专利技术属性】
技术研发人员：廖文龙，郑顺凯，何弢，谢荣荣，
申请(专利权)人：上海酷移机器人有限公司，
类型：发明
国别省市：