Интегральный термин ПИД-регулятора, вызывающий крайнюю нестабильность
У меня есть PID-контроллер, работающий на роботе, который предназначен для того, чтобы робот направился в направлении компаса. Поправка ПИД пересчитывается / применяется с частотой 20 Гц.
Несмотря на то, что ПИД-регулятор работает хорошо в режиме 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);
}
У кого-нибудь есть идеи, почему это происходит или как это исправить?
4 ответа
Похоже, вы применяете свою временную базу к интегралу три раза. Ошибка - это уже накопленная ошибка с момента последней выборки, поэтому вам не нужно умножать ее на 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'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);
}
Вы инициализируете SteadyError (причудливое название... почему не "интегратор")? Если он содержит какое-то случайное значение при запуске, он может никогда не вернуться к нулю (1e100 + 1 == 1e100).
Возможно, вы страдаете от нарастания интегратора, который обычно должен уходить, но не тогда, когда для его уменьшения требуется больше времени, чем для вашего транспортного средства, чтобы завершить полный оборот (и снова намотать интегратор). Тривиальное решение заключается в наложении ограничений на интегратор, хотя существуют более продвинутые решения (PDF, 879 кБ), если это требуется для вашей системы.
Есть ли Ki есть правильный знак?
Я бы настоятельно не рекомендовал использовать float для параметров PID из-за их произвольной точности. Используйте целые числа (возможно, с фиксированной точкой). Вам нужно будет наложить ограничение проверки, но это будет гораздо более разумным, чем использование поплавков.
Интегральный член уже накапливается с течением времени, умножение на deltaTime заставит его накапливаться со скоростью квадрата времени. Фактически, так как SteadyError уже ошибочно вычислен умножением ошибки на deltaTime, то это время!
В SteadyError, если вы пытаетесь компенсировать апериодическое обновление, было бы лучше исправить апериодичность. Однако расчет ошибочен в любом случае. Вы рассчитали в единицах ошибки / времени, тогда как вы хотите просто единицы ошибки. Арифметически правильным способом компенсации джиттера синхронизации, если это действительно необходимо, будет:
SteadyError += (error * 50.0f/deltaTime);
если deltaTime остается в миллисекундах, а номинальная частота обновления составляет 20 Гц. Однако deltaTime будет лучше рассчитываться как число с плавающей точкой или вообще не конвертироваться в миллисекунды, если вы пытаетесь обнаружить джиттер синхронизации; Вы без необходимости отбрасываете точность. В любом случае вам нужно изменить значение ошибки на отношение номинального и фактического времени.
Хорошее чтение - PID без доктора философии
Я не уверен, почему ваш код не работает, но я почти уверен, что вы не можете проверить его, чтобы понять почему. Вы можете добавить сервис таймера, чтобы смоделировать его и посмотреть, что происходит:
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(), и вы можете пройтись по коду и посмотреть, как значения выглядят.