1

I am trying to implement an algorithm called the 'rapidly exploring random belief tree'. The aim of this algorithm is to come up with a path for a robot, which, instead of connecting start and goal with a least distance metric or something similar, drives the robot into areas where it can get high accuracy measurements, and only then moves to the goal.

In the implementation of the algorithm, I start off by building a graph through random sampling of a given space. Every time a new point is sampled, a new edge is added to the graph. Initially, the error in the position of the robot will be high, as it won't be getting good measurements, but once the tree I am exploring through reaches the 'good' area, the robot's error suddenly drops, and with this knowledge of where the 'good' area is, I traverse backwards through the graph, pruning previously connected edges and connecting those vertices towards the good area. Given enough samples, in an optimal scenario, the graph would be propagating outward from the good area, hence connecting any two vertices would guarantee that you would pass through these points.

Here's where I have a problem. Once I start the traversal to prune and update my edges, the algorithm shouldn't prune the edges that connect the good area to the starting point, which would result in an infinite loop. An example is shown in this picture:

enter image description here

  1. The algorithm finds vertices 1, 2, 3 and 4. The connections are 1 -> 2, 2 -> 3, 2 -> 4.

  2. Now we find #5, which is a 'good' area for the robot. Visiting this vertex will magically enhance its accuracy in the future. I find neighboring vertices from 5: I find #3. I update the parent of #3 to #5 instead of #2. Now if there were more vertices surrounding #3, I would traverse those too, and update their parents to #3 etc., thus creating a path from #5 to all of those. (For more information, the traversal essentially computes the covariance of the robot if it were to move from #5 to #3 and whether that's more desirable than moving from #2 to #3).

  3. But in no circumstances should I update the parent of #2 to #3: because the only way the robot got to #5 in the first place was through #2.

To avoid this, a simple (but highly inefficient?) thing I did was to compute the shortest path to the vertex from where I began my traversal, and if any vertex I come across during traversal is part of this path, I don't update its parent. This works reasonably well, at the expense of a lot of computational cost because I need to do this every time I have to traverse backwards with an update, so I was wondering if there was a better, efficient way of doing this.

HighVoltage
  • 722
  • 7
  • 25
  • When you recompute shortest paths, are you making use of the matrix of costs from the previous computation? See e.g. http://stackoverflow.com/questions/4467884/updating-shortest-path-distances-matrix-if-one-edge-weight-is-decreased – mcdowella Feb 07 '17 at 05:47
  • Isn't that applicable only for weighted graphs? I'm not dealing with weighted edges here, really: I'm only concerned about the connections. – HighVoltage Feb 07 '17 at 06:08
  • you can reduce the cost of an edge from +infinity to 1 when you add it to the graph. For +infinity you can use any number you know is higher than the maximum possible distance from one point in the graph to another. Depending on the details of your minimum distance calculation you may not need to use +infinity explicitly. Your diagrams make it look like you are based on a grid, in which case I would have thought that Dijkstra distance finding and reducing edge costs would work quite well. – mcdowella Feb 07 '17 at 06:14

0 Answers0