Appendix A-Robot Trajectory Simulation

Appendix A-Robot Trajectory Simulation

Background

Most simulations for mobile robot motion require complex algorithms, however, the Denning Mobile Robot has orthogonal drive and steer axes, which simplifies this calculation. The simulation applies a running sum, or numerical integration, to the steer velocity. The running sum is then operated upon by a cosine and sine transformation. This results in an instantaneous direction vector which is multiplied by the drive speed at time delta-t producing an instantaneous velocity vector. A running sum is applied to this vector to result in an estimated x, y position for the robot. If delra-t is made very small, the algorithm becomes very accurate. This algorithm can be found in the "dotrajectory" module in the simulator source code, located in the last section of this appendix.

Interpreting Results

The following tables represent a line-arc trajectory. Note that none of the trajectories have stop commands, thus they do not terminate. These tables reflect the trajectory in figure 8.1. The optimized solution attempts to reduce error by breaking arcs into three sectors. The indirect result of the optimized solution is that the desired time of execution has been altered, typically increased by 5%. Item 2 in the optimized solution is in error, it should be a "STUAE" function, not "STUAC." This can be corrected by changing the program. The abbreviations used in the tables are as follows:

TA = Turn Angle, specified at runtime for arcs
TS = Turn Speed, calculated for arcs
DS = Drive Speed, specified at runtime for lines, computed for arcs
DD = Drive Distance, specified at runtime for lines, computed for arcs
AT = Arc Time, specified at runtime for arcs, computed for lines
AR = Arc Radius, specified at runtime for arcs
Final X position, computed at end of every run
Final Y position, computed at end of every run
Distance from desired position, vector distance from desired goal to actual goal
Distance travelled, integrated drive speed over entire run
Time to execute, estimated time to complete run

DESIRED SOLUTION:
Item: 0,STUA	TA:  90.000  TS:  40.909  DS:0.357  DD: 0.785  AT:  2.200  AR: 0.500
Item: 1,STUA	TA: -90.000  TS: -52.941  DS:0.924  DD: 1.571  AT:  1.700  AR: 1.000
Item: 2,drud	TA:   0.000  TS:   0.000  DS:1.000  DD: 3.000  AT:  3.000  AR: 0.000
Item: 3,STUA	TA:  82.000  TS:  13.898  DS:0.243  DD: 1.431  AT:  5.900  AR: 1.000
Item: 4,drud	TA:   0.000  TS:   0.000  DS:0.600  DD: 0.500  AT:  0.833  AR: 0.000
Item: 5,STUA	TA: 180.000  TS:  54.545  DS:1.047  DD: 3.456  AT:  3.300  AR: 1.100
Item: 6,drud	TA:   0.000  TS:   0.000  DS:0.200  DD: 1.200  AT:  6.000  AR: 0.000
Item: 7,STUA	TA:-270.000  TS: -38.028  DS:0.597  DD: 4.241  AT:  7.100  AR: 0.900
Item: 8,drud	TA:   0.000  TS:   0.000  DS:0.400  DD: 1.200  AT:  3.000  AR: 0.000

Final X position:			3.637 feet
Final Y position:			2.823 feet
Distance travelled:			17.38 feet
Time to execute:			33.03 seconds

NON-OPTIMIZED SOLUTION:
Item: 0,STUA	TA:  90.000  TS:  40.900  DS:0.300  DD: 0.660  AT:  2.200  AR: 0.500
Item: 1,STUA	TA: -90.000  TS: -53.000  DS:0.900  DD: 1.528  AT:  1.698  AR: 1.000
Item: 2,drud	TA:   0.000  TS:   0.000  DS:1.000  DD: 3.000  AT:  3.000  AR: 0.000
Item: 3,STUA	TA:  82.000  TS:  13.800  DS:0.200  DD: 1.188  AT:  5.942  AR: 1.000
Item: 4,drud	TA:   0.000  TS:   0.000  DS:0.600  DD: 0.500  AT:  0.833  AR: 0.000
Item: 5,STUA	TA: 180.000  TS:  54.500  DS:1.000  DD: 3.303  AT:  3.303  AR: 1.100
Item: 6,drud	TA:   0.000  TS:   0.000  DS:0.200  DD: 1.200  AT:  6.000  AR: 0.000
Item: 7,STUA	TA:-270.000  TS: -38.100  DS:0.500  DD: 3.543  AT:  7.087  AR: 0.900
Item: 8,drud	TA:   0.000  TS:   0.000  DS:0.400  DD: 1.200  AT:  3.000  AR: 0.000

