I have implemented both A* and D* Lite as they are called for in this assignment. Each of these algorithms used a prior implementation created for research purposes. This software is object oriented, as it abstracts the notions of heuristic, state/connectivity, planner, edge, and cost-map. Thus, any graph or roadmap planner (A*, D* Lite, RRT, etc.) can be used in any world with any state connectivity and any (admissible) heuristic. Explanations of the software components and each planning algorithm appear below, followed by examples.
As mentioned above, the software is broken into multiple components. The planners will be explained in greater detail below, but first the functioning of the other parts of the system need to be better understood. A key feature is that the planner only searches (or generates on the fly) graphs -- it does not understand how they are constructed. The class State has an abstract method, getSuccessors(), which supplies the connectivity information to the planner by providing it with a list of neighboring States. State also contains the state variables and extra bookkeeping parameters for the planning algorithms. There are a number of implementations of State, including NHState (nonholonomic lattice), GridState, BLState (based on the 1991 Barraquand and Latombe paper that described a way to do discretized nonholonomic planning), ArcState (limited to constant curvature arcs), etc. The method getSuccessors() is implemented by each class to create a set of neighboring States (children in a tree). Each child State contains a parent pointer back to the previous State as well as a pointer to a Curve object that describes that path connecting the pair of States.
The Curves generated by getSuccessors() may traverse multiple costs in the 2D cost-map because States are not required to be adjacent in a grid sense. Any pair of States in the world is permitted to be joined by a Curve (an edge in the graph) provided that the path between them is properly described. Thus, a Curve may traverse many cells (including small corners of cells). The State is not aware of the cost map and so it cannot determine whether an edge may be blocked by an obstacle. The list of successors must therefore be passed through Curve's convolve() function to check whether the shape of the robot (position and heading aware) may pass along the line without collision with a lethal obstacle. In addition, convolve() will sum up the fractional costs of traversing portions of cost-map cells along the path, thus assigning a cost to an edge in the graph.
A group of heuristics are provided for the planner to use, as specified by a base class, Heuristic. These include Euclidean distance, grid distance (4-, 8-, or 16-connected), and an exact solution for an arbitrary graph connectivity which has been previously stored in a heuristic look-up table (HLUT).
The A* graph search uses an admissible heuristic estimate to provide the optimal answer with the optimal number of expansions (according to how close to perfect the heuristic is). Initially, the start state is inserted into the open list, which is a priority queue sorted according to f (the sum of cost from the start and the heuristic estimate of cost-to-go). On each iteration, the lowest f valued state is popped from the open list and expanded. The neighboring states which are new or can be reached by a less costly route are then placed on the open list. A state's object is responsible for knowing who its neighbors are and what shape the paths to get there are. The search terminates when the goal state is popped from the open list.
In this case, a nonholonomic vehicle (a Mars Rover) is being simulated. It has Ackerman steering, so it has a minimum turning radius. Thus, all possible outbound paths will conform to these constraints. The NHState mentioned above is used to represent this behavior. Paths must be continuous in position, heading and curvature across states. That is, incoming vehicle state from one curve equals outgoing vehicle state in another curve. As mentioned above, edges may traverse multiple 2D cost-map cells, and each state in the world is discretized so that the outgoing edges from one vertex land perfectly on other states. This recombination allows A* to enhance search performance.
D* Lite is a complex algorithm which I explained a variant of in class, so I won't repeat the explanation here. Briefly, the algorithm performs incremental search for a vehicle moving across a changing cost map. This particular implementation uses a four-connected grid for simplicity. D* Lite is implemented using the same building blocks described above.
The A* examples do not show the candidate edges because it would quickly become too crowded. The colors indicate the status of a cell (open list = cool colors; closed list = warm colors). The intensity of color is used to make up for the fact that the display is only 2D but the plan is happening in 4D. Each time a new state at the same position gets onto the list, the color intensifies. The final path is shown once it is found. In this example, you can see how doing a search with state continuity constraints all the way from start to goal helps the planner figure out that in order to travel down the narrow corridor, it is necessary to first swerve to the left and then come back around to the right.
In the second example, a large search problem shows a case in which the high-fidelity search is less helpful. It takes longer, and the continuity is less helpful in a more open environment. In addition, the search could easily have failed because the maximum iteration count ran out before the goal was reached.
The D* Lite examples are simpler pictorially, but there is an additional time element which must be explained. In the A* examples, a robot is performing a static plan, which will then be executed. In the D* Lite examples, the path is replanned and then executed in small increments. To make this interesting, obstacles can only be seen in a very limited sensor horizon (which is not marked on the screen). As the robot moves (the thick blue line) more obstacles (grey) are revealed. Once an obstacle is revealed, is is never forgotten. At each stage, the current D* plan is shown in orange. In the first example, the robot eventually reaches the goal after discovering a route around a 3-sided box. In the second example, the fourth side of the box is blocked too, and D* Lite fails once it is discovered to be a closed shape.