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

Методист по олимпиадной робототехнике Университета Иннополис Алексей Овсянников рассказывает про алгоритмы регуляторов. Освоив урок, вы научитесь программировать движение робота по границе линии,  удерживать подводного робота на заданной глубине, регулировать скорость тележки при подъеме и спуске и решать другие задачи.

Продолжаю публиковать материалы для слушателей курсов повышения квалификации по подготовке к Innopolis Open in Robotics. Ранее я уже рассказал про подготовку моделей МанИРС для симулятора и про отслеживание показаний энкодеров у роботов на Arduino. Я обязательно продолжу эти темы, но чуть позже. Пока что хочется немного переключиться на алгоритмы. На всех наших КПК сильный акцент делается именно на них — мы не знаем, на чем в дальнейшем будут писать свои программы слушатели-педагоги и их ученики, поэтому важно объяснить принципы, логику работы программ. В каждом курсе рассматриваются свои алгоритмы (фильтрация и нормализация показаний датчиков, алгоритмы поиска объектов на изображении, алгоритмы навигации и поиска пути в лабиринте), но почти во всех соревнованиях, у всех возрастов, на всех этапах используются регуляторы. В этой статье я хочу рассмотреть, что же это за зверь такой, регулятор? И постараться рассказать это доступным языком.

Итак, что же такое регулятор? В робототехнике и автоматизации под этим понимают алгоритм, удерживающий систему (или робота) в нужном состоянии, чтобы датчики видели нужные показания. Чуть реже под этим понимают устройство, реализующее подобный алгоритм, но в DIY и образовательной робототехнике подобное редко встречается. Гораздо чаще регулятор прописывают как программу или часть программы управляющего контроллера.

Остановимся в этом месте и еще раз вчитаемся в предыдущий абзац, что мы поняли? Что регулятор — это алгоритм, программа. Если Вы собираете собственного робота из Lego или VEX’а, то не найдете у них какой-то особенной «детали-регулятора», вам придется описывать его в программе.

Второй важный момент — регулятор держит и направляет системы так, чтобы датчики видели заданное значение. То есть, регулятор всегда настраивается на показания одного или нескольких датчиков. Они позволяют определить, находится ли система в заданном состоянии. Без сенсоров регулятор не имеет смысла — непонятно будет, что сейчас происходит с роботом и как корректировать его поведение. Например, у нас есть коптер (робот-вертолет) и мы хотим, чтобы он летел на высоте 100 метров. Датчик робота видит 95 метров, следовательно ему нужно немного взлететь. Если датчик видит 110 метров, то коптеру нужно немного снизиться. Программа, которая будет автоматически управлять взлетом и снижением робота и будет регулятором. Если бы у робота не было датчика, то он не понимал бы, на какой он высоте и надо ли ему взлетать или снижаться.

Подобных примеров можно придумать очень много, почти в каждом соревновании:

• ехать строго на север по датчику-компасу и подруливать, если робот отклонился;
• удерживать руку робота на нужной высоте, даже если нагрузка на нее увеличивается;
• ехать по границе черной линии на белом фоне и подруливать, если робот отклонился сильно к черному или белому;
• удерживать одну и ту же скорость при движении робота на подъеме и спуске с горки;
• держаться подводным роботом на заданной глубине;
• и т.д.

Алгоритм «удерживания» нужного состояния можно реализовывать разными способами. Принято выделять четыре. Самый простой — релейный регулятор. Он подобен устройству реле. Это такой переключатель, который может иметь всего два состояния (выдавать два вида сигналов). Чаще всего это «вкл/выкл». Классический пример релейного переключателя — поплавковый клапан. Пока уровень воды низкий клапан открыт и вода продолжает поступать в емкость. Как только воды становится достаточно и поплавок всплывает, он закрывает клапан и вода перестает поступать:

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

Если температура в печи ниже заданной, то нагревательный тэн включается, иначе — выключается.

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

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

Для этого как раз подойдут три остальных способа регулирования:

• пропорциональная составляющая;
• интегральная составляющая;
• дифференциальная составляющая.

