Что такое ошибка пид регулирования

ПИД регулятор – один из самых распространенных автоматических регуляторов. Он настолько универсален, что применяется практически везде, где нужно автоматическое управление. Например температурой: специальные печи, холодильники, инкубаторы, паяльники, сопло и стол 3D принтера, ИК паяльные станции и прочее. Поддержание частоты оборотов мотора, например для станков. Всевозможные балансирующие штуки, гироскутеры, сигвеи, левитирующие магнитные платформы, и конечно же квадрокоптеры и самолёты с автопилотом. Это всё ПИД регулятор. Почему именно ПИД? Существуют и другие регуляторы, превосходящие ПИД по адаптивности к управляемой системе и стабильности, например линейно квадратичный. Но, чтобы грамотно синтезировать такой регулятор, нужно быть гораздо больше чем “семи пядей” во лбу, а настройка ПИД регулятора дело хоть и неприятное, но фактически очень простое и под силу любому, а сам ПИД регулятор универсален для почти любого процесса.

Система управления

Прежде чем переходить непосредственно к пиду, очень важно понять и запомнить несколько базовых понятий, из которых состоит автоматическая система. В первую очередь это регулятор, который всем заправляет и находится в центре системы. Регулятор в данном понимании – математический алгоритм или часть программы, которая крутится на микроконтроллере. Регулятор, как алгоритм, работает с обычными числами. Объект управления – это девайс, которым мы управляем, например печка или мотор. Для этого у нас есть управляющее устройство, например диммируемый тен или драйвер мотора. Управляющее устройство получает от регулятора управляющий сигнал, то есть конкретное число. Это может быть заполнение шим сигнала, от 0 до 255, а может быть угол поворота сервомашинки от 0 до 180, потому что регулятору без разницы чем управлять. В объекте управления у нас стоит датчик, с которого регулятор получает управляемую величину, то есть текущий сигнал с датчика. Это – обратная связь, которая и даёт возможность системе ирчно поддержать заданное значение. В случае с печкой это температура, а с мотором – частота оборотов. Ну и наконец регулятор получает установку (уставку), то есть число, к которому он должен привести текущее значение с датчика. Установка может задаваться каким угодно образом: крутилкой, ползунком, энкодером, кнопками, да хоть смской или голосовым вводом. Регулятору это неважно, для него это просто цифра. Задача регулятора состоит в том, чтобы сравнивать текущее значение с установкой и выдавать управляющий сигнал на управляющее устройство. То есть в программе это будет выглядеть условно так: регулятор получил установку, регулятор получил значение с датчика, регулятор выполнил вычисления и выдал нам управляющий сигнал, опять же число. Если это шим – мы его подаём через функцию генерации шим. Есть ещё один момент – регулятор должен делать расчёты и применять управляющий сигнал через равные промежутки времени, то есть с равным периодом или частотой. Эта частота называется частотой дискретизации системы, а период обозначается как dt, прямо как период интегрирования.  

Под капотом у ПИД регулятора

ПИД регулятор состоит из трёх составляющих: пропорциональной P, интегрирующей I и дифференциирующей D, формируется просто как сумма трёх значений, умноженных каждая на свой коэффициент. Эта сумма после вычислений становится управляющим сигналом, который подаётся на управляющее устройство, обозначим его как out.

out = P*kP + I*kI + D*kD

kP, kI и kD это и есть те самые коэффициенты, которые нужно настроить для работы ПИДа. Значения тут могут быть самые разные, от 0.001 то десятков и тысяч, это зависит от конкретной системы. Тут есть ещё один момент: любой коэффициент может быть равен нулю, и в таком случае обнуляется вся его компонента. То есть регулятор можно превратить в П, ПИ, ПД, и прочие сочетания. Разные системы требуют разного подхода, именно поэтому ПИД регулятор такой универсальный. В дальнейшем будем пользоваться следующими названиями переменных:

• out – выход с регулятора (управляющий сигнал)
• setpoint – установка (заданное значение)
• input – вход (значение с датчика)
• err – ошибка регулирования
• dt – период вычисления и регулирования

P составляющая

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

P = setpoint - input

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

I составляющая

Интегральная составляющая просто суммирует в саму себя ту же самую ошибку, разность текущего и заданного значения, умноженную на период дискретизации системы, то есть на время, прошедшее с предыдущего расчёта dt – фактически берёт интеграл от ошибки по времени.

I = I + (setpoint - input) * dt

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

D составляющая

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

err = setpoint - input
D = (err - prevErr) / dt
prevErr = err

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

Настройка регулятора

Для настройки регулятора нужно варьировать коэффициенты:

• При увеличении kP увеличивается скорость выхода на установленное значение, увеличивается управляющий сигнал. Чисто математически система не может прийти ровно к заданному значению, так как при приближении к установке П составляющая пропорционально уменьшается. При дальнейшем увеличении kP реальная система теряет устойчивость и начинаются колебания.
• При увеличении kI растёт скорость компенсации накопившейся ошибки, что позволяет вывести систему точно к заданному значению с течением времени. Если система медленная, а kI слишком большой – интегральная сумма сильно вырастет и произойдёт перерегулирование, которое может иметь характер незатухающих колебаний с большим периодом. Поэтому интегральную сумму в алгоритме регулятора часто ограничивают, чтобы она не могла увеличиваться и уменьшаться до бесконечности.
• При увеличении kD растёт стабильность системы, она не даёт системе меняться слишком быстро. В то же время kD может стать причиной неадекватного поведения системы и постоянных скачков управляющего сигнала, если значение с датчика шумит. На каждое резкое изменение сигнала с датчика Д составляющая будет реагировать изменением управляющего сигнала, поэтому сигнал с датчика нужно фильтровать (читай урок по фильтрам).

Вот так выглядит процесс стабилизации при изменении коэффициентов:   Настройка регулятора – дело не очень простое. Начальные коэффициенты для подбора можно получить по следующему алгоритму: сначала выставляем все коэффициенты в 0. Плавно увеличиваем kP до появления незатухающих колебаний. Значение kP, при котором они появились, запишем и обозначим как kP1. Далее замеряем период колебаний системы в секундах, обозначим как T. Итоговые коэффициенты получим так:

• kP = 0.6 * kP1
• kI = kP / T * 2 * dt
• kD = kP * T / 8 / dt

Например, незатухающие колебания появились при kP 20, период колебаний составил 3 секунды. Период dt в системе будет 50 мс (0.05 с). Считаем:

• kP: 0.6*20=12
• kI: 12/3*2*0.05=0.4
• kD: 12*2/8/0.05=60

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

Реализация на C++

Соединяя все рассмотренные выше уравнения, получим:

// (вход, установка, п, и, д, период в секундах, мин.выход, макс. выход)
int computePID(float input, float setpoint, float kp, float ki, float kd, float dt, int minOut, int maxOut) {
  float err = setpoint - input;
  static float integral = 0, prevErr = 0;
  integral = constrain(integral + (float)err * dt * ki, minOut, maxOut);
  float D = (err - prevErr) / dt;
  prevErr = err;
  return constrain(err * kp + integral + D * kd, minOut, maxOut);
}

Это готовая функция, которая принимает значение с датчика, установку, три коэффициента и время, а также ограничение выхода с регулятора. Как пользоваться этой функцией: функция должна вызываться с некоторым периодом, причем длительность этого периода нужно будет передать в функцию в секундах. Если попроще, можно использовать задержку. Но делать так не рекомендуется, лучше сделать таймер на миллис и работать с ним. Функция возвращает управляющий сигнал, то есть можно подать его например как ШИМ. Период dt имеет такой смысл: чем инерционнее у нас система, тем реже можно вычислять пид. Например для обогрева комнаты период можно поставить 1 секунду или больше, а для контроля за оборотами двигателя надо будет поставить пару десятков миллисекунд, то есть около сотни раз в секунду.

