Probabilistic roadmaps for efficient path planning

Transcription

1 Probabilistic roadmaps for efficient path planning Dan A. Alcantara March 25, Introduction The problem of finding a collision-free path between points in space has applications across many different fields. In robotics, path planning prevents articulated arms from colliding with other machinery and allows a car to drive autonomously to a destination. In computer animation and gaming, characters can be given destinations to reach without colliding with anything in their environment. For lower dimensions, such as planar robots which may only translate through space, theoretical methods have been developed which are guaranteed to find a path if it exists. However, as the dimension of the object increases, the work required to find a solution has been shown to increase exponentially. Because of this, newer developments in path planning have revolved around randomized algorithms. In this paper, I discuss a class of methods called probabilistic roadmaps, which have been shown to have a high success rate and a fast running time. However, the randomness of PRMs introduces its own set of problems, most often resulting in failures to find legal pathways through space. I will give a brief overview of the theoretical underpinnings of path planning in section 2, followed by a description of the basic PRM algorithm in section 3, then the shortcomings of the original PRM algorithm are introduced in 4, along with papers that address these shortcomings. 2 Theoretical background Let R be an object sitting in some environment. Typically, the environment will either be 2 or 3 dimensional, corresponding to a room that R lies in. The configuration space, or C-space of R is the set of all possible configurations of R, where a configuration defines a particular placement and orientation in the environment. For a translating planar robot, its configurations could be of the form (x, y), the coordinates of the robot s center. An articulated mechanical arm composed of 10 joints would have at least 10 degrees of freedom, corresponding to a 10D configuration. Note that although R may be embedded in a low dimensional space, the dimensionality of its configuration space can be arbitrarily high. Every point in C-space corresponds to a configuration of R, and paths through C-space correspond to movements of R in its environment. Note, however, that these paths may correspond to movements which lead R to collide with some obstacle in its environment. Define the free space of R as being the sections of C-space corresponding to collision-free configurations of R. As paths in C-space correspond to movements in the environment, paths through free space represent movements which completely avoid collisions. Using this correspondence, the path planning problem can be redefined as: given two configurations of R, is it possible to find a pathway in free space which connects them? For simpler cases, like purely translating robots lying on a plane, methods can be used to accurately calculate the free space. Using the notion of Minkowski sums, obstacles in the environment can be easily mapped to configuration space obstacles (C-obstacles). Intuitively, a Minkowski sum can be computed by taking the purely translating robot and sweeping it around the perimeter of an obstacle. The shape traced out by its configuration is the obstacle s equivalent in C-space. With knowledge of the exact form and position of the C-obstacles, the free space can be computed, and pathways between query configurations can be produced. However, the previous example was very simple: the C-space of the robot had the same dimensionality as its environment. Dropping this equivalency creates more complicated C-obstacles. Simply allowing the robot 1

