I am working on a control system for autonomous vehicles, i am using a kinematic bicycle model simulated on matlab simulink and the formula i used for the steering angle is given in the figures below along with the code and my simulink model:
Result :
Speed profile reference :
Code:
x=roadCenters( :,1);
y=roadCenters( :,2);
% Your data
t = 1:size(x)
% Fit a third-degree polynomial to the x-coordinates
px = polyfit(t, x, 3);
% Fit a third-degree polynomial to the y-coordinates
py = polyfit(t, y, 3);
% Find the first derivative of x(t) and y(t)
dx = polyder(px);
dy = polyder(py);
% Find the second derivative of x(t) and y(t)
d2x = polyder(dx);
d2y = polyder(dy);
% Calculate the curvature
k = abs(polyval(dx, t).*polyval(d2y, t) - polyval(dy, t).*polyval(d2x, t)) ./ (polyval(dx,
t).^2 + polyval(dy, t).^2).^(3/2);
% the steering angle
d=atan(L*k);
% moving d to simulink
t = (0:length(d)-1)'; % Create a time vector
D= timeseries(d, t); % Create a timeseries object
Result :
Speed profile reference :