controlador PID término integral causando inestabilidad extrema

Question

Tengo un controlador PID que se ejecuta en un robot que está diseñado para hacer que el robot de dirección en una dirección de la brújula. La corrección PID se vuelve a calcular / aplicado a una velocidad de 20 Hz.

Aunque el controlador PID funciona bien en el modo de PD (es decir, con el término integral zero'd fuera) incluso la más mínima cantidad de integral forzará la salida inestable de tal manera que el actuador de la dirección es empujado a la izquierda o extrema derecha.

Código:

        private static void DoPID(object o)
    {
        // Bring the LED up to signify frame start
        BoardLED.Write(true);

        // Get IMU heading
        float currentHeading = (float)RazorIMU.Yaw;

        // We just got the IMU heading, so we need to calculate the time from the last correction to the heading read
        // *immediately*. The units don't so much matter, but we are converting Ticks to milliseconds
        int deltaTime = (int)((LastCorrectionTime - DateTime.Now.Ticks) / 10000);

        // Calculate error
        // (let's just assume CurrentHeading really is the current GPS heading, OK?)
        float error = (TargetHeading - currentHeading);

        LCD.Lines[0].Text = "Heading: "+ currentHeading.ToString("F2");

        // We calculated the error, but we need to make sure the error is set so that we will be correcting in the 
        // direction of least work. For example, if we are flying a heading of 2 degrees and the error is a few degrees
        // to the left of that ( IE, somewhere around 360) there will be a large error and the rover will try to turn all
        // the way around to correct, when it could just turn to the right a few degrees.
        // In short, we are adjusting for the fact that a compass heading wraps around in a circle instead of continuing
        // infinity on a line
        if (error < -180)
            error = error + 360;
        else if (error > 180)
            error = error - 360;

        // Add the error calculated in this frame to the running total
        SteadyError = SteadyError + (error * deltaTime);

        // We need to allow for a certain amount of tolerance.
        // If the abs(error) is less than the set amount, we will
        // set error to 0, effectively telling the equation that the
        // rover is perfectly on course.
        if (MyAbs(error) < AllowError)
            error = 0;

        LCD.Lines[2].Text = "Error:   " + error.ToString("F2");

        // Calculate proportional term
        float proportional = Kp * error;

        // Calculate integral term
        float integral = Ki * (SteadyError * deltaTime);

        // Calculate derivative term
        float derivative = Kd * ((error - PrevError) / deltaTime);

        // Add them all together to get the correction delta
        // Set the steering servo to the correction
        Steering.Degree = 90 + proportional + integral + derivative;

        // We have applied the correction, so we need to *immediately* record the 
        // absolute time for generation of deltaTime in the next frame
        LastCorrectionTime = DateTime.Now.Ticks;

        // At this point, the current PID frame is finished
        // ------------------------------------------------------------
        // Now, we need to setup for the next PID frame and close out

        // The "current" error is now the previous error
        // (Remember, we are done with the current frame, so in
        // relative terms, the previous frame IS the "current" frame)
        PrevError = error;

        // Done
        BoardLED.Write(false);
    }

¿Alguien tiene alguna idea de por qué esto está ocurriendo o cómo solucionarlo?

Answer 1

Parece que usted está solicitando su base de tiempo para las integrales tres veces. Error ya es el error acumulado desde la última muestra de modo yo no necesita multiplicar DeltaTime veces. Así que me gustaría cambiar el código a la siguiente.

SteadyError += error ;

SteadyError es la integral o la suma de errores.

Así que la integral debe ser sólo SteadyError * Ki

float integral = Ki * SteadyError;

Editar:

He ido a través de su código de nuevo y hay varios otros artículos que iba a arreglar, además de la revisión anterior.

1) Usted no quiere delta de tiempo en milisegundos. En un sistema muestreado normal, el término delta sería uno, sino que están poniendo en un valor como 50 para la tasa de 20Hz esto tiene el efecto de aumentar Ki por este factor y la disminución de Kd en un factor de 50 también. Si usted está preocupado por la fluctuación entonces usted necesita para convertir el tiempo delta a un tiempo de la muestra relativa. Me gustaría utilizar la fórmula en su lugar.

float deltaTime = (LastCorrectionTime - DateTime.Now.Ticks) / 500000.0

el 500000.0 es el número de garrapatas esperados por muestra que para 20 Hz es de 50 ms.

2) Mantener el término integral dentro de un rango.

if ( SteadyError > MaxSteadyError ) SteadyError = MaxSteadyError;
if ( SteadyError < MinSteadyError ) SteadyError = MinSteadyError;

3) Cambiar el código siguiente para que cuando el error es de alrededor de -180 usted no consigue un paso en error con un pequeño cambio.

if (error < -270) error += 360;
if (error >  270) error -= 360;

4) Verificar Steering.Degree está recibiendo la resolución y la señal correcta.

5) Por último yo probablemente puede simplemente dejar DeltaTime todos juntos y calcular el término diferencial de la siguiente manera.

float derivative = Kd * (error - PrevError);

