Robot Motion Planning

One problem where graph search plays a major role is in planning the
motions of a simulated robot so as to help it navigate in an
environment full of obstacles.  We will call this a "robot motion
planning" (or just "motion planning") problem.  The major components
of a motion planning problem formulation are illustrated in Figure 1.
We have a moving object (the robot), which we will assume is a planar
polygon, that can rotate and translate under our command.  We also
have a set of known obstacles, also modeled as planar polygons, that
are stationary.  We specify both an initial and a goal position and
orientation for the moving object.  Our objective is to find a path
from the start state to the goal state so as to avoid any collisions
between the moving robot and the obstacles.

This problem is motivated by actual robotics problems, for example,
off-line programming for a welding robot in an automobile assembly
plant or control of mobile delivery robot in a hospital building.  It
is also potentially useful in other applications, such as laying out
the pipes in the interior of a ship or checking that a product
designed on an Computer-Aided Design system can actually be
assembled.  In "real life" the objects will be much more complex and
be three-dimensional, but the simple problem we will be looking at is
illustrative (if not representative).

Configuration-Space

The position and orientation of the robot is specified by three
numbers, (x,y,theta), the (x,y) position of some designated point on
the robot (call it the "reference point") and the orientation of the
robot (relative to some nominal orientation).  We assume that the
robot rotates about the reference point.  To make things simple, we
will assume that the robot is described as a polygon (a list of
vertices) in an initial coordinate frame and that the origin of this
frame is the reference point for the robot.  To place the robot at a
specified location in the world frame, we rotate the polygon by theta
about the origin of it's frame and then displace all the rotated
vertices by (x,y).

