Формулы для оценки точности и скорости прохождения траектории.

 

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

1) Найдем сумму скоростей движения МР в каждый момент времени:

2) Для соответствия точек теоретической и экспериментальной траекторий, точки теоретической траектории рассчитываются с шагом пропорциональным величине скорости в эксперименте.

3) Определим  в каждый момент времени

4) Определим координаты теоретического графика относительно экспериментального согласно параметрическим уравнениям функции

5) Определение относительной ошибки (100 равномерно взятых выборок)

Сумма 100 равномерно взятых ошибок определяет точность следования робота вдоль траектории. Количество значений, полученных с Vicon с учетом дискретизации системы, определяет время, затраченное на прохождение траектории.

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

 

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

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

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

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

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

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

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

 

Победителем соревнований становится тот, кто наберет меньшее количество баллов. Так как точность прохождения вдоль трассы имеет больший приоритет чем время прохождения, то коэффициент ошибки =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); % закрытие файла