UNIVERSIDAD DE PAMPLONA

Unidad de Ingeniería Mecatrónica

Facultad de Ingenierías. Programa de Ingeniería Mecatrónica.
Laboratorio I
Robótica II

CODIFICACIÓN Lab I Tema: Herramientas matemáticas,

cinemática y programación en
matlab

OBJETIVO:
Afianzar los conocimientos correspondientes a las herramientas matemáticas utilizadas en
en robotica y al igual que algunos conceptos cinemáticos.

OBJETIVOS ESPECIFICOS:
1.- Realizar ejercicios utilizando las herramientas matemáticas que permiten especificar la
posición y orientación de un objeto en el espacio.
2.- Dibujar objetos en el espacio 3D en matlab
3.- Programar funciones de uso común en el desarrollo de modelos cinemáticos de un robot.
REQUISITOS:
Conocer los conceptos generales de:
1.- Herramientas matemáticas para la localización espacial
2.- Cinemática del robot
3.- Programación básica en matlab
EQUIPOS Y HERRAMIENTAS
1.- Computador con matlab

PROCEDIMIENTO
1.- Diseñar un algoritmo que calcule un vector unitario ( v p ) el cual es perpendicutar a un
vector dado ( v i = v ixi x + v iy jx + v izk x ). Debe verificarse la siguiente propiedad:

vi ⋅ v p = 0

Nota: Observerse que existen infinitos vectores que cumplen esta propiedad. Por lo tanto se
debe seleccionar alguno de ellos.

2. Crear una función que dibuje un cono, tomando como datos de entrada un vector que
indica la posición del cono (centro de la base del cono), un vector que indica la dirección
del cono (orientación), un escalar que indica el diametro de la base del cono, un vector que
indica el color de la superficie del cono ( color = [R,G,B]) y un vector que indica el color
de los bordes del cono.

Ejemplo:

dibujar_cono(pos, vec,t,d, color, color_borde)

pos = [0,0,10];
vec = [1,1,1];
t = 5;

VERSION 1 – Ph.D. César Augusto Peña C

 UNIVERSIDAD DE PAMPLONA
Unidad de Ingeniería Mecatrónica
Facultad de Ingenierías. Programa de Ingeniería Mecatrónica.
Laboratorio I
Robótica II

d = 4;
color = [1,0,0];
color_borde = [0,0,0];

Procedimiento:

a) Dado el vector que indica la dirección del cono (vec), hallar un vector unitario
perpendicular al mismo. Haciendo uso de este vector unitario y conociendo el
diametro de la base del cono, calcular un punto ubicado sobre la base de cono.
b) A partir del punto calculado, el vector que indica la dirección y el diametro del
cono, obtener n puntos ubicados sobre la base del cono. Tomar n = 10 y realizar una
grafica similar a la figura 1.

13

12.5

12

11.5

11

10.5
z

10

9.5

8.5

8
4
2
0
ï2 0 1 2 3
ï2 ï1
y
x
figura 1. Generación de n puntos ubicados sobre la base del cono

c) Dibujar un triangulo que tenga como vertices dos puntos consecutivos de la base del
cono y la punta del mismo.

%ejemplo dibujo de un triangulo usando la funcion patch

f3=figure(3), clf
set(f3,'Color',[1, 1, 1]);
hold on
grid on
patchi = patch( [0 , 2 , 1] , [1 , 2 , 3] , [0 , 0 , 2] , 'g');
set(patchi, 'EdgeColor', 'k' );
xlabel('x','FontSize',15);
ylabel('y','FontSize',15);
zlabel('z','FontSize',15);
view(55,15)

VERSION 1 – Ph.D. César Augusto Peña C

 UNIVERSIDAD DE PAMPLONA
Unidad de Ingeniería Mecatrónica
Facultad de Ingenierías. Programa de Ingeniería Mecatrónica.
Laboratorio I
Robótica II

13

2 12.5

12

11.5
1.5
11

10.5

z
1
z

10

9.5
0.5
9

8.5

0 8
0 4
0.5 3
1 2.5 2
1.5 2 0
1.5 3
2 1 ï2 ï1 0 1 2
ï2
y y
x x

