Navigation Part 2: Planning the gross route

Randall Maas 8/11/2012 11:53:50 AM

If we have a clear path from a starting point to the goal - that is, one that does not cross a boundary - our route planning is easy. It is just a straight line.

But what if that path crosses a boundary though?

In that case, we need a more sophisticated approach.

Fortunately, Nils Nilsson told us almost everything we need to know about route planning: the A* search algorithm.

Planning a route is pretty much two parts:

  1. Plan the big picture
  2. Do the fine planning for the next step you have to take

We are going to create a gross (or coarse) route quickly, without a huge memory or processor load. This type of route has the advantage that it does not have to be re-planned as the bot experiences minor deviations from the ideal path.

Transforming the region into a graph

The catch is that A* works by traversing a graph. So the geographic region has to be converted into a graph, small areas that are connected logically.

The simplest method to transform the region into a graph is to

  1. 'bin' the points by dividing them a constant, and truncating the values (i.e. removing the fractional part).
  2. The points neighboring P are the 6 surrounding points (the combination of adding -1,0, and 1 to each axis), so long as the point is in the region and the path from P to the point doesn't cross a boundary. (See the geo-fencing entry for more details)

If the region is divided into very small steps, we can skip the boundary crossing check.

To put it another way, the division constant controls how much work we do. The ideal size is coarse enough that we are only checking a handful of points to see that they are in the region (and not crossing a boundary).

The good news is that, unless the region tree is describing a twisty maze with lots of small passages, too coarse is a huge division value. We probably do not need to worry about it too much.

Filtering a path

Once the A* algorithm produces a path thru graph, I like to simplify it. This amounts a filtering stage to remove unnecessary waypoints. What is unnecessary?

Smoothing the path is simple:

	Take three successive points, and compute the area of the triangle
	if (area < threshold1) or 
	   (the line segment from the first to third point doesn't cross a region boundary)
		 remove the middle point

The threshold is chosen to be small enough that the path will not cross the boundary.

Making the fine route

Now that we have the coarse route, we can benefit from a more detailed route. This route isn't about being fancy. It's about shortening the distance, since the coarse route is so ... coarse.

  1. Get the next node (P) on the coarse path
  2. If distance between that node and the current position < threshold
     Remove the node from the coarse path
     Repeat from step 1
  3. If there are no more points in the coarse path, return P as the fine path
  4. Get the second node (Q) in the coarse path
  5. Plot an A* route from the current position to the point Q, using smaller division steps
  6. Throw out the second half of this path
  7. Filter this path as before
  8. Return this path as the fine route

It is very simple to use the fine path:

  1. If fine path is empty, stop
  2. Get next node (P) on the fine path
  3. If distance between that node and the current position < threshold
     Remove the node from the coarse path
     Repeat from step 1
  4. Set proximal goal as the node P

With new position information, that process is repeated.

Next time

Next time I'll look at the steering