2 to rotate creates a 3D C-space, where the planar obstacles take the form of curvy, pillar-like C-obstacles. A more difficult case arises in the case of a 2-joint articulated arm, with a fixed base and its configuration represented by its two joint angles. The arm s C-space would be topologically equivalent to a torus, mapping the planar obstacles to some blobby C-obstacles on the torus surface. In general, attempting to calculate an exact representation of the free space becomes increasingly unwieldly as dimensions are added to the C-space. This increasing difficulty led to the development of randomized algorithms, such as probabilistic roadmaps. 3 Probabilistic roadmaps Notice that nearby points in free correspond to similar configurations in the environment. Because of this, large portions of free space can be represented by relatively few points and still be representative of the true free space. Using this idea, PRMs avoid computing the free space directly and instead give a rough approximation to it with a graph, or roadmap, of legal configurations running through the free space. This graph is created by taking random configurations of R and checking whether or not R collides with any objects in its environment. Legal random configurations are added and checked against nearby neighbors to see if a pathway exists between them. If adding the edge doesn t create a cycle in the graph and the path between them through C-space causes no collisions, PRMs add an edge connecting the two configurations. After running the algorithm, the roadmap provides a decent estimate for the free space, which can then be used to answer queries. The basic algorithm from Kavraki et al. [2] can be split into three phases: the learning phase, which creates an initial graph through free space; the expansion phase, which attempts to expand the graph into poorly sampled or poorly connected areas; and the query phase, which attempts to find a path through the roadmap to connect two query configurations. 3.1 Learning phase The learning phase builds the initial roadmap by randomly sampling the C-space until it finds a valid configuration c in the free space. The original version of the algorithm uses uniform sampling to pick configurations to test. If c is found to be valid, the algorithm adds it to the roadmap and finds configurations in C-space that are close. c and its nearest neighbors are passed to the local planner, which looks for collision-free paths between c and its neighbors. The definition of two configurations being close is dependent on the problem s definition of distance. For example, for the case of a planar translating and rotating robot, the distance between two configurations could be the Euclidean distance between the center of the robot at those two configurations. For an articulated arm, the distance could be defined as the volume swept by the arm as it moves from one configuration to the another. Using the distance metric, the local planner attempts to find a path through free space connecting the two configurations it is given. The original paper proposes a quick function which looks for a linear path between the two configurations. The length of the path is calculated and the path is then regularly sampled, creating a new set of configurations. Each of these configurations is checked against the obstacles in the environment. If no collisions occur along the path, it is assumed that the c and its neighbor are both connected through free space and an edge connecting them is added to the roadmap if doing so doesn t introduce a cycle. 3.2 Expansion phase After the learning phase, the roadmap will likely consist of several connected components, separated by difficult areas in the environment. These difficult areas can consist of tight passageways or clusters of obstacles. In order to unify the graph, random configurations are chosen from the graph and are expanded. The configurations are chosen according to the difficulty of connecting the configuration to its neighbors. Various suggestions are proposed to judge a configuration c, including the number of neighbors near c and the distance between c and the closest configuration in a different component. They settle on the ratio of failed attempts to connect c to its neighbors over the number of attempts that were made for c. 2

3 Once a configuration c is chosen, it is expanded by performing a random-bounce walk from the configuration. A random direction is chosen, and c is moved towards that direction until it hits an obstacle. This new configuration is recorded (but not added to the graph), and a new random direction is chosen. After several of these bounces, the final configuration reached by the walk is added to the graph, marked as being connected to c through the recorded configurations, and checked against its neighbors in different components to see if components can be now be combined through the final configuration. Because new configurations are connected to existing configurations in the roadmap, no new components are ever created: at worst, it fails to connect two unconnected areas. 3.3 Query phase Given two queries, the algorithm attempts to find paths through free space to connect them to the roadmap. If they cannot be directly connected to the same components in the roadmap, random-bounce walks are performed from the start and goal configurations in an attempt to get them connected to the same component. If the algorithm still can t connect them to the same component, the algorithm returns that it could not find a path. If the algorithm succeeds in connecting them to the same component of the roadmap, a simple Dijkstra search is performed through the graph to find the necessary movements required to connect the configurations. However, because the roadmap is acyclic, the paths will be unneccessarily long. In order to fix these paths, path smoothing is performed in order to cut out unnecessary steps. Because the paper did not define a particular way to do the smoothing, I chose to perform a second Dijkstra search. Prior to performing this new search, the local planner attempts to connect each node in the pathway to any nodes further down the pathway. If the planner is successful, new graph edges are added, and the search is then performed. This leads to more direct pathways through space, rather tran a path which snakes through the tree. Figure 1: Path planning for a 5 joint articulated arm and an ellipse which can translate, rotate, and stretch. The left image hides the roadmap to avoid clutter. 4 Shortcomings of probabilistic roadmaps While PRMs tend to be successful at a wide variety of problems, there are a number of issues with the original algorithm, as evidenced by the body of research following the original paper. Figure 2: 1st: Obstacle course without any objects in it. 2nd: Pathways snake through free space before finally arriving at the destination (blue). Path smoothing creates a more direct pathway (red). 3rd: Basic PRM oversamples space and still can t guarantee that both sides of the environment are connected. 4th and 5th: Before and after expansion. Note that expansions tend to bunch up near walls. 3

