一种面向动态环境的激光SLAM系统及方法

本发明属于智能移动平台定位与导航领域，具体涉及一种带面向动态环境的激光slam系统及方法。

背景技术：

1、slam(simultaneous localization and mapping，同时定位与地图构建)作为机器人领域的关键技术之一。大多数slam研究都是基于周围环境静止的假设，未针对动态物体进行处理。在真实环境中往往许多物体是动态的。包含大量动态物体的场景对slam系统提出了更高的要求。因此，当slam技术应用于真实环境中不可避免地需要考虑到其丰富多变的复杂环境。目前大部分针对动态环境的slam方法采用神经网络识别动态物体，这些方法在动态非结构化场景时，可能会遇到没有训练过的陌生动态物体，这样将造成识别效果不好甚至可能会失效。

2、中国专利申请：一种在高动态环境下的激光slam方法、系统设备及存储介质(申请号：cn202110872830.3)，本发明公开了一种在高动态环境下的激光slam方法、系统设备及存储介质，使用imu的预积分里程计来获得slam系统的初始状态估计；分别将当前激光点云和局部点云地图按照slam系统的定位误差所确定的分辨率投影为距离图像；比较这两幅距离图像，去除当前激光点云和局部点云地图中代表移动物体的点。该方法使用的将点云图转换为距离图像的方法滤除动态点云。

3、中国专利申请：一种面向动态环境的相机与固态激光雷达融合重定位方法(申请号：cn202211040009.6)，本发明结合了视觉和激光改善了在动态环境下的重定位的弊端。该方法基于深度学习的目标检测得到先验的动态物体，再通过极几何和贝叶斯公式用概率的形式计算每个特征点的动态概率，保留静态的特征点。该方法用到到神经网络的方法滤除动态物体，该方法在遇到没有训练过的陌生动态物体，可能造成识别效果不好甚至可能会失效。

4、中国专利申请：一种应用于动态环境的激光slam系统及方法(申请号：cn202210586859.x)，本发明将当前环境的三维点云数据进行球面投影，生成规则的顶点图；基于规则的顶点图，利用预先训练好的激光slam模型进行位姿估计和地图构建。该方法也用到到神经网络的方法滤除动态物体，在动态非结构化场景下，也有可能造成识别效果不好甚至可能会失效。

技术实现思路

1、本发明旨在解决以上现有技术的问题。提出了一种面向动态环境的激光slam系统及方法。本发明的技术方案如下：

2、一种面向动态环境的激光slam系统，其包括：slam子系统和动态滤除子系统，其中：

3、所述slam子系统包括数据采集模块、imu预测模块、子图构建模块、雷达里程计估计模块及后端优化模块，所述数据采集模块通过激光雷达采集实时点云信息，以及通过惯性测量单元采集imu数据；所述点云预处理模块用于去除杂波；imu预测模块用于通过imu数据预积分操作，获取初始位姿；子图构建模块，通过初始位姿和点云的坐标转换构建子图；雷达里程计估计模块，通过点云配准估计雷达运动的位姿变换矩阵；后端优化模块，通过非线性方程优化位姿变换矩阵，得到最优位姿；

4、所述动态滤除子系统包括感兴趣区域设置模块、栅格划分模块、占据描述子及其差值计算模块、占据描述子差值计算模块、拟合地平面模块、动态滤除模块及判断收敛性模块，所述感兴趣区域设置模块通过设置半径范围和点云高度得到当前帧和其子图的感兴趣区域；所述栅格划分模块按照角度和半径划分栅格子；所述占据描述子及其差值计算模块，根据栅格内的点云高度分布信息计算栅格占据描述子；所述占据描述子差值计算模块，根据当前帧和子图的占据描述子差值得到潜在动态点；拟合地平面模块，以每个栅格为单位拟合地平面确定地面点；动态滤除模块用于将潜在动态区域的地面以上的点滤除；判断收敛性模块用于计算滤除动态点后的点云的收敛得分。

5、一种基于所述系统的slam方法，其包括以下步骤：

6、2.1首先通过激光雷达和惯性测量单元获得点云数据和imu数据；

7、2.2再将点云数据除去杂波和将imu数据预积分，为构建子图提供点云和初始位姿t0；

8、2.3通过imu提供的初始位姿和坐标转换构建子图，然后将构建的子图发送到动态滤除子系统滤除动态点云；

9、2.4通过设置半径范围和点云高度得到当前帧和其子图的感兴趣区域；

10、2.5再将其按照角度和半径划分栅格；

11、2.6然后根据栅格内的点云高度分布信息计算栅格占据描述子；

12、2.7再根据当前帧和子图的占据描述子差值得到潜在动态点；

13、2.8再以每个栅格为单位拟合地平面确定动态点，然后滤除动态点；

