Робототехника. Выполнение работы
 Скачать 0.71 Mb.
|
|
Цель работы: изучить предложенную методику решения обратной задачи о положении манипулятора для конкретных кинематических схем. На основании проведенного машинного эксперимента определить программную траекторию робота. Выполнение работы Рассмотрим промышленный робот фирмы KUKA - KR 120 R3200 PA (Quantec)для укладки листового стекла с тележки на рольганг.  рис. 1. Внешний вид KUKA KR 120 R3200 PA (Quantec) Спецификация робота:   рис. 2. Геометрические размеры промышленного робота Таблица 1
 рис. 3. Кинематическая схема промышленного робота Кинематическая схема данного робота соответствует кинематической схеме №4.  рис. 4. Кинематическая схема №4. l1 =675 мм, l2 = 1350 мм, l3=1220 мм, l4=280 мм. -185º ≤q1≤ 185º -140º ≤q2≤-5º 0º ≤q3≤150º -135º ≤q4≤135º -350º ≤q5≤350º Таблица 2 Характеристики объекта манипулирования
Таблица 3 Установочные размеры
 α рис. 5.Схема расположения стекла на тележке  Матрицы перехода между системами координат 1)Переход от 5-й к4-ой системе: Rot Z q5 + Trans Y l4 + Rot X (-π/2)  2)Переход от 4-й системы к 3-й: Rot Z q4 + Trans Y l3  3)Переход от 3-й ко2-й: RotZq3 + TransYl2  4)Переход от 2-й системы к 1-й: Rot Z q2+Rot Y (π/2) + Trans Y p1 + Trans Z l1+Rot Z q1   Параметры РО для каждого цикла манипулирования: Захват стекла.  Отрыв листа от стопы  Подъем листа на высоту рольганга  Ориентация  Положение после поворота  Опускание на рольганг  Решение обратной задачи:        Результаты решения обратной задачи:  Рис. 8.Захват стекла  Рис. 10.Отрыв стекла  Рис. 12.Подъем стекла  Рис. 9.Захват стекла  Рис. 11.Отрыв стекла  Рис. 13.Подъем стекла  Рис. 14.Ориентация  Рис. 16.Положение после поворота  Рис. 18.Опускание на рольганг  Рис. 15.Ориентация  Рис. 17.Положение после поворота  Рис. 19.Опускание на рольганг Таблица 4. Результаты решения обратной задачи
