Ergodic Exploration

Motivation

Currently there are no exploration packages for ROS Noetic. The ROS packages that currently exist perform only frontier exploration. The goal of this project is to provide a robot agnostic information theoretic exploration strategy for known and unknown environments.

This package requires a user specified target distribution representing the expected information gain. The target distribution is represented as a Gaussian or multiple Gaussians.

As an alternative, mutual information can be used as the target distribution. Checkout the feature/sportdeath_mi branch. That branch is experimental because the mutual information implementation has not been verified yet.

This package was developed at Northwestern University during the Master of Science in Robotics program.

Table of Contents

• Motivation </br>
• Exploration </br>
  • Ergodic Control </br>
    • Target Information Density </br>
    • Mutual Information </br>
  • Exploration Stack </br>
  • Collision Detection </br>
• Getting Started </br>
  • Dependencies </br>
  • Workspace </br>
  • Launch </br>
• Nodes </br>
  • Subscribed Topics </br>
  • Published Topics </br>
  • Parameters </br>
  • Required tf Transforms </br>
• Future Improvements </br>
• Citing Ergodic Exploration </br>
• Credits </br>

Exploration

Ergodic Control

Ergodicity is defined as the fraction of time spent sampling an area should be equal to a metric quantifying the density information in that area. The ergodic metric is the difference between the probability density functions representing the spatial distribution and the statistical representation of the time-averaged trajectory [1]. The objective function includes the ergodic metric and the control cost.

The ergodic controller performs receding horizon trajectory optimization in real time. The real time performance is achieved by integrating the co-state backwards in time [2].

In both the target information density and the mutual information demonstrations below, rtabmap_ros was used for localization and mapping using a Velodyne lidar. Odometry was performed using wheel encoders.

Target Information Density

Two targets representing the information density are modeled as Gaussians are shown in yellow. The optimized trajectory from the erogdic controller is show in red. The trajectory oscillates from side to side but this behavior is expected. The predicted trajectory from the dynamic window approach is shown in blue.

See the full video in real time.

The resulting path from the exploration stack is shown in blue and the small white lines represent local loop closures. The robot was observed visiting each target several times and exploring the corridor to the left of the targets.

Mutual Information

In the case of the occupancy grid, mutual information is the expected information gain at each grid cell. The feature/sportdeath_mi branch computes it using the Fast Continuous Mutual Information (FCMI) algorithm presented in [3]. This is accomplished by simulating a 360 degree range finder.

Below, the robot explores the atrium using mutual information as the target distribution shown in the upper left. The dark areas represent more mutual information. After the robot has fully explored the space, the mutual information is zero.

See the full video in real time.

The occupancy grid and mutual information map are on the left and right respectively. The robot’s path is shown in blue and the small white lines represent local loop closures.

Exploration Stack

The ergodic controller cannot guarantee a collision free trajectory. The dynamic window approach is used as the local planner. The next twist from ergodic controller is propagated forward for a short time to detect collisions. If there is a collision, the dynamic window approach is used. The dynamic window approach forward propagates a constant twist for a short time and disregards it if there is a collision.

In the case where the ergodic control results in a collision, the dynamic window approach uses the optimized trajectory as a reference. The dynamic window approach finds a twist that will produce a trajectory most similar to the reference that is collision free. Similarity between the optimized trajectory and the dynamic window approach trajectory is defined as the distance between the robot’s position and the absolute difference in heading at each time step. The twist produced by the dynamic window approach is followed for a number of steps to ensure the robot is away from obstacles.

It is possible that the twist from the dynamic window approach can cause a collision in a dynamic environment. In this case the dynamic window approach will search the velocity space for the most similar collision free twist to the previous twist given by the dynamic window approach. Similarity is defined as the inner product of the twist. If the dynamic window approach fails, a flag is set for the ergodic controller to replan. The ergodic controller is sensitive to the pose of the robot and is capable of optimizing a different trajectory. This strategy prevents the robot from getting stuck.

Collision Detection

File truncated at 100 lines see the full file