A Motion-Aware Enhanced LiDAR ICP SLAM Framework for High-Speed Autonomy

Towards the goal of real-time navigation of autonomous robots, the Iterative Closest Point (ICP) based LiDAR odometry methods are a favorable class of Simultaneous Localization and Mapping (SLAM) algorithms for their high performance and efficiency. In the context of autonomous vehicles, which tend to be high motion scenarios, ICP-based SLAM algorithms provide a relatively robust and real-time localization and mapping framework, paving the way for full autonomy. However even with the recent methods, the traditional SLAM challenges persist, where odometry drifts under adversarial conditions such as featureless or dynamic environments, and under high motion of the robot. In this paper we present a motion aware, enhanced LiDAR ICP SLAM framework. We have three contributions: a novel ICP cost function that yields faster and more robust convergence compared to the state of the art methods, a sophisticated motion constraint that maintain robot localization under sudden motion changes, and evaluation of our method on an off road dataset to demonstrate ICP based SLAM performance for the more challenging scenes. We have evaluated our framework with both urban and off road datasets, and observed lesser trajectory estimation errors under high motion scenarios. This is indicative of a more robust SLAM framework for autonomous driving scenarios.