UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________

UNIVERSIDADE FEDERAL DE SO CARLOS

Centro de Cincias Exatas e Tecnolgicas Engenharia Eltrica

Robtica Industrial Exercicios: Gerao de trajetria e matriz Jacobiana

Data: 31 / 01 / 2013

Prof. Roberto Inoue Nome: Alex Rodrigues Fricelli RA: 348791

So Carlos 2013

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________

1. Problemtica
1.1. Desenvolver a Matriz Jacobiana para os seguintes modelos de robs:
Manipulador cilndrico Manipulador SCARA Manipulador Stanford

1.2.

Desenvolver os grficos em matlab para as trajetrias do polinmio de 3 grau e para a trajetria do tipo LSPB (Linear Segments with Parabolic Blends)

2. Desenvolvimento das matrizes Jacobiana

A seguir, so mostrados os manipuladores que devem ser estudados e seus respectivos diagramas de construo, dando aos sistemas de coordenadas posicionamento seguindo Denavit-Hartenberg.

Figura 1 - Manipulador cilindrico

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________

Figura 2 - Manipulador Stanford

Figura 3 - Manipulador SCARA

A partir dos parmetros de Denavit-Hartenberg de cada manipulador e da especificao quanto aos tipos de juntas (vetor r), possvel obter a matriz Jacobiana da velocidade angular e linear para qualquer manipulador. Isso acontece devido ao processo que se repete para obter a matriz jacobiana, que idntico para rob, alterando apenas os parmetros e as matrizes envolvidas. Assim, desenvolveu-se um algoritmo em matlab para obter a matriz jacobiana de um rob genrico, e ento, esse algoritmo foi aplicado aos manipuladores das figuras 1,2 e 3.

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________

2.1.
close all clear all clc

Cdigo em Matlab

%definio de parametros-------------------------------------------------%declaracao das variaveis syms a1 a2 a3 a4 a5 a6 syms d1 d2 d3 d4 d5 d6 syms teta1 teta2 teta3 teta4 teta5 teta6 %montagem da tabela de denavit (scara) % % % % % % % % % % % % % % % a=[a1 a2 0 0]; d=[d1 0 d3 d4]; alpha=[0 pi 0 0]; theta=[teta1 teta2 0 teta4]; %descrio sequencia do rob %1=rotacional 0=prismtica RP = [1 1 0 1]; %montagem da tabela de denavit (manipulador cilindrico) a=[0 0 0]; d=[d1 d2 d3]; alpha=[0 -pi/2 0]; theta=[teta1 0 0]; %descrio sequencia do rob %1=rotacional 0=prismtica RP = [1 0 0];

% %montagem da tabela de denavit (stanford) % a=[0 0 0 0 0 0]; % d=[0 d2 d3 0 0 d6]; % alpha=[-pi/2 pi/2 0 -pi/2 pi/2 0]; % theta=[teta1 teta2 0 teta4 teta5 teta6]; % %descrio sequencia do rob % %1=rotacional 0=prismtica % RP = [1 1 0 1 1 1]; %--------------------------------------------------------------------%matrizes de denavit [r,c]=size(a); for i=1:1:c H(:,:,i)=[cos(theta(i)) -sin(theta(i))*round(1000*cos(alpha(i)))/1000 sin(theta(i))*round(1000*sin(alpha(i)))/1000 a(i)*cos(theta(i)); sin(theta(i)) cos(theta(i))*round(1000*cos(alpha(i)))/1000 cos(theta(i))*round(1000*sin(alpha(i)))/1000 a(i)*sin(theta(i)); 0 round(1000*sin(alpha(i)))/1000 round(1000*cos(alpha(i)))/1000 d(i); 0 0 0 1]; end

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________ %gerao dos vetores z e O z0 = [0 ;0 ;1]; O0 = [0 ;0 ;0]; h_anterior=1; for j=1:1:c h_atual(:,:,j) = h_anterior*H(:,:,j); z(:,j) = [h_atual(1,3,j); h_atual(2,3,j); h_atual(3,3,j)]; O(:,j) = [h_atual(1,4,j); h_atual(2,4,j); h_atual(3,4,j)]; h_anterior = h_atual(:,:,j); end %Jacobiana da velocidade angular JW_0 = [RP(1)*z0]; for k=1:1:c-1 JW_1(:,k) = [RP(k+1)*z(:,k)]; end JW = horzcat(JW_0,JW_1); %Jacobiana da velocidade linear if(RP(1)==1) JV_0 = cross(z0,(O(:,c)-O0)); end if(RP(1)==0) JV_0 = z0; end for l=1:1:c-1 if (RP(l+1)==1) JV_1(:,l) = [cross(z(:,l),(O(:,c)-O(:,l)))]; end if (RP(l+1)==0) JV_1(:,l) = [z(:,l)]; end end JV = horzcat(JV_0,JV_1); JW = simplify(JW) JV = simplify(JV)

