Motion Planning HW #6
Due : Nov/05/2007
Problem
For a two-dimensional configuration space, implement the Voronoi diagram using incremental methods.
You may want to consider breaking your code down into separate modules
On your web page, show at least two images in different environments.
Solution
I implemented the GVD 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.)
GVD is the set of points where the distance to the two obstacles is the same. The purpose of drawing GVD is that robot finds the path in road of GVD. Since the GVD has the properties of accessibility, connectivity and departibility, once the GVD is constructed, the robot is able to find path easily. This is exactly the same concept with the way of finding path from one city to the other city. Firstly, one may find closest junction of highway - Accessibility. And one may change highway if neccessary - Connectivity. Then finally, one gets off closest junction of the other city from highway - Departibility. If there is path from start point to goal, then GVD guarantees these three properties so drawing GVD is quite indispensable process.
Accessibility : The robot moves from start point in negated gradient direction of tangential space of closest obstacle which means it moves away from closest obstacle until it encounters equidistant point with the other obstacle.
Connectivity : After getting to the equidistant point, the robot moves in set of equidistant point which is GVD. When it meets the other obstacle so current point is tri-equidistant point, then it chooses one of way and follows again along equidistant point with two obstacle. If it finds there is no path (when it faces with two walls), then it goes back to tri-equidistant point and moves the other direction. As repeating this process, it is able to draw all accessible points and get connectivities.
Departibility : Finally it finds closest point with goal position along roadmap, GVD.
- Algorithm details
- Equidistant point
Since the robot is not able to expect the direction of next equidistant point, the velocity vector cannot be obtained easily. In order to find the next equidistant point, the robot eminates ray tangent to the direction where it moves and then it makes the direction converge to the equidistant.
The robot has some threshold range which blocks it to move forward. For example, when robot toward between two walls, it goes back to tri-equidistant point again when the distance between obstacles and it is closer than threshold range in order to prevent it rushes into walls. This means when robot goes through two obstacles, if the distance between two obstacles is not quite enough to get through it, then it goes back to tri-equidistant point which causes failure of connectivity.
- Data structure and main routine
When drawing GVD, the robot constructs Node data structure. Definition of node is tri-equidistant point. Node data contains its coordinate and three obstacle which has the same distance. Also it has neighbor node index and the path to the neighbor. These data are really important when finding path using GVD. Every node should be visited at least twice because the node connects three lines. Thus GVD finishes when all nodes are visited more than twice and A* algorithm starts.
Whenever the robot moves, the distance beween the robot and goal position is recorded and it is used in A* algorithm in order to compute heuristic function. Since the path data are in node structure, path recreation is not taking a lot of time. For example, after constructing GVD, even though we change the start position and goal position, the computation time is even faster than before because we have roadmap. This is big advantage of this algorithm.
Actaully, it takes lots of time when doing GVD because whenever robot move one step the direction is recomputed and make it converges within some ranges. As you see below figure, the GVD is drawn with messy red cross which means the direction has change a little bit but almost every time.
<fig. 1>
In order to draw GVD, my algorithm has some conventions which prevent to miss nodes. Firstly, when the robot in tri-equidistant point, I put more priorty at wall so it follows wall first. For instance, when the robot is in Node3 from Node2, it has to determine which direction it should go. If it selects move toward Node4, then it should back sometime later and has to travel to bottom right vertex. This might takes more time so I set it move in direction of wall first. So it moves to bottom right vertex first. In case of Node2, it also moves toward Node3 because this path has wall. Using this order, the ourskirt of GVD is constructed first and then it gets through inside of GVD.
If one node is visited more than twice, then robot gets back to the tri-equidistant point. When drawing inside GVD, the robot searches lastest node first. For example, when the robot is located Node1 again after drawing outskirt of GVD, it searches node which is visited less than twice in lastest order so it moves toward Node7 and starts to draw new GVD inside. When it arrives at Node8, it moves any direction toward Node2 or Node5 since Node8 is visited first. After visiting Node2, it gets back to Node8 and searches whether other new direction exists. As from Node8 to Node5 road is not searched yet, it moves toward Node5. After arrived at Node8, all nodes are visited more than twice, the robot returns it completes the roadmap.
The numbered edge in fig.1 stands for the order of drawing GVD.
- Trade off (Pricision & Speed)
As I mention before, it takes really long time to draw all GVD. One of way to improve the speed of computation is increasing robot velocity but this makes increasing detecting range(low precision).
- A* algorithm
As same as visibility graph search, I implemented the A* algorithm in order to prove the departibility.
- Demo
Precedure : This demo shows how the robot constructs the GVD. Red cross means current position and blue circle means the closest points from current point in the boundary of obstacles. Since the demo takes long time, I edited the movie compact.
Result : This demo shows the summary of precedure of GVD and path planning in road map using A* algorithm
- Future works
3-D GVD extends the idea of 2-D. However, even if GVD is drawn using 3 equidistant point from each surface, there may have unreachable region depending on aspect ratio of workspace. In this case, new search method which searches the equidistant surface is neccessary. This maybe really complicated and conventions of the order of drawing GVD is more chosen carefully. It might be also to set priority at wall but it might have more conditions.
- 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.