You are on page 1of 6

24 Congreso Argentino de Control Automtico

27 al 29 de Octubre de 2014 Buenos Aires, Argentina.



rea Estudiantil






CONTROLADOR DE POSICION BASADO EN FPGA PARA ROBOT
MANIPULADOR



Gonzalo M. Berardo

Asesor: Pablo A. Salvadeo

Laboratorio de Computacin Reconfigurable
Facultad Regional Mendoza
Universidad Tecnolgica Nacional



Resumen: El trabajo presenta el diseo de un controlador implementado en FPGA (Field Programmable Gate
Array), para el control de posicin de un robot manipulador con 5 grados de libertad. Este desarrollo incluye
para cada articulacin la implementacin de dos cores (ncleos), un PID (Proporcional Integral Derivativo) y un
PWM (Pulse Width Modulation), para gobernar a cada motor. La comunicacin entre los ncleos y las etapas
externas se efecta a travs de un core SPI (Serial Peripheral Interface). Finalmente, un core central, acta como
unidad de gestin. En la implementacin en hardware se utiliza un kit de desarrollo de Digilent basado en un
FPGA Spartan-3E de Xilinx junto con su software ISE, mientras que la descripcin se realiza en VHDL (VHSIC
Hardware Description Language). Se presentan los resultados de la sntesis y el procedimiento de sintonizacin.


Palabras claves: Robot, Manipulador, Controlador, FPGA, VHDL.


1. INTRODUCCION


Un manipulador es un robot industrial multifuncional
reprogramable con varios grados de libertad, capaz de
manipular materias, piezas, herramientas o dispositivos
especiales segn trayectorias variables programadas para
realizar tareas diversas
1
.La incorporacin de los manipu-
ladores a los ambientes de trabajo, trae como ventajas la
liberacin del hombre a trabajos peligrosos, desagrada-
bles o montonos y arroja como resultado un aumento de
la productividad, calidad y competitividad de los proce-
sos.

El controlador para el robot manipulador se disea con el
objeto de controlar la posicin angular de las articulacio-
nes. Esto se logra por medio del clculo de algoritmos
PID. Los resultados se modulan en ancho de pulso para
gobernar las velocidades de los motores, de tal modo que
variando coherentemente dicha velocidad, se intenta
controlar la posicin del robot.


_______________________

1
Definicin adoptada por la Organizacin Internacional de
Estndares (ISO), de acuerdo a la Asociacin de Industrias de
Robtica (RIA, Robotic Industry Association).

Partiendo de la necesidad de que cada articulacin debe
iniciar y terminar su movimiento al mismo tiempo, de
manera que este parezca coordinado, se implementa el
controlador en una arquitectura tipo paralela, por medio
de procesos dentro de un FPGA. Este es un dispositivo de
hardware reconfigurable que mediante un lenguaje de
descripcin de hardware como VHDL permite desarrollar
diversos circuitos. Siendo posible gracias a programas
CAD (Computer Aided Design) llamados sintetizadores
(Pardo y Boluda, 1999).

El trabajo se organiza de la siguiente manera. En la Sec-
cin 2, se presenta una breve descripcin sobre el mani-
pulador a controlar, haciendo hincapi en su estructura
mecnica. La Seccin 3, describe al controlador, a travs
de sus cuatro bloques (PID, PWM, SPI y UG) y la im-
plementacin en un FPGA Spartan 3E de Xilinx. Dentro
de las secciones 4 y 5, se presenta la etapa de Adquisicin
de datos y potencia. En la seccin 6 se pone a la vista los
resultados de la implementacin. Y finalmente en la sec-
cin 7 se citan las conclusiones.






24 Congreso Argentino de Control Automtico
27 al 29 de Octubre de 2014 Buenos Aires, Argentina.

rea Estudiantil
2. MANIPULADOR


El manipulador a controlar posee 5 grados de libertad, los
cuales estn denotados por sus similares al ser humano,
estos son: cintura, hombro, codo, mueca-elevacin y
mueca-giro. En la Fig.1 se muestra la vista general del
robot indicando los movimientos relativos a cada grado
de libertad.

Fig. 1: Vista general de manipulador a controlar

El robot presenta una estructura mecnica antropomrfica
o articulada, de configuraciones cinemticas del tipo
RRR; cintura rotacional, hombro rotacional y codo rota-
cional. Posee como accionadores, motores de corriente
continua acoplados a cajas reductoras. Sensores internos
tipo potencimetros resistivos, forman su sistema senso-
rial y como elemento terminal, presenta una pinza mec-
nica, sin herramienta.