4 Uniform sampling does poorly in narrow passageways Because sampling is done without taking any obstacles into account, the chance of finding a valid configuration in a narrow passageway is slim. Instead, PRMs tend to oversample wide open areas and provide few (if any) samples inside narrow passageways. This fractures the roadmap into several different components. Useless configurations are added to the roadmap Wide open areas in free space are oversampled, resulting in a number of useless configurations in the roadmap. These configurations can often be dropped without affecting the connectivity or success of the PRM in answering queries, and essentially increase computation time without providing any benefit. The local planner isn t powerful enough The authors of the original paper propose that a fast and simple local planner is key to constructing the roadmap and answering queries quickly. However, there are a number of authors who suggest that more powerful planners should be used. More powerful planners attempt to find non-linear paths through C-space. Although successful connections result more often, these planners tend to be slow and require more computation. Loses most progress between two configurations during expansion if path isn t completed After performing the random-bounce walk, the final configuration is added to the graph. However, it is never selected for expansion. Instead, only configurations from the end of the learning phase are considered. Even if the path is very close to connecting two components, the algorithm will instead opt to start over from the beginning of the walk. The only time the final configuration from the walk will be useful is if the graph connects to it using a random-bounce walk from the other component. Many papers have since been presented which attempt to address these concerns. To examine their effectiveness, I have implemented a basic PRM planner, with some modifications to test some of the methods suggested by the following papers. I also employ stratified sampling, which partitions C-space and prevents clumping of sampled configurations. The PRM is used to plan the path of an ellipsoid robot which can translate, rotate, and stretch while preserving its volume. I also planned the path of a 5-joint articulated arm through space and tested the effectiveness of some of the methods on it. 4.1 OBPRM: Obstacle-based PRM Figure 3: Sampling strategy for OBPRM. Left: A binary search is performed between a valid and illegal configuration to find the obstacle boundary. Middle: Learning phase result for ellipsoid robot. Right: Two large components separated by a wall and narrow passageway. Small connected groups lie in the passageway and can be expanded to connect both sides of the environment. Images for the robot were uninteresting and thus omitted. Obstacle-based PRM [1] addresses the above concerns by augmenting the sampling strategy and the local planner. The sampling strategy is replaced by one that attempts to find configurations near object boundaries. It does this by finding a pair of configurations, one of which is a valid configuration in free space, and another which is colliding with an obstacle. A binary serach is then performed between the two configurations in an attempt to find a configuration sitting on the boundary of the obstacle (see figure 3: left). This has the tendency to bunch configurations around boundaries and away from open areas. However, this sampling strategy makes it more difficult to sample in narrow passageways. In contrast to the original, OBPRM requires finding both a configuration inside the passageway and one that is outside the passageway. 4

5 The second improvement they suggest is to use more powerful local planners. In addition to the linear local planner, they utilize two proposed in an earlier paper called rotate-at-s and another that is an A variant. Rather than linearly blend the translation and the rotation at the same time, rotate-at-s generates a pair of new configurations at s along the path. Initially, only the translation between the configurations is blended until reaching the first configuration at s. Then the configuration stops translating and instead rotates in place. After completing its rotation, it continues translating towards the target. While this planner may be useful, it seems to be useful only for non-articulated objects. Their A variant looks at possible nearby neighbors, then chooses the one that is most likely to lead to a successful connection. It iterates until it either finds the destination configuration or runs out of time. Experiments have shown this to be the most successful planner out of the three, but it is considerably slower. All of these planners are used at various stages of their version. Their final improvement replaces the expansion step. The original version of the algorithm would take a random-bounce walk from some random configuration. OBPRM instead splits the expansion phase into two different subphases. In the first phase, it tries to connect different components of the roadmap together by repeatedly expanding the nearest neighbors of each. If a thin wall completely separates the two components, then this step is useless. The algorithm will note that the two components are very close to each other, and continually produce nodes which don t help with the connectivity. In the second subphase, OBPRM tries to expand small components. They reason that small components must be based in difficult areas, and can be used to bridge two larger components. From my experience, I find this to be true. Figure 3:right shows two large connected components separated by a narrow passageway. Small connected components lie inside the passageway, and when expanded, cause the entire graph to become connected. 4.2 Visibility Based Probabilistic Roadmaps Figure 4: 1st: Typical roadmap discovered by VBPRM. Note how few nodes there are in the graph. 2nd: Arm free space with basic PRM. 3rd: Arm free space with VBPRM. Visibility based probabilistic roadmaps [3] aim to cut down on the number of useless configurations in the roadmap. It uses ideas from visibility graphs to determine whether or not a configuration should be added to the roadmap. Given any configuration c in free space, it is able to see other configurations in space and connect to them using the local planner. The area that c can see is said to be guarded by c, because adding c will prevent most other configurations from being added to the area that c can see. Adding a new configuration to the roadmap is allowed only if it covers an unguarded part of the roadmap, or if it can be seen by two guards. These latter configurations connect two guards together, producing a coherent roadmap. This results in a highly compact representation of the free space, since only a few configurations are kept in wide open areas. This speeds up the time required to process a query since the algorithm has to check fewer nodes to connect the queries to the graph. However, this still fails to help find narrow passageways it may even make finding these more difficult. Consider a pathological case of a passageway shaped like a V. In order to acknowledge traveling through the passage as being legal, the learning phase must be able to find three nodes: two which lie at the ends of the V, and can see its tip, where a third node must lie. Because no new nodes may be added to the roadmap if can be reached by only one node, the only place that a node can go in the passageway is at the very tip. Unless this node is sampled, both sides of the passageway will remain disconnected. 5

