Gaussian Process Distance Fields Obstacle and Ground Constraints for Safe Navigation

TL;DR

This paper proposes a tailored scene representation coupled with an advanced trajectory optimisation algorithm to enable safe navigation, suitable for any ground-based mobile robot, whether wheeled or legged, as well as for human assistance.

Abstract

Navigating cluttered environments is a challenging task for any mobile system. Existing approaches for ground-based mobile systems primarily focus on small wheeled robots, which face minimal constraints with overhanging obstacles and cannot manage steps or stairs, making the problem effectively 2D. However, navigation for legged robots (or even humans) has to consider an extra dimension. This paper proposes a tailored scene representation coupled with an advanced trajectory optimisation algorithm to enable safe navigation. Our 3D navigation approach is suitable for any ground-based mobile robot, whether wheeled or legged, as well as for human assistance. Given a 3D point cloud of the scene and the segmentation of the ground and non-ground points, we formulate two Gaussian Process distance fields to ensure a collision-free path and maintain distance to the ground constraints. Our method adeptly handles uneven terrain, steps, and overhanging objects through an innovative use of a quadtree structure, constructing a multi-resolution map of the free space and its connectivity graph based on a 2D projection of the relevant scene. Evaluations with both synthetic and real-world datasets demonstrate that this approach provides safe and smooth paths, accommodating a wide range of ground-based mobile systems.

Figures (8)

• Figure 1: Block diagram of the proposed approach. All steps related to representation are colored in light blue. Steps related to planning are colored in light green.
• Figure 2: (a) Point cloud map of the Cow and lady dataset, the ground points and any points above $2$ m in height have been removed. (b) Horizontal slice (top-view) of the Obstacle GPDF showing the distance to the nearest obstacle points from a horizontal slice taken $1$ m above the ground. (c) Vertical slice of the Ground GPDF along the magenta line shown in (a).
• Figure 3: Quadtree formulation on the Cow and lady dataset. Dark green boxes indicate cells containing points (occupied), and light green boxes denote empty cells (free space). In (a), the quadtree is shown with obstacle points plotted in 3D. Followed by the quadtree with projected 2D obstacle points in (b)
• Figure 4: Trajectory generated with PRM (blue), $A^*$ (cyan), CHOMP using a single trained GP for distance related query (red line) and CHOMP using two GPDFs (green line), on the Stanford 2D-3D's Office 1 dataset. For better visualisation, the wall on the side of the ball has been removed in the 3D view (a).
• Figure 5: (a) Trajectory generated with PRM, $A^*$ and CHOMP using single GPDF (blue) and CHOMP using two GPDFs (green), on the urban circuit dataset (b)Trajectory generated with PRM(blue), $A^*$ and CHOMP using single GPDF (cyan) and CHOMP using two GPDFs (green), on the cave circuit dataset