Por cada articulacin del manipulador existe un acople
mecnico entre el sistema motor-reductor y potencime-
tro. ste ltimo censa la rotacin mediante una seal
continua de 5v.


3. DISEO DEL CONTROLADOR


La Fig. 2 presenta la etapa del controlador junto a otras
etapas involucradas.


Fig. 2: Etapas involucradas

Para cada articulacin el controlador dispone de dos blo-
ques, PID y PWM. En la Fig. 3 se observa la representa-
cin para tres articulaciones (cintura, hombro, y codo),
dejando sin ilustrar, las correspondientes a mueca-
rotacin y mueca-elevacin.

El controlador recibe como seales de entrada, provenien-
tes de la etapa de adquisicin, las posiciones deseadas
(setpoint) y las posiciones reales de cada articulacin, a
travs del bloque SPI y enva como salida, las seales
PWM hacia la etapa de potencia.

3.1 PID

Un controlador PID es un mecanismo de control genrico
sobre una realimentacin de bucle cerrado, ampliamente
usado en la industria para el control de sistemas. El con-
trolador PID compara el valor real de la salida de una
planta con la entrada de referencia (valor deseado), de-
termina la desviacin y produce una seal de control que
reducir la desviacin a cero o a un valor pequeo (Oga-
ta, 1998).

En la Fig. 3 se representa esquemticamente un lazo de
control con controlador PID.






Fig. 3: Lazo de control PID.
Dnde: r(t) seal de referencia (posicin deseada), y(t)
seal de salida (posicin real), e(t) seal de error y u(t)
seal de control.

La ecuacin diferencial que rige el comportamiento
dinmico entrada-salida de un PID continuo, es:



Donde:

es la ganancia proporcional,

es la constante
de tiempo integral y

es la constante de tiempo deriva-


tiva. Estos tres parmetros interactan uno con el otro y
su ajuste para obtener el mejor control puede ser compli-
cado.

Existen distintas posibilidades a la hora de realizar de
manera prctica un controlador PID. En este trabajo se
utiliza un diseo paralelo debido a facilidad que presenta
a la hora de describir el algoritmo, la Fig. 4 muestra un
diagrama del mismo.










Fig. 4: Diseo tipo paralelo del controlador PID







CONTROLADOR







CINTURA
HOMBRO
CODO
MUECA
ETAPA
ADQUISICION
ETAPA
POTENCIA UG
PID
PID
PID
PWM
PWM
PWM
MOTOR/POTE.

Planta
Motor/Sensor
r
e u
y
PID
SPI


24 Congreso Argentino de Control Automtico
27 al 29 de Octubre de 2014 Buenos Aires, Argentina.

rea Estudiantil
Para un control digital, las seales continuas deben dis-
cretizarse. Si el tiempo de muestreo T es suficientemente
pequeo, se puede hacer una discretizacin directa de la
ecuacin (1) para obtener la ecuacin en diferencias. De
este modo la derivada primera se transforma en diferencia
de primer orden y la integral en sumatoria (Kuchen y
Carelli, 1996). La ecuacin (2) responde a un comporta-
miento discreto aproximado para un PID.



Siendo

ganancia proporcional,

ganancia
integral y

ganancia derivativa.

Para la implementacin se describe el PID discreto (gen-
rico) en VHDL siguiendo un estilo algortmico. En pri-
mera instancia se utilizan datos del tipo entero de 32 bits.

El core realiza en cada flanco ascendente de la seal de
muestreo (T=0.01s) el clculo del error entre la seal
deseada y la real. Si el error es cero o est por debajo de
un valor establecido como mnimo, la seal de control
ser nula. Por el contrario, si el error es distinto de cero,
verifica si este es positivo o negativo y lo almacena en
una variable sentido. Posteriormente, trabaja con el valor
absoluto del error y calcula los tres trminos del PID. El
resultado se guarda en una nueva variable como seal de
control. Para evitar problemas del tipo integral-windup
se limita el resultado entre un valor mximo y mnimo,
para impedir que el integrador actu cuando se superan
esos lmites.

El ncleo como salida entrega la seal de control ms una
seal de sentido de giro de motor. De esta manera, se
consigue implementar de forma sencilla un control PID
para cada articulacin del manipulador.

3.2 Sintonizacin del PID

