Разработка алгоритмов для поддержания стабильной скорости движения мобильного робота с дифференциальным приводом

Размещено на http://www.allbest.ru/

Введение

алгоритм робот управление

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

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

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

Постановка задачи

· Выполнить анализ типов приводов мобильных платформ

· Выполнить анализ подходов к стабилизации скорости

· Разработать алгоритм стабилизации скорости мобильной платформы Rover 5 с дифференциальным приводом.

· Систему управления реализовать на основе микроконтроллера Arduino.

· Протестировать данную систему

1. Анализ типов приводов мобильных платформ

Дифференциальный привод: мобильная платформа с таким типом привода (Рис. 1.1) имеет два активных колеса. Разница между скоростями колес определяет угол поворота. Это наиболее часто используемая кинематическая модель [5].

Рис. 1.1 Схема дифференциального привода

Синхронный привод: в мобильной платформе с данным типом привода (Рис. 1.2) все колеса приводятся в движение с одинаковой скоростью. Также все колеса вращаются одновременно и всегда параллельны друг другу [5].

Рис. 1.2 Схема синхронного привода

Трицикл: мобильная платформа с таким приводом (Рис. 1.3) имеет два мотора: один для движения, другой для рулежки. Только переднее колесо может поворачивать. Задние колеса приводят в движение робота [5].



Рис. 1.3 Схема привода трицикла

Всенаправленный привод: мобильная платформа с таким (Рис. 1.4) приводом может двигаться во всех направлениях от начальной точки. Такие роботы имеют специальные колеса, которые позволяют им добиться этого [5].

Рис. 1.4 Схема всенаправленного привода

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

· скорость или ускорение движения;

· точность позиционирования (повторяемость);

· гибкость и робастность (надежность) при различных условиях;

· эффективность (низкое энергопотребление).

2. Мобильная платформа с дифференциальным приводом

Rover 5 Chassis

Rover 5 Chassis (Рис. 2.1) - это роботизированное шасси с гусеницами, созданное специально для студентов или любителей. В отличие от обычных гусеничных шасси дорожный просвет может быть настроен путем поворота редукторов с шагом в 5 градусов. "Растягивающиеся" резиновые протекторы сохраняют натяжение с увеличением дорожного просвета.

Рис. 2.1 Гусеничная платформа Rover 5

Характеристики встроенных энкодеров, которые изображены на рисунке 2.2: 1000 импульсов на 3 оборота выходного вала (333,33 импульса за оборот).

Характеристики платформы:

· Длина: 235мм

· Ширина: 220мм

· Высота: 90мм

· Вольтаж двигателя рабочий: 7.2V

· Стартовый ток двигателя: 2.5A

· Усилие редуктора: 10кг/см

· Передаточное число: 86.8:1

· Скорость: 1км/ч [4]



Рис. 2.2 Встроенный энкодер

На рисунке 2.3 изображен чертеж мобильной платформы Rover 5, на котором указаны его размеры.

Рис. 2.3 Чертёж Rover 5

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

3.1 Обзор подходов к стабилизации скорости

Пропорциональная стабилизация скорости:

Равномерное изменение скорости при её регулировании

При отклонении скорости от заданной происходит увеличение (или уменьшение) скорости на определённое заданное число (коэффициент k).

При этом скорость изменяется линейно. При достижении заданного уровня скорости происходят автоколебания (Рис. 3.1).

Рис. 3.1 Графики пропорциональной стабилизации скорости

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

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



Рис.3.2 График использования пропорциональной стабилизации

Стабилизация скорости по отклонению от заданного значения:

Скорость регулировки зависит от степени отклонения.

При большом отклонени скорость нарастает быстро, при малом - медленно.

При этом скорость изменяется нелинейно (Рис. 3.3).

Рис. 3.3 Графики стабилизации скорости по отклонению

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



Рис.3.4 График использования стабилизации скорости по отклонению

ПИД-регулирование:

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

где P -- пропорциональная составляющая; I -- интегральная составляющая; D -- дифференциальная составляющая; e(t) - текущая ошибка; Kp -- пропорциональный коэффициент; Ki -- интегральный коэффициент; Kd -- дифференциальный коэффициент [3].

В программной реализации переходят к дискретной форме:

u(t) = P (t) + I (t) + D (t);

P (t) = Kp * e (t);

