|
Homework #2 Tangent
Bug I
have implemented the Tangent Bug in this homework assignment. I have chosen
this algorithm because it has given me the possibility to think how a real
robot works. The discrete distance value I have calculated in the reality is
the input of a distance sensor: this is the only difference. Maybe, I can test
my implementation in the future on a real robot. I have used the Matlab GUI to provide a
graphic interface to draw obstacles, to set start and end point, and to
calculate the path.
With
a first popup menu one can choose to draw as obstacles polygons or cubic spline
closed curves. To draw an obstacle one pushes the button “Draw obstacle” and
sets with the left mouse button the vertices (polygons) or the base points (for
splines) of the obstacles within the white area (robot workspace W). To
end the process, one can click with the right mouse button somewhere. To draw
path start and end points one pushes the button “Set start and end”, and sets
with the left mouse button the two points within the robot workspace W.
One can now choose the type of the path planning algorithm to use with a second
popup menu. Finally, one pushes the button “Calculate” to draw the path. The Tangent Bug algorithm is based on the
idea to use a robot equipped with a distance sensor. If the robot is in a
particular position in its free workspace Wfree the sensor gives
information about the distance between the robot and the obstacles WOi in each discrete direction.
Formally, we can represent this function as a saturated distance function:
Obviously,
infinity does not exist in real computers and therefore I will use a high
number. The name of this parameter is R_inf. The name of the
parameter which defines the sensor radius is R. I
have implemented a Matlab function rho = sensor(Workspace, X, step,
x_max, y_max, j_max, i_max, R, R_inf) which simulates the sensor.
The parameter step is the dimension in the workspace of each
step that the robot does during its motion. The
robot starts moving toward to the goal direction qgoal. If the sensor feels
the presence of obstacles, indeed the distance function has some values less
than R, some intervals of
continuity are defined on the boundary of the sensed objects. I have
implemented a Matlab function [Oi, dim_O] = int_cont(rho, step, X, X_goal,
R_inf)
which calculated the intervals of continuity at a given point. In this function
a very important parameter is continuity_jump that defines when
the discrete distance function has a discontinuity. In this case there is a
change between two different intervals of continuity. If the direction between the robot and the
goal intersects an interval of continuity, the robot will now move in the
direction of the end point Oi of some interval of
continuity which minimizes the following heuristic function:
When
the direction between robot and the goal does not intersect any interval of
continuity the robot will start again to move in the goal direction. In order to prevent that the algorithm fails
in case of local minima of the heuristic function, if the heuristic begins to
increase a boundary following planning is used. At the beginning the algorithm
moves the robot in the same direction as the most recent motion step. The
boundary following motion ends when the following relationship is fulfilled:
and
the robot will start again to move in the goal direction. In the above equation
dreach is
the shortest distance between the goal and the blocking obstacle (or the
distance between the robot and the goal if there are no blocking obstacles) and
dfollowed is
the shortest distance between the boundary which had been sensed and the goal.
I have implemented a Matlab function [d _reach, newdirection_angle]
= d_check(Oi, rho, X, X_goal, pre_direction_angle) which
calculate the distance dreach and the new motion
direction. If the robot completes a cycle around the obstacle the goal cannot
be reached. When the robot follows a boundary, the
implemented algorithm moves the robot along the orthogonal direction to the
boundary normal vector:
However, if the robot goes too near to the
boundary the implemented algorithm moves the robot for one step along the
boundary normal direction. The parameter that defines the safety distance
between robot and obstacles is offset*step. Homework
files:
Here are to download all MATLAB files:
HW2.fig,
HW2.m,
tangent.m,
sensor.m,
int_cont.m, and
d_check.m.
|