Библиотеки

• GyverPID – моя библиотека со встроенным тюнером коэффициентов
• QuickPID – хорошая библиотека с интересными возможностями (оптимизация интегральной суммы, смешанный режим пропорционально ошибке/измерению)
• sTune – тюнер коэффициентов, несколько алгоритмов

Видео

Полезные страницы

• Набор GyverKIT – большой стартовый набор Arduino моей разработки, продаётся в России
• Каталог ссылок на дешёвые Ардуины, датчики, модули и прочие железки с AliExpress у проверенных продавцов
• Подборка библиотек для Arduino, самых интересных и полезных, официальных и не очень
• Полная документация по языку Ардуино, все встроенные функции и макросы, все доступные типы данных
• Сборник полезных алгоритмов для написания скетчей: структура кода, таймеры, фильтры, парсинг данных
• Видео уроки по программированию Arduino с канала “Заметки Ардуинщика” – одни из самых подробных в рунете
• Поддержать автора за работу над уроками
• Обратная связь – сообщить об ошибке в уроке или предложить дополнение по тексту ([email protected])

Вместо введения

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

• системы стабилизации,
• системы программного управления,
• следящие системы

Это достаточно широкий класс систем, которые можно найти где угодно. Но какое это отношение имеет к Unity3D и вероятно к играм в частности? В принципе прямое: в любой игре так или иначе использующей симуляцию как элемент геймплея реализуются САУ, к таким играм относятся, например, Kerbal Space Programm, Digital Combat Simulator (бывший Lock On), Strike Suit Zero и т.д. (кто знает еще примеры — пишите в комментариях). В принципе любая игра, моделирующая реальные физические процессы, в том числе и просто кинематику с динамикой движения, может реализовывать те или иные САУ — этот подход проще, естественнее, а у разработчика уже есть есть набор готовых инструментов, предоставленных всякими Вышнеградскими, Ляпуновыми, Калманами, Чебышевами и прочими Коломогоровами, поэтому можно обойтись без изобретения велосипеда, т.к. его уже изобрели, да так, что получилась отдельная наука: Теория автоматического управления. Главное тут не переусердствовать. Одна тут только проблема: рассказывают про ТАУ не везде, не всем, зачастую мало и не очень понятно.

Немножко теории

Классическая система автоматического управления представленная на следующем рисунке:

Ключевым элементом любой САУ является регулятор представляющий из себя устройство, которое следит за состоянием объекта управления и обеспечивает требуемый закон управления. Процесс управления включает в себя: вычисление ошибки управления или сигнала рассогласования e(t) как разницы между желаемой уставкой (set point или SP) и текущей величиной процесса (process value или PV), после чего регулятор вырабатывает управляющие сигналы (manipulated value или MV).

Одной из разновидностью регуляторов является пропорционально-интегрально-дифференцирующий (ПИД) регулятор, который формирует управляющий сигнал, являющийся суммой трёх слагаемых: пропорционального, интегрального и дифференциального.

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

Пропорциональная составляющая P — отвечает за т.н. пропорциональное управление, смысл которого в том, что выходной сигнал регулятора, противодействует отклонению регулируемой величины (ошибки рассогласования или еще это называют невязкой) от заданного значения. Чем больше ошибка рассогласования, тем больше командное отклонение регулятора. Это самый простой и очевидный закон управления. Недостаток пропорционального закона управления заключается в том, что регулятор никогда не стабилизируется в заданном значении, а увеличение коэффициента пропорциональности всегда приводит к автоколебаниям. Именно поэтому в довесок к пропорциональному закону управления приходиться использовать интегральный и дифференциальный.

Интегральная составляющая I накапливает (интегрирует) ошибку регулирования, что позволяет ПИД-регулятору устранять статическую ошибку (установившуюся ошибку, остаточное рассогласование). Или другими словами: интегральное звено всегда вносит некоторое смещение и если система подвержена некоторыми постоянным ошибкам, то оно их компенсирует (за счет своего смещения). А вот если же этих ошибок нет или они пренебрежительно малы, то эффект будет обратным — интегральная составляющая сама будет вносить ошибку смещения. Именно по этой причине её не используют, например, в задачах сверхточного позиционирования. Ключевым недостатком интегрального закона управления является эффект насыщения интегратора (Integrator windup).

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

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

Казалось бы, вопрос реализации ПИД-регуляторов уже давно избит и здесь на Хабре есть парочка неплохих статей на эту тему в том числе и на Unity3D, также есть неплохая статья PID Without a PhD (перевод) и цикл статей в журнале «Современные технологии автоматизации» в двух частях: первая и вторая. Также к вашим услугам статья на Википедии (наиболее полную читайте в английском варианте). А на форумах коммьюнити Unity3D нет-нет, да и всплывет PID controller как и на gamedev.stackexchange

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

Попытка номер раз

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

Почему не 3D? Потому что реализация не измениться, за исключением того, что придется воротить ПИД-регулятор для контроля тангажа, рысканья и крена. Хотя вопрос корректного применения ПИД-регулирования вместе с кватернионами действительно интересный, возможно в будущем его и освящу, но даже в NASA предпочитают углы Эйлера вместо кватернионов, так что обойдемся простенькой моделью на двухмерной плоскости.

Для начала создадим сам объект игровой объект космического корабля, который будет состоять из собственно самого объекта корабля на верхнем уровне иерархии, прикрепим к нему дочерний объект Engine (чисто спецэффектов ради). Вот как это выглядит у меня:

А на сам объект космического корабля накидаем в инспекторе всяческих компонент. Забегая вперед, приведу скрин того, как он будет выглядеть в конце:

Но это потом, а пока в нем еще нет никаких скриптов, только стандартный джентльменский набор: Sprite Render, RigidBody2D, Polygon Collider, Audio Source (зачем?).

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

Т.к. помимо самого космического корабля есть куча других, менее умных космических объектов, то сначала опишем родительский класс BaseBody, который в себе будет содержать ссылки на на наши компоненты, методы инициализации и уничтожения, а также ряд дополнительных полей и методов, например для реализации небесной механики:

using UnityEngine;
using System.Collections;
using System.Collections.Generic;

namespace Assets.Scripts.SpaceShooter.Bodies
{
    [RequireComponent(typeof(SpriteRenderer))]
    [RequireComponent(typeof(AudioSource))]
    [RequireComponent(typeof(Rigidbody2D))]
    [RequireComponent(typeof(Collider2D))]

    public class BaseBody : MonoBehaviour
    {
        readonly float _deafultTimeDelay = 0.05f;

[HideInInspector]
        public static List<BaseBody> _bodies = new List<BaseBody>();

        #region RigidBody

        [HideInInspector]
        public Rigidbody2D _rb2d;

        [HideInInspector]
        public Collider2D[] _c2d;

        #endregion

        #region References

        [HideInInspector]
        public Transform _myTransform;

        [HideInInspector]
        public GameObject _myObject;

        /// <summary>
        /// Объект, который появляется при уничтожении
        /// </summary>
        public GameObject _explodePrefab;

        #endregion

        #region  Audio

        public AudioSource _audioSource;

        /// <summary>
        /// Звуки, которые проигрываются при получении повреждения
        /// </summary>
        public AudioClip[] _hitSounds;

        /// <summary>
        /// Звуки, которые проигрываются при появлении объекта
        /// </summary>
        public AudioClip[] _awakeSounds;

        /// <summary>
        /// Звуки, которые воспроизводятся перед смертью
        /// </summary>
        public AudioClip[] _deadSounds;

        #endregion

        #region External Force Variables
        /// <summary>
        /// Внешние силы воздйствующие на объект
        /// </summary>
        [HideInInspector]
        public Vector2 _ExternalForces = new Vector2();

