Как настроить ПИД-регулятор для гоночного робота?

Что такое ПИД-регулятор? Как сделать расчет рассогласования и обратную связь для движущегося по линии робота? Как не перерегулировать? Рассказывает Вячеслав Нефедов (GoodLancer.com).

Абсолютное большинство современных роботов для гонок по линии высокого класса как основу алгоритма движения по линии используют ПИД-регулятор (он же ПИД-контроллер). В то же время для многих юных поклонников робогонок ПИД-регулятор остается сложным и малопонятным механизмом. Давайте попробуем вместе разобраться что такое ПИД-регулятор и как он работает.

Что такое «регулятор»?

Само понятие ПИД-регулятор пришло из теории автоматического управления. Кто читает по-английски, то там эта дисциплина называется control theory. Эта наука занимается исследованием систем автоматического управления, а такие системы мы встречаем на каждом шагу. Автопилоты, системы наведения торпед и ракет, системы управления температурой в химическом производстве, регуляторы положения поглощающих стержней в атомном реакторе, система регулирования температуры в кондиционерах, поплавковый клапан в сливном бачке унитаза — это всё системы автоматического управления.

Общая схема системы автоматического управления нарисована ниже, а основными понятиями такой системы будут следующие:

1. Объект управления — это то, чем управляет система. В случае гоночного робота это сам робот, а точнее, положение робота относительно линии;
2. Уставка (цель, целевое значение) — это то условие, за соблюдением которого «смотрит» автомат. Для нашего случая целью является то, чтобы робот не отклонялся от линии;
3. Рассогласование (ошибка, ошибка управления) — это отклонение текущего состояния от желаемого. В нашем случае это отклонение робота от линии;
4. Датчики — то, чем мы определяем рассогласование. Для гоночного робота это обычно «линейка» — система из датчиков серого (пара светодиод/фототранзистор);
5. Обратная связь — сигнал с датчиков, ориентируясь на который автомат принимает решение об управляющем воздействии;
6. Регулятор. Ключевой элемент системы. Именно регулятор «принимает решение» о том что сделать, чтобы система вернулась к цели. В сливном бачке это механическая система, в случае гоночного робота это часть программы, реализующая ПИД-алгоритм;
7. Управление (управляющее воздействие, корректировка) — это действия, которые принимает система, чтобы вернуться к цели. В нашем случае это изменение скорости моторов робота;
8. Привод. Это та часть системы, которая реализует управляющее воздействие. В нашем случае это моторы.

Для демонстрации работы ПИД-контроллера дальше мы используем робота нашего клиента Марка из Казахстана.

Расчет рассогласования

Первый расчетный блок в этом перечне — расчет рассогласования. Как пример расчета согласования можно взять такой: считаем, что в нашей линейке датчиков каждый датчик даёт выходной сигнал, например, 200 на белом поле и 1000 на черной линии. В этом случае мы можем считать, что если датчик показывает больше, чем 600, то он находится над линией. Меньше или равно 600 — над белым полем.

Этот алгоритм для системы из восьми датчиков линии на Arduino будет выглядеть примерно так:


float bot_position()
{
int posSum = 0;
int posMedian = 0;
int signal[8];
int signal_corrected;
qtr.read(signal);
for (int i = 0; i < 8; i++) { if (signal[i] > 600)
{
signal_corrected = 1;
}
else
{
signal_corrected = 0;
}
posSum += signal_corrected ;
posMedian += signal_corrected ;
}
return posMedian / posSum - 7;
}


Здесь signal — массив для хранения результатов замера сигнала с каждого из восьми датчиков, а qtr.read(signal) — это функция, которая заполняет массив значениями с датчиков. Нетрудно убедиться, что данная функция вернет 0 в том случае, если линия находится точно посередине линейки датчиков и положительное или отрицательное значение — в случае, если робот отклонился вправо или влево.

Это сильно упрощённый расчет рассогласования, который обычно требует большого количества датчиков (8-14 на робогонках). Есть другой вариант — рассчитывать дробное значение положения робота на линии, пользуясь тем, что наш датчик по мере приближения с белого поля к черной линии будет показывать промежуточные значения между 200 и 600. В каких-то ситуациях при такой схеме даже робот с массивом из трёх датчиков линии может показывать хороший результат.

Обратная связь

Второй расчетный блок — обратная связь. Для простого робота обычно обратная связь реализована таким образом — регулятор рассчитывает величину поправки, которая добавляется к скорости одного мотора и вычитается из скорости другого мотора.

Например, задаем среднюю скорость для моторов как ШИМ 150 при 255 — максимальном ШИМ для нашего контроллера. Далее если рассчитанное управление равно 50, то мы к правому мотору добавляем 50 (получаем ШИМ 200), от левого вычитаем 50 (получаем ШИМ 100). В случае, если рассчитанное управление будет отрицательным, -50, то у нас наоборот левый мотор будет двигаться быстрее.

