ШАГ 2. Получения угловой и линейной скорости центра колесной пары МР

 

Приведем код для получения угловой и линейной скорости центра колесной пары МР для движения вдоль заданной траектории

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;

 

Особенности данного кода:

1) Так как точка старта имеет координаты [1,0] и график поднимается вверх по оси у, угол старта робота примем равным pi/2. Обозначим его как tettaStart.  

2) Функция atan2(x,y) вычисляет арктангенс отношения величины x на y. Таким образом можно вычислить угол между осью x и линией, проведенной из начала координат (0, 0) в точку с координатами (x, y). Угол определяется в радианах в диапазоне от -пи до пи, исключая -пи.

3) Если необходимо выполнить поворот против часовой стрелки на ¾ оборота – то проще это сделать, повернув робота на ¼ оборота по часовой стрелке. Эта особенность выполнена в последних 2 условиях if цикла for i=1:masSize-1.

4) При переходе от математики к реальному роботу необходимо учитывать дискретизацию системы управления dt. Для робота, на котором будут проведены экспериментальные исследования, dt равен 0.04 с. Соответственно, система управления роботом работает на частоте 25 Гц.

 

Графики линейной и угловой скоростей центра робота приведены на рисунке 5.

Рисунок 5 – График линейной (сплошная линия) и угловой (штриховая линия) скорости движения центра колесной пары МР. Ось х показывает номер итерации.

 

ШАГ 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); % закрытие файла

 

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