Алгоритмы управления мобильным LEGO-роботом. Движение по линии Сергей Александрович Филиппов, Санкт-Петербургский Физико-математический лицей №239
Алгоритмы управления мобильным LEGO-роботом. Движение по линии
Сергей Александрович Филиппов, Санкт-Петербургский Физико-математический лицей №239
План занятия
Релейный двухпозиционный регулятор П-регулятор – движение по линии ПД-регулятор – движение по линии Кубическая составляющая Плавающий коэффициент Интегральная составляющая
Подключение датчикаМеню Robot -> Motors and Sensors Setup -> Sensors
#pragma config(Sensor, S1, , sensorLightActive)
Варианты расположения датчика освещенности
• Равносторонний треугольник, образованный точками опоры колес и датчиком
Релейный регулятор: движение вдоль границы черного и белого int grey=45; while (true) // grey - значение серого { if (SensorValue[S1]>grey){ motor[MotorB]=100; motor[MotorC]=0; } else{ motor[MotorB]=0; motor[MotorC]=100; } wait1Msec(1); }
Поиск значения серого: View - > Light Sensor (active) - > Port 1
Пропорциональный регулятор
В задачах автоматического регулирования управляющее воздействие u(t) обычно является функцией динамической ошибки – отклонения e(t) регулируемой величины x(t) от ее заданного значения x0(t):
e(t)=x0(t)-x(t)
Пропорциональный регулятор – это устройство, оказывающее управляющее воздействие на объект пропорционально его отклонению от заданного состояния.
u0(t)=keЗдесь k – это коэффициент усиления регулятора.
Пропорциональный регулятор: движение по линии Также как и в релейном регуляторе,
необходимо определить среднее значение grey между черным и белым. Это будет то состояние датчика освещенности s1, к которому должна стремиться система.
float k=2, v=50; while(true) { u=k*(SensorValue[S1]-grey); motor[motorB]=v+u; motor[motorC]=v-u; wait1Msec(1); }
Калибровка Установить датчик на белое поле и включить робота
int white=SensorValue[S1]; PlaySound(soundBeepBeep);
Установить датчик на черное поле и нажать кнопку while(SensorValue[S2]==0); // Жди, пока не нажато wait1Msec(100); // Защита от залипаний while(SensorValue[S2]==1); // Жди, пока нажато wait1Msec(100); int black=SensorValue[S1]; PlaySound(soundBeepBeep); int grey=(white+black)/2;
Установить робота на старт и нажать кнопку while(SensorValue[S2]==0); wait1Msec(100); while(SensorValue[S2]==1);
Калибровка с выводом на экран ...
int grey=(white+black)/2;
Взять робота и посмотреть на экран
nxtDisplayBigTextLine(1, “white=%d”, white); nxtDisplayBigTextLine(3, “black=%d”, black); nxtDisplayBigTextLine(5, “grey=%d”, grey);
Установить робота на старт и нажать кнопку
while(SensorValue[S2]==0); wait1Msec(100); while(SensorValue[S2]==1);
Калибровка с одной переменной Установить датчик на белое поле и включить робота
int grey=SensorValue[S1]; PlaySound(soundBeepBeep);
Установить датчик на черное поле и нажать кнопку
while(SensorValue[S2]==0); // Жди, пока не нажато wait1Msec(100); // Защита от залипаний while(SensorValue[S2]==1); // Жди, пока нажато wait1Msec(100); grey=(grey+SensorValue[S1])/2; PlaySound(soundBeepBeep);
Установить робота на старт и нажать кнопку
while(SensorValue[S2]==0); wait1Msec(100); while(SensorValue[S2]==1);
Автоматическая круговая калибровка Установить робота на линию на границу черного и белого и включить
int white=SensorValue[S1]; int black=SensorValue[S1]; motor[motorB]=30; motor[motorC]=-30; nMotorEncoder[motorB]=0;
while(nMotorEncoder[motorB]<239*4) // Полный поворот на месте
{ if (SensorValue[S1]>white) white=SensorValue[S1]; if (SensorValue[S1]<black) black=SensorValue[S1]; wait1Msec(5); } motor[motorB]=0; motor[motorC]=0; grey=(white+black)/2; PlaySound(soundBeepBeep);
Вывести результат на экран Установить робота на старт и нажать кнопку
Калибровка в процессе движения
Установить робота на белое и включить
int white=SensorValue[S1]; int black=SensorValue[S1]-10; int grey=(white+black)/2;
while(true) { int Sv1=SensorValue[S1]; u=k*(Sv1-grey); motor[motorB]=v+u; motor[motorC]=v-u; if (Sv1>white) white=Sv1; if (Sv1<black) black=Sv1; grey=(white+black)/2; wait1Msec(1); } Для отладки выводить результат калибровки на экран
И-регулятор: накопление ошибки
Накопление ошибки в интегральной составляющей
float i=0, ki=0.01; while(true) { e=S1-grey; i=i+ki*e; u=i; motor[motorB]=v+u; motor[motorC]=v-u; wait1Msec(1); }
И-регулятор: управление с ограничением
Ограничение интегральной составляющей в пределах допустимого управляющего воздействия
float i=0, ki=0.01, maxi=50; while(true) { e=S1-grey; i=i+ki*e; if (abs(i)>maxi) i=sgn(i)*maxi; u=i; motor[motorB]=v+u; motor[motorC]=v-u; wait1Msec(1); }
ПИ-регулятор с ограничением
Объединение интегральной и пропорциональной составляющей в пределах допустимого управляющего воздействия
float i=0, kp=1, ki=0.01, maxi=50; while(true) { e=S1-grey; p=kp*e; i=i+ki*e; if (abs(i)>maxi) i=sgn(i)*maxi; u=p+i; motor[motorB]=v+u; motor[motorC]=v-u; wait1Msec(1); }
Кубический регулятор
Рассчитывается отклонение
e=SensorValue[S1]-grey Возводится в куб
uk=k2*(e*e*e), где k2≈0.01 Управляющее воздействие:
пропорциональное+кубическое
upk = k1*e + k2*e*e*e
Коэффициент k2 при кубической составляющей должен быть такой, чтобы при максимальном отклонении получать величину, сравнимую с мощностью моторов (100). Коэффициент k1 может быть невелик.
up
uk
up
uk
Движение по границе черного и белого с кубическим регулятором
Резкие повороты обеспечиваются кубическим регулятором. При малых отклонениях он не оказывает большого влияния
float k1=2, k2=0.03; while(true) { e=SensorValue[S1]-grey; u=k1*e + k2*e*e*e; motor[motorB]=50+u; motor[motorC]=50-u; wait1Msec(1); }
Движение по границе черного и белого с помощью ПД-регулятора
ud=k*(S1-Sold)/Δt, где S1 – текущие показания
датчика, Sold – показания на предыдущем шаге.
Дифференциальная составляющая компенсирует пропорциональную
u = up+ud u = k1*(S1-grey) + k2*(S1-Sold)
Можно показать, что для устойчивого достижения цели коэффициент k2 при дифференциальной составляющей должен превышать k1
S1
Soldup
ud
ПД-регулятор: вариант записи
Использование ошибки в Д-составляющей
eold=0; while(true) { e=SensorValue[S1]-grey; u=k1*e + k2*(e-eold); eold=e; motor[motorB]=v+u; motor[motorC]=v-u; wait1Msec(1); }
ПИД-регулятор
float ki=0.01, kp=1, kd=10, i=0, eold=0; while(true) { e=SensorValue[S1]-grey; p=kp*e; i=i+ki*e; if (abs(i)>maxi) i=sgn(i)*maxi; d=kd*(e-eold); eold=e; u=p+i+d; motor[motorB]=v+u; motor[motorC]=v-u; wait1Msec(1); }
Пропорциональный регулятор: движение
по линии с двумя датчиками При двух белых едет прямо, при одном черном поворачивает в
сторону черного, при двух черных едет прямо. Используем разность показаний.
while(true) { u=k1*(s1-s2); motor[motorB]=50+u; motor[motorC]=50-u; wait1msec(1); }
s1 s2
Пропорциональный регулятор: движение
по линии с двумя датчиками Датчики могут быть откалиброваны по-
разному Рассчитывается разность отклонений Начальные значения на одноцветной
поверхности
left=s1; right=s2; while(true) { u=k1*((s1-left)-(s2-right)); motor[motorB]=50+u; motor[motorC]=50-u; wait1Msec(1); }
Пропорциональный регулятор: устранение статической ошибки Пусть статическая ошибка показывает разность
показаний датчиков на белом: es = left – rightДинамическа ошибка e вычисляется в движении
и компенсируется статической ошибкой
e = (sv1-left)-(sv2-right) = sv1 – sv2 – es
es=sv1-sv2; while(true) { e=sv1-sv2-es; u=k1*e; motor[motorB]=v+u; motor[motorC]=v-u; wait1Msec(1); }
ПД-регулятор: движение по линии с двумя датчиками Запоминаем отклонение в переменной e,
предыдущее – в переменной eold Добавим контроль скорости на поворотахv=100-abs(u)*0.2
es=SensorValue[s1]-SensorValue[s2]; eold=0; while(true) { e=SensorValue[s1]-SensorValue[s2]-es; u=k1*e+k2*(e-eold); eold=e; v=100-abs(u)*0.2; motor[motorB]=v+u; motor[motorC]=v-u; wait1msec(1); }
s1 s2
Плавающий коэффициент
Центральный датчик «предсказывает будущее»
Центральный датчик калибруется на черном
center=S3 Пропорциональный
коэффициент зависит от его отклонения с линии
k1=1+(S3-center)/3
u=k1*e
s1 s2
s3
Плавающий коэффициент для пропорционального регулятора Плавающий коэффициент можно применить к
любому регулятору
left=s1; right=s2; center=s3; while(true) { e=(s1-left)-(s2-right); k1=1+(s3-center)/10; u=k1*e; Motor[MotorB]=50+u; Motor[MotorC]=50-u; wait1msec(1); }
s1 s2
s3
Инверсная линия
Предварительная калибровка двух датчиков
Движение на одном датчике на П- или ПД-регуляторе
Второй датчик рядом с первым следит за цветом поля
Изменение знака управляющего воздействия (направления управления) в зависимости от показаний второго датчика.
1 2
Инверсная линия
while(true)
{
if (SensorValue[s2]>grey2)
r=1;
else
r=-1;
e=SensorValue[S1]-grey1;
u=r*(k*e);
motor[MotorB]=50+u;
motor[MotorC]=50-u;
wait1msec(1);
}
1 2
Следование за объектом: двойное регулирование Регулирование
движения по линии с одним или двумя датчиками освещенности
Регулирование расстояния с ультразвуковым датчиком Релейное Пропорциональное
Контроль расстояния с совмещением регуляторов по датчику ультразвука
// ПРОПОРЦИОНАЛЬНЫЙ
int e;
float v, k=5;
while(true)
{
e=SensorValue[S3]-30;
v=e*k;
if (v>100) v=100;
motor[motorB]=v;
motor[motorC]=v;
wait1Msec(1);
}
// РЕЛЕЙНЫЙ
while(true)
{
if (SensorValue[S3]>30)
{ v=100; } // Сюда
else
v=0;
motor[motorB]=v;
motor[motorC]=v;
wait1Msec(1);
}
Контроль расстояния с совмещением регуляторов по датчику ультразвука
float e, v, kv=5;
while(true)
{
e=SensorValue[S3]-30;
if (e>0)
v=e*k;
else
v=0;
if (v>100) v=100;
motor[motorB]=v;
motor[motorC]=v;
wait1Msec(1);
}
Расчет отклонения по расстоянию
Запрет движения назад при отрицательном отклонении
Ограничение скорости не более 100
Контроль линии и расстояния с датчиком освещенности и ультразвука
float e, v, kv=5;while(true){ // Velocity control e=SensorValue[S3]-30; if (e>0) v=e*kv; else v=0; if (v>100) v=100; motor[motorB]=v; motor[motorC]=v; wait1Msec(1);}
int e, grey=45;
float u, v=50, kp=3;
while(true)
{ // Follow line
e=SensorValue[S1]-grey;
u=e*kp;
motor[motorB]=v+u;
motor[motorC]=v-u;
wait1Msec(1);
}
Добавление контроля расстояния до объекта через скорость
Движение по линии с контролем расстояния: результат совмещения
float u, v, e, kp=3, kv=5;
while(true) {
e=SensorValue[S3]-30;
if(e>0)
v=e*kv;
else v=0;
if (v>100) v=100; //Вместо grey
e=SensorValue[S1]-SensorValue[S2];
u=e*kp;
motor[motorB]=v+u;
motor[motorC]=v-u;
wait1Msec(1);
}
Регулирование скорости движения в зависимости от расстояния до объекта спереди
Регулирование направления вдоль линии по двум датчикам освещенности
Подача комплекса управляющих воздействий на моторы
Контроль скорости с использованием интегральной составляющей
int ev;
float p,i=0,kv=0.5,ki=0.01;
while(true)
{ // Velocity control
ev=SensorValue[S3]-30;
p=ev*kv;
i=i+ev*ki;
v=p+i;
...
wait1Msec(1);
}
При высоком коэффициенте kv теряется возможность управления направлением (робот сбивается с линии)
При низком коэффициенте kv робот не доезжает на заданное расстояние из-за нехватки мощности моторов
Интегральная составляющая суммирует ошибку и быстро увеличивает скорость
Контроль превышения скорости
int ev;
float p,i=0,kv=0.5,ki=0.01;
while(true)
{ // Velocity control
ev=SensorValue[S3]-30;
p=ev*kv;
i=i+ev*ki;
if (i>100) i=100;
v=p+i;
...
wait1Msec(1);
}
При высоком коэффициенте kv теряется возможность управления направлением (робот сбивается с линии)
При низком коэффициенте kv робот не доезжает на заданное расстояние из-за нехватки мощности моторов
Интегральная составляющая суммирует ошибку и быстро увеличивает скорость
Объезд препятствияint grey1, grey2;
void objezd()
{
...
}
task main()
{ ...
while(true)
{
...
objezd();
...
}
}
Подпрограмма объезда вызывается при условии, что препятствие близко
Подпрограмма приостанавливает цикл регулирования, поскольку выполняется последовательно
Переменные grey1 и grey2 объявляются глобально для использования в основной программе и подпрограмме
Благодарю за внимание!
Сергей Александрович Филиппов Физико-Математический лицей № 239
Санкт-Петербург[email protected]