Trabajo Final Robot Manipulador Mentor Educacional PDF

UNIVERSIDAD TECNOLOGICA NACIONAL
REGIONAL MENDOZA
LARMA
1 de Octubre
ROBOT
MANIPULADOR
MENTOR 2011
El presente proyecto se enfoca al control de un robot manipulador por
medio de una computadora. Sin disponer de la información de software
Educational Robots
ni de hardware del fabricante se procederá a diseñar y elaborar los
circuitos, interfaz y programas que conformaran el sistema de control Cybernetic Applications
del robot manipulador, así como la integración del mismo.
Se adjunta CD-Rom con simulación en Matlab y Proteus.
PROYECTO FINAL
CATEDRA: SISTEMA DE CONTROL
PROYECTO: MODELADO CINEMATICO Y SISTEMA DE CONTROL DE POSICION BRAZO ROBOTICO
ALUMNO: GONZALO BERARDO - LEGAJO: 26499 – Mail: Gonzalo_b@msn.com
Desarrollado por Gonzalo Berardo en dependencia al grupo de investigación LARMA (Laboratorio

de Robots Móvil Autónomos), para la cátedra Sistemas de Control.
2 ROBOT MANIPULADOR MENTOR
INDICE
OBJETIVOS
Objetivo General Pág. 3
Objetivo de Cátedra Pág. 5
DETALLE GENERAL DEL BRAZO ROBOTICO Pág. 6

Estructura Mecánica Pág. 6
Elementos Terminales Pág. 6
Sistema Sensorial Pág. 6
SISTEMA DE CONTROL Pág. 7

Términos Básicos Pág. 7
Planteamiento del problema Pág. 8
Elementos Pág. 9
Controlador PID Pág. 9

Reglas de sintonización de Ziegler-Nichols Pág. 10
Desarrollo Práctico Pág. 12
Sintonización Fina Pág. 16
Diseño del controlador PID Discreto Pág. 18

Implementación del controlador Pág. 19
CINEMATICA Y DINAMICA DE UN SISTEMA Pág. 21

Términos Básicos Pág. 21
Dinámica Inversa Pág. 21
Definición de Parámetros Pág. 22
Modelo Dinámico Pág. 22
Ecuación de Lagrange Pág. 25
Fricción Pág. 26
Dinámica Directa Pág. 29
Simulación en Matlab Pág. 30
DOCUMENTOS DE REFERENCIA Pág. 42
LARMA – Laboratorio de Robots Móviles Autónomos

Gonzalo Berardo
OBJETIVO GENERAL
El presente proyecto se enfoca al control de un robot manipulador por medio de una

computadora. Sin disponer de la información de software ni de hardware del fabricante se
procederá a diseñar y elaborar los circuitos, interfaz y programas que conformaran el sistema
de control del robot manipulador, así como la integración del mismo.
El proyecto se divide en Modelado Matemático y Sistema de Control de Posición.
 MODELADO MATEMATICO
Los modelos cinemáticos directo e inverso; estarán basados en las convenciones referidas
en Denavit-Hartenberg y en el método geométrico respectivamente.
Se pueden plantear dos problemas:

- Conocer la posición del extremo del robot basándose en la posición de las articulaciones
(Problema Cinemático Directo)
- Conocer la posición de las articulaciones del robot basándose en la posición del extremo
del robot (Problema Cinemático Inverso)
Los modelos dinámicos directo e inverso están fundamentados en la formulación de

movimiento de Lagrange.
De igual manera, se pueden plantear dos problemas:

- Dinámico directo: Calculo del movimiento del extremo del robot a partir de los pares y
fuerzas de los accionadores de las articulaciones.
- Dinámico inverso: Calculo de los valores de los pares y fuerzas de los accionadores de las
articulaciones a partir de las posiciones, velocidades y aceleraciones requeridas para el
extremo del robot.
 SISTEMA DE CONTROL DE POSICION
Para el control de posición de cada articulación del robot manipulador, se recurre al diseño
y construcción de un controlador PID discreto.
Partiendo del desconocimiento del modelo matemático de la planta a controlar, se

implementa una serie de procedimientos experimentales para la sintonía de los
controladores PID. Estos procedimientos se basan en la teoría de sintonización de Ziegler
Nichols.
A continuación se adjunta el diseño de diagrama en bloque general del proyecto.

Gonzalo Berardo
Ref 1 MOTOR
PID DSCRETO POTENCIA POTE
CINTURA
ADC
MICROCONTROLADOR 1
Ref 2 MOTOR
HOMBRO
ADC
MICROCONTROLADOR 2
CINEMATICA Ref 3 MOTOR

DIRECTA/INVERSA CODO
ADC
MICROCONTROLADOR 3
Ref 4 MOTOR
MUÑECA
(Giro y elevación)
ADC
MICROCONTROLADOR 4
Ref 5 MOTOR
MUÑECA
(Giro y elevación)
ADC
PC MICROCONTROLADOR 5
Figura 1. Diagrama en bloque general.

Gonzalo Berardo
MICROCONTROLADOR 5
OBJETIVO DE CATEDRA
Se requiere diseñar y construir un controlador PID para regular la posición de un motor de

corriente continua (CC). La figura 2 muestra el diagrama de bloques del sistema controlado, en
donde:
Figura 2. Diagrama de bloques del sistema controlado
- La señal de salida, y, corresponde a la salida del terminal móvil del potenciómetro.

- La señal de referencia, r, corresponde a la posición deseada.
- La señal de error, e, corresponde a la diferencia entre la señal de referencia y la señal de
salida.
- La señal de control, u, corresponde al voltaje producido por el controlador para
disminuir o anular el error.
Partiendo del deconocimiento del modelo matematico de la planta, se sintonizara los

parametros del controlador PID, por medio de la primer regla de sintonizacion de Ziegler
Nichols.
Se evaluaran las distintas contribucones proporcional, derivatica e integral para luego hacer un
refinamiento del mismo.

Gonzalo Berardo
DETALLE GENERAL DEL BRAZO ROBOTICO
Robot Manipulador: Mentor (Nº de Serie: 803132), fabricado por Cybernetic Applications, bajo
fines educativos.
El mismo presenta una configuración de tipo manipulador articulado y cuenta con una
arquitectura poliarticulada.
Es un robot de 5 GDL (grados de libertad) los cuales están denotados por sus similares al ser
humano, estos son: cintura, hombro, codo, muñeca-elevación y muñeca-giro.
Cada articulación realiza movimientos rotatorios desarrollados por actuadotes que son motores
de 12 voltios de corriente continua (CC) acoplados a una caja reductora y su posicionamiento se
registra a través de encoders analógicos (potenciómetros rotativos resistivos).
Los elementos que lo integran están construidos de acero, plástico y aluminio siendo este
último el que constituye la mayor parte de su estructura.
Su capacidad máxima de carga es de un kilogramo.
Estructura Mecánica
- Tipo: Robot Antropomórfico o Articulado de 5 GDL.

