PID-regler integral-term verursacht extreme Instabilität

Sieht es aus wie Sie sich bewerben Ihre Zeit Basis für die Integrale dreimal.
Fehler ist bereits der akkumulierten Fehler, die seit der letzten Probe, so dass yo nicht brauchen, um deltaTime multiplizieren es mal. Also ich würde den code wie folgt.

SteadyError += error ;

SteadyError ist das integral oder die Summe der Fehler.

Also das integral sollte nur SteadyError * Ki

float integral = Ki * SteadyError;

Edit:

Ich durchgemacht habe deinen code erneut ein und es gibt einige andere Elemente, die ich beheben würde zusätzlich zu den oben genannten fix.

1) Sie wollen nicht delta-Zeit in Millisekunden. In einer normalen Stichprobe-system die delta-Begriff wäre eine, aber Sie sind putting in einem Wert wie 50 für die 20 Hz-rate dies hat den Effekt der Verbesserung der Ki, die durch diesen Faktor und der abnehmenden Kd um einen Faktor von 50 als gut. Wenn Sie besorgt über die jitter-dann müssen Sie zum konvertieren von delta-Zeit, um eine relative Messung. Ich würde die Formel statt.

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

den 500000.0 ist die Anzahl der erwarteten ticks pro Probe, die bei 20 Hz liegt bei 50ms.

2) Halten Sie das integral-Begriff in einem Bereich.

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

3) Ändern Sie den folgenden code, so dass, wenn Fehler ist um -180 Sie nicht bekommen, ein Schritt in Fehler mit einer kleinen änderung.

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

4) Überprüfen Sie Die Steuerung.Grad ist der Erhalt der richtigen Auflösung und zu unterzeichnen.

5) Schließlich yo kann wohl nur drop-deltaTime alles zusammen, und berechnen Sie den differentiellen Ausdruck der folgenden Art und Weise.

float derivative = Kd * (error - PrevError);

Mit allen, dass Ihr 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;


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

Dem integral-Begriff ist bereits angesammelt im Laufe der Zeit, durch Multiplikation mit deltaTime wird es häufen sich in Höhe des Zeit-squared. In der Tat, da SteadyError ist bereits fälschlicherweise berechnet durch die Multiplikation Fehler durch deltaTime, das ist Zeit-cubed!

In SteadyError, wenn Sie versuchen, zu kompensieren, für eine aperiodische update, es wäre besser, zu beheben, der aperiodicity. Allerdings ist die Berechnung fehlerhaft auf jeden Fall. Sie errechnet haben, in Einheiten von Fehler - /Zeit-in der Erwägung, dass Sie nur Fehler Einheiten. Die arithmentiaclly richtige Art und Weise zu kompensieren-timing-jitter, wenn es wirklich notwendig wäre:

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

wenn deltaTime bleibt in Millisekunden und die nominal-update-rate 20 Hz. Allerdings deltaTime wäre besser berechnet als float oder nicht in Millisekunden, wenn es timing-jitter Sie versuchen, Sie zu erkennen; Sie sind unnötig verwerfen Präzision. So oder so, was Sie brauchen, ist zum ändern der Fehler-Wert durch das Verhältnis der nominalen Zeit der tatsächlichen Zeit.

Gut zu Lesen ist PID, ohne einen Doktortitel