Sean Hyde
16-735 Robotic Motion Planning
October 9, 2007
Homework #4

This is my implementation of the D*Lite planning algorithm.

The algorithm discretizes the world and classifies the obstacle squares. It then forms an 8-point connected graph of the nodes and finds the shortest path between the start and the goal.

The 4-connected squares have a cost of 1, the diagonal squares have a cost of 1.4, and the obstacles have a cost of 100,000.

Most of the algorithm is done is Java for speed reasons and because it supports pointers. The four classes I created were:

Node.java

Each node represents a discretized square of the world and contains the X,Y coordinates as well as a list of its neighbors and the cost to each of those neighbors (stored in a hash table). Two nodes are considered the same if they share the same x,y coordinates.

QNode.java

QNodes are containers for nodes in the priority queue. They contain a node as well as the sorting keys used in the algorithm.

Dstarlite.java

The dstarlite object actually performs the D*Lite algorithm. When an instance is created, it initializes the world map it is given (which is a two dimensional array of Nodes). It contains all of the procedures mentioned in the Lecture version of the D*Lite algorithm. The only change is that the "main" procedure has been renamed "doTheAlgorithm".

DStarRunner.java

The DStarRunner class is a static class meant to provide an interface back to Matlab. By calling the static methods contained therein, a world map is created and the algorithm is performed on it.

Algorithm Implementation Details:

1. Create the world map

2. For each node on the map, check to see if it is inside of an object. This step uses the p_poly_dist procedure created by Michael Yoshpe to see if a point is inside or outside of an object.

3. Put the goal on the queue and compute shortest path back to the start.

4. Step along the path checking to see if the edges are correct.

5. If they are wrong, correct and recompute shortest path.

GUI Considerations :

Although Matlab supports the creation of Java objects and the use of Java, it did not play well with the GUI. Using both user-defined Java classes and the GUI would regularly crash Matlab when a dialog box was created. As such, the GUI is not used in this assignment. Instead, the user can replace the load command on line 4 with any other environment created by the GUI.

Algorithm Considerations:

To test this algorithm, the two different obstacle lists are passed to the algorithm. One set has all of the obstacles (denoted by "real--" ), the other is missing the first obstacle. The planning algorithm is run on three different combinations of these. In the first, only the bad information is used. This can result in a path that goes through the object. In the second, the bad information is given for the original plan to be built with, and then the good information is used to check the edges for correctness. This does not work correctly for reasons I have not been able to determine. In the case, the algorithm is only given the correct map and successfully plans and follows a path that does not intersect any obstacles.

Results:

Here are 3 images described in Algorithm Considerations.

Source Files: dstarlite3.m      Node.java     QNode.java     Dstarlite.java     DStarRunner.java     p_poly_dist.m