In the last lecture, we talked about graph-search algorithms: depth-first, breadth-first, iterative deepening, uniform-cost, greedy, and A*. Given a clean, discrete graph, these algorithms have nice properties. For example, A* makes good use of a heuristic to reduce running time, while still optimizing path cost; iterative deepening can be used to find optimal paths while reducing memory requirements.
But how can we represent a configuration space as a graph? There are several problems that come up:
- Configuration space is continuous
- Configuration space may be high-dimensional (leading to a large graph)
- The shape of the obstacles in the configuration space may not be known explicitly
- Sensors may only show the robot a small portion of the space.
- There may be limitations on the directions the robot can move, making it difficult to connect configurations.
- The obstacles may be moving in unpredictable ways
We will first focus on the problem that configuration space is continuous. Initially, we will assume that the robot can move freely in any direction in configuration space (if there is not an obstacle there), that the obstacles don't move (or if they do, they move along fixed paths), that the environment is known fully ahead of time, and that the configuration space obstacles can be constructed.
For almost all motion planning problems we will make use of key abstraction: the configuration of the robot is represented by a point in parameterized configuration space. This reduces the motion planning problem to finding a trajectory or path for that point. This means, for example, that the problem of planning a motion or path for a snake-like robot with 20 degrees of freedom can be thought of in the same way as planning a path for a point robot in the plane. In fact, planning complicated manipulation problems can be abstracted in the same way. For example, a robot must assemble five planar rigid bodies into a completed product in a factory. Let 
. "Obstacles" in this fifteen dimensional space represent any configuration in which there is a collision between any of the parts.
Discretizing 2D motion planning problems
We will consider first the problem of planning the motion of a translating rigid body among polygonal obstacles in the plane. The configuration of the robot is given by 
, the location of the reference point of the robot. The configuration space has two components: 
, the configurations where there is no collision, and 
, the configurations where there is a collision.
In this case, we can explicitly construct the configuration space obstacles: they are polygons constructed by taking the flipped robot shape and dragging it around the boundary of the polygonal obstacles. (This operation is known as the Minkowski sum of the flipped robot and an obstacle.) So we can reduce the problem of planning a path for a polygon among polygons to the problem of planning a path for a point among polygons.
We now look at various methods of constructing a searchable graph that represents the connectivity of the configuration space.
Grid cell decomposition
The first approach is simple. Place a grid of some resolution over the configuration space. Each grid will be a discrete node in the graph. Adjacent grid cells (in either four or eight directions) are connected by edges. If a cell contains any piece of the obstacle, we will not include that cell in the graph. Once the graph is constructed, we could use any graph search algorithm to find a path.
There are likely to be many paths from the start to the goal. We probably want to find one that is relatively short, and we want to minimize the computation required to do that. A reasonable choice might therefore be an A* graph search; this will guarantee short paths if the heuristic is optimistic. A good choice of heuristic might be the Manhattan distance (if the graph is considered to be 4-connected) or Euclidean distance (if diagonal motions are allowed and the graph is considered 8-connected), ignoring obstacles. These heuristics are optimistic, because obstacles will only make the true path cost higher.
An alternate problem is to find paths from every starting configuration to a single goal. The wavefront planning algorithm discussed in the last lecture, which computes the cost-to-go at each configuration, would be a good starting point, and is an example of a technique called dynamic programming, where intermediate results are stored by an algorithm in cells.
This grid method will work in higher dimensions, obviously. It may not be possible to construct the boundary of the configuration space obstacles directly, but it is easy to test if a particular configuration causes a collision. For example, you might simply render the obstacle and the robot in a particular configuration using graphics hardware; if multiple pieces of the robot or obstacle end up at the same pixel (or voxel) end up at the same location, then that configuration is in and can be removed from the graph during the search.
How should the cell size be chosen?
If the cells are large, then obstacles will seem larger than they are, since any piece of an obstacle in a cell causes the cell to be removed from the graph. This may cause the algorithm not to return a path even in the case where one exists. We say the algorithm is resolution complete: if a path exists at the resolution considered, the algorithm finds it. This is unsatisfying.
If the cells are small, then the graph will be very large, and the run-time of the algorithm will be very bad.
A hybrid solution is possible, called hierarchical cell decomposition. Divide the space into large cells. Recursively divide each cell containing an obstacle. That way, there are a few large cells that are safe, far from obstacles, allowing rapid search, and there is still a reasonably-high resolution for the boundaries of obstacles. The behavior of the search is still unsatisfying — as we will see, shortest paths tend to brush closely by obstacles, so much of the search time will be spent in the region of the grid with the smallest cells. In higher dimensions, the boundary of may be very convoluted.
Exact cell decomposition for polygonal obstacles
One of the issues with choosing the resolution for grid cell decomposition is that very many cells may be needed to approximate the border of an obstacle, since the cells are not aligned with that border. What if we allowed cells to be more general polygons? The boundary of the obstacle could be used to form the boundary of the cell, and cells would either contain only a slice of or .
One approach to building this decomposition algorithmically would be to draw a vertical line through each vertex. Find the intersections of these vertical lines with the boundaries of the obstacles. Cells each have two vertical boundaries, and up to two boundaries (the upper and lower) that are formed from segments of the obstacle boundaries. The cells are possibly-open quadrilaterals. We can see from the shape of the cells that if two cells touch along an edge, any path from one cell to the other through that edge will not leave those cells, so two free cells can be easily connected by a path.
The number of cells depends on the number of vertices, rather than on some arbitrary resolution.
Visibility graphs
The next approach relies on an idea which may seem obvious: if there is a path between the start and goal, then there is at least one shortest path between the start and goal. This is not true for all motion problems, but it does hold for polygonal obstacles in the plane. It turns out that the structure of the shortest paths is quite simple, and there is a finite number of candidate shortest paths. We therefore restrict our search only to candidate shortest paths.
The idea is as follows: consider any path from the start to the goal. If we shorten that path as much as possible without breaking through the boundary of an obstacle, there will be no curves in the resulting path, but only line segments. The endpoints of these line segments will be the start, the goal, or a vertex of a polygonal obstacle.
If we connect all pairs of vertices (treating the start and goal as vertices), we will have a graph with 
edges. Some of those edges pass through obstacles; we remove such edges, leaving what is known as the visibility graph. (We say adjacent vertices in the graph can "see" each other, since they are connected by line segments that do not intersect obstacles.)
Skeletons; Voronoi diagrams
Visibility graphs reduce the dimensionality of the search space from 
to a graph that represents the connectivity of the space; the dimensionality of this graph is 1, since it is made up of connected line segments. The graph is connected if and only if is connected.
We call a one-dimensional structure that is connected if and only if the embedding space is connected a skeleton of the space.
A visibility graph is a skeleton of a planar space with polygonal obstacles. If the obstacles are not polygonal, it is not obvious how to construct the visibility graph. It turns out that an application of Snell's law describes the tangents to smooth obstacle boundaries, but we will not go into the details.
There are other ways to generate a skeleton of a two-dimensional space. One example is the Voronoi diagram. Label each point in by which obstacle it is closest to. It turns out that the boundaries of these regions form a skeleton of the space.
Higher dimensions
Things get trickier in higher dimensions. First, consider shortest paths among polyhedral obstacles in three dimensions. The paths are still made up of line segments, but the line segments may contact edges of the polyhedra rather than the vertices, so the shortest path may not be contained on the visibility graph of the vertices. (Consider taking a path around a cube from start to goal. Think of the path as a string, and tighten it. There is no reason to expect that it contacts the cube only at vertices!)
The Voronoi regions are now 3D cells, and their boundaries are surfaces. No skeleton to be found here, either. One idea is to build skeletons on those surfaces, and create a hierarchy for path planning.
Exact cell decompositions are also hard in higher dimensions — it tends to be difficult to represent boundaries of the obstacles, and the boundaries may be concave.
The curse of dimensionality
The number of nodes in a graph used for motion planning tends to scale exponentially with the dimensionality of the space. For example, consider grid cell decomposition, which is straightforward to implement in any dimension. With even a coarse division of cells, the number of cells explodes by the time we reach even five or six dimensions.
Non-integrable (non-holonomic) differential constraints
Most of the constraints we have looked at so far restrict the configurations that the system can achieve. An unconstrained system has N * D degrees of freedom, where N is the number of particles, and D is the dimensionality of the embedding space (2 in the plane, and 3 in space). There are two ways of writing the constraints: either explicit constraints on the locations of the particle, or parameterized equations that describe the configurations of the particle as a function of (fewer than N*D) parameters. For example, consider a simple particle in the plane constrained to move on the unit circle. We could write the constraint explicitly:
(1)
or parametrically:
(2)
Sometimes the constraints on the system should constrain velocities and not configurations, however. For example, consider a differential-drive robot like the Create. Let the configuration be given by 
. It is clear that any configuration can be achieved, if there are no obstacles, but this does not tell the whole story — the robot can drive forwards, but not instantaneously sideways.
The constraint should be written in terms of velocities, or time-derivatives of the configuration variables. Remember that the space of possible directions of motion at a point in the configuration space is called the tangent space; differential constraints constrain the tangent space.
Constraints on the tangent space of the steered car do not restrict the possible configurations of the car, and some surprising motions (like parallel parking) may be need to move from one configuration to another. Our planning system will have to deal with this problem in some way.
It is possible that a differential constraint actually ends up constraining the possible configurations of the robot in such a way that the constraint could have been written as a constraint directly on the configuration space. We say that such a constraint is integrable or holonomic. In fact, given any configuration-space constraint, we can create a resulting differential constraint. Take for example the point system moving on a unit circle.
(4)
Taking the time derivative, we get
(5)
a differential constraint. Geometrically, the interpretation is that the tangent vector (velocity vector) must be perpendicular to the vector describing the location of the point. If we were just given the differential constraint, it might take some careful thought to realize that the robot is constraint to the unit circle.
Integrable or holonomic constraints are "easier" to deal with in some sense; if a path exists in configuration space between two configurations, the robot can follow it. Non-integrable or nonholonomic constraints like that on the differential drive are harder, because of the need to find a feasible path in configuration space that respects the constraints on the tangent vectors.
Is there a simple way to test if a set of differential constraints are holonomic or non-holonomic? It turns out there is, though we won't go into the details. We apply a theorem called the Frobenius theorem, which tells us to apply a mathematical operation called the "Lie Bracket" to the motions resulting from the available controls for the robot. Examining the results of this operation tells us in a very straightforward way whether or not the differential constraints restrict possible configurations of the system or not.
