4. Построение траекторий звеньев робота - манипулятора сиспользованием кубических полиномов (сплайнов).
Для получения законов управления техническими системами зачастую применяются кубические сплайны. Интерполяционные процедуры, основанные на использовании кубических сплайнов, широко используются также в компьютерных системах управления технологическими процессами. Кубические сплайны, обладающие рядом "природных" свойств, например, свойством минимальной кривизны, равенством первых производных от функции ( справа и слева от узлов интерполяции ), позволяют обеспечивать высокое качество приближения кривых по небольшому числу экспериментальных данных. Сплайн аппроксимации дают возможность получать "гладкие" интерполяционные функции, по которым могут быть оценены текущие значения функции между узлами интерполяции для соответствующих элементов.
Если экспериментальные данные заданы в виде n пар измерений (x,y), то для интерполяции функции можно использовать (n-1) кубических полиномов (сплайнов).
Каждый полином имеет вид:
,
(4.1)
где
;
i = 1, 2, 3 …, n
– 1.
Коэффициенты ai, bi и ci определяются при соблюдении трех условий:
1.Полином должен проходить через точки на концах xi и xi+1.
2.Наклоны полиномов должны быть равны слева и справа от узла интерполяции.
Должно соблюдаться условие минимума кривизны.
Предположим, что экспериментальные данные представлены таблицей 4.1.
Таблица 4.1
Значения экспериментальных данных
-
x
7
9
11
12
y
49
57
71
75
В результате сложных расчетов коэффициентов ai, bi и ci, получаем полиномы (сплайны):
для
:
;
для
:
;
для
:
.
Эти же полиномы (сплайны) легко можно получить в среде MATLAB с помощью команды "spline".
Еёсинтаксис:
y_int = spline (x, y, x_int),
где: x и y - векторы, содержащие данные таблицы;
x_int - вектор интерполяции аргумента;
y_int - вектор значений интерполяции функции.
Выполним интерполяцию функции с помощью сплайнов в среде MATLAB для рассматриваемого примера в Командном Окне:
>> x=[7 9 11 12]; %Время (в часах).
>> y=[49 57 71 75]; %Температура (в градусах).
>>x_int=[7:0.01:12]; %Вектор значений аргумента.
>>y_int=spline(x,y,x_int); %Вектор значений функции.
>> [breaks,coeffs,m,n]=unmkpp(spline(x,y)) %Матрица коэффициентов
%полиномов размерности m*n.
Решение.
breaks =
7 9 11 12
coeffs =
-0.3500 2.8500 -0.3000 49.0000
-0.3500 0.7500 6.9000 57.0000
-0.3500 -1.3500 5.7000 71.0000
m = %Число строк.
3
n = %Число столбцов.
4
>>plot(x,y,'O',x,y,'--',x_int,y_int),grid % Построениеграфика
Рис.4.1 График экспериментальной траектории и траектории сплайна.
Р
ассмотрим
две схемы промежуточных положений
робота-манипулятора:
Рис.4.2 Схема № 1 промежуточного положения робота – манипулятора.
Рис.4.3 Схема № 2промежуточного положенияробота – манипулятора.
Пусть заданы величины звеньев L1, L2.
Необходимо определить углы α1, α2, Θ1, Θ2, а также величину R.
На схеме рис.4.2 α2 имеет положительные значения, а на схеме рис.4.3 – отрицательные значения. Углы Θ1 и Θ2 введены для вспомогательных построений. Используя для схемы рис.4.2 теорему косинусов, запишем:
,
(4.2)
т.к.
,
получим:
.
(4.3)
Используя x и y как координаты "руки R" манипулятора, запишем:
R2 = x2 + y2. (4.4)
Для определения угла Θ2, по теореме косинусов запишем:
. (4.5)
Очевидно, что Θ1 = arctg(y/x).
Из рис.4.2 следует: α1 = Θ1 - Θ2, если α2 > 0.
Из рис.4.3 следует: α1 = Θ1 + Θ2 , если α2 < 0.
Произведем проверку полученных теоретических соотношений для построения траекторий двухзвенного робота – манипулятора c использованием формирования линейного массива узлов для заданного закона изменения выходных координат, а также с использованием сплайнов.
Лабораторная работа №4."Построение траекторий звеньев манипулятора с формированием линейного массива узлов для заданного закона изменения выходных координат"
Цель работы:
Рассчитать количество промежуточных узлов "n" изменений манипулятора.
Cформировать линейный массив "x" равноотстоящих узлов и закон изменения выходных координат "y".
Рассчитать в радианах значения углов α1, α2, Θ1, Θ2 изменений звеньев манипулятора.
Рассчитать матрицу координат положений звеньев манипулятора.
5. Построить график траекторий движения звеньев манипулятора.
% File pt4.m.
% Manipulator's trajectories. %Траекториизвеньевманипулятора.
L1=4.25; L2=3.50; %Варианты размеров звеньев манипулятора.
x0=6.5; y0=0; xf=0; yf=6.5; %Варианты диапазона изменения начальных и конечных координат захвата.
n=3 %Количество узлов манипулятора.
%==================================
x=linspace(x0,xf,n+2) %Формирование линейного массива
%от x до xf для (n + 2) равноотстоящих %узлов.
%-------------------------------------
y=((yf-y0)/(xf-x0))*(x-x0)+y0 %Варианты законов изменения выходных %координат.
%-------------------------------------
R=sqrt(x.^2+y.^2); %Расстояние между началом и концом %звеньев манипулятора.
alf2=acos((R.^2-L1^2-L2^2)/(2*L1*L2)); %Расчет угла α2.
tet1=atan2(y,x); %Расчет угла Θ1.
tet2=acos((R.^2+L1^2-L2^2)./(2*L1*R)); %Расчет угла Θ2.
if alf2<0 %Проверка условия α2 < 0.
alf1=tet1+tet2; %Расчет угла α1 при выполнении
условия α2 < 0.
else
alf1=tet1-tet2; %Расчет угла α1 при не выполнении
условия α2 < 0.
end %Конец цикла.
tet1
tet2
alf1
alf2
%===================================================
xv=L1*cos(alf1); %Расчет координат (x1, y1) соединения
yv=L1*sin(alf1); %звеньев.
zx=[zeros(n+2,1) xv' x']; %Матрицы координат пяти положений
zy=[zeros(n+2,1) yv' y']; %звеньев манипулятора.
X=zx'
Y=zy'
plot(x,y,'o',xv,yv,'o',X,Y,0,0,'*'),grid %Построение графика
axis([-1 7 -3 7]);xlabel(' X , m ');ylabel(' Y , m ') % тректорий
%звеньев робота – манипулятора с
%формированием линейного массива узлов.
Таблица 4.2
Исходные данные для лабораторных работ № 4, 5
№ Варианта |
L1 |
L2 |
x0 |
y0 |
xf |
yf |
y |
1 |
4.25 |
3.50 |
6.5 |
0 |
0 |
6.5 |
((yf-y0)/(xf-x0))*(x-x0)+y0 |
2 |
4.25 |
3.50 |
6 |
0 |
0 |
4 |
[0 3 5 6 6.5] |
3 |
4 |
3 |
6.5 |
0 |
0 |
6.5 |
((yf-y0)/(xf-x0))*(x-x0)+y0 |
4 |
4 |
3 |
6 |
0 |
0 |
4 |
[0 3 5 6 6.5] |
Решение варианта задания № 1.
n =
3
x =
6.5000 4.8750 3.2500 1.6250 0
y =
0 1.6250 3.2500 4.8750 6.5000
tet1 =
0 0.3218 0.7854 1.2490 1.5708
tet2 =
0.5158 0.7412 0.8100 0.7412 0.5158
alf1 =
-0.5158 -0.4195 -0.0246 0.5078 1.0550
alf2 =
1.1579 1.7025 1.8848 1.7025 1.157
X =
0 0 0 0 0
3.6971 3.8815 4.2487 3.7137 2.0961
6.5000 4.8750 3.2500 1.6250 0
Y =
0 0 0 0 0
-2.0961 -1.7310 -0.1045 2.0666 3.6971
0 1.6250 3.2500 4.8750 6.5000
Рис.4.4 График траекторий звеньев робота – манипулятора
Лабораторная работа №5. "Построение траекторий движения звеньев манипулятора с использованием сплайнов"
Цель работы:
Рассчитать в радианах значения угловα1, α2, Θ1, Θ2 изменений
звеньев манипулятора.
Рассчитать матрицы сплайновых координат положений звеньев
манипулятора.
Рассчитать матрицы координат основного массива.
4. Построить график сплайновых траекторий движения звеньев
манипулятора.
%File pt5.m.
% Spline manipulator's trajectories.
L1=4.25 %Комментарии к файлу pt5.m аналогичны %комментариям к файлу pt4.m
L2=3.50
x0=6.5; y0=0; xf=0; yf=6.5;
n=3
x=linspace(x0,xf,n+2)
y=((yf-y0)/(xf-x0))*(x-x0)+y0
%Расчет параметров манипулятора.
R=sqrt(x.^2+y.^2);
alf2=acos((R.^2-L1^2-L2^2)/(2*L1*L2));
tet1=atan2(y,x);
tet2=acos((R.^2+L1^2-L2^2)./(2*L1*R));
if alf2<0
alf1=tet1+tet2;
else
alf1=tet1-tet2;
end
tet1
tet2
alf1
alf2
%Расчет траекторий звеньев манипулятора с использованием сплайнов.
t1=linspace(0,2,n+2); %Линейный массив между двух точек для (n+2) узлов
t=linspace(0,2,20); %То же для 20 узлов.
alfs1=spline(t1,alf1,t); %Расчет массива углов альфа1 и альфа2 с
alfs2=spline(t1,alf2,t); %использованием сплайнов.
xs=L1*cos(alfs1)+L2*cos(alfs1+alfs2); %Расчет сплайновых координат
ys=L1*sin(alfs1)+L2*sin(alfs1+alfs2); % x и y перемещения груза.
xvs=L1*cos(alfs1); %Расчет сплайновых координат xv и yv,
yvs=L1*sin(alfs1); %cвязующих звенья манипулятора.
p=size(yvs); %Размерность матрицы.
Axvs=[zeros(p(2),1) xvs' xs']; %Матрицы сплайновых координат
Ayvs=[zeros(p(2),1) yvs' ys']; %звеньев манипулятора.
Xa=Axvs'
Ya=Ayvs'
xv=L1*cos(alf1); %Расчет координат xv и yv
yv=L1*sin(alf1); %основного массива.
zx=[zeros(n+2,1) xv' x']; %Расчет координат звеньев манипулятора
zy=[zeros(n+2,1) yv' y']; %основного массива.
X=zx'
Y=zy'
plot(xs,ys,'.',x,y,'o',xvs,yvs,'.',xv,yv,'o',X,Y,Xa,Ya),grid
axis([-1 7 -3 7]);xlabel(' X , m ');ylabel(' Y , m ');
Решение варианта задания № 1.
L1 =
4.2500
L2 =
3.5000
n =
3
x =
6.5000 4.8750 3.2500 1.6250 0
y =
0 1.6250 3.2500 4.8750 6.5000
tet1 =
0 0.3218 0.7854 1.2490 1.5708
tet2 =
0.5158 0.7412 0.8100 0.7412 0.5158
alf1 =
-0.5158 -0.4195 -0.0246 0.5078 1.0550
alf2 =
1.1579 1.7025 1.8848 1.7025 1.1579
Xa =
Columns 1 through 7
0 0 0 0 0 0 0
3.6971 3.6695 3.6827 3.7322 3.8102 3.9067 4.0102
6.5000 6.1732 5.8349 5.4887 5.1384 4.7873 4.4383
Columns 8 through 14
0 0 0 0 0 0 0
4.1080 4.1879 4.2380 4.2484 4.2113 4.1212 3.9751
4.0934 3.7531 3.4170 3.0834 2.7496 2.4118 2.0671
Columns 15 through 20
0 0 0 0 0 0
3.7728 3.5170 3.2128 2.8681 2.4922 2.0961
1.7143 1.3550 0.9939 0.6391 0.3027 0.0000
Ya =
Columns 1 through 7
0 0 0 0 0 0 0
-2.0961 -2.1441 -2.1213 -2.0330 -1.8827 -1.6733 -1.4075
0 0.3017 0.6388 0.9943 1.3555 1.7141 2.0662
Columns 8 through 14
0 0 0 0 0 0 0
-1.0893 -0.7240 -0.3190 0.1164 0.5723 1.0385 1.5037
2.4107 2.7488 3.0833 3.4168 3.7525 4.0927 4.4379
Columns 15 through 20
0 0 0 0 0 0
1.9566 2.3861 2.7821 3.1364 3.4426 3.6971
4.7873 5.1385 5.4888 5.8348 6.1731 6.5000
X =
0 0 0 0 0
3.6971 3.8815 4.2487 3.7137 2.0961
6.5000 4.8750 3.2500 1.6250 0
Y =
0 0 0 0 0
-2.0961 -1.7310 -0.1045 2.0666 3.6971
0 1.6250 3.2500 4.8750 6.5000
ххххххххххххххххххххххххххххххххххххххххххххххххххххх
% Файл 'sah366.m'.
% Сплайн - траектории двухзвенного манипулятора.
%L1=4.25; L2=3.50;
% Размеры звеньев
L1=4;L2=3;
% Начальная и конечная точки траектории
x0=6.5; y0=0; xf=0; yf=6.5;
%x0=6; y0=0; xf=0; yf=4;
n=3;
%==================================
x=linspace(x0,xf,n+2); % n-число точек на линии
% (плюс 2 концевые)
%-------------------------------------
% Составление уравнения прямой линии (траектории
% движения спредера)
y=((yf-y0)/(xf-x0))*(x-x0)+y0;
%y=[0 3 5 6 6.5];
%-------------------------------------
R=sqrt(x.^2+y.^2);
alf2=acos((R.^2-L1^2-L2^2)/(2*L1*L2));
tet1=atan2(y,x);
tet2=acos((R.^2+L1^2-L2^2)./(2*L1*R));
if alf2<0
alf1=tet1+tet2;
else
alf1=tet1-tet2;
end
tet1;
alf1;
alf2;
%========================================
t1=linspace(0,2,n+2);
t=linspace(0,2,20);
alfs1=spline(t1,alf1,t);
alfs2=spline(t1,alf2,t);
xs=L1*cos(alfs1)+L2*cos(alfs1+alfs2);
ys=L1*sin(alfs1)+L2*sin(alfs1+alfs2);
xvs=L1*cos(alfs1);
yvs=L1*sin(alfs1);
p=size(yvs);
Axvs=[zeros(p(2),1) xvs' xs'];
Ayvs=[zeros(p(2),1) yvs' ys'];
Xa=Axvs';Ya=Ayvs';
xv=L1*cos(alf1);
yv=L1*sin(alf1);
zx=[zeros(n+2,1) xv' x'];
zy=[zeros(n+2,1) yv' y'];
X=zx'; Y=zy';
plot(xs,ys,'.',x,y,'o',xvs,yvs,'.',xv,yv,'o',X,Y,Xa,Ya),grid
axis([-1 7 -3 7]);xlabel(' X , m ');ylabel(' Y , m ');
%===============================================
% Polynomial coefficients for two (boundaries) points:
% for alf1(tau):
a1=2*(alf1(1)-alf1(5))*(180/pi)/2^3
b1=3*(alf1(5)-alf1(1))*(180/pi)/2^2
d1=alf1(1)*(180/pi)
% for alf2(tau):
a2=2*(alf2(1)-alf2(5))*(180/pi)/2^3
b2=3*(alf2(5)-alf2(1))*(180/pi)/2^2
d2=alf2(1)*(180/pi)
ххххххххххххххххххххххххххххххххххххххххххххххххххххх
% File 'sah366a.m'.
% Manipulator's trajectories.
%L1=4.25; L2=3.50;
L1=4;L2=3;
x0=6.5; y0=0; xf=0; yf=6.5;
%x0=6; y0=0; xf=0; yf=4;
n=3;
%==================================
x=linspace(x0,xf,n+2);
%-------------------------------------
y=((yf-y0)/(xf-x0))*(x-x0)+y0;
%-------------------------------------
R=sqrt(x.^2+y.^2);
alf2=acos((R.^2-L1^2-L2^2)/(2*L1*L2));
tet1=atan2(y,x);
tet2=acos((R.^2+L1^2-L2^2)./(2*L1*R));
if alf2<0
alf1=tet1+tet2;
else
alf1=tet1-tet2;
end
tet1;
alf1;
alf2;
%============================================================
xv=L1*cos(alf1);
yv=L1*sin(alf1);
zx=[zeros(n+2,1) xv' x'];
zy=[zeros(n+2,1) yv' y'];
X=zx'; Y=zy';
plot(x,y,'o',xv,yv,'o',X,Y,0,0,'*'),grid
axis([-1 7 -3 7]);xlabel(' X , m ');ylabel(' Y , m ');
ххххххххххххххххххххххххххххххххххххххххххххххххххххх
Рис.4.5 График траекторий движения звеньев робота – манипулятора, построенный с использованием сплайнов.