Rapidly-exploring random trees (RRT) and their variant are a very power solution for solving motion planning problem in robotics, but they suffer from finding an optimise solution and
the generated path is usually jerky with redundant movements. Sample based-optimisation-based planners benefit the robustness of RRT and the possibility of imposing a cost function. Here in this work, I set a cost function which is the combination of length and clearance of the trajectory. To start I define the environment as a black and white image (black obstacles, white collision free area). I obtain a signed distance field from this image in which every pixel show the distance to the closest obstacle (closer darker).
 |
 |
| image 1, environment representation, white free area, black obstacles | image 2, cost map obtained from signed distance field |
After this, I used RRTconnect to find a collision-free path between start and goal, as it can be seen the trajectory an unusual turns.
In the following, I defined a cost function which is the combination of clearance and length but has more weight on the clearance so the planner avoids the obstacle as mush it can while it tries to shorten the trajectory length.
Finally, I changed the cost function such that put more weights on the length of the planner so the planner avoids the obstacle but minimize the length as much as possible trajectory length.
