IROS2019国际学术会议论文集 1108

Sampling based motion planning of 3D solid objects guided by multiple approximate solutions Vojt ech Von asek1and Robert P eni cka1 Abstract Sampling based motion planners are often used to solve motion planning problems for robots with many degrees of freedom These planners explore the related confi guration space by random sampling The well known issue of the sampling based planners is the narrow passage problem Narrow passages are small collision free regions in the confi guration space that are due to their volume diffi cult to cover by the random samples The volume of the narrow passages can be artifi cially increased by reducing the size of the robot e g by scaling down its geometry which increases the probability of placing the random samples into the narrow passages This allows us to fi nd an approximate solution trajectory and use it as a guide to fi nd the solution for a larger robot Guiding along an approximate solution may however fail if this solution leads through such parts of the confi guration space that are not reachable or traversable by a larger robot To improve this guiding process we propose to compute several approximate solutions leading through different parts of the confi guration space and use all of them to guide the search for a larger robot We introduce the concept of disabled regions that are prohibited from the exploration using the sampling process The disabled regions are defi ned using trajectories already found in the space being searched The proposed method can solve planning problems with narrow passages with higher success rate than other state of the art planners I INTRODUCTION Path planning of 3D solid objects is considered in this pa per This problem is a variant of the general motion planning problem studied mainly in robotics but it is applicable also in other research fi elds For example path planning is used in computational biochemistry to detect exit pathways from proteins 11 in protein folding 33 In virtual prototyping path planners help to determine a sequence of actions to assemble a product from its parts 2 Path planning can be formulated using the concept of confi guration space 27 The number of dimensions of the confi guration space equals to the number of Degrees of Freedom DOF The confi guration space of 3D solid objects is six dimensional which is already considered as high dimensional space Generally there is no explicit rep resentation of the boundary of obstacles of the confi gura tion space 34 which precludes the utilization of planning algorithms relying on the boundary representation of the obstacles e g Voronoi diagrams Sampling based planners explore the confi guration space using random sampling The samples are classifi ed as free 1 FacultyofElectricalEngineering CzechTechnicalUniver sityinPrague Technick a2 16627Prague CzechRepublic vonasek labe felk cvut cz The presented work has been sup ported by the Czech Science Foundation GA CR under research project No 19 22555Y Computation was realized on MetaCentrum facilities project CESNET LM2015042 or non free using collision detection The free samples are stored in a graph like structure roadmap which approxi mates the free part of the space A path in the roadmap is then related to a collision free motion in the workspace 27 The well known bottleneck of sampling based methods is the narrow passage problem 25 22 29 21 Narrow passages are small regions whose removal changes con nectivity of the space 19 Due to their low volume the probability of placing samples into them is low which makes diffi cult to discover a path through the narrow passages Guiding based planners steer the search through the con fi guration space using a guiding path The guiding path are usually computed in the workspace which is fast and suffi cient in problems with few DOFs 14 41 Guiding using workspace based paths may be however poor for problems with many DOFs Fig 1a because a workspace based path does not describe the motion for all DOFs Approaches 4 42 scale down geometry of the robot or obstacles to widen the narrow passages Usually fi nding a trajectory for the scaled down robot is faster than solving the original problem The trajectory found with a scaled down robot is iteratively repaired while increasing the size of the robot until the solution for the original problem is found However the initial solution may lead through such parts of the confi guration space that are not reachable by a larger robot Fig 1b In this paper we propose an extension to the guiding using approximate solutions At each level of problem relaxation represented by different scale size of the robot we com pute several different solutions trajectories To fi nd these different solutions we introduce the concept of disabled regions The disabled regions are formed by the previously found trajectories and they defi ne regions of the confi guration start goal A B C a Hedgehog in the cage b Example of workspace based path Fig 1 a The task of the Hedgehog puzzle is to remove the spiky object from the cage A guiding path red computed in the workspace suggests only the 3D position of the object while the main diffi culty is in the rotational movements b Example of guiding using approximate solutions Part A is traversable only by the small robot the larger robots need to pass the regions B or C The proposed approach fi nds the guiding paths in all three regions 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 978 1 7281 4003 2 19 31 00 2019 IEEE1480 space where no further search is performed The introduction of the disabled regions forces the employed sampling based planner to fi nd a new alternative solution that is different from the previous ones The trajectories found at a given level of problem relaxation are used to guide the search at the next level of problem relaxation until the solution for the original problem is found By guiding the search using multiple trajectories the planner has a higher chance to fi nd a new solution than when guiding is realized only using one solution This consequently increases the success rate of the planning process II RELATED WORK Rapidly Exploring Random Trees RRT 26 and Prob abilistic Roadmap PRM 24 are widely used sampling based planners PRM fi rst samples the confi guration space classifi es the samples using collision detection and stores the free ones Afterwards the stored samples are connected if possible by collision free edges with their close neighbors which results in a graph structure roadmap Contrary to PRM which fi rst samples the space and then connect the samples to the roadmap RRT simultaneously performs the sampling and connection RRT builds a tree rooted at the initial confi guration and it iteratively expands it towards the randomly generated samples The expansion is realized from the node that is nearest to the random sample The algorithm terminates if the goal confi guration is approached by the tree close enough or after a predefi ned number of planning iterations Many modifi cations of RRT and PRM have been intro duced to cope with the narrow passage problem 15 In prob lems with few DOFs the diffi cult regions can be estimated from the properties of the workspace 25 39 3 35 In MA PRM 44 the random samples are generated around the medial axis of the workspace Several approaches attempt to estimate the narrow passage directly in the confi guration space 30 20 9 10 Gaussian PRM 30 generates the random samples close to the obstacles in the confi guration space which is achieved by drawing two samples close to each other and using them only if exactly one of them is collision free This idea is extended in the Bridge test that used two close samples to estimate if a point in the middle of them is collision free 20 The roadmap is extended by the midpoint only if both the samples are in a collision and the midpoint is free The strategies 30 20 need to be combined with the uniform sampling to enable sampling of free regions 37 The comparison of these PRM based approaches can be found in 21 17 43 Different approaches to cope with the narrow passage problem have also been developed for RRT based planners As RRT samples the space and builds the tree simultaneously the na ve increase of probability of sampling in a region does not ensure that RRT builds the tree through the region as the growth of the tree may be blocked by the obstacles In the Retraction RRT 47 28 the tree is retracted along the boundary of the obstacles in the confi guration space The boundary is estimated by a local sampling In RRT DD 45 an activation radius is assigned to each node of the tree The node is selected for the expansion only if the random sample is located within this radius The radius of new nodes is set to and it is decreased to a predefi ned default value if the node cannot be expanded Therefore the radius is small for nodes located near to the boundary of obstacles of the confi guration space An automatic adaptation of this default radius was proposed in 23 Another way to attract the tree towards the narrow pas sages is to place key confi gurations close to the narrow passage and increase the probability of sampling around the key confi gurations 38 The work 38 however does not specify how to fi nd the key confi gurations The generaliza tion of the key confi guration bias is the guided sampling where a sequence of waypoints a guiding path is used to generate samples in the confi guration space 41 14 13 In the guided sampling the probability of drawing the random samples around the waypoints is adapted according to the progress of the tree This is the main difference from the PRM based extensions that increase the probability of sampling in all regions simultaneously A low dimensional workspace can be discretized to a grid and searched e g by A to fi nd a sequence of cells to be visited Sampling based planners can be then used to fi nd continuous plan between the consecutive cells 32 Alterna tively Any Angle path planners can be used instead of A to fi nd the discrete path 31 The guiding paths in 2D 3D polygonal workspaces can be computed using the Visibility graph or using Voronoi diagrams assuming a point robot 41 The approach 12 pushes newly generated samples towards the medial axis of the workspace Multiple workspace based guiding paths are used simultaneously in 14 Guiding based planners are used for many DOF robots operating on rough terrain e g for walking robots 5 6 or for robots with reconfi gurable fl ippers 8 In these scenarios the guiding path is found in a 3D mesh or elevation map describing the terrain the planners then mainly samples other degrees of freedom e g the fl ippers or joints of the walking robots The narrow passages are located in the confi guration space and therefore their properties e g shape and vol ume depends both on the geometry of the robot and the obstacles An ideal guiding path should be fully defi ned in the confi guration space but it s construction would require to solve the motion planning problem of the same complexity This is usually time consuming unless the same space is searched repeatedly For example method 7 16 used previously found trajectories in the confi guration space for fast replanning in robotic soccer but they are not designed to cope with the narrow passages Therefore the problem has to be simplifi ed to fi nd a good guiding path in the confi guration space in a reasonable time By reducing the geometry of the robot e g by scaling it down shrinking or thinning the narrow passages became larger A similar effect can be achieved by allowing a little penetration between the robot and the obstacles This was fi rst discussed in 4 where the PRM roadmap computed for a relaxed version of the problem is reused for planning for a 1481 less relaxed version of the problem The paper 4 proposes a general idea but without specifying technical details for reusing the roadmap or for fi xing an invalidate solution In 19 also a PRM based planning is extended by using the dilated free space where both robots and the obstacles are scaled down by an adaptive factor This factor is determined using a binary search in each step of the algorithm III PROPOSED METHOD The guided based sampling of confi guration space allows the planners to focus on regions that are believed to contain a solution 38 14 12 42 41 By reducing the geometry of the robot and consequently increasing the volume of the narrow passages approximate solutions can be found in the confi guration space 4 42 The latter methods employ only one approximate solution i e one trajectory for a scaled down robot which may be located in such part of the confi guration space that is not traversable by the larger robot To prevent this situation we propose to utilize several different guiding paths simultaneously At each level of prob lem relaxation which is determined by the scaling factor we attempt to fi nd several trajectories step by step Every time a trajectory is found its waypoints are added to a set of disabled regions and the confi guration space is searched again In this subsequent search the disabled regions are ignored which forces the planner to fi nd an alternative trajectory The overview of this process is depicted in Fig 2 The work presented in this paper is an extension of our previous work 40 where the planning is guided using only a single guiding path In 40 the guiding path is iteratively repaired starting from the weakest point of the existing solution In this work the guiding paths are not repaired At each level of problem relaxation a new set of guiding paths is computed A Used Notation Let C denote the confi guration space The collision free confi gurations form the subset Cfree C where the robot can move The task is to compute a collision free path from an initial confi guration qstart Cfreeto the goal confi guration qgoal Cfree The distance between two confi gurations is denoted Several levels of robot scales are used we simply denote them by a factor s smin s 1 where sminis the smallest considered scale of the robot and s 1 represents the original robot Various methods can be used to manipulate the geometry of the robot e g scaling or thinning 19 Let R q1 qn qi C is a set of n confi gurations representing the disabled region A confi guration q is said to be in the disabled region R if there exists q0 R such that q q0 dsafe q qgoal dsafe where dsafe defi nes the safe zone around the start and the goal respectively We refer to this test as to region test in the rest of the paper The safe zones are depicted in Fig 2 A guiding path Piis a sequence of micollision free confi gurations Pi q1 qmi qi Cfree Let qi j Pi is the j th point in the path Pi The guided sampling requires to generate the random points along the guiding path while respecting the growth of the tree Simple yet effi cient method is to activate generation of the random samples only if the tree is nearby We achieve this using so called active waypoint vi 1 vi mi which denotes index of a point of path Pi around which the random samples are generated B Sampling with Disabled Regions The proposed planner denoted as SMAS Sampling with Multiple Approximate Solutions maintains a set of different approximate solutions that are used to guide sampling of the confi guration space The principle of RRT is used to sample the space we extend RRT by the concept of disabled regions and guided sampling along multiple guiding paths We denote this sampling procedure as RRT SMAS in the rest of the paper The main loop of SMAS is listed in Alg 1 and RRT SMAS is listed in Alg 2 In SMAS the scale of the robot is iteratively increased starting from sminand incremented by s At the beginning m initial distinct trajectories are found using RRT SMAS Alg 1 lines 6 10 If a feasible trajectory is found it is added to the set of guiding paths G The waypoints of the trajectory are added to the disabled region R if they pass the region test After the m initial guiding paths are found the algorithm increases the scale by s and fi nds new trajectories feasible at that scale The trajectories G found at the previous level are used as guiding paths The planner attempts to fi nd again m distinct trajectories step by step using RRT SMAS Alg 1 line 20 Similarly to the initial phase every time a new trajectory is found it is added to the disabled region R Alg 1 lines 21 23 and also to the set of the guiding paths G Practically it is necessary to limit the number of attempts to fi nd the m guiding path which is controlled by the variable Ntrials The algorithm terminates after a path is found for the original robot s 1 The resulting trajectory is in G The core of the proposed method is the sampling proce dure RRT SMAS that samples the confi guration space along the paths G while avoiding the regions R Alg 2 The guid ing is realized using the active waypoints vi that are fi rst set to the beginning of the guiding paths vi 1 i 1 m The random sample qrandis drawn from the whole C with probability 1 pbias otherwise it is generated along the guiding paths as follows A guiding path is selected randomly from G and the random sample qrandis generated around the active waypoint of the selected path lines 7 9 in Alg 2 The nearest neighbor qnear T towards qrandis found and it is extended using the straight line local planner If the resulting confi guration qnewis collision free it is tested to be located 1482 C A B obs C free C goal start C A B obs C goal start C A B obs C goal start C A B obs C goal start C A B obs C goal start a b c d e Fig 2 Example of fi nding distinct trajectories using disabled regions The fi rst trajectory is found through the region A a The waypoints of the trajectory are added to the set R of disabled regions red circles b When the confi guration space is searched again disabled regions are ignored RRT cannot grow tree into them and a new trajectory is found in the r