Комбинируя их, можно добиться разного поведения регулятора. И регуляторы принято называть по имеющимся в них частям: ПИД-регулятор имеет все три компоненты, П-регулятор содержит только пропорциональную, ПД-регулятор содержит пропорциональную и дифференциальную части и т.д.

Обычно П-часть делает основную часть работы регулятора, а И- и Д-части улучшают его функционирование. Поэтому обычно бывают П-, ПИ-, ПД-регуляторы, и редко встречаются И- или ИД-регуляторы. После того, как рассмотрим все компоненты, я расскажу, где они могут применяться.

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

Уставка — желаемое состояние системы (или робота), то есть, желаемые показания датчиков. Чаще всего ее задает сам разработчик-программист, ее можно запоминать в какой-то момент времени (когда система в нужном нам состоянии) и далее удерживать. Например, робот-футболист запоминает азимут на ворота противника и движется в их сторону по датчику-компасу.

Ошибка — разница между показаниями датчика и уставкой. Это не ошибка в программе, а ошибка в поведении системы. Она должна быть в состоянии уставки, а датчик видит иное. Коптер должен лететь на высоте 100 метров, а он летит на высоте 110 метров, следовательно, ошибка равна 10. Обратите внимание, что коптер летит на высоте 85 метров, то ошибка будет равна -15! Знак ошибки явно указывает, в какую сторону отклонение.

Управляющее воздействие — сигнал от регулятора, который должен влиять на систему, возвращая ее в состояние уставки. Оно складывается из всех частей регулятора: пропорциональной, интегральной и дифференциальной.

Учитывая эти термины, давайте рассмотрим

Принцип работы пропорциональной части регулятора: чем дальше система находится от состояния уставки, тем сильнее она к ней стремится. Вернемся к примеру с коптером — при ошибке -15 (робот летит на высоте 85 метров) он сильно взлетает. Чем ближе он к требуемой высоте, тем слабее/медленнее он взлетает. Если рассмотреть модель подводной лодки или самолета, то на нужной глубине/высоте ее рули устанавливаются вертикально, и чем дальше модель ушла от требуемой глубины/высоты, тем сильнее ее рули отклоняются:

Давайте теперь эти понятные слова «переобуем» в математику, чтобы стало непонятно. Ничего страшного, я ее объясню. Рассмотрим на примере движения робота по одному датчику вдоль линии:

где:

err — ошибка;

S — показания датчика;

ust — уставка (требуемые показания датчика);

P — пропорциональная составляющая;

kp — коэффициент пропорциональности;

U — управляющее воздействие;

Vleft, Vright — скорости левого и правого моторов робота;

Vbase — базовая скорость, которая должна быть у моторов, если робот в состоянии уставки.

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

Рассмотрим, как это работает. Уставку задает сам программист. Для робота, движущегося вдоль линии, за уставку  принимаются показания датчика на границе. Например, на черном датчик видит 10, на белом видит 80, на границе примерно среднее арифметическое между этими числами:

То есть, уставку делаем равной 45. Коэффициент пропорциональности — это просто число. Студенты и ученые рассчитывают его, исходя из математической модели робота, его физических параметров, электромеханических свойств моторов и т.д. В проектах детей его можно просто подобрать. Например, возьмем этот коэффициент равным 2. Базовая скорость пусть будет 50.

Предположим, что в момент запуска датчик видит как раз 45, получим:

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

В какой-то момент линия начнет поворачивать, а робот поедет прямо. Предположим, что линия ушла вправо и датчик робота стал видеть 50:

Получаем положительную ошибку, равную 5. Она умножается на коэффициент пропорциональности и в пропорциональной части (равно как и в управляющем воздействии) получаем 10. Скорость левого мотора увеличивается на 10, правого — замедляется на 10. В результате, так как левый мотор крутит быстрее, робот начинает поворачивать вправо, к линии.

Предположим, что робот проскочил границу линии и датчик теперь видит 40:

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

И чем больше будет значение (модуль) ошибки, тем больше будет получаться пропорциональная часть, тем сильнее она будет тянуть робота к линии.