We will denote the (x,y,theta) parameters necesary to completely
specify the placement of the robot as a "configuration" of the robot.
We can then think of these parameters as a point in a space of
configurations (abbreviated as C-space).  Obviously, any path of the
robot can be thought of as a path in this space.  It should also be
clear that there are points in this C-space that correspond to
placements which generate collisions between the robot and the
obstacles and there are other points that correspons to collision-free
("free") placements of the robot.  In fact, assume that there is a
single obstacle in the world.  The set of configurations that lead to
collisions between the robot and that obstacle correspond to the
mapping of the obstacle into the configuration space of the robot.  We
will refer this set of collision-inducing configurations as the
"configuration space obstacle" or "C-obstacle" corresponding to the
input obstacle.  Note that in this case the C-obstacle is (generally)
a three-dimensional entity, since the robot has three
degrees-of-freedom (x,y,theta) required to specify its position and
orientation.  If the robot had only two degrees-of-freedom (suppose it
can't rotate) then the C-space (and the C-obstacle) would be
two-dimensional.  For a three-dimensional robot moving in
three-dimensional obstacles, the C-space (and the C-obstacles) are six
dimensional - corresponding to three translational and three
rotational freedoms.

Given a mapping from the input space obstacles to the C-space
obstacles, we can re-formulate the motion planning problem 
as one of finding a path from the start configuration to the goal
configuration while staying outside of all the C-obstacles.  This
problem is completely equivalent to the initial problem and has some
desireable properties - in particular, this representation can make
the search process easier to implement (as we will see).

Of course, this approach requires us to devise an algorithm for
computing the C-space obstacles.  This is, in general, a difficult
problem but for the simple case when the robot and the stationary
obstacles are all (planar) convex polygons and the robot is restricted
to translation, there is a simple and efficient algorithm.  We
describe it briefly here without justification.  

o The robot polygon is defined relative to its reference point, which
is defined to have coordinates (0,0).  Define a new polygon which is
the "negative" of the robot polygon, i.e. every vertex position (x,y)
is replaced with (-x,-y).

o Note that one can associate an angle range with each vertex of any
polygon.  The ends of the range are determined by the orientations of
the polygon edges that impinge on the vetex.  For a convex polygon,
these ranges are totally ordered as we traverse around the polygon,
i.e. the angles increase as we go counterclockwise around the polygon.

o Go around the obstacle polygon and, for every vertex, identify any
vertices of the negated robot polygon whose angle range overlaps that
of the obstacle vertex.  For each such negated robot vertex, output a
vertex that is the sum of the obstacle vertex and the negated robot
vertex.  

o The resulting vertices define a new polygon which is precisely the
translation-only C-obstacle for the robot polygon relative to the
input obstacle.

Since the angle ranges for a convex polygon are already sorted,
identifying the overlapping ranges can be done in time proportional to
the sum of the number of vertices in the robot and the obstacle.
Thus, the complexity of this algorithm is linear in the complexity of
the input polygons.

This simple algorithms allows us to compute C-obstacles for (x,y)
C-space.  However, we need to be able to compute the C-obstacles for
an (x,y,theta).  It is not hard to convince oneself that this is
harder; in particular, the resulting obstacles have curved boundaries.
So, instead, we are going to approximate the (x,y,theta) C-space by
using a series of "slices" (cross-section) each defined for a fixed
orientation (theta) of the robot.  Each of these slices will be a set
of polygonal C-obstacles that we already know how to compute.  If the
slices are defined for nearby angles, the result should be a
reasonable approximation of the actual C-space (more on this later).

Visibility-Graph

Now that we have an approximation to the C-space obstacles, how do we
actually go about finding a collision-free path?  Once again, let's
start by considering the simpler case of translation-only (fixed
theta).  There are many ways one could set about finding a path.  For
example, one could define a grid that includes the start, the goal and
all the C-obstacles, fill in those grid locations that overlap the
C-obstacles and then search for a path in the grid.  This approach is
very general but somewhat wasteful, there would be many, nearly
equivalent, grid points to explore.  

An alternative is to define a "visibility graph" (V-graph) in the
C-space.  The basic idea in a visibility graph is to connect any two
vertices of C-space obstacles that can "see" each other, that is, that
can be connected by a line segment that does not penetrate any
obstacles.  Of course, we also have to connect the start and goal
configurations to any obstacle vertices that they can see.  Then, we
can look for the shortest path in this graph.  Interestingly, one can
show that the shortest actual path between the start and the goal will
in fact be a path in this graph (think of pulling a string taut
between the start and goal, it will in general touch vertices of the
polygons).

The key drawback of the V-graph is its size; potentially, if there are
N vertices in the obstacles, the V-graph can have on the order of N^2
edges.  This is not good.  Not only does it define a large graph to be
searched, but checking each of these potential edges to see if they
cross any obstacle will take time.  There is one simple trick that
limits the number of edges that need to be considered.  Note that for
a V-graph edge to be part of an optimal (shortest) path, that edge must
be tangent to the obstacle (think once again of pulling a string taut
over the obstacle).  Note that between two convex obstacles there are
at most four lines that are tangent to both of them, independent of
the number of vertices.  Although the size of the graph still grows
quadratically with the number of objects in the input, this trick
helps quite a bit.

The V-graph approach has one substantial drawback.  The paths that are
represented in the graph move as close to the obstacles as possible.
This is required if we insist that we want the shortest possible paths
but, in practice, these paths are susceptible to small errors.  One
way of coping with this is by "growing" the robot a little bit, to
give it a little margin of safety.

How do we deal with the third degree-of-freedom?  First, we can build
a V-graph for each "slice" of the C-space.  However, each of these
edges represent a pure translation, albeit in different orientations.
Then, we can add edges that connect vertices in adjacent slices, these
represent some combination of change in orientation as well as
position.  However, we need to figure out what condition these edges
must satisfy.  When operating at a single orientation we make sure
that the potential edge does not penetrate any of the obstacles.
However, when connecting two slices (A and B) we have two distinct set
of obtacles to deal with (Obst-A and Obst-B).  Do we insist that the
edge be outside both set of obstacles?  If we do, then clearly we
could translate between the end-points of the edge in either
orientation, they would both be safe.  In general, however, it is
sufficient that the edge be safe in one of two slices.  Then we can
carry out the translation in the free slice and we can change
orientation at the endpoints of the edge.  Note that we know that the
endpoints are safe in their own slices, otherwise they wouldn't be
part of the V-graph for their own slices.

Note that when we change orientations from one slice to another based
on the "safety" of a point, we are pushing our luck.  Technically, the
points outside the C-obstacles in a slice identify points that are
safe for a single orientation only.  When we change orientations from
one slice to another, we traverse all the orientations in between the
two slices.  We can handle this by "expanding" the robot polygon to
cover the space swept out during the change of orientation and using
this expanded polygon to compute the C-obstacles.  Actually, we can
use this to provide us with the safety margin we had mentioned
earlier.

Search Issues