ШАГ 1. Получения массива точек из функций траектории, заданной в параметрическом виде

Введение

Одной из часто возникающих задач при перемещении мобильного робота (МР) на плоскости является движение вдоль заранее выстроенной траектории. Критериями оценки выполнения данной задачи обычно выступают скорость и точность выполнения маневров. Повышение точности и скорости позиционирования МР является задачей, с которой часто сталкиваются разработчики и программисты при эксплуатации роботов в реальной жизни.

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

Рисунок 1 – Схема мобильного робота.

Робот, приведённый на рисунке 1, состоит из платформы, приводной оси и сферической всенаправленной опоры. Приводная ось состоит из двух колес, расположенных соосно и приводимых в движение отдельными приводами. Шаровая опора предназначена для придания роботу устойчивого горизонтального положения.

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

 

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

На первом шаге (1) подставляя в начальные уравнения [x=ϕ(t), y=g(t)] переменную t, c определенной дискретизацией, получим массив элементов [x(i), y(i)], состоящий из точек с координатами, описывающих траекторию движения МР. Здесь важным является выбор шага дискретизации. Чем выше дискретизация, тем большее количество элементов массива [x(i), y(i)] будет получено. Но это значение должно соотносится с возможностями работы системы управления роботом. Нет смысла делать высокую дискретизацию, когда робот не может ее обеспечивать.

На втором шаге (2) преобразуем массив из координат точек [x(i), y(i)] в массивы линейной и угловой мобильного робота (МР). С учетом дискретизации системы управления dt, рассчитаем линейную и угловую скорости робота в каждый момент времени:

На рисунке 3 приведен график произвольной траектории движения, состоящий из трех точек. Так же показаны углы, координаты точек и перемещения.

Рисунок 3 – Определение линейной и угловой скоростей МР

 

На третьем шаге (3) преобразуем угловую и линейную скорости центра колесной пары МР в угловые скорости вращения колес МР. Это делается по формуле:

 

 

Где  – угловая скорость правого колеса на i-ом участке траектории,

 - угловая скорость левого колеса на i-ом участке траектории,

 – расстояние между приводными колесами,

 – радиус приводного колеса.

 

 

 

Пример получения угловых скоростей приводных колес МР

 

Для работы будем применять программу MATLAB R2019b, но все вычисления можно сделать и в других инженерных программах, например, Octave, Maple, Mathematica и т.п, либо написав программу на любом языке программирования.

ШАГ 3. Определения угловых скоростей движения колес МР

 

Для определения угловых скоростей колес МР необходимо знать геометрические параметры робота. У робота, который будет использован для движения по траектории, межколёсное расстояние 27.4 см, а радиус колеса – 2.5 см. Приведем код для расчёта угловых скоростей колес робота:

r=0.025; % радиус колеса = 2,5см

l=0.274/2; % межколесное расстояние = 27,4см

for i= 1:masSize-1

wr(i) = (v(i)+l*w(i))/r; % угловая скорость правого колеса

wl(i) = (v(i)-l*w(i))/r; % угловая скорость левого колеса

end

figure(4);

hold on;

plot(wr,'*');

plot(wl,'--');

axis equal;

grid on;

 

График угловых скоростей движения колес МР представлен на рисунке 6.

Рисунок 6 – График угловой скорости левого (штриховая линия) и правого (точечная линия) колеса МР.

ШАГ 4. Запись данных в файл

 

fid = fopen(['myFile.csv'],'w'); % открытие файла на запись

if fid == -1                  % проверка корректности открытия

error('File is not opened');

end

formatSpec = '%f,%f\n';       % формат записи данных в файл

for i=1:masSize-1

fprintf(fid, formatSpec, v(i), w(i)); % запись в файл

end

fclose(fid); % закрытие файла

 

Весь код примера приведен в приложении Б.

Подготовка робота к прохождению траектории.

 

Рассмотрим задачу по оценке результатов движения МР вдоль указанной траектории. Для оценки точности и скорости движения, все автоматические перемещения МР фиксируются универсальной системой захвата движения Vicon (см.  рисунок 7).

Рисунок 7 – Система захвата движения Vicon

На испытуемом экземпляре робота должна быть расположена прямоугольная платформа со световозвращающими маркерами (выдается организаторами)

1) Один маркер по центру вращения робота (для построения траектории движения МР)

2) Второй маркер должен располагаться на продольной оси МР соосно с маркером, расположенным по центру вращения МР (по нему будет определяться угол расположения МР на старте)

