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

В этом уроке

• И-регулятор и за что он отвечает
• Настройка регулятора
  
• Управление машинкой с помощью ПИД-регулятора

Видео версия урока

Общие сведения о ПИД-регуляторе:

В прошлом уроке мы разобрались с работой ПД-регулятора.

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

Недостатком П-регулятора является то, что он не может работать на больших скоростях.

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

Дифференциальная составляющая (от лат. differentia – разность) — это разность между текущей и предыдущей ошибкой.

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

Чем выше скорость изменения ошибки, тем сильнее влияние Д-регулятора.

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

Недостатком Д-регулятора является его чувствительность к шумам (некорректным данным).

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

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

Интегральная составляющая (от лат. integra – составной) — это сумма нескольких последних ошибок.

Количество ошибок в сумме определяет скорость реакции И-регулятора:

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

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

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

Пример: представим, что машина едет по дуге, и бампер выдаёт ошибку 0,5. П-регулятор слабо доворачивает вправо, — этого хватает для движения по линии, но ошибка сохраняется на прежнем уровне до завершения поворота. Значит, весь путь центр бампера был смещён от центра линии. Сумма из 10 ошибок по 0,5 уже равна 5, и И-регулятор поможет П-регулятору скорректировать путь так, что центр бампера пройдёт по центру линии на протяжении всей дуги.

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

Поведение составляющих ПИД-регулятора:

Поведение составляющих ПД-регулятора было рассмотрено в предыдущем уроке. Ниже представлена картинка движения машины по линии с ПД-регулятором:

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

После добавления интегральной составляющей, картинка станет такой:

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

Код ПИД-регулятора:

float ERR[10] = {0,0,0,0,0,0,0,0,0,0};                 // Определяем массив последних ошибок
                                                       //
void loop(){                                           //
     i++;     if( i>=10 ){ i=0; }                      // Переходим к следующему элементу массива «ERR»
     ERR[i] = bum.getErrPID();                         // Получаем текущую ошибку
     SUM    = 0; for( auto a:ERR ){ SUM+=a; }          // Получаем сумму последних ошибок
     P      = ERR[i]            * kP;                  // Определяем пропорциональную составляющую регулятора
     I      = SUM               * kI;                  // Определяем интегральную составляющую регулятора
     D      = ( ERR[i]-ERR[j] ) * kD;                  // Определяем дифференциальную составляющую регулятора
     PID    = P + I + D;                               // Получаем результат ПИД-регулятора
     j      = i;                                       // Сохраняем номер элемента массива «ERR» в котором хранится текущая ошибка,
}                                                      // на следующем проходе цикла она станет предыдущей ошибкой

• До цикла определяем массив ERR для хранения 10 последних ошибок;
• Предполагается, что остальные переменные кода также объявлены;
• Первая строка цикла увеличивает значение i на 1, со сбросом в 0 при достижении 10;
• Вторая строка сохраняет текущую ошибку в элемент ERR[i];
• Третьей строкой мы получаем в переменную SUM сумму последних 10 ошибок;
• Следующие три строки определяют составляющие ПИД регулятора;
• Предпоследняя строка получает результат PID как сумму составляющих P + I + D;
• Последней строкой значение i сохраняется в j. Значит, при каждом новом проходе цикла элемент ERR[i] хранит текущую ошибку, а ERR[j]  хранит предыдущую ошибку.

Настройка ПИД-регулятора:

• Настройка ПИД-регулятора сводится к поиску коэффициентов для пропорциональной, интегральной и дифференциальной составляющих.
• Сброс любого из коэффициентов в 0 приводит к отключению его составляющей:
• Спрос kP в ПИД-регуляторе приведёт к получению ИД-регулятора;
• Спрос kI в ПИД-регуляторе приведёт к получению ПД-регулятора;
• Спрос kD в ПИД-регуляторе приведёт к получению ПИ-регулятора.
• Получаем П-регулятор, сбросив в 0 коэффициенты kI и kD.
  