14、2.9计算滤除动态点后的点云的收敛得分，发散则自适应调整栅格的划分数目重新滤除动态点，收敛则将输出滤除动态点的点云；

15、2.10最后通过点云配准估计雷达运动的位姿变换矩阵和非线性方程优化位姿，得到最优位姿t，返回修正子图。

16、进一步的，所述步骤2.5再将其按照角度和半径划分栅格；2.6然后根据栅格内的点云高度分布信息计算栅格占据描述子，具体包括：

17、3.1将当前帧和其子图的雷达点云坐标(x,y,z)，转换为(r,θ,z)坐标，其中极坐标半径极坐标夹角该极坐标z＝直角坐标z；

18、3.2设置感兴趣范围：正常情况下的动态物体都是位于地平面上，设置高度范围内的限制hmin＜z＜hmax，以及半径范围r＜lmax，然后生成图vtq和vtm，其中hmin为最低高度，hmax为最高高度，lmax为最大半径，vtq为t时刻当前帧的感兴趣区域部分，vtm为t时刻当前帧对应子图的感兴趣区域部分；

19、3.3按照角度和半径，在方位和径向上格则间隔，划分单个栅格，记为st(r,θ)，最后生成的栅格图，记为stq和stm；

20、

21、其中nr表示环形数和nθ表示扇形数。

22、3.4计算占据描述子：计算每个格子内n个点高度z的均值μ和方差σ。

23、进一步的，所述步骤2.7根据当前帧和子图的占据描述子差值得到潜在动态点，具体包括：

24、4.1将点云划分为两个相互排斥的子集：动态点集(.)d和静态点集(.)s，上述问题表示为：

25、

26、其中pq为当前帧点云，pq_d为当前帧动态点云，为当前帧静态点云，pm为当前帧子图点云，pm_d为当前帧子图动态点云，pq_spm_s为当前帧子图静态点云。

27、4.2根据stq和stm的占据描述子和计算stq和stm的描述子差值

28、

29、4.3通过设置差值阈值τ和栅格内的点云数量判断潜在动态区域：

30、其中当前帧栅格内的点云数量由nq(r,θ)表示，当前帧对应子图栅格内的点云数量由nm(r,θ)表示；

31、若nq(r,θ)＞nm(r,θ)且则令当前帧的该栅格内的点为潜在动态点若nq(r,θ)＜nm(r,θ)且则令子图的该栅格内的点为潜在动态点

32、进一步的，所述步骤2.8以每个栅格为单位拟合地平面确定动态点，然后滤除动态点；具体包括：

33、5.1初始地面点集：首先，设当前t时刻第i个潜在动态栅格为在栅格中选择高度最低的若干个点作为种子点，再取种子点的高度的均值z，得到初始地面点集

34、5.2区域生长：将初始点集向上3次区域生长，将整个生长过程重复三次之后，得到较为完整的地面点集

35、5.3动态点：最后将潜在动态点里面的地面点除去后，得到较为准确的动态点

36、

37、进一步的，所述步骤2.9计算滤除动态点后的点云的收敛得分，发散则自适应调整栅格的划分数目重新滤除动态点，收敛则将输出滤除动态点的点云，具体包括：

38、6.1特征提取：通过评估局部区域上点的粗糙度，提取当前帧和其对应子图的边缘特征和平面特征

39、6.2通过最近邻搜索在中找到对应的特征点；

40、6.3计算特征点与其对应的边缘或平面特征之间的距离和

41、6.4当特征点之间的距离和小于阈值τd时将该距离累加到得分score中；

42、6.5计算中所有特征点与中对应特征点的距离，重复上述步骤6.3至6.4，得到当前帧的得分score，再求均值

43、6.6设置τscore收敛阈值，小于该阈值则判定为收敛，反之为发散；

44、6.7若得分发散，则自适应调整栅格的划分数目nr和nθ，重复以上步骤，重新去除动态点；

45、6.8若得分收敛，则再通过雷达里程计和后端优化输出最优位姿。

46、本发明的优点及有益效果如下：

47、本发明提出一种面向动态环境的激光slam系统，该系统包括slam子系统及动态滤除子系统。其中动态滤除子系统根据当前帧和其对应子图的点云分布的差值判断潜在动态区域，再拟合地平面确定动态点，然后判断滤除动态点后的点云是否收敛，不收敛则重新滤除动态点，收敛则将滤除动态点后的点云传入slam子系统得到最优位姿。本发明利用了帧与其子图的点云高度分布关系，常见动态物体位于地平面之上的特性，以及滤除后的收敛性，更加准确的确定动态点，能在动态环境下实时有效滤除动态点云，构建更加鲁棒的点云地图，从而准确获得机器人的位置信息。本发明相对与其他采用神经网络识别动态物体的方法，不依赖于已有对象的先验信息，在动态非结构化场景下有着更强的适应能力。