Рисунок 8 – Платформа с маркерами

       Перед началом эксперимента робот располагается на полигоне наиболее удобным для съемок образом. Пример расположения приведен на рисунке 9.

Рисунок 9 – Пример расположения робота на полигоне. Красная сплошная линия – пример траектории в виде «восьмерки».

 

Движение вдоль траектории

 

Траектория МР, снятая на систему захвата движения Vicon по центральному маркеру, совмещается по углу расположения робота и координатам начала движения вдоль экспериментальной траектории с теоретической траекторий, полученной по заданной функции. Пример совмещения траекторий приведен на рисунке 10.

А)	Б)

Рисунок 10 – Совмещение траекторий. Красная сплошная линия – экспериментально полученная траектория, черная сплошная – график функции. А) несовмещенные траектории, Б) совмещенные траектории.

 

После совмещения графиков программно рассчитывается сумма расстояний между сответсвующими точками экспериметальной и теоретической траекториями. Расстояния между соответсвующими точками графиков отображены на рисунке 11.

Рисунок 11 – Графики экспериментальной и теоретической траекторий. Зеленые линии соединяют соответствующие точки данных траекторий.

 

Пример расчета баллов

 

Оценивается точность прохождения траектории и время движения. Формула для расчета баллов:

Где - время движения МР вдоль траектории,

 – максимальное время среди всех участников,

 – коэффициент времени,

 – суммарная относительная ошибка

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

 – коэффициент ошибки.

 

Победителем соревнований становится тот, кто наберет меньшее количество баллов. Так как точность прохождения вдоль трассы имеет больший приоритет чем время прохождения, то коэффициент ошибки =2, а коэффициент времени = 1. Пример расчета баллов участникам приведен в таблице 1.

Таблица 1 – Пример расчета баллов

		1
		2
		69,15
		14,5963
№	Команда	Время,с	Отн. ошибка,м	Баллы, Q	Место
1	Лосяш	61,58	9,6134	2,207766	2
2	Копатыч	69,15	13,0818	2,792482	3
3	Совунья	55,35	14,5963	2,800434	4
4	Пин	70,25	8,15	2,132629	1

 

Приложение А

Пример отчета

Рисунок А.1 – Пример отчета по траектории. Время приведено в секундах, ошибка – в метрах.

Рисунок А.2 – Пример отчета по траектории. Время приведено в секундах, ошибка – в метрах.

 

Приложение Б

Текст кода программы для среды Матлаб:

clc; % очистить командное окно

clear; % очистить рабочее пространство (удалить переменные)

close all; % закрыть все окна

i=1; % итератор

for t =0: 0.1: 2*pi % цикл старт:dt:стоп

x(i)=sin(t+(pi/2)); % массив х

y(i)=sin(2*t); % массив у

i=i+1; % увеличение итератора

end

masSize=i-1; % переменная с размером массива

figure(1); % график

plot(x,y,'*'); % график

axis equal; % оси одного масштаба

grid on; % сетка для осей

 

tettaStart = pi/2; % угол старта

for i=1:masSize-1 % цикл перебора от 1 через 1 до конца массива

dx=x(i+1)-x(i); % разность по х

dy=y(i+1)-y(i); % разность по у

dl(i)=(dx^2+dy^2)^(1/2); % расстояние по правилу Пифагора = скорость

tetta(i)=atan2(y(i+1)-y(i),x(i+1)-x(i)); % абсолютный угол

if tetta(i) < 0 % если угол меньше 0

   tetta(i)=tetta(i) + 2*pi; % переводим отрицательную часть в положительную

 end

if(i<2)

   dw(i)=tetta(i)-tettaStart; % первое значение угла

else

   dw(i)=tetta(i)-tetta(i-1); % остальные значения угла

end

if dw(i) > pi % если значение угла больше pi, есть смысл поворота в другую сторону

   dw(i) = dw(i) - 2*pi;

end

if dw(i) < -pi % -//-

   dw(i) = dw(i) + 2*pi;

end

v(i) = dl(i)/dt; % линейная скорость МР v=s/t

w(i) = dw(i)/dt; % угловая скорость МР

 end

figure(2);

hold on;

plot(v); % график линейной скорости центра МР

plot(w,'--'); % график угловой скорости центра МР

grid on;

 

 

xr(1)=1; % х координата старта

yr(1)=0; % у координата старта

tettar(1)=tettaStart; % угол старта

for i= 2:masSize

tettar(i)=tettar(i-1)+w(i-1)*dt;

xr(i)=xr(i-1)+v(i-1)*cos(tettar(i))*dt;