2.2.

Matriz Jacobiana Obtida

Para o rob SCARA, as matrizes jacobiana de velocidade angular JW e de velocidade linear JV foram:

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________

Figura 4 - Jacobiana do manipulador SCARA

Para o manipulador cilndrico, as matrizes jacobiana de velocidade angular JW e de velocidade linear JV foram:

Figura 5 - Jacobiana do manipulador cilindrico

Para o manipulador Stanford, as matrizes Jacobiana de velocidade angular JW e de velocidade linear JV foram: JW = Linha 1: [ 0, -sin(teta1), 0, cos(teta1)*sin(teta2), - cos(teta4)*sin(teta1) cos(teta1)*cos(teta2)*sin(teta4), cos(teta1)*cos(teta5)*sin(teta2) sin(teta5)*(sin(teta1)*sin(teta4) - cos(teta1)*cos(teta2)*cos(teta4))] Linha 2: [ 0, cos(teta1), 0, sin(teta1)*sin(teta2), cos(teta1)*cos(teta4) cos(teta2)*sin(teta1)*sin(teta4), sin(teta5)*(cos(teta1)*sin(teta4) + cos(teta2)*cos(teta4)*sin(teta1)) + cos(teta5)*sin(teta1)*sin(teta2)]

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________

Linha 3: [ 1, 0, 0, cos(teta2), cos(teta2)*cos(teta5) - cos(teta4)*sin(teta2)*sin(teta5)] JV =

sin(teta2)*sin(teta4),

Linha 1: [ - d2*cos(teta1) - d6*(sin(teta5)*(cos(teta1)*sin(teta4) + cos(teta2)*cos(teta4)*sin(teta1)) + cos(teta5)*sin(teta1)*sin(teta2)) - d3*sin(teta1)*sin(teta2), cos(teta1)*(d6*(cos(teta2)*cos(teta5) - cos(teta4)*sin(teta2)*sin(teta5)) + d3*cos(teta2)), cos(teta1)*sin(teta2), -d6*sin(teta5)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta4)), d6*cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5) - d6*cos(teta5)*sin(teta1)*sin(teta4) d6*cos(teta1)*sin(teta2)*sin(teta5), 0] Linha 2: [ d3*cos(teta1)*sin(teta2) - d6*(sin(teta5)*(sin(teta1)*sin(teta4) cos(teta1)*cos(teta2)*cos(teta4)) - cos(teta1)*cos(teta5)*sin(teta2)) - d2*sin(teta1), sin(teta1)*(d6*(cos(teta2)*cos(teta5) - cos(teta4)*sin(teta2)*sin(teta5)) + d3*cos(teta2)), sin(teta1)*sin(teta2), d6*sin(teta5)*(cos(teta1)*cos(teta4) - cos(teta2)*sin(teta1)*sin(teta4)), d6*cos(teta1)*cos(teta5)*sin(teta4) - d6*sin(teta1)*sin(teta2)*sin(teta5) + d6*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1), 0] Linha 3: [0, - d3*sin(teta2) - d6*cos(teta5)*sin(teta2) - d6*cos(teta2)*cos(teta4)*sin(teta5), cos(teta2), d6*sin(teta2)*sin(teta4)*sin(teta5), - d6*cos(teta2)*sin(teta5) - d6*cos(teta4)*cos(teta5)*sin(teta2), 0]

2.3.

Concluso parcial

Cabe ressaltar a importncia no estudo de matrizes jacobianas para relacionar a velocidade no espao das juntas com a velocidade do espao cartesiano. Essa primeira parte do trabalho obteve bons resultados prticos, envolvendo o desenvolvimento de um algoritmo compuational para obteno das matrizes Jacobianas at o estudo dos manipuladores cilndrico, SCARA e Stanford.

3. Desenvolvimento das trajetrias LSPB e polinmio de 3 grau

Para desenvolver o algoritmo para obteno das trajetrias desejadas, foi usado um rob com elo nico, a fim de facilitar os clculos. A partir disso, pode-se generalizar os resultados aqui obtidos expandindo o sistema para manipuladores mais complexos.

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________

3.1.
close all clear all clc % % s0 sf v0 vf tf % a0 a1 a2 a3

