CINEMÁTICA-DIRECTA-E INVERSA-DE-UN-BRAZO ROBÓTICO-con-LEGO-MINDSTORMS-Y-MATLAB PDF

Carolina
Silva P.
Gabriela Chambilla C.
CINEMÁTICA DIRECTA E INVERSA DE UN BRAZO ROBÓTICO
1. OBJETIVOS:

1.1. OBJETIVO GENERAL:

Realizar el análisis teórico del brazo robótico de dos grados de libertad,
cinemática directa y cinemática inversa, y luego implementar físicamente con
el kit de LEGO Mindstorm.

1.2. OBJETIVOS ESPECIFICOS:

Obtener el rango de funcionamiento de cada uno de los motores, trabajando
el primer motor de 0° a 90° y el segundo de 0° a 180°.

Obtener el valor de 𝜃! 𝑦 𝜃! en las tres trayectorias dadas por los puntos A y B a
través de la cinemática inversa.

2. MARCO TEORICO:

2.1. CINEMÁTICA DIRECTA

La cinemática directa permite conocer cuál es la posición y orientación que adopta el
extremo del robot cuando cada una de las variables que fijan la posición u orientación
de sus articulaciones toma valores determinados.
También podremos mencionar que no es de interés saber las causas que se presentan
en el movimiento de los motores. Si no, únicamente se toma en cuenta la descripción
del mismo.
Como ejemplo se muestra en la figura 1, un brazo de dos grados de libertad.
Carolina Silva P.

Figura1. Ejemplo de la cinemática directa de un BR de dos grados de libertad.

El análisis general que se realiza en la cinemática Directa es la siguiente:

2.2. CINEMÁTICA INVERSA

La cinemática inversa consiste en encontrar los valores que deben adoptar las
coordenadas articulares del robot para que su extremo se posicione y oriente según
una determinada localización espacial.

A diferencia de la cinemática Directa, el análisis general que se realiza es la siguiente:

Carolina Silva P.

2.3. MOTOR NXT:

Este motor es específico del NXT, incluye un

codificador de rotación, volviendo al NXT la
posición del eje con gran resolución. A
causa del conector especial de este motor
(el tipo de enchufe no estándar telefónico),
requiere de un adaptador de cable para
funcionar con fuentes de 9V. Velocidad de rotación lenta, reduciendo al mínimo la
necesidad de tren de engranaje externo.
2.4. CARACTERISTICAS DE MOTOR NXT:

2.4.1. CARACTERISTICAS EN VACIO:

Condiciones de prueba: el motor es impulsado por una fuente de energía
variable, regulada. Un amperímetro mide la corriente que fluye por el motor,
un voltímetro supervisa la tensión a través. La velocidad de rotación es
medida por un RCX equipado con un sensor ligero (de luz), mirando un cilindro
mitad-‐blanco/mitad-‐negro.

2.4.2. CARACTERISTICAS EN PARADO:

El momento de rotación en parado es medido para el peso máximo que puede
ser levantado por la máquina. La medida del momento de rotación es muy
imprecisa.

Carolina Silva P.

TABLA 1

CARACTERÍSTICAS DEL MOTOR NXT
Peso del motor 80g
CARACTERISTICA EN VACIO (Sin carga)
Velocidad de rotación 170 r.p.m
Corriente sin carga 60mA
CARACTERISTICAS EN PARADO
Torque en parado 50 N.cm
Corriente parado 2A

TABLA 1. En la tabla 1 podemos observar las características del motor NXT en vacío y
en parado.
El motor NXT también está protegido por un termistor (Raychem RXE065 o Arroyos
MF-‐R065). Esto significa que la corriente por el resulta muy alta, entorno a los 2A la
cual solo puede ser mantenida durante unos segundos.

2.4.3. CARACTERISTICAS EN CARGA:

Utilizando amperímetro y voltímetro para medir las condiciones del sistema
en carga, vemos que el poder mecánico entregado por el motor es evaluado a
partir del tiempo que tarda en levantar un peso una determinada altura (5
vueltas de cilindro – las dos primeras vueltas no son contadas para eliminar la
aceleración inicial). El momento de rotación aplicado es obtenido a partir del
radio del cilindro y del peso.
El cilindro se encuentra colocado directamente sobre el eje del motor.

Carolina Silva P.

TABLA 2

Torque o Velocidad Corriente Poder Poder Rendimiento
momento de mecánico eléctrico

