Homework #6

Generalized Voronoi Diagram

For a two-dimensional configuration space I have to implement the Voronoi diagram using incremental methods. In order to draw a two-dimensional configuration space I use a robot with an arbitrary shape that moves in a 2-D workspace with a fixed orientation. Therefore, the robot’s degrees of freedom are two and the configuration space is two-dimensional. Further implementation could taking into account also the rotation of the robot building the Voronoi diagram for a three-dimensional configuration space just connecting the Voronoi diagrams calculated for each layer.

At first, my algorithm gets the 2-dimensional configuration space Q starting from the workspace W and the robot shape R. Then it computates the Generalized Voronoi Diagram of the free configuration space Q_free.

Let us call the generalized Voronoi region as the closure of the set of points closest to any obstacle QO_i :

where  is the distance to an obstacle QO_i from q.

In order to calculate the distance, I have used at first the saturated distance function:

where the parameter R is chosen bigger than the average diameter of the configuration space. I have implemented a Matlab function rho = vor_d(Q, q, step, x_max, y_max, j_max, i_max, grid, R, R_inf) in order to computate the saturated distance. The parameter Q is the bitmap of the configuration space, the parameter q is the given configuration and the parameter R is the saturation limit.

Then I have computated for each obstacle to which the configuration q has line-of-sight the minimum of the saturated distance function:

In order to find the minima, I have implemented the Matlab function [O, m, rho_min_vec] = vor_cont(rho, R_inf). The matrix rho_min_vec contains for each obstacle the distance d_i and the angle .

Using the above functions I can define for each couple of obstacles a two-equidistant line in the configuration space:

In order to refine the two-equidistant line, eliminating the portions with non-distinct gradient vectors we can define a two-equidistant surjective line:

Moreover, in order to avoid that this line intersects other obstacles, I restrict  to the set of points that are equidistant to the two obstacles QO_i  and QO_j  and have them as the closest obstacles:

Since the function vor_cont returns the distance to all the obstacles within line-of-sight of the configuration q, I can easily computate  and . However, since the real bitmap is a discrete word in my implementation I have used the parameter d_thd as threshold to define when two distances are numerical “equal” or “different”.

Finally I can define the generalized Voronoi diagram (GVD):

Each edge terminates either at a meet point, which is a point equidistant to three or more obstacles, or at a boundary point, the distance of which to the closest obstacle is zero (or less than the threshold d_thd). In the picture below you can see that the implemented algorithm works also with concave obstacles defined in the configuration space adding more boundary points.

In order to incrementally build the GVD, the algorithm at first moves the robot on the edge  closest to the start configuration. Then it moves the robot along the edges computing their tangent space: it passes a line through the two closest points on the two closest obstacles and takes the line perpendicular to it. Then it moves the robot along this direction by an incremental step. If the new position is no more in the equidistance interval given by the parameter d_thd a small perpendicular motion is made in order to put the robot on the edge  again. In the algorithm the choice of the parameter step, which defines each step of the robot, and the parameter d_thd, which defines the "real zero" (if the difference between two distances is less of this value, we assume the two distances to be equal), is very important. In fact after several tests I have seen that for small steps and small values of d_thd the algorithm computates very smooth curves, but the number of the needed steps is very high. On the other hand, if the steps of the robot are longer, the algorithm computates the GVD very quickly, but the final result is coarser. Moreover, the two parameters are correlated to each other because if the zero is small then the robot is more likely to exit from the equidistance zone when the step is big.

Below I explain the flow of my implementation. I have used two different lists: the list vor_meet to memorize all the meet points and the list vor_bound to store all the boundary points. Then we have three different cases:

·       when the robot arrives in a meet point for the first time the algorithm adds this point to the list vor_meet;

·       when the robot arrives in a boundary point, the algorithm takes the first element in the list vor_meet that is the start point of some edges not yet explored and explores one of this edges;

·       when the robot arrives in a meet point that is already in the list vor_meet, if this meet point is connected to some edges not yet explored, the robot explores one of this edges. Otherwise the algorithm cancels this meet point from the list vor_meet, takes the first element in the list that is the start point of some edges not yet explored and explores one of this edges

The algorithm terminates when the list vor_meet is empty. In order to use the computated graph to plan a path between a start configuration and a goal configuration, the goal configuration has to be connected to the graph. After defining a lenthg for each edge of the graph, the A* algorithm can be used to find the shortest path within the GVD between the connection points of the start and the end configuration to the graph.

When the program starts, push the button “Draw Obstacle” to draw an obstacle and set with the left mouse button the vertices of the obstacles within the white area (robot workspace W). To end the process click with the right mouse button somewhere. To draw the robot push the button “Draw Robot” and set with the left mouse button the vertices of the robot within the white area. To end the process, click somewhere with the right mouse button. A black point will be placed on the reference vertex of the robot. To calculate the configuration space, push the button “Compute Q”. To calculate the GVD push the button “Calculate Path”. In order to show an animation and to register a movie of the GVD building process, there is a check box “Movie” that has to be checked before pushing the button “Calculate Path”.

Homework files:

Click here to download all MATLAB files: GVD.zip.

Click here to download some movies. In the movies I started to draw the GVD from the first meet point reached by the robot: Example1.avi, Example2.avi, Example3.avi, and Example4.avi.

Click here to download a pdf version of the report: GVD.pdf.