Multiple
Car-Like Robot Path Planning and Coordination
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.
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)
-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;
dtheta = increment
in theta;
phi = displacement
of robot in each time step in configuration space (another type of velocity
implementation);
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.
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.
Examples:
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
|
|
|
Conclusion:
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.
Bibiography:
1. Com S 575 Class Notes, Dr. Lavalle, Steve, 2000, http://janowiec.cs.iastate.edu/cs576/
2. Nonholonomic mobile robot path planning, Jun Qu, 1999, http://janowiec.cs.iastate.edu/~lavalle/cs576_1999/projects/junqu/
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.