        /// <summary>
        /// Текущий вектор скорости
        /// </summary>
        [HideInInspector]
        public Vector2 _V = new Vector2();

        /// <summary>
        /// Текущий вектор силы гравитации
        /// </summary>
        [HideInInspector]
        public Vector2 _G = new Vector2();
        #endregion

        public virtual void Awake()
        {
            Init();
        }

        public virtual void Start()
        {

        }

        public virtual void Init()
        {
            _myTransform = this.transform;
            _myObject = gameObject;

            _rb2d = GetComponent<Rigidbody2D>();
            _c2d = GetComponentsInChildren<Collider2D>();
            _audioSource = GetComponent<AudioSource>();

            PlayRandomSound(_awakeSounds);

            BaseBody bb = GetComponent<BaseBody>();
            _bodies.Add(bb);
        }

        /// <summary>
        /// Уничтожение персонажа
        /// </summary>
        public virtual void Destroy()
        {
            _bodies.Remove(this);
            for (int i = 0; i < _c2d.Length; i++)
            {
                _c2d[i].enabled = false;
            }
            float _t = PlayRandomSound(_deadSounds);
            StartCoroutine(WaitAndDestroy(_t));
        }

        /// <summary>
        /// Ждем некоторое время перед уничтожением
        /// </summary>
        /// <param name="waitTime">Время ожидания</param>
        /// <returns></returns>
        public IEnumerator WaitAndDestroy(float waitTime)
        {
            yield return new WaitForSeconds(waitTime);

            if (_explodePrefab)
            {
                Instantiate(_explodePrefab, transform.position, Quaternion.identity);
            }

            Destroy(gameObject, _deafultTimeDelay);
        }

        /// <summary>
        /// Проигрывание случайного звука
        /// </summary>
        /// <param name="audioClip">Массив звуков</param>
        /// <returns>Длительность проигрываемого звука</returns>
        public float PlayRandomSound(AudioClip[] audioClip)
        {
            float _t = 0;
            if (audioClip.Length > 0)
            {
                int _i = UnityEngine.Random.Range(0, audioClip.Length - 1);
                AudioClip _audioClip = audioClip[_i];
                _t = _audioClip.length;
                _audioSource.PlayOneShot(_audioClip);
            }
            return _t;
        }

        /// <summary>
        /// Получение урона
        /// </summary>
        /// <param name="damage">Уровень урона</param>
        public virtual void Damage(float damage)
        {
            PlayRandomSound(_hitSounds);
        }

    }
}

Вроде описали все что надо, даже больше чем нужно (в рамках этой статьи). Теперь отнаследуем от него класс корабля Ship, который должен уметь двигаться и поворачивать:

using UnityEngine;
using System.Collections;
using System.Collections.Generic;

namespace Assets.Scripts.SpaceShooter.Bodies
{
    public class Ship : BaseBody
    {
        public Vector2 _movement = new Vector2();
        public Vector2 _target = new Vector2();
        public float _rotation = 0f;

        public void FixedUpdate()
        {
            float torque = ControlRotate(_rotation);
            Vector2 force = ControlForce(_movement);

            _rb2d.AddTorque(torque);
            _rb2d.AddRelativeForce(force);
        }

        public float ControlRotate(Vector2 rotate)
        {
            float result = 0f;

            return result;
        }

        public Vector2 ControlForce(Vector2 movement)
        {
            Vector2 result = new Vector2();

            return result;

        }
    }
}

Пока в нем нет ничего интересно, на текущий момент это просто класс-заглушка.

Также опишем базовый(абстрактный) класс для всех контроллеров ввода BaseInputController:

using UnityEngine;
using Assets.Scripts.SpaceShooter.Bodies;

namespace Assets.Scripts.SpaceShooter.InputController
{
    public enum eSpriteRotation
    {
        Rigth = 0,
        Up = -90,
        Left = -180,
        Down = -270
    }

    public abstract class BaseInputController : MonoBehaviour
    {
        public GameObject _agentObject;
        public Ship _agentBody; // Ссылка на компонент логики корабля
        public eSpriteRotation _spriteOrientation = eSpriteRotation.Up; //Это связано с нестандартной 
                                                                           // ориентации спрайта "вверх" вместо "вправо"

        public abstract void ControlRotate(float dt);
        public abstract void ControlForce(float dt);

        public virtual void Start()
        {
            _agentObject = gameObject;
            _agentBody = gameObject.GetComponent<Ship>();
        }

        public virtual void FixedUpdate()
        {
            float dt = Time.fixedDeltaTime;
            ControlRotate(dt);
            ControlForce(dt);
        }

        public virtual void Update()
        {
            //TO DO
        }
    }
}

И наконец, класс контроллера игрока PlayerFigtherInput:

using UnityEngine;
using Assets.Scripts.SpaceShooter.Bodies;

namespace Assets.Scripts.SpaceShooter.InputController
{
    public class PlayerFigtherInput : BaseInputController
    {
        public override void ControlRotate(float dt)
        {
            // Определяем позицию мыши относительно игрока
            Vector3 worldPos = Input.mousePosition;
            worldPos = Camera.main.ScreenToWorldPoint(worldPos);

            // Сохраняем координаты указателя мыши
            float dx = -this.transform.position.x + worldPos.x;
            float dy = -this.transform.position.y + worldPos.y;

            //Передаем направление
            Vector2 target = new Vector2(dx, dy);
            _agentBody._target = target;

            //Вычисляем поворот в соответствии с нажатием клавиш
            float targetAngle = Mathf.Atan2(dy, dx) * Mathf.Rad2Deg;
            _agentBody._targetAngle = targetAngle + (float)_spriteOrientation;
        }

        public override void ControlForce(float dt)
        {
            //Передаем movement
            _agentBody._movement = Input.GetAxis("Vertical") * Vector2.up 
                + Input.GetAxis("Horizontal") * Vector2.right;
        }
    }
}

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

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Assets.Scripts.Regulator
{
    [System.Serializable] // Этот атрибут необходим для того что бы поля регулятора 
                                   // отображались в инспекторе и сериализовывались
    public class SimplePID
    {
        public float Kp, Ki, Kd;

        private float lastError;
        private float P, I, D;

        public SimplePID()
        {
            Kp = 1f;
            Ki = 0;
            Kd = 0.2f;
        }

        public SimplePID(float pFactor, float iFactor, float dFactor)
        {
            this.Kp = pFactor;
            this.Ki = iFactor;
            this.Kd = dFactor;
        }

        public float Update(float error, float dt)
        {
            P = error;
            I += error * dt;
            D = (error - lastError) / dt;
            lastError = error;

            float CO = P * Kp + I * Ki + D * Kd;

            return CO;
        }
    }
}

Значения коэффициентов по умолчанию возьмем с потолка: это будет тривиальный единичный коэффициент пропорционального закона управления Kp = 1, небольшое значение коэффициента для дифференциального закона управления Kd = 0.2, который должен устранить ожидаемые колебания и нулевое значение для Ki, которое выбрано потому, что в нашей программной модели нет никаких статичных ошибок (но вы всегда можете их внести, а потом героически побороться с помощью интегратора).

Теперь вернемся к нашему классу SpaceShip и попробуем заюзать наше творение в качестве регулятора поворота космического корабля в методе ControlRotate:

 public float ControlRotate(Vector2 rotate)
 {
      float MV = 0f;
      float dt = Time.fixedDeltaTime;

      //Вычисляем ошибку
      float angleError = Mathf.DeltaAngle(_myTransform.eulerAngles.z, targetAngle);

      //Получаем корректирующее ускорение
      MV = _angleController.Update(angleError, dt);

      return MV;
 }

