Writing the planning algorithm for following the unpredictable evader was really quite hard. In general, I found the first part of the assignment to be by far the most difficult, because it required taking the essentially analog information about the environment polygon and putting it into a format usable by a graph-search algorithm. On the other hand, making the state-space search even reasonably efficient was also quite hard. I found that with my code, it often took more than ten minutes to solve a problem I thought looked pretty easy. This occurs because of the fact that the state space has dimension which is exponential on the number of internal sub-polygons in the environment. On the whole, I learned that it's a lot easier to talk about motion planning algorithms than it is to implement them in practice. Intricacies such as how to represent vertices, segments and polygons can be glossed over when discussing the subject analytically. But when you're writing the code, it all must be taken into account.
Implementation
I am going to describe the implementation in four sections, corresponding to the four segments of the assignment: generating a roadmap based on constant edge visibility, generating a visibility polygon, searching the state-space graph for solutions, and implementing a solver for multiple robots. For the first part of the solution, I basically followed the IEEE article that Prof. LaValle handed out in class. For the remaining parts, I diverged somewhat to try out a technique I believe is pretty much equivalent. Essentially, instead of keeping track of the gap edges' values in a state space, I keep track of which internal polygons have been cleaned. Once all have been cleaned, the algorithm is done.
At first, this seems very different from the original algorithm, but I think it achieves the same results (albeit perhaps a bit slower). This is because a side-effect of generating the subpolygons based on constant edge visibility is the fact that, if the robot is in one subpolygon and it can see all of another subpolygon, then from anywhere it goes in that same subpolygon it will still be able to see all of the other subpolygon. So, you can take advantage of this fact and make a planner which does pretty much the same thing as the gap-edge planner. I believe the method I used was tougher to implement, but I also believe I learned a great deal implementing it. As far as I could tell through pretty extensive testing, my algorithm generated a complete result.
Part I
This was the toughest part of the assignment. My implementation takes in a set of vertices which describe the environment polygon. Then, it extends all pairs of vertices which can see each other or are members of the same segment. Extending these pairs of vertices consists of drawing a segment that extends an imaginary (or actual) segment between the pair until that extension hits an edge of the environment. One important thing to consider here is that the extensions will cross one another. This must be considered when building the internal polygons.
Once all the segments have been computed, the algorithm splits the
environment polygon into sub-polygons on each of the segments.
This is done iteratively for speed (though it's easier to write
recursively, it takes forever to run). Iteratively, it runs in
O(n
Once the roadmap is generated, the planner creates a shortest path from each robot starting point to each polygon in the environment. This is done because when you do multiple robots, you must know how to get each robot to the appropriate location in the environment. Finally, each polygon computes its centroid so that the robots know which vertices to follow along the roadmap.
Part II
Computing the visibility polygon was fairly easy. I chose not to use a radial sweep because the information that I wanted was actually just the set of internal segments which can be seen from each polygon, and the set of other polygons which can be completely seen from each polygon.
So, in order to implement this, I simply took each polygon and then computed whether a segment which is drawn between the polygon's centroid and the segment I want to compute visibility for intersects the environment polygon anywhere. If it does, the segment is not visible. If it does not, the segment is visible. Computing polygon visibility can be done by building upon this base.
Part III
The tricky parts about searching the state space are making the algorithm efficient and figuring out when and how to alter the state space. The basic idea is that you maintain a bitvector of values for each internal polygon in the environment. If the evader could be in a polygon, then the bitvector is set to 1, else it is set to zero. Here is an example of a state space computed when I executed the algorithm on an environment.
The algorithm I used to figure out how to alter the state space is fairly simple and works well. If a polygon is visible from another polygon, then that polygon is marked clean. If it is not completely visible, then if one of its edges which cannot be seen completely borders on another polygon which is marked 1, then that polygon gets marked 1. This corresponds to the evader moving from one polygon to the next.
Solving the problem requires dynamic programming. This amounts to a recursive routine which, starting at the robot's starting polygon, traverses the polygons, changing the state-space as it goes. When a state-space that has already been seen is hit, the algorithm does not go down that path. An optimization I did was to make the algorithm follow paths with more zeroes first. Though this will not always guarantee a quick solution (due to local minima), it does help substantially. The base case of the recursion is when all of the polyons have value zero. At this point, the path taken to get to this point is read out, and this is the solution to the problem.
Pros and Cons: The algorithm I used converged quickly, and I did not find an example which could be solved which it couldn't solve. However, I suspect that the gap edge algorithm described in the IEEE paper is faster than this one as the state spaces are generally smaller (with one bit per polygon, you can get 2^n state spaces here!). But, by following the path which leads to better state spaces each time, I found that solutions came really quite quickly -- on the order of 1 to 10 seconds, after the environment was created. One definite con to the approach I took is that it can cause backtracking -- especially in an environment that's not solvable. In this case, it can take the computer a long time to come back and report failure, since it's trying millions of different state possibilities.
Part IV
The kind of multiple-robot planning required for this assignment was a fairly straightforward extension of the algorithm generated in Part III. Essentially, you just attempt to solve the problem again and again, with only one robot actually moving. The other robot simply moves to one polygon and stands guard there. The way this is modelled in the state-space is that any polygons which can be seen by the guard robot cannot switch from zero to one, since the robot standing guard will see the evader pass through the polygon.
The way the planner picks where the robot should stand is exactly the way it says in the assignment. The planner attempts to path-plan with one robot. If this fails, it remembers where it had had the best state. Best state is defined as "most polygons cleared" for this planner. Then, it places a stationary robot there to simulate the first robot going to that point and stopping. It can do this for any number of robots, though I only got good results for two robots. (with more, I think the mazes you have to make become overly difficult -- one that I made took so long for it to solve that I just gave up!)
Errata
Ideas For Improvements
Conclusion
This was definitely one of the most challenging projects I've worked on at Stanford. In doing it, I have come to understand the true degree of complexity inherent in solving special cases of the general path planning problem.
Below are three examples of solutions to the pursuer-evader problem. The robot seems to execute random motions at some points in the solutions. This is a result of the fact that the planner must divide the environment into many small subpolygons based on constant edge visibility. In addition, it takes the first solution it finds. Often, this solution is not completely optimal, for the obvious reason that the planner is satisfied with finding a solution -- it need not find an optimal solution.
These examples are all fairly simple. This is because the output of my program was just a set of vertices for the robot to traverse, so I essentially made the movie files by hand. I think, however, that they demonstrate that the algorithm is working and finding a reasonable solution and then stopping when the solution is found.