• Подбираем значение kP, при котором бампер машины или не покидает линию трассы на всех её поворотах, или находится как можно ближе к линии на поворотах. При этом машина может "вилять" на прямых участках трассы — это нормально. Во время настройки допускается немного снизить скорость машины, но её прийдётся вновь повысить в следующем пункте.
• Постепенно увеличиваем значение kD, получая ПД-регулятор. При этом машина будет всё меньше "вилять" на прямых участках трассы и лучше проходить крутые повороты:
• Слишком высокий kD приведёт к тому, что машина начнёт "дрожать" на прямой линии;
• Слишком низкий kD не избавит от "виляний" машины на прямых участках и не улучшит прохождение крутых поворотов;
• Оптимальным является kD, при котором бампер машины не покидает линию на всех поворотах трассы. При этом машина не "виляет" и не "дрожит" на прямых участках;
• После настройки kD можно снизить kP, если это не ухудшит движение машины;
• Постепенно увеличиваем значение kI, получая ПИД-регулятор, уменьшая отклонение центра бампера от центра линии как на прямых участках трассы, так и на поворотах:
• Слишком высокий kI похож на перерегулировку kP — машина начнёт "вилять";
• В процессе настройки kI можно снизить подобранные ранее kP и kD.
• Коэффициенты kP, kI и kD являются константами, только если машина движется с одной скоростью.

• Обычно kD > kP, так как разность ошибок меньше одной ошибки, значит, коэффициент больше.
• Обычно kI < kP, так как сумма ошибок больше одной ошибки, значит, коэффициент меньше.

Скетч для одной скорости:

Скорость колёс в скетче задаётся функцией setSpeed() в процентах.
Выберем скорость машины, равную 60%. Именно эту скорость мы и указываем обоим колёсам.
Предположим, что при kI = 0, kD = 0, kP = 8 машина не покидает повороты трассы.
Предположим, что при kI = 0, kD = 14.4, kP = 8 машина перестала "вилять" на прямых участках.
Предположим, что при kI = 0.1, kD = 14.4, kP = 8 бампер не покидает центр линии на поворотах.

#include <Wire.h>                                 // Подключаем библиотеку для работы с аппаратной шиной I2C
#include <iarduino_I2C_Motor.h>                   // Подключаем библиотеку для работы с мотором  I2C-flash
#include <iarduino_I2C_Bumper.h>                  // Подключаем библиотеку для работы с бампером I2C-flash
                                                  //
iarduino_I2C_Motor mot_R (0x0A);                  // Объявляем объект mot_R для правого мотора, указав адрес модуля на шине I2C
iarduino_I2C_Motor mot_L (0x0B);                  // Объявляем объект mot_L для правого мотора, указав адрес модуля на шине I2C
iarduino_I2C_Bumper bum(0x0C);                    // Объявляем объект bum  для работы с бампером I2C-flash, указав адрес модуля на шине I2C
                                                  //
float   speed   = 60;                             // Скорость машины в %
float   kP      = 8;                              // Коэффициент пропорциональной составляющей
float   kI      = 0.1;                            // Коэффициент интегральной составляющей
float   kD      = 14.4;                           // Коэффициент дифференциальной составляющей
float   ERR[10] = {0,0,0,0,0,0,0,0,0,0};          // Определяем массив последних ошибок
uint8_t i;                                        // Объявляем переменную указывающую на элемент массива «ERR» хранящий текущую ошибку
                                                  //
void setup(){                                     //
     mot_R.begin();                               // Инициируем работу с левым  мотором I2C-flash
     mot_L.begin();                               // Инициируем работу с правым мотором I2C-flash
     bum.begin();                                 // Инициируем работу с бампером I2C-flash
     mot_R.setDirection(true);                    // Указываем правому мотору, что его вращение должно быть прямым (по часовой стрелке при положительных скоростях)
     mot_L.setDirection(false);                   // Указываем левому мотору, что его вращение должно быть обратным (против часовой стрелки при положительных скоростях)
}                                                 //
                                                  //
void loop(){                                      //
           i++;     if( i>=10 ){ i=0; }           // Переходим к следующему элементу массива «ERR»
           ERR[i] = bum.getErrPID();              // Получаем текущую ошибку
     float SUM    = 0; for(auto j:ERR){SUM+=j;}   // Получаем сумму последних ошибок
     float P      = ERR[i]                 * kP;  // Определяем пропорциональную составляющую регулятора
     float I      = SUM                    * kI;  // Определяем интегральную составляющую регулятора
     float D      = (ERR[i]-ERR[(i+9)%10]) * kD;  // Определяем дифференциальную составляющую регулятора
     float PID    = P + I + D;                    // Получаем результат ПИД-регулятора
     mot_R.setSpeed( speed - PID, MOT_PWM );      // Устанавливаем скорость правого мотора
     mot_L.setSpeed( speed + PID, MOT_PWM );      // Устанавливаем скорость левого  мотора
}