de rotación rotación
4.5 V 16.7 N.cm 33 rpm 0.6 A 0.58 W 2.7 W 21.4%
7 V 16.7 N.cm 82rpm 0.55 A 1.44 W 3.85 W 37.3%
9 V 16.7 N.cm 117rpm 0.55 A 2.03 W 4.95 W 41%
12 V 16.7 N.cm 177rpm 0.58 A 3.10 W 6.96 W 44.5%

TABLA2. En la tabla 2 podemos observar las características del motor NXT con
carga.

2.4.4. MOTO NXT VISTA INTERNA:

Carolina Silva P.

Figura 2 . Motor NXT – Vista Interna

3. ESTRUCTURA MECÁNICA

La estructura mecánica que se ha elegido para este experimento tenía que cumplir dos
importantes restricciones:
 Evitar el peso en sus articulaciones (Hacer uso de mecanismos).
 El largo de sus eslabones ser lo más preciso posible con respecto a los
valores iniciales que se han planteado (Para minimizar el error en
relación a los datos obtenidos por el programa y experimentales).
Por ello, su estructura final fue el siguiente que se observa en la figura 3.
Carolina Silva P.

Figura 3. Estructura Mecánica del Brazo Robótico

Como se puede observar en la figura 3, se logra minimizar el peso que se encontraba en el
motor 2 al no posicionar el motor NXT en el punto 2, tal y como se observa en la figura
siguiente (4).

Figura 4. Esquema del BR ideal.

Carolina Silva P.

Figura 5. Pieza – Barra

En la figura 5 se puede observar una de las piezas principales utilizadas, esta será
considerada como el eslabón 1, el cual debe tener 10 cm de largo. Se utilizó una
barra de 15 orificios, el cual mide 11.9 cm. Los 2centímetros restantes son
utilizados para que esté muy fijo en el motor NXT (MOTOR 1).
El MOTOR 2, no puede estar en la articulación 2, puesto que existiría mucho peso,
y el motor 1 realizaría mucho esfuerzo para levantarlo. Por ello, ambos motores
deben estar a una misma altura (óptimo).
Para la transmisión de movimiento que debe realizar el motor 2, estando a una
altura igual que el motor 1, debe existir un mecanismo para que esa transmisión
llegue a la articulación 2. Ese mecanismo es el que se presenta en la figura 6.

Carolina Silva P.

Figura 6. Transmisión de Movimiento circular por medio de engranajes.

Cabe mencionar, que estos engranajes deben estar sujetos con ejes (figura 7.a.) en vez de
utilizar los conectores (figura 7.b.). Puesto que el segundo presenta mucha fricción en la
transmisión de movimiento. Mientras que con la primera pieza mencionada no existe ello.
Y el error que llegaría a presentar el Brazo Robótico al experimentar trayectorias sería
mínimo.
(a) (b)
Figura 7. Piezas para sujetar los engranajes con la barra.

A continuación se observará la transmisión de engranajes que ya se mostró
anteriormente, en el Brazo Robótico montado con LEGO NXT.

Carolina Silva P.

Figura 8. Vista real del BR – Conexión de engranajes.

El segundo eslabón teóricamente debe medir 7 cm, sin embargo la pieza LEGO más
cercana a esta se diferencia por decimales: 7.04 cm.

Figura 9. Pieza – Eslabón 2 del Brazo Robótico.

Finalmente es necesario mencionar que el mecanismo utilizado para la estructura del
Brazo Robótico es óptimo. Puesto que las conexiones que se hicieron con los engranajes
Carolina Silva P.
sirven, aparte de transmitir el movimiento realizado por el motor 2, para mantener el
ángulo inicial que el eslabón tiene consigo. Gráficamente se observará a continuación:

(a)

(b)

Figura 10. Motor 1 en movimiento, Motor 2 sin movimiento.

Tal y como se observa, el motor 1 avanza aproximadamente 90° (b) y el motor 2 no
tiene ningún avance. Sin embargo, el motor 1 no altera el ángulo de inicio del
eslabón 2. El cual se observa claramente que sigue en la misma dirección y con el
mismo grado de ángulo -‐ (a) y (b).

Carolina Silva P.
4. ANALISIS DE LA CINEMATICA DIRECTA:
Primeramente realizamos el análisis de la cinemática directa1 para encontrar el rango de
operación de cada motor.
Para realizar el análisis de la cinemática directa realizamos la programación de la misma en
la interfaz de Matlab tomando las siguientes consideraciones:
𝝅
𝑹𝒆𝒑𝒆𝒕𝒊𝒕𝒊𝒗𝒊𝒅𝒂𝒅 𝒂𝒏𝒈𝒖𝒍𝒂𝒓 𝒅𝒆 𝒍𝒐𝒔 𝒎𝒐𝒕𝒐𝒓𝒆𝒔 = 𝟏° = 𝒓𝒂𝒅
𝟏𝟖𝟎
𝑳𝟏 = 𝟗. 𝟗𝒄𝒎
𝑳𝟐 = 𝟕. 𝟎𝟒𝒄𝒎

