Free-space polygon implementation for obstacle avoidance in path planning.

In a previous publication I went through the power and novelty of Nonlinear Model Predictive Control (NMPC) and while it can accurately create a trajectory to move Turtlebot3 from point A to point B there is no warranty that it will be done in a safely manner 😕. Safe means that if an obstacle is encountered in the path, Turtlebot3 should avoid it, have no collisions and thereby keeping the robot healthy for future experimentation!

In this post I will be focusing on one way to have an understanding of the free space around the robot and use that information to derive constraints that can be used in the trajectory optimization part to go around obstacles. Specifically I will be replicating the approach in section 3 of this paper.

Free-space polygons

In the context of avoiding obstacles that might be in the way of Turtlebot3 free-space refers to the area in a map, aka occupancy grid, that has low probability of being occupied. A very common way to interpret a map is to create a white space in area where the ranging sensors are not detecting any objects and black areas where something is present. The image in Fig. 1 can serve as an example.

Fig. 1 Map representation of a corner in a building.

Free space then could be any white region whereas black areas represent obstacles and the gray parts are unknown. Let us now consider a robot at point P in the map a possible free-space polygon could be represented as follows:

Fig. 2 Free polygon are around point P.

In the following I will focus on the generation on the green polygon and the red collision detection points.

Approach

The implementation or algorithm used is very clever and proposed by Meerpohl et al. The main idea is to use ray-casting techniques by estimating the points in a line going out of the current robot’s position in different directions. Additionally, for each line directions a half circle is moved through the line to obtain a smoother polygon. Here is the pseudo code:

Free-space polygon creation

The code I wrote to test it can be found here! But as usual visualizations are cooler 😁!

The blue line represents the direction of the ray. The green half-circle is moved along the line and when a collision is found the search stops.

Conclusion

Now that there is a way to better understand the world around Turtlebot3 we could use the collision points as constraints when solving the optimization step in NMPC. In the next article I will be focusing on that and specifically testing it on the actual robot.