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. MATERIAL Y EQUIPO
Una PC con SO Windows y MATLAB
Robot KUKA KR6
III. 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 𝑞1 675 260 90⁰
2 𝑞2 0 680 0⁰
3 𝑞3 0 0 -90⁰
4 𝑞4 670 0 -90⁰
5 𝑞5 0 0 90⁰
6 𝑞6 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.
Cuando:
𝑞1 = −90 , 𝑞2 = −90 , 𝑞3 = 45 , 𝑞4 = −40 , 𝑞5 = 90 , 𝑞6 = 0
Las matrices son:
0 0 1 0
0
1 𝐴1 = [ 1 0 0 260 ]
0 1 0 675
0 0 0 1
0 −1 0 0
1
1 𝐴2 = [ 1 0 0 680 ]
0 0 1 0
0 0 0 1
−0.7071 0 0.7071 0
2
1 𝐴3 = [ −0.7071 0 −0.7071 680 ]
0 −1 0 0
0 0 0 1
0.7660 0 0.6428 0
3
1 𝐴4 = [ −0.6428 0 0.7660 0 ]
0 −1 0 670
0 0 0 1
0 0 1 0
14 𝐴5 = [1 0 0 0]
0 1 0 0
0 0 0 1
1 0 0 0
1 𝐴6 = [ 0 1 0 0 ]
5
0 0 1 158
0 0 0 1
0 −0.8 0.6 101.6
𝑇=[ −0.7 0.5 0.5 844.1]
−0.7 −0.5 −0.5 1718.4
0 0 0 1
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 el angulo de la primera articulacion: ');
q1=q1*pi/180;
q2=input('ingrese el angulo de la segunda articulacion: ');
q2=q2*pi/180;
q3=input('ingrese el angulo de la tercera articulacion: ');
q3=q3*pi/180;
q4=input('ingrese el angulo de la cuarta articulacion: ');
q4=q4*pi/180;
q5=input('ingrese el angulo de la quinta articulacion: ');
q5=q5*pi/180;
q6=input('ingrese el angulo de la sexta articulacion: ');
q6=q6*pi/180;
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)