GRÁFICA 1
En la gráfica 1 podemos observar el lugar de trabajo de los motores 1 y 2 obtenidos a través de la
programación de la cinemática directa.

1
El programa se encuentra en los Anexos. – PROGRAMACIÓN 1 -‐ CINEMÁTICA DIRECTA DEL BR.
Carolina Silva P.
5. ANALISIS DE LA CINEMÁTICA INVERSA:

Para el análisis de la cinemática inversa debemos especificar las siguientes
consideraciones:
𝑹𝒆𝒑𝒆𝒕𝒊𝒕𝒊𝒗𝒊𝒅𝒂𝒅 𝒍𝒊𝒏𝒆𝒂𝒍 = 𝟎. 𝟓𝒄𝒎

Se dividió el espacio en 25 puntos, por tanto a continuación se presenta el programa para
cada trayectoria (diagonal, transversal y longitudinal):
5.1. PRIMERA TRAYECTORIA: DIAGONAL
Recordemos que el primer punto es:
𝑷 𝑨 = 𝟑 , 𝟓
𝑷 𝑩 = 𝟒 , 𝟏𝟓
Con el programa realizado en MATLAB2, tendremos la siguiente gráfica:
Entonces el programa:
GRAFICA 2
En la grafica 2 podemos observar la línea formada por los dos primeros puntos y el punto
medio, para el cual se hallan los ángulos a través de la cinemática inversa.

2
ANEXOS, Programa 2 : CI – TRAYECTORIA DIAGONAL
Carolina Silva P.
A partir del análisis de cinemática inversa obtuvimos los ángulos respectivos para cada
punto. Más adelante se observarán dichos valores (tabla 3)

5.2. SEGUNDA TRAYECTORIA: HORIZONTAL 3

Recordemos que el segundo punto es:
𝑃 𝐴 = −6 , 10
𝑃 𝐵 = 13 , 10

GRÁFICA 3
En la grafica 3 podemos observar la línea formada por los dos segundos puntos y el punto
medio, para el cual se hallan los ángulos a través de la cinemática inversa. En este caso
se trata de una línea horizontal.

punto, los cuales se mostrarán más adelante en la tabla 4.

3
PROGRAMACIÓN 3 – C.I. TRAYECTORIA HORIZONTAL -‐ ANEXOS

Carolina Silva P.

5.3. TERCERA TRAYECTORIA: VERTICAL
Recordemos que el tercer punto es:
𝑷 𝑨 = 𝟑 , 𝟓
𝑷 𝑩 = 𝟑 , 𝟏𝟎
4
Y con ayuda de su programa se obtendrá:
GRÁFICA 4
En la grafica 4 podemos observar la línea formada por los dos últimos puntos y el punto
medio, para el cual se hallan los ángulos a través de la cinemática inversa. En este caso
se trata de una línea completamente vertical.

punto, las cuales se observarán en la tabla 5.

4
PROGRAMACIÓN 4 – C.I. TRAYECTORIA VERTICAL -‐ anexos

Carolina Silva P.
6. PROGRAMACIóN MOTOR NXT:

Para realizar la implementación física a través del kit de LEGO MINDSTORMS se
realizó el respectivo programa para cada trayectoria.

Entonces la siguiente tabla indica los 23 puntos que varían dentro del rango del primer
punto de partida y el de llegada. Para ello tenemoS:

PRIM ERA TRAYECTORIA
TABLA 3

TRAYECTORIA
DIAGONAL

ÁNGULOS -‐ ÁNGULOS
-‐ ERROR ERROR ERROR
PROGRAMA TACÓMETRO ABSOLUTO RELATIVO PORCENTUAL
MOTO MOTO MOTO MOTO MOT MOT MOT MOT MOTOR MOTOR
R 1 R 2 R 1 R 2 OR 1 OR 2 OR 1 OR 2 1 2

1 3 140 2 139 1 1 0.333 0.007 33.3 0.7
-‐
2 5 137 4 138 1 -‐1 0.200 -‐0.007 20.0 0.7
-‐
3 8 133 8 134 0 -‐1 0.000 -‐0.008 -‐ 0.8
-‐
4 10 130 10 131 0 -‐1 0.000 -‐0.008 -‐ 0.8