Algoritmo em Matlab para polinmio de 3 grau

Polinmio de 3 Grau. Condies Iniciais. = 15; = 75; = 0; = 0; = 3; Coeficientes (ai). s0 0 (3/tf^2)*(sf - s0) -(2/tf^3)*(sf - s0)

= = = =

% Posio (p). t = [0:1/40:3] s = a0 + a1*t + a2*t.^2 + a3*t.^3 figure(1) plot(t,s) % Velocidade (v). v = a1 + 2*a2*t + 3*a3*t.^2 figure(2) plot(t,v) % Acelerao (a). a = 2*a2 + 6*a3*t figure(3) plot(t,a)

3.2.

Sistemtica do algoritmo

Podemos facilmente entender o algoritmo observando o clculo dos coeficientes e da posio em funo dos coeficientes. Vemos que, dadas as condies iniciais, obtemos para a trajetria do manipulador um polinmio de 3 grau, como era desejado. Em funo de caractersticas inerentes de um polinmio de 3 grau, obtemos os coeficientes para a montagem da equao S. J o polinmio da velocidade e da acelerao so simples derivaes temporais de primeira e segunda ordem, respectivamente, do polinmio de posio S.

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________

3.3.

Resultados obtidos

Figura 6 - Grfico da trajetria do manipulador em polinmio de 3 grau

Figura 7 - Grfico da velocidade do manipulador em polinmio de 3 grau

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________

Figura 8 - Grfico da acelerao de um manipulador em polinmio de 3 grau

3.4.
clc clear all close all %LSPB % s0 sf v0 t0 tf

Algoritmo em Matlab para trajetria do tipo LSPB

Condies Iniciais. = 15; = 75; = 0; = 0; = 3;

% Limitaes Vmin = (sf-s0)/tf; Vmax = 2*(sf-s0)/tf; % a velocidade v deve ser escolhida entre os seguintes valores: v = 19; if (v>=Vmax) v=Vmax-0.1; end if (v<=Vmin)

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________ v=Vmin+0.1; end

% tempo crtico tb = (s0 - sf + v*tf)/v; % temporizaes t0_b = 0:0.001:tb; tb_fb = tb:0.001:tf-tb; tfb_f = tf-tb:0.001:tf; % matriz de "um" para plotar [r,c1]=size(t0_b); [r,c2]=size(tb_fb); [r,c3]=size(tfb_f); m1=ones(1,c1); m2=ones(1,c2); m3=ones(1,c3); % Posio (p). s0_b = s0 + (v/(2*tb))*t0_b.^2; sb_fb = (sf+s0-v*tf)/2 +v*tb_fb; sfb_f = sf - (v*(tf-tfb_f).^2)/(2*tb); figure(1) plot(t0_b,s0_b) hold plot(tb_fb,sb_fb) plot(tfb_f,sfb_f) % Velocidade (v). v0_b = v*t0_b/tb; vb_fb = v*m2; vfb_f = -(v*tfb_f/tb)+(tf/tb)*v; figure(2) plot(t0_b,v0_b) hold plot(tb_fb,vb_fb) plot(tfb_f,vfb_f) % Acelerao (a). a0_b = v/tb*m1; ab_fb = 0*m2; afb_f = -v/tb*m3; figure(3) plot(t0_b,a0_b) hold plot(tb_fb,ab_fb) plot(tfb_f,afb_f)

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________

3.5.

Sistemtica do algoritmo

O mtodo LSPB caracteriza-se, principalmente, pela maneira que tratada a velocidade e a acelerao do rob, a fim de obter uma trajetria linear mas com curvas suaves. Assim, fica fcil perceber, graficamente, como o manipulador deve se comportar. Cabe destacar os trs momentos distintos em que o manipulador deve atravessar: do tempo inicial at tb, de tb at tf tb e de tf-tb at o tempo final tf. Sendo assim, a acelerao do rob deve ser constante entre t0 e tb e entre tf-tb at tf, para que, consequentemente, a velocidade seja linear nesses trechos. Assim, obtem-se uma trajetria do tipo LSPB. As equaes geradas para o modelo LSPB so similares ao estudo realizado com polinmios de 3 grau: devido aos parmetros intrnsecos da modelagem de trajetria, obtemse os coeficientes e as equaes para desenvolver tal movimentao.

3.6.

Resultados obtidos

Figura 9 - Trajetria do tipo LSPB

UNIVERSIDADE FEDERAL DE SO CARLOS CCET

____________________________________________________________________________

Figura 10 - Velocidade do tipo LSPB

Figura 11 - Acelerao do tipo LSPB