Online simultaneous localization and mapping with parallelization for dynamic line segments based on moving horizon estimation

In this paper, to render SLAM robust in dynamic environments, we propose a novel LiDAR SLAM algorithm that estimates the velocity of all objects in the scene while suppressing speed of static objects by moving horizon estimation (MHE). We approximate environment features as dynamic line segments having velocity. To deal with static objects as well, MHE is employed, so that its objective function allows the addition of velocity suppression terms that treat stationary objects. By considering association probability, the SLAM algorithm can track the endpoints of line segments to estimate the velocity along the line segments. Even if it is temporarily occluded, the estimation is accurate, because MHE considers a finite length of past measurements. Parallelization of the robot's localization with the map's estimation and careful mathematical elimination of decision variables allows online implementations. Post-process modifications remove possible spurious estimates by considering the piercing of LiDAR lasers and integrating maps. Simulation and experiment results of the proposed method prove that the presented algorithm can robustly perform online SLAM even with moving objects present.