5 12 127 12 127 0 0 0.000 0.000 -‐ -‐
-‐ -‐
6 14 123 15 124 -‐1 -‐1 -‐0.071 -‐0.008 7.1 0.8
-‐ -‐
7 16 120 17 121 -‐1 -‐1 -‐0.063 -‐0.008 6.3 0.8

8 18 116 18 115 0 1 0.000 0.009 -‐ 0.9
-‐
9 20 112 19 113 1 -‐1 0.050 -‐0.009 5.0 0.9
1
0 22 109 22 109 0 0 0.000 0.000 -‐ -‐
1 -‐
1 24 105 25 105 -‐1 0 -‐0.042 0.000 4.2 -‐
Carolina Silva P.
1 -‐
2 27 101 30 101 -‐3 0 -‐0.111 0.000 11.1 -‐
1
3 29 97 29 96 0 1 0.000 0.010 -‐ 1.0
1 -‐
4 31 92 33 92 -‐2 0 -‐0.065 0.000 6.5 -‐
1 -‐
5 33 88 35 88 -‐2 0 -‐0.061 0.000 6.1 -‐
1 -‐
6 36 83 39 83 -‐3 0 -‐0.083 0.000 8.3 -‐
1 -‐
7 38 78 41 78 -‐3 0 -‐0.079 0.000 7.9 -‐
1
8 41 73 41 73 0 0 0.000 0.000 -‐ -‐
1 -‐ -‐
9 43 68 44 69 -‐1 -‐1 -‐0.023 -‐0.015 2.3 1.5
2 -‐
0 46 62 49 62 -‐3 0 -‐0.065 0.000 6.5 -‐
2 -‐
1 50 55 52 54 -‐2 1 -‐0.040 0.018 4.0 1.8
2
2 53 48 51 48 2 0 0.038 0.000 3.8 -‐
2 -‐
3 57 40 57 41 0 -‐1 0.000 -‐0.025 -‐ 2.5

Carolina Silva P.
SEGUNDA TRAYECTORIA:
TABLA 4.
TRAYECTORIA
HORIZONTAL

-‐ ERROR
ERROR
ERROR
MOTOR MOTOR MOTOR MOTOR MOTO MOTO MOTO MOTO MOTOR MOTOR
1 2 1 2 R 1 R 2 R 1 R 2 1 2
-‐

1 -‐29 95 -‐29 96 0 -‐1 0.000 -‐0.011 -‐ 1.1
-‐ -‐
2 -‐30 99 -‐34 100 4 -‐1 -‐0.133 -‐0.010 13.3 1.0
-‐ -‐
3 -‐31 103 -‐33 104 2 -‐1 -‐0.065 -‐0.010 6.5 1.0
-‐ -‐
4 -‐31 106 -‐33 107 2 -‐1 -‐0.065 -‐0.009 6.5 0.9
-‐ -‐
5 -‐32 108 -‐35 109 3 -‐1 -‐0.094 -‐0.009 9.4 0.9
-‐
6 -‐32 110 -‐30 111 -‐2 -‐1 0.063 -‐0.009 6.3 0.9

7 -‐32 111 -‐31 111 -‐1 0 0.031 0.000 3.1 -‐
-‐
8 -‐32 112 -‐34 111 2 1 -‐0.063 0.009 6.3 0.9
-‐
9 -‐32 111 -‐35 111 3 0 -‐0.094 0.000 9.4 -‐
-‐
10 -‐32 110 -‐35 110 3 0 -‐0.094 0.000 9.4 -‐
-‐
11 -‐32 108 -‐33 107 1 1 -‐0.031 0.009 3.1 0.9
-‐
12 -‐32 106 -‐30 107 -‐2 -‐1 0.063 -‐0.009 6.3 0.9
-‐
13 -‐31 102 -‐30 103 -‐1 -‐1 0.032 -‐0.010 3.2 1.0
-‐
14 -‐31 99 -‐33 98 2 1 -‐0.065 0.010 6.5 1.0
-‐ -‐
15 -‐30 94 -‐32 95 2 -‐1 -‐0.067 -‐0.011 6.7 1.1
-‐
16 -‐29 89 -‐32 89 3 0 -‐0.103 0.000 10.3 -‐
-‐
17 -‐27 83 -‐28 83 1 0 -‐0.037 0.000 3.7 -‐
-‐
18 -‐25 77 -‐27 77 2 0 -‐0.080 0.000 8.0 -‐
-‐
19 -‐23 69 -‐24 69 1 0 -‐0.043 0.000 4.3 -‐