П-регулятор

Буква «П» в названии П-регулятора означает слово «пропорциональный». П-регулятор берет в расчет текущее отклонение робота от линии и рассчитывает управление пропорционально текущему отклонению.

Например, наш расчет рассогласования написан таким образом, что смещение робота на пол-датчика в сторону от линии выдает ошибку рассогласования в одну единицу. Т.е. в случае если робот отклонился на полдатчика, то мы получим ошибку 1, если на один датчик, то ошибка будет 2, а если робот сместится на два датчика в сторону, то величина ошибки на выходе у нашей функции bot_position() будет четыре.

В этом случае мы можем написать П-регулятор так:


int avgSpeed = 150; // средняя скорость моторов
int kP = 10; // коэффициент пропорциональной обратной связи
int error; // Это ошибка положения
error = bot_position();
correction = kP * error;
motor1.move(avgSpeed*(1+correction));
motor2.move(avgSpeed*(1-correction));


Здесь функции motor1.move() и motor2.move() будут управлять скоростью левого и правого моторов, от -255 до 255. Видно, что если робот сместится в сторону на полдатчика, то скорости моторов изменятся, один мотор получит на вход ШИМ 165, а другой 135 и робот начнет двигаться по дуге. Представим, что линия под роботом делает резкий поворот и радиус движения робота все равно оказался недостаточным, чтобы вернуться на линию. В этом случае робот скоро сместиться от линии в сторону уже на целый датчик, ошибка станет равна двум, а коррекция — 20. Скорости моторов изменятся и станут равны 180 и 120 соответственно и робот еще активнее попытается вернуться на линию. Т.е. чем резче поворачивает линия под роботом, тем больше робот будет смещаться в сторону и тем больше будет разница скорости между моторами.

Как мы подберем коэффициент kP? Робот с низким kP называется недорегулированным. Такой робот не умеет поворачивать достаточно резко для того, чтобы оставаться на линии. Робот с высоким kP называется перерегулированным. Он начинает «дергаться» и двигаться зигзагами, отклоняясь от линии то вправо, то влево. На видео выше видно, что робот немного перерегулирован — он двигается не «как влитой», а колеблется, причем бывает, что колебаниями линейку отводит влево от линии, несмотря, что робот в этот момент совершает поворот влево. Нормально отрегулированный робот с П-контроллером всегда поворачивает вправо когда нос находится слева от линии и влево — когда нос находится справа от линии, т.к. он «ищет потерянную линию».

Давайте посмотрим на робота со значительно перерегулированным П-контроллером и мы увидим, что он колеблется постоянно:

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

ПД-регулятор

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

Д-регулятор (дифференциальный регулятор) работает таким образом — считается изменение рассогласования за некоторый промежуток времени, а дальше необходимое управление считается как произведение этого изменения на некий коэффициент, который мы обозначим kD. Мы получим ПД-регулятор добавив Д-регулятор к уже имеющемуся П-регулятору.


int prev_error;
void loop(void)
{
int avgSpeed = 150; // средняя скорость моторов
int kP = 10; // коэффициент пропорциональной обратной связи
int kD = 1; // коэффициент дифференциальной обратной связи
int error; // Это ошибка положения
error = bot_position();
correction = kP * error + kD * (error-prev_error);
prev_error = error;
motor1.move(avgSpeed*(1+correction));
motor2.move(avgSpeed*(1-correction));
delay(10);
}


Мы добавили здесь переменную prev_error, расчёт дифференциальной коррекции kD * (error-prev_error), а также задержку в цикле — delay(10). Д-регулятор как видно из формулы, борется с высокой скоростью изменений ошибки. Т.е. он борется с инерцией и с колебаниями, и является обязательным для роботов, которые двигаются с высокими скоростями.

Недостаток Д-регулятора — это то, что в нашем случае он требует задержки в цикле управления. Значение ошибки error в нашем случае меняется редко, надо чтобы робот проехал какое-то расстояние чтобы ошибка изменилась, поэтому если мы задержку не введём, то Д-регулятор будет действовать очень кратковременно, только в тот короткий момент когда робот будет переходить с датчика на датчик. Величина задержки в цикле должна быть достаточной, чтобы за этот момент значение ошибки могло измениться на пару датчиков. Но то, что хорошо для Д-регулятора, то плохо для П-регулятора, который потеряет возможность быстро реагировать на изменение положения робота.

Второй недостаток Д-регулятора — подверженность шумам. Случайные скачки освещенности могут привести к неожиданному его срабатыванию.

Давайте рассмотрим более профессиональный код ПД-регулятора для нашего робота:

