a study on rapidly-growing random tree based algorithms
TRANSCRIPT
A study on Rapidly-growing Random Tree based algorithms
Kadir Firat UyanikKOVAN Research Lab.Dept. of Computer Eng.
Middle East Technical Univ.Ankara, Turkey
Abstract— The ability to traverse/navigate around withoutcolliding to the obstacles is one of the most essential require-ments for an autonomous mobile robot that is supposed torealize a particular task by itself. Assuming that the robot isprovided with its current global position and orientation (initialconfiguration), and goal configuration as well, the problem ofenabling a robot to reach its goal configuration in a reasonableamount of time, though seems simple, may present intriguingand difficult issues. In this study Rapidly-growing RandomTrees based algorithms are investigated for a robot havingan articulated-body. This report explains the implementationdetails and gives the preliminary test results.
I. INTRODUCTION
One particular feature of an autonomous mobile robot thatmakes them superior to the industrial robot manipulators isthat they can navigate and traverse around the environmentif the surface is smooth enough. Yet another special featureis that they can move from an initial position to a goalposition without requiring explicit intermediate supervisedaction commands (viz. no or minimum remote control). Thisimplies that the robot is capable of avoiding obstacles andunwanted regions during navigation. One basic family ofmethods to accomplish this is called Bug-algorithms [1].
Bug1 and Bug2, are known to be one of the earliestand 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 datato 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 connectspoints on the trajectory found by Bug2 algorithm containingno obstacles between. TangentBug algorithm(TBA) [3] usesrange data to produce a local tangent graph so as to chooselocally optimal direction while keeping approaching to thetarget position. TBA produces paths, in reasonably simpleenvironments, that approach the globally optimal path as thesensor’s maximal detection range increases.
Bug algorithms assumes that the robot is a point on a 2Dplanar surface, and the obstacles are stationary. Moreover,bug algorithms were developed for the robots that do nothave access to the global information of the environment.
There are several robot motion control and path planningalgorithms that make use of global information, such asartificial potential fields(APF), visibility graphs(VG), andrapidly-growing random trees(RRT).
APF [4] is a reactive approach since the trajectories arenot planned explicitly but obtained while executing actionsby 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 asthe energy and its negative gradient as the net-force actingon the robot.
APF methods can vary regarding how they measure thedistance to the goal, and the type of the functions they utilize,such as additive attractive/repulsive potential functions, andnavigation potential functions. Due to the local minimaproblem in the additive attractive/repulsive potential fields,many other local-minima free potential field methods areproposed such as wave-front planner [5],[6]. However thesemethods have discretization problems and that’s why theybecome computationally intractable for high dimensional andlarge configuration spaces.
To solve the local minima problem, a special function tobe constructed which has the only minimum at the goalposition. These functions are called navigation functions,formally defined in [7][8]. But it turns out that navigationpotential functions requires carefully tuned parameters (e.g.κ) and the behavior of the robot is not that predictable foran outsider observer.
VG [9] are consisted of a graph including intervisiblelocations, typically in the euclidean plane. Nodes of the graphrepresent a point location/vertex, and edges represent visibleconnections between the vertices. Visibility also refers to thecondition when each edge stays in the free configurationspace (viz. no obstacle inter-penetration). In other words,visible lines are the line segments that a robot supposedto follow to reach its goal position without colliding anyobstacle on its path.
The standard visibility graph is defined in a 2-D polygonalconfiguration space where the nodes includes the start andgoal location in addition to the vertices of the configurationspace 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 findingproblem, such as projecting search to a lower dimensionalspace, limiting the number of possibilities(i.e. adding con-
straints, reducing the volume of free space), or sacrificingthe precision, optimality, even completeness.
Random sampling based methods are one way to relaxthe problem. It also lets designer to control the precisionand solution time trade-off easily. The key idea behind thesampling based techniques is that rather than exhaustivelyexploring all possibilities, randomly explore a smaller subsetof possibilities as the progress is being tracked. They can godown in the search tree much earlier than any exhaustivealgorithm, which eases taking control of the search process.
However, the downside is that depending on the plannerconfiguration, optimality and completeness can be com-promised considerably. Moreover, narrow passages in theconfiguration space can be a real problem since the samplednodes might not be connected together unless the samplingis focused around these passages.
In this study, basic Rapidly-growing RandomTrees(RRT)[10], RRT-connect [11], Lazy-RRT [12],and Parallel-RRT. But there are also other variations ERRT[13], and Bidirectional Multi-Bridge ERRT[14] that are nottested in this study yet.
II. RAPIDLY GROWING RANDOM TREES
An RRT is basically a data structure and algorithm whichefficiently searches nonconvex high-dimensional spaces. Theway 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 goalnode. 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 isvalid. Instead, it is optimistic and attempts to find apath as soon as possible. Once a path is found, it ischecked for collision. If collisions are found, the invalidpath segments are removed and the search process iscontinued.
• 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 asolution, or increases the area coverage due to largernumber of samples.
• ERRT makes use of the idea of way-point caching.Therefore search operation becomes biased towards thepaths found in the earlier successful executions.
• Multi-Bridge ERRT is similar to the RRT-connect butit continues to execution although a connection is estab-lished between the trees until a pre-specified number ofconnections are established. Then the resultant structureis not a tree anymore. To find the shortest path anoptimal graph search algorithm is used, such as A*.
III. EXPERIMENTAL SETUP
Webots[15] is ised to simulate and visualize the robot andits environment. CGAL[17] is used as a library to detectcollisions which might occur during sampling, or whileconnecting sampled nodes. However, the core of this studyis based on a recently-released open source motion planninglibrary, OMPL[18].
OMPL consists of many state-of-the-art sampling-basedmotion planning algorithms. It doesn’t include any collisionchecking or visualization routine which is done on purposeto abstract the algorithms from the problem definition andany other visual load.
The class ownership diagram in the figure 3 shows therelationship between the essential base classes in OMPL.For example, SpaceInformation owns a StateSpace; Plan-ner does not own SpaceInformation, although a Plannerdoes know about the SpaceInformation, and uses providedfunctionality. By using the SimpleSetup class, it is onlynecessary to instantiate a ompl::base::StateSpace object,a ompl::control::ControlSpace object (when planning withdifferential constraints, i.e, planning with controls), anda ompl::base::StateValidityChecker object. Many commonstate spaces have already been implemented as derivedStateSpace classes.
The simulated robot and the its simplified version is shownin the figures 1 and 2.
Fig. 1. The simulated robot in Webots simulation environment with severalobstacles.
Fig. 2. The simulated robot in Webots simulation environment withseveral obstacles. Orange and blue balls show the start and goal positionrespectively. Joint angles start and goal configurations are set inside theprogram.
Fig. 3. OMPL API overview, adapted from OMPL documentation website
IV. IMPLEMENTATION DETAILS
Setting up the planning problem in OMPL is as follows:• identify the space we are planning in: C = R2×S1×S1,• select a corresponding state manifold from the available
ones, or implement one,• since C contains an R2 component, we need to define
bounds,• define the notion of state validity,• define start states and a goal representation.Once these steps are complete, the specification of the
problem is conceptually done.The state manifold that is used in this work can be given
as follows:
StateManifoldPtr cm;cm = StateManifoldPtr(new RealVectorStateManifold(2)) +StateManifoldPtr(new ompl::base::SO2StateManifold()) +StateManifoldPtr(new ompl::base::SO2StateManifold());
OMPL requires a state validity method to be implementedwhich is called during the sampling if any sample configu-ration is collided with a configuration space obstacle. To dothis, I’ve polygonized all the obstacles in the environment
and check if any intersection occurs in the state validitychecking routine. These are done by CGAL::do Intersectmethod.
Start and goal configuration settings are similar to thestate manifold representation. Planners in OMPL requiresspace information and problem definition information aswell. Space information is acquired via the state manifoldrepresentation that is done at the very first. Problem defini-tion takes the start, goal and space information to becomeready for planner configuration.
One other great feature is that, planning time can belimited by giving a time interval input argument to theplanner’s solve method. This is very helpful for a time-critical application.
V. RESULTS AND DISCUSSION
Although current setup allows exhaustive testing andbenchmarking the algorithms, due to a known-bug in thecode, simulation crashes after a couple of planning is run.This is due to the CGAL’s exact-inexact kernel usage issue,and happens during the polygon intersection tests.
The figures below show sample runs of the algorithm:
Fig. 4. Sample run. Please notice the visualization bug.
Another known bug is about the visualization as it can beseen in the figure 6 where a vertex is missing, and edges werenot drawn. This happens since in Webots the single messagesize limit is 1024Bytes that is sent from the supervisorcontroller process to the simulation process(physics plugin).Yet, this issue can be solved by casting float numbers toshort integers by multiplying them with a known scalar andthen dividing them with the same amount when the packageis received by the other end and casting them again to thefloat numbers. This is a quick-n-dirty solution that I’ve donefor this study. Better way is sending multiple packages andsending the overall size of the package that is intended tobe sent at the very first header of the package-sequences sothat the receiver can wait until the transmission is finished.
Fig. 5. Sample run 2. Notice that the path being found can vary due tothe random nature of the algorithm.
Yet another issue is that the states being shown in thefigures reflect only the main checkpoints. Actually, OMPLfinds many states but for memory issues it doesn’t storethem, but they can be reconstructed by OMPL’s interpolationmethods.
The solution can be improved by changing the weightsof the sub manifolds that are added at the first of theconstruction. These weights are used while calculating thenearest node of the tree to a randomly sampled goal position.This also affects the precision of the algorithm since the
Fig. 6. Sample run 3. Among convex/concave polygons. Planning requiresat most 0.5 seconds in this setup, given that the environment boundedbetween [0, 2] meters.
tolerance is calculated by taking difference of the values ofeach sub manifold dimensions with the goal configuration.
When OMPL picks a node to expand, it performs manylocal collision checks if that expansion is collision free. Thiscan be reduced by changing the step size to a more suitablevalue. But, for sure, this requires an expert knowledge andshould be calculated carefully.
A sample planning output is given below for the planners,RRT, Lazy-RRT, RRT-connect, Parallel-RRT respectively.Parallel RRT generates the largest number of nodes to coverthe area more than the other algorithms. As far as I can see,it could make use of 4 cores in the i7 machine in whichthe tests are done. RRT-base on the other hand created morenodes than the other non-parallel RRTs which resulted inmore time to return a solution. Lazy-RRT made less numberof collision checks compared to the RRT-base. RRT-connect,on the other hand, is the winner algorithm among the otherones since it created very less number of nodes, and returnedto solution quickly, due t its dual-root architecture whilegrowing.
A. RRT-base Algorithm:
Info: RRT: Maximum motion extension distance is as-sumed to be 1.822322
Info: RRT: Starting with 1 statesWarning: RRT: Found approximate solutionInfo: RRT: Created 45 statesFound solution:collision check cnt: 1565collision occured: 58n states in the tree: 45
B. Lazy-RRT Algorithm:
Info: LazyRRT: Maximum motion extension distance isassumed to be 1.822322
Info: LazyRRT: Starting with 1 statesInfo: LazyRRT: Created 344 statesFound solution:collision check cnt: 357
collision occured: 16n states in the tree: 344
C. RRT-Connect Algorithm:
Info: RRTConnect: Maximum motion extension distanceis assumed to be 1.822322
Info: RRTConnect: Starting with 1 statesInfo: RRTConnect: Created 6 states (2 start + 4 goal)Found solution:collision check cnt: 128collision occured: 4n states in the tree: 6
D. Parallel-RRT Algorithm:
Info: pRRT: Space information setup was not yet called.Calling now.
Info: pRRT: Maximum motion extension distance is as-sumed to be 1.822322
Info: pRRT: Starting with 1 statesWarning: pRRT: Found approximate solutionInfo: pRRT: Created 95 statesFound solution:collision check cnt: 3145collision occured: 135n states in the tree: 95
VI. CONCLUSIONS
In this study, RRT based algorithms are tested on asimulation environment with an articulated robot. A recently-released motion planning library, OMPL, is studied. It seemsthat OMPL is going to be a standard planning library inRobotics community in the near future as it is in the case ofComputer Vision community and OpenCV library.
RRT-connect algorithm is the best among the other RRTalgorithms, it could be optimized to work with multipleprocessor cores which even make this algorithm performmuch faster while allowing it to cover more regions.
The most powerful side of the OMPL is that it gives thedesigner freedom to use any kind of a robotic platform toplan its action by using various planning algorithms withoutmaking no change in the planning side of the code assoon as the state and control manifold representations, andstate validity checking routines are done correctly for thatparticular platform.
I’m hoping to implement other RRT-derivatives, suchas Execution-extended RRT, and multi-bridge ERRT for apossible contribution to this library.
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 andmobile robots. Int. J. Rob. Res. 5(1):90–98.
[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 potentialfield 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, 412–442, 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 pathplanning. 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, pages39-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 ComputationalGeometry 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