20 -‐20 60 -‐20 59 0 1 0.000 0.017 -‐ 1.7
21 -‐17 50 -‐18 51 1 -‐1 -‐0.059 -‐0.020 -‐ -‐
Carolina Silva P.
5.9 2.0

22 -‐12 37 -‐12 37 0 0 0.000 0.000 -‐ -‐

23 -‐4 13 -‐4 13 0 0 0.000 0.000 -‐ -‐

TERCERA TRAYECTORIA:
TABLA 5
TRAYECTORI
A VERTICAL

-‐ ERROR ERROR ERROR

MO
TOR MOT MOT MOT MOT MOT MOT MOTO MOTO
1 OR 2 MOTOR 1 OR 2 OR 1 OR 2 OR 1 OR 2 R 1 R 2
-‐ -‐
1 42 145 44 145 -‐2 0 0.048 0.000 4.8 -‐
-‐ -‐ -‐ -‐
2 43 144 45 145 -‐2 -‐1 0.047 0.007 4.7 0.7

3 43 142 43 141 0 1 0.000 0.007 -‐ 0.7

4 43 140 43 140 0 0 0.000 0.000 -‐ -‐
-‐ -‐
5 43 139 44 139 -‐1 0 0.023 0.000 2.3 -‐
-‐ -‐ -‐ -‐
6 43 137 44 138 -‐1 -‐1 0.023 0.007 2.3 0.7
-‐ -‐
7 43 135 43 136 0 -‐1 0.000 0.007 -‐ 0.7
-‐ -‐
8 43 134 45 134 -‐2 0 0.047 0.000 4.7 -‐

9 43 132 43 132 0 0 0.000 0.000 -‐ -‐
-‐ -‐ -‐ -‐
10 43 130 44 131 -‐1 -‐1 0.023 0.008 2.3 0.8
-‐ -‐
11 43 128 47 128 -‐4 0 0.093 0.000 9.3 -‐
-‐ -‐
12 43 127 44 127 -‐1 0 0.023 0.000 2.3 -‐
-‐ -‐ -‐ -‐
13 43 125 46 126 -‐3 -‐1 0.070 0.008 7.0 0.8
-‐ -‐
14 42 123 45 123 -‐3 0 0.071 0.000 7.1 -‐
Carolina Silva P.
-‐ -‐ -‐ -‐

15 42 121 46 122 -‐4 -‐1 0.095 0.008 9.5 0.8
-‐ -‐
16 42 120 44 120 -‐2 0 0.048 0.000 4.8 -‐
-‐ -‐ -‐ -‐
17 41 118 45 119 -‐4 -‐1 0.098 0.008 9.8 0.8
-‐ -‐
18 41 116 40 117 1 -‐1 0.024 0.009 2.4 0.9
-‐ -‐ -‐ -‐
19 41 114 43 115 -‐2 -‐1 0.049 0.009 4.9 0.9
-‐ -‐ -‐ -‐
20 40 112 43 113 -‐3 -‐1 0.075 0.009 7.5 0.9
-‐ -‐ -‐ -‐
21 40 110 41 111 -‐1 -‐1 0.025 0.009 2.5 0.9
-‐ -‐ -‐ -‐
22 39 109 41 110 -‐2 -‐1 0.051 0.009 5.1 0.9
-‐ -‐
23 39 107 42 107 -‐3 0 0.077 0.000 7.7 -‐

7. CONCLUSIONES:

Como se puede observar en las tablas mostradas anteriormente, la diferencia entre el
valor obtenido por el programa MATLAB, y leídos por el tacómetro, no son tan distintos.
Dicha diferencia no sobrepasa de 5°.
Se ha comprobado que los valores obtenidos por el programa, puedan realizar la
trayectoria deseada: diagonal, vertical, horizontal. Pero se observó también que existe
cierta forma brusca, es decir, a veces el eslabón 1 tenía que moverse 5°,
aproximadamente, y no se movía o no se podía observar claramente su movimiento del
motor. Sin embargo esta observación no fue algo repetitivo.
Algunos casos el ángulo que realiza es negativo, y ello gracias a su estructura se pudo
lograr.
Los errores porcentuales que se observan en las tres últimas tablas, es mínima como se
había mencionado anteriormente, y esto es debido a una estructura ideal que se obtuvo.
Se hizo la prueba con otra estructura y se observó mucho error relativo por su peso y
esfuerzo que realizaba el motor 1. Por ello fue muy necesario cambiar la estructura.

