Random sampling based path planning algorithms in the multi-agent robot soccer domain

8/4/2019 Random sampling based path planning algorithms in the multi-agent robot soccer domain

1/8

Random sampling based path planning algorithms in the multi-agent

robot soccer domainProject Report for EE586 Artificial Intelligence*

Kadir Firat UyanikKOVAN Research Lab.

Electrical and Electronics Engineering Dept.

Middle East Technical University

Ankara, Turkey

[email protected]

Abstract The ability to traverse/navigate around withoutcolliding to the obstacles is one of most essential require-ments for an autonomous mobile robot that is supposed torealize a particular task by itself. Assuming that the robotis provided with its current global position and orientation(initial configuration), and goal configuration as well, theproblem of enabling a robot to reach its goal configurationin a reasonable amount of time, though seems simple, maypresent intriguing and difficult issues. Besides, in multi-robotdomains where robots operate collaboratively, highly complexand unpredictable dynamics can arise. If an adversarial entityand unactuated-physical-body interactions are also taken intoaccount, the need for making navigation calculations withintight time constraints becomes more serious. In this report,several random sampling based path planing algorithms, suchas Rapidly-growing Random Trees (RRT), RRT-connect, Lazy-RRT, Parallel-RRT and Probabilistic Roadmap methods arecompared in terms of their memory usage, number of graphstates, success rate, solution time and other similar metrics.

I. INTRODUCTION

One particular feature of an autonomous mobile robot that

makes them superior to the industrial robot manipulators is

that they can navigate and traverse around the environment if

it is feasible to do so. Yet another special feature is that they

can move from an initial position to a goal position without

requiring explicit intermediate supervised action commands

(viz. no or minimum remote control). This implies that

the robot is capable of avoiding obstacles and unwanted

regions during navigation. One basic family of methods to

accomplish this is called Bug-algorithms [1].

Bug1 and Bug2, are known to be one of the earliest

and simplest sensor-based planners, minimizing the compu-

tational burden on the robot while still guaranteeing globalconvergence to the target if reachable. However, these algo-

rithms do not make the best use of the available sensory data

to produce relatively shorter paths by utilizing range data.

Although VisBug [2] algorithm uses the range sensor data,

it can only utilize this data to find shortcuts that connects

points on the trajectory found by Bug2 algorithm containing

no obstacles between. TangentBug algorithm(TBA) [3] uses

range data to produce a local tangent graph so as to choose

locally optimal direction while keeping approaching to the

*This study is a part of EE586 Artificial Intelligence course offered byDr. Afsar Saranli.

target position. TBA produces paths, in reasonably simple

environments, that approach the globally optimal path as the

sensors maximal detection range increases.

Bug algorithms assumes that the robot is a point on a 2D

planar surface, and the obstacles are stationary. Moreover,

bug algorithms were developed for the robots that do not

have access to the global information of the environment.

There are several robot motion control and path planning

algorithms that make use of global information, such as

artificial potential fields(APF), visibility graphs(VG), and

rapidly-growing random trees(RRT).

APF [4] is a reactive approach since the trajectories are

not planned explicitly but obtained while executing actions

by differentiating a function what is called potential function.

A potential function is differentiable real-valued function U :

Rm R. The output of a potential function can be taken as

the energy and its negative gradient as the net-force acting

on the robot.APF methods can vary regarding how they measure the

distance to the goal, and the type of the functions they utilize,

such as additive attractive/repulsive potential functions, and

navigation potential functions. Due to the local minima

problem in the additive attractive/repulsive potential fields,

many other local-minima free potential field methods are

proposed such as wave-front planner [5],[6]. However these

methods have discretization problems and thats why they

become computationally intractable for high dimensional and

large configuration spaces.

To solve the local minima problem, a special function to

be constructed which has the only minimum at the goal

position. These functions are called navigation functions,

formally defined in [7][8]. But it turns out that navigation

potential functions requires carefully tuned parameters (e.g.

) and the behavior of the robot is not that predictable for

an outsider observer.

VG [9] are consisted of a graph including intervisible

locations, typically in the euclidean plane. Nodes of the graph

represent a point location/vertex, and edges represent visible

connections between the vertices. Visibility also refers to the

condition when each edge stays in the free configuration

space (viz. no obstacle inter-penetration). In other words,

visible lines are the line segments that a robot supposed


2/8

to follow to reach its goal position without colliding any

obstacle on its path.

