Multi-Robot Collaborative Dense Scene Reconstruction Notes

  • 2019 年 10 月 8 日
  • 笔记

Abstraction

An autonomous scanning approach which allows multiple robots to perform collaborative scanning for dense 3D reconstruction of unknown indoor scenes.

We define the problem as a dynamic task assignment and introduce a novel formulation based on Optimal Mass Transport (OMT).

We then compute for each robot a smooth path over its assigned tasks by solving an approximate traveling salesman problem.

Simple Pipeline Description

(a) An algorithm for multi-robot collaborative dense reconstruction of unknown indoor scenes.

(b) (c) Given the partially scanned scene, we extract a set of task view points based on the uncertainty in the current reconstruction and assign them to the robots. The task assignment is formulated as an Optimal Mass Transport (OMT).

(d) Our algorithm enables the robots to efficiently coordinate with each other, evenly distribute their scanning effort, and efficiently achieve a full coverage and high-quality reconstruction.

Method

Since the map of the scene is unknown a priori, it is impossible to plan for the entire scanning process.

A natural approach is "scan-and-plan" where the system alternates between robot scanning and path planning.

The planning is based on the information gained by the scanning completed so far.

Therefore, we need a temporal discretization and conduct planning for consecutive time intervals, which we refer to as planning intervals.

In each planning interval, our method perform task view extraction, OMT-based task assignment, and per-robot path planning and camera trajectory optimization. The cause of re-planning and the termination criteria are elaborated.

We elaborate our problem setting by introducing the spatial/temporal discretization and the task definition of robot scanning.

Spatial discretization

Our method works with indoor scenes with walls and furniture but not staircases (assuming planar ground).

The scene is represented as a 3D volume of occupancy grid, based on OctoMap.

The occupancy grid is projected to the floor plane forming a flat layout of obstacles.

Each robot

is parameterized by a state

. (a)

Temporal discretization

The robot planning is conducted at consecutive planning intervals,

.

Specifically, at the beginning of each planning interval, we compute the scanning tasks and plan the motions for all robots for the time interval.

The robots are co-located at the initialization stage. To calibrate their relative poses before scanning, each robot takes a

scan and a joint localization is performed based on the AMCL (a localization algorithm implemented in ROS). Based on this collected scans, the control machine runs online occupancy map updating with OctoMap, online reconstruction using VoxelHashing, as well as our online path planning algorithm.

During this time interval, the robots move and scan to accomplish the current tasks before entering the next planning interval. This process repeats until no more scanning task exists.

Task definition and planning goal

Scanning task can be defined as the scene regions that require more scans due to incomplete or low-quality coverage.

We sample a set of scanning views pointing to the task regions, which we name task views.

In a planar world, a task view can be represented as

.

Multi-robot planning

Firstly, we construct a weighted task graph, by setting the task views as nodes and their pair-wise shortest path as edges.

The path length is used as the edge weight.

To minimize the scanning effort of all robots, a natural choice is to formulate the planning as a multiple traveling sales man problem (mTSP) which determines for each robot a tour such that the total traveling cost is minimized and that each task view is visited exactly once by one robot.

We propose a divide-and-conquer scheme.

(1) We solve a discrete optimal mass transport (OMT) for an optimal assignment of all task views to all robots.

(2) We solve a vanilla traveling salesman problem to determine an optimal visiting path for all tasks assigned to each robot.

Scanning Task Extraction

The task extraction take both coverage completeness and scanning quality into account.

The "coverage completeness" drives the robots to scan the unknown or uncertain regions.

The "scanning quality" is related to the limited resolution of a 3D scanner.

For a 3D scanner, good scanning quality is obtained within a fixed range of scanning distance and scanning angle.

So we compute two auxiliary maps in accordance to the two requirements above.

An uncertainty map defined in the 3D occupancy volume.

A validness map defined on the parametric space of camera view

Uncertainty map

We measure the scanning uncertainty of each voxel as follows.

An unknown voxel is assigned with a very high uncertainty.

For a known voxel,

If the known voxel is empty, its uncertainty is zero.

If the known voxel is not empty, its uncertainty is the weights in the TSDF.

Validness map

We define a validness map in the space of camera views.

The camera view point

is defined in the 3D parametric space

