hello all,

i don't know if it's the right place for this question so excuse me if it isn't. anyway it's been a while that i'm trying to implement a path tracker system based on pure pursuit algorithm but the results of my code are terrible. i mean that my car-like robot fails to follow the path correctly. my guess is that despite reading the algorithm many times, i haven't fully understood the algorithm. i'm not sure if my code is correct at all so i can't explain my problem more precisely. sorry about that.

i use webots for my simulation and so the global coordinate system is the same as the one specified by webots. i assume the terrain is flat so i only use X and Z component of robot 3D position (Y axis is upward). my code is as follows which tries to compute the correct steering angle for my robot based on the goal position (targetPos)

float purePursuit(double *targetPos, //(XZ) in global coord. system

double *robotInitialPos,//robot initial position (XYZ) in global coord. system given by

//webots GPS.

float robot_angle) //given by compass sensor mounted on the robot

{

//this function computes the steering angle (in radians) for a car-like robot using pure pursuit algorithm

//transform the target coords from global to car local coorndinate system

//(alpha, beta) are the coordinates of the origin of the destination coordinate system which in our case is

//the robot position. so we have : alpha = robotInitialPos[X] and beta = robotInitialPos[Z].

//X_new = x_old - alpha;

//Y_new = y_old - Beta;

double targetPos2[2];

float tmp1 = targetPos[0] - robotInitialPos[0], tmp2 = targetPos[1] - robotInitialPos[2];

targetPos2[0] = tmp1 * cos(robot_angle) + tmp2 * sin(robot_angle);

targetPos2[1] = -tmp1 * sin(robot_angle) + tmp2 * cos(robot_angle);

printf("\ntarget coords in robot ref frame : %f %f\n", targetPos2[0], targetPos2[1]);

float L; //lookahead distance for the pure pursuit algorithm

float R; //the turning radius of the car

float alpha; //steering angle for the car

//compute the lookahead distance (L)

L = sqrt( pow(targetPos2[0], 2.0) + pow(targetPos2[1], 2.0));

printf("\nlookahead distance is : %f\n", L);

//compute the turning radius

R = (L*L) / (2 * targetPos2[0]); // R = L^2 / 2x

//AXLES_DIST is the distance between front and rear axle

alpha = -AXLES_DIST/R;

printf("\ncomputed steering angle is : %f\n", alpha);

if(alpha < 0.0)

{

printf("\nsteering left");

}

else if(alpha > 0.0)

{

printf("\nsteering right");

}

else

{

printf("\ngoing straight");

}

return alpha;

}

so i was wondering if any of you guys could help me. thanks for your time.