Robot Motion Planning—Home Work 4

 

Q) For a three-dimensional SE(2) configuration space, implement the A* search.

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

                                                                                                                              Success—Euclidean Heuristic           

                 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

                                                                                                                              Success—Manhattan Heuristic

 

 

 

 

 

                                                                                                           

 

 

 

 

 

 

 

 

 

 

 

 

 

 

                                                                                                                              Failure

 

 

 

 

Movies: (Note: Right-click and “Save As” if you have a problem directly viewing the movies)

 

Success - Euclidean                                       Success - Manhattan                                                      Failure

 

My Implementation is as follows:

· The 3-dimensional SE(2) configuration space is defined by (x,y,theta). The configuration space is discretized into 20X20X5 cells. It is assumed that the robot can rotate only in place i.e. it cannot rotate and move at the same time.  I have assumed 10-point connectivity i.e. 8-point connectivity on the x-y plane and 2-point connectivity for theta.

· There is an adjacency matrix that is defined for every cell in the configuration space. That gives information of which cell

· I implemented two heuristics here, namely, Euclidean distance i.e. [sqrt((x_goal-x)^2 + (y_goal-y)^2 + (theta_goal-theta)^2)] and Manhattan distance i.e. [abs(x_goal-x) + abs(y_goal-y) + abs(theta_goal-theta)]. A comparison of the two heuristics is given below after the implementation.

· First, Add the start point to the openlist, with g=0.

· Then as long as the open list is not empty and the top element of the openlist is not the goal, pop the top element of the openlist. Check whether the popped element is already in the closed list and if it is, then pop the next element in the openlist.

· If the popped element is not in the closed list, then add the element to the closed list and generate all its neighbors and add them to the open list. If the neighbor is already in the closed list then do not add it to the open list. If the neighbor is already in the open list, then check for the ’f’ value and update the element with the lowest ’f’ value. Set the popped element whose neighbors are found as the parent of its neighbors that are added to the open list. Set the ‘g’ value equal to the ‘g’ value of its parent added to the cost to the parent. Determine the heuristic value of the element ‘h’ and finally calculate f = g+h. After the neighbors are added, sort the openlist based on ‘f’ value with the lowest ’f’ value on top. I implemented insertion sort here.

· The implementation repeats the process until it pops the goal point from the open list.

· If there is no element to pop in the open list, then it reports “NO PATH FOUND”.

· Here, in the movies and the figures, the red line represents the path taken while the yellow squares represent the nodes expanded (or nodes visited). I define nodes visited/ expanded as those which were popped out of the open list and whose neighbors were found.

· From the success figures, you can see that Manhattan distance heuristic does a better job than Euclidean distance as it expands lesser nodes and finds the path quicker.

· The failure figure shows that the implementation expands all the accessible elements to conclude that there is “NO PATH FOUND”.