The standard visibility graph is defined in a 2-D polygonal

configuration space where the nodes includes the start and

goal location in addition to the vertices of the configuration

space obstacles, and the edges that connect two distinct line-

of-sight nodes.

But, VG cannot scale well with the complexity of theenvironment (viz. the number of polygon vertices). Besides,

its complexity is exponential in the dimension of the config-

uration space due to the complex intersection tests.

There are different ways to simplify the path finding

problem, such as projecting search to a lower dimensional

space, limiting the number of possibilities(i.e. adding con-

straints, reducing the volume of free space), or sacrificing

the precision, optimality, even completeness.

Random sampling based methods are one way to relax

the problem. It also lets designer to control the precision

and solution time trade-off easily. The key idea behind the

sampling based techniques is that rather than exhaustively

exploring all possibilities, randomly explore a smaller subset

of possibilities as the progress is being tracked. They can go

down in the search tree much earlier than any exhaustive

algorithm, which eases taking control of the search process.

However, the downside is that depending on the planner

configuration, optimality and completeness can be com-

promised considerably. Moreover, narrow passages in the

configuration space can be a real problem since the sampled

nodes might not be connected together unless the sampling

is focused around these passages.

In this study, basic Rapidly-growing Random Trees (RRT)

[10], RRT-connect [11], Lazy-RRT [12], Parallel-RRT and

basic PRM are compared with each other. Yet, there areother RRT-variations to be included to this study as a future

work, such as ERRT [13], and Bidirectional Multi-Bridge

ERRT[14].

II . RAPIDLY GROWING RANDOM TREES

An RRT is basically a data structure and algorithm which

efficiently searches nonconvex high-dimensional spaces. The

way RRT is constructed can be given as follows:

Start with the initial state as the root of a trees

Pick a random state in anywhere or in the direction of

the target

Find the closest node in the current trees Extend that node toward the target if possible

To mention other RRT-variations briefly:

RRT-connect expands two trees towards each other,

one from the start node, and the other from the goal

node. They interchange the roles of expanding-towards-

the-other and expanding-randomly. The algorithm ter-

minates ones a connection is established between trees.

Lazy-RRT does not check to make sure the path is

valid. Instead, it is optimistic and attempts to find a

path as soon as possible. Once a path is found, it is

checked for collision. If collisions are found, the invalid

path segments are removed and the search process is

continued.

Parallel-RRT basically makes use of the parallel com-

puting utilities while generating new nodes and expand-

ing towards them. It either decreases the time to find a

solution, or increases the area coverage due to larger

number of samples.

ERRT makes use of the idea of way-point caching.Therefore search operation becomes biased towards the

paths found in the earlier successful executions.

Multi-Bridge ERRT is similar to the RRT-connect but

it continues to execution although a connection is estab-

lished between the trees until a pre-specified number of

connections are established. Then the resultant structure

is not a tree anymore. To find the shortest path an

optimal graph search algorithm is used, such as A*.

III. EXPERIMENTAL SETUP

Webots[15] is used to simulate and visualize the robots

and the environment. CGAL[17] is used as a library todetect collisions which might occur during sampling, or

while connecting sampled nodes. Finally, OMPL[18]is used

as the core of this study.

OMPL consists of many state-of-the-art sampling-based

motion planning algorithms. It doesnt include any collision

checking or visualization routine which is done on purpose

to abstract the algorithms from the problem definition and

any other visual load.

The class ownership diagram in the figure 3 shows the

relationship between the essential base classes in OMPL.

For example, SpaceInformation owns a StateSpace; Plan-

ner does not own SpaceInformation, although a Planner

does know about the SpaceInformation, and uses provided

functionality. By using the SimpleSetup class, it is only

necessary to instantiate a ompl::base::StateSpace object,

a ompl::control::ControlSpace object (when planning with

differential constraints, i.e, planning with controls), and

a ompl::base::StateValidityChecker object. Many common

state spaces have already been implemented as derived

StateSpace classes.

Overall software architecture has been designed so that

the information sources or the actuation modules can eas-

ily replaced with the actual robots and sensors. To do

this, Robot Operating System (ROS), particularly the pub-

lisher/subscriber type ROS communication system has beenutilized. Figure 4 shows the ROS communication graph.

IV. IMPLEMENTATION DETAILS

Setting up the planning problem in OMPL is as follows:

identify the space we are planning in: C= R2,

select a corresponding state manifold from the available

ones, or implement one,