Final X position:			3.585 feet
Final Y position:			2.389 feet
Distance from desired position:		0.438 feet
Distance travelled:			16.12 feet
Time to execute:			33.06 seconds

OPTIMIZED SOLUTION:
Item: 0,STUAE	TA:  30.100  TS:  34.300  DS:0.300  DD: 0.263  AT:  0.876  AR: 0.500
Item: 1,STUAE	TA:  30.000  TS:  45.800  DS:0.400  DD: 0.262  AT:  0.654  AR: 0.500
Item: 2,STUAC	TA:  30.100  TS:  34.300  DS:0.300  DD: 0.263  AT:  0.876  AR: 0.500
Item: 3,STUAE	TA: -29.900  TS: -51.600  DS:0.900  DD: 0.522  AT:  0.580  AR: 1.000
Item: 4,STUAE	TA: -30.000  TS: -57.300  DS:1.000  DD: 0.524  AT:  0.524  AR: 1.000
Item: 5,STUAC	TA: -29.900  TS: -51.600  DS:0.900  DD: 0.522  AT:  0.580  AR: 1.000
Item: 6,drud	TA:   0.000  TS:   0.000  DS:1.000  DD: 3.000  AT:  3.000  AR: 0.000
Item: 7,STUAE	TA:  27.400  TS:  11.400  DS:0.200  DD: 0.478  AT:  2.391  AR: 1.000
Item: 8,STUAE	TA:  27.300  TS:  17.100  DS:0.300  DD: 0.476  AT:  1.588  AR: 1.000
Item: 9,STUAC	TA:  27.400  TS:  11.400  DS:0.200  DD: 0.478  AT:  2.391  AR: 1.000
Item:10,drud	TA:   0.000  TS:   0.000  DS:0.600  DD: 0.500  AT:  0.833  AR: 0.000
Item:11,STUAE	TA:  60.100  TS:  52.000  DS:1.000  DD: 1.154  AT:  1.154  AR: 1.100
Item:12,STUAE	TA:  60.000  TS:  57.200  DS:1.100  DD: 1.152  AT:  1.047  AR: 1.100
Item:13,STUAC	TA:  60.100  TS:  52.000  DS:1.000  DD: 1.154  AT:  1.154  AR: 1.100
Item:14,drud	TA:   0.000  TS:   0.000  DS:0.200  DD: 1.200  AT:  6.000  AR: 0.000
Item:15,STUAE	TA: -89.900  TS: -31.900  DS:0.500  DD: 1.412  AT:  2.824  AR: 0.900
Item:16,STUAE	TA: -90.000  TS: -38.200  DS:0.600  DD: 1.414  AT:  2.356  AR: 0.900
Item:17,STUAC	TA: -89.900  TS: -31.900  DS:0.500  DD: 1.412  AT:  2.824  AR: 0.900
Item:18,drud	TA:   0.000  TS:   0.000  DS:0.400  DD: 1.200  AT:  3.000  AR: 0.000

Final X position:			3.632 feet
Final Y position:			2.841 feet
Distance from desired position:		0.018 feet
Distance travelled:			17.38 feet
Time to execute:			34.65 seconds

The Code

The following is the complete C code to generate the figures and tables for the line-arc commands. Solution is provided in three forms: Desired solution, using full precision; Non-Optimized solution, using truncations on full precision results; and Optimized solution, using truncations and separating each arc into three separate arcs.

