Motion Planning HW #5

Due : Oct/17/2007

Problem

Implement visibility graph for a two-dimensional polygonal (or three-dimensional polyhedral) robot and work space (Remember to construct the configuration space)

On your web page, show at least two images - one that illustrates a successful planning episode, and one that fails due to the start and goal being in different connected portions of the free space. Include on your web page a brief description (pdf file or html ONLY) of your approach. This report should include all relevant equations and algorithm descriptions, along with a description of any parameters used by your algorithm.

Solution

I implemented the visibility graph and A* algorithm to find the path using GUI of MATLAB.

Workspace Description : Workspace has 2-Dimension(no orientation) which is the same as before (bug, potential etc.

Configuration space : Since I am not concerning orientation, the configuration space in 2D can be built by Star algorithm using Minkowski sum. Minkowski sum of two sets A and B in Euclidean space is the result of element of A to every element of B, i.e.

From this summation, boundaries of obstacles in workspace are expanded as large as summation of boundaries in workspace and robot boundary. For below example, the obstacle boundary is changed by robot boundary. This depends on the robot boundary and the reference point.

So Minkowski sum provides configuration space robot will actually move.

- Algorithm details

- Minkowski's sum

The vector normal to edge obtained after setting obstacles and the algorithm computes the angles between horizontal line using atan2 function embeded in MATLAB. Then edgeSet(set of edge of obstacles) and rEdgeSet(set of edge of robot) are constructed increasing order with respect to angle. In my algorithm, obstacle structure has its id, vertices, edge etc, and the vetices are set to counter clock wise order even though user draws it differently. edgeSet and rEdgeSet also are structure and have information of left vertex and right vertex and angle. Using this information, firstly find Minkowski sum of robot vertex in coordinate system of reference frame and obstacle vertex in workspace coordinate system. The reference frame is set to actual start position. For example, when e2 is concerned, algorithm finds the intersection vertex between e2 and e1 and finds intersection vertex between r2 and r1. Now two vectors are found and its addition produces one of vertices of configuration space. With the same way, every edge can be computed and finally configuration boundary is able to be obtained.

- Vertex setting

In order to build visibility graph, it is necessary to have the VertexSet (set of available vertex) which contains points we have to concern when doing rotational plane sweep algorithm. The VertexSet is also structure which contains information of vertex coordinate, the edge crossing and id of configuration obstacle. Also start position is set to first and goal position to last. In my convention the vertex which is out of bound is not concerned so it does not affect to visibility graph.

- Visibility Graph

Definition of visibiltiy graph is that its nodes share an edge if they are within line of sight of each other and that all points in the robot's free space are within line of sight of at least one noed on the visibility map[1]. Visibility graph is constructed by Rotational plane sweep algorithm which adopts idea of beacon. A rotating beam of light emanating from a lighthouse beacon searches from 0 degree to 360 degree for one cycle. Constructing visibility graph is the same. From 0 degree, the line emanating from a certain node visits every vertices and find which is visible.

• FOR each vertex v(i), calculate alpha(i)
• NEW vertexSet = set of alpha(i) which are increasing order
• NEW activeSet = set of edge which intersect the horizontal half line emanating from v
• FOR all alpha(i)
  • IF v(i) is visible v
    • THEN visibleSet.PUSH(edge(v,v(i))
  • ENDIF
  • IF v(i) is the beginning of an edge, vertexSet, not in activeSet
    • THEN activeSet.PUSH(vertexSet)
  • ENDIF
  • IF v(i) is the end of an edge in activeSet
    • THEN activeSet.DELETE(edge(v(i), v))
  • ENDIF
• ENDFOR

- A* algorithm

Once the visibility graph is built, then robot starts to find the path among visibility graph. Since the visibility map posses the properties of accessibility and departability by definition, if it is connected, then several kinds of searching algrithm can be implemented such as potential function and various star algorithm. Among these, I chose A* which is known as optimal. Cost between nodes is set to distance of nodes and heuristic cost function is set to estimated cost of shortest path from node to goal. This heuristic function is computed by recursive way from goal to start like D* algorithm. Actually when I implemented this, algorithm is quite similar to ProcessState algorithm in D*.

- Demo

The configuration space is confined to current axis which means if the vertices out of bound are assumed are not connected. Thus, when the start/goal position is isolated by obstacles and axis, then the robot is not able to find path so returns mission failure. And Minkowski sum works very well if the obstacles do not have any concave part. In this case, the star algorithm cannot apply because star algorithm follows the order of angle of each edge but if we have concave polygon then the order might change. However, in this demo concave configuration of obstacle can be made using two obstacles.

VISIBILITY GRAPH SUCCESS 1

VISIBILITY GRAPH SUCCESS 2 (HIGH ASPECT RATIO ROBOT)

VISIBILITY GRAPH SUCCESS 3 (CONCAVE)

VISIBILITY GRAPH FAILURE

- Future works

In visibility graph, even though I used rotational plane sweep algorithm, if the number of the vertices become large, the combinations of vertices goes rapidly up. In order to reduce the combination of vertices, in text book, it erases except for supporting lines and seperating lines. This might reduce the number of computation highly so would be more efficient. So in the future, recognizing the supporting/seperating algorithm should be implemented.

- Reference

[1] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki and S. Thrun, Principles of Robot Motion: Theory, Algorithms, and Implementations,
MIT Press, Boston, 2005.