RRT Path Planning

Description 

• Abstract

Trajectory planning is to find the path from one point to another point and avoid the collision during the path. There are various ways to conduct the trajectory planning. In this lab, our team selected Rapidlyexploring Random Trees (RRTs) as algorithm. At first, we developed our program via the method introduced in the class. However, because the limitation of obstacles in the map is two weak, thus it only took one or two steps to reach the goal. Therefore, we polished our code by constrain the growth of each step and erased some weird nodes along the path to make the path smoother. Then, we tested several couples of start and goal positions by both simulation and physical experiment to verify the validity of our program.

• Computing Waypoint in Workspace

In the original algorithm, the elements in the tree is all configuration parameters and use configuration parameters to check for collision. It could be done when workspace and configuration are the same but could not be applied to Lynx. 4 For the maps, obstacles and function to check collision are all represented in workspace, we decide to represent the points in workspace during the computation. After finding the path, we then convert these waypoints to configuration space.

•  Constrain the growth rate

The growth speed in former method is too fast, and due the obstacles in the given map is too weak, there always exists a random point that could connect to the start and goal points at the same time without collision. Therefore, the number of waypoints along the path is very small. To solve this problem, we set a threshold to limit the length of each branch. At each step, we create another point called extend point in the direction from closest point to the random point which is much closer to the closest point than the random point. The diagram is shown in Figure.2. Figure.2 By applying this function, more waypoints could be generated along the path.

•  Find the Path from the Tree

After we finished the loop and connected the two trees, we need to find the path from the points stored in the trees. In previous, we assign the points in the tree with an index, thus we could retrace these points via the index. We realized this objective by doing these: (1) Start from the last point of the tree of which the index is X1; (2) Find the No. X1 point in tree of which the index is X2; (3) Find the No. X2 point in the tree of which the index is X3; … … (4) Repeat the process until reach the end (Start or Goal); (5) Combine the two parts of the path. 

•  Erase Unnecessary Waypoints

It is unavoidable that it will create some retracements or curls along the path because the direction of each growth of the tree is random. To eliminate these problems, we need to write subroutine to erase unnecessary waypoints. We realized this objective by doing these: (1) Set a threshold as the minimum distance between the two points on the path; (2) For each waypoint along the path, compute the distance between this point with other waypoints; (3) If the smallest distance in last step is smaller than the threshold, connect the two points on the both ends of the smallest segment. (4) If the smallest segment dose not collide with the obstacles, erase the points between the two points. The diagram is of this process is shown in Figure.3(a) and an example shown in Figure.3(b) of which the black line is the improved path.