/*****************************************************************************
 *
 *	Robot Trajectory Simulator
 *		By Roman Mach 
 *
 *	2/27/92
 *
 *	Written using Turbo C++, v1.0
 *
 *****************************************************************************/
#include < stdio.h> 
#include < string.h> 
#include < graphics.h> 
#include < stdlib.h> 
#include < conio.h> 
#include < math.h> 
#include "shade.h"  /* use for generating bitmapped output of display */


void dotrajectory(double, double, double, int);
void drawbackground(void);
void writefinal(void);
void initcolors(void);
void opengraph(void);
void calculate(void);
void makearray(void);
void getoptimum(void);

#define rad		(3.14159265 / 180)

/* available command types */

#define STUA	"STUA"
#define STUAC	"STUAC"
#define STUAE	"STUAE"
#define drud	"drud"

#define MAINWIDTH	4		/* number of supplied parameters */
#define TYPES		3		/* number of different tests to perform */


/*****************************************************************************

 1. Enter desired time to complete arc, ignored for lines, in first column 
 2. Enter desired arc radius for arcs or drive dist for lines in second column
 3. Enter turn angle for arcs, ignored for lines, in third column
 4. Enter zero if arc or drive speed if line in fourth column

 Example:		  

 double driver[][MAINWIDTH] =
 {
	3,	0.5,	90.0,	0.0,
	0,	1.0,	0.0,	0.5,
 };


 This will move the robot (viewed from top) in an arc clockwise,
 and then in a straight line.

 NOTE 1: angles for robot are revesed from standard cartesian space so reverse
 when entering into the array.

 NOTE 2: it is possible to specify negative drive speeds.

 NOTE 3: LLAMA's STUA functions expect positive and negative turn speeds and
 only positive turn angles, which is not the case here.

 NOTE 4: LLAMA's STUA functions accept negative drive velocities.  These 
 algorithms do not use negative drive speeds in the conversion process.

	arc time,	arc radius,	turn angle,		drive speed(0==arc,other=line)
	seconds,	feet,		degrees,		feet per second

			(drive dist)

*****************************************************************************/

double driver[][MAINWIDTH] =
{ 
	2.2,		0.5,		90.0,		0.0,
	1.7,		1.0,		-90.0,		0.0,
	0.0,		3.0,		0.0,		1.0,
	5.9,		1.0,		82.0,		0.0,
	0.0,		0.5,		0.0,		0.6,
	3.3,		1.1,		180.0,		0.0,
	0.0,		1.2,		0.0,		0.2,
	7.1,		0.9,		-270.0,		0.0,
	0.0,		1.2,		0.0,		0.4
};

FILE *output;

/* pointers to matrix containing various parameters for each test */
/* these pointers need to be initiallized by doing a "malloc" */

double **arctime;
double **arcradius;
double **turnangle;
double **turnspeed;
double **drivespeed;
double **drivedist;
char ***desc;

/* this contains the number of "until" commands needed to execute a test */

int sizevect[TYPES];

/* various motion simulator variables */

double ap, bp;			/* "previous position" variables for x, y */
double ds, tm;			/* distance moved, and time elapsed */
double x0,a0,b0;		/* instantaneous drive speed, x and y positions */
double edx, edy, err;		/* error difference variables for positional tests */
double slice = 0.001;		/* simulator resolution, decrease to improve accuracy */
double dt;			/* delta time variable */
double scale = 500;		/* used to scale image size on screen */
int xscr, yscr;			/* used to draw images on screen */
double xlsc, ylsc;		/* used as temp variables for screen */
int gmc, gac;			/* maximum and average color variables for screen */
int xmax, ymax;			/* maximum screen x and y positions */
int ux, uy;			/* temp variables */


