Professional Documents
Culture Documents
Presentado por:
GUAYAQUIL – ECUADOR
AÑO 2015
I
AGRADECIMIENTO
incondicional.
grado.
Ronald Ponguillo.
DEDICATORIA
mi vida académica.
obstáculos presentados en el
brindado.
TRIBUNAL DE SUSTENTACIÓN
DECANA SUBROGANTE
DECLARACIÓN EXPRESA
__________________________
__________________________
RESUMEN
recorrer.
vez es controlado para fijar la dirección del robot y velocidad del centro de
masa.
VII
ÍNDICE GENERAL
AGRADECIMIENTO ....................................................................................... I
RESUMEN................................................................................................... VI
ABREVIATURAS ........................................................................................ XX
CAPÍTULO 1.................................................................................................. 1
1 GENERALIDADES ................................................................................. 1
CAPÍTULO 2................................................................................................ 11
CAPÍTULO 3................................................................................................ 59
3 DISEÑO E IMPLEMENTACIÓN............................................................ 59
ÍNDICE DE FIGURAS
Figura 2.1 Tarjeta PCB y diagrama de componentes de la DE0-Nano (vista
Figura 2.5 Diagrama de bloques del núcleo del procesador NIOS II [5]. .......21
..............................................................................................................30
..............................................................................................................54
Figura 3.13 Conexión de los pines del módulo DE0-Nano con los diversos
actuadores. ............................................................................................83
XIV
Figura 3.15 Conexión de los pines del módulo de sensores inerciales. ........85
..............................................................................................................87
Figura 3.24 Diagrama de Bloques del Sistema Completo sobre la FPGA. ....95
NIOS II...................................................................................................99
motores. ..............................................................................................127
correctamente. .....................................................................................137
ajustadas. ............................................................................................138
XVI
infrarrojos. ...........................................................................................141
Figura 3.50 Salida del Filtro unificando datos de los sensores infrarrojos y del
giroscopio. ...........................................................................................146
............................................................................................................153
Figura 3.54 Simulación del sistema realimentado del motor con un tiempo de
............................................................................................................157
angular. ...............................................................................................160
Figura 4.1 Velocidad angular del motor con y sin filtro de Kalman. .............167
Figura 4.2 Control de velocidad angular del motor ante un pulso de entrada.
............................................................................................................168
Figura 4.3 Posición angular del robot con y sin Filtro de Kalman. ...............168
pista. ....................................................................................................170
Figura 4.7 Separación angular del velocista con respecto a la pista durante
su recorrido..........................................................................................170
la pista. ................................................................................................171
Figura 4.11 Ángulo de orientación del magnetómetro del robot con respecto
Figura 4.12 Pista recreada con diferentes datos obtenidos por sensores. ..173
ÍNDICE DE TABLAS
ABREVIATURAS
DC Direct Current
mAh Milliampere-hour
MB Megabyte
MHz Megahertz
PC Personal Computer
INTRODUCCIÓN
Tanto a nivel nacional como a nivel internacional existe una gran cantidad de
Robótica (CER) y Robot Games Zero Latitud. Entre los principales a nivel
cerrado marcado con una línea. Existen además reglas adicionales que
usando ingeniería.
1 GENERALIDADES
una línea de color negro o blanco sobre una superficie del color
opuesto.
datos.
de desarrollo DE0-Nano.
FPGA.
los que más destacan. Entre tantas categorías existentes en este tipo
de robot velocistas. Hay una versión muy básica del robot en la cual se
manera suave.
implementado.
1.3 Justificación
dificultad.
sobre FPGA.
5
1.4 Objetivos
1.5 Metodología
internacionalmente.
mecánico.
grados de inclinación.
Micromouse1.
tamaño.
1
Competencia donde un pequeño robot debe encontrar la salida de un Laberinto.
10
2 MARCO TEÓRICO
LEDs y pulsadores.
a continuación [3]:
Dispositivos destacados
Elementos de configuración
Cabeceras de expansión
Dispositivos de memoria
o 32Mb SDRAM
Interfaces de entrada/salida
o 8 LEDs verdes
14
o 4 DIP switch
G-Sensor
resolución (13-bits)
Convertidor A/D
o Oscilador de 50 MHz
Fuentes de alimentación
VDC)
Diseño y Componentes
principales.
15
sistemas embebidos.
17
rendimiento y el costo.
32 fuentes de interrupción.
18
fuentes de interrupción.
32 bits.
y 128 bits.
NIOS II.
Controlador de excepción.
Bus de instrucción.
Bus de datos.
Archivo de registro.
Figura 2.5 Diagrama de bloques del núcleo del procesador NIOS II [5].
En nuestro país existen varios eventos de robótica de los cuáles los que
deben ser capaces de recorrer un camino cerrado sin salirse del mismo
usados están los PIC de Microchip y los AVR de ATMEL. Para detectar
tres motores, dos a los lados para mover las ruedas y otro para girar el
pequeño. Los motores que se usa tienen un alto costo debido a que no
digital.
Ecuaciones de diferencia
señal de entrada.
definidos por:
31
( ) ( ) ( )
( ) ( ) ( )
Transformada Z
define como:
( ) * ( )+ ∑ ( ) ;zє₵
( ) ( ) ( ) ( )
Transformada Inversa de Z
( )
X(z). Para ellos se descompone en fracciones simples y mediante
( )
( ) ( ) ( )
( )
transformada Z.
( )
, donde el grado de N(z) es menor o igual que el grado de D(z).
( )
( )
x(k) = { , ,…}
33
tenemos que:
( )
( )
( )
Y(z)=R(z)G(z)
proveer información al sistema, que puede ser usada para una variedad
35
métodos usados y los medios por los que el proceso de fusión mejora la
{[( ) ] } ,
( ) ( )
puede ser utilizado para separar estas señales de manera que puedan
que los filtros analógicos [9]. Esto hace una gran diferencia en cómo se
una de los tres, las otras dos son fijas y se pueden calcular
directamente.
Filtro de Kalman
un modelo, y el error del sistema como el error del sensor pueden ser
actual de un sistema como una predicción del estado futuro del sistema.
mediciones reales.
modelo de estado
y el modelo de medición
E{ w( ) ( )} = Q( )
E{ v( ) ( )} = R( ) ,
Kronecker.
determinada usando
Dónde
P(0|0) = .
través de una interfaz I²C / TWI compartida, lo que permite que los
los dos integrados están alineados al tablero para facilitar los cálculos
de fusión de sensores.
descritos en la Tabla I:
PIN Descripción
2.6.1 Giroscopio
de 6 grados de libertad.
Giroscopios rotantes.
Giroscopio óptico.
47
L3GD20H.
2.6.2 Magnetómetro
acelerómetros y giroscopios.
49
dado.
( )
en la siguiente sección.
50
2.6.3 Acelerómetro
Tipos de acelerómetros:
51
3 ejes.
±16 g.
la posición.
54
Conductora.
Óptica.
Magnética.
integrados en el motor.
arreglos en el mercado.
en una placa. Los sensores usados para aquello son los QTR-1A
Bluetooth o Wi-Fi.
tenemos [12]:
Modulación: GFSK
Sensitividad: -84dBm
58
3 DISEÑO E IMPLEMENTACIÓN
tratará sobre el hardware, esto incluye el aspecto mecánico y electrónico del robot,
HC-06
Dispositivo DIP-SWITCHES Botones
Bluetooth
Codificador
rotatorio
Codificador
rotatorio
Medidor de voltaje
Batería
3.1 Hardware
todo el sistema.
Cinemática Diferencial
Ecuación 3. 1
( *
Ecuación 3. 2
( *
Ecuación 3. 3
( *
Ecuación 3. 4
64
Ecuación 3. 5
√ √( ) ( )
Ecuación 3. 6
Dinámica
rueda esférica delante del robot, Figura 3.4, esta fuerza aparece
superficie.
∑
Ecuación 3. 7
66
Ecuación 3. 8
lineal del robot, siempre que este valor que deba tomar no supere
en esta situación.
∑
Ecuación 3. 9
2
Comercialmente existen llantas de diferente dureza fabricadas para el
automodelismo RC.
68
∑
Ecuación 3. 10
Ecuación 3. 11
esta soportado por las ruedas fijas sin que el robot se levante, es
las ruedas traseras pero este empezará a girar alrededor del eje Z,
masa esté lo más bajo posible y lo más lejos de las ruedas fijas.
Por lo que otro detalle importante es que para que sea mínima
Ecuación 3. 12
curvatura.
Ecuación 3. 13
√
el centro de gravedad.
Ecuación 3. 14
Ecuación 3. 15
velocidad angular.
Ecuación 3. 16
es:
radio de la rueda.
( )
Ecuación 3. 17
Esta última ecuación dice que se debe mantener la masa del robot
de frente.
De la Ecuación 3. 12 se tiene:
entonces:
( )
Ecuación 3. 18
78
adecuadas.
Voltaje de salida: 5V
sobrecalentamiento.
puede encontrar el pin Enable, que sirve para que en caso de que
NIOS II.
funcionamiento.
Figura 3.13 Conexión de los pines del módulo DE0-Nano con los diversos
actuadores.
porque todos ellos están unidos dentro del módulo, pero si se deja
84
Módulo Bluetooth
Giroscopio
Divisor de voltaje
,
( )
manera:
( )
( )
( )
87
Sensores infrarrojos
módulo NIOS II, para ello se usa un bus de datos que une ambas
del motor.
Tabla II.
un solo módulo.
90
Cada motor viene con 6 pines para conexión, los cuales son los
son las señales enviadas del encoder hacia el módulo NIOS II.
energía hacia estos, los cuales van conectados a las salidas del
Elementos auxiliares
para que se pueda adaptar algún tipo de sensor que pueda servir
Circuito impreso
cable plano flexible (FFC), que es por donde pasan las señales de
forma está pensado en ello, cuenta con dos entradas en las cuales
conectores, rueda loca y otros que sirven para ubicar la base del
motor.
94
3.2 Software
periféricos adicionales.
H.
lenguaje C.
95
Figura 3.24:
PWM ENCODER
PIO1 PIO2 PIO3 STDBY 1 CONTROLER 1
NIOS II
SDRAM
JTAG PORT CONTROLLER
Clock Source:
System ID Peripheral:
hexadecimal 55.
NIOS II Processor:
kB respectivamente.
memoria EPCS.
JTAG UART:
[19].
SDRAM Controller:
101
tarjeta DE-Nano.
a usar:
102
Interval Timer:
interrupción.
led [20].
University Program.
ADC CONTROLLER
Performance Counter
medir.
I2C
Opencores [22].
109
en modo de espera.
Altera, pero este fue modificado para que además maneje la señal
adelante.
controlador Pololu.
Encoder
con signo del número de pasos que rota el eje del motor, el signo
codificador rotatorio.
COMPONENTES HDL
II, este sea probado con una simulación y de ser posible sobre la
interrupciones.
codificador rotatorio.
codificador rotatorio.
de giro.
correctamente.
señales que maneja son de un bit, las entradas son: clk, resetn,
chA y chB. Las salidas son: enC y enUD, todas estas señales ya
cero.
115
codificador rotatorio.
codificador rotatorio.
usada.
usada.
de Espera.
tiempo.
3
Software predecesor de Qsys
118
interfaz de comunicación.
La idea básica del código que proporciona Altera es, primero llevar
lógico.
√( ) ( )
de 98.83%.
( )
( )
Donde:
transformada de Laplace.
siguiente forma:
Ecuación 3. 19
es el vector de estados.
es el vector de salida.
es el vector de entradas .
es la matriz de estados.
es la matriz de entrada.
es la matriz de salida.
es la matriz de transmisión directa.
. /
. /
( )
controlador.
sobrepasar los 6V, ni estar por debajo de -6V, para evitar daños
4
Estos números indican el valor en ciclos de reloj del tiempo en alto de la señal PWM generada por la
FPGA, 5000 es el valor máximo.
128
( ) ( )
( ) ( ) ( ) ( )
129
5
En Sisotool al diseñar el controlador se especificó esta limitación, pero sólo corresponde a una
entrada de amplitud 1.
130
del controlador.
Las señales que pasan por el proceso de filtrado son las señales
6
En la implementación se aumenta el valor de referencia para alcanzar el valor deseado, por ejemplo
con una referencia de 1.73 se tendrá una salida de 1.
131
giroscopio se las pasa juntas por otro filtro que además fusiona
Codificadores rotatorios
en tiempo discreto.
̇
̇
132
añade:
∑
Además:
∑
el modelo anterior.
133
cantidad .
Para que las variables de estado del sistema sean x1, x2, x3, x4 se
̇
134
̇
̇
̇
̇
En matrices se tiene:
̇
̇
( , ( )( ) ( )
̇
̇
( )( )
( )
( )
( )
Una vez que se tiene el modelo del sistema se puede usar las
Predicción:
̂ ̂
Actualización:
̃ ̂
̂ ̂ ̃
( )
respectivamente.
filtrado.
( )
Además una vez que se encuentra los valores para las matrices Q
valores.
Dónde:
( )
( )
( )
( )
explica a continuación.
140
representado con θ.
medir.
controlador.
7
Por ejemplo, no se puede obtener una primera diferencia, pues los valores de la señal están
cuantificados.
142
relacionados.
medición.
Ecuación 3. 21
errores de modelización.
Ahora se nombra ̇ a y a :
144
velocidad angular con la que se aleja la pista del robot, sobre esta
rotatorios.
Ecuación 3. 22
̇
Ecuación 3. 23
̇
Expresado en Matrices:
̇
( * . /. / . /
̇
( ). /
145
( )
continuo.
. / . /. / . /
( ). /
( * . /( * . /
( )( *
146
FPGA.
. /
Figura 3.50 Salida del Filtro unificando datos de los sensores infrarrojos y
del giroscopio.
147
implementar es:
. /
. /
( )
148
. /
Ecuación 3. 4 se tiene:
Ecuación 3. 24
angular medida.
√( ) ( )
cero:
esto implique controlar , sin importar cuál sea el valor que tome
( )
error de entrada:
( )
( ) ( ) ( ( ) ( ))
posición.
Figura 3.54 Simulación del sistema realimentado del motor con un tiempo
de muestreo de 1.5ms.
3.42 es mejor.
Para lograr que se ejecuten todas las tareas dentro del tiempo
de control y filtrado.
Ʃ Controlador
Primer Orden
M(z)
-
Filtro de Kalman Encoder
velC
1/r
+
Ʃ Mr1(z)
Q+Q
ωref
l/2r
velC Ʃ r/l ω
1/r
m-m Q-Q
Ʃ Mr2(z)
para esto.
de la lineal se tiene:
159
Ecuación 3. 25
( )
Ecuación 3. 26
( )
De estas ecuaciones se tiene que para que la velocidad lineal del robot
3.57.
160
θref θ
Ʃ Controlador
PD
H(z)
∫
Giroscopio
Filtro de Kalman
Sensores IR
controlador.
límites de velocidad del robot, este valor define la velocidad del centro
de masa.
Diseño físico.
radio ya que los engranes que se disponían daban una relación de 8:1 y
tamaño se redujo respecto al primer diseño. Son dos las placas que
.
Figura 3.59 Circuito Impreso del Diseño Final
sostienen los motores y los aros donde van las llantas del robot, los
cable plano flexible. Las ruedas tienen un radio de 1.5 cm, esta
del tiempo que le toma al sistema en ejecutar cada una de las tareas, estas fueron
En la sección 3.2.5 se explicó acerca del tiempo que debe transcurrir entre
(uS) (uS)
Infrarrojos
Figura 4.1 Velocidad angular del motor con y sin filtro de Kalman.
168
Figura 4.2 Control de velocidad angular del motor ante un pulso de entrada.
Figura 4.3 Posición angular del robot con y sin Filtro de Kalman.
169
Figura 4.6 Diversas velocidades del robot velocista durante su recorrido en la pista.
Figura 4.7 Separación angular del velocista con respecto a la pista durante su
recorrido.
171
Figura 4.11 Ángulo de orientación del magnetómetro del robot con respecto al
campo magnético de la tierra.
173
Figura 4.12 Pista recreada con diferentes datos obtenidos por sensores.
del velocista.
interrupción.
protocolo I2C.
el core implementado.
medida en conteos del encoder cada 500 uS. Esta señal se obtiene al
datos provenientes del encoder son discretos por lo que tiene influencia
estos.
angular del motor una vez filtrada, se puede observar que el tiempo en
En las figuras 4.3 y 4.4 se muestra cómo actúa el sistema para controlar
segundos.
Para las respectivas pruebas del diseño final del robot velocista sobre
una pista, se procedió a elaborar una con cinta aislante negra sobre
longitud total de 5.84 metros. Para los resultados finales sobres esta
pista, se tomó datos de los diversos sensores del robot, mientras este
sentido mostrado.
178
Ecuación 3. 4.
una de las más cerradas de este, una vez detectada esta curva a través
derecha, por lo que ocurre un giro hacia ese lado, lo que también
trayectoria sea lo menor posible. Una vez que el robot gira y termina la
Entre los principales resultados obtenidos sobre esta pista tenemos que
los valores máximos y mínimos alcanzados por las ruedas para esta
obtuvieron los valores límites en una curva que gira hacia la derecha,
trayectoria que sigue esta pista y que pueden variar dependiendo del
curva con el menor radio de curvatura posible, así como del ángulo del
robótica nacionales.
que necesita un tiempo para que ocurra esto, el cual se logra solo en
suficientemente cerca.
-15.35.
mismas curvas, y esto será la tendencia para todas las gráficas, cuyo
En las Figuras 4.9, 4.10 y 4.11 se tienen gráficas obtenidas a través del
Con estos resultados se puede obtener una gráfica estimada del ángulo
CONCLUSIONES
integrar en un solo chip los módulos necesarios para el control del robot
sin realizar alguna modificación física. Entre las tareas que se realizó
trabajo del sistema añadiendo un módulo PLL digital, leer las señales
digital y con ruido debido a que los motores y codificadores usados son
diseño.
física del robot, se debe escoger las ruedas del material que
robot pero también aumenta la carga equivalente sobre los motores, por
RECOMENDACIONES
batería que no se descargue tan rápido sino que permita realizar las
para la competencia.
4. Altera provee una aplicación que es muy útil para trabajar con la DE0-
forma serial.
BIBLIOGRAFÍA
http://www.altera.com/literature/ug/DE0_Nano_User_Manual_v1.9.pdf
USA, 2012.
https://es.scribd.com/doc/28358833/Estudio-del-microprocesador-Nios-II
2010.
http://www.altera.com/literature/hb/nios2/n2cpu_nii5v1.pdf, 2014.
http://www.isa.uma.es/C14/Presentaciones%20de%20Clase%20(ppt)/D
ocument%20Library/SISTEMAS%20DE%20CONTROL%20DISCRETO
Control, http://www.atp.ruhr-uni-bochum.de/rt1/syscontrol/node1.html,
2005.
[9] Smith Steven W, The Scientist and Engineer´s Guide to Digital Signal
2013.
https://www.pololu.com/file/0J703/LSM303D.pdf, 2012.
http://silabs.org.ua/bc4/hc06.pdf, 2011.
2012.
ftp://ftp.altera.com/up/pub/Altera_Material/11.1/University_Program_IP_
Cores/Communications/RS232.pdf, 2011.
194
http://www.altera.com/literature/tt/tt_nios2_hardware_tutorial.pdf, 2011.
ftp://ftp.altera.com/up/pub/Altera_Material/12.0/Computer_Systems/DE0
-Nano/DE0-Nano_Basic_Computer.pdf, 2010.
http://www.altera.com/literature/ug/ug_embedded_ip.pdf, 2014
ftp://ftp.altera.com/up/pub/Altera_Material/9.1/University_Program_IP_C
ores/Input_Output/Parallel_Port.pdf, 2010.
ftp://ftp.altera.com/up/pub/Altera_Material/12.1/University_Program_IP_
Cores/Input_Output/DE0-Nano_ADC_Controller.pdf, 2012.
http://opencores.org/project,i2c, 2014.
ftp://ftp.altera.com/up/pub/Altera_Material/12.0/Tutorials/making_Qsys_c
omponents.pdf, 2012
2224…SR,https://fmcc.faulhaber.com/resources/img/EN_2224_SR_DFF
.PDF, 2015.
http://www.altera.com/literature/manual/mnl_avalon_spec.pdf, 2015.
ANEXOS
197
resetn
Ta
chA 1 chB 1
0
0 0
enC
chB
enDU
enC
Tb
chA 0 chB
0
1 0
enC
chB
enDU
1
enC
Tc
chA 0 chB
1
0
1
enC
enDU
chB
enC
Td
chA 1 chB
1
0 1
enC
chB
enDU
enC
198
B. Diagramas de Tiempo
( ) ( )
Ecuación D. 1
permite encontrar los valores a,b,c,d,e que hacen que los vectores de
la web de MATLAB.
√
√
√
202
Figura D.1
en la Figura D.2.
203
tediosa y no es práctica.
En este proyecto lo que se hizo fue tomar datos con los sensores y
y = Xβ + ε
Donde
y es el vector de respuestas de tamaño nX1.
β es el vector de coeficientes de tamaño mX1.
X es la matriz de diseño del modelo de tamaño nxm.
ε es el vector de errores de tamaño nx1.
La idea es encontrar el vector β que minimice el error ε. El vector que
minimiza este error está dado por:
( )
Para este caso solo se necesita encontrar dos valores constantes
( +
( +
( *
Donde
diferentes.