I (t) = I (t -- 1) + Ki * e (t);

D (t) = Kd * {e (t) -- e (t -- 1)}.

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

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



Рис. 3.5 Графики стабилизации скорости ПИД-регулированием

3.2 Программно-аппаратная реализация

Система стабилизация скорости мобильной платформы Rover 5 реализована на микроконтроллере Arduino Mega (Рис. 3.6). Для управления двигателями использован Motor Driver (Рис. 3.7).

Рис. 3.6 Arduino Mega 2560

Таблица 1. Характеристики Arduino Mega [1]

Микроконтроллер	ATmega2560
Рабочее напряжение	5В
Входное напряжение (рекомендуемое)	7-12В
Входное напряжение (предельное)	6-20В
Цифровые Входы/Выходы	54 (14 из которых могут работать также как выходы ШИМ)
Аналоговые входы	16
Постоянный ток через вход/выход	40 mA
Постоянный ток для вывода 3.3 В	50 mA
Флеш-память	256 KB (из которых 8 КB используются для загрузчика)
ОЗУ	8 KB
Энергонезависимая память	4 KB
Тактовая частота	16 MHz

Рис. 3.7 Motor Driver

Таблица 2. Характеристики Motor Driver [2]

Напряжение питания логики	5В
Максимальное напряжение питания двигателей	12В
Максимальный ток питания двигателей	4,5А

Двигатели Rover 5 питаются от батареи напряжением 11,5В и подключены к плате Motor Driver к которой также подключены оба энкодера (Рис. 3.8).

На энкодерах: 1 - Vcc; 2 и 3 - выходы сигналов A и B; 4 - Gnd.



Рис. 3.8 Схема подключения Arduino Mega и Motor Driver

Платы Arduino Mega и Motor Driver крепятся сверху мобильной платформы Rover 5 (Рис. 3.9).

Рис. 3.9 Rover 5 с подключенной электроникой

При программировании Arduino будут проверены 2 метода стабилизации скорости: пропорциональный и стабилизации по отклонению.

spe_1 - итоговое число импульсов за период с 1-го энкодера

spe_2 - итоговое число импульсов за период с 2-го энкодера

enk - число импульсов с энкодера для установления нужной скорости

a - коэф. изменения ШИМ

shim1 - уровень ШИМ 1-го двигателя

shim2 - уровень ШИМ 2-го двигателя

P - максимально допустимое число ШИМ

Алгоритм пропорциональной стабилизации скорости:

//определение ШИМ для 1-го двигателя

if (spe_1<enk) shim1 = shim1+a;

if (spe_1>enk) shim1 = shim1-a;

if (shim1>P) {shim1 = P;} //ограничение ШИМ

if (shim1<0) {shim1 = 0;}

//определение ШИМ для 2-го двигателя

if (spe_2<enk) shim2 = shim2+a;

if (spe_2>enk) shim2 = shim2-a;

if (shim2>P) {shim2 = P;} //ограничение ШИМ

if (shim2<0) {shim2 = 0;}

Алгоритм стабилизации скорости по отклонению:

//определение ШИМ для 1-го двигателя

if (spe_1<enk) //если число импульсов с 1-го энкодера < заданного

{

K = enk-spe_1; K = K/enk*a;

shim1 = round(shim1+K); //увеличение ШИМ для 1-го двигателя

}

if (spe_1>enk) //если число импульсов с 1-го энкодера > заданного

{

K = spe_1-enk; K = K/spe_1*a;

shim1 = round(shim1-K); //уменьшение ШИМ для 1-го двигателя

}

if (shim1>P) shim1 = P; //ограничение ШИМ

if (shim1<0) shim1 = 0;

//определение ШИМ для 2-го двигателя

if (spe_2<enk) //если число импульсов с 2-го энкодера < заданного

{

K = enk-spe_2; K = K/enk*a;

shim2 = round(shim2+K); //увеличение ШИМ для 2-го двигателя

}

if (spe_2>enk) //если число импульсов с 2-го энкодера > заданного

{

K = spe_2-enk; K = K/spe_2*a;

shim2 = round(shim2-K); //уменьшение ШИМ для 2-го двигателя

}

if (shim2>P) shim2 = P; //ограничение ШИМ

if (shim2<0) shim2 = 0;

4. Результаты тестирования системы