Carolina Silva P.
ANEXOS
PROGRAMACIÓN 1 -‐ CINEMÁTICA DIRECTA DEL BRAZO ROBÓTICO
close all;
clear all;
clc;
L1 = 10;
L2 = 7;
dt = pi/36;
% *********************************************************
% CÁLCULO DE LA CINEMÁTICA DIRECTA
% *********************************************************
t1 = 0:dt:pi/2;
t2 = 0:dt:pi;
[T1, T2] = meshgrid(t1,t2);
X1 = L1*cos(T1);
Y1 = L1*sin(T1);
X2 = L2*cos(T1+T2);
Y2 = L2*sin(T1+T2);
X = X1 + X2;
Y = Y1 + Y2;
plot(X(:), Y(:), 'r.');

grid on;
xlabel('X en centimetros');
ylabel('Y en centimetros');
title('Lugar de movimiento del BR');
(CONSIDERANDO 5 GRADOS COMO SU REPETITIVIDAD ANGULAR.

Sin em bargo, sólo se desea saber el rango de operación de los
m otores)
Carolina Silva P.
PROGRAMACIÓN 2 – C.I. TRAYECTORIA DIAGONAL
close all;
clear all;
clc;
L1 = 10;
L2 = 7;
dt = pi/180;
% *********************************************************
% CALCULO DE LA CINEMATICA DIRECTA
% *********************************************************
t1 = 0:dt:pi/2;
t2 = 0:dt:pi;
X1 = L1*cos(T1);
Y1 = L1*sin(T1);
X2 = L2*cos(T1+T2);
Y2 = L2*sin(T1+T2);
X = X1 + X2;
Y = Y1 + Y2;
plot(X(:), Y(:), 'r.');
grid on;
% *********************************************************
% CALCULO DE LA CINEMATICA INVERSA

Carolina Silva P.
% *********************************************************
xAt = 3;
yAt = 5;
xBt = 4;
yBt = 15;
hold on;
plot(xAt, yAt,'bo');
plot(xBt, yBt,'bo');
hold off;
line([xAt xBt],[yAt yBt],'Color','b');
axis square;
dxy = 0.5;
m = (yBt-yAt)/(xBt-xAt);
y0 = yBt-m*xBt;
x = xAt:dxy:xBt;
x = linspace(xAt, xBt, 25)
y = m*x + y0;
[Xinv, Yinv] = meshgrid(x,y);
p = x(13)
q = y(13)
L = L1^2 + L2^2;
R2 = p^2 + q^2;
cs2 = (R2 - L)/(2*L1*L2);
cs2(abs(cs2)<= 1e-3)= 0;
whos cs2
Carolina Silva P.
K1 = cs2^2;
K2 = 1-K1;
K2(abs(K2)<= 1e-3)= 0;
sn2 = sqrt(K2);
clear K1 K2
a = L1 + L2*cs2;
b = L2*sn2;
hold on;
plot (p, q,'bo');
hold off;
T2D = atan2(sn2,cs2)
T1D = atan2(q,p) - atan2(b,a)
T2D =(T2D*180)/pi
T1D =(T1D*180)/pi
T1D(abs(T1D)<= 1) = 0;
T2D(abs(T2D)<= 1) = 0;
Carolina Silva P.
PROGRAMACIÓN 3 – C.I. TRAYECTORIA HORIZONTAL
close all;
clear all;
clc;
L1 = 10;
L2 = 7;
dt = pi/180;
% *********************************************************
% *********************************************************
t1 = 0:dt:pi/2;
t2 = 0:dt:pi;
X1 = L1*cos(T1);
Y1 = L1*sin(T1);
X2 = L2*cos(T1+T2);
Y2 = L2*sin(T1+T2);
X = X1 + X2;
Y = Y1 + Y2;
plot(X(:), Y(:), 'r.');
grid on;
Carolina Silva P.
% *********************************************************
% *********************************************************
xAt = -6;
yAt = 10;
xBt = 13;
yBt = 10;
hold on;
hold off;
axis square;
dxy = 0.5;
m = (yBt-yAt)/(xBt-xAt);
y0 = yBt-m*xBt;
x = xAt:dxy:xBt;
x = linspace(xAt, xBt, 25)
y = m*x + y0;
p = x(13)
q = y(13)
L = L1^2 + L2^2;
R2 = p^2 + q^2;
Carolina Silva P.
cs2 = (R2 - L)/(2*L1*L2);
cs2(abs(cs2)<= 1e-3)= 0;
whos cs2
cs2
K1 = cs2^2;
K2 = 1-K1;
K2(abs(K2)<= 1e-3)= 0;
sn2 = sqrt(K2);
clear K1 K2
a = L1 + L2*cs2;
b = L2*sn2;
hold on;
plot (p, q,'bo');
hold off;
T2D =(T2D*180)/pi
T1D =(T1D*180)/pi
T1D(abs(T1D)<= 1) = 0;
T2D(abs(T2D)<= 1) = 0;

