Interactive Distance Field Mapping and Planning to Enable Human-Robot Collaboration

TL;DR

An interactive distance field mapping and planning framework that handles dynamic objects and collision avoidance through an efficient representation that performs incremental updates and handles dynamic objects reliably by identifying moving points via a simple and elegant formulation based on queries from a temporary latent model is presented.

Abstract

Human-robot collaborative applications require scene representations that are kept up-to-date and facilitate safe motions in dynamic scenes. In this letter, we present an interactive distance field mapping and planning (IDMP) framework that handles dynamic objects and collision avoidance through an efficient representation. We define interactive mapping and planning as the process of creating and updating the representation of the scene online while simultaneously planning and adapting the robot's actions based on that representation. The key aspect of this work is an efficient Gaussian Process field that performs incremental updates and handles dynamic objects reliably by identifying moving points via a simple and elegant formulation based on queries from a temporary latent model. In terms of mapping, IDMP is able to fuse point cloud data from single and multiple sensors, query the free space at any spatial resolution, and deal with moving objects without semantics. In terms of planning, IDMP allows seamless integration with gradient-based reactive planners facilitating dynamic obstacle avoidance for safe human-robot interactions. Our mapping performance is evaluated on both real and synthetic datasets. A comparison with similar state-of-the-art frameworks shows superior performance when handling dynamic objects and comparable or better performance in the accuracy of the computed distance and gradient field. Finally, we show how the framework can be used for fast motion planning in the presence of moving objects both in simulated and real-world scenes. An accompanying video, code, and datasets are made publicly available https://uts-ri.github.io/IDMP.

Figures (9)

• Figure 1: Interactive generation of a distance and gradient field in an HRC setting. Top right: Depth image from an Intel Realsense camera. Bottom left: Coloured point cloud and a horizontal slice of the field (red/blue means close/far to the nearest object with arrows pointing away from it).
• Figure 2: System diagram of IDMP. We first model the temporary latent Frustum Field (blue) using only $\mathcal{P}_{\{i\}}$ as training points. All prior training points $\mathcal{P}_{\{0,...,i-1\}}$ in the Fused Field (yellow) are then passed to the Frustum Field. Given the sensor pose, the Frustum Field selects from $\mathcal{P}_{\{0,...,i-1\}}$ the points that are within the frustum area $\mathcal{P}_{f\{0,...,i-1\}}$ and returns the inferred values of $\hat{d}_f$, $\nabla \hat{d}_f$ to the Fused Field. These distances and gradients are used to perform fusion and dynamic updates by updating the training points that model the Fused Field. The path planner then queries for $\hat{d}_{o,s}$, $\nabla \hat{d}_{o,s}$ in order to compute and adapt its motion plans in response to a changing map.
• Figure 3: A simplified illustration of the proposed method. (1) Before the update, we have the prior training points $\mathcal{P}_{\{0,...,i-1\}}$ in the Fused Field. (2) The coloured area is the Frustum Field (blue is close to the surface and yellow is far from it). The current points $\mathcal{P}_{\{i\}}$ are shown as red and yellow crosses. The prior training points within the frustum $\mathcal{P}_{f\{0,...,i-1\}}$ are marked by red and pink. Fusion, dynamic update, and insertion processes are shown in red, pink, and yellow respectively. (3) After the update, we have the updated training points $\mathcal{P}_{\{0,...,i\}}$ that model the Fused Field used by the planner.
• Figure 4: Example of an IDMP query on a horizontal plane for the Cow & Lady dataset. The colour of the arrows represents the distance to the nearest object. The arrows are normalised gradients that point in the direction away from the nearest object. The updated training points are coloured using RGB data from the camera.
• Figure 5: Quantitative evaluation of distance RMSE (lower is better), gradients cosine similarity (higher is better) and computation time (lower is better) using the Cow & Lady dataset for IDMP, FIESTA and Voxblox.