Таблица 5
Код MatLab: proc = 1;%коэф для нахождения отклонений teta1 = [1 0 0]; teta2 = [0 1 0]; teta3 = [0 0 1]; l1 = 675; l2 = 1350; l3 = 1220; l4 = 280; l5 = 0; l = 753; %длина стекла f = 621; %Ширина стекла h = 5.5; %высота стекла a = 500; %уровень тележки b = 1100; %от оси робота до пирамиды c = 850; %уровень рольганга d = 1120; %от оси робота до рольганга e = 2.5; %уровень опускания стекла k1 = 200; %подъём alpha = 75*(pi/180); %в радианах %Координаты векторов для каждого этапа %Захватстекла r51 = [b+(l/2)*cos(alpha);0;(l/2)*sin(alpha)+h*cos(alpha)+a]; f51 = [sin(alpha);0;-cos(alpha)]; g51 = [cos(alpha);0;sin(alpha)]; %Отрыв r52 = [b+(l/2)*cos(alpha)-k1;0;(l/2)*sin(alpha)+h*cos(alpha)+a]; f52 = [sin(alpha);0;-cos(alpha)]; g52 = [cos(alpha);0;sin(alpha)]; %Подъем r53 = [b/2;0;c+10]; f53 = [1;0;0]; g53 = [1;0;0]; %Ориентация r54 = [b/2;0;c+10]; f54 = [0;0;-1]; g54 = [1;0;0]; %Положение после поворота r55 = [0;b/2;c+10]; f55 = [0;0;-1]; g55 = [1;0;0]; %Опускание на рольганг r56 = [0;d+(l/2);c+e]; f56 = [0;0;-1]; g56 = [1;0;0]; r1 = [0;0;l1]; r5 = r56; f5 = f56; g5 = g56; r3 = r5 - (l4+l5)*f5; q11 = sign(r3(2))*acos(r3(1)/(sqrt(r3(1)^2+r3(2)^2)))+pi*fix(0.5*(1-(r3(1)/(sqrt(r3(1)^2+r3(2)^2))))); q11 = proc * q11; q12 = mod((q11+pi),(2*pi)); q12 = proc * q12; r31 = r3 - r1; %определяемматрицуА1=RotZ(q1)*TransZ(l1)*RotX(pi\2) и u1 = (A1)' A11 = [cos(q11) -sin(q11) 0 0; sin(q11) cos(q11) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 0; 0 0 1 l1; 0 0 0 1]*[1 0 0 0; 0 cos(pi/2) -sin(pi/2) 0; 0 sin(pi/2) cos(pi/2) 0; 0 0 0 1]; u11 = (A11(1:3,1:3)).'; A12 = [1 0 0 0; 0 1 0 0; 0 0 1 l1; 0 0 0 1]*[cos(q12) -sin(q12) 0 0; sin(q12) cos(q12) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 cos(pi/2) -sin(pi/2) 0; 0 sin(pi/2) cos(pi/2) 0; 0 0 0 1]; u12 = (A12(1:3,1:3)).'; modl31 = sqrt(r3(1)^2+r3(2)^2+(r3(3)-l1)^2); q21 = sign(teta1*u11*r31)*acos((teta2*u11*r31)/modl31)+acos((l2^2+modl31^2-l3^2)/(2*l2*modl31)); q21 = proc * q21; q22 = sign(teta1*u11*r31)*acos((teta2*u11*r31)/(sqrt(r3(1)^2+r3(2)^2+(r3(3)-l1)^2)))-acos((l2^2+(sqrt(r3(1)^2+r3(2)^2+(r3(3)-l1)^2))^2-l3^2)/(2*l2*(sqrt(r3(1)^2+r3(2)^2+(r3(3)-l1)^2)))); q22 = proc * q22; q23 = sign(teta1*u12*r31)*acos((teta2*u12*r31)/(sqrt(r3(1)^2+r3(2)^2+(r3(3)-l1)^2)))+acos((l2^2+(sqrt(r3(1)^2+r3(2)^2+(r3(3)-l1)^2))^2-l3^2)/(2*l2*(sqrt(r3(1)^2+r3(2)^2+(r3(3)-l1)^2)))); q23 = proc * q23; q24 = sign(teta1*u12*r31)*acos((teta2*u12*r31)/(sqrt(r3(1)^2+r3(2)^2+(r3(3)-l1)^2)))-acos((l2^2+(sqrt(r3(1)^2+r3(2)^2+(r3(3)-l1)^2))^2-l3^2)/(2*l2*(sqrt(r3(1)^2+r3(2)^2+(r3(3)-l1)^2)))); q24 = proc * q24; %определяем матрицу А2=RotZ(q2)*TransY(l2) и u2 = (A2)' A21 = A11*[cos(-q21) -sin(-q21) 0 0; sin(-q21) cos(-q21) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l2; 0 0 1 0; 0 0 0 1]; u21 = (A21(1:3,1:3)).'; A22 = A11*[cos(-q22) -sin(-q22) 0 0; sin(-q22) cos(-q22) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l2; 0 0 1 0; 0 0 0 1]; u22 = (A22(1:3,1:3)).'; A23 = A12*[cos(-q23) -sin(-q23) 0 0; sin(-q23) cos(-q23) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l2; 0 0 1 0; 0 0 0 1]; u23 = (A23(1:3,1:3)).'; A24 = A12*[cos(-q24) -sin(-q24) 0 0; sin(-q24) cos(-q24) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l2; 0 0 1 0; 0 0 0 1]; u24 = (A24(1:3,1:3)).'; %ветвление по графу допустимых решений if r3(2,1)==0 rez = [sin(q21)*l2;0;cos(q21)*l2]; else rez = [0;sin(q21)*l2;cos(q21)*l2]; end; r2 = r1 + rez; r21 = r2 - r1; r312 = u21*r31; r212 = u21*r21; r322 = r312-r212; q31 = -sign(teta1*r322)*acos((teta2*r322)/(norm(r322))); q31 = proc * q31; q33 = q31; if r3(2,1)==0 rez = [sin(q22)*l2;0;cos(q22)*l2]; else rez = [0;sin(q22)*l2;cos(q22)*l2]; end; r2 = r1 + rez; r21 = r2 - r1; r312 = u22*r31; r212 = u22*r21; r322 = r312-r212; q32 = -sign(teta1*r322)*acos((teta2*r322)/(norm(r322))); q32 = proc * q32; q34 = q32; %ОпределяемматрицуА=RotZ(q3)*TransY(l3) иu3 = (A3)' A31 = A21*[cos(q31) -sin(q31) 0 0; sin(q31) cos(q31) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l3; 0 0 1 0; 0 0 0 1]; u31 = (A31(1:3,1:3)).'; A32 = A22*[cos(q32) -sin(q32) 0 0; sin(q32) cos(q32) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l3; 0 0 1 0; 0 0 0 1]; u32 = (A32(1:3,1:3)).'; A33 = A23*[cos(q33) -sin(q33) 0 0; sin(q33) cos(q33) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l3; 0 0 1 0; 0 0 0 1]; u33 = (A33(1:3,1:3)).'; A34 = A24*[cos(q34) -sin(q34) 0 0; sin(q34) cos(q34) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l3; 0 0 1 0; 0 0 0 1]; u34 = (A34(1:3,1:3)).'; q41 = sign(teta1*u31*f5)*acos(teta2*u31*f5); q41 = proc * q41; q42 = sign(teta1*u32*f5)*acos(teta2*u32*f5); q42 = proc * q42; q43 = sign(teta1*u33*f5)*acos(teta2*u33*f5); q43 = proc * q43; q44 = sign(teta1*u34*f5)*acos(teta2*u34*f5); q44 = proc * q44; %ОпределяемматрицуА4=RotZ(q4)*TransY(l4)*RotX(-pi\2) и u4 = (A4)' A41 = A31*[cos(-q41) -sin(-q41) 0 0; sin(-q41) cos(-q41) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l4; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 cos(-pi/2) -sin(-pi/2) 0; 0 sin(-pi/2) cos(-pi/2) 0; 0 0 0 1]; u41 = (A41(1:3,1:3)).'; A42 = A32*[cos(-q42) -sin(-q42) 0 0; sin(-q42) cos(-q42) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l4; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 cos(-pi/2) -sin(-pi/2) 0; 0 sin(-pi/2) cos(-pi/2) 0; 0 0 0 1]; u42 = (A42(1:3,1:3)).'; A43 = A33*[cos(-q43) -sin(-q43) 0 0; sin(-q43) cos(-q43) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l4; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 cos(-pi/2) -sin(-pi/2) 0; 0 sin(-pi/2) cos(-pi/2) 0; 0 0 0 1]; u43 = (A43(1:3,1:3)).'; A44 = A34*[cos(-q44) -sin(-q44) 0 0; sin(-q44) cos(-q44) 0 0; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 1 0 l4; 0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 cos(-pi/2) -sin(-pi/2) 0; 0 sin(-pi/2) cos(-pi/2) 0; 0 0 0 1]; u44 = (A44(1:3,1:3)).'; q51 = -sign(teta2*u41*g5)*acos(teta1*u41*g5); q51 = proc * q51; q52 = -sign(teta2*u42*g5)*acos(teta1*u42*g5); q52 = proc * q52; q53 = -sign(teta2*u43*g5)*acos(teta1*u43*g5); q53 = proc * q53; q54 = -sign(teta2*u44*g5)*acos(teta1*u44*g5); q54 = proc * q54; %Определяем матрицу А5 - матрица положения рабочего органа A51 = A41*[cos(q51) -sin(q51) 0 0; sin(q51) cos(q51) 0 0; 0 0 1 0; 0 0 0 1]; A52 = A42*[cos(q52) -sin(q52) 0 0; sin(q52) cos(q52) 0 0; 0 0 1 0; 0 0 0 1]; A53 = A43*[cos(q53) -sin(q53) 0 0; sin(q53) cos(q53) 0 0; 0 0 1 0; 0 0 0 1]; A54 = A44*[cos(q54) -sin(q54) 0 0; sin(q54) cos(q54) 0 0; 0 0 1 0; 0 0 0 1]; %Векторы обобщенныx координат Q1 = [q11 q21 q31 q41 q51]; Q2 = [q11 q22 q32 q42 q52];% Q3 = [q12 q23 q33 q43 q53]; Q4 = [q12 q24 q34 q44 q54]; q1 = q11*(180/pi) q2 = q22*(180/pi) q3 = q32*(180/pi) q4 = q42*(180/pi) q5 = q52*(180/pi) %Построениеробота X1 = [0 A11(1,4) A22(1,4) A32(1,4) A42(1,4) A52(1,4)]; Y1 = [0 A11(2,4) A22(2,4) A32(2,4) A42(2,4) A52(2,4)]; Z1 = [0 A11(3,4) A22(3,4) A32(3,4) A42(3,4) A52(3,4)]; figure; holdon; plot3(X1,Y1,Z1,'-ok'); xlim([-2000 2000]); ylim([-2000 2000]); zlim([0 2500]); xlabel('x'); ylabel('y'); zlabel('z'); plot3(r5(1),r5(2),r5(3),'*k');%схват gridon; holdoff; %длятаблицы Q222 = [q11*(180/pi) q22*(180/pi) q32*(180/pi) q42*(180/pi) q52*(180/pi) X1(6) Y1(6) Z1(6)]; Вывод:В ходе выполнения данной лабораторной работы была изучена предложенная методика решения обратной задачи о положении манипулятора для конкретной кинематической схемы. На основании проведенного машинного эксперимента определена программная траектория робота. | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||