Carolina Silva P.
PROGRAMACIÓN 4 – C.I. TRAYECTORIA VERTICAL

close all;
clear all;
clc;
L1 = 10;
L2 = 7;
dt = pi/180;
% *********************************************************
% *********************************************************
t1 = 0:dt:pi/2;
t2 = 0:dt:pi;
X1 = L1*cos(T1);
Y1 = L1*sin(T1);
X2 = L2*cos(T1+T2);
Y2 = L2*sin(T1+T2);
X = X1 + X2;
Y = Y1 + Y2;
plot(X(:), Y(:), 'r.');
grid on;
Carolina Silva P.
% *********************************************************
% *********************************************************
xAt = 3;
yAt = 5;
xBt = 3;
yBt = 10;
hold on;
hold off;
axis square;
dxy = 0.5;
y = yAt:dxy:yBt;
y= linspace(yAt, yBt, 25)
x = ((xBt-xAt)*(y-yBt)/(yBt-yAt))+xBt;
p = x(13)
q = y(13)
L = L1^2 + L2^2;
R2 = p^2 + q^2;
cs2 = (R2 - L)/(2*L1*L2);

Carolina Silva P.
cs2(abs(cs2)<= 1e-3)= 0;
whos cs2
cs2
K1 = cs2^2;
K2 = 1-K1;
K2(abs(K2)<= 1e-3)= 0;
sn2 = sqrt(K2);
clear K1 K2
a = L1 + L2*cs2;
b = L2*sn2;
hold on;
plot (p, q,'bo');
hold off;
T2D =(T2D*180)/pi
T1D =(T1D*180)/pi
T1D(abs(T1D)<= 1) = 0;
T2D(abs(T2D)<= 1) = 0;

Programación 5. Motores NXT 1 TRAYECTORIA
COM_CloseNXT all
h = COM_OpenNXT('bluetooth.ini');
COM_SetDefaultNXT(h);
mA = NXTMotor('A', 'Power', 50);
mA.ResetPosition();
mB = NXTMotor('B', 'Power', 50);
Carolina Silva P.
t1k = mA.ReadFromNXT();
% sprintf('Motor A is finally at position %5.2f',t1k);
mB.ResetPosition();
t1k_taco = zeros(25)';
t2k_taco = zeros(25)';
double(t1k_taco)
double(t2k_taco)
for k=1:25
mA.TachoLimit = T1D(k);
mA.SendToNXT();
pause(2);
mB.TachoLimit = T2D(k);
mB.SendToNXT();
pause(2);
mA.Stop('brake');
mB.Stop('brake');
t1k_taco(k)= mA.ReadFromNXT();
pause(2);
t2k_taco(k)= mB.ReadFromNXT();
end
%disp(sprintf('Motor B is finally at position %d', data1.Position));
COM_CloseNXT all
mA = NXTMotor('A');
mA.Power = 50;
mA.SendToNXT();
mB = NXTMotor('B');
mB.Power = 50;
mB.SendToNXT();
Carolina Silva P.
pause(7);
mB.Stop('off');
mA.Stop('brake');
pause(1);
mA.Stop('off');
mA.ResetPosition();
mB.ResetPosition();
pause(5);
mA.TachoLimit = M1;
mB.TachoLimit = M2;
mA.SendToNXT();
mB.SendToNXT();
data = mA.ReadFromNXT();
data1 = mB.ReadFromNXT();
disp(sprintf('Motor A is currently at position %d', data.Position));

disp(sprintf('Motor B is currently at position %d', data1.Position));
% wait until the motor doesn't move anymore

pause(2);
% and display the position again:
disp(sprintf('Motor A is finally at position %d', data.Position));
disp(sprintf('Motor B is finally at position %d', data1.Position));

COM_CloseNXT all
mA = NXTMotor('A');
mA.Power = 50;
mA.SendToNXT();
mB = NXTMotor('B');
mB.Power = 50;
Carolina Silva P.
mB.SendToNXT();
pause(7);
mB.Stop('off');
mA.Stop('brake');
pause(1);
mA.Stop('off');
mA.ResetPosition();
mB.ResetPosition();
pause(5);
mA.TachoLimit = M1;
mB.TachoLimit = M2;
mA.SendToNXT();
mB.SendToNXT();
disp(sprintf('Motor A is currently at position %d', data.Position));