Рассмотрим коэффициент пропорциональности. Как уже было сказано, это просто число и его можно просто подобрать. Число не должно быть отрицательным (иначе робот станет не возвращаться к уставке, а убегать от нее), оно может быть не целым. Подбор kp можно сделать буквально «на глаз» — если система слишком слабо реагирует на изменение состояния, то kp слишком мал и его надо поднять. Если система начинает резко реагировать на изменения и дергается, то kp слишком большой. Начинать можно с числа 1 или 2 и подбирать коэффициент несколькими итерациями.

Даже при самом идеально подобранном коэффициенте kp система с П-регулятором неизбежно будет «раскачиваться» вокруг состояния уставки. На примере робота, движущегося вдоль линии наглядно видна причина: если робот немного отклонился, то, возвращаясь к границе, находит состояние уставки. В этот момент он едет прямо, но куда это относительно линии?

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

Успокоить эти колебания, позволив роботу плавно выйти на линию, позволяет дифференциальная составляющая.

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

Здесь добавились:

D — дифференциальная составляющая;

kd — коэффициент дифференциальности;

errold — ошибка на предыдущем шаге.

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

Рассмотрим вычисления на трех шагах: начало движения и ошибка равна 0, далее резкий поворот и ошибка равна 15, затем робот начинает приближаться к линии и ошибка уменьшается до 10:

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

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

Избежать этого можно разными способами. Идеально вычисление Д-части делать по следующей формуле:

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

В первом математическом описании дифференциальной части я делал задержку на 5 миллисекунд командой pause(0.005). Это один из самых простых способов — я намеренно делаю большую паузу на каждой итерации. Предполагается, что все вычисления выполняются за более короткое время, примерно за 50 микросекунд. Я делаю паузу в 100 раз больше и получаю длину одной итерации 50+5000=5050 микросекунд. Даже если какой-то шаг будет вычисляться значительно дольше обычного, 500 микросекунд, общая длина шага получится 500+5000=5500 микросекунд, то есть всего на 9% дольше. И далее я могу принимать dt за константу (постоянную величину) и подбирать не kd, а kd/dt.

Еще один способ, применимый на Arduino (с использованием регистров микроконтроллера ATMega) или STM — выполнение каждого нового вычисления регулятора по прерываниям таймера. В этом случае промежуток времени тоже строго определен и заранее задан в таймере, ситуация аналогична предыдущему способу.

Осталась всего одна компонента регулятора, переходим к ней.

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

Математически полный регулятор, содержащий все три компоненты, в том числе накапливающую ошибку И-часть, выглядит следующим образом:

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

Коэффициент интегральности ki должен быть на несколько порядков меньше kp. Например, если ki меньше kp в 100 раз, то И-часть станет такой же по силе как и П-часть только через 100 шагов (с заданной выше паузой — через 0,5 секунды).

Интегральная составляющая также зависит от времени. В идеале ее надо вычислять по следующей формуле:

Тут используется та же самая dt. И теми же способами можно от нее избавиться (сделать паузу или запускать вычисления по таймеру).

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

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

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

Конечно, наш народ хитер на выдумки и придумал огромное количество вариаций. Например, я встречал накапливание И-части с «забыванием» давних значений ошибки; в книге С.А. Филиппова «Робототехника для детей и родителей» упоминается кубический регулятор; многие используют плавающие коэффициенты, возрастающие на резких поворотах и плавно ведущие по прямым линиям. Надеюсь, каждый из Вас найдет свой собственный вариант, адаптированный для конкретного робота. Начните с классического ПИД-регулятора и творите!

В следующих статьях я буду рассматривать применение регуляторов в различных задачах. Хитрости подбора коэффициентов и многие другие моменты мы расскажем на курсах повышения квалификации и школе олимпиадной подготовки к Innopolis Open in Robotics.

---

Читайте также, как настроить ПИД-регулятор для гоночного робота.

Теги: headline, Алексей Овсянников, Манипуляционные ИРС, ПИД-регулятор, робототехника, Университет Иннополис