figura 2. Generación de triangulos: Izq. Ejemplo, Der. Triangulo del cono

d) General la superficie del cono, tal como se muestra en la figura.

13

12.5

12

11.5

11

10.5
z

10

9.5

8.5

8
4
2
0
ï2 0 1 2 3
ï2 ï1
y
x

Figura 3. Superficie del cono

e) Dibujar la base del cono. Dibujar el cono aumentando el número de puntos a 100.

13 13

12.5 12.5

12 12

11.5 11.5

11 11

10.5 10.5
z

10 10

9.5 9.5

9 9

8.5 8.5

8 8
4 4
2 2
0 0
ï2 0 1 2 3 ï2 0 1 2 3
ï2 ï1 ï2 ï1
y y
x x

Figura 4. Cono generado con 10 puntos

3. De forma analoga al ejercicio 2, crear una función que dibuje una flecha teniendo en
cuenta los siguientes parametros de entrada:
- Vector de posición del origen de la flecha

VERSION 1 – Ph.D. César Augusto Peña C

 UNIVERSIDAD DE PAMPLONA
Unidad de Ingeniería Mecatrónica
Facultad de Ingenierías. Programa de Ingeniería Mecatrónica.
Laboratorio I
Robótica II

- Vector que indica la dirección de la flecha (orientación).

- Longitud total de la flecha.
- Longitud de la punta de la flecha (diametro de la base del cono)
- Diametro del cilindro
- Diametro de la base del cono
- Vector que indica el color ([R,G,B])

% ejemplo del dibujo de una flecha

f7=figure(7), clf
set(f7,'Color',[1, 1, 1]);
grid on;
xlabel('x','FontSize',15);
ylabel('y','FontSize',15);
zlabel('z','FontSize',15);
view(10,28)

pos = [1,1,1];
vec = [1,1,1];
tflecha = 2;
tpunta = 0.8;
dcil = 1;
dcono = 2;
color = [0,0,1];
dibujar_flecha3D(pos, vec,tflecha, tpunta,dcil,dcono, color)

2.5

1.5
z

1
2

1
0.5
0.5 1 1.5 0
2 2.5 3 y
x

Figura 5. Flecha 3D.

4) Crear una función que dibuje un sistema coordenado. Teniendo en cuenta los siguientes
parámetros de entrada:
dibujar_sistema_coordenadas_3D(A, L, G, texto)
- A: Matriz homogenea
- L: Longitud de los ejes coordenados
- G: Grosor de los ejes coordenados
- texto: nombre del sistema coordenado

%ejemplo del dibujo de un sistema de coordenadas

VERSION 1 – Ph.D. César Augusto Peña C

 UNIVERSIDAD DE PAMPLONA
Unidad de Ingeniería Mecatrónica
Facultad de Ingenierías. Programa de Ingeniería Mecatrónica.
Laboratorio I
Robótica II

f8=figure(8);, clf
set(f8,'Color',[1, 1, 1]);
grid on;
xlabel('x','FontSize',15);
ylabel('y','FontSize',15);
zlabel('z','FontSize',15);
view(28,54)
Sn = [sqrt(2)/2, -sqrt(2)/2 , 0 , 0 ; ...
0 , 0 , 1 , 0 ; ...
sqrt(2)/2 , sqrt(2)/2 , 0 , 0 ; ...
0 , 0 , 0 , 1 ];
dibujar_sistema_coordenadas_3D(Sn, 10, 2, 'n')

yn
10 zn

5
xn
z

15
0
10
ï5
ï10 5
ï5
0 0
5
10 ï5 y
x
Figura 6. Sistema coordenado

5) Obtener los parámetros Denavit-Hartenberg y las matrices de transformación

homogenea correspondientes a cada uno de los ejes coordenados. Recomendación: crear
funciones que calculen las matrices homogeneas a partir de los parametros de cada una de
las articulaciones. Crear una función que dibuje los ejes coordenados y represente los
eslabones del robot por medio de lineas, teniendo como parametros de entrada un vector
con las coordenadas articulares del robot (Cinematica directa del robot).

