Pure Pursuit controller should follow y = x + 5

Hi

I have coded a pure pursuit controller from scratch and have a vehicle following y = x with the goal to switch left to y = x + 5.

Anyhow, my vehicle is always a bit too far away from y = x + 5. I assume there is some mistake in my alpha calculation. Can someone help me?

ld = k * v;                             		         % look ahead
alpha = asin( (5 + x(i) - y(i) ) / ld ) - theta(i);      % calculate alpha 
phi(i) = atan(( 2 * L * sin(alpha) ) / ld);  			 % steering angle phi

Answer 1

[Pratik]

Hi Wegge,

As per my understanding, you are trying to implement a pure pursuit controller. The goal of this controller is to steer a vehicle from its current path, which is the line y = x, to a new path, the line y = x + 5. However, the vehicle is having trouble aligning with this new path.

Assuming that the remaining code is correct, here is how the calculation of ‘alpha’ can be modified to get the desired result. Please refer to the following code snippet:

Ld = 5; % Lookahead distance

% Extract the vehicle state
x = x(i);
y = y(i); 
theta = theta(i);

% For the line y = x + 5, find a point ahead at the lookahead distance  
x_ahead = x + Ld / sqrt(2);
y_ahead = x_ahead + 5;
goal_point = [x_ahead, y_ahead];

% Compute the relative position of the goal point to the vehicle
dx = goal_point(1) - x;
dy = goal_point(2) - y;

% Transform to vehicle coordinates
alpha = atan2(dy, dx) - theta;

Please refer to the following documentation of ‘pure-pursuit-controller’ for more information:

https://www.mathworks.com/help/robotics/ug/pure-pursuit-controller.html

Hope this helps!