1
$\begingroup$

I am working on autonomous vehicle control, i used the kinematic bicycle model,

  • For the speed reference I used a speed profile not a constant speed(given below [![speed][1]][1]
  • for the steering angle I used the radius to generate the steering angle d=atan(L/r), you will find the code used below. where did i go wrong so that i don't have any pursuit ?
  • the results of the (x,y) vehicle position (in blue) and the road (in red) are given by [![result][2]][2]
  • Code:

`roadCenters = [114.0758 129.2616 -0.002332852; 123.7942 132.6021 -0.002582644; 133.8967 137.3962 -0.002888271; 137.0677 138.9055 -0.00298834; 150.6001 145.0092 -0.003429421; 173.4132 160.0353 -0.004368632; 285.7534 229.03 -0.01051842; 395.1082 301.1 -0.01935268; 454.6491 327.2592 -0.02460683; 461.2142 328.7133 -0.02515263; 471.513 329.7459 -0.02595824; 496.1926 330.8792 -0.02788709; 526.3301 325.9428 -0.030045; 555.0835 315.8794 -0.03197174; 626.267 278.9304 -0.03682783; 785.6708 195.2591 -0.05133038; 806.3408 188.8024 -0.05371195; 839.4267 183.6897 -0.0578257; 867.7778 186.2227 -0.06168913; 896.191 191.5745 -0.06577247; 967.1287 218.0381 -0.07697441; 1008.957 234.3338 -0.08402597; 1039.905 250.1733 -0.08959433; 1055.937 261.9383 -0.09269901; 1060.51 267.1768 -0.09367474; 1075.818 284.7013 -0.09699582; 1093.205 314.3776 -0.1013468; 1105.474 343.11 -0.1049445];

x=roadCenters(:,1); y=roadCenters(:,2);

nPoints = size(roadCenters, 1);

theta = [0:0.1:2*pi, 0];

nCurves = nPoints - 2; gridSize = ceil(sqrt(nCurves));

plotRow = 1; plotCol = 1; figure(1) clf; for currentPt = 1:nCurves x0 = x(currentPt + 0); x1 = x(currentPt + 1); x2 = x(currentPt + 2);

y0 = y(currentPt + 0); y1 = y(currentPt + 1); y2 = y(currentPt + 2);

A = [2*(x1-x0) , 2*(y1-y0); ... 2*(x2-x0) , 2*(y2-y0)];

B = [((x1^2 - x0^2) + (y1^2 - y0^2)); ... ((x2^2 - x0^2) + (y2^2 - y0^2))]; X = (A^(-1))*B; xc = X(1); yc = X(2);

r = sqrt((x0 - xc)^2 + (y0 - yc)^2); d=atan(L/r); disp(d) circle_x = rcos(theta); circle_y = rsin(theta);

subplot(gridSize, gridSize, currentPt) plot([x0, x1, x2], [y0, y1, y2], 'LineWidth', 1, 'Color', [0 0 0]); hold on; plot([x0, x1, x2], [y0, y1, y2], 'x', 'LineWidth', 1, 'Color', [1 0 0]); plot(circle_x + xc, circle_y + yc, 'LineStyle', '-.', 'LineWidth', 0.5, 'Color', [0 0 0.8]); axis equal title(['Curvature at point ', num2str(currentPt)]); end t = (0:length(d)-1); % Create a time vector D= timeseries(d, t); % Create a timeseries object `

$\endgroup$

1 Answer 1

0
$\begingroup$

It's not exactly clear what you're looking for help with here because you seem to be asking two distinctly different questions in the same post:

  1. How do I convert a polyline into a steering angle, and
  2. How do I make the lateral control independent of speed?

For the first question, I would try to convert the polyline into a radius of curvature, and then convert the radius of curvature to a steering angle via Ackermann geometry (assuming it's an Ackermann-steered vehicle).

Assuming the vehicle starts at a point in the polyline, transform the next two points into vehicle coordinates, then fit a circle to those points. This will give you the radius of curvature. Then you can calculate the steering angle as

$$\mbox{atan}\left(\frac{L}{r}\right)$$

where $L$ is the wheel base (distance between front and rear axles) and $r$ is the radius of curvature.

For the question of making it independent of speed, what you're really saying is that you want to remove time as a factor, which is totally doable. Instead of having a PID response curve that plots reference and feedback versus time, you could use longitudinal position.

Instead of calculating PID terms with respect to time, try:

proportional_error = ref - fbk;
integral_error = integral_error + (proportional_error * dX);
derivative_error = (proportional_error - prev_proportional_error) / dX; 
prev_proportional_error = proportional_error;

Finally, if this were me, I would use the calculated steering angle as a "feed-forward" term that does the bulk of the steering work as a proactive control term, and then supplement that with the result of a PID control that's wrapped around the lane offset value as a reactive control term, like:

y_ref = 0; // Assuming 0 is the center of the lane
y_fbk = // lane position feedback from some sensor

desired_steering_angle = // calculated from polyline and vehicle geometry

dX = // new forward vehicle motion since last loop update

proportional_error = y_ref - y_fbk;
integral_error = integral_error + (proportional_error * dX);
derivative_error = (proportional_error - prev_proportional_error) / dX;

kP = // Proportional gain, this is a parameter you tune
kI = // Integral gain, 
kD = // Derivative gain

pid_output = (kP * proportional_error) + (kI * integral_error) + (kD * derivative_error);

steering_command = desired_steering_angle + pid_output;

and this should be something that performs consistently across a large variety of lane curvatures and vehicle speeds.

:EDIT:

Based on the update to your question, it looks like you're putting the entire x/y points array into the curvature equation, but I think you need to go point by point and reevaluate. In looking at the results here, you also have a pretty hard "knee" in the data where you go [0,0] to [2.8249,0] and then from there to [-34.4, 35.2]. You'll need to break up the path into smaller parts to get a smooth trajectory.

Here's some code that iterates across the points:

roadCenters = [0      0 0; ...
               2.8249 0 0; ...
               -34.4 35.2 0; ...
               -145 -57 0; ...
               -148 -211 0; ...
               -41 -326 0; ...
               165 -400 0; ...
               417 -425 0; ...
               950 -32 0; ...
               1034 111 0];

x=roadCenters(:,1);
y=roadCenters(:,2);

nPoints = size(roadCenters, 1);

theta = [0:0.1:2*pi, 0];

nCurves = nPoints - 2;
gridSize = ceil(sqrt(nCurves));

plotRow = 1;
plotCol = 1;
figure(1)
clf;
for currentPt = 1:nCurves
  x0 = x(currentPt + 0);
  x1 = x(currentPt + 1);
  x2 = x(currentPt + 2);

  y0 = y(currentPt + 0);
  y1 = y(currentPt + 1);
  y2 = y(currentPt + 2);

  A = [2*(x1-x0) , 2*(y1-y0); ...
       2*(x2-x0) , 2*(y2-y0)];

  B = [((x1^2 - x0^2) + (y1^2 - y0^2)); ...
       ((x2^2 - x0^2) + (y2^2 - y0^2))];
  X = (A^(-1))*B;
  xc = X(1);
  yc = X(2);

  r = sqrt((x0 - xc)^2 + (y0 - yc)^2);

  circle_x = r*cos(theta);
  circle_y = r*sin(theta);

  subplot(gridSize, gridSize, currentPt)
  plot([x0, x1, x2], [y0, y1, y2], 'LineWidth', 1, 'Color', [0 0 0]);
  hold on;
  plot([x0, x1, x2], [y0, y1, y2], 'x', 'LineWidth', 1, 'Color', [1 0 0]);
  plot(circle_x + xc, circle_y + yc, 'LineStyle', '-.', 'LineWidth', 0.5, 'Color', [0 0 0.8]);
  axis equal
  title(['Curvature at point ', num2str(currentPt)]);
end

And that gives this set of plots:

Plot of curvatures

You can see that knee in the first plot, and then also pay attention to the units here - the first plot has axes scaled in the tens and later plots are scaled in the thousands.

$\endgroup$
5
  • $\begingroup$ thank you very much for your help, and what i mean by the speed independent from the steering angle, when calculating the steering angle at several attempts it would not give me a path pursuit so i thought , the problem may be with my speed reference but since you made it clear can you please explain to me more about how to convert the polyline into a radius of curvature, because i tried this formula and i am sure its the curvature i got wrong. $\endgroup$
    – Asmaa GZ
    Feb 13 at 9:57
  • $\begingroup$ @AsmaaGZ you said you tried it and you're sure the curvature is wrong; can you please edit your question to show what you tried and the answer you got? $\endgroup$
    – Chuck
    Feb 13 at 19:27
  • $\begingroup$ thank you again for your help, I have have added the code and the results $\endgroup$
    – Asmaa GZ
    Feb 15 at 11:49
  • $\begingroup$ @AsmaaGZ I updated the answer, please see if this is helpful. $\endgroup$
    – Chuck
    Feb 16 at 17:43
  • $\begingroup$ your code was really helpful, I added some code lines to calculate the steering angle at each radius so that i get a path pursuit and i also modified the road into a better one ( you could see that in the edited question where i put the new result along with my program), now i am not sure where is the problem and why my bicycle model isn't following the road. $\endgroup$
    – Asmaa GZ
    Feb 19 at 13:06

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.