Para sintonizar el PID se toma como punto de partida que
se desconoce el modelo matemtico de la planta y por
medio de procedimientos prcticos se obtiene la funcin
de transferencia (FT) que la modela. Una vez conocida la
planta, se sintoniza al PID, con la herramienta PID-Tuner
del software Simulink de MatLab.

Para obtener la funcin de transferencia se aplica una
seal escaln al sistema motor-potencimetro (de una
articulacin), se grafica su salida y se halla las ecuaciones
de cada variable en el dominio del tiempo. Luego, se pasa
al dominio de Laplace a fin de obtener la FT que describe
a la planta.

Para generar el escaln y poder graficar la salida se utili-
za el software Simulink con la librera ArduinoIO
2
conec-

_______________________

2
ArduinoIO MATLAB Support Package for Arduino.
tado a una placa ArduinoMega como placa de adquisicin
de datos. Se ilustra en la Fig. 5, el diagrama en bloque
utilizado.


Fig. 5. Bloque Arduino-Matlab.

SetupArduino1, ArduinoDigitalWrite y ArduinoAnalo-
gRead son bloques de la librera ArduinoIO, mientras que
Step y Scope son libreras por defecto de simulink.

SetupArduino1 configura el puerto dnde se encuentra
conectado la placa Arduino (COM3, COM4, etc.), Ardui-
noDigitalWrite establece un pin a un valor lgico dado,
Step genera un escaln entre dos niveles definidos en un
espacio de tiempo especificado, ArduinoAnalogRead Lee
el valor de tensin en el pin analgico referido. Scope
representa grcamente la entrada conectada a este blo-
que con respecto al tiempo de simulacin. En la Fig. 6 se
presenta una fotografa del procedimiento.

De esta manera, cuando el bloque Step genera el escaln,
ArduinoDigitalWrite pone una tensin de 5v en el pin 3
de la placa ArduinoMega. Luego la tensin es elevada a
13v por la placa de potencia (puente H) para comandar al
motor. Al mismo tiempo, la tensin continua del poten-
cimetro es leda por el bloque ArduinoAnalogRead (pin
0 de la placa) y es graficada por Scope.


Fig. 6. Imagen del procedimiento practico.
24 Congreso Argentino de Control Automtico
27 al 29 de Octubre de 2014 Buenos Aires, Argentina.

rea Estudiantil
La seal de entrada corresponde a un escaln de amplitud
igual al voltaje aplicado (

) y cuya transformada de
Laplace es:

;
La seal de salida corresponde a una recta de pendiente
m, y de transformada de Laplace igual a:

;
La FT de la planta es:



En la Fig. 7 se representa el sistema de control realimen-
tado que incluye al bloque TransferFcn con la FT calcu-
lada, y los bloques PID-Cotroller y ZeroOrderHold. El
primeropermite implementar algoritmos de control PID
en tiempo continuo como en tiempo discreto, e incluye
una herramienta (PID-Tuner) para la sintonizacin au-
tomtica de las ganancias del controlador. El segundo
acta como elemento muestreador.


Fig. 7. Lazo de control PID Discreto.
Haciendo doble clic sobre el bloque PID se abre la venta-
na Parameters-PID-Controller (Fig. 8), en donde se
especifica al controlador como tipo PID, de estructura
Paralela, y dominio de tiempo Discreto con muestreo de
T=0.01s.


Fig. 8. Ventana de parmetros de PID-Controller.

La sintonizacin se lleva a cabo haciendo clic en el botn
Tune de la ventana en mencin y se obtiene los valores
de las constantes de ganancia.


Es importante destacar que los resultados de la sintoniza-
cin corresponden a una implementacin en coma flotan-
te. Haciendo clic en Data-Type se puede cambiar el tipo
de datos. La Fig. 9 muestra la respuesta al escaln del
sistema usando representacin en: (a) coma flotante, (b)
coma fija y (c) enteros.


Fig. 9. Respuesta al escaln la implementacin con: (a) ente-
ros, (b) coma fija, (c) coma flotante.

Se observa la funcin escaln junto a tres curvas. Una de
trazo discontinuo (a) que corresponde a la implementa-
cin con enteros. Otra descripta por puntos (b) que perte-
nece a coma fija y por ltimo, de punto y trazo (c) para
coma flotante. Vemos que al utilizar coma fija o coma
flotante el desempeo es muy similar, mientras que la
implementacin con enteros provoca un error mayor en
estado transitorio y aparece un error considerable (12%)
en estado permanente.

3.3 PWM

Una unidad PWM permite asignar cierta duracin de
tiempo en alto o en bajo a un dato digital de n bits que se
considera salida de la etapa procesadora. Lo anterior se
logra conectando un contador y un circuito comparador,
tal y como se aprecia en la Fig. 10.