Информация о данных с энкодеров и уровня ШИМ выводилась в COM порт. По этим данным строились графики, на которых указаны: заданная скорость, скорость 1го двигателя, скорость 2го двигателя (измеряются числом импульсов с энкодера за период Т) и уровень мощностей 1го и 2го двигателя (определялся по ШИМ).

Тестирование пропорциональной стабилизации скорости:

Тестирования проводились с разными коэффициентами изменения скорости: низким k=2 и высоким k=5, период измерения импульсов с энкодеров Т=50мс (Рис. 4.1).

Рис. 4.1 Графики пропорциональной стабилизации скорости

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

Тестирование стабилизации скорости по отклонению:

Тестирования проводились с разными коэффициентом изменения скорости: высоким k=50 и низким k=5, период измерения импульсов с энкодеров Т=100мс (Рис. 4.2).

Рис. 4.2 Графики стабилизации скорости по отклонению

Как видно коэффициент изменения скорости влияет на скорость стабилизации, при этом видно, что скорость изменяется нелинейно. Отклонения скорости являются небольшими (всего на 1 импульс от заданной), но они быстро убираются. Это указывает на высокое быстродействие стабилизации.

Таблица 3. Сравнение ошибок стабилизации

Тип стабилизации	Значения коэф. k	СКО
пропорциональная	2	2,2294
	5	1,7174
по отклонению	50	1,5377
	5	1,5912

Заключение

В ходе выполнения данного курсового проекта были выполнены следующие задачи:

· Анализ типов приводов мобильных платформ;

· Анализ подходов к стабилизации скорости;

· Разработка алгоритмов стабилизации скорости;

· Реализация системы управления;

· Тестирование системы.

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

Так же были получены новые знания и навыки для работы с мобильной платформой Rover 5, разработки алгоритмов стабилизации и программировании Arduino.

Приложение

Код программы пропорциональной стабилизации скорости

#include <SimpleTimer.h>

int dirPin1 = 9; //пин направления 1-го двигателя

int pvmPin1 = 10; //пин шим-а 1-го двигателя

int dirPin2 = 7; //пин направления 2-го двигателя

int pvmPin2 = 6; //пин шим-а 2-го двигателя

int encoderPinA = 2; //пин считывающий сигналы энкодера 1 (они вызывают прерывание)

int encoderPinB = 3; //пин считывающий сигналы энкодера 2 (они вызывают прерывание)

int time_1; //счетчик прерываний(каждый импульс с энкодера вызывает прерывание) 1-го энкодера

int spe_1; //итоговое число импульсов за период с 1-го энкодера

int time_2; //счетчик прерываний(каждый импульс с энкодера вызывает прерывание) 2-го энкодера

int spe_2; //итоговое число импульсов за период со 2-го энкодера

SimpleTimer timer; //конструктор (см библиотеку SimpleTimer)

String S = ""; //строка считанного числа скорости

int U = 0; //заданная скорость в процентах (U = от 0% до 100%)

float MaxEnk = 10; //значение энкодера при U = 100%

int a = 2; //число изменения ШИМ

int P = 200; //максимально допустимое число ШИМ

int T = 50; //период подсчитывания числа импульсов с энкодеров

int shim1 = 0; //уровень ШИМ 1-го двигателя

int shim2 = 0; //уровень ШИМ 2-го двигателя

int enk = 0; //число импульсов с энкодера для установления нужной скорости

void setup()

{

Serial.begin(9600); //скорость обмена данными с COM портом

pinMode(pvmPin1, OUTPUT); //настройка портов на прием или передачу данных

pinMode(dirPin1, OUTPUT);

pinMode(pvmPin2, OUTPUT);

pinMode(dirPin2, OUTPUT);

pinMode (encoderPinA,INPUT);

pinMode (encoderPinB,INPUT);

attachInterrupt(0, encoder_1, RISING); //настройка прерывания 0-номер прерывания, которому соответствует порт 2 encoder_1 процедура вызываемая прерыванием.

attachInterrupt(1, encoder_2, RISING); //RISING - режим прерывания означающий что прервание возникнет при переходе от низкого уровня сигнала к высокому

timer.setInterval(T, timer_1); //настройка таймера: после T мс запускать процедуру timer_1

digitalWrite(dirPin1,LOW); //направление 1-го двигателя

digitalWrite(dirPin2,LOW); //направление 2-го двигателя

}