since C contains an R2 component, we need to define

bounds, which are the field boundaries,

define the notion of state validity,

define start states and a goal representation.


3/8

Fig. 3. OMPL API overview, adapted from OMPL documentation website

Fig. 1. Detailed model of the simulated robot in Webots simulationenvironment, it is not a reasonable model in terms of the rendering time itrequires.

Fig. 2. Simplified model of the simulated robot in Webots. It causesproblem while translational and rotational velocity components are addedtogether to generate motor velocities due to the frictions on the wheels.Author is not aware of an easy method to change the friction parameters toobtain good model of an omniwheel without using rollers on the wheels asit is successfully done on the detailed robot model.


4/8

Fig. 4. This is an automatically generated graph by using ros rxgraph command. Elliptical shapes represents the nodes (running processes), rectangularshapes represents the topic names that the corresponding (ros TCP) messages are being sent and retrieved. This graph representing the communicationarchitecture for Blue colored robots, except the actuation command related connections (not implemented yet).


5/8

Once these steps are complete, the specification of the

problem is conceptually done.

OMPL requires a state validity method to be implemented

which is called during the sampling if any sample configu-

ration is collided with a configuration space obstacle. To do

this, robots are taken as if they are circular planar entities.

Then collision checks are done on the obstacle space formed

by the inclusion of the all obstacles which can be consistedof several circles and several rectangles.

Start and goal configuration settings are similar to the

state manifold representation. Planners in OMPL requires

space information and problem definition information as

well. Space information is acquired via the state manifold

representation that is done at the very first. Problem defini-

tion takes the start, goal and space information to become

ready for planner configuration.

One other great feature of OMPL is that, planning time

can be limited by setting a time limit as an input argument

to the planners solve method. This is very helpful for a

time-critical application like small size league robot soccer.

V. EXPERIMENTAL RESULTS

Planners have been exhaustively tested by using the OMPL

benchmark utilities. The results for a basic environment2,

where the target has been set one meter away from the robots

actual positions, and there were no obstacles between the

robots and the target position(but there were 10 obstacles in

total), are shown below.

Fig. 5. Five of the blue robots are controllable entities whereas the 5blue robots are static obstacles in the environment. A video showing thereal time performance of the path plannar , utilizing pRRT, can be foundhere. In this scenario, robots have a goal position that are two meters awayfrom the them, as the end of the lines shows.

One problem of roadmap or random sampling based

methods is the path is represented as the piecewise linear line

segments. If the path is followed, robots are supposed to do

sharp turns at the connection ends of the segments. To avoid,

or at least, reduce this effect, a path smoothing approach can

be utilized such as the leader-followerapproach as it is used

Fig. 6. The time it requires for planners to return a solution. Time limitwere set to 0.01 sec. If there werent any solution or the planner couldntfind one in this time limit, it would return in anyway in 0.01 sec. Note that

the time is measured in seconds.

Fig. 7. Memory usage of each algorithm, calculated in MBs.

in [14]. The figures 9 and 10 show the number of piecewise

linear line segments before and after the smooth operation

applied, respectively. After simplification total path length

decreases almost 20% as it is indicated in the figure 11.

However, this simplification procedure should be lightweight

so that the planned path is returned as soon as possible,

shown in the figure 12.

V I. DISCUSSION

Current experiment setup supports objects that can be

approximated by circles, polygons and any combination of

these shape primitives. This enables us to represent the

space that is allocated by one robot in one control cycle by

using a circular sweep (two circles and one rectangle in this

case). Therefore safe navigation can be guaranteed among
http://www.youtube.com/watch?v=d_8qLn4d4ZU&feature=channel_video_titlehttp://www.youtube.com/watch?v=d_8qLn4d4ZU&feature=channel_video_title


6/8

Fig. 8. Length of the path being found which was actually 1m in euclideandistance without obstacles, measured in meters.

Fig. 9. Number of line line segments being returned by the path planners.

the multiple controllable robots.

Since the paths for each robot are planned consecutively,

obstacle space can be updated after a plan is made for a robot

by considering the control signal being sent to the robot and

its current configuration. In order to guarantee the safety, at

each iteration, these circular sweeps are calculated so that it

is assured to be no collision event if the robot is braked in

the next control cycle.

Dynamic safety search [14] makes use of the this idea to

make corrections on the velocity vectors by considering the

robots acceleration window model to guarantee no collision

between controllable multiple robots, as it is shown in figure

