ПИД-регулятор для гоночного робота

ПИД-регулятор для гоночного робота
13.01.2019 Вячеслав Нефедов

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

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

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

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

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

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

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

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

float bot_position()
{
  float posSum = 0;
  float posMedian = 0;
  float signal[8];
  float 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 * i * 2;
  }

  if (posSum > 0)
  {
    return posMedian / posSum - 7;
  }
  else
  {
    return 0;
  }
}

Здесь 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-регулятор будет мешать работать П-регулятору и робот опять начнёт дёргаться, как это видно на видео ниже. Обычно коэффициент kP несколько понижают в случае ПИД-регулятора по сравнению с ПД-регулятором, в такой схеме инерционный регулятор помогает пропорциональному.

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

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

Что дальше?

Описанные выше алгоритмы — это маленькая часть того, что используется на серьёзных роботах. У нас в планах продолжить цикл статей о ТАУ для гоночных роботов и я буду благодарен за любую обратную связь. В этой версии статьи раздел с описанием рассчёта рассогласования поправлен с учётом ценных замечаний от Эдуарда Петренко (клуб спортивной робототехники МФТИ).

Что часто используется, но чего пока нет в этой статье:

  1. Обработка схода с линии — в этом случае робот делает резкий поворот для поиска линии;
  2. Нелинейный пропорциональный регулятор. Пропорциональным регулятором называют регулятор, зависящий только от текущей ошибки. Но вот сам расчёт ошибки (или обратная связь) могут быть и нелинейными. Например, в ситуации когда линия содержит длинные участки с очень плавными поворотами, плюс резкие повороты. В этом случае ПР разрабатывают таким образом, что он даёт плавные повороты при небольшой ошибке, но резкие повороты при значительной;
  3. Регулятор скорости. В статье описан регулятор положения. Часто это не единственный автомат в гоночном роботе. Используются автоматы регулировки скорости вращения прижимной турбины, автомат скорости вращения колёс (в случае наличия датчиков скорости — колёсных энкодеров), но второй по частоте использования автомат после регулятора положения — регулятор скорости. Рассогласованием в этом случае считается накопленная сумма модулей ошибки. Чем больше робот ошибается — тем ниже падает его скорость. На прямой такой робот будет разгоняться;
  4. Нечёткие регуляторы. Такие регуляторы являются альтернативой ПИД-регуляторам;
  5. Аналоговая обработка ошибки с датчиков линии — сигнал с датчиков линии преобразуется не в дискретную ошибку (0 или 1), а в непрерывную — дробное число между 0 и 1;
  6. Адаптивные регуляторы — робот ведёт счисление пройденного пути и направления линии (второе — с помощью датчика направления, IMU) и запоминает профиль трассы, далее корректируя скорость, смещение от линии и ПИД в зависимости от типа текущего участка трассы;
  7. Защита датчиков линии от засветки и процедуры защиты от световых шумов (усреднение нескольких замеров ошибки, например).

Список неполон. Неоднократный всеяпонских гонок Хираи Масааки писал, что на алгоритмы движения у него уходит 80% от времени подготовки робота, когда на конструирование только 20%. Это для робота с самодельными редукторами, подвижной носовой частью (двигается в стороны с помощью сервомотора) и самодельными печатными платами!