Uploaded October 2015 | Updated November 2026, 13 minutes ago
This work is under review as:
Philipp Krüsi, Paul Furgale, Michael Bosse, and Roland Siegwart, “Driving on Point Clouds: Motion Planning, Trajectory Optimization, and Terrain Assessment in Generic Nonplanar Environments ”, Journal of Field Robotics. (submitted)
We present a practical approach to global motion planning and terrain assessment for ground robots in generic 3D environments, including rough outdoor terrain, multi-level facilities, and more complex geometries. Our method computes optimized six-dimensional trajectories compliant with the robot's kinematic and dynamic 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.
These methods are embedded in a complete autonomous navigation system based on localization and mapping by means of a 3D laser scanner and iterative closest point matching (ICP), suitable for both static and dynamic environments. This video shows the results of two navigation experiments, one in rough outdoor terrain and one in a dynamic urban environment, demonstrating how the proposed methods enable reliable and precise autonomous navigation in complex 3D terrain.
This work is under review as:
Philipp Krüsi, Paul Furgale, Michael Bosse, and Roland Siegwart, “Driving on Point Clouds: Motion Planning, Trajectory Optimization, and Terrain Assessment in Generic Nonplanar Environments ”, Journal of Field Robotics. (submitted)
We present a practical approach to global motion planning and terrain assessment for ground robots in generic 3D environments, including rough outdoor terrain, multi-level facilities, and more complex geometries. Our method computes optimized six-dimensional trajectories compliant with the robot's kinematic and dynamic 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.
These methods are embedded in a complete autonomous navigation system based on localization and mapping by means of a 3D laser scanner and iterative closest point matching (ICP), suitable for both static and dynamic environments. This video shows the results of two navigation experiments, one in rough outdoor terrain and one in a dynamic urban environment, demonstrating how the proposed methods enable reliable and precise autonomous navigation in complex 3D terrain.