Con todo esto se convierte en su código.

private static void DoPID(object o)
{
    // Bring the LED up to signify frame start
    BoardLED.Write(true);

    // Get IMU heading
    float currentHeading = (float)RazorIMU.Yaw;


    // Calculate error
    // (let's just assume CurrentHeading really is the current GPS heading, OK?)
    float error = (TargetHeading - currentHeading);

    LCD.Lines[0].Text = "Heading: "+ currentHeading.ToString("F2");

    // We calculated the error, but we need to make sure the error is set 
    // so that we will be correcting in the 
    // direction of least work. For example, if we are flying a heading 
    // of 2 degrees and the error is a few degrees
    // to the left of that ( IE, somewhere around 360) there will be a 
    // large error and the rover will try to turn all
    // the way around to correct, when it could just turn to the right 
    // a few degrees.
    // In short, we are adjusting for the fact that a compass heading wraps 
    // around in a circle instead of continuing infinity on a line
    if (error < -270) error += 360;
    if (error >  270) error -= 360;

    // Add the error calculated in this frame to the running total
    SteadyError += error;

    if ( SteadyError > MaxSteadyError ) SteadyError = MaxSteadyError;
    if ( SteadyError < MinSteadyError ) SteadyError = MinSteadyError;

    LCD.Lines[2].Text = "Error:   " + error.ToString("F2");

    // Calculate proportional term
    float proportional = Kp * error;

    // Calculate integral term
    float integral = Ki * SteadyError ;

    // Calculate derivative term
    float derivative = Kd * (error - PrevError) ;

    // Add them all together to get the correction delta
    // Set the steering servo to the correction
    Steering.Degree = 90 + proportional + integral + derivative;

    // At this point, the current PID frame is finished
    // ------------------------------------------------------------
    // Now, we need to setup for the next PID frame and close out

    // The "current" error is now the previous error
    // (Remember, we are done with the current frame, so in
    // relative terms, the previous frame IS the "current" frame)
    PrevError = error;

    // Done
    BoardLED.Write(false);
}

Answer 2

¿Usted está inicializando SteadyError (nombre extraño ... ¿por qué no "integrador")? Si contiene algún valor aleatorio en la puesta en marcha que nunca podría volver a cerca de cero (1e100 + 1 == 1e100).

podría estar sufriendo de integrador de cuerda , que normalmente debería desaparecer, pero no si se necesita más tiempo para disminuir de lo que hace para su vehículo para completar una rotación completa (y el integrador de cuerda de nuevo). La solución trivial es imponer límites en el integrador, aunque hay soluciones más avanzadas (PDF, 879 kB) si su sistema requiere.

¿El Ki tienen el signo correcto?

Me muy desalentar el uso de flotadores para los parámetros PID debido a su precisión arbitraria. Use números enteros (punto quizá fijo). Usted tiene que imponer la comprobación de límite, pero será mucho más sano que el uso de flotadores.

Answer 3

El término integral ya se acumula con el tiempo, multiplicando por DeltaTime hará que se acumule a un ritmo de tiempo-cuadrado. De hecho, desde SteadyError ya está aplicado erróneamente por un error por DeltaTime multiplicación, es decir el tiempo en cubos!

En SteadyError, si usted está tratando de compensar una actualización no periódica, que sería mejor para solucionar el aperiodicidad. Sin embargo, el cálculo es erróneo, en cualquier caso. Ha calculado en unidades de error / hora mientras que usted quiere unidades de error sólo. La forma correcta arithmentiaclly para compensar saltos temporales cuando sea realmente necesario sería:

SteadyError += (error * 50.0f/deltaTime);

Si DeltaTime permanece en milisegundos y la frecuencia de actualización nominal es de 20 Hz. Sin embargo DeltaTime estaría mejor calcula como un flotador o no convertir en milisegundos en absoluto si es tiempo de fluctuación que está tratando de detectar; usted va a desechar innecesariamente precisión. De cualquier manera lo que necesita es modificar el valor de error por la relación del tiempo nominal en tiempo real.

Una buena lectura es PID sin un doctorado

Answer 4

No estoy seguro de por qué su código no está trabajando, pero estoy casi seguro de que no se puede probar para ver por qué, tampoco. Es posible inyectar un servicio de temporizador para que pueda burlarse de él hacia fuera y ver lo que está pasando:

public interace ITimer 
{
     long GetCurrentTicks()
}

public class Timer : ITimer
{
    public long GetCurrentTicks() 
    {
        return DateTime.Now.Ticks;
    }
}

public class TestTimer : ITimer
{
    private bool firstCall = true;
    private long last;
    private int counter = 1000000000;

    public long GetCurrentTicks()
    {
        if (firstCall)
            last = counter * 10000;
        else
            last += 3500;  //ticks; not sure what a good value is here

        //set up for next call;
        firstCall = !firstCall;
        counter++;

        return last;
    }
}

A continuación, cambie ambas llamadas a DateTime.Now.Ticks con GetCurrentTicks(), y se puede recorrer el código y ver lo que los valores parecen.