Computationally efficient way to represent joint C space for a multi-robot RRT

Question

I am working on writing code for a coordinated multi-robot rapidly exploring random tree (RRT) based planner, which would naturally involve a lot of sampling, nearest neighbor searching and 'radius' searching. Because this is a coordinated/cooperative planning step, all robots' paths are incrementally created in every iteration until all robots reach their respective goals, and the planner needs all the robots' positions during the planning phase.

I have a basic framework for this working in MATLAB, which was only a proof of concept and not really efficient: but I am not sure what the best way to program it in, say, C++ would be. Normally for an RRT, I would go for a KD tree implementation, but in a multi-robot point of view, the environment would be a joint configuration space and this would mean a pseudo-high-dimensional KD tree: which is not actually high dimensional, but just needs to perform nearest neighbor searches in a space that combines the states (x,y,z, yaw) of all the robots - over and over again during the planning phase. The metric is simple enough, as it is just Euclidean distance, but I don't know if using KD trees for this will be computationally efficient. I'm looking for some suggestions on how to describe the configuration space for an efficient multi-robot RRT (I am thinking of a maximum of five robots) with a state dimension of 4.

Answer 1

I don't want to sound too pessimistic, but from my experience a straightforward RRT approach for multiple robots would not be able to scale well with the number of robots, due to excessive collision checking. Therefore, nearest-neighbor search (NN) would certainly not be the bottleneck, as you will not be able to experiment with more than 10 robots, at best.

If you do manage to experiment, possible by using a different method, with a large fleet of robots, and the high-dimensional NN technique turns out to be the bottleneck, it might be worth to consider a dimension reduction: for $m$ robots having $d$ degrees of freedom you might be able to efficiently encode the distance relations between $md$-dimensional composite configuration using dimension reduction.

You may want to elaborate on the reason you chose to implement an direct RRT approach, so that we can help in finding a better alternative.

Answer 2

I'd start by considering your problem as a single K-dimensional problem (K would be 4 DoF by the number of robot, so K=20 in the case you mention), and have a look to FLANN, which is distributed as a C++ library, for the nearest neighbors search.

Or simply start by an exhaustive search using Euclidean distance in that K-dimensional space. Also, I don't have a reference at hand, but I think you can use a search among some randomly sampled points of you graph: on each nearest-neighbor search, pick randomly n points in your graph and exhaustively look for the nearest-neighbor in that subset. It will degrade some qualities of your tree, but may result faster, depending on the value for n you pick.

But the solution you need depends a lot on your problem specification, mainly size and clutteredness of your environment, presence of bottlenecks...

Hope it helps...