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.

  • $\begingroup$ Something like Finding a needle in an exponential haystack: Discrete RRT for exploration of implicit roadmaps in multi-robot motion planning? I'm not sure what you're looking for - an algorithm, or an approach to convert the algorithm from Matlab to C++, but unfortunately I think either question is going to be an off-topic discussion rather than an answerable question. Is it possible to rephrase the question to make it less open-ended? Please also remember you can always discuss projects in Robotics Chat! $\endgroup$
    – Chuck
    Commented Aug 2, 2017 at 20:19
  • $\begingroup$ Thanks for that link, Chuck. I am not looking for an algorithm, really, but my question is along the lines of "what's the best way to represent what that paper calls "composite roadmap generated from the joint C space of multiple robots" in a computationally efficient way?" While that might not be a completely robotics specific question, I ended up posting here because people here might have expertise with planners like this! $\endgroup$ Commented Aug 2, 2017 at 20:27

2 Answers 2


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.


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...


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.