ПИД-регулятор будет осуществлять точное угловое позиционировая космического корабля только за счет крутящего момента. Все честно, физика и САУ, почти как в реальной жизни.

 if (!_rb2d.freezeRotation)
     rb2d.freezeRotation = true;

 float deltaAngle = Mathf.DeltaAngle(_myTransform.eulerAngles.z, targetAngle);
 float T = dt *  Mathf.Abs( _rotationSpeed / deltaAngle);

 // Трансформируем угол в вектор
Quaternion rot = Quaternion.Lerp(
                _myTransform.rotation,
                Quaternion.Euler(new Vector3(0, 0, targetAngle)),
                T);

 // Изменяем поворот объекта
 _myTransform.rotation = rot;

using UnityEngine;
using Assets.Scripts.Regulator;

namespace Assets.Scripts.SpaceShooter.Bodies
{
    public class Ship : BaseBody
    {
        public GameObject _flame;

        public Vector2 _movement = new Vector2();
        public Vector2 _target = new Vector2();

        public float _targetAngle = 0f;
        public float _angle = 0f;

        [Header("PID")]
        public SimplePID _angleController = new SimplePID();

        public void FixedUpdate()
        {
            float torque = ControlRotate(_targetAngle);
            Vector2 force = ControlForce(_movement);

            _rb2d.AddTorque(torque);
            _rb2d.AddRelativeForce(force);
        }

        public float ControlRotate(float rotate)
        {
            float MV = 0f;
            float dt = Time.fixedDeltaTime;

            _angle = _myTransform.eulerAngles.z;

            //Вычисляем ошибку
            float angleError = Mathf.DeltaAngle(_angle, rotate);

            //Получаем корректирующее ускорение
            MV = _angleController.Update(angleError, dt);

            return MV;
        }

        public Vector2 ControlForce(Vector2 movement)
        {
            Vector2 MV = new Vector2();

            //Кусок кода спецэффекта работающего двигателя ради
            if (movement != Vector2.zero)
            {
                if (_flame != null)
                {
                    _flame.SetActive(true);
                }
            }
            else
            {
                if (_flame != null)
                {
                    _flame.SetActive(false);
                }
            }

            MV = movement;
            return MV;
        }
    }
}

Все? Расходимся по домам?

WTF! Что происходит? Почему корабль поворачивается как-то странно? И почему он так резко отскакивает от других объектов? Неужели этот глупый ПИД-регулятор не работает?

Без паники! Давайте попробуем разобраться что происходит.

В момент получения нового значения SP, происходит резкий (ступенчатый) скачок рассогласования ошибки, которая, как мы помним, вычисляется вот так: соответственно происходит резкий скачок производной ошибки , которую мы вычисляем в этой строчке кода:

D = (error - lastError) / dt;

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

Думаю что настал момент построить графики переходного процесса: ступенчатое воздействие от S(t) = 0 в SP(t) = 90 градусов для тела массой в 1 кг, длинной плеча силы в 1 метр и шагом сетки дифференцирования 0.02 с — прям как в нашем примере на Unity3D (на самом деле не совсем, при построении этих графиков не учитывалось, что момент инерции зависит от геометрии твердого тела, поэтому переходный процесс будет немножко другой, но все же достаточно похожий для демонстрации). Все величены на грифике приведены в абсолютных значениях:

Хм, что здесь происходит? Куда улетел отклик ПИД-регулятора?

Поздравляю, мы только что столкнулись с таким явлением как «удар» (kick). Очевидно, что в момент времени, когда процесс еще PV = 0, а уставка уже SP = 90, то при численном дифференцировании получим значение производной порядка 4500, которое умножится на Kd=0.2 и сложится с пропорциональным теромом, так что на выходе мы получим значение углового ускорения 990, а это уже форменное надругательство над физической моделью Unity3D (угловые скорости будут достигать 18000 град/с… я думаю это предельное значение угловой скорости для RigidBody2D).

• Может стоит подобрать коэффициенты ручками, так чтобы скачок был не таким сильным?
• Нет! Самое лучше чего мы таким образом сможем добиться — небольшая амплитуда скачка производной, однако сам скачок как был так и останется, при этом можно докрутиться до полной неэффективности дифференциальной составляющей.

Впрочем можете поэкспериментировать.

Попытка номер два. Сатурация

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

public float ControlRotate(Vector2 rotate, float thrust)
{
    float CO = 0f;
    float MV = 0f;
    float dt = Time.fixedDeltaTime;

    //Вычисляем ошибку
    float angleError = Mathf.DeltaAngle(_myTransform.eulerAngles.z, targetAngle);

    //Получаем корректирующее ускорение
    CO = _angleController.Update(angleError, dt);

    //Сатурируем
    MV = CO;
    if (MV > thrust) MV = thrust;
    if (MV< -thrust) MV = -thrust;

    return MV;
}

namespace Assets.Scripts.SpaceShooter.Bodies
{
    public class Ship : BaseBody
    {
        public GameObject _flame;

        public Vector2 _movement = new Vector2();
        public Vector2 _target = new Vector2();

        public float _targetAngle = 0f;
        public float _angle = 0f;

        public float _thrust = 1f;

        [Header("PID")]
        public SimplePID _angleController = new SimplePID(0.1f,0f,0.05f);

        public void FixedUpdate()
        {
            _torque = ControlRotate(_targetAngle, _thrust);
            _force = ControlForce(_movement);

            _rb2d.AddTorque(_torque);
            _rb2d.AddRelativeForce(_force);
        }

        public float ControlRotate(float targetAngle, float thrust)
        {
            float CO = 0f;
            float MV = 0f;
            float dt = Time.fixedDeltaTime;

            //Вычисляем ошибку
            float angleError = Mathf.DeltaAngle(_myTransform.eulerAngles.z, targetAngle);

            //Получаем корректирующее ускорение
            CO = _angleController.Update(angleError, dt);

            //Сатурируем
            MV = CO;
            if (MV > thrust) MV = thrust;
            if (MV< -thrust) MV = -thrust;

            return MV;
        }

        public Vector2 ControlForce(Vector2 movement)
        {
            Vector2 MV = new Vector2();

            if (movement != Vector2.zero)
            {
                if (_flame != null)
                {
                    _flame.SetActive(true);
                }
            }
            else
            {
                if (_flame != null)
                {
                    _flame.SetActive(false);
                }
            }

            MV = movement * _thrust;

            return MV;
        }

        public void Update()
        {

        }        
    }
}

Итоговая схема нашего САУ тогда станет уже вот такой

При этом уже становится понятно, что выход контроллера CO(t) немного не одно и тоже, что управляемая величина процесса MV(t).

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

Запустив игру, мы обнаружим, что космический корабль стал наконец управляемым:

Если построить графики, то можно увидеть, что реакция контроллера стала уже вот такой:

Здесь уже используются нормированные величены, углы поделены на значение SP, а выход контроллера отнормирован относительно максимального значения на котором уже происходит сатурация.

Теперь на графике видно наличие ошибки перерегулирования (overshooting) и затухающие колебания. Уменьшая Kp и увеличивая Kd можно добиться уменьшения колебаний, но зато увеличится время реакции контроллера (скорость поворота корабля). И наоборот, увеличивая Kp и уменьшая Kd — можно добиться увеличения скорости реакции контроллера, но появятся паразитные колебания, которые при определенных (критических) значениях, перестанут быть затухающими.

Ниже приведена известна таблица влияния увеличения параметров ПИД-регулятора (как уменьшить шрифт, а то таблица безе переносов не лезет?):

А общий алгоритм ручной настройки ПИД-регулятора следующий:

1. Подбираем пропорциональный коэффициенты при отключенных дифференциальных и интегральных звеньях до тех пор пока не начнутся автоколебания.
2. Постепенно увеличивая дифференциальную составляющую избавляемся от автоколебаний
3. Если наблюдается остаточная ошибка регулирования (смещение), то устраняем её за счет интегральной составляющей.

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