- Configuraciones Cinemáticas: RRR Cintura rotacional, Hombro rotacional, Codo
rotacional.
Ventajas: Máxima flexibilidad. Volumen de trabajo mayor que el volumen del robot.
Apto para entornos corrosivos. Permite accionadores eléctricos.
Desventajas: Modelo cinemático complejo (movimientos lineales).
- Accionadores: Eléctricos. Motores a DC.
Elementos Terminales
- Tipo: Pinza Mecánica.

- Herramientas: No posee.
Sistema Sensorial
- Sensores Internos: Encoders Analógicos. Localizados en la propia estructura del robot.

Gonzalo Berardo
SISTEMA DE CONTROL
Términos Básicos
- Señal de salida: es la variable que se desea controlar (posición, velocidad, presión,

temperatura, etc.). También se denomina variable controlada.
- Señal de referencia: es el valor que se desea que alcance la señal de salida.
- Error: es la diferencia entre la señal de referencia y la señal de salida real.
- Señal de control: es la señal que produce el controlador para modificar la variable

controlada de tal forma que se disminuya, o elimine, el error.
- Señal análoga: es una señal continua en el tiempo.
- Señal digital: es una señal que solo toma valores de 1 y 0.
- Conversor análogo/digital: es un dispositivo que convierte una señal analógica en una

señal digital (1 y 0).
- Conversor digital/análogo: es un dispositivo que convierte una señal digital en una señal
analógica (corriente o voltaje).
- Planta: es el elemento físico que se desea controlar. Planta puede ser: un motor, un
horno, un sistema de disparo, un sistema de navegación, un tanque de combustible, etc.
- Proceso: operación que conduce a un resultado determinado.
- Sistema: consiste en un conjunto de elementos que actúan coordinadamente para

realizar un objetivo determinado.
- Perturbación: es una señal que tiende a afectar la salida del sistema, desviándola del
valor deseado.
- Sensor: es un dispositivo que convierte el valor de una magnitud física (presión, flujo,
temperatura, etc.) en una señal eléctrica codificada ya sea en forma analógica o digital.
También es llamado transductor. Los sensores, o transductores, analógicos envían, por
lo regular, señales normalizadas de 0 a 5 voltios, 0 a 10 voltios o 4 a 20 mA.
- Sistema de control en lazo cerrado: es aquel en el cual continuamente se está

monitoreando la señal de salida para compararla con la señal de referencia y calcular la
señal de error, la cual a su vez es aplicada al controlador para generar la señal de control
y tratar de llevar la señal de salida al valor deseado. También es llamado control
realimentado.

Gonzalo Berardo
- Sistema de control en lazo abierto: en estos sistemas de control la señal de salida no es

monitoreada para generar una señal de control.
Planteamiento del problema

Se requiere diseñar y construir un controlador PID para regular la posición de un motor de
corriente continua (CC). La figura 2 muestra el diagrama de bloques del sistema controlado, en
donde:
La señal de salida, y, corresponde a la salida del terminal móvil del potenciómetro. Si éste se
alimenta con 5 voltios en sus terminales fijos (a y b), producirá un voltaje en su terminal móvil
(c) equivalente a su posición. Podemos decir entonces que cuando produce 0 voltios esta en la
posición equivalente a 0 grados, 1.25 voltios corresponderá a 90 grados, 2.5 voltios a 180
grados, etc.
La señal de referencia, r, corresponde a la posición deseada. Es decir, si queremos que el motor

alcance la posición 180 grados debemos colocar una referencia de 2.5 voltios, si queremos 270
grados colocamos referencia de 3.75 voltios, etc. Esta señal de referencia será determinada a
partir de la cinemática y de la dinámica del sistema.
La señal de error, e, corresponde a la diferencia entre la señal de referencia y la señal de salida.

Por ejemplo, si queremos que el motor alcance la posición de 90 grados colocamos una señal
de referencia de 1.25 voltios y esperamos dónde se ubica exactamente. Si se posiciona en 67.5
grados el potenciómetro entregará una señal de salida de 0.9375 voltios y la señal de error, e,
será de 0.3125 voltios (22.5 grados).
La señal de control, u, corresponde al voltaje producido por el controlador para disminuir o

anular el error. Si la señal de error es positiva indica que la referencia es mayor que la salida
real, entonces el controlador coloca un voltaje positivo al motor para que continúe girando
hasta minimizar o anular el error. Si por el contrario la señal de error resulta negativa indica que
la salida sobrepasó la referencia entonces el controlador debe poner un voltaje negativo para
que el motor gire en sentido contrario hasta minimizar o anular el error.
Figura 2. Diagrama de bloques del sistema controlado

Gonzalo Berardo
Elementos
- Motor de CC de imán permanente de 12V, 500mA y 500 r.p.m. Los motores de CC de imán
permanente comerciales normalmente no giran a la misma velocidad en sentido dextrógiro
que en sentido levógiro por lo que el controlador no tendrá la misma respuesta en ambos
sentidos.
- Potenciómetro lineal de 5 KΩ , de una sola vuelta.
- Acople mecánico entre el eje del motor y el eje del potenciómetro.
- Fuente de 5 voltios de corriente continúa para alimentar los terminales fijos del
potenciómetro.
ACOPLE MOTOR POTENCIOMETRO
+5 V (Terminal a)
0 a +5 V (Terminal c)
GND (Terminal b))
+5 V
GND
Figura 3. Motor - Potenciómetro.
Controlador PID
Los controladores PID son ampliamente usados en los sistemas de control industrial , cuyas
siglas corresponden; P: proporcional, I: integral, D: derivativo, se refieren a los tres términos
que operan sobre la señal de error para producir la señal a la salida del controlador.
Se aplican a la mayoría de los sistemas de control, pero se aprecia más su utilidad cuando el
modelo de la planta a controlar no se conoce y los métodos analíticos no pueden ser
empleados. En este caso se debe recurrir a procedimientos experimentales para la sintonía de
los controladores PID.

Gonzalo Berardo
Si se pude obtener un modelo matemático de la planta, es posible aplicar diversas técnicas de

diseño con el fin de determinar los parámetros del controlador que cumpla con las
especificaciones del transitorio y del estado estacionario del sistema en lazo cerrado.
Un controlador PID se define matemáticamente como:
Donde e(t) es el error de la señal, u(t) es la entrada de control del proceso, es la ganancia
proporcional, es la constante de tiempo integral y es la constante de tiempo derivativa.
- El primer sumando aplica una señal proporcional al error.

- El segundo evalúa la evolución del error y se vuelve más significativo cuando el error es
pequeño pero constante.
- El tercer término considera la tendencia en el error y se nota más cuando el error se
produce por instantes.
En el dominio s, el controlador PID se puede escribir como:
El esquema habitual de uso del controlador PID es:
) Planta
Figura 4. Control PID de una planta
Reglas de Ziegler Nichols
El proceso de seleccionar los parámetros del controlador que cumplan con las especificaciones
de comportamiento dadas se conoce sintonía del controlador.
Ziegler y Nichols sugirieron reglas para sintonizar los controladores PID (es decir, dar valores
para , y ) basándose en las respuestas escalón experimentales (1er método) o en el valor
de (2do método) que produce estabilidad marginal cuando solo se usa la acción de control
proporcional.