6 4.3 Finding Narrow Passages with Probabilistic Roadmaps: The Small Step Retraction Method Narrow passageways are difficult to find configurations in because they are so small. This paper by Saha et al. [4] attempts to solve the problem by finding a roadmap in a different C-space. Given a set of obstacles in the environment, they are shrunk so that narrow passageways become wide enough to encourage the generation of new samples. A roadmap is generated in this shrunken environment, which is more likely to allow passage through the widened passageway. They then propose two strategies, one optimisic and one pessimistic. The optimistic approach returns the first path it can find in the shrunken environment, and checks to see whether or not the configurations composing the path can be repaired to fit in the correctly-sized passageway. While this method is fast, it often fails because it doesn t check the pathway nodes in the real C-space before returning it. In contrast, the pessimistic approach checks to see whether or not a pathway node can be repaired before placing it in the path. Although it is much slower, the paths returned by the pessimistic approach are guaranteed to be valid. The main problem with this method is that shrinking obstacles introduces false passageways in the roadmap. Consider an environment where two large blocks are sitting on top of each other, with no free space between them. Once the obstacles are shrunk, a false passageway is introduced, which will prevent any nodes in it from being repaired to be valid in the real C-space. False passageways cause the optimistic planner to fail repeatedly as it returns irreparable paths through the false passages. The pessimistic planner realizes that these passageways aren t real and attempts to find other paths. Figure 5: Creation of a false passageway after shrinking two touching obstacles. 5 Summary Before the advent of probabilistic roadmaps, path planning in high-dimensions was computationally intensive and impractical for most applications. Their introduction allowed the construction of paths for many new classes of objects in reasonably short amounts of time. Probabilistic roadmaps have been applied from things like holonomic robots, to autonomous vehicles, and even to finding collision-free walks through space for autonomous characters. Although the original version of the algorithm had a few shortcomings, including overgeneration of useless nodes and problems with narrow passageways, work that followed circumvented many of them. However, the generality of PRMs also makes it difficult to summarize the corpus of work that has been done since they were introduced. In addition, many of the improvements proposed in recent work has been specific to a particular path planning problem. For example, a paper on Medial Axis PRMs showed promise in finding paths through narrow passageways, but could only be used on rigid bodied robots. Another paper focuses on non-holonomic robots, which are robots that cannot turn in place. Nonetheless, the number of recent papers still citing the original paper suggests that work in this area is still progressing, and new applications will undoubtedly be found. References [1] N. Amato, O. Bayazit, L. Dale, C. Jones, and D. Vallejo. Obprm: An obstacle-based prm for 3d workspaces,

7 [2] Lydia Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark Overmars. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. Technical Report CS-TR , [3] C. Nissoux, T. Simeon, and J. Laumond. Visibility based probabilistic roadmaps, [4] M. Saha, J. Latombe, Y. Chang, and F. Prinz. Finding narrow passages with probabilistic roadmaps: The small-step retraction method,