В общем не будем о грустном, дальше нас ждет самое интересное…

Попытка номер три. Еще раз производные

Приделав костыль в виде ограничения значений выхода контроллера мы так и не решили самую главную проблему нашего регулятора — дифференциальная составляющая плохо себя чувствует при ступенчатом изменении ошибки на входе регуляторе. На самом деле есть множество других костылей, например, в момент скачкообразного изменения SP «отключать» дифференциальную составляющую или же поставить фильтры нижних частот между SP(t) и операцией за счет которого будет происходить плавное нарастание ошибки, а можно совсем развернуться и впендюрить самый настоящий фильтр Калмана для сглаживания входных данных. В общем костылей много, и добавить наблюдателя конечно хотелось бы, но не в этот раз.

Поэтому снова вернемся к производной ошибки рассогласования и внимательно на неё посмотрим:

Ничего не заметили? Если хорошенько присмотреться, то можно обнаружить, что вообще-то SP(t), не меняется во времени (за исключением моментов ступенчатого изменения, когда регулятор получает новую команду), т.е. её производная равна нулю:

тогда

Иными словами, вместо производной ошибки, которая дифференцируема не везде мы можем использовать производную от процесса, который в мире классической механики как правило непрерывен и дифференцируем везде, а схема нашей САУ уже приобретет следующий вид:

Модифицируем код регулятора:

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Assets.Scripts.Regulator
{
    [System.Serializable]
    public class SimplePID
    {
        public float Kp, Ki, Kd;
        private float P, I, D;

        private float lastPV = 0f;   

        public SimplePID()
        {
            Kp = 1f;
            Ki = 0f;
            Kd = 0.2f;
        }

        public SimplePID(float pFactor, float iFactor, float dFactor)
        {
            this.Kp = pFactor;
            this.Ki = iFactor;
            this.Kd = dFactor;
        }

        public float Update(float error, float PV, float dt)
        {
            P = error;
            I += error * dt;
            D = -(PV - lastPV) / dt;

            lastPV = PV;

            float CO = Kp * P + Ki * I + Kd * D;

            return CO;
        }
    }
}

И немного изменим метод ControlRotate:

public float ControlRotate(Vector2 rotate, float thrust)
{
     float CO = 0f;
     float MV = 0f;
     float dt = Time.fixedDeltaTime;

     //Вычисляем ошибку
     float angleError = Mathf.DeltaAngle(_myTransform.eulerAngles.z, targetAngle);

     //Получаем корректирующее ускорение
     CO = _angleController.Update(angleError, _myTransform.eulerAngles.z, dt);

     //Сатурируем
     MV = CO;
     if (CO > thrust) MV = thrust;
     if (CO < -thrust) MV = -thrust;

     return MV;
}

И-и-и-и… если запустить игру, то обнаружиться, что на самом деле ничего ничего не изменилось с последней попытки, что и требовалось доказать. Однако, если убрать сатурацию, то график реакции регулятора будет выглядеть вот так:

Скачок CO(t) по прежнему присутствует, однако он уже не такой большой как был в самом начале, а самое главное — он стал предсказуемым, т.к. обеспечивается исключительно пропорциональной составляющей, и ограничен максимально возможной ошибкой рассогласования и пропорциональным коэффициентом ПИД-регулятора (а это уже намекает на то, что Kp имеет смысл выбрать все же меньше единицы, например, 1/90f), но не зависит от шага сетки дифференцирования (т.е. dt). В общем, я настоятельно рекомендую использовать именно производную процесса, а не ошибки.

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

Попытка номер четыре. Альтернативные реализации ПИД-регулятор

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

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

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

Основные принципы настройки ПИД-регулятора в стандартной форме аналогичны идеализированному ПИД-регулятору:

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

namespace Assets.Scripts.Regulator
{
    [System.Serializable]    
    public class StandartPID
    {
        public float Kp, Ti, Td;
        public float error, CO;
        public float P, I, D;

        private float lastPV = 0f;

        public StandartPID()
        {
            Kp = 0.1f;
            Ti = 10000f;
            Td = 0.5f;
            bias = 0f;
        }

        public StandartPID(float Kp, float Ti, float Td)
        {
            this.Kp = Kp;
            this.Ti = Ti;
            this.Td = Td;
        }

        public float Update(float error, float PV, float dt)
        {
            this.error = error;
            P = error;
            I += (1 / Ti) * error * dt;
            D = -Td * (PV - lastPV) / dt;

            CO = Kp * (P + I + D);
            lastPV = PV;

            return CO;
        }
    }
}

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

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

Вместо заключения. Куда бы еще присобачить ПИД-регулятор

Теперь попробуем немного усложнить управление корабля используя двухконтурное управление: один ПИД-регулятор, уже знакомый нам _angleController, отвечает по прежнему за угловое позиционирование, а вот второй — новый, _angularVelocityController — контролирует скорость поворота:

public float ControlRotate(float targetAngle, float thrust)
{
    float CO = 0f;
    float MV = 0f;
    float dt = Time.fixedDeltaTime;

    _angle = _myTransform.eulerAngles.z;

    //Контроллер угла поворота
    float angleError = Mathf.DeltaAngle(_angle, targetAngle);
    float torqueCorrectionForAngle = 
    _angleController.Update(angleError, _angle, dt);

    //Контроллер стабилизации скорости
    float angularVelocityError = -_rb2d.angularVelocity;
    float torqueCorrectionForAngularVelocity = 
        _angularVelocityController.Update(angularVelocityError, -angularVelocityError, dt);

    //Суммарный выход контроллера
    CO = torqueCorrectionForAngle + torqueCorrectionForAngularVelocity;

    //Дискретизируем с шагом 100            
    CO = Mathf.Round(100f * CO) / 100f;

    //Сатурируем
    MV = CO;
    if (CO > thrust) MV = thrust;
    if (CO < -thrust) MV = -thrust;

    return MV;
}

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

Помимо этого, добавим новый класс ввода игрока — PlayerInputCorvette, в котором повороты буду осуществляться уже за счет нажатия клавиш «вправо-влево», а целеуказание с помощью мыши мы оставим для чего-нибудь более полезного, например, для управления турелью. Заодно у нас теперь появился такой параметр как _turnRate — отвечающий за скорость/отзывчивость поворота (не понятно только куда его поместить лучше в InputCOntroller или все же Ship).

public class PlayerCorvetteInput : BaseInputController
{
     public float _turnSpeed = 90f;

     public override void ControlRotate()
     {
         // Находим указатель мыши
         Vector3 worldPos = Input.mousePosition;
         worldPos = Camera.main.ScreenToWorldPoint(worldPos);

         // Сохраняем относительные координаты указателя мыши
         float dx = -this.transform.position.x + worldPos.x;
         float dy = -this.transform.position.y + worldPos.y;

         //Передаем направление указателя мыши
         Vector2 target = new Vector2(dx, dy);
         _agentBody._target = target;

         //Вычисляем поворот в соответствии с нажатием клавиш
         _agentBody._rotation -= Input.GetAxis("Horizontal") * _turnSpeed * Time.deltaTime;
    }

    public override void ControlForce()
    {            
         //Передаем movement
         _agentBody._movement = Input.GetAxis("Vertical") * Vector2.up;
    }
}

namespace Assets.Scripts.SpaceShooter.UI
{
    [RequireComponent(typeof(Ship))]
    [RequireComponent(typeof(BaseInputController))]
    public class Debugger : MonoBehaviour
    {
        Ship _ship;
        BaseInputController _controller;
        List<SimplePID> _pids = new List<SimplePID>();
        List<string> _names = new List<string>();

        Vector2 _orientation = new Vector2();

