Question:
Brian, one question to the Stanley Controller:
If I have a look in papers around stanley, the wheel angle consists of a sum of the heading error between vehicle and the goal point on line AND the atan() part to reduce the track error of the front axle.
In the code I just found the atan part implemented… What the reason for that? Or did I get it wrong?
Answer:
You will notice in the second term the tangent is taken of eFa which is a distance x a gain factor over the speed (Vx). The steer angle error is independent of that tangent term.
xTrackCorrection = Math.Atan((distanceFromCurrentLine * mf.ast.stanleyGain)
/ ((Math.Abs(mf.pn.speed * 0.277777)) + 2));
if (mf.pn.speed > -0.1)
steerAngleGu = glm.toDegrees((xTrackCorrection + abFixHeadingDelta) * -1.0);
else
steerAngleGu = glm.toDegrees((xTrackCorrection - abFixHeadingDelta) * -1.0);
You can see that is what is happening here in this code. atan of the distance term + steering error
You will also notice its all backwards for steering in reverse. the non linear term is the atan of the distance. this is what turns the tractor away from the before it gets to the line. The 2 gains set in the settings affect the interaction between the 2 independently.
abFixHeadingDelta is the difference between the heading at the point 90 degrees from center of steer axle to closest point on the line and the heading of the tractor
Now it should be obvious what happens in v4.3 with a side hill. you can put the steer axle on the line, but the back of the tractor is below. What worked very well was to move the “steering axle” point very close to the pivot point so the distance error was more representative of the actual distance off the line. The tuning is a bit more sensitive because now it is a very short wheelbase
Without the distance measurement, comment it out and try, the vehicle will steer to be parallel to the guidance line, but never get there.
without the steer angle, it will zig zag across the line.
The 2 together - wonderful music
