Navigation Part 1: Geo-fencing

Randall Maas 8/10/2012 4:07:15 PM

The first step is to have a plan to keep the bot in the yard - and out of the plants. This means the knowing where the bot a can (and can't) go. Geo-fencing is a virtual perimeter that keeps the drone (robot) inside the "fence." DIYDrone's "Ardupilot" and "Ardupilot-Mega" implement a fence. However, DIYDrone's fences appear to be a simple box. My yard (and parks) have irregular shapes.

My normal "go to" reference on geographic data structures is  Fundamentals of Spatial Information Systems Robert Laurini & Derek Thompson, 1992 Academic Press. But it has big, hairy data structures for this problem - things like R-Trees. Here is an approach that better serves my bot.

The in crowd

First, we represent the region (my yard) as a polygon:

First, how do we know if a point is in the region - or not in the region? The answer is the winding number. When the winding number is zero, the point is not in the region; otherwise, the point is in the region.

This is useful for telling if the bot has found itself outside of the yard... or that a point it is considering will be outside the yard.

The out crowd

The utility of the winding number is that we can add a second polygon that excludes an area of the region. For instance, my house, flower beds, and other crud around my house:

Notice that the polygon for the "good" area goes clockwise, while the polygon for the keep-out is counter-clockwise. This is critical:

This is put together to find out if a point is within the geo-fence. First, the winding numbers are limited -1, 0,+1. Next, compute the winding number for the point with respect to each polygon. Sum the numbers. If the number is negative, the point is in the fence; otherwise, it is not.

(This approach can be refined to use a tree structure to reduce the number of polygons we are looking at).

Boundary conditions: Don't hop over or pee on an electric fence.

Finally, we need to tell if we are crossing a boundary. This is used to tell if we're going to go outside of the yard or go into a bad area. For instance:

Both the start and the goal points are in the region' but if the bot were to take the direct path, it would get into trouble.

Let's call the two points: P and Q. P is the current point, and Q is the end point. The approach is simple:

For each polygon:
	For each line segment (E to F) of the polygon
		if the line segments P->Q and E->F cross, the path crosses the fence

This just leaves the question of whether or not the points cross.

We have to do two tests:

  1. Are P and Q on opposite sides of E->F ?
  2. Are E and F on opposite sides of P->Q ?

If both are true, the line segments cross.

We can tell the points (P,Q) are on opposite sides of a segment (E,F) by taking the cross product :

	if sign(U × V) != sign(U × W)  then opposite sides

or

	if sign(Ux*Vy-Uy*Vx) != sign(Ux*Wy-Uy*Wx) then opposites sides

where

Note: this approach only works if we don't have a "good" region inside of a "bad" region.

Next time

We'll look at navigation planning next time