一种基于增量式点云配准的建图方法、系统及计算机程序与流程

摘要: 本发明涉及视觉处理领域,尤其涉及一种基于增量式点云配准的建图方法、系统及计算机程序。背景技术:1、在传统的agv(automatedguidedvehicle,...
一种基于增量式点云配准的建图方法、系统及计算机程序与流程

本发明涉及视觉处理领域,尤其涉及一种基于增量式点云配准的建图方法、系统及计算机程序。

背景技术:

1、在传统的agv(automated guided vehicle,自动导引车)系统中,车辆定位模块大多采用磁条、2d激光导航、相机获取周围环境的二维图像。在室内环境下,利用二维图像可以正常实现定位;但一旦切换到室外,这些方法存在难部署、易失效、精确度低等问题,严重影响整个agv系统的正常工作。

2、当前的agv工作场景需求多变,往往存在室内、室外(动态、遮挡)场景均要稳定运行的需求,现有技术一般是通过提前采集点云信息,构建三维点云图像来实现定位。但这种方法需要提前构建地图,无法应对未知的环境,可扩展性低;且当周围环境出现变化时,三维点云图像无法及时更新,导致定位会出现差错。

技术实现思路

1、鉴于目前技术存在的上述不足,本发明提供一种基于增量式点云配准的建图方法,通过对激光雷达扫描的点云进行配准,获取关键帧,从而增量式地更新体素地图。

2、为达到上述目的,本发明的实施例采用如下技术方案:

3、一种基于增量式点云配准的建图方法,包括以下步骤:

4、获取初始帧点云和初始帧车辆位姿;

5、将初始帧作为关键帧,构建初始局部体素地图;

6、重复获取下一帧点云作为当前帧,基于点云配准和车辆位姿迭代优化,获取关键帧,增量式更新体素地图。

7、依照本发明的一个方面,所述基于增量式点云配准的建图方法还包括:

8、采用基于密度的聚类方法,对点云数据进行去噪处理。

9、依照本发明的一个方面,所述采用基于密度的聚类方法,对点云数据进行去噪处理包括:

10、设定邻域半径和核心点的最小邻域点数;

11、从任意一个未访问的数据点开始,遍历数据集,判断该点为核心点还是噪声点;

12、从核心点开始,递归地将所有在其邻域半径内的核心点添加到聚类中,直到无法再扩展。

13、将未在聚类中的噪声点去除。

14、依照本发明的一个方面,所述将初始帧作为关键帧,构建初始局部体素地图包括:

15、将初始帧作为关键帧,按照固定分辨率对关键帧点云进行体素化滤波,构建初始局部体素地图。

16、依照本发明的一个方面,所述重复获取下一帧点云作为当前帧,基于点云配准和车辆位姿迭代优化,获取关键帧,增量式更新体素地图包括:

17、获取下一帧点云作为当前帧,将其点云投射到体素地图中,获取当前位姿待优化方程;

18、利用拟牛顿法对当前位姿待优化方程进行迭代,直至收敛后更新当前位姿;

19、比较当前位姿和上一关键帧的车辆位姿,若当前位姿变化超过预设的阈值,则将当前帧作为关键帧,添加入体素地图中;

20、重复进行上述步骤,增量式更新体素地图。

21、依照本发明的一个方面,所述获取下一帧点云作为当前帧,将其点云投射到体素地图中,获取当前位姿待优化方程包括:

22、获取体素地图的每个体素中的点云高斯分布,得到所有体素的均值和方差;

23、基于上一帧车辆位姿推断当前帧车辆预测位姿;

24、根据当前帧车辆预测位姿、所有体素的均值和方差,获取当前帧点云与对应体素的残差;

25、根据当前帧车辆预测位姿、当前帧点云与对应体素的残差,获得当前位姿待优化方程。

26、依照本发明的一个方面,所述利用拟牛顿法对当前位姿待优化方程进行迭代,直至收敛后更新当前位姿包括:

27、选择一个初始点,并计算该点的梯度;

28、使用海瑟矩阵的近似矩阵更新梯度方向;

29、沿着更新后的梯度方向进行线搜索,找到一个新的点;

30、使用新的点和梯度信息更新海森矩阵的近似矩阵;

31、重复更新点和梯度,直至满足收敛条件,将当前点作为最优解。

32、依照本发明的一个方面,所述预设的阈值为:

33、当前帧与上一关键帧的距离变化的阈值为1m或角度变化的阈值为15度。

34、一种基于增量式点云配准的建图系统,基于如上所述的基于增量式点云配准的建图方法,包括:

35、初始数据获取模块,用于获取初始帧点云和初始帧车辆位姿;

36、初始地图构建模块,用于将初始帧作为关键帧,构建初始局部体素地图;

37、迭代模块,用于重复获取下一帧点云作为当前帧,基于点云配准和车辆位姿迭代优化,获取关键帧,增量式更新体素地图。

38、一种计算机程序,所述计算机程序被执行时实现如上所述的基于增量式点云配准的建图方法的步骤。

39、本发明实施的优点:

40、本发明提供一种基于增量式点云配准的建图方法,通过对激光雷达扫描的点云进行配准,获取关键帧,从而增量式地更新体素地图;通过选取关键帧更新体素地图的方式,免去点云数量多时寻找最近邻时间、提取点云特征时间、局部地图重新构建的时间,可以显著地减少消耗算力,提高定位模块运行效率,优化速度快,从而达到实时效果,可以让agv更加稳定高效的工作;同时由于使用了体素化方法,在环境不发生大幅度变化时仍可以达到较高的匹配精度,定位结果波动小。

1.一种基于增量式点云配准的建图方法,其特征在于,包括以下步骤:

2.根据权利要求1所述的基于增量式点云配准的建图方法,其特征在于,所述基于增量式点云配准的建图方法还包括:

3.根据权利要求2所述的基于增量式点云配准的建图方法,其特征在于,所述采用基于密度的聚类方法,对点云数据进行去噪处理包括:

4.根据权利要求1所述的基于增量式点云配准的建图方法,其特征在于,所述将初始帧作为关键帧,构建初始局部体素地图包括:

5.根据权利要求1所述的基于增量式点云配准的建图方法,其特征在于,所述重复获取下一帧点云作为当前帧,基于点云配准和车辆位姿迭代优化,获取关键帧,增量式更新体素地图包括:

6.根据权利要求5所述的基于增量式点云配准的建图方法,其特征在于,所述获取下一帧点云作为当前帧,将其点云投射到体素地图中,获取当前位姿待优化方程包括:

7.根据权利要求5所述的基于增量式点云配准的建图方法,其特征在于,所述利用拟牛顿法对当前位姿待优化方程进行迭代,直至收敛后更新当前位姿包括:

8.根据权利要求5所述的基于增量式点云配准的建图方法,其特征在于,所述预设的阈值为:

9.一种基于增量式点云配准的建图系统,其特征在于,基于权利要求1至8任一项所述的基于增量式点云配准的建图方法,包括:

10.一种计算机程序,其特征在于,所述计算机程序被执行时实现如权利要求1至8任一项所述的基于增量式点云配准的建图方法的步骤。