Motion Planning HW2
Ross Hatton
September 12, 2007
Figure:
Robot has successfully found a path to the goal. Video
|
|
Figure:
Robot has determined that there is no path to the goal. Video
|
|
Matlab files
Two base utilities used in my programs are
- meetpoint.m
- Finds intersections between sets of line segments. I got the base
code for this off of the Mathworks code sharing site, then reworked it
extensively to properly handle floating point math.
- distance.m
- Find the distance between two points, or from on point to a set of points.
The obstacles and robot are both implemented as structures
containing geometric information (in both body and world coordinates)
and special information for plotting. The following support functions
provide a structure for using these objects:
- detect_collisions.m
- Iterates over the (polygonal) world-representations of two objects, and returns the locations of all boundary intersections.
- draw_object.m
- Draw an object to the current axis and delete any previous drawings of that object.
- draw_set.m
- Apply
draw_object.m to each object in a structure array.
- update_vertices.m
- Ensure that the world-coordinate representation of the object reflects its current reference configuration.
Both draw_object.m and update_vertices.m
are ``smart", in that they check the reference configuration of the
object when it was last drawn or updated, respectively. If it has not
changed configuration, then the functions leave it untouched and move
on.
I chose the Bug 1 algorithm as it seemed a good test ground for implementing wall following and related behavior.
The support functions for the bug algorithm are
- can_move_towards_goal.m
- Determines whether there are any obstacles between the robot and
the goal, and the distance to the first obstacle (or to the goal if
there are no obstacles.
- first_obstacle_distance.m
- Finds the distance from the robot to the nearest obstacle, along a line between the robot and an arbitrary point.
- tangen_normal.m
- Find the tangent and normal directions of an object the robot is
interacting with, along with the distance to the object along the
normal line. The tangent is found by detecting intersection points
between the obstacle and a sensing ring outboard of the robot, then
finding the line connecting the intersection points.
The Bug 1 algorithm itself is in bug1.m. The main algorithm (disregarding draw commands, etc) runs as a state machine. The robot states are:
- Move towards goal
- Check if the path towards goal is open. If so, move towards the goal. Otherwise, switch two wall-following.
- Follow wall
- Check if the robot has moved away from where it started
wall-following and has subsequently returned. If so, determine whether
retracing forward or backwards will be more efficient, the switch to
the appropriate mode. Otherwise, find and follow the tangent vector,
while compensating for any discrepancy between the distance to the
obstacle and the target distance.
- Forward trace
- Having returned to the initial entry point and determined that the
closest point to the goal was in the first half of the path around the
obstacle, move through the previously followed path until the closest
point is reached. If the path to goal is open, move in that direction.
Otherwise, declare the robot to be stuck and exit the routine.
- Backtrace
- Same as forward trace, but the closest point was in the second half
of the explored path, so move backwards to reach it more quickly.
The files motion_planning_hw2_success.m and motion_planning_hw2_failure.m
set up the robot and obstacles, such that the robot has a path to the
goal in the first program, but not in the second. The resulting paths
and videos are shown at the top of the page.
