I was at a seminar last Friday on path planning so I decided to implement one of the topics, namely a rapidly exploring random tree or RRT for short. Furthermore, I wrote some code to generate a random map to run the RRT over.
So for fun, let’s run an RRT from the center!
Now for the more practical stuff. RRTs can be used for path planning, so let’s work towards finding a path from one point to another. To do this, we want to expand on both of these points. However this is only fun if we add in some obstacles. To do the obstacle generation, I create a defined number of rectangles to random locations in the array. I allow the rectangles to vary in size. Since I constrained the minimum size of the obstacle and maintain a small epsilon parameter when running RRT, I can make a pretty good first order approximation to speed things up. Currently, I just check if the new point I am placing is within an obstacle. This can be done by just checking the pixel value in this case. There are some spots that barely go onto the edge by cutting the corner slightly. I am choosing to ignore this issue since it doesn’t seem to present much of a problem.
So now let’s add our goal point and see what happens. I set them to be at opposite corners for this example.
I wrote a kth nearest neighbor’s (KNN) algorithm to post process the graph and add edges. After that, I am running a breadth first search on the graph. I plan to implement a simple smoothing algorithm on the path. But I think I will save that for the next post.