Немного пояснений к коду loop():

• Текущая ошибка сохраняется в элемент ERR[i];
• Цикл for(auto j:ERR) проходит по всем элементам массива ERR, копируя их значения в переменную j. В теле цикла выполняется суммирование всех значений в SUM;
• Запись ERR[(i+9)%10] равносильна записи ERR[i-1] , только при i=0 получим ERR[9]. Эта запись позволяет получить предыдущую ошибку.

Скетч для диапазона скоростей:

В предыдущем скетче все коэффициенты были найдены для скорости 60%. Если уменьшить скорость движения машины speed с 60% до 30%, то найденные ранее коэффициенты будут слишком велики, и мы получим перерегулирование.

Выход из положения — найти коэффициенты для минимальной и максимальной скоростей машины, после чего вывести формулы зависимости коэффициентов от скорости. Мы уже так делали в предыдущих уроках с коэффициентами kP и kD. Коэффициент kI обычно столь мал, что не сильно зависит от скорости.

Теперь можно написать скетч, у которого скорость можно менять от 20 до 60%:

#include <Wire.h>                                 // Подключаем библиотеку для работы с аппаратной шиной I2C
#include <iarduino_I2C_Motor.h>                   // Подключаем библиотеку для работы с мотором I2C-flash
#include <iarduino_I2C_Bumper.h>                  // Подключаем библиотеку для работы с бампером I2C-flash
                                                  //
iarduino_I2C_Motor mot_R (0x0A);                  // Объявляем объект mot_R для правого мотора, указав адрес модуля на шине I2C
iarduino_I2C_Motor mot_L (0x0B);                  // Объявляем объект mot_L для правого мотора, указав адрес модуля на шине I2C
iarduino_I2C_Bumper bum(0x0C);                    // Объявляем объект bum  для работы с бампером I2C-flash, указав адрес модуля на шине I2C
                                                  //
float   speed   = 60;                             // Скорость машины в %.
float   kP      = 3 + 0.125*(speed-20);           // Коэффициент пропорциональной составляющей
float   kI      = 0.1;                            // Коэффициент интегральной составляющей
float   kD      = speed*speed/250;                // Коэффициент дифференциальной составляющей
float   ERR[10] = {0,0,0,0,0,0,0,0,0,0};          // Определяем массив последних ошибок
uint8_t i;                                        // Объявляем переменную, указывающую на элемент массива «ERR», который хранит текущую ошибку
                                                  //
void setup(){                                     //
     mot_R.begin();                               // Инициируем работу с левым  мотором I2C-flash
     mot_L.begin();                               // Инициируем работу с правым мотором I2C-flash
     bum.begin();                                 // Инициируем работу с бампером I2C-flash
     mot_R.setDirection(true);                    // Указываем правому мотору, что его вращение должно быть прямым (по часовой стрелке при положительных скоростях)
     mot_L.setDirection(false);                   // Указываем левому мотору, что его вращение должно быть обратным (против часовой стрелки при положительных скоростях)
}                                                 //
                                                  //
void loop(){                                      //
           i++;     if( i>=10 ){ i=0; }           // Переходим к следующему элементу массива «ERR»
           ERR[i] = bum.getErrPID();              // Получаем текущую ошибку
     float SUM    = 0; for(auto j:ERR){SUM+=j;}   // Получаем сумму последних ошибок
     float P      = ERR[i]                 * kP;  // Определяем пропорциональную составляющую регулятора
     float I      = SUM                    * kI;  // Определяем интегральную составляющую регулятора
     float D      = (ERR[i]-ERR[(i+9)%10]) * kD;  // Определяем дифференциальную составляющую регулятора
     float PID    = P + I + D;                    // Получаем результат ПИД-регулятора
     mot_R.setSpeed( speed - PID, MOT_PWM );      // Устанавливаем скорость правого мотора
     mot_L.setSpeed( speed + PID, MOT_PWM );      // Устанавливаем скорость левого  мотора
}

Этот скетч отличается от предыдущего только тем, что значения kP и kD определены не числами, а формулами.

При изменении значений скорости speed в пределах от 20% до 60%, будут автоматически пересчитываться коэффициенты kP и kD, и машина продолжит корректно двигаться по линии

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

Недостаток ПИД-регулятора:

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