[发明专利]一种基于多线激光雷达的车辆运动轨迹估计方法及系统

权利要求书

1.一种基于多线激光雷达的车辆运动轨迹估计方法，其特征在于，包括：

获取车辆的初始位姿和惯性传感器的测量值，根据所述初始位姿和测量值得到车辆的粗选运动轨迹；

获取多线激光雷达不同时刻的三维点云数据，分别对所述不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图；

将所述粗选运动轨迹作为粗选范围，在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对；根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系；

根据所述变换关系，得到相邻时刻车辆的相对位姿，根据初始位姿和各个相邻时刻的相对位姿得到车辆的精确运动轨迹。

2.如权利要求1所述的车辆运动轨迹估计方法，其特征在于，根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系的步骤，包括：

从所述特征匹配点对中任取三对不同线的特征匹配点对，通过该三对特征匹配点对计算出所述相邻时刻的三维点云数据之间的变换关系。

3.如权利要求1所述的车辆运动轨迹估计方法，其特征在于，所述分别对所述不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图的步骤，包括：

建立目标栅格平面，分别将多线激光雷达不同时刻的三维点云数据投射到所述目标栅格平面上，标记出所述三维点云数据与每个栅格之间的映射关系，并根据所述映射关系将三维点云数据映射到所述灰度图中作为灰度图中的特征点。

4.如权利要求3所述的车辆运动轨迹估计方法，其特征在于，所述标记出所述三维点云数据与每个栅格之间的映射关系，并根据所述映射关系将三维点云数据映射到所述灰度图中作为灰度图中的特征点的步骤，包括：

在目标栅格平面上形成与三维点云数据对应的点阵；点阵中的点与三维点云数据中的点对应；计算目标栅格平面上每个栅格内所有点的灰度值的平均值，将有灰度的栅格作为特征点得到灰度图；其中，所述点的灰度值由三维点云数据的回波强度得到。

5.如权利要求1所述的车辆运动轨迹估计方法，其特征在于，所述在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对的步骤，包括：

在粗选范围内通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对，再采用随机抽样一致性算法从所述候选特征匹配点对中去除无效的特征匹配点对，得到所述特征匹配点对。

6.如权利要求5所述的车辆运动轨迹估计方法，其特征在于，所述通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对的步骤，包括：

对于任意相邻时刻的两帧灰度图，取其后一帧灰度图中的每一个特征点，计算该特征点与前一帧灰度图中每个特征点之间的欧氏距离，再计算出最小欧氏距离和次小欧氏距离的比值，如果所述比值小于预设的阈值，则认为最小欧式距离对应的这一对特征点为候选特征匹配点对。

7.一种基于多线激光雷达的车辆运动轨迹估计系统，其特征在于，包括：

粗选运动轨迹获取模块，用于获取车辆的初始位姿和惯性传感器的测量值，根据所述初始位姿和测量值得到车辆的粗选运动轨迹；

栅格化处理模块，用于获取多线激光雷达不同时刻的三维点云数据，分别对所述不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图；

初始配准模块，用于将所述粗选运动轨迹作为粗选范围，在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对；根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系；

精确运动轨迹获取模块，用于根据所述变换关系，得到相邻时刻车辆的相对位姿，根据初始位姿和各个相邻时刻的相对位姿得到车辆的精确运动轨迹。