problem in understanding and implementing pure pursuit algorithm

[hhnavid]

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.

[vhaa]

Hi

I am trying to implement the same tracking path algorithm. Did you solved your problem? Can you send the code?

[rouncer]

Just some basic trigonometry here, it should definitely work when you get it finished.

But I think it could be alot simpler than this.

Just get the difference of the target and the current position.

then get the angle of the vector.

then subtract the angle from the cars angle,  and the tricky thing with this way,  is you have to decide to wrap the circle or not.

Keep going,  its a basic program youll get it,  its in lots of old arcade games - this code,  for running spaceships around like asteroids or space pilot.  (you should be right by yourself.)