.

The validness of a view point is

(the maximum value) if the view is within the valid distance range and has

scanning angle against the nearest obstacle that is visible to the view point.

The value decreases as the scanning distance deviates from the valid range or the angle from

.

We discretize the parametric view space where the 2D position simply takes the 2D (floor) projection of the 3D occupancy grid and the turning angle is discretized at

intervals.

The validness map is defined in a

volume.

For a view

in this volume,

.

Find its closest obstacle visible along the view direction, find the first intersection with an obstacle voxel in the 3D occupancy grid.

If the ray does not intersect an obstacle voxel, we set the validness

.

Otherwise, its validness is computed as

, where

measures the deviation of the scanning angle

from

, with

. The function

measures the scanning distance deviation with respect to the valid scanning range of the sensor

.

is the distance from the position of view

to the closest visible obstacle.

is set to 6m.

Task view extraction

Sorting all voxels in the uncertainty map into a priority queue based on their uncertainty values.

Starting from the head voxel in the priority queue, we select the best scanning view

for it.

Once

is selected, it's marked as a task view. The entries for all voxels covered by the view cone (FOV) of

v∗, as well as the head, are removed from the priority queue.

Planning Objective Formulation

To assign the robots to take the tasks so that the moving effort of all robots is minimized.

Given two distributions, an optimal transport plan transforms a source distribution into a target distribution, where optimality is defined according to a given metric.

The source is the spatial distribution of robots within the environments:

is the Dirac function.

The target distribution describes the spatial distribution of the scanning tasks within the scene:

Multi-robot task assignment objective

Finding a mapping

that minimize the objective:

where

measures the cost if

is mapped to

.

Discretization of Planning Objective

We propose a discretized planning objective through defining the cost of the OMT map

and provide a solution to the optimization.

Movement cost

The blue dot represents a robot.

The black dots are task views assigned to the robot.

The lines indicate distance cost.

Since an efficient scanning path is a one-pass traverse of all tasks, TSP-based distance cost (b) is more natural than the distance summation in (a), while the former is costly to compute.

We opt for an approximate cost which is the shortest distance from the robot to the centroid of the set of assigned tasks.

We instead measure the distance from the robot to the centroid (the green dot in c) of all task points as the distance cost.

Robot capacity

The time required for finishing one task depends not only on robot moving speed but also on the distance of the task.

The latter is related to both room size and robot count. Therefore, an accurate estimation of robot capacity is difficult.

We set the capacity of a robot

as

is the total number of tasks.

the moving speed of the robot.

the maximum speed of all robots.

Discretized objective

We have the following objective function:

is the set of assigned tasks for

.

is the centroid of

.

is the shortest distance constrained on the currently scanned scene geometry.

The compactness of the tasks assigned to one robot by the sum of inter-task distances.

The distance term minimizes the moving cost of robot in finishing its tasks.

The capacity term tries to match the number of task assigned to a robot to its capacity.

TSP-based Path Planning

The OMT-based planning partitions the scanning tasks into groups so that each robot can focus on its own set of tasks.

We compute an optimal traverse path for each robot based on its assigned tasks.

TSP path

We build a weighted graph where the nodes are the positions of a task

or the robot

while directed edges connect every two nodes with the shortest path between them.

Our problem is slightly different from the original TSP, since the robot does not need to return to its starting point.

1 Set the robot position as the starting node and the farthest task to the robot as the ending one;

2 Remove all outgoing edges from the ending node except the one that directs to the starting node;

3 Solve TSP over the current graph;

4 Remove the path from the ending node to the starting node.

Path optimization

We optimize the TSP path to meet the following requirements:

1 The movement path of any robot should be as smooth as possible.

2 We need to optimize the camera pose to achieve a smooth scan.

Uniformly sampling a sequence of points along the path.

Let

denote the ordered list of points encompassing both the sampled points and the set of task view points assigned to

.

is set of indices of the task points in

.

is the outside distance function computed for the 2D projection of the currently scanned scene. The function value is 0 for points inside the scene surface.

is the original position of

.

The smooth term is devised to smooth the path curve while keeping the curve away from scene obstacles.

The penalty term regularizes the optimization by fixing the task view points.

We set

in throughout all our experiments.