Introduction

  There are millions of humans in cars every day. Certainly some people's planned paths conflict and intersect with others' every day. Yet thanks to traffic lights and laws, each of us is able to get around to our destination successfully without much problem (assuming the usual human knowledge of knowing how to obey the law, drive and avoid accidents). Assuming, however, that all traffic lights break down and laws are nonexistent, many of us would agree that if drivers are reasonable, they should still be able to get around by using human reasoning to prevent undesirable collisions (i.e. with the negotiating skill and willingness to compromise if necessary, the drivers involved in any immiment collison can find a way for all to reach final destinations). Given that motion planning in general is a NP-hard problem, is it at all feasiable, though, for a motion planner to solve or simulate humans' driving-and-collision-avoidance behavior in such situation? Such is the question and motivation for my planner in traffic, nicknamed PT's PIT.

Using Probabilistic RoadMap (PRM) motion planning approach, this project implements a single-query planner that aims to deals with theorectically unlimited, practically a large number (restricted, for instance, because of memory constraint) of moving robots in an obstacle-cluttered environement. The goal of this planner and simulator is to simulate how different humans in cars (holonomic in PIT), each with his/her own randomly planned path, will move at a maximum velocity comfortable to him/herself, communicate and yield if necessary to other cars during imminent collisions, replan during a hopeless traffic jam, and finally arrive safely at his/her destinations.

The interface is based on Carlos Guestrin's Soccer Simulator: robots and obstalces are all circular discs in a 2D, 600x400 open field. During the initial set-up, users set the maximum velocity, initial and goal position for each robot, and arbitrarily place each obstacle, of arbitraty size, in the environment. This project considers obstacles as defined in Carlos' simulator to be static (just as buildings, roadblocks, etc. are in the real world) -- henceforth referred to as environment obstalces; each robot, however, regards other robots about to collide with it as moving obstacles (just as humans in cars do). In this sense, each robot is potentially a much more complexly behaving moving obstacle than the original fixed-moving-direction obstacles in Soccer Simulator. Therefore, the planner increases rather than reduces the complexity initially presented in the Simulator (and proposed during project review session early in the quarter).

Ideas, Outline of Algorithms and Heuristics

  The major flow of the algorithm is as follows:

• Each robot plans its own path among environemnt obstacles, oblivious to other robots's plans. If not all robots can find such path itself, the planner reports failure. (When planning using PRM, the planner currently will try for five times, each time increasing the max. tree depth level in the PRM method by 50. The number of tries and magnitude of increments definitely can be further fine-tuned or set by users during set-up time. For the lack of time, these two parameters are fixed currently.)
• Each robot executes its own, found path.
• During execution, if the robot is about to collide with others, the planner picks a winner: first based on who is more free in terms of the number of other robots or environment obstcles currently almost touching it (almost touching so as to allow for floating-point round-off errors and provide a little more tolerance); and second based on who is moving fastest -- in case of a tie, the first one is selected. (In this sense, the policy is a conservative one: who has more space to manuver will go first; if every one has enough room to locally move around, the fastest one goes first.)
• Other robots yield to the winner by randomly selecting a free node around itself, regarding the winner as an obstacle in addition to environment obstacles during node selection. The neighborhood size during the random node selection is adaptive: it is based on the number of obstacles almost touching the yielder and whether the traffic is jammed. When the situation is worse (i.e. the number is large or traffic is jammed, or both), the size increases (and hence adaptive to traffic situation). The maximum range, however, is the number of touching obstacles plus the default range for local avoidance (which. depending on whether traffic is jammed or not, is either two or the number of robots in the environment) -- please see PRM algorithm description below for the definition of "range."
• The winner waits until yielders have moved to the selected new node before starting to move to its next milestone.
• If, however, a yielder is already at goal, the winner replans it's path using PRM: with its current position as the initial, its original goal as the final, and all currently touching robots (again, this means those robots very close to it) in addition to static environment ones as obstacles. If such path isn't possible, yielders still need to yeild as described above.
• If the winner's current path is 1.5 times longer than the original path, it replans as well, assuming that the current traffic situation is horrible. (Again, such multiplicative factor is experimental: one can set it to be two or much closer to one so as to give up original plan faster.)

The core PRM algorithm is based on that presented in class (and project review session), with my own customization. For a given robot with desired initial and final configuration, the planner:

1. divides the environment into fixed-size cells,
2. grows two trees -- spanning in the free space and rooted at inital and final configuration, respectively --,
3. randomly generate four children of the currently selected node (which are the initial and goal nodes in the beginning) by:
   • randomly selecting a cell, with probablity inversely proportional to its number of node(milestone) occupancy, in 2*((2*robo.radius)/robo.gridResolution)+1 size neighborhood;
   • randomly selecting, with uniform probability, a node that is not overlapping with any existing one in the selected cell, lies in the free space and reachable from the root node;
