Driving on Point Clouds: Motion Planning, Trajectory Optimization, and Terrain Assessment in Generic Nonplanar Environments

Abstract

We present a practical approach to global motion planning and terrain assessment for ground robots in generic three‐dimensional (3D) environments, including rough outdoor terrain, multilevel facilities, and more complex geometries. Our method computes optimized six‐dimensional trajectories compliant with curvature and continuity constraints directly on unordered point cloud maps, omitting any kind of explicit surface reconstruction, discretization, or topology extraction. We assess terrain geometry and traversability on demand during motion planning, by fitting robot‐sized planar patches to the map and analyzing the local distribution of map points. Our motion planning approach consists of sampling‐based initial trajectory generation, followed by precise local optimization according to a custom cost measure, using a novel, constraint‐aware trajectory optimization paradigm. We embed these methods in a complete autonomous navigation system based on localization and mapping by means of a 3D laser scanner and iterative closest point matching, suitable for both static and dynamic environments. The performance of the planning and terrain assessment algorithms is evaluated in offline experiments using recorded and simulated sensor data. Finally, we present the results of navigation experiments in three different environments—rough outdoor terrain, a two‐level parking garage, and a dynamic environment, demonstrating how the proposed methods enable autonomous navigation in complex 3D terrain.