Multiple Car-Like Robot Path Planning and Coordination

Project description:
This project attempts to solve nonholonomic path planning and multiple robot coordination problems. In this project, car-like robots are selected to be honholonomic models. The robots have to be able to find their collision free paths from initial configurations to reach their final configurations. Moreover, there are more than one robot in the world space, so the robots have to know how to correspond to each other so that collision is avoided among the robots.
Nonholonomic path planning is done using a method called RRT. Although the solutions found are not the optimal solutions, this method basically can solve most of the path planning problems, just the matter of time. For multiple robot coordination, a Stop Sign method is introduced. It works fairly well. Again, the solution for  multiple robot coordination is not optimal.

Conceptual Description:
    Mobile robots are known as one of the nonholonomic robotics systems because of their nonholonomic constraints, like velocity and maximum steering angles. For a car like robot, control laws have been developed for helping the path planner to find a collision free path from an initial configuration to a defined final configuration based upon the nonholonomic constrains.
Control law of car-like robot:
Configuration Space for a car-like robot, C = R2xS1 ( 3 degrees of freedom)
Initial configuration, q (x, y, theta)
dtheta = (phi/L)*tan(alpha)
theta’= theta + dtheta;
dx = phi*cos(theta’)
dy = phi*sin(theta’)
-dx*sin(theta’)+dy*cos(theta’) = 0    -------------------------------------------------- (1)
where x = location of the robot in x axis in configuration space;
           y = location of the robot in y axis in configuration space;
          theta = orientation of the robot respect to x axis in configuration space;
          theta’= updated theta;
          dx = increment in x;
          dy = increment in y;
          dtheta = increment in theta;
          phi = displacement of robot in each time step in configuration space (another type of velocity  implementation);
         alpha = steering angle;
         L = the length between front and rear wheels of the car-like robot.
Equation (1) is a simple control law to implement the nonholonomic constraints for car-like robot. The paths found have to fulfill this basic equation.
Rapid-Exploring Random Tree (RRT):
  RRT works quite well for both holonomic and nonholonomic with high degree of freedom problems, although the solutions are not optimal. For nonholonomic problems, control laws have to implement into RRT so that a nonholonomic path could be found. To suite RRT into this project, equation (1) has implemented into RRT method. Moreover, decoupledized method with fixed path coordination (plans all paths independently)  is used to find paths for all robots.
Steps of modified RRT:
   1.Generate a random point in configuration space.
   2.Find the nearest node (using Distance,D)  to expand the tree.
   3.Generate a random steering angle in a range from -30 to 30 degree.
   4.If D<Critical Zone Distance.
      i) dual direction movement enable ( forward and backward), otherwise robots can only move forward.
     ii) If one node is already inside the critical zone, the further nodes have to be inside the zone.
   5. If collision is free, expand the tree use the steering angle and the fixed velocity based on control laws.
   6. If the configurations of the robot is within a tolerance range of its goal configurations, return Reach.
   7. Otherwise, if the tree is expand then return Advance.
   8. Else return Trapped.
     Although this modified RRT version will not work for some cases, which requires robots to move backward in order to go further, it could be easily changed by allowing robots to have dual direction movements all the time. The benefits of turning off the dual direction movement and assigning critical zone are to save run time and reduce the number of nodes generated. Basically, it would filter out quite a quite amount of junk nodes.
    While a node is found for a robot to reach its final configurations, the final path could be easily searched from the trees by backtracking its parents from the node because each node only has one parent.
Stop Sign Method for Multiple robot coordination:
    After all robots have found their paths to reach their goal configurations, they have to deal with each other to avoid collision if there are conflicts in their paths. To schedule the robot movement, a method based on stop sign idea is introduced, named Stop Sign Method.  Basically, a tolerance distance is defined (it has to at least longer than twice the maximum  body length of the robots). If the distance between any segment of two paths are less than the defined tolerance distance, then the segments of the two paths would be defined as share path; otherwise they would defined as free path.
    Share paths are the paths where collisions will be occurred among robots. A number of share paths may be found. The rule of the game is simple. The functionality of the share path is fundamentally like a stop sign junction. While one robot,R1 , is in a share path section, and other robots,R2,R3, ...,Rn, which have reached the boundary of the same share path section, have to stop and can not continue their paths until the robot,R1,  has passed the share path region. Priorities can be assigned to each robot so that the robots would be queued up to pass the share path sections.
    This method works quite well for multiple robot coordination to avoid collision, however,  it does not give optimal solution. In some cases, the robots waste quite a lot of time in waiting the other robots to pass the share zone while it is not necessary.
    Because the path planning is based on fixed path method, which plans all path independently, the solutions are not complete. There are cases that  the goal configurations of one robot,R1 , might block the other robots,R2,R3, ...,Rn, to reach their goal configurations if R1 reaches its goal configurations while  R2,R3, ...,Rn, not yet pass the share path section.
   Although this method basically could be applied for multiple robot coordination problem, the sharezone tolerance distance has to be chosen smartly for the robots to coordinate well.

Click on the images to get more details.
Four examples are shown here.  Two car-like robots are here to show how RRT can be used to solve parallel parking problem. Moreover, multiple robot coordination based on stop sign idea is shown.
Green and dark blue lines are the trees generated for the two robots by RRT.
In addition to show the final paths for the two robots, red lines represent the free path segment and black lines represent the share path segment.
## The world space is unbounded.
# Notice that there is a straight line connected from lower left corner of the world space to the initial position of a robot. This line was a drawing mistake. It is not generated by RRT.
Case1: Robot orientations are not concerned.
          Multiple Robot coordination. 
  Case 2: Parellel parking problem 

Case 3: Parellel parking with multiple robot coordination

Case 4: Another example of multiple robot     coordination

      Overall, the combination of the modified RRT  and stop sign method works quite well for most of the cases of parallel parking with multiple robot coordination problems. However, it sometimes takes quite a long time to get a solution and more than three thousand nodes are genearated for each robot. Better goal bias methods could be implemented into RRT so that time is saved. With the assumption of the goal configurations of a robot will not be on the paths of other robots, stop sign method could be used to schedule the movements of the robots so that collision is avoided.

1. Com S 575 Class Notes, Dr. Lavalle, Steve, 2000,
2. Nonholonomic mobile robot path planning, Jun Qu, 1999,

Souce Codes:
This project is done by using a program called Rapidapp, which generate GUI, and OpenGL to create graphics.
1. BullentinBoard.C : A C++ file generated by Rapidapp for drawing graphics. OpenGL codes were added.
2. Build_rrt.h: Building RRT, searching final path, and defining sharezones.
3. collide.h: Collision detection algorithm and obstacle data points.