        // Use this for initialization
        void Start()
        {
            _ship = GetComponent<Ship>();
            _controller = GetComponent<BaseInputController>();

            _pids.Add(_ship._angleController);
            _names.Add("Angle controller");

            _pids.Add(_ship._angularVelocityController);
            _names.Add("Angular velocity controller");

        }

        // Update is called once per frame
        void Update()
        {
            DrawDebug();
        }

        Vector3 GetDiretion(eSpriteRotation spriteRotation)
        {
            switch (_controller._spriteOrientation)
            {
                case eSpriteRotation.Rigth:
                    return transform.right;
                case eSpriteRotation.Up:
                    return transform.up;
                case eSpriteRotation.Left:
                    return -transform.right;
                case eSpriteRotation.Down:
                    return -transform.up;
            }
            return Vector3.zero;
        }

        void DrawDebug()
        {
            //Направление поворота
            Vector3 vectorToTarget = transform.position 
                + 5f * new Vector3(-Mathf.Sin(_ship._targetAngle * Mathf.Deg2Rad), 
                    Mathf.Cos(_ship._targetAngle * Mathf.Deg2Rad), 0f);

            // Текущее направление
            Vector3 heading = transform.position + 4f * GetDiretion(_controller._spriteOrientation);

            //Угловое ускорение
            Vector3 torque = heading - transform.right * _ship._Torque;

            Debug.DrawLine(transform.position, vectorToTarget, Color.white);
            Debug.DrawLine(transform.position, heading, Color.green);
            Debug.DrawLine(heading, torque, Color.red);
        }

        void OnGUI()
        {
            float x0 = 10;
            float y0 = 100;

            float dx = 200;
            float dy = 40;

            float SliderKpMax = 1;
            float SliderKpMin = 0;
            float SliderKiMax = .5f;
            float SliderKiMin = -.5f;
            float SliderKdMax = .5f;
            float SliderKdMin = 0;

            int i = 0;
            foreach (SimplePID pid in _pids)
            {
                y0 += 2 * dy;

                GUI.Box(new Rect(25 + x0, 5 + y0, dx, dy), "");

                pid.Kp = GUI.HorizontalSlider(new Rect(25 + x0, 5 + y0, 200, 10), 
                    pid.Kp, 
                    SliderKpMin, 
                    SliderKpMax);
                pid.Ki = GUI.HorizontalSlider(new Rect(25 + x0, 20 + y0, 200, 10), 
                    pid.Ki, 
                    SliderKiMin, 
                    SliderKiMax);
                pid.Kd = GUI.HorizontalSlider(new Rect(25 + x0, 35 + y0, 200, 10), 
                    pid.Kd, 
                    SliderKdMin, 
                    SliderKdMax);

                GUIStyle style1 = new GUIStyle();
                style1.alignment = TextAnchor.MiddleRight;
                style1.fontStyle = FontStyle.Bold;
                style1.normal.textColor = Color.yellow;
                style1.fontSize = 9;

                GUI.Label(new Rect(0 + x0, 5 + y0, 20, 10), "Kp", style1);
                GUI.Label(new Rect(0 + x0, 20 + y0, 20, 10), "Ki", style1);
                GUI.Label(new Rect(0 + x0, 35 + y0, 20, 10), "Kd", style1);

                GUIStyle style2 = new GUIStyle();
                style2.alignment = TextAnchor.MiddleLeft;
                style2.fontStyle = FontStyle.Bold;
                style2.normal.textColor = Color.yellow;
                style2.fontSize = 9;

                GUI.TextField(new Rect(235 + x0, 5 + y0, 60, 10), pid.Kp.ToString(), style2);
                GUI.TextField(new Rect(235 + x0, 20 + y0, 60, 10), pid.Ki.ToString(), style2);
                GUI.TextField(new Rect(235 + x0, 35 + y0, 60, 10), pid.Kd.ToString(), style2);

                GUI.Label(new Rect(0 + x0, -8 + y0, 200, 10), _names[i++], style2);
            }
        }
    }
}

namespace Assets.Scripts.SpaceShooter.Bodies
{
    public class Ship : BaseBody
    {
        public GameObject _flame;

        public Vector2 _movement = new Vector2();
        public Vector2 _target = new Vector2();

        public float _targetAngle = 0f;
        public float _angle = 0f;

        public float _thrust = 1f;

        [Header("PID")]
        public SimplePID _angleController = new SimplePID(0.1f,0f,0.05f);
        public SimplePID _angularVelocityController = new SimplePID(0f,0f,0f);

        private float _torque = 0f;
        public float _Torque
        {
            get
            {
                return _torque;
            }
        }

        private Vector2 _force = new Vector2();
        public Vector2 _Force
        {
            get
            {
                return _force;
            }
        }

        public void FixedUpdate()
        {
            _torque = ControlRotate(_targetAngle, _thrust);
            _force = ControlForce(_movement, _thrust);

            _rb2d.AddTorque(_torque);
            _rb2d.AddRelativeForce(_force);
        }

        public float ControlRotate(float targetAngle, float thrust)
        {
            float CO = 0f;
            float MV = 0f;
            float dt = Time.fixedDeltaTime;

            _angle = _myTransform.eulerAngles.z;

            //Контроллер угла поворота
            float angleError = Mathf.DeltaAngle(_angle, targetAngle);
            float torqueCorrectionForAngle = 
                _angleController.Update(angleError, _angle, dt);

            //Контроллер стабилизации скорости
            float angularVelocityError = -_rb2d.angularVelocity;
            float torqueCorrectionForAngularVelocity = 
                _angularVelocityController.Update(angularVelocityError, -angularVelocityError, dt);

            //Суммарный выход контроллера
            CO = torqueCorrectionForAngle + torqueCorrectionForAngularVelocity;

            //Дискретизируем с шагом 100            
            CO = Mathf.Round(100f * CO) / 100f;

            //Сатурируем
            MV = CO;
            if (CO > thrust) MV = thrust;
            if (CO < -thrust) MV = -thrust;

            return MV;
        }

        public Vector2 ControlForce(Vector2 movement, float thrust)
        {
            Vector2 MV = new Vector2();

            if (movement != Vector2.zero)
            {
                if (_flame != null)
                {
                    _flame.SetActive(true);
                }
            }
            else
            {
                if (_flame != null)
                {
                    _flame.SetActive(false);
                }
            }

            MV = movement * thrust;

            return MV;
        }

        public void Update()
        {

        }        
    }
}

А вот, собственно заключительное видео того, что должно получиться:

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

Немного ссылок

1. Годная статья на английской вики
2. PID tutorial
3. ПИД-регуляторы: вопросы реализации. Часть 1
4. ПИД-регуляторы: вопросы реализации. Часть 2
5. PID Without a PhD
6. PID Without a PhD. Перевод
7. Derivative Action and PID Control
8. Control System Lab: PID
9. ПИД-регулятор своими руками
10. Корректная реализация разностной схемы ПИД регулятора
11. Программируем квадрокоптер на Arduino (часть 1)
12. Виртуальный квадрокоптер на Unity + OpenCV (Часть 1)
13. Поляков К.Ю. Теория автоматического управления для чайников
14. PID control system analysis, design, and technology
15. Aidan O’Dwyer. Handbook of PI and PID Controller Tuning Rules (3rd ed.)
16. PID process control, a “Cruise Control” example
17. https://www.mathworks.com/discovery/pid-control.html
18. http://scilab.ninja/study-modules/scilab-control-engineering-basics/module-4-pid-control/
19. https://sourceforge.net/p/octave/control/ci/default/tree/inst/optiPID.m

Еще немного ссылок на другие примеры
http://luminaryapps.com/blog/use-a-pid-loop-to-control-unity-game-objects/
http://www.habrador.com/tutorials/pid-controller/3-stabilize-quadcopter/
https://www.gamedev.net/articles/programming/math-and-physics/pid-control-of-physics-bodies-r3885/
https://ksp-kos.github.io/KOS/tutorials/pidloops.html

