В настоящей статье рассматривается планирование траектории движения манипулятора в системе Matlab.
MATLAB (сокращение от англ. «Matrix Laboratory», в русском языке произносится как Матлаб) — пакет прикладных программ для решения задач технических вычислений и одноимённый язык программирования, используемый в этом пакете.
1. Общие сведения
Планирование траекторий движения манипулятора – это задача выбора закона управления, обеспечивающего движение манипулятора вдоль некоторой заданной траектории. Перед началом движения манипулятора важно знать:
1. существуют ли на его пути какие-либо препятствия;
2. накладываются ли какие-либо ограничения на траекторию схвата.
В зависимости от ответов на эти вопросы выбирается один из четырех типов управления манипулятором (табл. 1).
Таблица 1. Типы управления манипулятором
Препятствия на пути манипулятора
| |||
Присутствуют
|
Отсутствуют
| ||
Ограничения на траекторию манипулятора
|
Присутствуют
|
I. Автономное планирование траектории, обеспечивающее обход препятствий, плюс регулирование движения вдоль выбранной траектории в процессе работы манипулятора
|
II. Автономное планирование траектории плюс регулирование движения вдоль выбранной траектории в процессе работы манипулятора
|
Отсутствуют
|
III. Позиционное управление плюс обнаружение и обход препятствий в процессе движения
|
IV. Позиционное управление
| |
Рассмотрим планирование траектории манипулятора при отсутствии препятствий (II и IV тип). Задача состоит в разработке математического аппарата для выбора и описания желаемого движения манипулятора между начальной и конечной точками траектории.
При планировании траекторий обычно применяется один из двух подходов:
1. Задается точный набор ограничений (например, непрерывность и гладкость) на положение, скорость и ускорение обобщенных координат манипулятора в некоторых (называемых узловыми) точках траектории. Планировщик траекторий после этого выбирает из некоторого класса функций (как правило, среди многочленов, степень которых не превышает некоторое заданное n) функцию, проходящую через узловые точки и удовлетворяющую в них заданным ограничениям. Определение ограничений и планирование траектории производится в присоединенных координатах.
2. Задается желаемая траектория манипулятора в виде некоторой аналитически описываемой функции, как, например, прямолинейную траекторию в декартовых координатах. Планировщик производит аппроксимацию заданной траектории в присоединенных или декартовых координатах.
Планирование в присоединенных переменных обладает тремя преимуществами:
1) задается поведение переменных, непосредственно управляемых в процессе движения манипулятора;
2) планирование траектории может осуществляться в реальном времени;
3) траектории в присоединенных переменных легче планировать.
4) Должны быть сведены к минимуму бесполезные движения типа «блуждания».
Рисунок 1. Блок-схема планировщика траекторий
Недостаток – сложность определения положения звеньев и схвата в процессе движения. Это необходимо для предотвращения столкновения с препятствием.
2. Реализация программы для расчёта траектории типа 4-3-4
Программа рассчитывает траекторию типа 4-3-4 для одного сочленения манипулятора при условии отсутствия препятствий.
На вход программы подаётся точный набор ограничений на положение, скорость и ускорение обобщенных координат манипулятора в узловых точках траектории.
Программа рассчитывает неизвестные коэффициенты
Результаты работы программы:
1. Ввод данных
Рисунок 2. Окна ввода входных данных
2. Полученные коэффициенты (вектор-столбец С):
Элемент матрицы коэффициентов C №1-1: 20
Элемент матрицы коэффициентов C №1-2: 40
Элемент матрицы коэффициентов C №1-3: 600
Элемент матрицы коэффициентов C №1-4: -1100.3559
Элемент матрицы коэффициентов C №1-5: 470.3559
Элемент матрицы коэффициентов C №2-1: 30
Элемент матрицы коэффициентов C №2-2: -269.4661
Элемент матрицы коэффициентов C №2-3: 272.4025
Элемент матрицы коэффициентов C №2-4: -12.9363
Элемент матрицы коэффициентов C №2-5: 0
Элемент матрицы коэффициентов C №3-1: 20
Элемент матрицы коэффициентов C №3-2: 91.3484
Элемент матрицы коэффициентов C №3-3: 25.9548
Элемент матрицы коэффициентов C №3-4: -195.9548
Элемент матрицы коэффициентов C №3-5: 118.6516
Сравним полученные коэффициенты с рассчитанными в (3), стр.
239:
Из приведённых данных следует, что полученные в ходе выполнения программы коэффициенты, верны.
3. Построение графиков положения, скорости и ускорения от времени.
Рисунок 3. График положения
Рисунок 4. График скорости
Рисунок 5. График ускорения
планирование траектории типа 4-3-4
clc; clear;
% Ввод входных данных:
title = 'Ввод временных интервалов';
promt = {'Введите интервал 1:', 'Введите интервал 2:',...
'Введите интервал 3:', 'Введите шаг построения графиков:'};
lines = 1;
defval = {'20', '30', '10', '0.001'};
answer = inputdlg(promt, title, lines, defval); % Окно ввода данных
t1 = str2num(char(answer(1)));
t2 = str2num(char(answer(2)));
t3 = str2num(char(answer(3)));
stp = str2num(char(answer(4)));
title = 'Ввод входных параметров';
promt = {'Начальное положение схвата: ', 'Начальная скорость робота:', ...
'Начальное ускорение: ', 'Положение при захвате (в момент t1)',...
'Положение схвата в момент t2',...
'Конечное положение схвата (в момент t3)', 'Конечная скорость',...
'Конечное ускорение'};
lines = 1;
defval = {'20', '2', '3', '30', '20', '60', '3', '3'};
% Данные по умолчанию введены для первого сочленения
% (см. Шахинпур, стр. 237)
answer = inputdlg(promt, title, lines, defval);
Qi0 = str2num(char(answer(1)));
Qi01 = str2num(char(answer(2)));
Qi02 = str2num(char(answer(3)));
Qi1 = str2num(char(answer(4)));
Qi2 = str2num(char(answer(5)));
Qi3 = str2num(char(answer(6)));
Qi31 = str2num(char(answer(7)));
Qi32 = str2num(char(answer(8)));
% Матрица B (стр. 231):
B(1:9, 1:9) = 0;
% Задаём элементы матрицы:
B(1, 1) = 1;
B(1, 2) = 1;
B(2, 1) = 3*(t1^(-1));
B(2, 2) = 4*(t1^(-1));
B(2, 3) = -(t2^(-1));
B(3, 1) = 6*(t1^(-2));
B(3, 2) = 12*(t1^(-2));
B(3, 4) = -2*(t2^(-2));
B(4, 3) = 1;
B(4, 4) = 1;
B(4, 5) = 1;
B(5, 3) = t2^(-1);
B(5, 4) = 2*(t2^(-1));
B(5, 5) = 3*(t2^(-2));
B(5, 6) = -(t3^(-1));
B(6, 4) = 2*(t2^(-2));
B(6, 5) = 6*(t2^(-2));
B(6, 7) = -2*(t3^(-2));
B(7, 6:9) = 1;
B(8, 6) = 1;
B(8, 7) = 2;
B(8, 8) = 3;
B(8, 9) = 4;
B(9, 7) = 2;
B(9, 8) = 6;
B(9, 9) = 12;
% Вектор-столбец сигма:
SIG(1:9, 1) = 0;
SIG(1, 1) = Qi1-(Qi0+Qi01*t1+0.5*Qi02*(t1^2));
SIG(2, 1) = -(Qi01+Qi02*t1);
SIG(3, 1) = -Qi02;
SIG(4, 1) = Qi2-Qi1;
SIG(7, 1) = Qi3-Qi2;
SIG(8, 1) = t3*Qi31;
SIG(9, 1) = (t3^2)*Qi32;
% Формула (4.8.34), стр. 230
Ctemp = B^(-1) * SIG;
% Матрица-столбец коэффициентов:
C(1, 1) = Qi0;
C(1, 2) = t1*Qi01;
C(1, 3) = 0.5*(t1^2)*Qi02;
C(1, 4) = Ctemp(1, 1);
C(1, 5) = Ctemp(2, 1);
C(2, 1) = Qi1;
C(2, 2) = Ctemp(3, 1);
C(2, 3) = Ctemp(4, 1);
C(2, 4) = Ctemp(5, 1);
C(2, 5) = 0;
C(3, 1) = Qi2;
C(3, 2) = Ctemp(6, 1);
C(3, 3) = Ctemp(7, 1);
C(3, 4) = Ctemp(8, 1);
C(3, 5) = Ctemp(9, 1);
for i=1:3
for j=1:5
msg=['Элемент матрицы коэффициентов C №', num2str(i), '-',...
num2str(j),': ', num2str(C(i, j))];
disp(msg);
end
end
% Данные для построения графиков:
sum = 0;
% Первый участок:
for i=0:stp:1
sum = sum+1;
tim = stp*t1*(sum-1);
T(sum) = tim;
Position(1, sum) = C(1, 1)+C(1, 2)*i + C(1, 3)*(i^2)+C(1, 4)*(i^3)+...
+C(1, 5)*(i^4);
Speed(1, sum) = (C(1, 2) + 2*C(1, 3)*i + 3*C(1, 4)*(i^2)+4*C(1, 5)*...
(i^3))/t1;
Velocity(1, sum) = (2*C(1, 3)+6*C(1, 4)*i + 12*C(1, 5)*(i^2))/(t1^2);
end
% Второй участок:
tmp1 = 0;
for i=stp:stp:(1-stp)
sum = sum+1;
tmp1 = tmp1+1;
tim = t1 + t2*stp*(tmp1);
T(sum) = tim;
Position(1, sum) = C(2, 1)+C(2, 2)*i + C(2, 3)*(i^2)+C(2, 4)*(i^3);
Speed(1, sum) = (C(2, 2) + 2*C(2, 3)*i + 3*C(2, 4)*(i^2))/t2;
Velocity(1, sum) = (2*C(2, 3)+6*C(2, 4)*i)/(t2^2);
end
% Третий участок
tmp2 = 0;
for i=0:stp:1
sum = sum+1;
tmp2 = tmp2+1;
tim = t1+t2+t3*stp*(tmp2-1);
T(sum) = tim;
Position(1, sum) = C(3, 1)+C(3, 2)*i + C(3, 3)*(i^2)+C(3, 4)*(i^3)+...
+C(3, 5)*(i^4);
Speed(1, sum) = (C(3, 2) + 2*C(3, 3)*i + 3*C(3, 4)*(i^2)+4*C(3, 5)*...
(i^3))/t3;
Velocity(1, sum) = (2*C(3, 3)+6*C(3, 4)*i + 12*C(3, 5)*(i^2))/(t3^2);
end
% Построение графиков:
T1(1, 1:tmp1) = T(1, 1:tmp1);
T2(1, 1:sum-tmp2) = T(1, 1:sum-tmp2);
pos1(1, 1:tmp1) = Position(1, 1:tmp1);
pos2(1, 1:sum-tmp2) = Position(1, 1:sum-tmp2);
spd1(1, 1:tmp1) = Speed(1, 1:tmp1);
spd2(1, 1:sum-tmp2) = Speed(1, 1:sum-tmp2);
vel1(1, 1:tmp1) = Velocity(1, 1:tmp1);
vel2(1, 1:sum-tmp2) = Velocity(1, 1:sum-tmp2);
figure();
hold on;
grid on;
plot(T, Position);
plot(T2, pos2, 'g');
plot(T1, pos1, 'r');
ylabel('Положение, м');
xlabel('Время, с');
figure();
hold on;
grid on;
plot(T, Speed);
plot(T2, spd2, 'g');
plot(T1, spd1, 'r');
ylabel('Скорость, м/с');
xlabel('Время, с');
figure();
hold on;
grid on;
plot(T, Velocity);
plot(T2, vel2, 'g');
plot(T1, vel1, 'r');
ylabel('Ускорение, м/с*с');
xlabel('Время, с');
3. Список используемой литературы
1. Фу К., Гонсалес Р., Ли К. Робототехника / Пер. с англ. — М.: Мир, 1989. — 624 с.
2. Зенкевич С. Л, Ющенко А. С. Управление роботами — М.: МГТУ им. Н. Э. Баумана, 2000.
3. Шахинпур М. Курс робототехники / Пер. с англ. — М.: Мир, 1990. — 527 с.
Комментариев нет:
Отправить комментарий