The Z3{Method for Fast Path Planning in Dynamic Environments

The Z 3 {Methodfor Fast Path Planning in Dynamic EnvironmentsBoris BaginskiReal{Time Systems and Robotics Group, Fakultat fur InformatikTechnische Universitat Munchen, Orleansstr. 34, 81667 Munich, Germanye-mail: baginski@informatik.tu-muenchen.deAbstractWe present a method to plan collision free paths forrobots with any number of degrees of freedom in dynamicenvironments. The method proved to be very ecientas it ommits a complete representation of the high dimensionalsearch space. Its complexity is linear in thenumber of degrees of freedom. A preprocessing of thegeometry data of the robot or the environment is not required.With the time as an additional dimension of thesearch space it is possible to use the method in knowndynamic environments or for multiple robots sharing acommonworkspace.Keywords: Path Planning, Dynamic Environments,Randomized Algorithms1 IntroductionThe method presented in this paper is part of our researchonintelligent and autonomous robot systems. Animportant component ofsuch a system is a path plannerthat creates collision free and physically possible motionsbetween positions that were planned on a higher, moreabstract level.A path planner for practical use must be able towork fast in any kind of environment and with any loadof the robot. For example, when operating a manipulatoron a mobile platform in a known environment, the geometryof the manipulator's workspace gets known whenthe platform stops and the precise position is sensed.Now the manipulator should be able to work almost immediately.In addition, there are dynamic changes inthe environment that makes it necessary to take timeinto consideration for the motion planning, e.g. whenusing the mobile manipulator while the platform is inmotion.The Z 3 {method (ZZZ is a german abbreviationwith the meaning of goal{directed andrandomized planningin temporally changing environments) avoids preprocessingand reduces complexity as far as possible tond a path with high ecency. The planned path isnot optimal, but more important is the speed and thegeneral applicability of the scheme.2 The Z 3 {MethodThe only requirement for the use of the Z 3 {method isa geometric and kinematic model of the robot and theenvironment. In industrial applications this data is available.For service robots, sensors, for example range sensors,can be used to scan the geometry of the environment.The method is a further development of the ZZ{method by B. Glavina [6]. It consists of two hierarchicallycoupled components. The lower level is an ecientgoal{directed planner that only uses local information totry to pass obstacles. The upper level is a randomizedplanner that uses the local planner and combines the results.The global planer explores the whole search spaceheuristically.Local Goal{Directed PlanningThe pose of a robot can be described by the positionsof its joints if the geometry is known. One point inthespace of possible joint values (the so called congurationspace, short c-space) is the precise description of a robot'sposition. The solution of the path planning task for ann-joint robot is to nd a curve between start and goalin the n-dimensional c-space. In a dynamic environmentthe c-space is extended by the dimension time and thesolution has to be found in the n+1-dimensional c-spacetime.This extension is not hogenous, as the dimensiontime is bound to strictly monotonous increase, while theother dimensions allow any motions, only constrained bythe robot's dynamics.Local Planning in Static EnvironmentsTo avoid exponential complexity with respect to thenumber of dimensions no complete representation of thesearch space is constructed. In contrast, only one dimensionalsubspaces are explored. The goal directed searchmoves linear from start to goal. The motion is calculatedin discrete steps, collision checks are performed inshort distances. The stepwidth is calculated from thetolerance that was added to the environment (or robot)geometry model and assures collision free continuous motionbetween two test points [6].

If a collision with an obstacle occures an avoidingmovement (slide step) is tried. First a point that is veryclose to the obstacle's surface is calculated with a depthlimitedbisection. Then directions that are orthogonalto the desired direction and orthogonal to each otherare computed. These directions and the respective reversedirections are the possible avoiding directions. Inthe n-dimensional case this results in 2(n ; 1) possibledirections. Figure 1 (a) illustrates this calculation. Theunderlying assumption is the local atness of the (n;1)-dimensional surface of the c-space obstacles.The stepwidth for the slide step is now calculated tonot exceed the discretizing stewpwidth mentioned above.Then the avoiding points that leads closer to the goalare checked for collision. If none of the allowed avoidingpoints is collision free the local planner terminates withoutsuccess (dead end). The restriction on steps thatleads closer to the goal is necessary to avoid 'uttering'that leads into an innite loop. This method can beinterpreted as the motion in a potential eld with itsglobal minimum at the goal. Only decreasing motionsare allowed. Dead ends can be seen as local minima.After a succesful slide step the linear motion in thedirection of the goal is tried again, and again it may benecessary to calculate a slide step. Figure 1 (b) shows asequence of slide steps along an obstacle's surface.In many cases the proposed combination of goal directedsteps and slide steps can nd a solution for thepath planning task by only considering local information.Figure1(c)shows an example. In static environmentstime is irrelevant and the local planner can retry a failedtask in the reverse direction from goal to start. In manycases dead ends can be avoided with reverse search.Local Consideration of TimeTime can be included as an additional dimension inthe local planning process if the following requirementsare fullled: the starting time is known the status of the system can be calculated at anylater time (the dynamics of the environment areknown) for all discretizing steps, the time of the current stepcan be calculated from the time of the last step ora limited number of earlier steps (the dynamic behaviourof the robot is known).Starting the local planner from a known point inc-space-time allows to plan a path andanarrival time.For the application in reality the transition from linearsteps to slide steps | resulting in a sudden change ofdirection | must be considered with care. This canbe achieved by correcting the time of some earlier stepsto slow down the motion appropriately. The positionsmoved in c-space-time then have to be checked againfor collision, this may result in an earlier start of theslide steps. In the general case, the adjustments for thedirectional change are limited to a xed numberofstepsand happen only locally.free C-space12C-space obstacleC-space obstaclestart3(a)free C-spacefree C-space(b)0C-space obstacles(c)goalFigure 1: (a) The last collision free position of the linearmovement is 0, the next step would be 1 but collides.With a bisection (points 2 and 3) a point very close tothe surface is calculated and orthogonal avoiding directionsare computed. (b) A sequence of successful slidesteps along a c-space obstacle. (c) An example for apath planned with the local planner as a combination ofgoal directed and slide steps.

Global Randomized PlanningFirst of all, it's tried to solve the task with the localplanner. If this fails, random collision free subgoals areused to create partial tasks that can solve the whole taskthrough combination. There are two parameters to bechosen for the global planner, rst the maximum numberM of subgoals that are used to nd solutions, and secondthe maximum number m of subgoals that are allowed ona path from start to goal.When using the ZZ-method [6] it shows that mostof all tasks can be solved with one or very few subgoalsalong the path. It proved to be more ecient to restartthe planner with M new random subgoals instead of allowingany number (< M) of subgoals on the path. Forthe same reason it is better to check the possible pathswith one subgoal rst, then the paths with two subgoalsandsoon.As far as possible unnecessary local planningshall be avoided. The following algorithm follows thisconception:check the direct path from start to goal, ifits possible then SUCCESScreate M random subgoals as the initialmembers of set U (set of unconnected points)initialize the set S(points that can bereached from the start) with the startingpoint.Loop from 1 to minitialize the set S new as empty setLoop for all points s i out of SLoop for all points u j out of Utry to connect s i with u j with thelocal plannerif this is successful:end Loopremove s iend Loopmake Stry to connect u j with the goalwith the local plannerif this is possible:SUCCESSif this is not possible:remove u jadd u jfrom Sequal with S newfrom Uto S newif S is empty, then NO SOLUTIONRETRY WITH NEW SUBGOALSend LoopNO SOLUTIONRETRY WITH NEW SUBGOALSThis algorithm creates a tree from the start in theset of subgoals. For every height of the tree possibleconnections to the goal are tested before the growthcontinues. This principle is illustrated in gure 2. Instatic environments the global planning can start simultanouslyfrom the goal. This results in a faster reductionof the number of unconnected points and thus in fasterplanning. An example for the whole planning process isshown in gure 3.Global Consideration of TimeThe time can be included in the global planning aswell, if the starting time and the dynamics are known.In this case the subgoals are not xed in the time whenthey are created. The time is xed when the subgoal isreached with the local planner, where the time is calculatedincrementally in discrete steps. Not being xed intime, the subgoals are not guaranteed to be collision free,but the random generation can estimate the time heuristicallyand thus reduce the danger of colliding subgoals.The time is propagated forward in the growing tree, everysubgoal is reached only once with the local planneryielding a subgoal arrival time. In the end, a goal arrivaltime can be returned. It is not possible to plan from goalto start in dynamic environments.Complexity and CompletenessThe presented algorithm evaluates all paths with up to msubgoals. This testing is complete, so if such apathexistsfor the given random subgoal distribution it is found.The restriction of the search depth to a constant valueresults in a complexity that is linear with respect to thenumber of M, the subgoals used in total. The complexityof the local planner is linear with respect to thenumber of degrees of freedom. This number determinesthe numberofavoiding directions that have to be tested.The underlying assumption of the Z 3 -method isthat there exists a large number of possible paths andone of those can be found with random subgoals withhigh probability. The search space is examined in verysmall parts only. In complex environments, only a largenumber M of possible subgoals may lead to a success.The maximum amount of time to nd an existing solutionis unbound. If M and m would grow unlimited,the method would be probabilisticly complete: if thereexists any solution, it is found.For the practical use these properties or the Z 3 -method are of no meaning. It shows to be very ecientin its conception, because in most cases a path can beplanned in short time. The durations for planning inrealistic environments are fractions of seconds or a fewseconds. Only very complex tasks that can not be solvedwith complete planners due to complexity maytake minutesor hours.3 Practical ResultsAll experiments shown in this chapter were performedon a Hewlett Packard Unix Workstation 9000/730. Onlythe user time of the planning process was measured. Thegeometry simulation was developed in our laboratory. It

goalgoalgoalgoalstartstartsubgoal 1(a)(b)subgoal 2start(a)start(b)goalgoalgoalstartsubgoal 1(c)(d)start(c)goalstart(d)goalFigure 3: An example for the planning process in a statictwo dimensional environment shown in (d). In the c-space map in (a) the failure of the local planner startingfrom start and from goal can be seen. In (b) a rst subgoalis tested thatcan be reached from the start but failsto be connected with the goal. In (c) the second subgoaltried leads to a solution. The planned path is shown in(d) as the movement of the tool center point.is based on an automatically created hierarchy of hullbodies that allows very ecient collision testing. Up tonow, our system is only capable to plan paths for robotsin static environments.start(e)startFigure 2: Systematic illustration of the global planning(there are no slide steps shown for clarity). The directconnection between start and goal fails, so M =9randomsubgoals are created. In picture (a) all possible connectionswith one subgoal are checked. In just one caseaconnection between the start and an unconnected pointis found. It is tested fora direct connection to the goalimmediately. This fails, the point itself is the rst leaf ofthe tree growing from the start. Beginning at this point,all connections with two subgoals are evaluated in(b), resultingin three newleaves. They are expanded in(c), (d)and (e), thus testing all possible connections with threesubgoals. In (c) two new leaves are created, (d) and (e)result in dead ends. The decreasing computational costsfor the tests, resulting from the reduced number of unconnectedpoints, can be seen well in this example. In (f) asolution for the path planning problem is found with foursubgoals. Given this random distribution, this is the connectionwith the smallest number of subgoals, found withthe smallest number of local planning.(f)Scenario MOBROBFigure 4: MOBROB scenarioThis scenario shows a manipulator on a mobile platformin an industrial environment. For the experimentsonly the six joints of the manipulator are used (the platformis static). The six dimensional c-space contains57.6% free space. The simulation system needs an averagetime of 0.4 ms to check a position for collision.The experiments are performed with M = 25 total sub-

goals and a maximum of m = 4 subgoals on one path.5,000 tasks were created by combining random positionsthat are very close to obstacle surfaces, so these taskscan be described as random pick-and-place{tasks. Thefollowing results are measured: 100% of the tasks are solved the average run time is 0.064 sec, max. is 2.56 sec there is an average of 0.042 subgoals per plannedpath. This shows the ecency of the local planner,that can solve almost all tasks the local planner is used 1.16 times on an average.Especially the results in this very realistically modelledscenario prove that the Z 3 -method is a method ofchoice for path planning in real applications with hardconstraints on time. Even in the rare cases were theglobal planner becomes necessary the planning times areusually below one second. the local planner is used 26.87 times on an average the ratio between the length of the planned pathand the length of the path explored with the localplanner is 8.13.In the second experiment 5,000 random tasks (combinationsof two random positions) are planned. Thefollowing results are obtained: 99.54% of the tasks are solved the average run time is 3.68 sec, max. is 114.76 sec there is an average of 0.35 subgoals per plannedpath. This shows the ecency of the local planner,that can solve about two third of all tasks evenin such a small workcell the local planner is used 4.45 times on an average.Scenario GRIPPERScenario ROTEXFigure 6: GRIPPER scenarioFigure 5: ROTEX scenario (front walls removed)The ROTEX{workcell was used for a number ofrobotic experiments in space on board the space shuttlemission D{2 [7]. The real geometry data from the DLRis used for the simulation. The workcell is very small, thesix dimensional c-space contains only 1.35% free space.The simulation system needs an average time of 0.8 msto check a position for collision. Both experiments wereperformed with M = 25 total subgoals and a maximumof m = 4 subgoals on one path.In the rst experiment the task shown in gure 5 issolved 100 times. The diculty of the task is not obviousin the picture. The lower half of the arm has to be turnedcompletely. The following results are measured: the average run time is 42.65 sec, max. is 117.55 sec only an average of 17.65 subgoals out of the 25 are'touched' by the local planner (because a solutionwith one subgoal is found) there is an average of 1.49 subgoals per planned pathThe scenario in gure 6 follows a scenario used in[8]. The robot has 10 degrees of freedom. These arethree translatoric joints and three rotational joints in thearm and two rotational joints in the two 'ngers'. Theleft slot is very narrow and to move the arm from theleft position into the upper free space is all but trivial.The c-space contains only 3.8% free space. A collisioncheck takes an average of 0.47 ms in the simulation. Theplanner is started with M = 500 total subgoals and amaximum of m = 4 subgoals on one path. In the experimentthe task shown in gure 6 has to be solved 30times. The following results are measured: the average planning time is 23,220 sec (ca. 6.5 h),max. time is 98,400 sec (ca. 27 h) on an average, the global planner restarts 2 timesper run and creates 500 new random subgoals an average of 1,316.7 subgoals are 'touched' by thelocal planner there is an average of 3.13 subgoals per planned path the local planner is used 12,741 times on an average the ratio between the length of the planned pathand the length of the path explored with the localplanner is 3,911.The results achieved in this scenario show that theZ 3 -method is not only theoretically but as well practi-

cally 'probabilistic complete'. Even for very complicatedtasks with a very high dimensional search space a pathcan be planned.4 Future WorkThe global planner repeatedly uses the local planner,but in many cases these local plannings are independentfrom each other. Based on this observation a concept toparallelize the planner on a network of workstations wasdeveleoped and is currently implemented.The run time of the Z 3 -method is coupled with theecency of the geometric simulation. In this area we aredeveloping new ecient data structures and algorithmsand try to use results from the growing research area'computational geometry'.One problem of the local planner is that the slidesteps are very close to the obstacles, yielding 'dangerouspaths' in uncertain environments. We try to integratedynamic protection shields to guarantee a safety distancewhenever this is possible. This has to be done withoutdecreasing the planners ecency and without increasingits conceptual complexity.Further investigations are done in developing a newlocal planer that is especially ecient for hyperredundantmanipulators. By shrinking and expanding themodel of the robot along its trajectory its possible to ndsolutions in much more cases than with the slidesteps,with the same linear computational complexity. Firstresults are very promising [1].A modication of the Z 3 -method is used for pathplanning for cooperating manipulators with dependenttool coordinate systems, e.g. cooperative transportation[5]. These conceptions shall be extended and generalizedto plan all classes of tasks for cooperating manipulatorswith overlapping workspaces.5 Discussion and ConclusionWe presented a method that is able to plan paths in dynamicenvironments. Its complexity is linear in the numberof degrees of freedom. The usability of the methodwas demonstrated in several scenarios. Some problemsand objections shall be discussed in this chapter.The path planned with the Z 3 -method is not optimal.The use of subgoals results in sharp corners anddetours in the path. For static environments we use alocal polygon optimizer that improves the quality ofthepathes very eciently [3].In static environments the Z 3 -method can not usethe 'experiences' of prior planning. The 'subgoal{tree' isremoved when a solution is found. Other path planningalgorithms that represent the free part of the c-spacewith a graph, e.g. [4, 8], or with cells, show better resultsfor repeated planning in the same environment. Inour opinion, the dominating criterium is the constantecency in unknown and dynamic environments thatis a property of the Z 3 -method. The only planner weknow with comparable good results is the RandommizedPath Planner [2, 9, 10], but its goal-directed componentrequires an expensive precomputation of the workspaceto calculate a potential eld, and the so called 'randomwalks' that escape local minima will not use the wholefree space in a way the random subgoals do.To summarize, the Z 3 -method is a reliable and versatileconcept. The global planner can be used in otherareas as well and is not bound with robotics. All planningtasks that can not be solved in one step and wherethe decomposition is not obvious can be adressed withthis strategy.References[1] Boris Baginski. Local motion planning for manipulatorsbased on shrinking and growing geometry models. InProceedings of IEEE Conference on Robotics and Automation(submitted), Minneapolis, April 1996.[2] J. Barraquand and J.-C Latombe. A monte-carlo algorithmfor path planning with many degrees of freedom.In Proceedings of IEEE Conference onRobotics and Automation,pages 1712{1717, Cincinnati, Ohio, May 1990.[3] Stefan Berchtold and Bernhard Glavina. A scalable optimizerfor automatically generated manipulator motions.In Proccedings of IEEE/RSJ/GI International Conferenceon Intelligent Robots and Systems IROS'94, pages1796{1802, Munich, September 1994.[4] Martin Eldracher. Neural subgoal generation with subgoalgraph: An approach. In Proceedings of World ConferenceonNeural Networks WCNN '94, 1994.[5] Max Fischer. Ecient path planning for cooperatingmanipulators. In Proccedings of IEEE/RSJ/GI InternationalConference on Intelligent Robots and SystemsIROS'94, pages 673{678, Munich, September 1994.[6] Bernhard Glavina. Planung kollisionsfreier Bewegungenfur Manipulatoren durch Kombination vonzielgerichteter Suche und zufallsgesteuerter Zwischenzielerzeugung.PhD thesis, Technische UniversitatMunchen, February 1991.[7] G. Hirzinger. Rotex { the rst space robot technologyexperiment. In Preprints of the Third InternationalSymposium on Experimental Robotics, pages 302{320,Kyoto, Japan, October 1993.[8] Lydia Kavraki and Jean-Claude Latombe. Randomizedpreprocessing of conguration space for fast motionplanning. In Proceedings of IEEE Conference onRobotics and Automation, pages 2138{2145, San Diego,California, May 1994.[9] Jean-Claude Latombe. Robot Motion Planning. KluverAcademic Publishers, 1991.[10] Jean-Claude Latombe. Geometry and search in motionplanning. Annals of Mathemathics and Articial Intelligence,8:215{227, 1993.