QuadTree Motion Planner
Introduction
For my final project, I decided to implement a QuadTree motion planner. The planner can plan paths for robots that are not capable of rotation, but can translate in any direction in the plane. The program allows for a boundary of any shape, but is currently only restricted to convex obstacles. An example of a path that this planner generates is illustrated below:
In the above example the
planner provides a path for a triangular shaped robot in a world with a
concave border and a single convex obstacle. In this image, a few
specific things are noticable. The green triangle signifies the starting position of the
robot. The red triangle
indicates the goal position of
the robot. It is important to note that the robot used in this
example is triangular. If a different-shaped robot is used, this
will be reflected appropriately. Configuration
space
obstacles are in purple.
The empty cells in the
quadtree are displayed in light blue.
The yellow cells in the image
signify the e-channel that
connects the start cell to the goal cell and the black line through the e-channel
illustrates the path that the robot
should take. Before any of the aforementioned items were
displayed, the environment looked like this:
Click here for a full-sized image
Click here for a full-sized image
The blue
line signifies the border
of the environment, while the blue
diamond in the center represents an obstacle in the workspace.
Planner
Specifics
Third-party Libraries
The planner makes use of two third-party libraries: DOLT and CGAL. Dolt is used to display all of the items the environment. This includes such things as: the world boundary, the obstacles, the C-space obstacles, the start and goal positions of the robot, the cells in the quadtree, and the path that the robot should take. All of these items are noticable in the previous two examples. The CGAL library is used for two main purposes. The first is the convex hull function which is used to assist in the creation of C-space objects. The CGAL library is also utilized to identify itersections between cells in the quadtree and c-space obstacles. Specifically, CGAL is used to identify edge intersections, as well as whether or not a specified point lies within a given polygon. More on this later.
Implementation Details
The quadtree is made up of a bunch of objects I called QuadTreeCells. Each QuadTreeCell contains information such as: the depth of the cell, whether or not the cell is a leaf in the quadtree, who the children of this cell are, a list of adjacent cells, the status of the cell, (Unknown, Empty, Mixed, or Full) and a list of C-space obstacles that the cell intersects.
Creating the QuadTree
The first thing that must occur before the quadtree can be created is the generation of the C-space obstacles. The C-space obstacles are generated using the convex hull method with the assistance of the CGAL library. The C-space must be determined before the tree is created or else each cell will not know what it's status is. The status obviously cannot be determined without knowledge of the C-space obstacles. After the C-space is generated the quadtree can be created.
When the tree is first created the initial cell is the same size as the bounding box for the world. This bounding box is kindly provided by dolt. When any cell is created it is passed a list of any C-space obstacles that it may intersect with. In the case of the first cell in the quadtree it is passed all of the C-space obstacles. In the case of this cell's children, they are only passed the cells that the parent cell intersects with. So, when any cell is decomposed, it passes the list of cells that it intersects with to its children. The children then only need to check these cells for possible collisions. In this way, computing time is saved because only obstacles local to this cell need to be checked rather than all of the cells in the environment.
Click here for a full-sized image
E-channels and M-channels
E-channels and m-channels are found by using the A* algorithm. If there are any e-channels or m-channels in the environment then the A* algorithm will always return the shortest one. Starting with the cell that the robot starts in, the A* algorithm commences using the adjacencies list that has already been calculated. Just to recap, the A* algorithm is as follows:
1. Let Open be a list initially containing the start node
2. Let Closed be a list, initially empty
3. If Open is empty then return failure
4. Remove the node on Open with minimum f(), let this node be N
5. Add N to the Closed list
6. If N is the goal node, then return success
7. For each child N' of N
a. If N' is on the Open list, then update its f() value
b. If N' is on the Closed list, then do nothing
c. Otherwise, add N' to the Open list
8. Go to step 3
Path
The path through the E-channel is found by looking through the e-channel and connecting the midpoints of shared edges between adjacent cells. This approach provides a shorter and more pleasant path then would be formed by connecting the midpoints of cells.
Limitations
As mentioned before, the planner can only plan accurate paths for worlds which only contain convex obstacles. I thought about the best way for allowing for concave obstacles in the environment for a bit. I initially looked into breaking up any concave obstacles into separate triangular obstacles, and I was hoping that CGAL provided functionality for this. Regrettably, due to CGAL's cryptic documentation, I could not find any such functionality if it exists. I thought about implementing this myself, but given the time constraints, it was not possible.
As I was writing this document I thought of an easy way to allow for concave obstacles in the environment. Just like the C-space obstacles for the world boundary are created, I could iterate through all of the edges for each obstacle and create a C-space obstacle for each. A potential problem with this approach is that the C-Space obstacles will not be solid. As a result of this, in less fine refinements of the quadtree m-channels may be found that go through an obstacle. This problem may not be too severe because it can happen in enivronments with large cell size and obstacles that stradle these cells. The following example illustrates this:
The left image shows the environment intially. The right image shows the results after a few refinements of the quadtree. The reddish cells display the shortest m-channel from the start to the goal. It can be observed that m-channel displayed does go through obstacles. Once an apropriate level of refinement has been reached this should not be a problem.
*UPDATE* 12/5/03
The planner now supports envrionments with concave obstacles using the method described above! It is important to note that the planner now only creates the outline of the C-space obstacles. Each edge of the world obstacle gets converted to a C-space obstacle. This is why you can still see part of the world obstacles. See the below example:
Disclaimer
I tried to make the planner pretty redundant to avoid simple user failure. For example, trying to refine the quadtree when no quadtree exists creates the quadtree. By the same token the function that creates the quadtree makes sure that the C-space is created. However, in order to get the best results out of the planner please do things in a logical order as this was how the planner was meant to be used and how it was tested the majority of the time.
Other Examples
Third-party Libraries
The planner makes use of two third-party libraries: DOLT and CGAL. Dolt is used to display all of the items the environment. This includes such things as: the world boundary, the obstacles, the C-space obstacles, the start and goal positions of the robot, the cells in the quadtree, and the path that the robot should take. All of these items are noticable in the previous two examples. The CGAL library is used for two main purposes. The first is the convex hull function which is used to assist in the creation of C-space objects. The CGAL library is also utilized to identify itersections between cells in the quadtree and c-space obstacles. Specifically, CGAL is used to identify edge intersections, as well as whether or not a specified point lies within a given polygon. More on this later.
Implementation Details
The quadtree is made up of a bunch of objects I called QuadTreeCells. Each QuadTreeCell contains information such as: the depth of the cell, whether or not the cell is a leaf in the quadtree, who the children of this cell are, a list of adjacent cells, the status of the cell, (Unknown, Empty, Mixed, or Full) and a list of C-space obstacles that the cell intersects.
Creating the QuadTree
The first thing that must occur before the quadtree can be created is the generation of the C-space obstacles. The C-space obstacles are generated using the convex hull method with the assistance of the CGAL library. The C-space must be determined before the tree is created or else each cell will not know what it's status is. The status obviously cannot be determined without knowledge of the C-space obstacles. After the C-space is generated the quadtree can be created.
When the tree is first created the initial cell is the same size as the bounding box for the world. This bounding box is kindly provided by dolt. When any cell is created it is passed a list of any C-space obstacles that it may intersect with. In the case of the first cell in the quadtree it is passed all of the C-space obstacles. In the case of this cell's children, they are only passed the cells that the parent cell intersects with. So, when any cell is decomposed, it passes the list of cells that it intersects with to its children. The children then only need to check these cells for possible collisions. In this way, computing time is saved because only obstacles local to this cell need to be checked rather than all of the cells in the environment.
Click here for a full-sized image
QuadTree
Refinement
In the above example, take the cell with
the red dot in it into consideration (I would have outlined it, but
didn't have time to learn all of the intricacies of the Gimp). In
any case, when decomposing this cell, it would be pointless to check
for collisions with any of the other obstacles in the C-space because
they do not occur.
Cell collisions with C-space obstacles occur as follows: For each obstacle in the potential collisions list I check to see if for each edge in this obstacle, there are any intersections with any of the edges of the cell using the CGAL edge intersection functions. If there are any intersections, this obstacle is added to this cell's collision list and we update the cell's status if it is unknown. Otherwise we leave the cell status alone, and we proceed on to check the next obstacle. If this test is inconclusive, the planner then checks to see if the obstacle lies entirely within the cell. I do this using CGAL's point intersection algorithms. If all of the obstacles vertices lie within the cell, then the cell is labeled as mixed if its status is unknown and we proceed to the next potential collision. If the cell is not determined to be mixed, we check to see if the cell is full. This is again done by using CGAL's point intersection algorithms, but this time we check to see if the cell's vertices lie entirely within that of the obstacle if they do, then the cell is labeled as full. If we check all of the potential obstacles that could cause collisions and no collisions occur, then the cell is labeled as empty.
After each refinement of the quadtree, two things automatically happen. The first thing that happens is the recalculation of all of the visible cells in the quadtree. As a result of the decomposition, all of the leaf cells that were formerly of mixed status are no longer leaves. The original list of visible (leaf) cells is erased and recalculated by simply traversing through the tree and adding all of the cells that are leaves. At the same time that the list of visible cells is being recalculated, the planner also determines the cell adjacencies.
Cell Adjacencies
Cell adjacencies are calculated at the same time that the visible cells are being calculated because at this point for two reasons. The first is that it is guarenteed that all of the cells in the quadtree have been refined. We cannot determine the adjacencies for a cell at the time it was refined because the cells around it may not have been refined as well. The second reason is that we are traversing through the tree and finding all of the leaf cells anyway, so there is no reason to traverse through the tree a second time if we don't need to.
It took me a little while to figure out how to determine adjacencies. I decided to look at some suggestions presented to by Professor Huang to his students here. While I didn't really like the ideas that were presented in their current form, they inspired me to do the following:
I already had a function in my code (getCell()) that took a point (x,y), and returned the leaf cell that this node was in. I had used this function so that I could color the start and goal cells of the quadtree appropriately, so that the user could see which cell the planer thought the start and goal cells were in. Starting at the top of the tree, this function simply looks at the point you pass it, and determines if it lies in the upper-left, upper-right, lower-left, or lower-right portion of the cell. It then looks at the appropriate child of this cell and continues until it reaches a leaf cell. This is the cell in the quadtree that the point lies in.
In order to apply this to adjacencies, for each cell in the quadtree that I want to calculate adjacencies for, I first find the midpoint of the cell. I then figure out the size of this cell using the depth of this cell which is known. I will continue this example explaining how I find the neighbors to the North of a given cell, but the method is similar for finding the South, East, and West adjacencies as well.
To find the Northen neighbors of a cell, I take the cells midpoint and add the height of the cell. Ignoring the cells at the very top of the quadtree, which are special cases, this generates a point that lies in the cell to the North of this one. I then use the getCell() function to determine the cell which this point lies in. I then check the depth of this cell. If the depth of the new cell is less than or equal to the depth of the cell I am finding adjacencies for, then this cell is an adjacency and it is added to the adjacencies list. If the depth of the new cell is greater than the depth of the cell we are finding adjacencies for, then there will be more than one adjacency to the North. I then traverse backward through the tree looking at this cell's parents until I find a cell that is of the same depth. I then recursively look at this cells lower children and add any leaves that are present to the adjacencies list. At this point, the Northern adjacencies have been found.
Cell collisions with C-space obstacles occur as follows: For each obstacle in the potential collisions list I check to see if for each edge in this obstacle, there are any intersections with any of the edges of the cell using the CGAL edge intersection functions. If there are any intersections, this obstacle is added to this cell's collision list and we update the cell's status if it is unknown. Otherwise we leave the cell status alone, and we proceed on to check the next obstacle. If this test is inconclusive, the planner then checks to see if the obstacle lies entirely within the cell. I do this using CGAL's point intersection algorithms. If all of the obstacles vertices lie within the cell, then the cell is labeled as mixed if its status is unknown and we proceed to the next potential collision. If the cell is not determined to be mixed, we check to see if the cell is full. This is again done by using CGAL's point intersection algorithms, but this time we check to see if the cell's vertices lie entirely within that of the obstacle if they do, then the cell is labeled as full. If we check all of the potential obstacles that could cause collisions and no collisions occur, then the cell is labeled as empty.
After each refinement of the quadtree, two things automatically happen. The first thing that happens is the recalculation of all of the visible cells in the quadtree. As a result of the decomposition, all of the leaf cells that were formerly of mixed status are no longer leaves. The original list of visible (leaf) cells is erased and recalculated by simply traversing through the tree and adding all of the cells that are leaves. At the same time that the list of visible cells is being recalculated, the planner also determines the cell adjacencies.
Cell Adjacencies
Cell adjacencies are calculated at the same time that the visible cells are being calculated because at this point for two reasons. The first is that it is guarenteed that all of the cells in the quadtree have been refined. We cannot determine the adjacencies for a cell at the time it was refined because the cells around it may not have been refined as well. The second reason is that we are traversing through the tree and finding all of the leaf cells anyway, so there is no reason to traverse through the tree a second time if we don't need to.
It took me a little while to figure out how to determine adjacencies. I decided to look at some suggestions presented to by Professor Huang to his students here. While I didn't really like the ideas that were presented in their current form, they inspired me to do the following:
I already had a function in my code (getCell()) that took a point (x,y), and returned the leaf cell that this node was in. I had used this function so that I could color the start and goal cells of the quadtree appropriately, so that the user could see which cell the planer thought the start and goal cells were in. Starting at the top of the tree, this function simply looks at the point you pass it, and determines if it lies in the upper-left, upper-right, lower-left, or lower-right portion of the cell. It then looks at the appropriate child of this cell and continues until it reaches a leaf cell. This is the cell in the quadtree that the point lies in.
In order to apply this to adjacencies, for each cell in the quadtree that I want to calculate adjacencies for, I first find the midpoint of the cell. I then figure out the size of this cell using the depth of this cell which is known. I will continue this example explaining how I find the neighbors to the North of a given cell, but the method is similar for finding the South, East, and West adjacencies as well.
To find the Northen neighbors of a cell, I take the cells midpoint and add the height of the cell. Ignoring the cells at the very top of the quadtree, which are special cases, this generates a point that lies in the cell to the North of this one. I then use the getCell() function to determine the cell which this point lies in. I then check the depth of this cell. If the depth of the new cell is less than or equal to the depth of the cell I am finding adjacencies for, then this cell is an adjacency and it is added to the adjacencies list. If the depth of the new cell is greater than the depth of the cell we are finding adjacencies for, then there will be more than one adjacency to the North. I then traverse backward through the tree looking at this cell's parents until I find a cell that is of the same depth. I then recursively look at this cells lower children and add any leaves that are present to the adjacencies list. At this point, the Northern adjacencies have been found.
E-channels and M-channels
E-channels and m-channels are found by using the A* algorithm. If there are any e-channels or m-channels in the environment then the A* algorithm will always return the shortest one. Starting with the cell that the robot starts in, the A* algorithm commences using the adjacencies list that has already been calculated. Just to recap, the A* algorithm is as follows:
1. Let Open be a list initially containing the start node
2. Let Closed be a list, initially empty
3. If Open is empty then return failure
4. Remove the node on Open with minimum f(), let this node be N
5. Add N to the Closed list
6. If N is the goal node, then return success
7. For each child N' of N
a. If N' is on the Open list, then update its f() value
b. If N' is on the Closed list, then do nothing
c. Otherwise, add N' to the Open list
8. Go to step 3
Path
The path through the E-channel is found by looking through the e-channel and connecting the midpoints of shared edges between adjacent cells. This approach provides a shorter and more pleasant path then would be formed by connecting the midpoints of cells.
Limitations
As mentioned before, the planner can only plan accurate paths for worlds which only contain convex obstacles. I thought about the best way for allowing for concave obstacles in the environment for a bit. I initially looked into breaking up any concave obstacles into separate triangular obstacles, and I was hoping that CGAL provided functionality for this. Regrettably, due to CGAL's cryptic documentation, I could not find any such functionality if it exists. I thought about implementing this myself, but given the time constraints, it was not possible.
As I was writing this document I thought of an easy way to allow for concave obstacles in the environment. Just like the C-space obstacles for the world boundary are created, I could iterate through all of the edges for each obstacle and create a C-space obstacle for each. A potential problem with this approach is that the C-Space obstacles will not be solid. As a result of this, in less fine refinements of the quadtree m-channels may be found that go through an obstacle. This problem may not be too severe because it can happen in enivronments with large cell size and obstacles that stradle these cells. The following example illustrates this:
 Click here for a full-sized image |
 Click here for a full-sized image |
The left image shows the environment intially. The right image shows the results after a few refinements of the quadtree. The reddish cells display the shortest m-channel from the start to the goal. It can be observed that m-channel displayed does go through obstacles. Once an apropriate level of refinement has been reached this should not be a problem.
*UPDATE* 12/5/03
The planner now supports envrionments with concave obstacles using the method described above! It is important to note that the planner now only creates the outline of the C-space obstacles. Each edge of the world obstacle gets converted to a C-space obstacle. This is why you can still see part of the world obstacles. See the below example:
 Click here for a full-sized image |
 Click here for a full-sized image |
Disclaimer
I tried to make the planner pretty redundant to avoid simple user failure. For example, trying to refine the quadtree when no quadtree exists creates the quadtree. By the same token the function that creates the quadtree makes sure that the C-space is created. However, in order to get the best results out of the planner please do things in a logical order as this was how the planner was meant to be used and how it was tested the majority of the time.
Other Examples
