本发明专利技术公开了一种自动驾驶技术领域的基于自适应结合CPD和ICP算法的激光扫描建图方法及系统，旨在解决现有技术中无法在不同场景结构自适应选择合适的算法来进行点云匹配的问题，其包括将上一帧的特征点云数据与当前帧的特征点云数据进行帧间匹配，在帧间匹配第一次迭代时计算场景特征变量和表面曲率来判断场景结构，根据判断结果决定是否采用CPD算法进行点云的粗配准，将当前帧的特征点云数据与上一帧的特征点云数据进行匹配得到激光里程计数据，对激光里程计数据结合CPD和ICP算法进行配准，通过配准后的点云数据进行车辆位姿的融合与地图的构建。本发明专利技术实现CPD算法和ICP算法的自适应结合，降低计算量和时间消耗。
【技术实现步骤摘要】
本专利技术涉及一种基于自适应结合cpd和icp算法的激光扫描建图方法及系统，属于自动驾驶。
技术介绍
1、自动驾驶技术正处于蓬勃发展的阶段，激光slam(simultaneous localizationand mapping)技术在自动驾驶领域中扮演着至关重要的角色。激光slam旨在通过使用激光雷达来实现车辆的同时定位和环境地图构建，从而使车辆能够准确地感知周围环境，实现精确定位和路径规划。
2、2014年ji zhang和sanjiv singh提出了基于点到线和点到面的特征匹配的loam(lidar odometry and mapping)算法，使用迭代最近点(icp)算法来进行点云配准。icp算法通过不断迭代优化的方式，将不同帧的点云数据对齐到同一个坐标系下，以实现精确的匹配和融合。然而，icp算法在处理广场、街道等非结构化场景时会因为难以提取可靠稳定的特征而出现退化问题，容易受到噪声、遮挡、表面变化等因素的影响，导致点云匹配困难度增加，定位精度和建图可靠性降低。
3、然而，通过基于变体高斯混合模型的相干点漂移(cpd：coherentpoint drift)算法，对点云数据进行特征提取，并使用概率模型进行匹配优化，能够处理非刚性变形、部分遮挡等问题，并且在配准精度上也有一定的优势。但是，全部帧使用cpd算法进行点云的粗配准是不合理的，因为cpd算法在提供更准确、更鲁棒的点云配准结果的同时，也带来了更大的计算代价。当场景处于结构化场景时只使用icp算法在提高运行效率以及精度上更有优势。
<
p>4、因此，在实际应用中，如何自适应地选择合适的算法来处理不同类型的场景仍然是一个挑战。
技术实现思路
1、本专利技术的目的在于克服现有技术中的不足，提供一种基于自适应结合cpd和icp算法的激光扫描建图方法及系统，实现cpd算法和icp算法的自适应结合，降低计算量和时间消耗。
2、为达到上述目的，本专利技术是采用下述技术方案实现的：
3、一方面，本专利技术提供一种基于自适应结合cpd和icp算法的激光扫描建图方法，包括以下步骤：
4、s1、获取激光雷达数据；
5、s2、对激光雷达数据进行预处理，得到特征点云数据；
6、s3、将当前帧的特征点云数据与上一帧的特征点云数据进行帧间匹配：
7、在帧间匹配第一次迭代时计算场景特征变量和表面曲率，根据场景特征变量和表面曲率判断是否采用cpd算法进行点云的粗配准；
8、当帧间匹配第二次迭代时，采用icp算法进行点云的配准；
9、当帧间匹配的迭代次数大于2时，利用配准后的当前帧特征点云数据对上一帧特征点云数据进行更新，得到更新后的上一帧特征点云数据，根据更新后的上一帧特征点云数据得到该帧的激光里程计数据；
10、s4、将当前帧的特征点云数据作为新一轮中上一帧的特征点云数据，将下一帧的特征点云数据作为新一轮中当前帧的特征点云数据，对二者进行帧间匹配，得到更新后的新一轮中上一帧的点云数据，根据更新后的新一轮中上一帧的点云数据得到该帧的激光里程计数据；
11、s5、重复步骤s3～s4，直至新一轮的当前帧为最后一帧，结合所有帧的激光里程计数据得到总激光里程计数据；
12、s6、对总激光里程计数据先使用cpd算法进行粗配准，再使用icp算法进行精配准，得到配准后的总激光里程计数据；
13、s7、通过配准后的总激光里程计数据进行车辆位姿的融合与地图的构建，输出车辆轨迹和地图。
14、进一步的，对激光雷达数据进行预处理，得到特征点云数据，包括：
15、对激光雷达数据进行运动去畸变，得到去畸变后的激光雷达数据；
16、对去畸变后的激光雷达数据进行离群点去除，得到去除离群点后的激光雷达数据；
17、对去除离群点后的激光雷达数据进行体素滤波预处理，并对其进行下采样，得到特征点云数据。
18、进一步的，对激光雷达数据进行运动去畸变，得到去畸变后的对激光雷达数据进行运动去畸变，得到去畸变后的激光雷达数据，包括：
19、通过估计一帧内每个点所在采集时刻的雷达位姿将一帧内所有点变换到统一坐标系下，并用估计进行时间插值，获得所有点所在采集时刻的雷达位姿，从而将点云转换到统一坐标系下，得到去畸变后的激光雷达数据，其表达式如下：
20、
21、其中，tk-1，i为第k-1帧点云数据中第i个点采集时刻的雷达位姿估计，tk-1，i为第k-1帧点云数据中第i个点的采集时刻，tk-1为第k-1帧的帧间运动，tk-1为第k-1帧的帧间运动的起始时间，tk为第k-1帧的帧间运动的结束时间。
22、进一步的，对去畸变后的激光雷达数据进行离群点去除，得到去除离群点后的激光雷达数据，包括：
23、计算去畸变后的激光雷达数据中每个点的曲率，其表达式如下：
24、
25、其中，c为曲率，l表示激光雷达坐标系，为激光雷达坐标系下第k帧中的第i个点，为激光雷达坐标系下第k帧中的第j个点，其为的邻域点，s表示第i个点的邻域点的集合；
26、根据每个点的曲率判断是否为离群点，若曲率大于设定值，则去除，否则保留，得到去除离群点后的激光雷达数据。
27、进一步的，对去除离群点后的激光雷达数据进行体素滤波预处理，并对其进行下采样，得到特征点云数据，包括：
28、将去除离群点后的激光雷达数据划分为立方体单元，每个立方体单元作为一个体素；
29、将距离体素中心最近的点作为采样点，组合所有采样点作为特征点云数据。
30、进一步的，所述帧间匹配包括点到线的特征匹配和点到面的特征匹配；所述点到线的特征匹配包括：
31、从上一帧的特征点云数据中的边缘点集合选取一点i，在当前帧的特征点云数据的边缘点集合中选取与点i最近的点j以及与点j相同激光扫描束的最近点l，将三个点分别记为和
32、连接jl，计算点i到线jl的最短距离，其表达式为：
33、
34、其中，dl为点i到线jl的最短距离；
35、所述点到面的特征配匹配包括：
36、从上一帧的特征点云数据的平面点集合中选取一点i，在当前帧的特征点云数据的平面点集合中选取与点i最近的点j以及和点j相邻扫描线中最近的点l，并在其相邻帧中选取和点j最相近的点m，将四个点分别记为和
37、连接jlm，计算点i到面jlm的最短距离，其表达式为：
38、
39、其中，dp为点i到面jlm的最短距离。
40、进一步的，在帧间匹配第一次迭代时计算场景特征变量和表面曲率，根据场景特征变量和表面曲率判断是否采用cpd算法进行点云的粗配准，包括：
41、计算场景特征变量，其表达式如下：
42、
43、其中，sfv为场景特征变量本文档来自技高网
...
【技术保护点】
1.一种基于自适应结合CPD和ICP算法的激光扫描建图方法，其特征在于，包括：
2.根据权利要求1所述的基于自适应结合CPD和ICP算法的激光扫描建图方法，其特征在于，对激光雷达数据进行预处理，得到特征点云数据，包括：
3.根据权利要求2所述的基于自适应结合CPD和ICP算法的激光扫描建图方法，其特征在于，对激光雷达数据进行运动去畸变，得到去畸变后的对激光雷达数据进行运动去畸变，得到去畸变后的激光雷达数据，包括：
4.根据权利要求2所述的基于自适应结合CPD和ICP算法的激光扫描建图方法，其特征在于，对去畸变后的激光雷达数据进行离群点去除，得到去除离群点后的激光雷达数据，包括：
5.根据权利要求2所述的基于自适应结合CPD和ICP算法的激光扫描建图方法，其特征在于，对去除离群点后的激光雷达数据进行体素滤波预处理，并对其进行下采样，得到特征点云数据，包括：
6.根据权利要求1所述的基于自适应结合CPD和ICP算法的激光扫描建图方法，其特征在于，所述帧间匹配包括点到线的特征匹配和点到面的特征匹配；
7.根据权利要求1所述的基于自适应结合CPD和ICP算法的激光扫描建图方法，其特征在于，在帧间匹配第一次迭代时计算场景特征变量和表面曲率，根据场景特征变量和表面曲率判断是否采用CPD算法进行点云的粗配准，包括：
8.根据权利要求7所述的基于自适应结合CPD和ICP算法的激光扫描建图方法，其特征在于，CPD算法进行粗配准的方法包括：
9.根据权利要求1所述的基于自适应结合CPD和ICP算法的激光扫描建图方法，其特征在于，当帧间匹配第二次迭代时，采用ICP算法进行点云的粗配准，包括：当帧间匹配第二次迭代时，采用ICP算法进行点云的配准，包括：
10.一种基于自适应结合CPD和ICP算法的激光扫描建图系统，其特征在于，包括处理器和存储介质；
...
【技术特征摘要】
1.一种基于自适应结合cpd和icp算法的激光扫描建图方法，其特征在于，包括：
2.根据权利要求1所述的基于自适应结合cpd和icp算法的激光扫描建图方法，其特征在于，对激光雷达数据进行预处理，得到特征点云数据，包括：
3.根据权利要求2所述的基于自适应结合cpd和icp算法的激光扫描建图方法，其特征在于，对激光雷达数据进行运动去畸变，得到去畸变后的对激光雷达数据进行运动去畸变，得到去畸变后的激光雷达数据，包括：
4.根据权利要求2所述的基于自适应结合cpd和icp算法的激光扫描建图方法，其特征在于，对去畸变后的激光雷达数据进行离群点去除，得到去除离群点后的激光雷达数据，包括：
5.根据权利要求2所述的基于自适应结合cpd和icp算法的激光扫描建图方法，其特征在于，对去除离群点后的激光雷达数据进行体素滤波预处理，并对其进行下采样，得到特征点云数据，包括：
6....
【专利技术属性】
技术研发人员：孙伟，叶健峰，张小瑞，郭邦祺，曾豪霆，王煜，段元鹏，沈欣怡，管菲，
申请(专利权)人：南京信息工程大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。