Fig. 10. PWM.

El comparador determinar si el dato aplicado a la entra-
da de la unidad es igual al valor binario del contador que
cambia constantemente. El tiempo que durar la seal en
alto depende de la cantidad de pulsos de reloj que se
apliquen hasta que el contador presente un dato binario
mayor o igual al de la entrada.


CONTADOR
COMPARADOR
A<B
RESET

CLOCK

Entrada
Digital
PWM
Salida
Modulada
(a)

(b)
(c)
24 Congreso Argentino de Control Automtico
27 al 29 de Octubre de 2014 Buenos Aires, Argentina.

rea Estudiantil
El periodo completo de un ciclo PWM es igual al produc-
to del periodo del reloj de la seal de referencia (reloj del
sistema) con

, donde n es el nmero de bits del conta-


dor propuesto.



La implementacin en hardware del ncleo, se lleva a
cabo siguiendo un estilo algortmico de VHDL. En donde
describe dentro de un nico proceso un contador y un
comparador (ambos de 10 bits), como elementos base de
la unidad PWM. Como entrada digital recibe la seal de
control proveniente de un core PID y es comparada en
cada flaco ascendente de la seal de reloj (

con el valor del contador en ese instante.



De acuerdo a la ec. (6), si el contador es de 10 bits, se
tendr que para un periodo completo de un ciclo PWM
ser de:



Si se aumenta el nmero de bits del contador para mejo-
rar la resolucin, el periodo PWM tender a hacerse ms
grande reduciendo drsticamente la frecuencia de salida.

3.4 SPI

La comunicacin SPI, es muy utilizada para conectar
perifricos como memorias, DSP, conversores Analgi-
cos/Digitales (ADC), entre otros. SPI es un estndar
establecido por Motorola que utiliza un bus de 4 lneas
para interconectar dispositivos perifricos de baja y me-
dia velocidad. La comunicacin se realiza siguiendo la
estructura de un modelo maestro/esclavo donde el maes-
tro selecciona al esclavo y comienza el proceso de trans-
misin/recepcin de informacin. La Fig. 11 muestra un
esquema de conexionado.










Fig. 11. Modelo de conexionado de comunicacin SPI.
SPI constituye un bus full dplex, es decir, que se puede
enviar y recibir informacin de manera simultnea, lo
cual, eleva la tasa de transferencia de los datos.

En cuanto a la implementacin, se reus el core SPI des-
arrollado por Scott Larson
3
, el mismo presenta de carac-
terstica, nmero de esclavos, ancho de palabra y
velocidad de transmisin configurable y polaridad y fase
seleccionable. Los puertos: enable, cpol, cpha, cont,
_______________________
3
Codigo extrado del sitio web www.eewiki.net (SPI 3-Wire
Master VHDL).
clk_div y cddrse se configuran de acuerdo al dispositivo
esclavo a enlazar. La misma se lleva a cabo por medio del
ncleo UG (Unidad de Gestin).

La tarjeta Spartan-3E Starter Kit incluye una etapa anal-
gica de dos canales de captura, formada por un preampli-
ficador con escala programable (LTC6912) y por un
conversor analgico a digital (TLC14071). Ambos se
configuran y se controlan por medio de una comunicacin
SPI. Es decir, estos dos CI constituyen los esclavos de la
comunicacin. Para lo cual, de las hojas de datos, se veri-
fica la polaridad y fase del flanco de reloj SCLK (cpol=0,
cpha=0) y la velocidad de transmisin (

,
siendo:

(7)

Para que se cumpla la ecuacin (7) se configura clk_div
igual a 1 en el core SPI.

3.5 UG

La unidad de gestin, se encarga de otorgar la configura-
cin requerida del core SPI (de acuerdo a tabla 1), y acta
como rbitro entre los ncleos restantes.

Tabla 1. Parmetros de configuracin SPI
Puerto Valor Descripcin
cpol 0 Polaridad del reloj por flanco asc.
cpha 0 Fase de reloj por 1er flanco
cont 1 Modo continuo habilitado
clk_div 1 Velocidad de transmisin
addr 0 Esclavo de destino

Cuando se establece la alimentacin del sistema, la UG
por medio del core de comunicacin configura al pream-
plificador de escala programable (LTC6912) con la ga-
nancia deseada. Posteriormente da inicio el ciclo de
conversin analgica digital.

4. ADQUISICION DE DATOS


La tarjeta Spartan-3E Starter Kit incluye una etapa anal-
gica de dos canales de captura, formada por un preampli-
ficador con escala programable (LTC6912) y por un
conversor de analgico a digital (TLC14071). Ambos
programados y controlados desde la FPGA usando una
interfaz de comunicacin SPI.

A la entrada de cada canal se conecta la salida de un mul-
tiplexor analgico 4 a 1de referencia CD4052, de manera
de muestrear las seis tensiones de inters (correspondien-
te a las articulaciones de cintura, codo, hombro, mueca-
elevacin, mueca-rotacin y pinza), de acuerdo a lo
indicado en la Fig. 12. La seleccin de canal se gobierna
desde el ncleo UG.




SCLK
MOSI
MISO
C_S
SPI ESCLAVO
SCLK
MOSI
MISO
SS1
SS2
SSn
SPI MAESTRO
24 Congreso Argentino de Control Automtico
27 al 29 de Octubre de 2014 Buenos Aires, Argentina.

rea Estudiantil



















Fig. 12. Etapa de adquisicin de datos.

5. POTENCIA


Cuando se desea controlar la velocidad y el sentido de
giro de un motor de corriente continua se suelen utilizar
transistoresen configuracin Puente en H. En la prcti-
ca, existen multitud de circuitos integrados (CI) que im-
plementan el puente H, como el driver L293B. El circuito
elctrico recomendado por el fabricante del CI es el mos-
trado en la Fig. 13, el cual permite un control de giro
bidireccional.

Fig. 13. Circuito elctrico L293B.

Al ser bidireccional, se necesitan dos bits para indicar el
sentido de giro (DC=01
2
para dextrgiro, DC=10
2
para
levgiro, DC=00
2
para detenerlo) y un tercer bit, para
habilitacin (pin 1). Este ltimo se sincroniza con la seal
pwm para el control de velocidad.

La etapa de potencia utilizada cuenta de tres drivers
L293B, conectados de acuerdo al esquema bidireccional.
De este modo, se consigue gobernar los seis motores
existentes en el manipulador.




6. RESULTADOS


Se presentan en la Tabla 2 los resultados de la sntesis
empleando ISE de Xilinx sobre el FPGA Spartan-3E. Se
aprecia que el uso de recursos es despreciable y permite
agregar varios conjuntos PID-PWM para los cinco nodos
(articulaciones) del robot. Lo que adems permite combi-
nar este controlador con otros core para llevar a cabo
tareas ms sofisticadas.

Tabla 2. Resumen de lgica usada
Recursos PID PWM SPI UG
FlipFlops 53 1% 11 1% 103 1% 67 1%
Slices 92 1% 16 1% 102 2% 94 1%
LUTs 180 1% 30 1% 145 1% 165 1%
IOBs 34 14% 13 5% 59 25% 44 18%
B-MUXs 1 4% 1 1% 1 4% 1 4%

7. CONCLUSIONES


Se logr implementar exitosamente la arquitectura hard-
ware propuesta para el control de posicin, en un FPGA
Spartan-3E de Xilinx.

El uso de VHDL como lenguaje de descripcin permiti
descomponer la estructura principal de diseo en sub-
diseos e interconectarlos. Esto trajo como ventaja definir
bancos de pruebas (testbench) de manera global e indivi-
dual.

El uso de una arquitectura tipo paralela permiti contem-
plar la sintonizacin de cada PID de manera separada,
reduciendo considerablemente la complejidad de los
procedimientos.

Como trabajo futuro se proyecta la implementacin usan-
do punto fijo en vez de enteros. Los resultados encontra-
dos al analizar la respuesta del PID indican que podra ser
conveniente tal adicin, pero debe evaluarse su impacto
en los recursos consumidos.

AGRADECIMIENTOS

Este trabajo fue parcialmente cubierto por el Programa de
Becas Doctorales en reas Prioritarias de la Universidad
Tecnolgica Nacional y por las Becas de Investigacin y
Servicio entregadas por la Facultad Regional Mendoza de
dicha Universidad.

REFERENCIAS

Kuchen B. y Carelli R. (1996). Control Digital Discreto.
Ogata, K. (1998). Ingeniera de control moderna.
Pardo F. y Boluda J. A. (1999). VHDL: Lenguaje para
Sntesis y Modelado de Circuitos.

G
G
ADC
ADC
SPI SPI
FPGA
MUX
MUX
SPARTAN KIT
LTC6912 TLC1407
1
CD4052
4
4