VERSION 1 – Ph.D. César Augusto Peña C

 UNIVERSIDAD DE PAMPLONA
Unidad de Ingeniería Mecatrónica
Facultad de Ingenierías. Programa de Ingeniería Mecatrónica.
Laboratorio I
Robótica II

Figura 7. Robot de 4 GDL

7) Realizar una función que calcule la cinemática inversa del robot de la figura 7 y pinte el
robot en la posición indicada.

MARCO TEORICO – FORMULAS UTILES

Rotaciones:

Par de rotación:
Rot(k,θ )p = pcosθ + (k × p) senθ + k(k ⋅ p)(1− cosθ )

Algoritmo Denavit – Hartenberg:

•D-H 1.- Numerar los eslabones comenzando con 1 (primer eslabón móvil de la cadena) y
acabando con n (último eslabón móvil). Se numerará como eslabón 0 a la base fija del
robot.
•D-H 2.- Numerar cada articulación comenzando por 1 (la correspondiente al primer grado
de libertad) y acabando en n
•D-H 3.- Localizar el eje de cada articulación. Si ésta es rotativa, el eje será su propio eje de
giro. Si es prismática, será el eje a lo largo del cual se produce el desplazamiento.
•D-H 4.- Para i de 0 a n-1 situar el eje zi sobre el eje de la articulación i+1.
•D-H 5.- Situar el origen del sistema de la base {S0} en cualquier punto del eje z0. Los ejes
x0 e y0 se situarán de modo que formen un sistema dextrógiro con z0
•D-H 6.- Para i de 1 a n-1, situar el sistema {Si} (solidario al eslabón i) en la intersección
del eje zi con la línea normal común a zi-1 y zi. Si ambos ejes se cortasen se situaría {Si} en
el punto de corte. Si fuesen paralelos {Si} se situaría en la articulación i+1
•D-H 7.- Situar xi en la línea normal común a zi-1 y zi
•D-H 8.- Situar yi de modo que forme un sistema dextrógiro con xi y zi .
•D-H 9.- Situar el sistema {Sn} en el extremo del robot de modo que zn coincida con la
dirección de zn-1 y xn sea normal a zn-1 y zn .
•D-H 10.- Obtener qi como el ángulo que hay que girar en torno a zi-1 para que xi-1 y xi
queden paralelos.
•D-H 11.- Obtener di como la distancia, medida a lo largo de zi-1, que habría que desplazar
{Si-1} para que xi y xi-1 quedasen alineados.

VERSION 1 – Ph.D. César Augusto Peña C

 UNIVERSIDAD DE PAMPLONA
Unidad de Ingeniería Mecatrónica
Facultad de Ingenierías. Programa de Ingeniería Mecatrónica.
Laboratorio I
Robótica II

•DH 12.- Obtener ai como la distancia medida a lo largo de xi (que ahora coincidiría con xi-
1) que habría que desplazar el nuevo {Si-1} para que su origen coincidiese con {Si}.
•DH 13.- Obtener ai como el ángulo que habría que girar entorno a xi (que ahora coincidiría
con xi-1), para que el nuevo {Si-1} coincidiese totalmente con {Si}.
•DH 14.- Obtener las matrices de transformación i-1Ai
•DH 15.- Obtener la matriz de transformación entre la base y el extremo del robot T = 0A1
1 n-1
A2 ... An .
•DH 16.- La matriz T define la orientación (submatriz de rotación) y posición (submatriz de
traslación) del extremo referido a la base en función de las n coordenadas articulares

i−1
Ai = Rotz(θ i )T(0,0,di )T(ai ,0,0)Rotx (α i )

⎡Cθ i −Cα i Sθ i Sα i Sθ i aiCθ i ⎤

⎢ Sθ Cα iCθ i −Sα iCθ i ai Sθ i ⎥
Ai = ⎢ ⎥ (Matriz D-H)
i−1 i

⎢ 0 Sα i Cα i di ⎥
⎢ 0 1 ⎥⎦
⎣ 0 0

VERSION 1 – Ph.D. César Augusto Peña C