void
main(void)
{
	int i, j;
	double drv, ome;

	opengraph();
	if((output = fopen("simout1.txt","w"))==NULL)	/* write results to file */
	{
		printf("error opening file\n");
		exit(0);
	}
	makearray();					/* malloc space for arrays */
	initcolors();					/* get screen details */
	ux = xlsc = (int) ((float) xmax / -32.0 );	/* place origin on screen */
	uy = ylsc = (int) ((float) ymax * 1.05 );
	dt = slice / 2;
	drawbackground();				/* draw grid marks */
	calculate();					/* calculate all variables */
	for(j=0;j< TYPES;j++)				/* perform tests */
	{
		if(!j)
		{
			setcolor(gac);
			setlinestyle(0,0,3);
		}
		else
		{
			setcolor(gmc);
			setlinestyle(0,0,1);
		}
		x0 = a0 = b0 = ds = tm = ap = bp = 0;	/* reset simulator for each test */
		xlsc = ux;				/* reset "line-to" function */
		ylsc = uy;
		for (i = 0; i< sizevect[j]; i++)		/* do once for each command */
		{
			fprintf(output,"Item:%2d,%s\tTA:%8.3f  TS:%8.3f  DS:%5.3f  DD:%6.3f AT:%7.3f  AR:%6.3f\n",
				i, desc[j][i],
				turnangle[j][i], turnspeed[j][i], 
				drivespeed[j][i], drivedist[j][i],
				arctime[j][i], arcradius[j][i]
			);

/* go do running simulation of motion */

			dotrajectory(arctime[j][i], drivespeed[j][i], turnspeed[j][i], i);
		}
		if(!j)			/* check for delta position output */
		{
			edx = a0;
			edy = b0;
			err = 0;
		}
		else
		{
			err = hypot(edx-a0, edy-b0);
		}
		fprintf(output,"\n");
		fprintf(output,"Final X position:\t%7.3f feet\n",a0*2); 
		fprintf(output,"Final Y position:\t%7.3f feet\n",b0*2); 
		if(j!=0)
			fprintf(output,"Distance from desired position:\t%7.3f feet\n",2*err);
		fprintf(output,"Distance travelled:\t%8.2f feet\n",ds*2);
		fprintf(output,"Time to execute:\t%8.2f seconds\n\n\n",tm); 
	}
/*	saveimg("x1.bmp",gmc,INVIMG);*/ /* used to create bitmapped file of screen */
	close(output);
	getch(); 
	closegraph();
}

/***********************************************
 *
 *	Calculate unknown parameters
 *
 ***********************************************/

void
calculate()
{
	int i,j;

/* first calculate full precision STUA and drud commands */

	for(i=0;i< sizevect[0];i++)
	{
		arctime[0][i] = driver[i][0];		/* obtain values from matrix */
		arcradius[0][i] = driver[i][1];
		turnangle[0][i] = driver[i][2];

/* if STUA do this */

		if(!driver[i][3])
		{
			drivespeed[0][i] = rad*arcradius[0][i]*fabs(turnangle[0][i])/arctime[0][i];
			turnspeed[0][i] = turnangle[0][i] / arctime[0][i];
			drivedist[0][i] = fabs(drivespeed[0][i] * arctime[0][i]);
			strcpy(desc[0][i],STUA);
		}

/* otherwise do this */

		else
		{
			drivespeed[0][i] = driver[i][3];
			turnspeed[0][i] = 0;
			drivedist[0][i] = fabs(driver[i][1]);
			arctime[0][i] = fabs(drivedist[0][i] / drivespeed[0][i]);
			arcradius[0][i] = 0;
			strcpy(desc[0][i],drud);
		}
	}

/* next calculate simple STUA reduced precision commands */
/* use "floor" function to put appropriate values into tenths-of */

	for(i=0;i< sizevect[1];i++)
	{
		arcradius[1][i] = arcradius[0][i];
		turnangle[1][i] = floor(driver[i][2] * 10) / 10;
		drivespeed[1][i] = floor(drivespeed[0][i] * 10) / 10;
		turnspeed[1][i] = floor(turnspeed[0][i] * 10) / 10;
		if(!driver[i][3])
		{
			arctime[1][i] = fabs(turnangle[1][i] / turnspeed[1][i]);
			drivedist[1][i] = fabs(drivespeed[1][i] * arctime[1][i]);
			strcpy(desc[1][i],STUA);
		}
		else
		{
			drivedist[1][i] = floor(drivedist[0][i] * 10) / 10;
			arctime[1][i] = fabs(drivedist[1][i] / drivespeed[1][i]);
			strcpy(desc[1][i],drud);
		}
	}

/* finally, get optimize STUAE and STUAC functions */

	getoptimum();
}