void loop()

{

timer.run(); //запуск таймера

while (Serial.available() > 0) //если в буфере есть символы

{

int inChar = Serial.read(); //считать символ

if (isDigit(inChar)) //если символ является цифрой

{

S += (char)inChar; //прибавить символ к строке

}

if (inChar == '!') //символ окончания строки

{

U = S.toInt(); //перевод string в integer

S = ""; //очистка строки

}

}

//ограничения в заданной скорости

if (U>100) U = 100;

if (U<0) U = 0;

//алгоритм стабилизации скорости

enk = round((MaxEnk/100)*U); //необходимое число импульсов на энкодере для установления нужной скорости

if (enk == 0) //если число импульсов равно 0

{ //ШИМ равен 0

shim1 = 0;

shim2 = 0;

}

else

{

//определение ШИМ для 1-го двигателя

if (spe_1<enk) shim1 = shim1+a;

if (spe_1>enk) shim1 = shim1-a;

if (shim1>P) {shim1 = P;} //ограничение ШИМ

if (shim1<0) {shim1 = 0;}

//определение ШИМ для 2-го двигателя

if (spe_2<enk) shim2 = shim2+a;

if (spe_2>enk) shim2 = shim2-a;

if (shim2>P) {shim2 = P;} //ограничение ШИМ

if (shim2<0) {shim2 = 0;}

}

//установление уровней ШИМ для двигателей

analogWrite(pvmPin1, shim1);

analogWrite(pvmPin2, shim2);

}

void encoder_1()

{

time_1+=1; //счетчик прерываний (числа импульсов с 1-го энкодера)

}

void encoder_2()

{

time_2+=1; //счетчик прерываний (числа импульсов с 2-го энкодера)

}

void timer_1() //подпрограмма, запускаемая каждые T мс

{

spe_1 = time_1; //число импульсов с 1-го энкодера

time_1 = 0; //обнуление счетчика прерываний

spe_2 = time_2; //число импульсов с 2-го энкодера

time_2 = 0; //обнуление счетчика прерываний

//вывод данных

Serial.print(U); //вывод заданной скорости %

Serial.print(" ");

Serial.print(enk); //вывод расчётного числа импульсов с энкодера для установления нужной скорости

Serial.print(" ");

Serial.print(spe_1); //вывод числа импульсов с 1-го энкодера

Serial.print(" ");

Serial.print(spe_2); //вывод числа импульсов с 2-го энкодера

Serial.print(" ");

Serial.print(shim1); //вывод уровня ШИМ для 1-го двигателя

Serial.print(" ");

Serial.println(shim2); //вывод уровня ШИМ для 2-го двигателя

}

Код программы стабилизации скорости по отклонению

#include <SimpleTimer.h>

int dirPin1 = 9; //пин направления 1-го двигателя

int pvmPin1 = 10; //пин шим-а 1-го двигателя

int dirPin2 = 7; //пин направления 2-го двигателя

int pvmPin2 = 6; //пин шим-а 2-го двигателя

int encoderPinA = 2; //пин считывающий сигналы энкодера 1 (они вызывают прерывание)

int encoderPinB = 3; //пин считывающий сигналы энкодера 2 (они вызывают прерывание)

int time_1; //счетчик прерываний(каждый импульс с энкодера вызывает прерывание) 1-го энкодера

int spe_1; //итоговое число импульсов за период с 1-го энкодера

int time_2; //счетчик прерываний(каждый импульс с энкодера вызывает прерывание) 2-го энкодера

int spe_2; //итоговое число импульсов за период со 2-го энкодера

SimpleTimer timer; //конструктор (см библиотеку SimpleTimer)

String S = ""; //строка считанного числа скорости

int U = 0; //заданная скорость в процентах (U = от 0% до 100%)

float MaxEnk = 8; //значение энкодера при U = 100%

int a = 50; //коэф. изменения ШИМ

int P = 200; //максимально допустимое число ШИМ

int T = 50; //период подсчитывания числа импульсов с энкодеров

float K; //вычисляющийся коэф. (К = от 0 до 1)

int shim1 = 0; //уровень ШИМ 1-го двигателя

int shim2 = 0; //уровень ШИМ 2-го двигателя

int enk = 0; //число импульсов с энкодера для установления нужной скорости

void setup()