Gonzalo Berardo
Hay que tener presente que las reglas de sintonía de Ziegler y Nichols dan una estimación
razonable de los parámetros del controlador proporcionando un punto de partida para la
sintonización fina.
En este proyecto se abordara el caso de que se desconoce el modelo matemático de la planta,

por lo que se recurrirá a la 1er método de Ziegler y Nichols para la sintonía del controlador PID.
1er Método de Sintonización:
En el primer método, la respuesta de la planta a una entrada escalón unitario se obtiene de

manera experimental, tal como se muestra en la figura 5. Si la planta no contiene integradores
ni polos dominantes complejos conjugados, la curva de respuesta escalón unitario puede tener
forma de S, como se observa en la figura 6.
La curva con forma de S se caracteriza por dos parámetros: el tiempo de retardo L y la

constante de tiempo T.
1
Planta
u(t c(t)
)
Figura 5. Respuesta a un escalón unitario de una planta
El tiempo de retardo y la constante de tiempo se determinan dibujando una recta tangente en

el punto de inflexión de la curva con forma de S y determinando las intersecciones de esta
tangente con el eje de tiempo y con la línea C(t)=K, tal como se muestra en la figura 6.
Los parámetros se pueden

Recta tangente al punto de inflexión obtener de esta respuesta:
C(t)=K
Figura 6. Respuesta de salida a una entrada escalón

Gonzalo Berardo
En este caso, la función de transferencia se aproxima mediante un sistema

de primer orden con un retardo del modo siguiente:
Ziegler y Nichols sugirieron establecer los valores de de acuerdo con la formula que
aparece en la tabla 1.
Tipo de Controlador
P 0
PI 0
PID
Tabla 1. Regla de sintonía de Ziegler Nichols basada en la respuesta escalón de la planta (primer método).
Por lo tanto:
El controlador PID sintonizado mediante el primer método de las reglas de Ziegler y Nichols
presentara un polo en el origen y un cero doble en .
Desarrollo Práctico:
Aplicamos una señal escalón de 5V de amplitud a una de las articulaciones del manipulador,
más precisamente a la articulación del hombro.
Por medio de una placa de adquisición de datos, obtenemos la señal de respuesta al escalón.
Esta señal es proveída por el cursor móvil del potenciómetro a medida que la articulación se
desplaza rotacionalmente.

Gonzalo Berardo
Con la ayuda de la herramienta Matlab obtenemos la ubicación del puno de inflexión de la

curva ‘S’ y graficamos una recta tangente a este, como se observa en la siguiente grafica.
Figura 7. Respuesta al escalón
Figura 8. Respuesta al escalón (LA) y Recta tangente al punto de inflexión.
Las intersecciones de la recta tangente con el eje de tiempo y con la línea C(t)=5V obtenemos el
tiempo de retardo L y la constante de tiempo T.
Se adjunta el código utilizado en Matlab para graficar la recta tangente sobre la respuesta al
escalón (en la) y los correspondientes cálculos de L y de T.

Gonzalo Berardo
% SITONIZACION DE UN PID BASADO EN LA REGLA DE ZIEGLER NICHOLS

%-------------------------------------------------------------
H; % Planta
pp=pole(H); % Polos de lazo abierto
dt=0.05;
t=0:dt:8;
y=step(H,t)'; % Respuesta al escalón
dy=diff(y)/dt; % Derivada
[m,p]=max(dy); % Punto de inflexión
d2y=diff(dy)/dt; % Segunda derivada
yi=y(p);
ti=t(p);
L=ti-yi/m; % Retardo
Tau=(y(end)-yi)/m+ti-L; % Constante de tiempo
figure(1)
plot(t,y,'b',[0 L L+Tau t(end)],[0 0 y(end) y(end)],'k')
title('Respuesta al escalón')
ylabel('Amplitud')
xlabel('tiempo (s)')
legend('Exacta','Aproximacion Lineal')
Figura 9. Respuesta al escalón
El algoritmo nos arroja los resultados de:
; ;
De igual modo, el código en Matlab para el cálculo de , , conforme a la tabla 1.
% Control PID con primer metodo de ZIEGLER NICHOLS

Kp=1.2*Tau/L;
Ti=2*L;
Td=0.5*L;
Ki=Kp/Ti;
Kd=Kp*Td;
G=tf([Kd Kp Ki],[1 0]) % Controlador
% Lazo cerrado
HLC=feedback(G*H,1)
figure(2)
step(HLC)
figure(3)
pzmap(HLC)
grid
Figura 10. Calculo de parámetros y Función de Transferencia del controlador PID en LA y LC

Gonzalo Berardo
Resultados:
; ;
Cuya función de transferencia será:
Función de
Transferencia en
lazo abierto
El controlador PID sintonizado mediante el primer método de las reglas de Ziegler y Nichols
presentara un polo en el origen y un cero doble en .
Al final de código de la figura 10, se representa la función de transferencia en lazo cerrado y se

grafica la respuesta escalón y la constelación de polos y ceros.
Función de
Transferencia en
lazo cerrado
Figura 11. Constelación de polos y ceros
Polos en LC:
-3.7528 + 4.9994i ; -3.7528 - 4.9994i ; -2.6280

Gonzalo Berardo
Figura 12. Respuesta al escalón del PID
De la grafica 12, podemos observar que existe un sobre paso del 24% y un tiempo de
establecimiento de 1.16 seg.
Como se menciono anteriormente la sintonización de Ziegler Nichols es un procedimiento de
punto de partida al momento de sintonizar un PID. Ahora en más, solo basta con realizar un
ajuste fino para disminuir el tiempo de establecimiento, el sobre paso, y otros aspectos de
interés.
Sintonización Fina
A continuación se grafica (figura 13) la variación de la respuesta al escalón del PID (en LC) por
medio de un refinamiento de los parámetros calculados.
Código:
% Refinamiento Opción 1
Kp1=1.5*Kp;
Kd1=3*Kd;
Ki1=1.0*Ki;
G1=tf([Kd1 Kp1 Ki1],[1 0]);
HLC1=feedback(G1*H,1);
% Refinamiento Opción 2
Kp2=1.5*Kp;
Kd2=5*Kd;
Ki2=1.0*Ki;
G2=tf([Kd2 Kp2 Ki2],[1 0]);
HLC3=feedback(G2*H,1);
figure(4)
step(HLC,HLC1,HLC3)

Gonzalo Berardo
legend('PID Z-N','PID Refinado Opción 1','PID Refinado Opcion 2')
Figura 13. Refinamiento del PID