/***********************************************
 *
 *	Convert STUA's to STUAE's and STUAC's
 *
 ***********************************************/

void
getoptimum()
{
	int i,j;
	double getang, getspd;
	double maxspeed, minspeed;
	double minang, maxang;

	j=0;					/* set up variable to build up new elements */
	for(i=0;i< sizevect[0];i++)		/* go through accurate list */
	{
		if(!driver[i][3])		/* do this if STUA-type */
		{
			getang = turnangle[0][i] / 3;	/* divide desired angle into 3 parts */
			minang = floor(getang * 10) / 10;	/* get a min and max version */
			maxang = floor(getang * 10 + 1) /10;

/* get drive speed, then get integer version and convert to min and max values */

			getspd = rad * arcradius[0][i] * fabs(turnangle[0][i]) / arctime[0][i];
			minspeed = floor(getspd * 10) / 10;	/* get a min and max version */
			maxspeed = floor(getspd * 10 + 1) / 10;

/* set up first STUAE */
/* note that minspeed is used with maxangle to help minimize errors */

			arctime[2][j] = rad * arcradius[0][i] * fabs(maxang) / minspeed;
			turnspeed[2][j] = floor((maxang / arctime[2][j]) * 10) / 10;
			drivedist[2][j] = minspeed * arctime[2][j];
			arcradius[2][j] = arcradius[0][i];
			drivespeed[2][j] = minspeed;
			turnangle[2][j] = maxang;
			strcpy(desc[2][j],STUAE);
			j++;

/* set up second STUAE */
/* note that min's and max's have been reversed from previous */

			arctime[2][j] = rad * arcradius[0][i] * fabs(minang) / maxspeed;
			turnspeed[2][j] = floor((minang / arctime[2][j]) * 10) / 10;
			drivedist[2][j] = maxspeed * arctime[2][j];
			arcradius[2][j] = arcradius[0][i];
			drivespeed[2][j] = maxspeed;
			turnangle[2][j] = minang;
			strcpy(desc[2][j],STUAE);
			j++;

/* finally, do a STUAC, similar to the first STUAE */

			arctime[2][j] = rad * arcradius[0][i] * fabs(maxang) / minspeed;
			turnspeed[2][j] = floor((maxang / arctime[2][j]) * 10) / 10;
			drivedist[2][j] = minspeed * arctime[2][j];
			arcradius[2][j] = arcradius[0][i];
			drivespeed[2][j] = minspeed;
			turnangle[2][j] = maxang;
			strcpy(desc[2][j],STUAC);
			j++;
		}
		else		/* otherwise do a drud */
		{
			arcradius[2][j] = 0;
			drivespeed[2][j] = floor(driver[i][3] * 10) / 10;
			drivedist[2][j] = floor(fabs(driver[i][1]) * 10) / 10;
			turnspeed[2][j] = 0;
			turnangle[2][j] = 0;
			arctime[2][j] = fabs(drivedist[2][j] / drivespeed[2][j]);
			strcpy(desc[2][j],drud);
			j++;
		}
	}
}

/***********************************************
 *
 *	Initialize all array/matrix pointers
 *
 ***********************************************/