13.

Having mentioned the safe navigation, currently Dy-

namic Window Approach(DWA) [19] is being utilized for

this system. DWA based local planner runs inside the

Fig. 10. Number of line line segments being returned by the path planners.

Fig. 11. Number of line line segments being returned by the path plannersafter path smooth operation is applied, measured in meters.

motion controller nodes of each robot. As it is shown in

the figure 15, a costmap is generated for each robot assuming

that the obstacles in the world are represented in terms of

points. This controller serves to connect the path planner

to the robot. Using the kinematically safe path found bythe path planner node for each robot, dwa local planner

operates as follows [20]:

Discretely sample in the robots control space

(dx,dy,dtheta)

For each sampled velocity, perform forward simulation

from the robots current state to predict what would

happen if the sampled velocity were applied for some

(short) period of time.

Evaluate (score) each trajectory resulting from the

forward simulation, using a metric that incorporates

characteristics such as: proximity to obstacles, prox-


7/8

Fig. 14. This is the costmap generated my the motion planner of the robot that is at the center of the field. Ones the points(shown in red) are acquired,this information is used to inflate the world (shown in green) with the radius of the robot. Left sub-figure shows the simulation environment and the rightthe the main visualization tool of ROS, rviz. To generate costmap ros dwa local planner package is used but critical modifications were done on the sourcecode to obtain these results. Please note that the two sub figures are not to the scale since they are obtained as a screenshot.

Fig. 12. The time it takes to smooth the path being found, calculated inseconds.

imity to the goal, proximity to the global path, and

speed. Discard illegal trajectories (those that collide

with obstacles).

Pick the highest-scoring trajectory and send the associ-

ated velocity to the mobile base.

Rinse and repeat.

Preliminary results for obtaining the local costmap is

shown in the figure15, and this video shows how the costmap

is updated (currently overall system loop operates with

30Hz).

Velocity profile of the robot is obtained through experi-

ments as it is shown in the figure ??. Next step is connecting

the local planner to the actuation nodes of the robots to see

the real time performance of the local planner in a reasonable

high velocity and acceleration limits.

Fig. 13. An example of an iteration of DSS with two agents. Each agentstarts by assuming it will stop (a), and then each agent chooses an action (b)-(c), while making sure the action will allow a safe stop afterward. Finally,the actions are executed (d) and the agents can safely assume that stoppingis a valid action. Adapted from [14].
http://www.youtube.com/watch?v=2CInqYomGUo&feature=channel_video_titlehttp://www.youtube.com/watch?v=2CInqYomGUo&feature=channel_video_title


8/8

Fig. 15. Velocity profile of a four-asymmetrically-distributed-omniwheeled

robot. Individual motor speeds are set to a maximum limit that is safe.

RRTs requires configuration of the goal bias probabilities

and the step size parameter. These can be tuned empirically,

or a statistical learning method could be utilized to find the

most proper values.

Results show that the PRM outperforms RRT algorithms in

this environment, but it requires memory even more than the

LazyRRT. Although ParallelRRT is better than the RRT-base,

RRT-connect is better in the sense that the time it takes to

return a solution due to its dual-root nature. LazyRRT seems

a very reasonable choice for this scenario due to its greedy

behavior towards reaching the goal as soon as possible whileskipping the collision checks, it is expected to result in worse

performance in cluttered environments.

VII. CONCLUSIONS

In this study, RRT based algorithms are tested on a simu-

lation environment with a multi-agent experimental setup. A

recently-released motion planning library, OMPL, is studied.

The most powerful side of the OMPL is that it gives the

designer freedom to use any kind of a robotic platform to

plan its action by using various planning algorithms without

making no change in the planning side of the code as

soon as the state and control manifold representations, and

state validity checking routines are done correctly for that

particular platform. It seems that OMPL is going to be a

standard planning library in Robotics community in the near

future as it is in the case of Computer Vision community

and OpenCV library.

Although it requires more extensive testings scenarios,

RRT-connect is to one of the best among the other RRT

algorithms, it could be optimized to work with multiple

processor cores which even make this algorithm perform

much faster while allowing it to cover more regions.

Currently, dwa local planner is being added to the system.

Safety and performance analysis is to be done by optimizing

the costmap parameters such as proximity to obstacles,

proximity to the goal, proximity to the global path, and

speed. Once the optimal values for the costs of the local

map (costmap) is obtained, the planners tested in this study

will be compared against each other.