disp(sprintf('Motor B is currently at position %d', data1.Position));
% wait until the motor doesn't move anymore

pause(2);
% and display the position again:
disp(sprintf('Motor A is finally at position %d', data.Position));
disp(sprintf('Motor B is finally at position %d', data1.Position));

CINEMÁTICA-DIRECTA-E INVERSA-DE-UN-BRAZO ROBÓTICO-con-LEGO-MINDSTORMS-Y-MATLAB PDF

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

CINEMÁTICA-DIRECTA-E INVERSA-DE-UN-BRAZO ROBÓTICO-con-LEGO-MINDSTORMS-Y-MATLAB PDF

Cargado por

Copyright:

Formatos disponibles

Carolina

CINEMÁTICA DIRECTA E INVERSA DE UN BRAZO ROBÓTICO

Este motor es específico del NXT, incluye un

2.4. CARACTERISTICAS DE MOTOR NXT:

Peso del motor 80g

CARACTERISTICA EN VACIO (Sin carga)

Velocidad de rotación 170 r.p.m

Corriente sin carga 60mA

CARACTERISTICAS EN PARADO

Torque en parado 50 N.cm

4.5 V 16.7 N.cm 33 rpm 0.6 A 0.58 W 2.7 W 21.4%

7 V 16.7 N.cm 82rpm 0.55 A 1.44 W 3.85 W 37.3%

9 V 16.7 N.cm 117rpm 0.55 A 2.03 W 4.95 W 41%

12 V 16.7 N.cm 177rpm 0.58 A 3.10 W 6.96 W 44.5%

4. ANALISIS DE LA CINEMATICA DIRECTA:

5. ANALISIS DE LA CINEMÁTICA INVERSA:

6. PROGRAMACIóN MOTOR NXT:

-­‐ -­‐ -­‐ -­‐

plot(X(:), Y(:), 'r.');

(CONSIDERANDO 5 GRADOS COMO SU REPETITIVIDAD ANGULAR.

PROGRAMACIÓN 2 – C.I. TRAYECTORIA DIAGONAL

% CALCULO DE LA CINEMATICA DIRECTA

[T1, T2] = meshgrid(t1,t2);

plot(X(:), Y(:), 'r.');

title('Lugar de movimiento del BR');

% CALCULO DE LA CINEMATICA INVERSA

line([xAt xBt],[yAt yBt],'Color','b');

x = linspace(xAt, xBt, 25)

[Xinv, Yinv] = meshgrid(x,y);

cs2 = (R2 - L)/(2*L1*L2);

plot (p, q,'bo');

T1D = atan2(q,p) - atan2(b,a)

PROGRAMACIÓN 3 – C.I. TRAYECTORIA HORIZONTAL

% CALCULO DE LA CINEMATICA DIRECTA

[T1, T2] = meshgrid(t1,t2);

plot(X(:), Y(:), 'r.');

title('Lugar de movimiento del BR');

% CALCULO DE LA CINEMATICA INVERSA

line([xAt xBt],[yAt yBt],'Color','b');

x = linspace(xAt, xBt, 25)

[Xinv, Yinv] = meshgrid(x,y);

cs2 = (R2 - L)/(2*L1*L2);

plot (p, q,'bo');

T1D = atan2(q,p) - atan2(b,a)

PROGRAMACIÓN 4 – C.I. TRAYECTORIA VERTICAL

% CALCULO DE LA CINEMATICA DIRECTA

[T1, T2] = meshgrid(t1,t2);

plot(X(:), Y(:), 'r.');

title('Lugar de movimiento del BR');

% CALCULO DE LA CINEMATICA INVERSA

line([xAt xBt],[yAt yBt],'Color','b');

y= linspace(yAt, yBt, 25)

[Xinv, Yinv] = meshgrid(x,y);

cs2 = (R2 - L)/(2*L1*L2);

plot (p, q,'bo');

T1D = atan2(q,p) - atan2(b,a)

%disp(sprintf('Motor B is finally at position %d', data1.Position));

Programación 6. Motores NXT 2 TRAYECTORIA

disp(sprintf('Motor A is currently at position %d', data.Position));

% wait until the motor doesn't move anymore

disp(sprintf('Motor A is currently at position %d', data.Position));

% wait until the motor doesn't move anymore

También podría gustarte

-‐ -‐ -‐ -‐

cs2 = (R2 - L)/(2L1L2);

cs2 = (R2 - L)/(2L1L2);

cs2 = (R2 - L)/(2L1L2);