DINÁMICA DEL MANIPULADOR

1. DINÁMICA DEL MANIPULADOR

2. DINÁMICA
• Dis1nguimos:
– Dinámica inversa: ¿cuál es el par en cada
ar1culación para un conjunto de q, qp, qpp?
• Dada q, qp, qpp  calcula el par necesario en cada
ar1culación para llevar al brazo a ese estado.
– Dinámica directa: ¿cuál es el movimiento del brazo
si aplicamos unos pares det.?
• Dadas q, qp y tau  calcula la aceleración en cada eje.
Podemos integrar el movimiento (simulación). Necesita
conocer la dinámica inversa para resolverse.

3. DINÁMICA INVERSA
• Hace uso de las ecuaciones clásicas de
movimiento rela1vo entre sistemas de
referencia.
• El algoritmo se divide en dos fases:
– Propagación hacia delante de las velocidades y
aceleraciones ar1culares. Desde la ar1culación 1 a
la n.
– Cálculo hacia atrás de los pares/fuerzas desde el
extremo del brazo hasta la base.

4. DINÁMICA INVERSA: ECUACIONES

5. DINÁMICA INVERSA: ECUACIONES

6. DINÁMICA INVERSA: ECUACIONES
NÓTESE: los pares Ni y fuerzas Fi se conocen, ya que se han calculado en las
ecuaciones hacia adelante

8. EJEMPLO:
• Cálculo de la dinámica directa usando la
toolbox de matlab.
• Robot puma 560 de Unimate.

13. EJEMPLO:
• Se agrupan todos los datos que influyen en la
dinámica.
• Probar en diferentes posiciones conocidas.
• Probar con diferentes velocidades.
• Después, repe1r con nuestro robot,
cambiando los parámetros.

14. %matriz puma
% alpha A theta D sigma m rx ry rz Ixx Iyy Izz Ixy Iyz Ixz Jm
% G B Tc+ Tc-
p560 = [
pi/2 0 0 0 0 0 0 0 0 0 0.35 0 0 0 0 200e-6 -62.6111 1.48e-3 .395 -.435
0 .4318 0 0 0 17.4 -.3638 .006 .2275 .13 .524 .539 0 0 0 200e-6 107.815 .817e-3 .126 -.071
-pi/2 .0203 0 .15005 0 4.8 -.0203 -.0141 .070 .066 .086 .0125 0 0 0 200e-6 -53.7063 1.38e-3 .132 -.105
pi/2 0 0 .4318 0 0.82 0 .019 0 1.8e-3 1.3e-3 1.8e-3 0 0 0 33e-6 76.0364 71.2e-6 11.2e-3 -16.9e-3
-pi/2 0 0 0 0 0.34 0 0 0 .3e-3 .4e-3 .3e-3 0 0 0 33e-6 71.923 82.6e-6 9.26e-3 -14.5e-3
0 0 0 0 0 .09 0 0 .032 .15e-3 .15e-3 .04e-3 0 0 0 33e-6 76.686 36.7e-6 3.96e-3 -10.5e-3
];
% some useful poses
qz = [0 0 0 0 0 0]; % zero angles, L shaped pose
qr = [0 pi/2 -pi/2 0 0 0]; % ready pose, arm up
qstretch = [0 0 -pi/2 0 0 0];
%TAU = rne(DYN, Q, QD, QDD, GRAV, FEXT)
% torques en cada articulaciÛn con el robot parado
tau = rne(p560, qz, [0 0 0 0 0 0], [0 0 0 0 0 0], [0 0 9.81]', [0 0 0 0 0 0]');
plotbot(p560, qz, 'l')
tau
t = [0:.056:2]; % create time vector
[q,qd,qdd] = jtraj(qz, qr, t); % compute joint coordinate trajectory
tau = rne(p560, q, qd, qdd);
plotbot(p560, q, 'l')
figure, plot(tau)
legend('tau_1', 'tau_2','tau_3','tau_4','tau_5','tau_6')

15. Parámetros que modelan la dinámica

16. • P560: Parámetros
• Parámetros DH: alfa, a, theta, D
p560 = [
pi/2 0 0 0
0 .4318 0 0
-pi/2 .0203 0 .15005
pi/2 0 0 .4318
-pi/2 0 0 0
0 0 0 0];
• Col 5; Sigma=0, rotacional, sigma=1, prismá1ca
• Col 6: masas de cada elemento:
>> p560(:,6)
ans =
0
17.4000
4.8000
0.8200
0.3400
0.0900
• Posición del CDM respecto de su sist. de referencia.
p560(:,7:9)
ans =
0 0 0
-0.3638 0.0060 0.2275
-0.0203 -0.0141 0.0700
0 0.0190 0
0 0 0
0 0 0.0320

17. EJEMPLO:
• Probar en diferentes posiciones conocidas.
tau = rne(p560, qz, [0 0 0 0 0 0], [0 0 0 0 0 0], [0 0 9.81]', [0 0 0 0 0 0]');
plotbot(p560, qz, 'l')
tau =
0 37.4837 0.2489 0 0 0
Se puede aproximar como:
tau_1=17.4*9.81*0.071 + 6.05*9.81*0.4318

18. %matriz puma
% alpha A theta D sigma m rx ry rz Ixx Iyy Izz Ixy Iyz Ixz Jm
% G B Tc+ Tc-
p560 = [
pi/2 0 0 0 0 0 0 0 0 0 0.35 0 0 0 0 200e-6 -62.6111 1.48e-3 .395 -.435
0 .4318 0 0 0 17.4 -.3638 .006 .2275 .13 .524 .539 0 0 0 200e-6 107.815 .817e-3 .126 -.071
-pi/2 .0203 0 .15005 0 4.8 -.0203 -.0141 .070 .066 .086 .0125 0 0 0 200e-6 -53.7063 1.38e-3 .132 -.105
pi/2 0 0 .4318 0 0.82 0 .019 0 1.8e-3 1.3e-3 1.8e-3 0 0 0 33e-6 76.0364 71.2e-6 11.2e-3 -16.9e-3
-pi/2 0 0 0 0 0.34 0 0 0 .3e-3 .4e-3 .3e-3 0 0 0 33e-6 71.923 82.6e-6 9.26e-3 -14.5e-3
0 0 0 0 0 .09 0 0 .032 .15e-3 .15e-3 .04e-3 0 0 0 33e-6 76.686 36.7e-6 3.96e-3 -10.5e-3
];
% some useful poses
qz = [0 0 0 0 0 0]; % zero angles, L shaped pose
qr = [0 pi/2 -pi/2 0 0 0]; % ready pose, arm up
qstretch = [0 0 -pi/2 0 0 0];
%TAU = rne(DYN, Q, QD, QDD, GRAV, FEXT)
% torques en cada articulaciÛn con el robot parado
tau = rne(p560, qz, [0 0 0 0 0 0], [0 0 0 0 0 0], [0 0 9.81]', [0 0 0 0 0 0]');
plotbot(p560, qz, 'l')
tau
t = [0:.056:2]; % create time vector
[q,qd,qdd] = jtraj(qz, qr, t); % compute joint coordinate trajectory
tau = rne(p560, q, qd, qdd);
plotbot(p560, q, 'l')
figure, plot(tau)
legend('tau_1', 'tau_2','tau_3','tau_4','tau_5','tau_6')