differential drive PID controller

Question

I have a differential drive robot that works fine (good PD parameters) driving at say 1 m/s. Now, if it speeds up (to 1.2 m/s) it starts wobbling again. What would be a good strategy for a controller that is able to cope with the whole speed range of 0 - 4 m/s?

edit 14th of April:

The robot is a line follow robot but I do not see how this would be related to my question since a robot following a trajectory would have the same problem.

I recently talked to other developers of differential drive robots and they are facing similar issues e.g. they told me that they need to adjust PID parameters once the battery is not fully charged hence the robot drives at a different speed.

I do not know if you guys are into youtube, but if your are really interested in my robot this link would be helpful: https://www.youtube.com/watch?v=vMedNPhXlEo

PID parameters are: P 0.31, D 0.59, I 0.00

PID controller programmed using C:

  // note: the inner wheel turns backwards for narrow curves
  // cte is -128..128 depending on the robots position
  // relative to a trajectory / black line 
  /** Execute the PID controller and update motor speeds */
  void PID()
  {
    int32_t steer;
    int32_t cte;
    cte = 128 - get_segment_center(0);
    // Compute PID equation
    steer = (int)(
      -P * (float)cte 
      -D * (float)(cte - diff_cte) / (float)PERIOD_MS
      -I * (float)int_cte
    );
    if (steer < -5)
    {
      // turn left
      turn = -1;
      uXbot_move(MAX_SPEED + steer, MAX_SPEED);
    } 
    else if (steer > 5)
    {
      // turn right
      turn = 1;
      uXbot_move(MAX_SPEED, MAX_SPEED - steer);
    }
    else
    {
      // go straight
      turn = 0;
      uXbot_move(MAX_SPEED, MAX_SPEED);
    }
    diff_cte = cte;
    int_cte += cte;
  }

Answer 1

You aren't properly mapping your steering value to the wheel speeds. In fact, I don't think you're applying the PID correctly at all.

From your code, my guess is that you're using get_segment_center to determine the adjustment that you need to make. My assumption is that this describes a distance measurement of how far the line sensor is off of the center of the line. If that's the case, you are lacking the necessary calculations.

The output of a PID takes an error and gives you a correction, so if your error is in inches then your correction will be in inches; if the error is in degrees, your correction will be in degrees, and so on. You need to pick either of those and stick with it. As written, you're taking a distance error and plugging it directly into a speed correction – no wonder it only works at one speed!

My recommendation would be to calculate the steering error as an angle, which you can get by measuring (1) the distance over which your sensor can pick up the line and (2) the distance between the axle and the sensor. With the tangent function, you can map sensor readings to angle error. Then, you can implement the function I wrote about in this post to convert the steering angle to wheel velocities.

Answer 2

I see that his question is from a long time ago but, just in case it still gets attention, here are some thoughts based on running a small line follower:

Let's address the battery problem first:

I assume that the statement uXbot_move(MAX_SPEED + steer, MAX_SPEED); has parameters that map to PWM values - possibly by some scaling factor.

A fixed PWM value will produce a different drive voltage as the battery voltage sags. In your motor driver, measure the battery voltage with an ADC input. Use that value to adjust the actual PWM duty cycle so that the average voltage stays constant. Change the uxBot() to accept a Voltage rather than a PWM. Now you are able to command, say, 3 Volts to the motor and be confident that is what it gets regardless (within limits) of the actual battery voltage. If you had 8 bits of PWM resolution you might end up with code fragments like these:

void setLeftMotorVolts(float volts) {
  int motorPWM = (255 * volts) / batteryVolts;
  setLeftMotorPWM(motorPWM);
}

void setRightMotorVolts(float volts) {
  // reverse the right motor
  int motorPWM = (-255 * volts) / batteryVolts;
  setRightMotorPWM(motorPWM);
}

How about the performance tuning with speed:

    if (steer < -5)
    {
      // turn left
      turn = -1;
      uXbot_move(MAX_SPEED + steer, MAX_SPEED);
    } else if (steer > 5)
    {
      // turn right
      turn = 1;
      uXbot_move(MAX_SPEED, MAX_SPEED - steer);
    } else {
      // go straight
      turn = 0;
      uXbot_move(MAX_SPEED, MAX_SPEED);
    }

Part of the problem here is that the robot forward speed will change as it turns. Any turning motion will reduce the forward speed. The effect is clearly visible in the video you linked to. While this can help navigate sharp turns it messes with the dynamics somewhat.

If, instead, you make changes to BOTH wheel speeds together, the forward speed will remain constant. Clearly, there must be some headroom left for the outer wheel to be able to increase speed. With both wheels changing speed, the code fragment above would reduce to just:

uXbot_move(MAX_SPEED + steer, MAX_SPEED - steer);

That will make the response consistent but the controller may still have trouble adapting to different speeds. To sort that out, consider scaling the steering correction with the current speed. That is, the correction will be larger at higher speeds.

Thinking about the controller, I have three observations.

First, it is important that the PID function is called at regular, constant intervals. It just does not work well unless that is true. Your code does not specify that so it may already be true. Indeed, the existence of the division by PERIOD_MS indicates that you are aware of this.

Second, for the specific line following task that you describe, you do not need an I term in the controller. In fact, because you do nothing to limit integral windup, it may be doing more harm than good.

Finally, unless you are seriously limited for processor power, do your calculations in floating point as much as possible. Only reduce to an int when you actually calculate the PWM duty cycle for the timer register.

Answer 3

Nice robot.

The +/- 5 threshold in your code is a red flag to me. This introduces a non-linearity/discontinuity which shouldn't be needed. What is the rationale for that?

If you have enough memory to capture some real-time data try logging your cte and steer and then review them as you tune.

Speed is going to have an impact because of the way you've set up your steering and the vehicle dynamics. As the speed increases your steering command is going to cause a different turning radius. Also as speed increases it becomes hard to steer the vehicle. This isn't a trivial system. When your battery voltage goes down and assuming that voltage is your motor power tail and that you're not actually closing a velocity loop the effect of the lower voltage would be lower speed and therefore the effect of your PID coefficients will be reduced linearly to that. Note that going at a lower speed with a full battery and going at a low speed due to a low battery will behave differently (the impact of your steer command will be different).

Here's what I would suggest as a first go:

1. Get rid of that +5/-5 threshold.
2. Add some data logging in and review while tuning.
3. Start with I=0, D=0 and pick a fixed velocity. Start on the slow end.
4. Ramp up P, keep doubling it until the system becomes unstable. Back off (e.g. take half or 1/4 of the unstable P.)
5. Experiment with adding a little I or D and see if they improve performance. If not leave at 0.
6. Repeat tuning at a few speeds and plot your tuned variables vs. speed.
7. Try fitting linear or 2nd order segments to that graph to determine the coefficients at different speeds if needed.

Answer 4

After thinking about this problem a little more I had an important insight:

For the robots differential drive PID controller to be operational at different speeds, the turning angle R used for steering must be consistent over the whole speed range.

This post inspired my thinking and explains the turning angle of a differential drive robot: Calculate position of differential drive robot

Thanks to Guy Sirton and Ian for discussing my PID controller and providing useful debugging tips.