Интегральный термин контроллера PID, вызывая крайнюю нестабильность

Question

У меня есть контроллер PID, работающий на роботе, который предназначен для того, чтобы робот рулялся на заголовок компаса. Коррекция PID пересчитывается/применяется со скоростью 20 Гц.

Хотя PID-контроллер хорошо работает в режиме PD (т. Е. С помощью неотъемлемого срока нуля), даже малейшее количество интеграла принудительно принудительно вывод нестабильна таким образом, что механизм рулевого управления находится либо влево или вправо.

Код:

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

У кого -нибудь есть идеи, почему это происходит или как это исправить?

Answer 1

Похоже, вы применяете свою временную базу в интеграл три раза. Ошибка уже накапливаемая ошибка, поскольку последний образец, так что вам не нужно, чтобы разумно разумно разумно. Поэтому я бы изменил код до следующего.

SteadyError += error ;

Устойчивая интенсивность является интегральной или суммой ошибки.

Итак, интеграл должен быть просто укрепителем * ки

float integral = Ki * SteadyError;

Редактировать:

Я снова прошел через ваш код, и есть несколько других предметов, которые я бы исправил в дополнение к вышеуказанному исправлению.

1) Вы не хотите Delta Time в миллисекундах. В нормальной выборке системы дельта -термин будет одним из них, но вы вкладываете значение, подобное 50 для скорости 20 Гц, это влияет на увеличение KI с помощью этого фактора и уменьшая KD в 50. Если вы беспокоитесь о джиттере, вам нужно преобразовать время Delta в относительное время. Я бы использовал формулу вместо этого.

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

500000.0 - это количество ожидаемых клещей на выборку, которая для 20 Гц составляет 50 мс.

2) Держите интегральный термин в пределах диапазона.

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

3) Измените следующий код, чтобы, когда об ошибке около -180, вы не получите шаг в ошибке с небольшим изменением.

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

4) Проверьте рулевое.

5) Наконец, вы, вероятно, можете просто сбросить Deltatime все вместе и рассчитать дифференциальный термин следующим образом.

float derivative = Kd * (error - PrevError);

Со всем этим становится ваш код.

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

Вы инициализируете SteadyError (странное имя ... почему не "интегратор")? Если он содержит некоторое случайное значение при запуске, он может никогда не вернуться к нулю (1e100 + 1 == 1e100).

Вы можете страдать от Интегратор Windup., который обычно должен уйти, но не если это занимает больше времени для уменьшения, чем для вашего автомобиля, чтобы завершить полное вращение (и снова включить интегратор). Тривиальное решение состоит в том, чтобы наложить ограничения на интегратор, хотя есть более продвинутые решения (PDF, 879 кб), если ваша система требует.

Делает Ki есть правильный знак?

я буду сильно Объедините использование поплавков для параметров PID из -за их произвольной точности. Используйте целые числа (возможно фиксированная точка) Вам придется навязывать проверку пределов, но она будет намного более вменяемо, чем использовать поплавки.

Answer 3

Интегральный термин уже накапливается с течением времени, умножение на Deltatime заставит его накапливаться со скоростью квадрата. Фактически, поскольку устойчиво-экологически чистый уже ошибочно рассчитывается путем умножения ошибки на Deltatime, это заполнено времени!

В StudyError, если вы пытаетесь компенсировать обновление апериодического, было бы лучше исправить апериодичность. Однако расчет ошибочен в любом случае. Вы рассчитали в единицах ошибки / времени, тогда как вы хотите просто единицы ошибок. Арифмементический правильный способ компенсировать временную дрожь, если действительно необходимо:

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

Если Deltatime остается в миллисекундах, и номинальная скорость обновления составляет 20 Гц. Однако Deltatime будет лучше рассчитать как поплавок или не преобразуется в миллисекунды вообще, если он промежуточный джиттер, вы пытаетесь обнаружить; Вы без необходимости отбрасывать точность. В любом случае, то, что вам нужно, это изменять значение ошибки по отношению коэффициентом номинального времени на фактическое время.

Хороший читал PID без кандидата

Answer 4

Я не уверен, почему ваш код не работает, но я почти уверен, что вы не можете проверить его, чтобы понять, почему. Вы могли бы ввести услугу таймера, чтобы вы могли издеваться над этим и посмотреть, что происходит:

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

Затем замените оба вызова на DateTime.Now.Ticks с GetCurrentTicks(), И вы можете пройти через код и посмотреть, как выглядят значения.