To make the current design work, it required more than

5000 lines of C++ code, and tens of parameters to be set. Yet,

there are many modules to be optimized, especially duringthe costmap generation and clearance operation.

REFERENCES

[1] V. J. Lumelsky and A. A. Stepanov. Path-planning strategies for apoint mobile automaton moving amidst obstacles of arbitrary shape.Algoritmica, 2:403-430, 1987

[2] H. Choset and J. W. Burdick. Sensor based planning, part ii: Incremen-tal construction of the generalized voronoi graph. In IEEE InternationalConference on Robotics and Automation, Nagoya, Japan, May 1995.

[3] I. Kamon, E. Rivlin, E.Rimon. A new range-sensor based globallyconvergent navigation algorithm for mobile robots. Robotics andAutomation, 1996. Proceedings., 1996 IEEE International Conferenceon In Robotics and Automation, 1996. Proceedings., 1996 IEEEInternational Conference on, Vol. 1 (1996), pp. 429-435 vol.1.

[4] Khatib, O. 1986. Real-time obstacle avoidance for manipulators and

mobile robots. Int. J. Rob. Res. 5(1):9098.[5] R. Jarvis. Coliision free trajectory planning using distance trans-

forms.Mech. Eng. Trans. of the IE Aus, 1985.[6] J. Barraquand, B. Langlois, and J.C.Latombe. Numerical potential

field techniques for robot planning. IEEE Transactions on Man andCybernetics, 22(2):224-241, Mar/Apr 1992.

[7] Koditschek, Daniel E., and Rimon, Elon: Robot navigation functionson manifolds with boundary, Advances in Applied Mathematics 11(4),volume 11, 412442, 1990.

[8] Rimon, Elon and Koditschek, Daniele; Exact robot navigation usingartificial potential functions IEEE Transactions on Robotics and Au-tomation. Vol. 8, no. 5, pp. 501-518. Oct. 1992

[9] Lozano-Perez T. and Michael A. Wesley. 1979. An algorithm forplanning collision-free paths among polyhedral obstacles. Commun.ACM 22, 10 (October 1979), 560-570

[10] S. M. LaValle. Rapidly-exploring random trees: A new tool for path

planning. TR 98-11, Computer Science Dept., Iowa State Univ., Oct.1998

[11] Kuffner, J.J., Jr.; LaValle, S.M.; , RRT-connect: An efficient approachto single-query path planning , Robotics and Automation, 2000.Proceedings. ICRA 00. IEEE International Conference on , vol.2,no., pp.995-1001 vol.2, 2000

[12] Yoshiaki Kuwata and Gaston A. Fiore and Justin Teo and EmilioFrazzoli and Jonathan P. How, Motion planning for urban drivingusing rrt, In Proceedings of the IEEE/RSJ International Conferenceon Intelligent Robots and Systems, 2008, pp. 1681-1686.

[13] Bruce, J.; Veloso, M.; , Real-time randomized path planning forrobot navigation, Intelligent Robots and Systems, 2002. IEEE/RSJInternational Conference on , vol.3, no., pp. 2383- 2388 vol.3, 2002

[14] Based on: Bruce J. R., Real-Time Motion Planning and Safe Naviga-tion in Dynamic Multi-Robot Environments, PhD. Thesis, 2006

[15] Michel, O. Webots: Professional Mobile Robot Simulation, Interna-tional Journal of Advanced Robotic Systems, Vol. 1, Num. 1, pages

39-42, 2004[16] Principles of Robot Motion by Howie Choset etal., MIT Press, 2005,

ISBN: 0-262-03327-5[17] R. C. Veltkamp. Generic programming in CGAL, the Computational

Geometry Algorithms Library. In F. Arbab and P. Slusallek, editors,Proceedings of the 6th Eurographics Workshop on ProgrammingParadigms in Graphics, Budapest, Hungary, 8 September 1997, pages127-138, 1997.

[18] The Open Motion Planning Library (OMPL),http://ompl.kavrakilab.org, 2010

[19] D. Fox, W. Burgard, and S. Thrun. The dynamic window approach tocollision avoidance. The Dynamic Window Approach to local control.

[20] E. Marder-Eppstein, Dynamic Window Approach local planner ROSpackage. http://www.ros.org/wiki/dwa local planner

Random sampling based path planning algorithms in the multi-agent robot soccer domain

Documents

Transcript of Random sampling based path planning algorithms in the multi-agent robot soccer domain