Safe Human-Robot Interactions with Continuous and Differentiable Distance Fields

1Technical University of Applied Sciences Würzburg-Schweinfurt
2University of Technology Sydney

Abstract

Human-robot collaboration applications require safe and reactive planning. Euclidean distance fields (EDF) are a promising representation of such dynamic scenes due to their ability to reason about free space and the readily available distance to collision costs. A key challenge for the commonly used discrete EDF representations, however, is the need for differentiable distance fields to produce smooth collision costs and efficient updates of dynamic objects. In this paper, we propose to use a Gaussian Process (GP) distance field-based framework that enables both, differentiable distance fields and fast dynamic scene updates. Moreover, we combine this framework with the Riemannian Motion Policies as a local reactive planner to enable safe human-robot interactions. We design a collision avoidance policy that models the repulsive motion using the distance and gradient fields from our GP. We show our reactive planner in an experiment with a UR5e interacting safely and smoothly with a human.

System diagram of our proposed framework.

Proposed Framework

System diagram of our proposed framework.

The Figure shows proposed system architecture, where IDMP takes as input the depth sensor's data and pose. These inputs are used to generate the local Frustum Field which determines the implicit semantics of the scene and is then used to fuse the new observation with the global GPDF. Our RMP policy queries distance and gradient information from the fused global GPDF to generate accelerations which are passed to a controller for execution on the robot. The key aspect of the IDMP framework is that it uses a Frustrum Field to fuse and identify the dynamic regions locally before passing the information to the Fused Field that contains the global information. The following figures are showing the internal update process of IDMP. The background displays the distance field within the sensor's field of view generated by the frustum GPDF. While the fused GPDF is trained on all points from the internal global map, the frustum GPDF only utilizes the latest observations, capturing changes in the scene. By querying the frustum GPDF with the fused GPDF's training points, we can directly retrieve implicit semantics based on distance metrics. Training points in the fused GPDF are classified as static if their queried distance in the frustum GPDF is below a certain threshold, indicating the object has not moved. Training points are classified as dynamic when this distance exceeds the sensor noise threshold, indicating that the object has moved. For the final case we query newly observed sensor points with the fused GPDF. Those points with distances greater than a certain threshold are classified as new and are fused into the global GPDF.

Avatar woman
Avatar woman

Quantitative Results

We evaluate our method in a mock human-robot interaction scene where the robot is tasked to cycle between two waypoints. During the execution, a human enters the workspace and places their arm in the way of the robot. We compare the behaviour of our framework against an occupancy-based reactive method implemented in ROS package MoveIt. This baseline method builds an Octomap which is continuously updated with the sensor input. A trajectory is then planned using the Bi-directional Fast Marching Tree (BFMT*). During execution the trajectory is checked for possible collisions which then triggers replanning.

Avatar woman

As can be seen in the following Table the trajectories produced by our method result in much smoother trajectories. Notably the mean squared jerk and change in curvature were approximately 2x and 3x lower, respectively, than the baseline.

Avatar woman

Videos

BibTeX

BibTex Code Here