Хлеба и зрелищ

Проголосовали 129 пользователей.

Воздержались 23 пользователя.

Общие сведения

   Обычно, при использовании пропорционально- интегрально-дифференциального или ПИД- (PID- Proportional-Integral-Derivative) регулятора и грамотной его настройке, достигается лучшая точность управления по сравнению с двухпозиционным (релейным) регулятором. Но для оптимальной настройки регулятора и, как следствие, получение желаемого качества управления, необходимо понимание механизмов и принципов работы ПИД-регулятора.

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

Математическое выражение ПИД-регулятора

где:

   Xp — полоса пропорциональности

   Ei = (SP-PV) = (уставка-тек) = ошибка (рассогласование)

   Тд — постоянная времени дифференцирования

   ∆Ei — разность ошибок соседних измерений (Ei — Ei-1)

   ∆tизм — время между соседними измерениями (ti — t i-1)

   Ти — постоянная времени интегрирования

— Накопленная к i-ому шагу сумма рассогласований (интегральная сумма)

   Легко заметить, что сигнал управления является суммой трех составляющих: пропорциональной (слагаемое 1), дифференциальной (слагаемое 2), и интегральной (слагаемое 3).

   Пропорциональная составляющая зависит от текущей ошибки Ei и компенсирует текущую ошибку пропорционально ее величине.

   Дифференциальная составляющая зависит от скорости изменения ошибки ∆Ei / ∆tизм и компенсирует резкие возмущения.

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

   Обычно ПИД-регулятор имеет дополнительные параметры помимо трех коэффициентов (Xp, Ти, Тд). Рассмотрим их более подробно на примере скриншота меню параметров ПИД-регулятора прибора “Параграф PL20”.

Рис. 1

    Каналов (выходов) ПИД-регулирования в приборе может быть несколько и параметры для каждого из них свои собственные. Поэтому выберите желаемый канал в первой графе.

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

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

    Уставка (SP) – это желаемая величина, на которую регулятор должен выйти в установившемся режиме.

    Xp – зона пропорциональности. Задается в единицах контролируемой величины (для терморегулятора в градусах). Зона пропорциональности называется так, потому что только в ней ((SP — Xp)…(SP + Xp)) пропорциональная составляющая ПИД-регулятора может формировать мощность выходного сигнала управления пропорционально ошибке. А за ее пределами мощность будет равна либо 0%, либо 100%. Таким образом, чем уже эта зона, тем быстрее отклик регулятора, но слишком высокое быстродействие может ввести систему в автоколебательный режим.

    Ти – постоянная времени интегрирования.

    Тд – постоянная времени дифференцирования.

    Текущая мощность – это информационный параметр.

    Минимальная и максимальная мощность определяют границы мощности выхода ПИД-регулятора.

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

   Последним параметром идет период ШИМ. Этот параметр один для всех ПИД-регуляторов, т.к. каналы ШИМ  синхронизированы между собой от одного таймера. ШИМ сигнал позволяет регулировать мощность посредством  регулировки скважности сигнала (регулируется ширина импульса при постоянной частоте модуляции). Разрядность ШИМ (число позиций мощности) равна 8192 дискреты (13 бит). Период ШИМ (от 1 мс до 250 сек). Этот параметр зависит от типа и коммутационных способностей силовых исполнительных ключей (м.б. реле, пускатель, твердотельное реле, симистор). Чем выше частота коммутации (чем меньше период) тем больше тепловые потери в ключах (квадратичная зависимость потерь от частоты) и больше износ механических коммутаторов, но лучше качество регулирования. Важно найти золотую середину.

Настройка пропорциональной компоненты (Xp)

   Перед настройкой зоны пропорциональности интегральная и дифференциальная компоненты отключаются, постоянная интегрирования устанавливается максимально возможной (Ти = макс), а постоянная дифференцирования минимально возможной (Тд = 0). Устанавливается безопасная величина уставки, равная (0,7…0,9)×SP, где SP – это реальная уставка настраиваемой системы. Зона пропорциональности устанавливается минимально возможной (Xp = 0).

   В этом случае регулятор выполняет функции двухпозиционного релейного регулятора с гистерезисом равным нулю. Регистрируется переходная характеристика.

Рис. 2

    Т0 — начальная температура в системе;

    ТSP — заданная температура (уставка);

    ∆T — размах колебаний температуры;

    ∆t — период колебаний температуры.

    Установить зону пропорциональности равной размаху колебаний температуры: Xp = ∆T. Это значение служит

первым приближением для зоны пропорциональности.

   Следует проанализировать переходную характеристики еще раз и при необходимости скорректировать значение зоны пропорциональности. Возможные варианты переходных характеристик показаны на рис. 3.

Рис. 3

   Переходная характеристика типа 1: Значение зоны пропорциональности очень мало, переходная характеристика далека от оптимальной. Зону пропорциональности следует значительно увеличить.

   Переходная характеристика типа 2: В переходной характеристике наблюдаются затухающие колебания (5 — 6 периодов).     Если в дальнейшем предполагается использовать и дифференциальную компоненту ПИД-регулятора, то выбранное значение зоны пропорциональности является оптимальным. Для этого случая настройка зоны пропорциональности считается законченной.

   Если в дальнейшем дифференциальная компоненты использоваться не будет, то рекомендуется еще увеличить зону пропорциональности так, чтобы получились переходные характеристики типа 3 или 4.

   Переходная характеристика типа 3: В переходной характеристике наблюдаются небольшой выброс (перерегулирование) и быстро затухающие колебания (1 — 2 периода). Этот тип переходной характеристики обеспечивает хорошее быстродействие и быстрый выход на заданную температуру. В большинстве случаев его можно считать оптимальным, если в системе допускаются выбросы (перегревы) при переходе с одной температуры на другую.

   Выбросы устраняются дополнительным увеличением зоны пропорциональности так, чтобы получилась переходная  характеристика типа 4.

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

   Переходная характеристика типа 5: Сильно затянутый подход к установившемуся значению говорит о том, что зона пропорциональности чрезмерно велика. Динамическая и статическая точность регулирования здесь мала.

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

Настройка дифференциальной компоненты (Tд)

   Этот этап присутствует только в том случае, если применяется полнофункциональный ПИД-регулятор. Если  дифференциальная компонента применяться не будет (используется пропорционально-интегральный (ПИ) регулятор), то следует пропустить этот этап.

   На предыдущем этапе была задана зона пропорциональности, соответствующая переходной характеристике типа 2, в которой присутствуют затухающие колебания (см. рис. 3, кривая 2, рис. 4, кривая 1.).

Рис. 4

   Следует установить постоянную времени дифференцирования Тд так, чтобы переходная характеристика имела вид кривой 2 на рис. 4. В качестве первого приближения постоянная времени дифференцирования делается равной Тд = 0,2×∆t.

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

  Настройка интегральной компоненты (Ти )

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

Рис. 5

   Интегральная компонента предназначена для того, чтобы убрать остаточное рассогласование между установившимся в системе значением температуры и уставкой. Начинать настраивать постоянную времени интегрирования следует с величины Ти = ∆t.

   Переходная характеристика типа 2: Получается при чрезмерно большой величине постоянной времени интегрирования. Выход на уставку получается очень затянутым.

   Переходная характеристика типа 4: Получается при слишком малой величине постоянной времени интегрирования. Если постоянную времени интегрирования уменьшить еще, то в системе могут возникнуть колебания.

   Переходная характеристика типа 3: Оптимальная.

Использованные источники информации

• Сабинин Ю.А. Ковчин С.А. “Теория электропривода”
• Шрейнер Р. Т. “Системы подчиненного регулирования электроприводов”
• Олссон, Пиани “Цифровые системы автоматизации и управления”
• Материалы сайта www.asu-tp.org

