Robot Motion Planning HW1
Juan Pablo Gonzalez
Implementation of the Tangent Bug algorithm in a Robotic Platform
1 Robotic Platform
The robot team (shown in Figure 1) consists of a homogenous set of off-the-shelf mobile robot platforms outfitted with additional sensing and computing. Serving as the mobility platform is an ActivMedia Pioneer II DX indoor robot. A Mobile Pentium 266 with MMX is the main processor. Attached is a 1-gigabyte hard drive for program and data storage and 802.11b wireless card for ad-hoc communication between robots. Encoder data from the drive wheels is collected onboard from which dead reckoning position (x, y, θ) is calculated. Encoders provide a relatively accurate measure of linear travel, but relatively inaccurate angle measurement, such that small errors in angle compound over time resulting in large displacement errors. A solution to these pose errors is the addition of alternate angle measurements using a fiber optic rate gyroscope (KVH E-Core 1000). The gyroscope provides highly stable and accurate angle measurement (four degrees drift per hour). Robots sense their environment using an 180˚ scanning laser range finder (SICK LMS 200). Horizontal scan-range-data is incorporated with position data to create a 2D map. In addition to providing information to the operator, the map is used for local navigation and cost estimation during trading.
Figure 1. Robot team
1.1 Architecture
Figure 2. Architectural Layout
Design and implementation of the system supporting the architecture was focused on extensibility and scalability. The system can be conceptualized as a 4-tier structure (as illustrated in Figure 2): hardware, hardware abstraction, autonomous navigation, and multi-robot (inter-robot) communication. The hardware layer consists of the motors, encoders, laser sensor and gyroscope. A process called RobotCntl, which serves as a hardware abstraction to higher-level processes, controls all components of the hardware layer. RobotCntl manages the state of the hardware, collects, timestamps, and provides access to data, and interprets and executes hardware control commands from higher-level processes.
Two separate processes, TaskExec and DataServer, in conjunction with the hardware abstraction layer accomplish autonomous navigation. TaskExec executes local navigation with a map provided from DataServer. DataServer aggregates position and laser range data from the hardware abstraction level and provides maps to other processes that require map information.
The algorithm implemented for Tangent bug is a task within the TaskExec module, and replaces the existing D*-based navigator in the robots.
2 Implementation
2.1 Description
The algorithm implemented is a variation of tangent bug, with the following characteristics:
In order to take full advantage of the precise odometry in the robot, and compensate for the limited field of view of the laser range finder, the algorithm collects laser measurements and puts them together in a map. We then select a circle in the map as a simulated 360o sensor and proceed with the usual tangent bug implementation.
Tangent bug provides intermediate goals for the robot based on the local tangent graph (LTG). In order to reach an intermediate goal, the robot has to follow an arc with certain speed and curvature. The arc with an endpoint that is closest to the intermediate goal is chosen as the desired trajectory for the robot. This allows us to partially account for the non-holonomic constraints of the robot. Figure 3 shows the set of arcs considered for the robot’s motion.
Figure 3. Arcs considered for robot motion
Since the robot is not a point robot, C-space obstacle expansion is performed in order to prevent the robot from hitting obstacles.
The laser range finder in the robot is configured to output ranges of up to 5 meters. The virtual sensor used for tangent bug can be configured to use any sensor range from 0 to 5 meters. However, because of the inertia of the robot and the lack of contact sensors, ranges of less than 2 meters are not practical.
Figure 4 shows the algorithm running in the Pioneer robots. Light green denotes obstacles. The next shade of green denotes C-space expanded obstacles. Dark green is free space, and black is unknown. The blue circle is the sensing range, and the blue line coming out of the robot is the chosen arc to take the robot to the intermediate goal selected by tangent bug.
Figure 4. Tangent Bug showing sensing range and C-space expansion
2.2 Implementation Issues
Even though the theoretical understanding of tangent bug is relatively simple, the implementation is quite complex. Some of the problems encountered in the implementation were:
- Finding an appropriate reference system for the angle measurements. It is very important to have a coordinate system (angle reference) that makes the task of following obstacles easier, not more difficult. The first two choices were to have a fixed angle reference (i.e.: north), and to have the line from the robot to the goal (angle_to_goal) as the reference. We found out that neither one of these reference systems allowed easily for continuous and consistent tracking of the obstacle boundaries in the LTG. The reference system that we used in the final implementation was to create an LTG with its origin (0o orientation) aligned with the robot heading. Since most of the time we were following a wall that was either on the left or the right side of the robot, this approach proved to be the most convenient one.
- Dealing with quantization errors and noisy measurements. Even though the sensor used is very precise and clean, there is always noise and measurement errors. Additionally, since we are using a grid-based map as the source for our virtual sensor readings, there are quantization errors introduced. When comparing threshold measurements in tangent bug (for example, whether dreach < dfollow) we must take into consideration all these errors. The way we perform such comparisons is by adding an uncertainty term, that helps prevent repeated switching from one mode to the other. In this way, what we compare is, for example, whether dreach + e <dfollow. Likewise, it is very difficult to reliably detect discontinuities in the range data, since many (or most) of them are caused by quantization either in space or in angle.
- Blocked goal. Tangent bug calls for circling an obstacle before determining whether the goal is reachable. Due to lack of time, this behavior was not implemented, and tangent bug will continuously try to reach the goal until aborted.
3 Results
The algorithm implemented performed very well in most situations. It was able to drive the Pioneer robots at speeds up to 0.5 m/s
The following pictures / videos show tangent bug running in a simulated world, with different sensing ranges.
Figure 5. Tangent bug running in simulated world with sensing ranges of 2.0 m, 3.5 m and 5.0 m.
(Click on the images for a video) (Download required codec here)
The following pictures / videos show tangent bug running in the Pioneer robots, in the high bay at FRC.
Figure 6. Tangent bug running in Pioneer robots in the high bay at FRC, with sensing ranges of 2.0 m, 3.5 m and 5.0 m.
(Click on the images for a video) (Download required codec here)
The algorithm is able to handle moderately crowded spaces, but starts having problems in very tight areas, when the robot is surrounded by obstacles. Likewise, if the goal is blocked, the algorithm performs erratically.
1 Code
Below is the code that implements the tangent bug algorithm. Please note that this code requires the rest of the software architecture in order to run. The runOnce() method of bugTask is called in each execution cycle until termination.
/****************************************************************************
* BugTask.cpp
*
* ******************************
* * Copyright (C) 2003 *
* *
* * All Rights Reserved *
* ******************************
*
*****************************************************************************/
#include "bugTask.h"
BugTask::BugTask(const char * description, mapT * map, float x_goal, float y_goal, float max_range, Task * parent_task)
: Task (description, parent_task)
{
m_x_goal = x_goal;
m_y_goal = y_goal;
setWeight(1.0);
m_map_ptr = map;
m_max_range = max_range;
m_mode = SEEK_GOAL;
m_d_followed = 1e9;
}
int BugTask::runOnce()
{
int ret = 0;
int i;
double th_goal;
double distance_to_goal;
if (Task::runOnce()) {
return TRUE;
}
/* clear vote pointer */
for (i = 0; i < m_n_vote_entries; i++ ) {
m_votes[i].vote = 0.0;
}
th_goal = atan2(m_y_goal - getPosY(), m_x_goal - getPosX());
distance_to_goal = sqrt((m_y_goal - getPosY())*(m_y_goal - getPosY()) +
(m_x_goal - getPosX())*(m_x_goal - getPosX()));
float adif = (float)addangs((float)getPosTh(), - (float)th_goal);
if (adif > 15*TE_PI/180)
{
//turn to the left
}
else if (adif < -15*TE_PI/180)
{
//turn to the right
}
else
{
if (distance_to_goal < 1.0)
{
ret = endTask();
}
}
TE_PTURN *pturn;
double xv, yv;
double xw = 0, yw = 0;
static double prev_xw = 0;
static double prev_yw = 0;
double scale = m_map_ptr->cpm;
int xn, yn;
double xnf, ynf;
float mincost = TE_MAXCOST;
float turncost;
double th;
double xworld = getPosX();
double yworld = getPosY();
double thworld = getPosTh();
double costh;
double sinth;
int xoff = (int)(m_map_ptr->xoff * m_map_ptr->cpm);
int yoff = (int)(m_map_ptr->yoff * m_map_ptr->cpm);
static double ranges[360];
static double distances[360];
double MAX_RANGE = m_max_range;
double dr = 1.0/m_map_ptr->cpm;
double min_distance = 1e9;
int min_distance_index = 1000;
double min_range = MAX_RANGE;
int min_range_index = 1000;
int angle_index_left = 1000;
int angle_index_right = 1000;
unsigned char map_val;
double angle_to_goal;
bool goal_reachable;
double angle_reference;
angle_to_goal = atan2((m_y_goal - yworld),(m_x_goal - xworld));
printf("\nAng.to G: %2.1f ", angle_to_goal*180/TE_PI);
angle_reference = thworld;
printf("Ang.ref: %2.1f ", angle_reference*180/TE_PI);
goal_reachable = false;
int k, kk;
double r;
//Check all angles: -180 is (-x) and 0 is (+x)
for (kk = 0; kk <=180; kk++)
{
int k1 = -kk;
int k2 = kk;
float th1 = addangs((float)angle_reference, -k1*TE_PI/180.0f);
float th2 = addangs((float)angle_reference, -k2*TE_PI/180.0f);
//th = addangs((float)thworld, k*TE_PI/180.0f);
double costh1 = cos(th1);
double sinth1 = sin(th1);
double costh2 = cos(th2);
double sinth2 = sin(th2);
ranges[180+k1] = MAX_RANGE;
ranges[180+k2] = MAX_RANGE;
for (int k = k1; k <=k2; k+=(k2-k1))
{
for (r = 0; r <= MAX_RANGE; r+= dr)
{
if (k == k1)
{
xv = r*costh1;
yv = r*sinth1;
}
else
{
xv = r*costh2;
yv = r*sinth2;
}
xw = xv + xworld;
yw = yv + yworld;
/* Transform the world coordinates into map coordinates */
xn = (int)(rint) (xw * scale + xoff);
yn = (int)(rint) (yw * scale + yoff);
map_val = (unsigned char)mapGetCost(m_map_ptr,yn,xn);
if ( map_val > 128)
{
break;
}
}
//maximum obstacle-free distance at this angle
ranges[180+k] = r;
//distance from the maximum range at this angle to the goal
distance_to_goal = sqrt((m_y_goal - yw)*(m_y_goal - yw) +
(m_x_goal - xw)*(m_x_goal - xw));
//estimated distance to the goal
if (r < MAX_RANGE)
{
//if path is blocked, penalize distance to goal
distances[180+k] = r + 1000000*distance_to_goal;
if ((r < min_range))
{
//r = min_range;
min_range_index = k;
min_range = r;
}
}
else
{
distances[180+k] = MAX_RANGE + distance_to_goal;
/*
if ((angle_index_left == 0) && (k == k1))
{
angle_index_left = k1;
}
if ((angle_index_right == 0) && (k == k2))
{
angle_index_right = k2;
}
*/
//If goal is reachable
if (fabs(addangs((float)angle_reference - k*TE_PI/180, -(float)angle_to_goal)) < 1.0*TE_PI/180)
{
distances[180+k] = distance_to_goal;
goal_reachable = true;
}
/*
if ( k == 0 )
{
distances[180+k] = distance_to_goal;
goal_reachable = true;
}
*/
}
if ((distances[180+k] <= min_distance))
{
min_distance = distances[180+k];
min_distance_index = k;
}
//break the loop for the first iteration
if (k == 0)
{
k++;
}
}
}
int left_index;
int right_index;
double range_left;
double range_right;
double angle_left;
double angle_right;
if (min_distance_index != 1000)
{
int start_point = min_distance_index;
if (ranges[180 + min_distance_index + 1] < MAX_RANGE)
{
start_point = min_distance_index + 1;
}
else if (ranges[180 + min_distance_index - 1] < MAX_RANGE)
{
start_point = min_distance_index - 1;
}
else
{
if (m_mode == FOLLOW_OBST)
{
printf("\n\n\nWarning: error detecting followed obstacle\n");
printf("%2.2f %2.2f %2.2f\n",ranges[180 + min_distance_index - 1],
ranges[180 + min_distance_index],
ranges[180 + min_distance_index - 1]);
}
start_point = min_range_index;
}
double prev_range_left = ranges[180 + start_point];
double prev_range_right = ranges[180 + start_point];
for (k = 0; k < 360; k++) {
left_index = (360 + 180 + start_point - k) % 360;
right_index = (180 + start_point + k) % 360;
range_left = ranges[left_index];
range_right = ranges[right_index];
if (angle_index_left == 1000)
{
if (range_left >= MAX_RANGE)
{
angle_index_left = left_index;
}
else if (fabs(range_left - prev_range_left) > MAX_RANGE/2)
{
//angle_index_left = left_index;
}
}
if (angle_index_right == 1000)
{
if (range_right >= MAX_RANGE)
{
angle_index_right = right_index;
}
else if (fabs(range_right - prev_range_right) > MAX_RANGE/2)
{
//angle_index_right = right_index;
}
}
prev_range_left = range_left;
prev_range_right = range_right;
}
//Now look for finer discontinuities
if ((angle_index_left == 1000) || (angle_index_right == 1000))
{
prev_range_left = ranges[180 + start_point];
prev_range_right = ranges[180 + start_point];
for (k = 0; k < 360; k++) {
left_index = (360 + 180 + start_point - k) % 360;
right_index = (180 + start_point + k) % 360;
range_left = ranges[left_index];
range_right = ranges[right_index];
if (angle_index_left == 1000)
{
if (fabs(range_left - prev_range_left) > MAX_RANGE/5)
{
angle_index_left = left_index;
}
}
if (angle_index_right == 1000)
{
if (fabs(range_right - prev_range_right) > MAX_RANGE/5)
{
angle_index_right = right_index;
}
}
prev_range_left = range_left;
prev_range_right = range_right;
}
}
}
angle_left = (angle_index_left - 180)*TE_PI/180;
angle_right = (angle_index_right - 180)*TE_PI/180;
if ((m_mode == FOLLOW_OBST))
{
printf("<angles: %2.1f, %2.1f >",angle_left*180/TE_PI, angle_right*180/TE_PI);
if ((angle_index_left == 1000) || (angle_index_right == 1000))
{
printf("\n\nWarning undef_index: %d, %d\n\n\n",angle_index_left, angle_index_right);
}
}
distance_to_goal = sqrt((m_y_goal - yworld)*(m_y_goal - yworld) +
(m_x_goal - xworld)*(m_x_goal - xworld));
float map_resolution = 1.0f/m_map_ptr->cpm;
//Check conditions to continue in moving towards target
if ((min_distance > m_d_followed + 3*map_resolution) && (m_mode == SEEK_GOAL) && (min_range_index != 1000) && (!goal_reachable))
{
m_mode = FOLLOW_OBST;
m_last_angle = min_distance_index;
if (addangs((min_distance_index - 180)*TE_PI/180, -(float)angle_left) <
addangs((min_distance_index - 180)*TE_PI/180, -(float)angle_right))
{
m_follow_direction = LEFT;
}
else
{
m_follow_direction = RIGHT;
}
printf("\n\nSwitching to FOLLOW_OBST: min_d = %2.2f, d_to_goal = %2.2f DIR = %d\n",
min_distance, distance_to_goal, m_follow_direction);
printf("min_distance_index = %d",min_distance_index);
}
else if ((m_mode == FOLLOW_OBST) &&
//(((min_distance < m_d_followed) && (goal_reachable)) || (min_range >= MAX_RANGE)))
((min_distance + 3*map_resolution < m_d_followed) || (goal_reachable && (distance_to_goal < MAX_RANGE))
|| (min_range >= MAX_RANGE)))
{
m_mode = SEEK_GOAL;
printf("\n\nSwitching to SEEK_GOAL: min_d = %2.2f, d_follow = %2.2f\n",
min_distance, m_d_followed);
}
if (min_distance < m_d_followed)
{
m_d_followed = min_distance;
}
double xgoal, ygoal;
if ((distance_to_goal > MAX_RANGE) || (!goal_reachable))
{
if (m_mode == SEEK_GOAL)
{
//th = addangs((float)thworld, min_distance_index*TE_PI/180.0f);
if (min_distance_index < 0)
{
th = addangs((float)angle_reference, -(min_distance_index)*TE_PI/180.0f);
}
else
{
th = addangs((float)angle_reference, -(min_distance_index)*TE_PI/180.0f);
}
//th = addangs((float)angle_to_goal, angle_index_left*TE_PI/180.0f);
//if (!goal_reachable
}
else
{
/*
if (min_range_index < 0)
{
th = addangs((float)angle_to_goal, (min_range_index + 90)*TE_PI/180.0f);
}
else
{
th = addangs((float)angle_to_goal, (min_range_index + 90)*TE_PI/180.0f);
}
*/
//th = addangs((float)angle_to_goal, (min_distance_index)*TE_PI/180.0f);
//th = addangs((float)angle_reference, -angle_left);
if (m_follow_direction == LEFT)
{
th = addangs((float)angle_reference, -(float)angle_left);
}
else
{
th = addangs((float)angle_reference, -(float)angle_right);
}
//printf("[%d->%2.0f]",min_range_index,th*180/TE_PI);
printf("[%2.1f + %2.1f =>%2.1f]",angle_reference*180/TE_PI, angle_left*180/TE_PI,th*180/TE_PI);
//th = addangs((float)thworld, closest_to_previous_index*TE_PI/180.0f);
}
xgoal = MAX_RANGE*cos(th) + xworld;
ygoal = MAX_RANGE*sin(th) + yworld;
}
else
{
xgoal = m_x_goal;
ygoal = m_y_goal;
}
costh = cos(thworld);
sinth = sin(thworld);
for (k = 0; k < TE_NUM_PTURNS; k++) {
pturn = &(pturns[k]);
th = pturn->th;
xv = pturn->dx;
yv = pturn->dy;
xw = xv * costh - yv * sinth + xworld;
yw = xv * sinth + yv * costh + yworld;
/* Transform the world coordinates into map coordinates */
xn = (int)(rint) (xw * scale + xoff);
yn = (int)(rint) (yw * scale + yoff);
xnf = (xw * scale + xoff);
ynf = (yw * scale + yoff);
/* Abort this arc if the way point is out of bounds */
if (xn < 0 || xn > m_map_ptr->j || yn < 0 || yn > m_map_ptr->i) {
m_votes[pturn->vote_index].vote = -1.0; //ban this arc
//n_banned_arcs++;
}
if ((unsigned char)mapGetCost(m_map_ptr,yn,xn) > 128)
{
m_votes[pturn->vote_index].vote = -1.0; //ban this arc
//n_banned_arcs++;
}
//vote on pturn proportionally to the distance to the goal angle
distance_to_goal = sqrt((ygoal - yw)*(ygoal - yw) +
(xgoal - xw)*(xgoal - xw));
if (m_votes[pturn->vote_index].vote != -1.0)
{
//turncost = (float)fabs(addangs(adif, th)) + distance_to_goal;
turncost = (float)distance_to_goal;
//m_votes[pturn->vote_index].vote = turncost/0.05 /*divide by speed*/;
m_votes[pturn->vote_index].vote = turncost /*divide by speed*/;
if (turncost < mincost)
{
mincost = turncost;
}
}
}
// Loop through arcsets
for (i = 0; i < snum; i++)
{
// number of points per arc. Is the same for all arcs in arcset
int pnum = sets[i].pnum;
float arccost = 0;
TE_ARC * arc = NULL;
//Loop through arcs in arcset
for (int j = 0; j < sets[i].anum; j++)
{
arc = &sets[i].arcs[j];
arccost = 0;
//Loop through points in arc
for (int k = 0; k < pnum; k++)
{
xv = arc->pts[k].x;
yv = arc->pts[k].y;
xw = xv * costh - yv * sinth + xworld;
yw = xv * sinth + yv * costh + yworld;
// Transform the world coordinates into map coordinates
xn = (int)(rint) (xw * scale + xoff);
yn = (int)(rint) (yw * scale + yoff);
xnf = (xw * scale + xoff);
ynf = (yw * scale + yoff);
// Abort this arc if the way point is out of bounds
if (xn < 0 || xn > m_map_ptr->j || yn < 0 || yn > m_map_ptr->i) {
m_votes[arc->vote_index].vote = -1.0; //ban this arc
//n_banned_arcs++;
break;
}
if ((unsigned char)mapGetCost(m_map_ptr,yn,xn) > 128)
{
m_votes[arc->vote_index].vote = -1.0; //ban this arc
break;
//n_banned_arcs++;
}
if (k == pnum-1)
{
th_goal = atan2(m_y_goal - yw, m_x_goal - xw);
distance_to_goal = sqrt((ygoal - yw)*(ygoal - yw) +
(xgoal - xw)*(xgoal - xw));
adif = (float)addangs((float)getPosTh(), - (float)th_goal);
th = atan2(yw - prev_yw, xw - prev_xw);
//arccost = (float)fabs(addangs(adif, th));
arccost = (float)distance_to_goal;
//make it cheaper to take faster arcs.
if (sets[i].speed > 0) {
//m_votes[arc->vote_index].vote = (float)(arccost / fabs(sets[i].speed));
m_votes[arc->vote_index].vote = (float)(arccost);
}
else {
m_votes[arc->vote_index].vote = 0;
}
if ((arccost < mincost) && (arccost > 0))
{
mincost = arccost;
}
}
}
}
}
for (i = 0; i < m_n_vote_entries; i++ ) {
if (m_votes[i].vote > 0) {
//normalize votes such that maximum votes is 1.0
m_votes[i].vote = mincost / m_votes[i].vote;
}
}
//if (m_nruns > 5) {
// ret = endTask();
//}
return ret;
}
/*****************************************************************************
* $Id: bugTask.h,v 1.3 2004/02/03 06:30:58 jgonzale Exp $
* $Revision: 1.3 $
* $Date: 2004/02/03 06:30:58 $
* $Author: jgonzale $
* ******************************
* * Copyright (C) 2003 *
* *
* * All Rights Reserved *
* ******************************
*
* AUTHOR: Juan Pablo Gonzalez
* PROJECT:
* DESCRIPTION: Task in charge of implementint tangent Bug
*
*****************************************************************************/
#ifndef _BUGTASK_H_
#define _BUGTASK_H_
#define MAP_SERVER
#include <stdio.h>
#include "arcs.h"
#include "task.h"
#include <map.h>
class TaskExec;
class Task;
class BugTask : public Task
{
public:
BugTask(const char * description, mapT * map, float x_goal = TE_UNKNOWN, float y_goal = TE_UNKNOWN, float max_range = 100.0f, Task * parent_task = 0);
virtual int runOnce();
protected:
double m_x_goal;
double m_y_goal;
mapT * m_map_ptr;
double m_max_range;
enum {SEEK_GOAL, FOLLOW_OBST} m_mode;
double m_d_followed;
double m_last_angle;
double m_prev_min_distance;
enum {LEFT = -1, RIGHT = 1} m_follow_direction;
};
#endif