Робот движущийся по линии, с использованием PID управления

1. PID - это простой, но очень эффективный метод управления, в котором контролируется конкретная физическая либо электрическая величина и устанавливается равной заданному значению, называемому «установкой». В этом случае мы будем контролировать скорость правого и левого двигателей робота, чтобы робот следовал за центром черной линии, используя Arduino. Это делается путем вычисления величины отклонения робота от центра линии с помощью ИК-датчиков.

2. Необходимые компоненты

• 5 цифровых ИК-датчиков
• Arduino Board (например UNO)
• 2 электродвигателя постоянного тока
• Драйвер двигателей, например на базе L298N
• Провода для перемычек

3. Подключение

• Подключите 5 цифровых ИК-датчиков к аналоговым входам (вы можете считывать цифровые значения с аналоговых входов).
• Подключите левый двигатель двигателя к цифровым контактам 7 и 6
• Подсоедините правильные выводы двигателя к цифровым контактам 5 и 4
• Подключите штыри ШИМ к контактам 9 и 3
Так же проверьте подключения по схеме ниже

4. Создание программы

Поскольку мы используем цифровые ИК-датчики, выход датчиков будет 0, если он обнаружит белый и 1, если он обнаружит черный цвет. Итак, помните об этом:

4.1. Объявление и инициализация переменных

Давайте инициализируем все переменные в 0 и пусть начальная скорость двигателя будет равна 100. Также инициализируем порты. Можно так же использовать последовательный монитор для просмотра значений разных переменных. Это будет полезно для настройки.

float Kp=0,Ki=0,Kd=0;
float error=0, P=0, I=0, D=0, PID_value=0;
float previous_error=0, previous_I=0;
int sensor[5]={0, 0, 0, 0, 0};
int initial_motor_speed=100;
/*
В данном разделе переменных можно указать свои номера портов для подключения
драйвера двигателей
*/
#define pwm1 9
#define pwm2 3
#define MotLeftP1 7
#define MotLeftP2 6
#define MotRightP1 5
#define MotRightP2 4

void read_sensor_values(void);
void calculate_pid(void);
void motor_control(void);

void setup()
{
   pinMode(pwm1,OUTPUT); //PWM Pin 1
   pinMode(pwm2,OUTPUT); //PWM Pin 2
   pinMode(MotLeftP1,OUTPUT); //Left Motor Pin 1
   pinMode(MotLeftP2,OUTPUT); //Left Motor Pin 2
   pinMode(MotRightP1,OUTPUT); //Right Motor Pin 1
   pinMode(MotRightP2,OUTPUT); //Right Motor Pin 2
   Serial.begin(9600); //Enable Serial Communications
}

4.2 Расчет ошибок:

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

void read_sensor_values()
{
   sensor[0]=digitalRead(A0);
   sensor[1]=digitalRead(A1);
   sensor[2]=digitalRead(A2);
   sensor[3]=digitalRead(A3);
   sensor[4]=digitalRead(A4);

   if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==1))
     error=4;
     else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==1)&&(sensor[4]==1))
            error=3;
            else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==1)&&(sensor[4]==0))
                   error=2;
                   else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[4]==1)&&(sensor[4]==0))
                         error=1;
                         else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[4]==0)&&(sensor[4]==0))
                               error=0;
                               else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==1)&&(sensor[4]==0)&&(sensor[4]==0))
                                     error=-1;
                                     else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
                                        error=-2;
                                        else if((sensor[0]==1)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
                                            error=-3;
                                            else if((sensor[0]==1)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
                                                 error=-4;
                                                 else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
   if(error==-4) error=-5;
     else error=5;
}

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

4.3 Вычисление значения PID

Теперь значение из переменной Error, которая рассчитывается в функции read_sensor_values() следует обработать в calculate_pid, которая будет вычислять значение PID и отправлять это значение на двигатель. Ошибка будет добавляться к интегральному члену на каждой итерации. Кроме того, текущее значение «error» должно стать значением «previous_error» для следующей итерации. 

void calculate_pid()
{
   P = error;
   I = I + previous_I;
   D = error-previous_error;
   PID_value = (Kp*P) + (Ki*I) + (Kd*D);
   previous_I=I;
   previous_error=error;
}

4.4 Блок управления двигателем

Значение PID_value может быть положительным или отрицательным. Поэтому, если оно отрицательное, то скорость левого двигателя увеличивается, а скорость правого двигателя уменьшается, и наоборот, если значение PID положительное.

void motor_control()
{
  // Расчет оптимальной скорости двигателя
   int left_motor_speed = initial_motor_speed-PID_value;
   int right_motor_speed = initial_motor_speed+PID_value;

  // Скорость двигателя не должна превышать максимальное значение ШИМ
  constrain(left_motor_speed,0,255);
  constrain(right_motor_speed,0,255);
  analogWrite(pwm1,initial_motor_speed-PID_value); //Left Motor Speed
  analogWrite(pwm2,initial_motor_speed+PID_value); //Right Motor Speed

  //следующие строки кода - заставят бота двигаться вперед
  digitalWrite(MotLeftP1,HIGH);
  digitalWrite(MotLeftP2,LOW);
  digitalWrite(MotLeftP1,LOW);
  digitalWrite(MotLeftP2,HIGH);
}

4.5. Функция цикла loop()

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

void loop ()
{
read_sensor_values ​​();
calculate_pid ();
motor_control()
}

5. Настройка значений Kp, Ki, Kd

Это самая важная часть вашей программы. Константы PID, т. е. Kp, Ki и Kd, настраиваются только методом проб и ошибок. Эти значения будут разными для каждого робота и для каждой конфигурации. Попробуйте этот метод во время настройки:

Это самая важная часть вашей программы. Константы PID, то есть., Kp, ki и KD значения настраиваются только методом проб и ошибок. Эти значения будут различны для каждого робота и для каждой конфигурации.

• Попробуйте следующий метод для настройки:
• Начните с Kp, Ki и Kd равными 0 и работать с Kp в первую очередь. Попробуйте установить значение Kp равным 1 и понаблюдайте за роботом. Цель состоит в том, чтобы заставить робота следовать по линии, даже если линия очень узкая или нечеткая. Если робот потеряет линию, уменьшите значение Kp. Если робот не сможет поворачивать или Вам кажется, что робот долго ищет направление поворота , увеличьте значение Kp.
• Как только робот сможет немного двигаться по линии, присвойте Kd значение 1 (пропустите настройку Ki в данный момент). Попробуйте увеличить это значение, пока не увидите меньшее количество колебаний робота на линии.
• После того, как робот будет достаточно устойчив при движении по линии, присвойте Ki значение от 0,5 до 1,0. Если значение Ki слишком велико, робот будет часто дергаться влево и вправо. Поскольку Интеграл является кумулятивным, значение Ki оказывает значительное влияние. Вы можете регулировать Ki с шагом 0.01.
• После того, как робот будет двигаться по линии с хорошей точностью, вы можете увеличить скорость и посмотреть, сможет ли он по-прежнему следовать по линии. Скорость влияет на регулятор PID и будет требовать настройки по мере того как изменяется скорость.

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