The purpose of this assignment is to plan using a Generalized Voronoi Graph (GVG) in 2D. Exploration is performed using a point robot without any dynamic constraints. This process is broken into three phases:
Access the GVG.
Construct the complete graph.
Use the GVG to get to the goal (if possible) or return failure (if not).
The code to implement this process is rather simple. At each planning step, the shortest distance to each obstacle in the world is found, and the obstacles are then sorted from shortest to farthest distance from the robot. All but the top three obstacles can be thrown out. Note here that this code assumes there are never ties for the third-closest obstacle.
A state machine is used to keep track of the current mode and to advance through the search. The states are:
ACCESS – accessing the GVG from the start state
TRAVERSE_EDGE – following an edge (double equidistance) to get to a meet point (triple equidistance)
MEET_POINT – at a triple equidistant point
EDGE – following an edge after departing a meet point
DEPART – using the GVG to get to the goal
Each state in the state machine is responsible for two things (in addition to maintaining data structures): it must determine which direction to step in order to continue the search, and it must determine the next state (which can be the same state again). Each state's process is described in greater detail below.
The ACCESS state is the simplest. It drives directly away from the closest obstacle. If another obstacle becomes equidistant or closer, then the mode changes to TRAVERSE_EDGE.
The TRAVERSE_EDGE state attempts to maintain double-equidistance. It draws a line through the nearest points on the two nearest obstacles, and then drives in perpendicular to that line. The direction along this line must not be in the opposite direction of the last motion on this edge (as determined by the sign of the dot product). The state changes to MEET_POINT if a third obstacle becomes equidistant with (or closer than) the first two. One additional feature is that if the distance to the nearest obstacle drops below the step size, then the direction is reversed. In this way, an edge that leads to a corner is effectively a loop edge with both end points at the nearest meet point.
The state only remains in MEET_POINT for a single cycle. First, the algorithm determines by a position test whether or not this meet point has been reached before. If not, a new data structure is created to keep track of the location and three edges connecting this to other meet points. Their initial directions are computed and stored using the same technique as described above in TRAVERSE_EDGE. After ensuring that a meet point structure exists, the algorithm then attempts to find an outgoing direction that has not yet been explored. If one is found, then a step is taken in that direction, and the state is switched to EDGE. If this meet point has no unexplored outgoing edges, then a search is performed for nearby unexplored edges using Dijkstra's algorithm. If one is found, then a step is taken along the neighboring edge which leads to that meet point, and the state transitions to TRAVERSE_EDGE. If no unexplored edges are found, then the state becomes DEPART.
The EDGE state exists for the purpose of keeping data structures. In this state, a new edge data structure is created. Its first node is made equal to the meet point just left. The MEET_POINT state sets the edge's second node if applicable. This state transitions to TRAVERSE_EDGE.
The DEPART state's job is to reach the goal. While TRAVERSE_EDGE mode, at each step a distance test to the goal is performed. If the current distance is shorter than any successful distance seen previously, than a line-of-sight test is performed. This test is successful if the goal can be seen from this position. The edge with the shortest line-of-sight distance is remembered. Then, when DEPART is entered, Dijkstra's algorithm is again used to find a path to the edge with the shortest line-of-sight. When the closest point is achieved a straight line is executed to the goal.
Two resulting runs are shown below.
The successful run movie shows a plan in a connected environment. It correctly navigates narrow corridors. By chance, the GVG goes directly over the goal, so the DEPART stage cannot be seen.
The failure case arises in an unconnected environment. In this case, a star-shaped obstacle has a hole cut out of it, in which the goal resides. After completely exploring the GVG, no line-of-sight path has been found from the graph to the goal, so the space is known to be unconnected. Thus, failure is returned.