SESIÓN 6: “ CINEMÁTICA DIRECTA – ALGORITMO DE DENAVIT - HARTENBERG
I.
”
OBJETIVO
Sistematizar la solución del problema cinemático directo mediante el algoritmo de Denavit-Hartenberg.
Desarrollar programas en MATLAB que permitan determinar la cinemática del manipulador con el método de Denavit- Hartenberg.
II.
III.
MATERIAL Y EQUIPO
Una PC con SO Windows y MATLAB
Robot KUKA KR6
PROCEDIMIENTO 1. Sea el robot KUKA de la siguiente figura, donde aparecen todos los elementos mecánicos que lo conforman.
2. Dibujar el esquema del robot KUKA KR6 (según la simbología del libro de Barrientos)
3. Aplicar el Algoritmo de Denavit-Hartenberg y resolver el modelo cinemático directo del robot.
4. Consignar en los recuadros siguientes su esquema y los sistemas asignados según el algoritmo D-H. 5. 6. Consignar en una Tabla los parámetros de Denavit-Hartenberg del robot KUKA KR6
Articulación 1 2 3 4 5 6
Ɵ 3
675
260
90⁰
0
680
0⁰
0
0
-90⁰
670
0
-90⁰
0
0
90⁰
158
0
0⁰
7. Con ayuda del docente y/o asistente del laboratorio, colocar al robot en su
“posición cero”, es decir, todos los parámetros articulares con un valor angular que satisfagan esa posición.
Dar un primer valor diferente de cero a cada uno de los ejes del robot. Remplazar este valor en las ecuaciones obtenidas por medio del algoritmo de D-H y obtener los valores numéricos para la posición y orientación del efector final del robot. Dibujar esta localización espacial indicando la posición y la orientación del EF. Dibujar el sistema solidario al EF.
=−90 =−90 3 =45 =−40 =90 =0 0 0 1 0 1 =[100 010 000 2606751 ] 0 −1 0 0 1 =[ 001 000 100 68001 ] −0. 7 071 0 0. 7 071 0 1 3 =[ −0.00 7071 −100 −0.700071 68001 ] 0. 7 660 0 0. 6 428 0 13 =[ −0.006428 −100 0.766000 67001 ] 0 0 1 0 1 =[100 010 000 001] 1 0 0 0 1 =[000 001 100 15801 ] 0 −0. 8 0. 6 101. 6 =[ −0.−0.077 −0.0.505 −0.0.055 1718.844.1 41]
Cuando:
,
Las matrices son:
,
,
,
,
8. Con ayuda del docente y/o asistente de laboratorio mueva las articulaciones del robot a los valores dados en el paso anterior y con una cinta métrica medir los valores de la posición del EF (O BRIDA) del robot referidos a la base del robot y contrastar con los obtenidos según su análisis de D-H. También contrastar con los valores mostrados por la unidad de control del robot. Haga sus observaciones. NOTA IMPORTANTE: NO MUEVA EL ROBOT A VALORES ARTICULARES QUE ESTEN FUERA DEL RANGO ESTABLECIDO POR LA TABLA. PREFERENTEMENTE NO EMPLEE VALORES QUE ESTEN CERCANOS A LOS LIMITES ESTABLECIDOS POR LA TABLA. clc, clear all,close all %definimos las longitudes de los brazos q1=input('ingrese q1=q1*pi/180; q2=input('ingrese q2=q2*pi/180; q3=input('ingrese q3=q3*pi/180; q4=input('ingrese q4=q4*pi/180; q5=input('ingrese q5=q5*pi/180; q6=input('ingrese q6=q6*pi/180;
el angulo de la primera articulacion: ' ); el angulo de la segunda articulacion: ' ); el angulo de la tercera articulacion: ' ); el angulo de la cuarta articulacion: ' ); el angulo de la quinta articulacion: ' ); el angulo de la sexta articulacion: ' );
A1=[cos(-q1) 0 sin(-q1) 260*cos(-q1); sin(-q1) 0 -cos(-q1) 260*sin(-q1); 0 1 0 705; 0 0 0 1]; A2=[cos(-q2) -sin(-q2) 0 680*cos(-q2); sin(-q2) cos(-q2) 0 680*sin(-q2); 0 0 1 0; 0 0 0 1]; A3=[cos(-q3-pi/2) 0 -sin(-q3-pi/2) 35*cos(-q3-pi/2); sin(-q3-pi/2) 0 cos(-q3-pi/2) 35*sin(-q3-pi/2); 0 -1 0 0; 0 0 0 1]; A4=[cos(-q4) 0 -sin(-q4) 0; sin(-q4) 0 cos(-q4) 0; 0 -1 0 670; 0 0 0 1]; A5=[cos(q5) 0 sin(q5) 0; sin(q5) 0 -cos(q5) 0; 0 1 0 0; 0 0 0 1]; A6=[cos(q6) -sin(q6) 0 0; sin(q6) cos(q6) 0 0; 0 0 1 158; 0 0 0 1]; T=A1*A2*A3*A4*A5*A6 %posicion 1 A=A1; %posicion 2 B=A*A2; %posicion 3 C=B*A3; %posicion 4 D=C*A4;
%posicion5 E=D*A5; %posicion 6 F=E*A6; x=[0 A(1,4) B(1,4) C(1,4) D(1,4) E(1,4) F(1,4)]; y=[0 A(2,4) B(2,4) C(2,4) D(2,4) E(2,4) F(2,4)]; z=[0 A(3,4) B(3,4) C(3,4) D(3,4) E(3,4) F(3,4)]; %GRAFICA plot3(x,y,z, 'k','linewidth',3);grid on axis([-2500 2500 -2500 2500 0 2500]) %EJES DE ORIENTACION DE LA PRIMERA ARTICULACION hold on x0=[0 A(1,1)*100]; x1=[0 A(2,1)*100]; x2=[0 A(3,1)*100]; plot3(x0,x1,x2,'r','LineWidth', 2) hold on y0=[0 A(1,2)*100]; y1=[0 A(2,2)*100]; y2=[0 A(3,2)*100]; plot3(y0,y1,y2,'b','LineWidth', 2) hold on z0=[0 A(1,3)*100]; z1=[0 A(2,3)*100]; z2=[0 A(3,3)*100]; plot3(z0,z1,z2,'g','LineWidth', 2) %EJES DE ORIENTACION DE LA SEGUNDA ARTICULACION hold on x01=[A(1,4) A(1,4)+B(1,1)*100]; x11=[A(2,4) A(2,4)+B(2,1)*100]; x21=[A(3,4) A(3,4)+B(3,1)*100]; plot3(x01,x11,x21, 'r','LineWidth', 2) hold on y01=[A(1,4) A(1,4)+B(1,2)*100]; y11=[A(2,4) A(2,4)+B(2,2)*100]; y21=[A(3,4) A(3,4)+B(3,2)*100]; plot3(y01,y11,y21, 'b','LineWidth', 2) hold on z01=[A(1,4) A(1,4)+B(1,3)*100]; z11=[A(2,4) A(2,4)+B(2,3)*100]; z21=[A(3,4) A(3,4)+B(3,3)*100]; plot3(z01,z11,z21, 'g','LineWidth', 2) %EJES DE ORIENTACION DE LA TERCERA ARTICULACION hold on x01=[B(1,4) B(1,4)+C(1,1)*100]; x11=[B(2,4) B(2,4)+C(2,1)*100]; x21=[B(3,4) B(3,4)+C(3,1)*100]; plot3(x01,x11,x21, 'r','LineWidth', 2) hold on y01=[B(1,4) B(1,4)+C(1,2)*100]; y11=[B(2,4) B(2,4)+C(2,2)*100]; y21=[B(3,4) B(3,4)+C(3,2)*100]; plot3(y01,y11,y21, 'b','LineWidth', 2) hold on z01=[B(1,4) B(1,4)+C(1,3)*100]; z11=[B(2,4) B(2,4)+C(2,3)*100]; z21=[B(3,4) B(3,4)+C(3,3)*100]; plot3(z01,z11,z21, 'g','LineWidth', 2) %EJES DE ORIENTACION DE LA CUARTA ARTICULACION hold on x01=[C(1,4) C(1,4)+D(1,1)*100];
x11=[C(2,4) C(2,4)+D(2,1)*100]; x21=[C(3,4) C(3,4)+D(3,1)*100]; plot3(x01,x11,x21, 'r','LineWidth', 2) hold on y01=[C(1,4) C(1,4)+D(1,2)*100]; y11=[C(2,4) C(2,4)+D(2,2)*100]; y21=[C(3,4) C(3,4)+D(3,2)*100]; plot3(y01,y11,y21, 'b','LineWidth', 2) hold on z01=[C(1,4) C(1,4)+D(1,3)*100]; z11=[C(2,4) C(2,4)+D(2,3)*100]; z21=[C(3,4) C(3,4)+D(3,3)*100]; plot3(z01,z11,z21, 'g','LineWidth', 2) %EJES DE ORIENTACION DE LA QUINTA ARTICULACION hold on x01=[D(1,4) D(1,4)+E(1,1)*100]; x11=[D(2,4) D(2,4)+E(2,1)*100]; x21=[D(3,4) D(3,4)+E(3,1)*100]; plot3(x01,x11,x21, 'r','LineWidth', 2) hold on y01=[D(1,4) D(1,4)+E(1,2)*100]; y11=[D(2,4) D(2,4)+E(2,2)*100]; y21=[D(3,4) D(3,4)+E(3,2)*100]; plot3(y01,y11,y21, 'b','LineWidth', 2) hold on z01=[D(1,4) D(1,4)+E(1,3)*100]; z11=[D(2,4) D(2,4)+E(2,3)*100]; z21=[D(3,4) D(3,4)+E(3,3)*100]; plot3(z01,z11,z21, 'g','LineWidth', 2) %EJES DE ORIENTACION DE LA SEXTA ARTICULACION hold on x01=[E(1,4) E(1,4)+F(1,1)*100]; x11=[E(2,4) E(2,4)+F(2,1)*100]; x21=[E(3,4) E(3,4)+F(3,1)*100]; plot3(x01,x11,x21, 'r','LineWidth', 2) hold on y01=[E(1,4) E(1,4)+F(1,2)*100]; y11=[E(2,4) E(2,4)+F(2,2)*100]; y21=[E(3,4) E(3,4)+F(3,2)*100]; plot3(y01,y11,y21, 'b','LineWidth', 2) hold on z01=[E(1,4) E(1,4)+F(1,3)*100]; z11=[E(2,4) E(2,4)+F(2,3)*100]; z21=[E(3,4) E(3,4)+F(3,3)*100]; plot3(z01,z11,z21, 'g','LineWidth', 2)