OBSERVACION ES:
Cuando en el sistema se emplea un controlador proporcional, la señal a la salida del controlador es múltiplo
P
R
(ganancia KP) del porcentaje de cambio en la señal medida.
O
P
Existe un valor límite de la ganancia del controlador proporcional. Más allá de este límite el sistema alcanza valores
O superiores a los deseados, fenómeno conocido como sobreoscilación. Por razones de seguridad, la sobreoscilación
R
C no debe superar el 30%.
I
O El controlador proporcional no considera el tiempo. Por esta razón, para solucionar el error permanente y lograr
N
A que el sistema contenga alguna componente que considere la variación respecto al tiempo, se incluyen las acciones
L integral y derivativa.
El modo de control integral tiene como propósito disminuir y eliminar el error en estado estacionario, provocado
por el modo proporcional.
I
N
El control integral actúa cuando hay una desviación entre la variable y el punto de consigna, integrando esta
T desviación en el tiempo y sumándola a la acción proporcional.
E
G El error es integrado, es decir es promediado o sumado por un período determinado, para posteriormente ser
R
A multiplicado por una constante KI.
L
Cuando la respuesta integral es adicionada al modo proporcional para formar el control P + I, se obtiene una
respuesta estable del sistema sin error estacionario.
La acción derivativa se manifiesta cuando hay un cambio en el valor absoluto del error.
La función de la acción derivativa es mantener el error al mínimo corrigiéndolo proporcionalmente con la misma
D
E velocidad que se produce; de esta manera evita que el error se incremente, y reducirá el sobrepico.
R
I El error se deriva con respecto al tiempo y se multiplica por una constante KD.
V
A
El control derivativo se caracteriza por el tiempo de acción derivada. Cuando el tiempo de acción derivada es
T
I
grande, hay inestabilidad en el proceso. Cuando el tiempo de acción derivada es pequeño, la variable oscila
V demasiado con relación al punto de consigna. El tiempo óptimo de acción derivativa es el que retorna la variable al
O
punto de consigna con las mínimas oscilaciones
Suele ser poco utilizada debido a la sensibilidad al ruido que manifiesta y a las complicaciones que ello conlleva.
Gonzalo Berardo
Diseño del controlador PID Discreto
El PID será programado en un microcontrolador PIC, por lo que la realización del mismo en
tiempo discreto estará dado por la transformada z:
Siendo:
Donde:
para T=300
Existen distintas posibilidades de la realización práctica de un controlador PID. Este proyecto

