Drive to point PID doesn’t work properly

Question

This is my first post here. I am trying to get a robot I’m working on to drive to a point via a PID controller. My code works fine if the point is in front of the robot, but completely falls apart if the point is behind or to the side of the robot. I have Odometry running in the background, and I am able to access updated position information easily.

Here’s the code I’m using now to determine the error which I feed to the PID:

current = odom->getPos();

// calculate our error
// our distance error is the distance to the target
double distError = util::distance(current, target);
double angError = (util::getAngDiff(current, target) - current.theta);

if (angError > 180)
{
    angError -= 360;
}

else if (angError < -180)
{
    angError += 360;
}

And my getAngDiff:

double util::getAngDiff(Point a, Point b)
{
    // current = a, target = b

    double tAngle = atan2(b.y - a.y, b.x - a.x);
    lib727::util::deg2rad(a.theta);

    // robot angle is in radians, target angle is in radians
    double aDiff = tAngle - a.theta;

    // convert difference to degrees
    lib727::util::rad2deg(aDiff);

    // round to 100th place
    return round(aDiff * 100) / 100;
}

It’s worth noting that my current angle (current.theta) is in degrees and is in the range [0-360), and I use the Euclidean distance formula to calculate distance error. From there, I simply add/subtract my angular error from my distance error and multiply the final error per side by kP. Is there anything obvious wrong with my math? Here’s a video (in a simulator) of what happens when I tell the robot to go to the point {0, 3}:

https://i.stack.imgur.com/V74X0.jpg

It should turn 90 degrees and then drive straight, but it obviously doesn’t. Thanks for any help!

Answer 1

I don't understand

lib727::util::deg2rad(a.theta);

You're not assigning the output anywhere, so are you expecting that this is going to modify a.theta? If so, you're not modifying it back! This would seem to me then that you're constantly applying a degrees-to-radians conversion over top of the existing angle, which I believe should quickly drive it to zero, because the conversion of degrees to radians is approximately divide-by-sixty. First call it'd convert 120 to about 2, then the second call it'd convert a.theta from 2 to about 0.03, then on the third call it's basically zero.

I think it'd be easier to keep everything in degrees instead:

double tAngle = lib727::util::rad2deg(atan2(b.y - a.y, b.x - a.x));
double aDiff = tAngle - a.theta;

It's not clear what kind of robot you've got, or what you're doing with the PID control, but if you don't tune your angular controller to be significantly more aggressive than your linear controller then you'll probably wind up with some form of a limit cycle where your robot is just driving circles around the target position.

If you're trying to do this with an Ackermann-steered vehicle (as opposed to a differentially-steered vehicle) then you're probably not going to succeed with brute PID control alone; you'd need to do some trajectory planning and PID to that trajectory.