Homework #4

A* and 3D Configuration Space

Part I: Star Algorithm to build 3D Configuration Space

I have implemented again the Star Algorithm to compute the 3-dimensional configuration space Q starting from the workspace W and the robot shape R. Since the robot can now translate and rotate, the Q space is  or . The obstacles  in the configuration space are given by the following relationship:

where the configuration  is a vector containing the two coordinates  and  of a reference point on the robot and its rotation angle  about this point.

If we assume that the robot and the obstacles in the workspace are convex polygons, the Star Algorithm computes the obstacles in the configuration space using the normal vectors of each side of the polygons for each possible robot’s rotation angle . I have used the Matlab function norm = c_norm(tang, dir) that returns the normal vector for each side of a polygon. The function is the same used in Homework #3.

The algorithm sorts the sides of the robot and of one obstacle by increasing the angles of normal vectors. Thereafter it builds the new convex polygon adding all the sides of the robot and of the obstacle in the calculated sequence. In order to define the position of the new obstacle in the configuration space for each robot’s rotation angle , I define the vertex with the higher x-value in the non rotated configuration as reference point on the robot. This reference points does not change during the rotation of the robot.

To define the obstacle reference point, I search for the obstacle vertex that the robot’s reference point can touch for a given rotation angle . I have implemented a Matlab function inside = vertex_check(Xr,Yr,Xo,Yo) that performs this check. After finding the correct reference point on the obstacle, I can transform an obstacle of the workspace in the correspondent obstacle in the configuration space using the modified Matlab function [X, Y, num] = q_obstacle(robot_X, robot_Y, robot_num, robot_n, obstacle_X, obstacle_Y, obstacle_num, obstacle_n, R_ref_i, O_ref_i). The new parameter R_ref_i and O_ref_i are the indices of the reference point on the robot and on the obstacle. In order to understand which vertex of the new obstacle is reference point, that means that it must be on the same position of the reference point of the old obstacle, when I insert the sides to build the new polygon I memorize the position of the reference point of the old obstacle and of the robot. For each of the two points, I will have two possible positions on the new obstacle. Then:

·       if the two positions corresponding to the reference point of the old obstacle are the same, the new reference point is this position;

·       elseif the two positions corresponding to the reference point of the robot are the same, the new reference point is this position;

·       elseif the two positions corresponding to the reference point of the old obstacle are different, but one of these is the same of one of the two positions corresponding to the reference point of the robot, the new reference point is this position;

I repeat the whole procedure for each possible robot’s rotation angle . In the following picture a representation of the Q-obstacle.

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 set the end configuration, push the button “End Configuration”, and set with the left mouse button the point. Than insert the value of the final rotation alpha in the text box “Angle”. To calculate the 3D-configuration space and the path, push the button “Calculate Path”.

Part II: A* Algorithm

In order to compute the path within the configuration space, I have implemented the A* algorithm. The problem by a 3-D grid search is the huge computational cost. In fact at each step we have 26 neighbors to take into account. Therefore I have based my implementation on a multiscale pyramid of bitmap arrays.

We define the following functions:

·       star(n): represents the set of node which are adjacent to n; I have implemented a Matlab function star_x = expand(x, i_max, j_max, k_m) that takes into account that the third coordinate is defined over ;

·       c(n₁,n₂): represents the length of the edge connecting n₁ and n₂; I have implemented a Matlab function c = eval_c(x1, x2) that takes into account that the third coordinate is defined over ;

·       g(n): represents the total length of back pointer from n to q_start;

·       h(n): represents the heuristic cost function which returns the estimated cost of shortest path from n to q_goal; I have implemented two Matlab functions. The first H = init_dist(q_goal, H_in, i_max, j_max, k_max) computates a wave front expantion of q_goal. using the classic non-Euclidean distance of A*. However, since the computational cost of the previous method is huge, I have used the Euclidean distance d_euc(X, Y, k_max). Both functions take into account that the third coordinate is defined over .

·       f(n)=g(n)+h(n): is the estimated cost of shortest path from q_start to q_goal via n.

The Algorithm is the follow:

repeat

Pop n_best from O such f(n_best) ≤ f(n),  n  O and add it to C. I have implemented the Matlab function [O, C] = pop(O, C, x);

If n_best = q_goal EXIT

Expand n_best. For all x  star(n_best) that are not in C:

if x  O then

add x to O

else if g(n_best) + c(n_best,x) < g(x) then

update x’s backpointer to point to n_best

end if

until O is empty

The common sampling factor used for the coordinate and the angle is 10. I have also calculated the path in higher resolution levels, but the computation time was up to an hour. Therefore in the movies the path is not a smooth curve. However, it is easy to smooth the calculated path in a post processing operation. In order to register a movie of the path planning, there is a check box “Movie” that has to be checked before pushing the button “Calculate Path”. Finally, in order to get a 3D graph of the first configuration space obstacle, use the check box “3D Graph”

I have implemented this algorithm because I'm really interested on working with a three dimensional configuration space. I think that this is closer to the real task of path planning.

Homework files:

Here are to download all MATLAB files: Astar.zip. Run with Matlab the file GUI.m.

Here are to download the successful planning episode movies: Example1.avi (given end angle 273°), Example2.avi (given end angle 137°), Example3.avi (given end angle 37°), and Example4.avi (given end angle 123°).

Here are to download a small example of planning and a second movie that represents the order in which A* visits nodes: Example part a.avi (given end angle 37°) and Example part b.avi. The blue "o" are the nodes added to the open list at each iteration. The red "x" are the nodes added to the closed list at each iteration. If a node is on a obstacle it is immedately added to delclosed list. You can see that the third coordinate, which corresponds to the angle, is a circle.

Here is to download the movie of the planning episode which has failed Example5.avi. If the A* algorithm does not find a path, it draws just the robot in the end position without any path.

Here is to download a pdf version of the report: Astar 3D.pdf.