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

Project Report for EE586 Articial Intelligence*

Kadir Firat Uyanik

KOVAN Research Lab. Electrical and Electronics Engineering Dept. Middle East Technical University Ankara, Turkey kadir@ceng.metu.edu.tr

Abstract The ability to traverse/navigate around without colliding to the obstacles is one of most essential requirements for an autonomous mobile robot that is supposed to realize a particular task by itself. Assuming that the robot is provided with its current global position and orientation (initial conguration), and goal conguration as well, the problem of enabling a robot to reach its goal conguration in a reasonable amount of time, though seems simple, may present intriguing and difcult issues. Besides, in multi-robot domains where robots operate collaboratively, highly complex and unpredictable dynamics can arise. If an adversarial entity and unactuated-physical-body interactions are also taken into account, the need for making navigation calculations within tight time constraints becomes more serious. In this report, several random sampling based path planing algorithms, such as Rapidly-growing Random Trees (RRT), RRT-connect, LazyRRT, Parallel-RRT and Probabilistic Roadmap methods are compared in terms of their memory usage, number of graph states, success rate, solution time and other similar metrics.

I. I NTRODUCTION 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 computational burden on the robot while still guaranteeing global convergence to the target if reachable. However, these algorithms 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 nd 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 Articial Intelligence course offered by Dr. 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 articial potential elds(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 elds, many other local-minima free potential eld 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 conguration 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 dened 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 conguration space (viz. no obstacle inter-penetration). In other words, visible lines are the line segments that a robot supposed

to follow to reach its goal position without colliding any obstacle on its path. The standard visibility graph is dened in a 2-D polygonal conguration space where the nodes includes the start and goal location in addition to the vertices of the conguration space obstacles, and the edges that connect two distinct lineof-sight nodes. But, VG cannot scale well with the complexity of the environment (viz. the number of polygon vertices). Besides, its complexity is exponential in the dimension of the conguration space due to the complex intersection tests. There are different ways to simplify the path nding problem, such as projecting search to a lower dimensional space, limiting the number of possibilities(i.e. adding constraints, reducing the volume of free space), or sacricing 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 conguration, optimality and completeness can be compromised considerably. Moreover, narrow passages in the conguration 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 are other RRT-variations to be included to this study as a future work, such as ERRT [13], and Bidirectional Multi-Bridge ERRT[14]. II. R APIDLY G ROWING R ANDOM T REES An RRT is basically a data structure and algorithm which efciently 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 briey: 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-towardsthe-other and expanding-randomly. The algorithm terminates 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 nd 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 computing utilities while generating new nodes and expanding towards them. It either decreases the time to nd 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 established between the trees until a pre-specied number of connections are established. Then the resultant structure is not a tree anymore. To nd the shortest path an optimal graph search algorithm is used, such as A*. III. E XPERIMENTAL S ETUP

Webots[15] is used to simulate and visualize the robots and the environment. CGAL[17] is used as a library to detect 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 denition and any other visual load. The class ownership diagram in the gure 3 shows the relationship between the essential base classes in OMPL. For example, SpaceInformation owns a StateSpace; Planner 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 easily replaced with the actual robots and sensors. To do this, Robot Operating System (ROS), particularly the publisher/subscriber type ROS communication system has been utilized. Figure 4 shows the ROS communication graph. IV. I MPLEMENTATION D ETAILS Setting up the planning problem in OMPL is as follows: 2 identify the space we are planning in: C = R , select a corresponding state manifold from the available ones, or implement one, 2 since C contains an R component, we need to dene bounds, which are the eld boundaries, dene the notion of state validity, dene start states and a goal representation.

Fig. 3.

OMPL API overview, adapted from OMPL documentation website

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

Fig. 2. Simplied model of the simulated robot in Webots. It causes problem while translational and rotational velocity components are added together to generate motor velocities due to the frictions on the wheels. Author is not aware of an easy method to change the friction parameters to obtain good model of an omniwheel without using rollers on the wheels as it is successfully done on the detailed robot model.

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

Once these steps are complete, the specication of the problem is conceptually done. OMPL requires a state validity method to be implemented which is called during the sampling if any sample conguration is collided with a conguration 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 consisted of several circles and several rectangles. Start and goal conguration settings are similar to the state manifold representation. Planners in OMPL requires space information and problem denition information as well. Space information is acquired via the state manifold representation that is done at the very rst. Problem denition takes the start, goal and space information to become ready for planner conguration. 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. E XPERIMENTAL R ESULTS 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. 6. The time it requires for planners to return a solution. Time limit were set to 0.01 sec. If there werent any solution or the planner couldnt nd 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.

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

in [14]. The gures 9 and 10 show the number of piecewise linear line segments before and after the smooth operation applied, respectively. After simplication total path length decreases almost 20% as it is indicated in the gure 11. However, this simplication procedure should be lightweight so that the planned path is returned as soon as possible, shown in the gure 12. VI. D ISCUSSION 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

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-follower approach as it is used

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

Fig. 10.

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

Fig. 9.

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

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

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 conguration. 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 gure 13. Having mentioned the safe navigation, currently Dynamic Window Approach(DWA) [19] is being utilized for this system. DWA based local planner runs inside the

motion controller nodes of each robot. As it is shown in the gure 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 by the 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-

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

Fig. 12. seconds.

The time it takes to smooth the path being found, calculated in

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 associated velocity to the mobile base. Rinse and repeat. Preliminary results for obtaining the local costmap is shown in the gure15, and this video shows how the costmap is updated (currently overall system loop operates with 30Hz). Velocity prole of the robot is obtained through experiments as it is shown in the gure ??. 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 agent starts 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 stopping is a valid action. Adapted from [14].

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 during the costmap generation and clearance operation. R EFERENCES
[1] V. J. Lumelsky and A. A. Stepanov. Path-planning strategies for a point 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: Incremental construction of the generalized voronoi graph. In IEEE International Conference on Robotics and Automation, Nagoya, Japan, May 1995. [3] I. Kamon, E. Rivlin, E.Rimon. A new range-sensor based globally convergent navigation algorithm for mobile robots. Robotics and Automation, 1996. Proceedings., 1996 IEEE International Conference on In Robotics and Automation, 1996. Proceedings., 1996 IEEE International 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 transforms.Mech. Eng. Trans. of the IE Aus, 1985. [6] J. Barraquand, B. Langlois, and J.C.Latombe. Numerical potential eld techniques for robot planning.IEEE Transactions on Man and Cybernetics, 22(2):224-241, Mar/Apr 1992. [7] Koditschek, Daniel E., and Rimon, Elon: Robot navigation functions on manifolds with boundary, Advances in Applied Mathematics 11(4), volume 11, 412442, 1990. [8] Rimon, Elon and Koditschek, Daniele; Exact robot navigation using articial potential functions IEEE Transactions on Robotics and Automation. Vol. 8, no. 5, pp. 501-518. Oct. 1992 [9] Lozano-Perez T. and Michael A. Wesley. 1979. An algorithm for planning 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 efcient approach to 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 Emilio Frazzoli and Jonathan P. How, Motion planning for urban driving using rrt, In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2008, pp. 1681-1686. [13] Bruce, J.; Veloso, M.; , Real-time randomized path planning for robot navigation, Intelligent Robots and Systems, 2002. IEEE/RSJ International Conference on , vol.3, no., pp. 2383- 2388 vol.3, 2002 [14] Based on: Bruce J. R., Real-Time Motion Planning and Safe Navigation in Dynamic Multi-Robot Environments, PhD. Thesis, 2006 [15] Michel, O. Webots: Professional Mobile Robot Simulation, International 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 Programming Paradigms in Graphics, Budapest, Hungary, 8 September 1997, pages 127-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 to collision avoidance. The Dynamic Window Approach to local control. [20] E. Marder-Eppstein, Dynamic Window Approach local planner ROS package. http://www.ros.org/wiki/dwa local planner

Fig. 15. Velocity prole of a four-asymmetrically-distributed-omniwheeled robot. Individual motor speeds are set to a maximum limit that is safe.

RRTs requires conguration of the goal bias probabilities and the step size parameter. These can be tuned empirically, or a statistical learning method could be utilized to nd 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 while skipping 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 simulation 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