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

Support Architecture

Two base utilities used in my programs are

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.
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:

Iterates over the (polygonal) world-representations of two objects, and returns the locations of all boundary intersections.
Draw an object to the current axis and delete any previous drawings of that object.
Apply draw_object.m to each object in a structure array.
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.

Bug1 algorithm

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

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.
Finds the distance from the robot to the nearest obstacle, along a line between the robot and an arbitrary point.
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.
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.