一种在高动态环境下的激光slam方法、系统设备及存储介质
阅读说明：本技术 一种在高动态环境下的激光slam方法、系统设备及存储介质 (Laser SLAM method, system equipment and storage medium in high dynamic environment ) 是由 孙宏滨 钱成龙 张旭翀 张敬敏 杨彦龙 于 2021-07-30 设计创作，主要内容包括：本发明公开了一种在高动态环境下的激光SLAM方法、系统设备及存储介质,包括如下过程：使用IMU的预积分里程计来获得SLAM系统的初始状态估计；分别将当前激光点云和局部点云地图按照SLAM系统的定位误差所确定的分辨率投影为距离图像；比较这两幅距离图像,去除当前激光点云和局部点云地图中代表移动物体的点；将当前激光点云和局部点云地图进行点云匹配,获得SLAM系统的状态估计；判断点云匹配是否收敛；若点云匹配收敛,将获得的SLAM系统的状态估计作为最终的SLAM系统的状态估计；若点云匹配不收敛,重复进行所述距离图像获取、去除代表移动物体的点、点云匹配和收敛性判断的过程,直至点云匹配收敛。本发明能够同时提高动态点的去除率和SLAM的精度。(The invention discloses a laser SLAM method, system equipment and storage medium under a high dynamic environment, which comprises the following processes: obtaining an initial state estimate of the SLAM system using a pre-integrated odometer of the IMU; respectively projecting the current laser point cloud and the local point cloud map into a distance image according to the resolution determined by the positioning error of the SLAM system; comparing the two distance images, and removing points representing moving objects in the current laser point cloud and the local point cloud map; carrying out point cloud matching on the current laser point cloud and the local point cloud map to obtain state estimation of the SLAM system; judging whether the point cloud matching is converged; if the point cloud matching is converged, the obtained state estimation of the SLAM system is used as the final state estimation of the SLAM system; if the point cloud matching is not converged, the processes of distance image acquisition, point removal representing a moving object, point cloud matching and convergence judgment are repeated until the point cloud matching is converged. The invention can simultaneously improve the removal rate of the dynamic point and the precision of the SLAM.)
技术领域
本发明属于自动驾驶汽车技术领域，涉及一种在高动态环境下的激光SLAM方法、系统设备及存储介质。
背景技术
基于激光雷达的同步定位与地图构建(Simultaneous LocalizationAndMapping，SLAM)技术可以为没有全球导航卫星系统(Global Navigation SatelliteSystem，GNSS)或者处于GNSS信号不稳定、不可用状态下的自动驾驶汽车或移动机器人提供鲁棒的厘米级定位和高精度的点云地图。然而，现有的基于激光雷达SLAM的定位方法基本上都是基于静态环境假设，即假设环境中不存在移动的物体。但实际上，自动驾驶汽车通常工作在具有大量移动物体(如车辆、行人等)的真实环境中。由于移动物体对传感器视野的遮挡，大多数激光点将落在移动物体上而非静态环境上，此时已有基于静态环境假设的激光雷达SLAM方法将会失效。此外，在传统激光雷达SLAM方法构建点云地图的过程中，移动物体将在点云地图中留下运动重影。这些覆盖在点云地图上的运动重影轨迹会造成严重遮挡，从而大大增加在点云地图中提取道路标记、交通标志和其他关键静态特征的难度。
为了解决上述提到的问题，需要对移动的物体进行识别并去除。然而，目前现有的能够去除移动物体的激光SLAM方法都是先获得系统准确的定位信息，然后才能根据准确的定位信息来判断点云中的移动物体并将之移除。但在高动态环境下，基于静态世界假设的激光SLAM方法将会失效，从而难以获得准确的定位信息，也就无法进一步识别移动物体并将之移除。因此，已有方法在高动态环境下陷入了一个“先有鸡还是先有蛋”的问题：移动物体点云的去除依赖于精确的定位信息，但是当点云地图中包含许多移动物体时，SLAM系统无法获得精确的定位信息。上述问题使得已有的激光SLAM方法在高动态环境下难以取得很好的效果。
发明内容
本发明的目的在于解决上述现有技术中存在的问题，提供一种在高动态环境下的激光SLAM方法、系统设备及存储介质。该方法通过多步迭代先去除动态点再进行激光点云匹配这一方式来实现高动态环境下的激光SLAM定位，并生成仅包含静态环境的点云地图，显著提高建图质量与定位鲁棒性。
为了实现上述目的，本发明采用以下技术方案予以实现：
一种在高动态环境下的激光SLAM方法，包括如下过程：
初始状态估计：使用惯性测量单元(也称为IMU)的预积分里程计来获得SLAM系统的初始状态估计；
距离图像获取：分别将当前激光点云和局部点云地图按照当前SLAM系统的定位误差所动态确定的分辨率投影为距离图像；
去除代表移动物体的点：比较这两幅距离图像，去除当前激光点云和局部点云地图中代表移动物体的点；
点云匹配：将去除了代表移动物体的点的当前激光点云和局部点云地图进行点云匹配，获得SLAM系统的状态估计；
收敛性判断：判断点云匹配是否收敛；
确定最终的SLAM系统的状态估计：若点云匹配收敛，则将获得的SLAM系统的状态估计作为最终的SLAM系统的状态估计；若点云匹配不收敛，则利用去除了代表移动物体的点的当前激光点云和局部点云地图重复进行所述距离图像获取、去除代表移动物体的点、点云匹配和收敛性判断的过程，直至点云匹配收敛，得到最终的SLAM系统的状态估计。
优选的，首次进行点云匹配前的SLAM系统的定位误差为紧耦合的IMU的预积分里程计和激光雷达里程计之间的定位误差，该定位误差通过下式计算：
其中，δXk表示当前激光点云时刻通过IMU的预积分里程计获得的SLAM系统定位误差，δXk-1表示上一激光点云时刻通过IMU的预积分里程计获得的SLAM系统定位误差，表示上一时刻的SLAM系统的定位误差对时间t的导数，Δt表示当前激光点云时刻和上一激光点云时刻之间的时间间隔；经过收敛性判断且点云匹配不收敛时，重复进行上述距离图像获取的过程时，SLAM系统的定位误差由当前激光点云匹配的误差得到。
优选的，点云匹配收敛，利用去除了代表移动物体的点的当前激光点云和局部点云地图再进行一次所述距离图像获取以及去除代表移动物体的点过程，得到最终的SLAM系统的状态估计。
优选的，SLAM系统的定位误差所确定的分辨率r如下：
r＝αδp+δθ
其中，α为缩放系数，用于平衡位置误差和姿态误差对于分辨率的贡献度，一般取0.1即可，δp表示位置误差，δθ表示位姿误差。
优选的，将当前激光点云和局部点云地图按照当前SLAM系统的定位误差动态所确定的分辨率投影为距离图像时：
距离图像中的每个像素对应的是当前激光点云和局部点云地图球面坐标的角度范围大小，像素值是球心到球面扇形区域中最近激光点的距离。
优选的，比较当前激光点云和局部点云地图对应的两幅距离图像，去除当前激光点云和局部点云地图中代表移动物体的点的过程包括：
将两幅距离图像逐像素相减，得到这两幅距离图像的视差图像；
当视差图像中某一像素的像素值大于阈值时，对应的当前激光点云的距离图像的像素对应的点云为动态点，将确定出的动态点从当前激光点云中去除；
当视差图像中某一像素的像素值小于阈值的相反数时，局部点云地图的距离图像的像素对应的点云为动态点，将确定出的动态点从局部点云地图中去除。
优选的，使用紧耦合的IMU的预积分里程计来获得SLAM系统的初始状态估计的过程包括：
通过对IMU的数据进行预积分得到IMU预积分里程计，使用IMU预积分里程计补偿激光雷达的运动畸变，获得SLAM系统的初始状态估计。
本发明还提供了一种在高动态环境下的激光SLAM系统，包括：
初始状态估计模块：用于使用IMU的预积分里程计来获得SLAM系统的初始状态估计；
距离图像获取模块：用于分别将当前激光点云和局部点云地图按照SLAM系统的定位误差所确定的分辨率投影为距离图像；
去除模块：用于比较这两幅距离图像，去除当前激光点云和局部点云地图中代表移动物体的点；
点云匹配模块：用于将去除了代表移动物体的点的当前激光点云和局部点云地图进行点云匹配，获得SLAM系统的状态估计；
收敛性判断模块：判断点云匹配是否收敛；
最终状态估计确定模块：若点云匹配收敛，则将获得的SLAM系统的状态估计作为最终的SLAM系统的状态估计；若点云匹配不收敛，则将去除了代表移动物体的点的当前激光点云和局部点云地图发送给距离图像获取模块。
本发明还提供了一种电子设备，包括：
一个或多个处理器；
存储装置，其上存储有一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现本发明如上所述的在高动态环境下的激光SLAM方法。
本发明还提供了一种存储介质，其上存储有计算机程序，其中，所述计算机程序被处理器执行时实现本发明如上所述的在高动态环境下的激光SLAM方法。
本发明具有以下有益效果：
本发明不依靠容易受到动态环境影响的点云匹配方法来获得当前的精确初始位姿，而是采用不受动态环境影响的IMU预积分方法来预测当前点云的位姿。这一方法使得激光SLAM系统在高动态环境下具有更高的鲁棒性和定位精度。本发明能够根据当前的定位误差动态地调整分辨率，因此能够在仅有相对粗糙的定位精度的情况下去除动态点。本发明将动态点去除和SLAM的过程结合在一起，通过多步迭代的方式进行先动态点去除再点云匹配的步骤，同时提高了动态点的去除率和SLAM的精度。
附图说明
图1为本发明激光SLAM方法的系统工作流程图；
图2为本发明中激光点云的球面坐标投影为距离图像的示意图；
图3为本发明根据定位误差的自适应分辨率曲线图。
具体实施方式
下面结合附图和实施例对本发明做进一步详细的说明：
本发明在处理一个新获取的激光点云时，不会立即对当前激光点云与局部点云地图进行匹配以获得准确的状态估计，因为这样直接匹配的方法很容易受到动态环境的影响，导致定位不准确。相反，本发明采用以下步骤进行：
步骤1：首先使用紧耦合的惯性测量单元(IMU)的预积分里程计来获得SLAM系统的初始状态估计；
步骤2：分别将当前激光点云和局部点云地图按照SLAM系统定位误差所确定的分辨率投影为距离图像(Range Image)。并通过逐像素的比较两幅距离图像的可视性去除这两幅距离图像所对应的点云(即当前激光点云和局部点云地图)中代表移动物体的点。
步骤3：在当前激光点云和局部点云地图中代表移动物体的点去除后，将当前激光点云与局部点云地图进行一次粗略的匹配获得相对精确的SLAM系统的状态估计。
步骤4：判断此时点云匹配是否收敛。
步骤5：如果匹配收敛，步骤3得到的SLAM系统的状态估计即是最终的SLAM系统的状态估计。如果不收敛，将去除了代表移动物体的点的当前激光点云和局部点云地图重复上述步骤2和步骤3，迭代地进行动态点去除和点云匹配，最终在高动态环境中获得精确的SLAM系统定位信息和不包含移动物体的点云地图。
其中，将激光雷达最新获取的激光点云称为当前激光点云。将之前激光点云所构成的点云集合称为点云地图。将点云地图中当前SLAM系统位置周围一定范围的点云称为局部点云地图。将SLAM系统的位置和姿态称为SLAM系统的状态估计，也就是SLAM系统的定位信息。将表示移动物体的点云称为动态点。利用惯性测量单元(IMU)的数据积分得到SLAM系统连续时刻的状态估计称为IMU预积分里程计。使用和当前激光点云时间对应的预积分里程计的值作为SLAM系统的初始状态估计。
参见图1，本发明在高动态环境下的激光SLAM方法具体包括以下步骤：
S1，通过对惯性测量单元IMU的数据进行预积分得到IMU预积分里程计。然后，使用IMU预积分里程计补偿激光雷达的运动畸变并获得SLAM系统的初始状态估计。SLAM系统在第k个激光点云时刻的状态估计用Xk表示，状态估计用Xk由两部分构成，分别是位置p和姿态θ。
S2，初始投影分辨率估计：计算上一激光点云时刻，惯性测量单元IMU预积分里程计和激光雷达里程计之间的定位误差，并通过非线性系统的误差传递关系确定在当前激光点云未进行点云匹配之前估计当前激光点云时刻的SLAM系统的定位误差，所述非线性系统的误差传递关系为：其中，δXk表示当前激光点云时刻的SLAM系统定位误差，δXk-1表示上一激光点云时刻的SLAM系统定位误差，表示上一时刻的SLAM系统的定位误差对时间t的导数，Δt表示当前激光点云时刻和上一激光点云时刻之间的时间间隔。
新投影分辨率估计：因为此时已经进行了激光点云匹配，新的定位误差的可以由当前激光点云匹配的误差得到。
S3，使用S2中估计的当前激光点云时刻的定位误差确定投影分辨率r：r＝αδp+δθ。其中，α＝0.1，δp表示位置误差，δθ表示位姿误差。参见图2，投影分辨率即把三维点云投影为距离图像时，距离图像中的每个像素对应的点云球面坐标的角度范围大小，其中像素值是球心到球面扇形区域中最近激光点的距离。
S4，使用S3所述的方法，分别从当前激光点云和对应的局部点云地图构建距离图像。通过将局部点云地图的距离图像与当前激光点云的距离图像逐像素相减，得到两幅距离图像的视差图像。当视差图像某一像素的像素值大于阈值τ时，对应的当前激光点云的距离图像的像素对应的点云即为动态点。反之视差图像某一像素的像素值小于阈值-τ时局部点云地图的距离图像的像素对应的点云即为动态点。分别将当前激光点云和局部点云地图中找到的动态点去除。
S5，将当前激光点云和局部点云地图进行点云匹配，并判断点云匹配是否收敛。
S6，如果匹配收敛，经过图优化后，生成精细的投影分辨率。优选的，可再一次重复S4所述方法，移除当前关键帧中剩余的动态点，并得到SLAM系统最终更为准确的状态估计。
S7，,如果点云匹配失败(即点云匹配不收敛)，将生成新的粗分辨率，并重复S4-S6步骤。
S8，最后生成激光雷达里程计数据，并将其输入IMU预积分模块估计IMU的bias。完成一个循环。
参见图3，因为定位误差是时刻改变的，因此将激光点云数据投影为2D距离图像的过程中，投影分辨率根据系统当时的定位误差自适应动态调整。同时为了平衡动态点去除率和SLAM系统的实时性，本发明将预测得到的分辨率适当放大，并限制多线激光雷达每一线对应的垂直方向的视角大小为分辨率最小的截断值，得到最后用于投影的分辨率。图3中的三条曲线分别为根据真实的定位误差生成的分辨率曲线。根据上一时刻误差预测当前时刻误差生成的分辨率曲线。以及将预测分辨率曲线放大2倍并采用一个下界截断的产生的最终分辨率。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：用于对机器人进行自动充电的方法和相关产品