In this paper, I describe my work on the development of an efficient and robust algorithm for computing safe paths for a mobile robot. I have specifically designed the algorithm for use in the Autonomous Vehicle for Exploration and Navigation in Urban Environments (AVENUE) project. The environment currently being investigated is the Morningside Heights campus of Columbia University, but will eventually be expanded to other urban areas. In the general problem, one is given a two-dimensional map of the complicated region in which a robot will be operating. Such a map includes a variety of polygonal obstacles that are to be avoided. To determine paths along which the robot can safely move through this environment, I use an approach based on the generalized Voronoi diagram for a planar region with specified obstacles. Once this diagram has been constructed, I can search it to find robot paths that pass, with maximal clearance, around the obstacles.
The two-dimensional region in which the robot moves will contain buildings and other types of barriers, each of which can be represented by a convex or concave polygonal obstacle. To find the generalized Voronoi diagram for this collection of polygons, one can either compute the diagram exactly or use an approximation based on the simpler problem of computing the Voronoi diagram for a set of discrete points. In my work, I use the latter method. First, I approximate the boundaries of the polygonal obstacles with the large number of points that result from subdividing each side of the original polygons into very small segments. Second, I compute the Voronoi diagram for this collection of approximating points. Once this complicated Voronoi diagram is constructed, I then eliminate those Voronoi edges which have one or both endpoints lying inside any of the obstacles. The remaining Voronoi edges form a good approximation of the generalized Voronoi diagram for the original obstacles in the map. In this Voronoi diagram, I locate the robot's starting and stopping points and then compute the Voronoi vertices which are closest to these two points. I then use straight lines to connect the robot's starting and stopping points to these closest vertices. Special consideration is given to those situations in which one or both of the connecting straight lines pass through an obstacle. Once I have determined the starting and stopping vertices on the Voronoi diagram; I can implement a standard search, such as Dijkstra's algorithm, to find a "best" path which is a subset of the Voronoi diagram and which connects the starting and stopping vertices. This path can then be expanded to a path between the robot's original starting and stopping points. The method generates a route that for the most part remains equidistant between the obstacles closest to the robot and gives the robot a relatively safe path along which to travel.
The more traditional method of path planning makes use of visibility graphs. First, one segments the robot's free space into (convex) polygonal cells. Then, a representative point is chosen inside each cell and on each common edge of contiguous cells. The visibility graph is formed by connecting, with straight lines, each representative interior cell point to each representative edge point on the boundary of that cell. One can then join, also with straight lines, a robot's actual starting and stopping points to the visibility graph's nodes that are located inside the corresponding starting and stopping cells. A search of the resulting graph would yield a possible robot path.
This method is simple but has a significant drawback. Some of the cells necessarily touch the obstacles which form the boundaries of the free space. Therefore, parts of the visibility graph can be very close to these boundaries and can give rise to robot paths which come dangerously close to the obstacles. Such paths are theoretically safe, with an infinitely-accurate robot avoiding a direct collision with a precisely-specified obstacle. However, a base map is not absolutely perfect but contains to some extent measurement inaccuracies in the location of buildings and other barriers. In addition, errors in determining the robot's position can occur, despite the combined use of GPS data and odometry. Such errors can propagate and cause the robot to stray from its theoretical path. Because the visibility-graph method does not prevent the robot from getting close to obstacles, map inaccuracies and robot errors could lead to a disastrous collision.
A method using generalized Voronoi diagrams to generate a mobile robot's path greatly reduces the possibility that the robot will actually come in contact with an obstacle. The Voronoi diagram for a collection of given points (called sites) is the graph formed by the boundaries of specially-constructed cells. Each of these cells surrounds one of the given sites and has the property that all points within the cell are closer to the enclosed site than to any other site. Voronoi diagrams can be generalized to situations in which the given sites are two-dimensional obstacles rather than mere points. In this type of problem, the boundaries of the specially-constructed cells are equidistant between the two nearest obstacles. These cell boundaries form the generalized Voronoi diagram and are ideal for a mobile robot's path. By having the robot travel midway between the closest obstacles, one minimizes the chance that an error in the localization of the robot or an inaccuracy in the base map will result in a robot-obstacle collision.
Computing the Voronoi diagram for a collection of n given points (sites) in two dimensions is a well-known problem. The simplest way to obtain the diagram is to use the method of intersecting half planes. One connects each site, with straight lines, to every other site. For a specified given site, the perpendicular bisectors of all the lines connected to the other sites yield a set of half planes, each chosen to contain the given site. The intersection of these half planes forms the Voronoi cell for the given site. This process is then repeated for each of the n given sites. The boundaries of the computed cells are necessarily straight lines and form the edges of the Voronoi diagram. This method, though straightforward, is very inefficient and has a time complexity of O((n^2)(logn)).
There are, however, much more efficient methods for computing Voronoi diagrams for a collection of given discrete points. In 1986, Steven Fortune presented a novel and extremely efficient method for the computation. By using a sweep-line algorithm, he was able to reduce the time complexity to order O(n(logn)). Throughout my work on robot path planning, I have used Fortune's algorithm to compute the needed Voronoi diagrams.
In general, a sweep-line algorithm makes use of a horizontal line which extends across a planar map and which moves from the top to the bottom of the plane. As the sweep line travels, it gathers information above the given structures on the map. There is a subtlety which arises when one uses this type of algorithm to determine the Voronoi diagram for a specified set of point sites. The sweep line may pass the location of needed Voronoi edges before it encounters the point sites which actually generate these edges. Fortune solved this problem by continually updating the Voronoi diagram above the sweep line as this line encounters new structures. Of course, the algorithm does not actually have the sweep line travel down the map in small infinitesimal steps. Instead, the line jumps from the location of a special event down to the location of another special event. Two types of events occur in this algorithm: site events and circle events. The site events help determine the Voronoi diagram's edges, while the circle events determine the diagram's vertices. Fortune's method for a collection of point sites is spectacularly successful, and I have used it for all my calculations of Voronoi diagrams.
The obstacles encountered in robot path planning are actually extended two-dimensional objects, not simple points. One therefore needs an appropriate generalization of the Voronoi diagram in order to deal with such obstacles. Although the usual Voronoi diagram for a discrete set of given points only contains edges that are straight line segments, the edges of a generalized Voronoi diagram will contain parabolic arcs as well as straight lines. This complication stems from the fact that, in this generalization, there are three different basic generators of the Voronoi edges. Each two-dimensional obstacle can be represented as a convex or concave polygon, with straight lines as sides and points as corners. (I use the terms "side" and "corner" for the polygonal obstacles and reserve the terms "edge" and "vertex" for the graph of the Voronoi diagram.) The Voronoi edge equidistant between two obstacle sides (line-line) or between two obstacle corners (point-point) will be a straight line segment; however, the Voronoi edge equidistant between an obstacle corner and an obstacle side (point-line) will be a parabolic arc. It is not an easy task to generate such a Voronoi combination of parabolas and straight lines (linked together at point vertices). Even if one did produce this generalized Voronoi diagram exactly, one would still have to approximate each parabolic arc by a collection of small line segments so as to produce usable commands for the robot's motion. Okabe, Boots, and Sugihara have suggested a very useful approximation method for finding the generalized Voronoi diagram for a collection of two- dimensional obstacles. The boundaries of the polygonal obstacles can be approximated by the large number of points that result from subdividing each side of the original polygons into very small segments. In this way, one obtains a set of discrete points for which one can compute the ordinary Voronoi diagram. Okabe, Boots, and Sugihara suggest that one then eliminate each and every Voronoi edge that was generated by two points located on the same obstacle. The remaining edges presumably give a good approximation for the generalized Voronoi diagram determined by the original two-dimensional obstacles. Although I use the suggested discrete-point approximation for the polygonal obstacles, I have modified the elimination procedure. Instead of removing edges that were generated by points on the same object, I remove all edges that intersect an obstacle boundary or are located entirely inside an obstacle. To accomplish this, I simply check the endpoints of each Voronoi edge to see if either endpoint lies inside any of the obstacles. If either endpoint does, I then eliminate the edge. The benefit of this procedure is clear when concave obstacles are present. Pieces of the generalized Voronoi diagram I construct can actually go into the recesses of a building, which is useful if one wants the robot to travel into such areas. The elimination procedure of Okabe, Boots, and Sugihara would actually remove these paths, which are generated by points on the same obstacle but on different sides of a recess.
Once the generalized Voronoi diagram for a given set of obstacles has been computed, one must connect the robot's actual starting and stopping points to the diagram. I have developed two possible ways to accomplish this. The first method involves adding two sets of four points to the collection of original points which approximate the given obstacles. One set of the additional four points forms the corners of a small square which surrounds the robot's starting position (located at the square's center). The other four points are similarly placed around the robot's stopping position. I then compute the generalized Voronoi diagram for the enlarged set containing the obstacle points and these eight new points. The resulting diagram will necessarily have Voronoi vertices at the robot's starting and stopping points and will have Voronoi edges emanating in four perpendicular directions from each of these two vertices. In this way, one has the starting and stopping positions automatically in the diagram, with four possible paths connected to each of these positions. This method works very well, but it has a significant drawback. It requires the Voronoi diagram to be recomputed each time new starting and stopping points are used with the same map. This is particularly inefficient if one is going to use many different paths on the same map during one robot session. I have therefore developed an alternative method in which one computes the generalized Voronoi diagram only once for a given map and then is able to choose many different starting and stopping points on that map. First, one computes the generalized Voronoi diagram for the given map. Once the robot's starting point is specified, I sort all the Voronoi vertices by ascending distance from the starting point. This can be done using a quick sort, so the cost is only O(nlogn). Then, beginning with the closest vertex, I test to see if the straight line from the starting point to this vertex intersects any of the polygonal obstacles. This test uses a standard polygon-clipping algorithm. If there is no intersection, then I have an appropriate starting Voronoi vertex connected to the robot's starting position. If there is an intersection, then I repeat the test with the next closest vertex. The process continues until I find a Voronoi vertex that can be connected to the robot's starting point by a straight line that does not touch any of the obstacles. In practice, the closest vertex usually works. The entire procedure is repeated for the robot's stopping point.
Now that starting and stopping vertices on the generalized Voronoi diagram have been chosen, one has to search for a practical path between these two vertices. One could take a brute-force approach and simply do a weighted search (so that one always expands the cheapest node first). This would have a time complexity with exponential dependence on the number of vertices. Alternatively, one could take a greedy approach and use Dijkstra's algorithm. Since the number of vertices in the generalized Voronoi diagram for a typical map is enormous, it makes sense to use Dijkstra's algorithm even though it produces paths that are not necessarily optimal. This algorithm is greedy in the sense that, at each step, it always takes the path that seems the best in getting closer to the goal. Since the algorithm does not take into account the obstacles which are present, it might not always produce the best overall path. However, in almost all the practical cases I have tried, the paths produced are so close to optimal that the differences are negligible. Despite the fact that it is not a brute-force search, Dijkstra's algorithm does have the potential to take a long time to execute. However, at each point on the graph there are really only two or three possible paths that can be followed, and the algorithm turns out to be quite fast.
With a variety of goals in mind, I decided to implement the path- planning algorithm in three different ways. The initial implementation (which makes use of the first method for handling starting and stopping points) uses a series of C programs. This has been done to provide fast run time and to allow integration with existing code. The second version is implemented completely as a Java 2 application. This has been done to allow for greater portability and ease of integration into the AVENUE robot's existing systems as well as to provide for a graphical user interface. The final implementation is a port of the Java 2 application to a Java applet that conforms to an older API in order to allow the path-planning algorithm to be done via the web. This implementation isn't fully featured and has been written primarily for web-based demonstrations.
The first implementation has five distinct C programs. One program does the discrete-point approximation for the obstacles. Another one adds the four-point sets for the starting and stopping positions. A third program computes the full Voronoi diagram using Fortune's sweep- line algorithm, while a fourth performs the elimination of irrelevant Voronoi edges resulting from the discrete-point approximation. The final program does the search for an appropriate path. All of these programs are tied together by a shell script so that one can easily specify all the desired parameters and then execute the entire computation. The program that performs the sweep-line algorithm is based rather heavily on code written and made publicly available by Steven Fortune himself. A significant benefit of using the C implementation is the very fast run time. For example, the total run time for the entire path-planning algorithm in C turns out to be faster than just the final search portion of the Java code. On the downside, the C code does not provide a particularly easy way for the user to supply input to the program and there is a need to use a third-party package (I have used gnuplot) to view and manipulate the results.
I have also ported the entire application to Java. This version will be much easier to integrate into the AVENUE robot's existing CORBA framework and Java-based front end. My Java program provides a particularly simple graphical user interface which allows easy selection of starting and stopping points. The user simply clicks on the desired initial and final robot positions and then clicks "compute". This version of the path-planner can work in two different modes. In the first mode, a base map is initially loaded. The program then computes the needed Voronoi diagram and selects an appropriate robot path from the user-chosen starting and stopping points. In the second mode, the input is actually a pre-calculated Voronoi diagram, thereby speeding up the program's execution significantly.
The final version of the program is a Java applet demonstration, which lacks some of the capabilities of the main application. For example, it cannot compute the actual Voronoi diagram since that would be much to expensive an operation to ask an unknown remote computer to do. However, it does perform Dijkstra's search on a given diagram. This version of the program has been back-ported to an earlier version of Java so that it is compatible with most modern web browsers (without the need to download a plugin). The Java applet's path-planner has been developed strictly for demonstration purposes.
Using my path-planning algorithm, I have investigated a detailed map of the Morningside Heights campus of Columbia University and have found safe paths for a mobile robot having various starting and stopping points. The campus map is shown in figure 1 and contains 4,407 point sites in the discrete-point approximation. These sites result in the Voronoi diagram shown in figure 2. This diagram has 13,103 edges and still includes the irrelevant edges resulting from the use of discrete points. After the elimination process, the final generalized Voronoi diagram, shown in figure 3, has 3,788 edges. Figure 4 depicts a typical robot path going from a specified starting point to a specified stopping point. The next project is to have the AVENUE robot actually follow such a path.