yr(i)=yr(i-1)+v(i-1)*sin(tettar(i))*dt;

end

f3=figure(3);

hold on;

plot(xr,yr);

axis equal;

grid on;

 

r=0.025; % радиус колеса = 2,5см

l=0.274/2; % межколесное расстояние = 27,4см

 for i= 1:masSize-1

wr(i) = (v(i)+l*w(i))/r; % угловая скорость правого колеса

wl(i) = (v(i)-l*w(i))/r; % угловая скорость левого колеса

end

figure(4);

hold on;

plot(wr,'*');

plot(wl,'--');

axis equal;

grid on;

 

fid = fopen(['myFile.csv'],'w'); % открытие файла на запись

if fid == -1                  % проверка корректности открытия

error('File is not opened');

end

formatSpec = '%f,%f\n';       % формат записи данных в файл

for i=1:masSize-1

fprintf(fid, formatSpec, v(i), w(i)); % запись в файл

end

fclose(fid); % закрытие файла

 

Введение

Одной из часто возникающих задач при перемещении мобильного робота (МР) на плоскости является движение вдоль заранее выстроенной траектории. Критериями оценки выполнения данной задачи обычно выступают скорость и точность выполнения маневров. Повышение точности и скорости позиционирования МР является задачей, с которой часто сталкиваются разработчики и программисты при эксплуатации роботов в реальной жизни.

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

Рисунок 1 – Схема мобильного робота.

Робот, приведённый на рисунке 1, состоит из платформы, приводной оси и сферической всенаправленной опоры. Приводная ось состоит из двух колес, расположенных соосно и приводимых в движение отдельными приводами. Шаровая опора предназначена для придания роботу устойчивого горизонтального положения.

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

 

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

На первом шаге (1) подставляя в начальные уравнения [x=ϕ(t), y=g(t)] переменную t, c определенной дискретизацией, получим массив элементов [x(i), y(i)], состоящий из точек с координатами, описывающих траекторию движения МР. Здесь важным является выбор шага дискретизации. Чем выше дискретизация, тем большее количество элементов массива [x(i), y(i)] будет получено. Но это значение должно соотносится с возможностями работы системы управления роботом. Нет смысла делать высокую дискретизацию, когда робот не может ее обеспечивать.

На втором шаге (2) преобразуем массив из координат точек [x(i), y(i)] в массивы линейной и угловой мобильного робота (МР). С учетом дискретизации системы управления dt, рассчитаем линейную и угловую скорости робота в каждый момент времени:

На рисунке 3 приведен график произвольной траектории движения, состоящий из трех точек. Так же показаны углы, координаты точек и перемещения.

Рисунок 3 – Определение линейной и угловой скоростей МР

 

На третьем шаге (3) преобразуем угловую и линейную скорости центра колесной пары МР в угловые скорости вращения колес МР. Это делается по формуле:

 

 

Где  – угловая скорость правого колеса на i-ом участке траектории,

 - угловая скорость левого колеса на i-ом участке траектории,

 – расстояние между приводными колесами,

 – радиус приводного колеса.

 

 

 

Пример получения угловых скоростей приводных колес МР

 

Для работы будем применять программу MATLAB R2019b, но все вычисления можно сделать и в других инженерных программах, например, Octave, Maple, Mathematica и т.п, либо написав программу на любом языке программирования.

ШАГ 1. Получения массива точек из функций траектории, заданной в параметрическом виде

 

 Рассмотрим в качестве примера фигуру в виде восьмерки. Параметрический вид:

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

i=1; % итератор

for t =0: 0.1: 2*pi % цикл старт:dt:стоп

x(i)=sin(t+(pi/2)); % массив х

y(i)=sin(2*t); % массив у

i=i+1; % увеличение итератора

end

masSize=i-1; % переменная с размером массива

figure(1); % график

plot(x,y,'*'); % график

axis equal; % оси одного масштаба

grid on; % сетка для осей

 

Здесь:

for – цикл для получения массива точек [x(t),y(t)] от 0 до 2pi c дискретизацией 0.1

i – итератор

masSize – переменная, хранящая количество элементов массива;

plot() – функция отрисовки графика по точкам, координаты которых записаны в массивах x и y.

 

С шагом dt=0.1 получаем 63 точки траектории. Начало находится в точке с координатами [1,0]. Робот должен находиться под углом pi/2 к оси X.

График функции приведен на рисунке 4

Рисунок 4 – график функции. По оси х и у приведены расстояния в м.