void
makearray()
{
	int size, i, j;

	size = sizeof (driver) / (sizeof (double) * MAINWIDTH);
	arctime = (double **) malloc(sizeof (double *) * TYPES);
	arcradius = (double **) malloc(sizeof (double *) * TYPES);
	turnspeed = (double **) malloc(sizeof (double *) * TYPES);
	drivespeed = (double **) malloc(sizeof (double *) * TYPES);
	drivedist = (double **) malloc(sizeof (double *) * TYPES);
	desc = (char ***) malloc(sizeof (char **) * TYPES);
	sizevect[0] = sizevect[1] = sizevect[2] = size;
	for(i=0;i< size;i++)
		if(!driver[i][3])
			sizevect[2] += 2;/* need to generate new elements, get more space */
	for(i=0;i< TYPES;i++)
	{
		arctime[i] = (double *) malloc(sizeof (double) * sizevect[i]);
		arcradius[i] = (double *) malloc(sizeof (double) * sizevect[i]);
		turnspeed[i] = (double *) malloc(sizeof (double) * sizevect[i]);
		drivespeed[i] = (double *) malloc(sizeof (double) * sizevect[i]);
		drivedist[i] = (double *) malloc(sizeof (double) * sizevect[i]);
		desc[i] = (char **) malloc(sizeof (char *) * sizevect[i]);
		for(j=0;j< sizevect[i];j++)
			desc[i][j] = (char *) malloc(sizeof (char) * 10);
	}
}

/***********************************************
 *
 *	Draw reference grids on screen
 *
 ***********************************************/

void
drawbackground()
{
	int i, j, tx, ty;

	line(ux-1000,uy,ux+1000,uy);
	line(ux,uy-1000,ux,uy+1000);

	for(i=-1;i< 12;i++)
		for(j=-12;j< 1;j++)
		{
			tx = i * scale / 2 + ux;
			ty = j * scale / 2 + uy;
			line(tx-1,ty,tx+1,ty);
			line(tx,ty+1,tx,ty-1);
			rectangle(tx-2,ty-2,tx+2,ty+2);
		}
	outtextxy(ux+3,3,"y");
	outtextxy(xmax-20,uy+3,"x");
}

/***********************************************
 *
 *	Calculate running position estimate
 *
 ***********************************************/

void
dotrajectory(double time, double drv, double ome, int sequence)
{
	double sumtot = 0;
	double pie, fill;
	char strn[30];
	int i;

	tm += time;			/* calculate accumulated time */
	pie = dt * ome * 2;		/* this represents an instantaneous velocity vector */
	for(i=0; (sumtot+=slice) <  time; i++)
	{
		x0 += pie;		/* this represents an integrated velocity vector */
		fill = x0 * rad;		/* convert to appriopriate representaion */
		a0 += dt * drv * cos(fill); /* calculate instantaneous x,y change of positions */
		b0 += dt * drv * sin(fill); /* and integrate to get current*/
		ds += hypot(a0-ap, b0-bp); /* calculate change of position, distance moved */
		ap = a0;			/* save for later */
		bp = b0;
		if(!(i%10))		/* draw a line on the screen every 10 calculations */
		{
			xscr = ux + a0 * scale;
			yscr = uy - b0 * scale;
			if(!i)		/* draw a marker for every command */
			{
				rectangle(xscr-1,yscr-1,xscr+1,yscr+1);
			/*	outtextxy(xscr+4,yscr+6,itoa(sequence,strn,10));*/
			}
			line (xscr, yscr, xlsc, ylsc);
			xlsc = xscr;
			ylsc = yscr;
		}
	}
}

/***********************************************
 *
 *	Get screen data
 *
 ***********************************************/

void
initcolors()
{
	gmc = getmaxcolor();
	if (gmc != 1)
		gac = gmc / 2;
	else
		gac = 0;
	setcolor(gac);
	xmax = getmaxx();
	ymax = getmaxy();
}

/***********************************************
 *
 *	Initialize Graphics
 *
 ***********************************************/

void
opengraph()
{


	int gdriver = DETECT, gmode, errorcode;

	initgraph(& gdriver, & gmode, "d:\\tc\\bgi");
	errorcode = graphresult();
	if (errorcode != grOk)
	{
		printf("Graphics error: %s\n",grapherrormsg(errorcode));
		printf("press any key to halt:");
		getch();
		exit(1);
	}
}


Return to Thesis Index