Общие сведения

    Обычно, при использовании пропорционально-интегрально-дифференциального или ПИД- (PID- Proportional-Integral-Derivative) регулятора и грамотной его настройке, достигается лучшая точность управления по сравнению с двухпозиционным (релейным) регулятором. Но для оптимальной настройки регулятора и, как следствие, получение желаемого качества управления, необходимо понимание механизмов и принципов работы ПИД-регулятора.

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

Математическое выражение цифрового ПИД-регулятора

где:

    Xp — полоса пропорциональности

    Ei = (SP-PV) = (уставка-тек) = ошибка (рассогласование)

    Тд — постоянная времени дифференцирования

    ∆Ei — разность ошибок соседних измерений (Ei — Ei-1)

    ∆tизм — время между соседними измерениями (ti — t i-1)

    Ти — постоянная времени интегрирования

    — Накопленная к i-ому шагу сумма рассогласований (интегральная сумма)

    Легко заметить, что сигнал управления является суммой трех составляющих: пропорциональной (слагаемое 1), дифференциальной (слагаемое 2), и интегральной (слагаемое 3).

    Пропорциональная составляющая зависит от текущей ошибки Ei и компенсирует текущую ошибку пропорционально ее величине.

    Дифференциальная составляющая зависит от скорости изменения ошибки ∆Ei / ∆tизм и компенсирует резкие возмущения.

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

    Обычно ПИД-регулятор имеет дополнительные параметры помимо трех коэффициентов (Xp, Ти, Тд). Рассмотрим их более подробно на примере скриншота меню параметров ПИД-регулятора прибора “ПАРАГРАФ PL20”.

Рис. 1

    Каналов (выходов) ПИД-регулирования в приборе может быть несколько и параметры для каждого из них свои собственные. Поэтому выберите желаемый канал в первой графе.

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

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

    Уставка (SP) – это желаемая величина, на которую регулятор должен выйти в установившемся режиме.

    Xp – зона пропорциональности. Задается в единицах контролируемой величины (для терморегулятора в градусах). Зона пропорциональности называется так, потому что только в ней ((SP — Xp)…(SP + Xp)) пропорциональная составляющая ПИД-регулятора может формировать мощность выходного сигнала управления пропорционально ошибке. А за ее пределами мощность будет равна либо 0%, либо 100%. Таким образом, чем уже эта зона, тем быстрее отклик регулятора, но слишком высокое быстродействие может ввести систему в автоколебательный режим.

    Ти – постоянная времени интегрирования.

    Тд – постоянная времени дифференцирования.

    Текущая мощность – это информационный параметр.

    Минимальная и максимальная мощность определяют границы мощности выхода ПИД-регулятора.

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

    Последним параметром идет период ШИМ. Этот параметр один для всех ПИД-регуляторов, т.к. каналы ШИМ синхронизированы между собой от одного таймера. ШИМ сигнал позволяет регулировать мощность посредством регулировки скважности сигнала (регулируется ширина импульса при постоянной частоте модуляции). Разрядность ШИМ (число позиций мощности) равна 8192 дискреты (13 бит). Период ШИМ (от 1 мс до 250 сек). Этот параметр зависит от типа и коммутационных способностей силовых исполнительных ключей (м.б. реле, пускатель, твердотельное реле, симистор). Чем выше частота коммутации (чем меньше период) тем больше тепловые потери в ключах (квадратичная зависимость потерь от частоты) и больше износ механических коммутаторов, но лучше качество регулирования. Важно найти золотую середину.

Настройка пропорциональной компоненты (Xp)

   Перед настройкой зоны пропорциональности интегральная и дифференциальная компоненты отключаются, постоянная интегрирования устанавливается максимально возможной (Ти = макс), а постоянная дифференцирования минимально возможной (Тд = 0). Устанавливается безопасная величина уставки, равная (0,7…0,9)×SP, где SP – это реальная уставка настраиваемой системы. Зона пропорциональности устанавливается минимально возможной (Xp = 0).

   В этом случае регулятор выполняет функции двухпозиционного релейного регулятора с гистерезисом равным нулю. Регистрируется переходная характеристика.

Рис. 2

    Тο — начальная температура в системе;

    Тsp — заданная температура (уставка);

    ∆T — размах колебаний температуры;

    ∆t — период колебаний температуры.

    Установить зону пропорциональности равной размаху колебаний температуры: Xp = ∆T. Это значение служит

первым приближением для зоны пропорциональности.

    Следует проанализировать переходную характеристики еще раз и при необходимости скорректировать значение зоны пропорциональности. Возможные варианты переходных характеристик показаны на рис. 3.

Рис. 3

    Переходная характеристика типа 1: Значение зоны пропорциональности очень мало, переходная характеристика далека от оптимальной. Зону пропорциональности следует значительно увеличить.

    Переходная характеристика типа 2: В переходной характеристике наблюдаются затухающие колебания (5 — 6 периодов). Если в дальнейшем предполагается использовать и дифференциальную компоненту ПИД-регулятора, то выбранное значение зоны пропорциональности является оптимальным. Для этого случая настройка зоны пропорциональности считается законченной.

    Если в дальнейшем дифференциальная компоненты использоваться не будет, то рекомендуется еще увеличить зону пропорциональности так, чтобы получились переходные характеристики типа 3 или 4.

    Переходная характеристика типа 3: В переходной характеристике наблюдаются небольшой выброс (перерегулирование) и быстро затухающие колебания (1 — 2 периода). Этот тип переходной характеристики обеспечивает хорошее быстродействие и быстрый выход на заданную температуру. В большинстве случаев его можно считать оптимальным, если в системе допускаются выбросы (перегревы) при переходе с одной температуры на другую.

    Выбросы устраняются дополнительным увеличением зоны пропорциональности так, чтобы получилась переходная характеристика типа 4.

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

    Переходная характеристика типа 5: Сильно затянутый подход к установившемуся значению говорит о том, что зона пропорциональности чрезмерно велика. Динамическая и статическая точность регулирования здесь мала.

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

Настройка дифференциальной компоненты (Tд)

    Этот этап присутствует только в том случае, если применяется полнофункциональный ПИД-регулятор. Если дифференциальная компонента применяться не будет (используется пропорционально-интегральный (ПИ) регулятор), то следует пропустить этот этап.

    На предыдущем этапе была задана зона пропорциональности, соответствующая переходной характеристике типа 2, в которой присутствуют затухающие колебания (см. рис. 3, кривая 2, рис. 4, кривая 1.).

Рис. 4

   Следует установить постоянную времени дифференцирования Тд так, чтобы переходная характеристика имела вид кривой 2 на рис. 4. В качестве первого приближения постоянная времени дифференцирования делается равной Тд = 0,2×∆t.

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

Настройка интегральной компоненты (Ти )

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

Рис. 5

    Интегральная компонента предназначена для того, чтобы убрать остаточное рассогласование между установившимся в системе значением температуры и уставкой. Начинать настраивать постоянную времени интегрирования следует с величины Ти = ∆t.

    Переходная характеристика типа 2: Получается при чрезмерно большой величине постоянной времени интегрирования. Выход на уставку получается очень затянутым.

    Переходная характеристика типа 4: Получается при слишком малой величине постоянной времени интегрирования. Если постоянную времени интегрирования уменьшить еще, то в системе могут возникнуть колебания.

    Переходная характеристика типа 3: Оптимальная.

Использованные источники информации

• Сабинин Ю.А. Ковчин С.А. “Теория электропривода”
• Шрейнер Р. Т. “Системы подчиненного регулирования электроприводов”
• Олссон, Пиани “Цифровые системы автоматизации и управления”
• Материалы сайта www.asu-tp.org

ООО «Автоматика» © 2012