Gain Scheduling

Some systems may fail with a simple linear controller such as PID. Let's fix that!

Why Gain Scheduling?

Controllers like PID and Full State Feedback are inherently linear. They compose of simple operations such as multiplication, subtraction, integration, and derivative calculation. This means that the controllers are easy to design and understand. Unfortunately, we live in the real world. In the real world, there is almost never such a thing as a linear system. We are generally able to assume that our system is linear enough, but sometimes, this simply isn't true. This is where gain scheduling comes into play.

What is Gain Scheduling?

Gain scheduling is the idea where we modify our controller parameters relative to where our system is in space. Imagine a rotating arm on your robot. You begin by tuning your controller to stabilize in an upright position such as this:

You are happy with the performance of having the arm swing up to the vertical position, so you proceed to test it in a horizontal position:

You find that after requesting your arm to move to 0°, it ends up settling below your target angle. Initially, you are confused, as it worked perfectly for the 90° setpoint. But then, you remembered that your system is not linear! As a result of your system being nonlinear, the linear controller you tuned for one setpoint may not cover the dynamics of the entire operating region.

Operating Region: the space in which the system operates. If you only operate your system in a small region, it is more likely that a linear controller will be adequate.

A solution to this problem and the inherent concept of gain scheduling is that we can change our controller coefficients depending on where our system is in the operating region.

Let's say that this is our current code:

// Imaginary PID controller and coefficient objects
PIDCoefficients for90Degrees = new PIDCoefficients();
PIDController controller = new PID(for90Degrees);


while (loopIsRunning) {
    double armAngle = getArmAngle();
    double targetAngle = getReference();
    double output = controller.calculate(targetAngle, armAngle);
    arm.setPower(output);
}

The code sample above has its PID coefficients tuned to operate at the 90° range. As we saw with the previous example, it fails whenever we go to 0°.

One approach we can take is to split our operating region into two halves — suppose, in the middle, at 45°. Then, we will use one set of coefficients above 45° and one set below.

// imaginary PID controller and coefficients objects
PIDCoefficients above45Degrees = new PIDCoefficients(); // tune for above
PIDCoefficients below45Degrees = new PIDCoefficients(); // tune for below
PIDController controller = new PID(for90Dgrees);

while (loopIsRunning) {
    double armAngle = getArmAngle();
    double targetAngle = getReference();
    if (armAngle < 45) {
        controller.setCoefficients(below45Degrees);
    } else {
        controller.setCoefficients(above45Degrees);
    }
    double output = controller.calculate(targetAngle, armAngle);
    arm.setPower(output);
}

This approach works well but doesn't necessarily scale the best. If you had 10 different operating regions, that means you would have to create an if/else statement for each one. That would get messy very quickly. In addition, setting your target angle to 45° could have a bit of extra oscillation as it switches gains every time it passes over the switching point.

An approach to improve this is to use interpolation.

A really simple way to accomplish this is to use the FTCLib InterpLUT (Interpolated Look Up Table).

An interpolated look-up table will take a key (the current angle) and then give you an estimate of what our PID coefficients should be at the given point. It does this by allowing us to put in a few known values, and then it will generate a continuous function to go between them.

// look up tables for PID coefficients
InterpLUT pCoefficients = new InterpLUT();
InterpLUT iCoefficients = new InterpLUT();
InterpLUT dCoefficients = new InterpLUT();

pCoefficients.add(0, KpAt0);
pCoefficients.add(90, KpAt90);

iCoefficients.add(0, KiAt0);
iCoefficients.add(90, KiAt90);

dCoefficients.add(0, KdAt0);
dCoefficients.add(90, KdAt90);

pCoefficients.createLUT();
iCoefficients.createLUT();
dCoefficients.createLUT();

while (loopIsRunning) {
    double armAngle = getArmAngle();
    double targetAngle = getReference();

    double Kp = pCoefficients.get(armAngle);
    double Ki = iCoefficients.get(armAngle);
    double Kd = dCoefficients.get(armAngle);

    PIDCoefficients coefficients = new PIDCoefficients(Kp, Ki, Kd);
    controller.setCoefficients(coefficients);

    double output = controller.calculate(targetAngle, armAngle);
    arm.setPower(output);
}

You will now have a very robust controller powered by gain scheduling!

You aren't limited to just PID. You can also use this method to adjust your Feedforward or use Full State Feedback instead of PID.