{

Serial.begin(9600); //скорость обмена данными с COM портом

pinMode(pvmPin1, OUTPUT); //настройка портов на прием или передачу данных

pinMode(dirPin1, OUTPUT);

pinMode(pvmPin2, OUTPUT);

pinMode(dirPin2, OUTPUT);

pinMode (encoderPinA,INPUT);

pinMode (encoderPinB,INPUT);

attachInterrupt(0, encoder_1, RISING); //настройка прерывания 0-номер прерывания, которому соответствует порт 2 encoder_1 процедура вызываемая прерыванием.

attachInterrupt(1, encoder_2, RISING); //RISING - режим прерывания означающий что прервание возникнет при переходе от низкого уровня сигнала к высокому

timer.setInterval(T, timer_1); //настройка таймера: после T мс запускать процедуру timer_1

digitalWrite(dirPin1,LOW); //направление 1-го двигателя

digitalWrite(dirPin2,LOW); //направление 2-го двигателя

}

void loop()

{

timer.run(); //запуск таймера

while (Serial.available() > 0) //если в буфере есть символы

{

int inChar = Serial.read(); //считать символ

if (isDigit(inChar)) //если символ является цифрой

{

S += (char)inChar; //прибавить символ к строке

}

if (inChar == '!') //символ окончания строки

{

U = S.toInt(); //перевод string в integer

S = ""; //очистка строки

}

}

//ограничения в заданной скорости

if (U>100) U = 100;

if (U<0) U = 0;

}

void encoder_1()

{

time_1+=1; //счетчик прерываний (числа импульсов с 1-го энкодера)

}

void encoder_2()

{

time_2+=1; //счетчик прерываний (числа импульсов с 2-го энкодера)

}

void timer_1() //подпрограмма, запускаемая каждые T мс

{

spe_1 = time_1; //число импульсов с 1-го энкодера

time_1 = 0; //обнуление счетчика прерываний

spe_2 = time_2; //число импульсов с 2-го энкодера

time_2 = 0; //обнуление счетчика прерываний

//алгоритм стабилизации скорости

enk = round((MaxEnk/100)*U); //необходимое число импульсов на энкодере для установления нужной скорости

if (enk == 0) //если число импульсов равно 0

{ //ШИМ равен 0

shim1 = 0;

shim2 = 0;

}

else

{

//определение ШИМ для 1-го двигателя

if (spe_1<enk) //если число импульсов с 1-го энкодера меньше заданного

{

K = enk-spe_1;

K = K/enk*a;

shim1 = round(shim1+K); //увеличение ШИМ для 1-го двигателя

}

if (spe_1>enk) //если число импульсов с 1-го энкодера больше заданного

{

K = spe_1-enk;

K = K/spe_1*a;

shim1 = round(shim1-K); //уменьшение ШИМ для 1-го двигателя

}

if (shim1>P) shim1 = P; //ограничение ШИМ

if (shim1<0) shim1 = 0;

//определение ШИМ для 2-го двигателя

if (spe_2<enk) //если число импульсов с 2-го энкодера меньше заданного

{

K = enk-spe_2;

K = K/enk*a;

shim2 = round(shim2+K); //увеличение ШИМ для 2-го двигателя

}

if (spe_2>enk) //если число импульсов с 2-го энкодера больше заданного

{

K = spe_2-enk;

K = K/spe_2*a;

shim2 = round(shim2-K); //уменьшение ШИМ для 2-го двигателя

}

if (shim2>P) shim2 = P; //ограничение ШИМ

if (shim2<0) shim2 = 0;

}

//установление уровней ШИМ для двигателей

analogWrite(pvmPin1, shim1);

analogWrite(pvmPin2, shim2);

//вывод данных

Serial.print(U); //вывод заданной скорости %

Serial.print(" ");

Serial.print(enk); //вывод расчётного числа импульсов с энкодера для установления нужной скорости

Serial.print(" ");

Serial.print(spe_1); //вывод числа импульсов с 1-го энкодера

Serial.print(" ");

Serial.print(spe_2); //вывод числа импульсов с 2-го энкодера

Serial.print(" ");

Serial.print(shim1); //вывод уровня ШИМ для 1-го двигателя

Serial.print(" ");

Serial.println(shim2); //вывод уровня ШИМ для 2-го двигателя

}

Размещено на Allbest.ru