utiliza un diseño paralelo debido a facilidad que presenta a la hora de programar el algoritmo.
+ integral
Ki
+
T
proporcional +
e(kT) Kp u(kT)
+
derivativa +
Kd
-
T
Fi gura 14. Diseño pa ralelo del controlador PID
El muestreo debe ser mucho menor que el tiempo de establecimiento del sistema en lazo
abierto. En el modelo de Ziegler Nichols se toma un valor (también puede utilizarse

Gonzalo Berardo
Implementación del controlador
Posición a alcanzar (Set Point) Por medio de la cinemática del

rT=4.5; sistema obtenemos la señal de Cálculo del término
referencia rT integral
iT=Ki*eT+iT0;
Sintonización
Kp=900; // Constante Proporcional
Ki=0; // Constante Integral
Kd=400; // Constante Derivativa Cálculo del término
T=300; // Tiempo de Muestreo derivativo
pwm_min=0.0; // Valor PWM Mínimo dT=Kd*(eT-eT0);
pwm_max=950.0; // Valor PWM Máximo
pote_min=0; // Valor POTE Mínimo

pote_max=4.9; // Valor POTE Máximo
Cálculo de la salida PID

uT=iT+Kp*eT+dT;
REFINAMIENTO
Kp=1.5*Kp;
Ki=1.0*Ki;
Kd=3.0*Kd;
Salida PID > MAX NO Salida PID < MAX NO

if (uT>pwm_max) if(uT<pwm_min
¿Llave en ON? NO Mostramos por display

rT … yT
if(input(pin_B2)==1)
eT … STOP Salida PID = MAX Salida PID = MAX
uT=pwm_max; uT=pwm_max;
Bloquea motor
ADC output_low(pin_B4);
valor=read_adc(); output_low(pin_B5);
Transferencia de salida
PID a señal PWM
control=(int16)uT;
Conversión ADC
1023 a 5V valor=read_adc();
yT=(float)valor*5.0/1023.0;
Cargamos PWM
set_pwm1_duty(control);
Conversión
Cálculo error 1023 a 5V
eT=rT-yT; yT=(float)valor*5.0/1023.0;
Guardar variables
iT0=iT;
eT0=eT;
¿Error es negativo? NO
Pone bandera en 0
If(eT<0) sentido=0;
¿Bandera = 1? NO
if(sentido==1)
Calcula modulo
Pone bandera en 1
eT=eT*(-1); Motor giro a derecha Motor giro a izquierda
output_low(pin_B5); output_high(pin_B5);
sentido=1;
output_high(pin_B4); output_low(pin_B4);
¿Error es < Emin? Mostramos por display

if(eT<Emin) rT … yT
eT … control
Reinicio de variables
eT=0; Tiempo de muestreo
iT0=0; delay_ms(T);
eT0=0;
Figura 15. Diagrama en bloque. Código PID ADC PMW en CCS:

Gonzalo Berardo
La señal de referencia proveída por el modelo cinemático (Directo/Inverso) y la señal de salida

proveída por el cursor móvil del potenciómetro (convertida previamente a digital) se restan
para obtener la señal de error. Esta última es amplificada, derivada e integrada.
La señal de salida del PID (señal actuante) es convertida en una señal PWM (de Modulación de
Ancho de Pulso) para comandar la etapa de potencia del motor por medio de un puente H
(Figura 16).
Cada articulación poseerá su propio e individual sistema de control de posición desarrollado en

un microcontrolador. Por su parte El modelo cinemático correrá en un programa en una PC
(Figura 1).
CINEMATICA Ref Error PID DSCRETO Actuante POTENCIA MOTOR

PWM POTE
DIRECTA/INVERSA Kp ; Ki ;Kd ; T Puente H Articulación
Salida Digital
ADC
PC MICROCONTRO LADOR
Figura 16

Gonzalo Berardo
CINEMATICA Y DINAMICA DE UN SISTEMA
NOTA: A continuación se expone parte del material: Caracterización de un Robot –

CENIDET
Términos Básicos
Cinemática
Se pueden plantear dos problemas:
 Conocer la posición del extremo del robot basándose en la posición de las articulaciones
(Problema CinemáticoDirecto)
 Conocer la posición de las articulaciones del robot bas ándose en la posición del extremo
del robot (Problema CinemáticoInverso)
Dinámica
De igual manera, se pueden plantear dos problemas:
 Dinámico directo: Calculo del movimiento del extremo del robot a partir de los pares y
fuerzas de los accionadores de las articulaciones.
 Dinámico inverso: Calculo de los valores de los pares y fuerzas de los accionadores de las
articulaciones a partir de las posiciones, velocidades y aceleraciones requeridas para el
extremo del robot.
Dinámica Inversa
El modelo dinámico inverso expresa las fuerzas y pares que intervienen, en función de la
evolución temporal de las coordenadas articulares y sus derivadas. Resolver las ecuaciones
dinámicas de movimiento para un robot no es una tarea sencilla debido a la cantidad de
números de grados de libertad y sistemas no lineales que lo componen; para poder definir
dichas ecuaciones, se empleó la formulación de movimiento de Lagrange.
Consideraciones
Para el desarrollo del modelo se omitieron los dos últimos grados de libertad, ya que
pertenecen a la orientación del efector final y sus efectos no son significativos para el
comportamiento dinámico del robot. En general se consideró al manipulador como un sistema
rígido, concentrando las masas en el centro de cada eslabón tal y como lo muestra la figura 2;
además fueron considerados los efectos de fricción provocados por las transmisiones
mecánicas que en este caso se da por engranajes (esta consideración es importante ya que
estos efectos pueden alcanzar un orden de un 25% del par requerido para mover al
manipulador en situaciones típicas [1]). Esto hace que el sistema sea no conservativo, sin
embargo, el sistema puede ser resuelto por medio de la formulación de Lagrange y
finalmente aplicando la función de disipación de Rayleigh se permite adicionar los efectos de
fricción al modelo dinámico.
En el análisis de cada eslabón, los efectos de las Energías Cinética y Potencial de los eslabones
anteriores (si existen) repercuten en el análisis del eslabón en cuestión. Las pérdidas debidas a

Gonzalo Berardo
los huelgos y excentricidades en los engranajes, así como la eficiencia del motor fueron
despreciadas dentro del análisis de la ecuación de movimiento de Lagrange.
Definición de Parámetros
La figura 12 muestra las condiciones dinámicas del manipulador; donde cada variable está
definida de la siguiente manera:
W = Carga aplicada
mi = Masa del i-ésimo eslabón
g = Constante de gravedad
l = Radio de giro para la masa de la base
a = Longitud del primer eslabón
b = Longitud del segundo eslabón
Ii = Inercia i-ésima del eslabón
Vi = Velocidad lineal del i-ésimo eslabón
Ki = Energía cinética del i-ésimo eslabón
Pi = Energía potencial del i-ésimo eslabón
FIGURA 12. DINAMICA INVERSA
Modelo Dinámico
A continuación se muestra el desarrollo del modelo dinámico inverso [1] [2] [3] [4] [5].

Gonzalo Berardo
La energía potencial es cero debido a que un posible cambio en el ángulo θ1 no altera la altura
de la masa mb sobre la vertical.
Derivando y elevando al cuadrado las ecuaciones (9) y (10) para sustituir en (8) y aplicando
identidades trigonométricas tenemos:

Gonzalo Berardo
Derivando y elevando al cuadrado las ecuaciones (18) y (19 ) para sustituir en (17 ) y aplicando
identidades trigonométricas tenemos:

Gonzalo Berardo
Ecuación de Lagrange
La ecuación de movimiento de Lagrange se obtiene con la sumatoria de las energías cinéticas y

potenciales en el manipulador [2].
Por lo que el Lagrangiano quedaría como:
Para obtener el par aplicado a partir de la ecuación de movimiento de Lagrange, se emplea:
Fricción
Se sabe que la formulación de movimiento de Lagrange sólo hace referencia a sistemas

conservativos, sin embargo, a través de la función de disipación de Rayleigh se pueden incluir
pérdidas como la fricción en un sistema conservativo. Un modelo en el que se incluyen estos
efectos de fricción quedaría de la siguiente manera:

Gonzalo Berardo
Expresando la ecuación 28 en forma matricial se tiene:

Gonzalo Berardo

Gonzalo Berardo
Tomando en cuenta los cofactores nulos finalmente se puede rescribir la ecuación 29

finalmente como:
Por lo que la ecuación 40 representa el modelo dinámico inverso del robot manipulador
articulado.

Gonzalo Berardo
Dinámica Directa
El modelo dinámico directo expresa la evolución temporal de las coordenadas articulares y sus
derivadas, en función de las fuerzas y pares que interviene.
Para la obtención del modelo dinámico directo; se emplea el modelo dinámico inverso,
partiendo de la ecuación 40; y aplicando la regla de Cramer para resolver las siguientes
ecuaciones.
Utilizando un cambio de variables para comodidad en el manejo de términos se emplea:
Sustituyendo los pares en las ecuaciones 47, 48 y 49, sufren la siguiente modificación:
Resultando:

Gonzalo Berardo
Por lo que las ecuaciones 53, 54 y 55 representan el modelo dinámico directo del robot
manipulador articulado.
Simulación en Matlab
Cinemática Directa:
%La cinemática directa obtiene como resultado la posición en que se

encuentra el efector
%final en coordenadas cartesianas (xyz), por lo que es necesario introducir
como valores
%iniciales los ángulos que tiene cada elemento del robot de acuerdo a su
respectiva referencia.
clear,clf,clc%Limpieza de las variables en memoria, de gráficas y de

pantalla
hold on %Activa gráficos
%PRESENTACIÓN
fprintf ('\n SIMULACIÓN DE LA CINEMÁTICA DIRECTA\n');
fprintf ('-----------------------------------------------------------------
\n\n\n');
%Se piden valores de los ángulos TETAS
%Se consideran los límites REALES del SCORBOT-ER V plus
teta1 = input ('Valor del ángulo de la cintura en grados: ');
while (teta1<0 || teta1>310)
fprintf('\n Valor incorrecto\n');
teta1 = input ('Valor del ángulo de la cintura en grados: ');
end
teta2 = input ('Valor del ángulo del hombro en grados: ');
while (teta2<-35 || teta2>130)
teta2 = input ('Valor del ángulo del hombro en grados: ');
end
teta3 = input ('Valor del ángulo del codo en grados: ');
teta3 = input ('Valor del ángulo del codo en grados: ');
end
teta4 = input ('Valor del ángulo de elevación (pitch) en grados: ');
teta4 = input ('Valor del ángulo de elevación (pitch ) en grados: ');
end
teta5 = input ('Valor del ángulo de giro (roll) en grados: ');
end
%Se dan las longitudes de los eslabones
%pidiéndoselas al usuario
link1 = input ('\n Longitud del primer eslabón en cm: ');
link2 = input (' Longitud del segundo eslabón en cm: ');
link3 = input (' Longitud de la garra (gripper) en cm: ');

Gonzalo Berardo
%Límites de los ejes
eje = (link1+link2+link3);
axis([-eje,eje,-eje,eje,-eje,eje]);
title ('SIMULACIÓN DE LA CINEMÁTICA DIRECTA');
xlabel('X (cm)'); ylabel('Y (cm)'); zlabel('Z (cm)');
grid;%Activa cuadrícula
%Conversiones de grados a radianes
rad1=(teta1*pi)/180;
%Dibujo de la base
sphere (20)
%Cálculo y graficación del link1
%Cintura-Hombro
x1=0;
y1=0;
z1=0;
x2=cos(rad1)*cos(rad2)*link1;
y2=sin(rad1)*cos(rad2)*link1;
z2=sin(rad2)*link1;
%azul
line ([x1,x2],[y1,y2],[z1,z2],'LineWidth',8,'Color',[0 0 1])
%fprintf('\n Los valores de las coordenadas finales del primer eslabón
son:\n');
%fprintf('\tx2= %12.5f\n',x2);
%fprintf('\ty2= %12.5f\n',y2);
%fprintf('\tz2= %12.5f\n',z2);
%Codo-muñeca
x3=x2;
y3=y2;
z3=z2;
x4= (((cos(rad1)*cos(rad2)*cos(rad3))-
(cos(rad1)*sin(rad2)*sin(rad3)))*link2)+(cos(rad1)*cos(rad2)*link1);
y4= (((sin(rad1)*cos(rad2)*cos(rad3))-
(sin(rad1)*sin(rad2)*sin(rad3)))*link2)+(sin(rad1)*cos(rad2)*link1);
z4=(((sin(rad2)*cos(rad3))+(cos(rad2)*sin(rad3)))*link2)+(sin(rad2)*link1);
line ([x3,x4],[y3,y4],[z3,z4],'LineWidth',8,'Color',[1 0 0])
fprintf('\n\n Los valores de las coordenadas finales del segundo eslabón
son:\n\n');
fprintf('\tX = %12.5f\n',x4); %x4
fprintf('\tY = %12.5f\n',y4); %y4
fprintf('\tZ = %12.5f\n',z4); %z4
%Graficación del gripper
%Vector N
fen=2;%Factor de escalamiento
nxa= cos(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*cos(rad5);
nxb= -cos(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*cos(rad5);
nxc= -cos(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*cos(rad5);
nxd= -cos(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*cos(rad5);
nxe= -sin(rad1)*sin(rad5);
nx= (nxa+nxb+nxc+nxd+nxe)*fen;
nya= sin(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*cos(rad5);
nyb= -sin(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*cos(rad5);
nyc= -sin(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*cos(rad5);
nyd= -sin(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*cos(rad5);
nye= -cos(rad1)*sin(rad5);
ny= (nya+nyb+nyc+nyd+nye)*fen;
nza= -sin(rad2)*cos(rad3)*cos(rad4)*cos(rad5);
nzb= -cos(rad2)*sin(rad3)*cos(rad4)*cos(rad5);
nzc= sin(rad2)*sin(rad3)*sin(rad4)*cos(rad5);
Gonzalo Berardo
nzd= -cos(rad2)*cos(rad3)*sin(rad4)*cos(rad5);
nz= (nza+nzb+nzc+nzd)*-fen;
%negra
line ([x4,x4+nx],[y4,y4+ny],[z4,z4+nz],'LineWidth',2.5,'Color',[0 0 0])
%Vector S
fes=1.5;%Factor de escalamiento
sxa= -cos(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*sin(rad5);
sxb= cos(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*sin(rad5);
sxc= cos(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*sin(rad5);
sxd= cos(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*sin(rad5);
sxe= -sin(rad1)*cos(rad5);
sx= (sxa+sxb+sxc+sxd+sxe)*fes;
sya= -sin(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*sin(rad5);
syb= sin(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*sin(rad5);
syc= sin(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*sin(rad5);
syd= sin(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*sin(rad5);
sye= cos(rad1)*cos(rad5);
sy= (sya+syb+syc+syd+sye)*fes;
sza= sin(rad2)*cos(rad3)*cos(rad4)*sin(rad5);
szb= cos(rad2)*sin(rad3)*cos(rad4)*sin(rad5);
szc= -sin(rad2)*sin(rad3)*sin(rad4)*sin(rad5);
szd= cos(rad2)*cos(rad3)*sin(rad4)*sin(rad5);
sz= (sza+szb+szc+szd)*-fes;
%verde
line ([x4,x4-sx*2],[y4,y4-sy*2],[z4,z4-sz*2],'LineWidth',2.5,'Color',[0 1
0])
%gris
line ([x4,x4+sx],[y4,y4+sy],[z4,z4+sz],'LineWidth',6,'Color',[0.5 0.5 0.5])
%gris
line ([x4,x4-sx],[y4,y4-sy],[z4,z4-sz],'LineWidth',6,'Color',[0.5 0.5 0.5])
%Vector A
fea=3.5;%Factor de escalamiento
axa= cos(rad1)*cos(rad2)*cos(rad3)*sin(rad4);
axb= -cos(rad1)*sin(rad2)*sin(rad3)*sin(rad4);
axc= cos(rad1)*cos(rad2)*sin(rad3)*cos(rad4);
axd= cos(rad1)*sin(rad2)*cos(rad3)*cos(rad4);
ax= (axa+axb+axc+axd)*fea;
aya= sin(rad1)*cos(rad2)*cos(rad3)*sin(rad4);
ayb= -sin(rad1)*sin(rad2)*sin(rad3)*sin(rad4);
ayc= sin(rad1)*cos(rad2)*sin(rad3)*cos(rad4);
ayd= sin(rad1)*sin(rad2)*cos(rad3)*cos(rad4);
ay= (aya+ayb+ayc+ayd)*fea;
aza= -sin(rad2)*cos(rad3)*sin(rad4);
azb= -cos(rad2)*sin(rad3)*sin(rad4);
azc= -sin(rad2)*sin(rad3)*cos(rad4);
azd= cos(rad2)*cos(rad3)*cos(rad4);
az= (aza+azb+azc+azd)*-fea;
%amarrilo
line([x4,x4+ax*1.2],[y4,y4+ay*1.2],[z4,z4+az*1.2],'LineWidth',2.5,'Color',[
1 1 0])

Gonzalo Berardo
%tomando de origen el vector S verde

%línea gris (Dedo 1)
line ([x4-sx,x4+ax-sx],[y4-sy,y4+ay-sy],[z4-
sz,z4+az+sz],'LineWidth',6,'Color',[0.5 0.5 0.5])
line([x4+sx,x4+ax+sx],[y4+sy,y4+ay+sy],[z4+sz,z4+az+sz],'LineWidth',6,'Colo
r',[0.5 0.5 0.5])
hold off %Desactiva gráficos
Resultado:
SIMULACIÓN DE LA CINEMÁTICA INVERSA

Gonzalo Berardo
%La cinemática inversa obtiene como resultado los ángulos necesarios para
llegar a una
%posición deseada en donde se encuentra el efector final, por lo que es
necesario
%introducir como valores iniciales las coordenadas cartesianas (xyz).
clear,clf,clc %Limpieza de las variables en memoria, de gráficas y de

pantalla
hold on %Activa gráficos
%PRESENTACIÓN
fprintf ('\n SIMULACIÓN DE LA CINEMÁTICA INVERSA\n');
fprintf ('----------------------------------------------------------------
-\n\n\n');
link1 = input ('Longitud del primer eslabón en cm: ');
link2 = input ('Longitud del segundo eslabón en cm: ');
link3 = input ('Longitud de la garra (gripper) en cm: ');
%Se piden las coordenadas del efector final
x = input ('\nValor de la coordenada en X: ');
y = input ('Valor de la coordenada en Y: ');
z = input ('Valor de la coordenada en Z: ');
teta4 = input ('Valor del ángulo de elevación (pitch)en grados: ');
teta4 = input ('Valor del ángulo de elevación (pitch)en grados: ');
end
end
%Tipo de configuración
ca = input ('\nLa simulación la desea con configuración Codo Arriba (1) o
Codo Abajo (0): ');
%Condición para asegurar el alcance de la posición deseada
R=sqrt(x^2+y^2+z^2);
L=link1+link2+link3;
while (R>L)
fprintf('\n Posición fuera de alcance!!!\n');
fprintf('\n Modifique algún parámetro\n\n');
link1 = input ('Longitud del primer eslabón en cm: ');
link2 = input ('Longitud del segundo eslabón en cm: ');
link3 = input ('Longitud de la garra (gripper) en cm: ');
%Se piden las coordenadas del efector final
x = input ('Valor de la coordenada en X: ');
y = input ('Valor de la coordenada en Y: ');
z = input ('Valor de la coordenada en Z: ');
ca = input ('\nLa solución la desea con configuración Codo Arriba(1) o
Codo Abajo(0): ');
R=sqrt(x^2+y^2+z^2);
L=link1+link2+link3;
end

Gonzalo Berardo
%Límites de los ejes

eje = (link1+link2+link3);
axis([-eje,eje,-eje,eje,-eje,eje]);
xlabel('X (cm)'); ylabel('Y (cm)'); zlabel('Z (cm)');
grid;%Activa cuadrícula
%Dibujo de la base
sphere (20)
if (x==0)
%if y<0
% rxy=y*-1;
%end
rxy=y;
%Cálculo y graficación de link1
%Teta1 de Cintura-Hombro
teta1 = asin(y/rxy);
ar=sqrt(y^2+z^2);
beta=atan(z/y);
else
%Cálculo y graficación de link1
%Teta1 de Cintura-Hombro
teta1 = atan(y/x);
rxy=x/cos(teta1);
ar=sqrt(rxy^2+z^2);
beta=atan(z/rxy);
end
%Por ley de cosenos
alfa=acos((link1^2+ar^2-link2^2)/(2*link1*ar));
%Si es configuración codo arriba se mete al IF
if ca==1
teta2=beta+alfa;
if x<0
teta2=beta+alfa+180;
end
f=link1*sin(teta2);
h=z-f;
mu=asin(h/link2);
teta3=teta2-mu;
g=link1*cos(teta2);
gx=g*cos(teta1);
gy=g*sin(teta1);
%Configuración codo arriba
x1=0;
y1=0;
z1=0;
line ([x1,gx],[y1,gy],[z1,f],'LineWidth',8,'Color',[1 0 1])
%Codo-muñeca
x3=gx;
y3=gy;
z3=f;
title ('SIMULACION DEL SCORBOT-ER V plus en Configuración Codo Arriba');
line ([x3,x],[y3,y],[z3,z],'LineWidth',8,'Color',[0 1 0])
rad1=teta1;
rad2=teta2;
rad3=teta3;
rad4=teta4;
rad5=teta5;
x4=x;
y4=y;
z4=z; LARMA – Laboratorio de Robots Móviles Autónomos
%Vector N
Gonzalo Berardo
rad5=teta5;
x4=x;
y4=y;
z4=z;
%Vector N
%negra
%Vector S
%verde
0])
%gris
line ([x4,x4+sx],[y4,y4+sy],[z4,z4+sz],'LineWidth',6,'Color',[0.5 0.5
0.5])
%gris
line ([x4,x4-sx],[y4,y4-sy],[z4,z4-sz],'LineWidth',6,'Color',[0.5 0.5
0.5])
%Vector A
ax= (axa+axb+axc+axd)*fea; Gonzalo Berardo
%amarillo
line([x4,x4+ax*1.2],[y4,y4+ay*1.2],[z4,z4+az*1.2],'LineWidth',2.5,'Color',
[1 1 0])
line([x4+sx,x4+ax+sx],[y4+sy,y4+ay+sy],[z4+sz,z4+az+sz],'LineWidth',6,'Col
or',[0.5 0.5 0.5])
%Conversiones de radianes a grados
tetac1=(teta1*180)/pi;
tetah1=(teta2*180)/pi;
tetaco1=(teta3*180)/pi;
%Cuando los ángulos son mayores a 360
while tetac1>360
tetac1=tetac1-360;
end
while tetah1>360
tetah1=tetah1-360;
end
while tetaco1>360
tetaco1=tetaco1-360;
end
%Cuando los ángulos son mayores a -360
while tetac1<-360
tetac1=tetac1+360;
end
while tetah1<-360
tetah1=tetah1+360;
end
while tetaco1<-360
tetaco1=tetaco1+360;
end
fprintf('\n Cuando los ángulos calculados se encuentran en el segundo ');
fprintf('\n cuadrante, el ángulo mostrado es el complementario\n');
fprintf('\n Los valores de los ángulos en codo arriba son:\n');
fprintf('\tCintura =%12.2f\n',tetac1);
fprintf('\tHombro =%12.2f\n',tetah1);
fprintf('\tCodo =%12.2f\n',tetaco1);
fprintf('\n Recuerde que los límites físicos (en grados) en el robot
son:');
fprintf('\n\t Cintura de 0 a 310');
fprintf('\n\t Hombro de -35 a 130'); LARMA – Laboratorio de Robots Móviles Autónomos
fprintf('\n\t Codo de -130 a 130'); Gonzalo Berardo
fprintf('\n\t Hombro de -35 a 130');

fprintf('\n\t Codo de -130 a 130');
break%Finaliza el programa
end
teta2=beta-alfa;
if x<0
teta2=beta-alfa+180;
end
a=link1*sin(teta2);
b=link1*cos(teta2);
c=rxy-b;
d=z-a;
w=atan(d/c);
teta3=w-teta2;
bx=b*cos(teta1);
by=b*sin(teta1);
%Configuración codo abajo
x1=0;
y1=0;
z1=0;
title ('SIMULACIÓN en Configuración Codo Abajo');
line ([x1,bx],[y1,by],[z1,a],'LineWidth',8,'Color',[0 0 1])
%Codo-muñeca
x3=bx;
y3=by;
z3=a;
line ([x3,x],[y3,y],[z3,z],'LineWidth',8,'Color',[1 0 0])
rad1=teta1;
rad2=teta2;
rad3=teta3;
rad4=teta4;
rad5=teta5;
x4=x;
y4=y;
z4=z;
%Vector N
%negra
%Vector S
sxb= cos(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*sin(rad5); Gonzalo Berardo
%negra
%Vector S
%verde
0])
%gris
line ([x4,x4+sx],[y4,y4+sy],[z4,z4+sz],'LineWidth',6,'Color',[0.5 0.5
0.5])
%gris
line ([x4,x4-sx],[y4,y4-sy],[z4,z4-sz],'LineWidth',6,'Color',[0.5 0.5
0.5])
%Vector A
%amarillo
line([x4,x4+ax*1.2],[y4,y4+ay*1.2],[z4,z4+az*1.2],'LineWidth',2.5,'Color',
[1 1 0])
line([x4+sx,x4+ax+sx],[y4+sy,y4+ay+sy],[z4+sz,z4+az+sz],'LineWidth',6,'Col
or',[0.5 0.5 0.5])
tetac=(teta1*180)/pi;
tetah=(teta2*180)/pi; LARMA – Laboratorio de Robots Móviles Autónomos
tetaco=(teta3*180)/pi; Gonzalo Berardo
,[0.5 0.5 0.5])

tetac=(teta1*180)/pi;
tetah=(teta2*180)/pi;
tetaco=(teta3*180)/pi;
%Cuando los ángulos son mayores a 360
while tetac>360
tetac=tetac-360;
end
while tetah>360
tetah=tetah-360;
end
while tetaco>360
tetaco=tetaco-360;
end
%Cuando los ángulos son mayores a -360
while tetac<-360
tetac=tetac+360;
end
while tetah<-360
tetah=tetah+360;
end
while tetaco<-360
tetaco=tetaco+360;
end
fprintf('\n Cuando los ángulos calculados se encuentran en el segundo ');
fprintf('\n cuadrante, el ángulo mostrado es el complementario\n');
fprintf('\n Los valores de los ángulos en codo abajo son:\n');
fprintf('\tCintura =%12.2f\n',tetac);
fprintf('\tHombro =%12.2f\n',tetah);
fprintf('\tCodo =%12.2f\n',tetaco);
fprintf('\n Recuerde que los límites físicos (en grados) en el robot
son:');
fprintf('\n\t Cintura de 0 a 310');
fprintf('\n\t Hombro de -35 a 130');
fprintf('\n\t Codo de -130 a 130');

Gonzalo Berardo
Resultado:
SIMULACIÓN DE LA CINEMÁTICA INVERSA

Gonzalo Berardo
DOCUMENTOS DE REFERENCIA
- http://www.automatas.org/hardware/teoria_pid.htm
- Caracterización de un Robot – CENIDET
- Introducción a la Robótica - Humberto A. Secchi - Instituto de Automática - Universidad
Nacional de San Juan
- Ingenieria de Control Moderna - K. Ogata
- Sistemas de control moderno 10a Ed. - Dorf y Bishop

Gonzalo Berardo

Trabajo Final Robot Manipulador Mentor Educacional PDF

Загружено:

Сведения о документе

Исходное описание:

Оригинальное название

Авторское право

Доступные форматы

Поделиться этим документом

Поделиться или встроить документ

Параметры публикации

Этот документ был вам полезен?

Это неприемлемый материал?

Авторское право:

Доступные форматы

Trabajo Final Robot Manipulador Mentor Educacional PDF

Загружено:

Авторское право:

Доступные форматы

UNIVERSIDAD TECNOLOGICA NACIONAL

Se adjunta CD-Rom con simulación en Matlab y Proteus.

ALUMNO: GONZALO BERARDO - LEGAJO: 26499 – Mail: Gonzalo_b@msn.com

Desarrollado por Gonzalo Berardo en dependencia al grupo de investigación LARMA (Laboratorio

DETALLE GENERAL DEL BRAZO ROBOTICO Pág. 6

SISTEMA DE CONTROL Pág. 7

Controlador PID Pág. 9

Diseño del controlador PID Discreto Pág. 18

CINEMATICA Y DINAMICA DE UN SISTEMA Pág. 21

Simulación en Matlab Pág. 30

DOCUMENTOS DE REFERENCIA Pág. 42

LARMA – Laboratorio de Robots Móviles Autónomos

El presente proyecto se enfoca al control de un robot manipulador por medio de una

El proyecto se divide en Modelado Matemático y Sistema de Control de Posición.

Se pueden plantear dos problemas:

Los modelos dinámicos directo e inverso están fundamentados en la formulación de

De igual manera, se pueden plantear dos problemas:

 SISTEMA DE CONTROL DE POSICION

Partiendo del desconocimiento del modelo matemático de la planta a controlar, se

A continuación se adjunta el diseño de diagrama en bloque general del proyecto.

LARMA – Laboratorio de Robots Móviles Autónomos

CINEMATICA Ref 3 MOTOR

Figura 1. Diagrama en bloque general.

LARMA – Laboratorio de Robots Móviles Autónomos

Se requiere diseñar y construir un controlador PID para regular la posición de un motor de

Figura 2. Diagrama de bloques del sistema controlado

- La señal de salida, y, corresponde a la salida del terminal móvil del potenciómetro.

Partiendo del deconocimiento del modelo matematico de la planta, se sintonizara los

LARMA – Laboratorio de Robots Móviles Autónomos

DETALLE GENERAL DEL BRAZO ROBOTICO

Su capacidad máxima de carga es de un kilogramo.

- Tipo: Robot Antropomórfico o Articulado de 5 GDL.

- Tipo: Pinza Mecánica.

- Sensores Internos: Encoders Analógicos. Localizados en la propia estructura del robot.

LARMA – Laboratorio de Robots Móviles Autónomos

- Señal de salida: es la variable que se desea controlar (posición, velocidad, presión,

- Señal de referencia: es el valor que se desea que alcance la señal de salida.

- Error: es la diferencia entre la señal de referencia y la señal de salida real.

- Señal de control: es la señal que produce el controlador para modificar la variable

- Señal análoga: es una señal continua en el tiempo.

- Señal digital: es una señal que solo toma valores de 1 y 0.

- Conversor análogo/digital: es un dispositivo que convierte una señal analógica en una

- Proceso: operación que conduce a un resultado determinado.

- Sistema: consiste en un conjunto de elementos que actúan coordinadamente para

- Sistema de control en lazo cerrado: es aquel en el cual continuamente se está

LARMA – Laboratorio de Robots Móviles Autónomos

- Sistema de control en lazo abierto: en estos sistemas de control la señal de salida no es

Planteamiento del problema

La señal de referencia, r, corresponde a la posición deseada. Es decir, si queremos que el motor

La señal de error, e, corresponde a la diferencia entre la señal de referencia y la señal de salida.

La señal de control, u, corresponde al voltaje producido por el controlador para disminuir o

Figura 2. Diagrama de bloques del sistema controlado

LARMA – Laboratorio de Robots Móviles Autónomos

ACOPLE MOTOR POTENCIOMETRO

Figura 3. Motor - Potenciómetro.

LARMA – Laboratorio de Robots Móviles Autónomos

Si se pude obtener un modelo matemático de la planta, es posible aplicar diversas técnicas de

Un controlador PID se define matemáticamente como:

- El primer sumando aplica una señal proporcional al error.

En el dominio s, el controlador PID se puede escribir como:

El esquema habitual de uso del controlador PID es:

Figura 4. Control PID de una planta