Целевой состав ПИД-регулятора, вызывающий экстремальную нестабильность

Ответ 1

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

SteadyError += error ;

SteadyError - интеграл или сумма ошибок.

Таким образом, интеграл должен быть просто SteadyError * Ki

float integral = Ki * SteadyError;

Изменить:

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

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

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) Убедитесь, что Steering.Degree получает правильное разрешение и знак.

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

Ответ 2

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

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

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

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

Ответ 3

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

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

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

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

Хорошее чтение PID без PhD

Ответ 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(), и вы можете пройти через код и посмотреть, как выглядят значения.