int avgSpeed = 150; // средняя скорость моторов
int kP = 10; // коэффициент пропорциональной обратной связи
int kD = 5; // коэффициент дифференциальной обратной связи
int correction;
int err;
int err_arr[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int err_p = -1;
prevErr = bot_position();
void loop(void)
{
err = bot_position();
err_p = (err_p + 1) % 10;
err_arr[err_p] = err;
P = err * KP;
D = (err_arr[err_p] - err_arr[(err_p+11) % 10])*KD;
correction = P + D;
motor1.move(avgSpeed*(1+correction));
motor2.move(avgSpeed*(1-correction));
delay(2);
}

В данном примере последние десять ошибок хранятся в массиве err_arr и Д-регулятор берет за основу вычисления коррекции разницу между текущим значением ошибки и значением ошибки 20 миллисекунд назад (время цикла 2мс * 10, где 10 — количество элементов в массиве). Д-регулятор позволяет значительно повысить скорость движения робота.

Если значение коэффициента обратной дифференциальной связи слишком велико, то робот с ПД-контроллером начнет «злиться» или «нервничать» — для такого робота характерны высокочастотные колебания, причем если для перерегулированной связи по «П» характерны колебания вправо-влево от желаемого направления движения, то перерегулировка по «Д» приводит к тому, что робот просто «дрожит».

Давайте уменьшим коэффициент kD и мы увидим, что робот стал идти намного ровнее, чем на П-контроллере, более того, теперь мы можем даже уменьшить коэффициент kP и робот все-равно будет успешно проходить трассу. Этот робот может на ПД-контроллере идти более чем вдвое быстрее, чем на П-контроллере.

Видно, что робот стал ехать как будто на хорошо настроенном П-контроллере, хотя мы знаем, что на такой скорости П-контроллер с этим роботом бы не справился. Или его пришлось бы сильно перерегулировать и это было бы заметно по его колебаниям. Д-контроллер в нашем случае как-будто делает двигатели более мощными, улучшает баланс робота, и повышает точность линейки.

И-регулятор

Обратите внимание на предыдущем видео — робот в повороте движется «на крайних датчиках», сильно смещаясь от середины линии. Это особенность работы П-регулятора, для его корректной работы нужно чтобы робот двигался с ошибкой, а если нет ошибки, то нет и коррекции.

Тем не менее, есть простой регулятор, который отвечает как раз за движение робота в точности по центру линии. Это И-регулятор (интегральный регулятор). Посмотрите на видео ниже — робот с хорошо настроенным И-регулятором движется уже четко посередине линии, даже в повороте.

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


int avgSpeed = 150; // средняя скорость моторов
int kP = 10; // коэффициент пропорциональной обратной связи
int kD = 5; // коэффициент дифференциальной обратной связи
int kI = 5; // коэффициент интегральной обратной связи
int correction;
int err;
int err_arr[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int err_p = -1;
prevErr = bot_position();
void loop(void)
{
err = bot_position();
err_p = (err_p + 1) % 10;
err_arr[err_p] = err;
P = err * KP;
D = (err_arr[err_p] - err_arr[(err_p+11) % 10])*KD;
int err_sum = 0;
for (int i = 0; i < 10; i++) err_sum += err_arr[i];
I = err_sum/10*KI;
correction = P + I + D;
motor1.move(avgSpeed*(1+correction));
motor2.move(avgSpeed*(1-correction));
delay(2);
}


В этом коде вы уже видите полноценный ПИД-регулятор. Робот с настроенным ПИД-регулятором уже идет посередине линии. А что будет если И-регулятор перенастроить, задать слишком большие значения коэффициента kI? В этом случае I-регулятор будет мешать работать П-регулятору и робот опять начнет дёргаться, как это видно на видео ниже.

Порядок настройки ПИД-регулятора

Обычный порядок настройки робота такой:

1. На невысокой скорости настраиваем П-регулятор (подбираем значение kP такое, чтобы в самых крутых поворотах робот проходил держа линию близко к своим крайним датчикам). kD и kI при этом равны нулю, т.е. пользуемся чисто П-регулятором;
2. Повышаем скорость, подбираем значение kD. Если при настройке П-регулятора робот шел без инерции, то значение kP можно не менять. Если робот уже шёл с инерцией, как это обычно бывает у быстрых роботов, то значение kP надо будет понизить — мы это увидим по тому, что робот прекратит сильно отклоняться от линии благодаря помощи Д-регулятора;
3. Когда ПД-регулятор настроен, то можно подбирать значение I, уменьшая отклонения робота от линии. Значения коэффициентов kD и KP при этом обычно тоже несколько понижаются. И-регулятор полезен для гонок где линия может делать петли. Отклонение робота от прямой при этом чревато выбором ошибочного направления движения. При гонках на трассах без петель зачастую используется ПД-регулятор, так как в общем случае он позволяет развивать более высокую скорость.

Источник: goodlancer.com.

Теги: Вячеслав Нефедов, ПИД-регулятор, уроки