Régulateur PID terme intégral provoquant l'extrême instabilité

Question

I ai un régulateur PID en cours d'exécution sur un robot qui est conçu pour rendre le braquage du robot sur une position de la boussole. La correction PID est recalculée / appliqué à une vitesse de 20 Hz.

Bien que le régulateur PID fonctionne bien en mode PD (IE, avec le terme intégral zero'd out) la moindre quantité de l'intégrale va forcer la sortie instable de telle sorte que l'actionneur de direction est poussé vers la gauche ou extrême droite.

Code:

        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);
    }

Est-ce que quelqu'un a une idée pourquoi cela se passe ou comment y remédier?

Answer 1

Il semble que vous appliquez votre base de temps aux trois fois entiers. L'erreur est déjà l'erreur accumulée depuis le dernier échantillon si yo ne pas besoin de multiplier les temps de deltaTime il. Donc, je changerais le code à ce qui suit.

SteadyError += error ;

SteadyError est l'intégrale ou la somme de l'erreur.

Alors l'intégrale devrait juste être SteadyError * Ki

float integral = Ki * SteadyError;

Edit:

Je suis passé par votre code et il y a plusieurs autres éléments que je fixerait en plus de la solution ci-dessus.

1) Vous ne voulez pas le temps delta en millisecondes. Dans un système échantillonné normal, le terme delta serait un mais vous mettez une valeur comme 50 pour le taux de 20Hz ce qui a pour effet d'augmenter Ki par ce facteur et la diminution de Kd par un facteur de 50 aussi. Si vous êtes inquiet au sujet de la gigue alors vous devez convertir le temps delta à un temps d'échantillonnage relatif. J'utiliser la formule à la place.

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

le 500000,0 est le nombre de tiques attendues par échantillon pour 20 Hz est de 50 ms.

2) Maintenir le terme intégral dans une gamme.

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

3) Modifiez le code suivant de sorte que lorsque l'erreur est d'environ -180 vous ne recevez pas une étape dans l'erreur avec un petit changement.

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

4) Vérifier Steering.Degree reçoit la résolution correcte et signe.

5) Enfin yo peut probablement déposer deltaTime tous ensemble et calculer le différentiel terme de la manière suivante.

float derivative = Kd * (error - PrevError);

Avec tous que votre code devient.

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

Êtes-vous SteadyError initialise (nom bizarre ... pourquoi pas "intégrateur")? Si elle contient une valeur aléatoire au démarrage, il pourrait ne jamais revenir à près de zéro (1e100 + 1 == 1e100).

Vous pourriez souffrir de , ce qui devrait normalement aller, mais pas si il prend plus de temps à diminuer que le fait pour votre véhicule pour effectuer une rotation complète (et windup à nouveau l'intégrateur). La solution triviale est d'imposer des limites à l'intégrateur, mais il y a point fixe ). Vous devrez imposer un contrôle de limite, mais il sera beaucoup plus sain d'esprit que d'utiliser des flotteurs.

Answer 3

Le terme intégral est déjà accumulée au fil du temps, en multipliant par deltaTime le fera accumuler à un taux de temps au carré. En fait, depuis SteadyError est déjà calculée de manière erronée en multipliant par erreur deltaTime, qui est le temps-cubed!

Dans SteadyError, si vous essayez de compenser une mise à jour apériodique, il serait préférable de fixer le apériodicité. Toutefois, le calcul est erroné en tout cas. Vous avez calculé en unités d'erreur / heure alors que vous voulez seulement des unités d'erreur. si cela est vraiment nécessaire être la façon arithmentiaclly correcte pour compenser la gigue temporelle:

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

si deltaTime reste en millisecondes et le taux de mise à jour nominale est 20Hz. Cependant deltaTime serait mieux calculé comme un flotteur ou non converti en millisecondes du tout si elle est la gigue de synchronisation que vous essayez de détecter; vous terminez d'utiliser inutilement la précision. De toute façon ce que vous avez besoin est de modifier la valeur d'erreur par le rapport du temps nominal en temps réel.

Une bonne lecture est PID sans un doctorat

Answer 4

Je ne sais pas pourquoi votre code ne fonctionne pas, mais je suis presque certain que vous ne pouvez pas le tester pour voir pourquoi, que ce soit. Vous pouvez injecter un service de minuterie afin que vous puissiez se moquer et voir ce qui se passe:

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;
    }
}

Ensuite, remplacez les deux appels à DateTime.Now.Ticks avec GetCurrentTicks(), et vous pouvez parcourir le code et voir ce que les valeurs ressemblent.