Lighting-invariant Adaptive Route Following Using Iterative Closest Point Matching



Topological/metric route following, also called teach and repeat (T&R), enables long-range autonomous navigation even without globally consistent localization. In the teach pass, the robot is driven manually and builds up a topological/metric map of the environment, a graph of metric submaps connected by relative transformations. For repeating the route autonomously, the map only needs to be locally consistent; errors on the global level due to localization drift are irrelevant. This renders T&R ideal for applications in which a global positioning system may not be available, such as navigation through street canyons or forests in search and rescue, reconnaissance in underground structures, surveillance, or planetary exploration. We present a T&R system based on iterative closest point matching (ICP) using data from a spinning three-dimensional (3D) laser scanner. Our algorithm is highly accurate, robust to dynamic scenes and extreme changes in the environment, and independent of ambient lighting. It enables autonomous navigation along a taught path in both structured and unstructured environments, including highly 3D terrain. Furthermore, our system is able to detect obstacles and avoid them by adapting its path using a local motion planner. It enables autonomous route following in nonstatic environments, which is not possible with classical T&R systems. We demonstrate our algorithm's performance in two long-range driving experiments, one in a highly dynamic urban environment, the other in unstructured, rough, 3D terrain. In these experiments, our robot autonomously drove a distance of over 22 km in both day and night. We analyze the localization accuracy of our system and show that it is highly precise. Moreover, we compare our ICP-based method to a state-of-the-art stereo-vision-based technique and show that our approach has a greatly increased robustness to path deviations and is less dependent on environmental conditions.