Coordination of 3 Car-like Robots
March 22, 1997
Project Description
The goal of this project is plan paths for 3 car-like robots that
satisfy non-holonomic constraints.
The project consisted of several parts:
- Computing a local-minima-free potential of the workspace and
writing a planner using best-first search to find a path.
- Using these computed holonomic paths, computing a path that
satisfies non-holonomic constraints.
- Generating paths for 3 robots, such that they do not collide.
Each of these goals and the results are described below.
Motion Planning Examples
Note that all of these examples have been stripped to contain paths of short length.
A holonomic planner
To plan a holonomic path, I first computed C-Obstacles for each of
the obstacles and a rotating robot. The C-Obstacles were
represented in discretized form in a 3-d bitmap. Given a goal, the NF2 potential field
was computed. Best-first search was used to determine a path from
a start point to the goal. An example of a holonomic path can be seen
in the movie Holonomic.
Modifications and Observations
One of the biggest problems I encountered with implementing NF2 was
the time it takes to calculate exact distances from an obstacle to a
point. To improve running time, I computed approximation of NF2. The
technique used involved sweeping over each level of the bitmap in 8
directions (horizontally in both directions, vertically in both
directions, and in both directions along the diagonals). In each
sweep, when an obstacle was found, the distances from the obstacle
were computed for the steps "behind" the obstacle in the sweep. The
distances were computed by incrementing a count (1 in the horizontal
and vertical directions; sqrt(2) in the diagonal) and assigning the
count until the cutoff was reached. The minimum distance found in all
sweeps was used as the final distance. A typical sweep over an
obstacle would result in the "distances" shown below. 0's represent the
obstacle, and s2 is used to abbreviate sqrt(2).
5s2 4s2 3s2 3 3 3 3 3s2 4s2 5s2 6s2
4s2 3s2 2s2 2 2 2 2 2s2 3s2 4s2 5s2
3s2 2s2 s2 1 1 1 1 s2 2s2 3s2 4s2
3 2 1 0 0 0 0 1 2 3 4
3 2 1 0 0 0 0 1 2 3 4
3 2 1 0 0 0 0 1 2 3 4
3 2 1 0 0 0 0 1 2 3 4
3s2 2s3 s2 1 1 1 1 s2 2s2 3s2 4s2
4s2 3s2 2s2 2 2 2 2 2s2 3s2 4s2 5s2
5s2 4s2 3s2 2 2 2 2 3s2 4s2 5s2 6s2
Although this is only an approximation of the distances true NF2
would use, I could detect no difference between paths produced with
true distances and these approximated ones. The speedup seems
to warrant using the approximate version.
I encountered a different problem with best-first search suggested in
the traditional NF2 algorithm. Here, the prioritization of
configurations is only based on the values from the potential field.
Since this heuristic does not take into account the length of the path
computed, many paths produced were far from optimal. The performance
was greatly improved by taking into consideration the path length when
considering the next best step. The difference in results is shown in
Figure 1 a and b below.
 |
 |
Figure a.
Prioritization by potential only. |
Figure b.
Prioritization with path length. |
A non-holonomic planner
The holonomic paths computed were
used to compute non-holonomic paths using Reeds and Shepp curves. The
shortest curve was found between end points of the path. If the
discretizion of the curve showed that collisions occurred, curves were
attempted from the start to midpoint and midpoint to start. The path
was recursively split in this way until a path was found. A movie of a
non-holonomic path can be seen in NonHolonomic.
Modifications and Observations
To implement these paths, I use the Reeds and Shepp curve code
provided by Professor Latombe. I encountered one very serious
problem. Often, the shortest curve between two consecutive
configurations of the holonomic path still produces collisions. In
order to continue searching for a path, I interpolated between the
consecutive configurations. Even so, some paths could not be computed
since at some point, due to limited precision, the configurations
cannot distinguished. In these cases, I tried to simply approximate
by entering the point at which failure occurred, but this method lead
to ugly solutions where the paths did not look non-holonomic.
Problems where maneuvering space was very tight had the most problems.
Coordinating paths
To coordinate paths, I first computed the non-holonomic paths for all
3 robots. Next, I built a coordination space representing the time
points in the paths where collisions between robots occurred. To find
collision points, I took pairs of robots, treating one robot as the
obstacle,and the other as a moving robot. By rotating the resulting
C-Obstacles as necessary, I could determine whether an collision
occurred between pairs of robots at any configuration. Next, I
constructed 3 2-d bitmaps showing the collisions between pairs of
robots. These bitmaps are the projections of the 3-d collision space.
From these bitmaps, I constructed a 3-d bitmap which showed where
collisions occurred. I used this 3-d bitmap as the basis for a new
NF2 search which specified the coordination of the paths. See the
movies 3Car1, 3Car2 and 3Car3 for examples.
Modifications and Observations
This coordination scheme is
very limited, since it cannot handle cases where one robot must move
out of the way of another. Since the paths are predetermined, and
only the rates at which each robot moves along its path can be
controlled, problems requiring complex coordination cannot be solved.
A combination of this method and a prioritization method would help a
great deal. On the other hand, the simple method is very fast and
works for most cases. I made another interesting discovery since
my original NF2 planner allowed either movement toward or away from
the goal. In terms of path coordination, this equates to letting
robots move forward OR backward along their paths!
This actually allows for much more freedom, and many difficult
problems can be solved in this way even though they could not with a
forward-only planner. An early example of such coordination between
holonomic paths can be seen in the movie FrontBack.
Since a car is allowed to move in reverse as well as forward, I
allowed for backtracking in the path coordination in the final version
of the project.
Source Code
Many thanks to Steve LaValle for a great class.
Comments? Send to: Laane@cs.stanford.edu