4. tries to connect the two trees (at the current depth) using straight segments;
5. if successful in such connecting attempt, declares success and returns the found path; if not, randomly selects a child, and go to step 3.

The size of the cell (so-called grid resolution in PIT), if not customized by users during set-up, is one half of the robot radius (such default value is found by experiment to be fine enough for difficult regions). Thus, each robot has its own different grid resolution during planning time.

The number of children to generate as well as the number of neighboring cells to consider during the random generation process are experiemental and provide good matching for PIT. By using ((2*robo.radius)/robo.gridResolution), i.e. the so-called range in PIT, for calcuating the size of neighborhood, such parameter becomes adaptive to each robot's size and desired resolution. Using default grid resolution, the size of neighborhood to consider when picking a cell is (2*4+1) 9x9, large enough for general purpose (again. there is room to further experiment and analysis here).

The algorithm stops when a user-defined maximum tree depth has been reached so as to keep from running hopelessly long (as PRM approach tends to in pathological cases). As aforementioned, though, the planner will try for five times, each time increasing the depth by 50, before giving up finding a path during this planning invocation.

Results

  A picture is worth a thousand words, not to mention animated simulation provided by PIT. Therefore, you are encouraged to test (or help me test) the planner's limits. Some pathological cases that I think of so far are tested: involving five robots, no environment obstacles, starting in and ending in a pentagon configuration -- with each robot's goal directly opposite to its initial. More than likely, one can observe a traffic jam situation and see how PIT gets out of it (excitingly!) in simulation. Usual cases with weird shapes of obstacle region and a few number of robots are tested successfully as well. I am confident that such planner works with many more robots; however, one person's efforts in designing pathological cases is limited. So please help me test it out and let me know if you find any glitches. Because currently the probability isn't biased during node-generation and -selection process (except the inversely-proportional-to-cell-occupancy bias when selecting a cell), the planned path may be undesirably long if the environment is complicated (i.e. obstacles form pathlogical, tricky regions where the escape route is narrow) -- in such case, though, users should increase the maximum tree depth using the simulator options pull-down menu.

There is currently a pathological case that I think other planners may fail (assuming no scheduler available to them), whereas PIT's current approach can succeed. By referring to other planners, I mean planners that plan each robot's motion in sequence, taking into account the previously planned path of others before it. The pathological in mind is what I call "onion-ring" configuration, where robots' initial configurations take on a shape similar to layers of onion rings, with the outermost layer further surrounded by environment obstacles -- except a narrow escape for only one of the outermost robots to escape. Using the other planners' approach, the planner needs to be able to schedule the sequence of planning correctly in order for this group of robots to clear out of their original coonfiguration and reach their respective goals. Scheduling adds much complexity to the planner (not to mention the general scheduling is also NP-hard). Using PIT's approach, however, one can eventually detect that outermost robot as the most free one and select it as the winner to go first, thus graudually helping this initially jammed group get out of trouble. (Just to be meticulously precise: each robot in the internal rings should still have some small room to move about before the planner finally is selecting a winner out of a subgroup that the outermost, escapable one is a member of -- of course, one can insert another good piece of heuristics here to handle such pathologocial case.)

Conclusion

  PRM motion planner finds exciting application in PT's PIT. Taking advantage of probabilities, the planner is probabilistically complete given enough running time (or deep enough tree depth in this implementation). There are certainly other heuristics, improvements, and parameters inter-relation to anaylze and possibly refine. For instance, one can arguably find a better heuritics on the number of children to generate based on robot size, grid resolution and neighborhood range. The grid resolution, size of neighborhood and number of generated children at the expansion step are obviously inter-related. Furthermore, to improve the planner's efficiency, one can further bias the probability during next-node-random-selection process so that the final path will generally be much shorter than that found currently. Not only can such probability-bias heuristics help in finding the first, other-robots-oblivious paths, but it also will prove valuable during replanning when avoiding other robots in traffic jams or immiment collisions. With the work presented here, however, I hope I have successfully shown the promise of applying PRM approach to complex, multi-robot motion planning problem and that you will enjoy as much as I do seeing such planner in traffic in action :-).

JavaApplet source

  For some weird reason, browser display doesn't work with the applet (it has to do with not finding java.awt path). Java's appletviewer, on the other hand, works perfectly; so if you are interested in playing with PIT, please use appletviewer for now (until I have a chance to or you can help me find out how to fix this weird browser bug).