ESCUELA TÉCNICA SUPERIOR DE INGENIERÍA (ICAI)

Grado en Ingeniería en Tecnologías Industriales (GITI)

PROYECTO FIN DE GRADO

CONTROL DE UN VEHÍCULO
EQUILIBRISTA MEDIANTE UNA
RASPBERRY PI

Autora: Carmen Jiménez Cortés

Director: Prof. Dr. Juan Luis Zamora Macho

MADRID
junio 2019
Copyright © 2019 by Carmen Jiménez Cortés

This dissertation was typeset with LATEX and compiled in TEXstudio.

AUTORIZACIÓN PARA LA DIGITALIZACIÓN, DEPÓSITO Y DIVULGACIÓN EN RED DE
PROYECTOS FIN DE GRADO, FIN DE MÁSTER, TESINAS O MEMORIAS DE
BACHILLERATO

1º. Declaración de la autoría y acreditación de la misma.

Carmen Jiménez Cortés
El autor D.____________________________________________________________________
DECLARA ser el titular de los derechos de propiedad intelectual de la obra:
Control de un vehículo equilibrista mediante una RaspberryPi3Bt
______________________________________________________________________________,
que ésta es una obra original, y que ostenta la condición de autor en el sentido que otorga la Ley de
Propiedad Intelectual.

2º. Objeto y fines de la cesión.

Con el fin de dar la máxima difusión a la obra citada a través del Repositorio institucional de la
Universidad, el autor CEDE a la Universidad Pontificia Comillas, de forma gratuita y no exclusiva,
por el máximo plazo legal y con ámbito universal, los derechos de digitalización, de archivo, de
reproducción, de distribución y de comunicación pública, incluido el derecho de puesta a disposición
electrónica, tal y como se describen en la Ley de Propiedad Intelectual. El derecho de transformación
se cede a los únicos efectos de lo dispuesto en la letra a) del apartado siguiente.

3º. Condiciones de la cesión y acceso

Sin perjuicio de la titularidad de la obra, que sigue correspondiendo a su autor, la cesión de
derechos contemplada en esta licencia habilita para:
a) Transformarla con el fin de adaptarla a cualquier tecnología que permita incorporarla a
internet y hacerla accesible; incorporar metadatos para realizar el registro de la obra e
incorporar “marcas de agua” o cualquier otro sistema de seguridad o de protección.
b) Reproducirla en un soporte digital para su incorporación a una base de datos electrónica,
incluyendo el derecho de reproducir y almacenar la obra en servidores, a los efectos de
garantizar su seguridad, conservación y preservar el formato.
c) Comunicarla, por defecto, a través de un archivo institucional abierto, accesible de modo
libre y gratuito a través de internet.
d) Cualquier otra forma de acceso (restringido, embargado, cerrado) deberá solicitarse
expresamente y obedecer a causas justificadas.
e) Asignar por defecto a estos trabajos una licencia Creative Commons.
f) Asignar por defecto a estos trabajos un HANDLE (URL persistente).

4º. Derechos del autor.

El autor, en tanto que titular de una obra tiene derecho a:
a) Que la Universidad identifique claramente su nombre como autor de la misma
b) Comunicar y dar publicidad a la obra en la versión que ceda y en otras posteriores a través
de cualquier medio.
c) Solicitar la retirada de la obra del repositorio por causa justificada.
d) Recibir notificación fehaciente de cualquier reclamación que puedan formular terceras
personas en relación con la obra y, en particular, de reclamaciones relativas a los derechos
de propiedad intelectual sobre ella.

5º. Deberes del autor.

El autor se compromete a:
a) Garantizar que el compromiso que adquiere mediante el presente escrito no infringe ningún
derecho de terceros, ya sean de propiedad industrial, intelectual o cualquier otro.
b) Garantizar que el contenido de las obras no atenta contra los derechos al honor, a la
intimidad y a la imagen de terceros.
c) Asumir toda reclamación o responsabilidad, incluyendo las indemnizaciones por daños, que
pudieran ejercitarse contra la Universidad por terceros que vieran infringidos sus derechos e
 intereses a causa de la cesión.
d) Asumir la responsabilidad en el caso de que las instituciones fueran condenadas por infracción
de derechos derivada de las obras objeto de la cesión.

6º. Fines y funcionamiento del Repositorio Institucional.

La obra se pondrá a disposición de los usuarios para que hagan de ella un uso justo y respetuoso
con los derechos del autor, según lo permitido por la legislación aplicable, y con fines de estudio,
investigación, o cualquier otro fin lícito. Con dicha finalidad, la Universidad asume los siguientes
deberes y se reserva las siguientes facultades:

➢ La Universidad informará a los usuarios del archivo sobre los usos permitidos, y no
garantiza ni asume responsabilidad alguna por otras formas en que los usuarios hagan un
uso posterior de las obras no conforme con la legislación vigente. El uso posterior, más allá
de la copia privada, requerirá que se cite la fuente y se reconozca la autoría, que no se
obtenga beneficio comercial, y que no se realicen obras derivadas.
➢ La Universidad no revisará el contenido de las obras, que en todo caso permanecerá bajo
la responsabilidad exclusive del autor y no estará obligada a ejercitar acciones legales en
nombre del autor en el supuesto de infracciones a derechos de propiedad intelectual derivados
del depósito y archivo de las obras. El autor renuncia a cualquier reclamación frente a la
Universidad por las formas no ajustadas a la legislación vigente en que los usuarios hagan uso
de las obras.
➢ La Universidad adoptará las medidas necesarias para la preservación de la obra en un futuro.
➢ La Universidad se reserva la facultad de retirar la obra, previa notificación al autor, en
supuestos suficientemente justificados, o en caso de reclamaciones de terceros.

20 de …………………………...
Madrid, a ……….. Junio 2019
de ……….

ACEPTA

Giménez
Fdo………………………………………………

Motivos para solicitar el acceso restringido, cerrado o embargado del trabajo en el Repositorio
Institucional:
Declaro, bajo mi responsabilidad, que el Proyecto presentado con el título
Control de un vehículo mediante
.………..…………………………………………………………………….……
equilibrista
una Pi
……………………………………………………………………………………
Raspberry
31314
en la ETS de Ingeniería - ICAI de la Universidad Pontificia Comillas en el

curso académico ………………. es de mi autoría, original e inédito y

no ha sido presentado con anterioridad a otros efectos. El Proyecto no es

plagio de otro, ni total ni parcialmente y la información que ha sido tomada

de otros documentos está debidamente referenciada.

Fdo.: Jiménez Fecha: 20 19

06 ……
……/ ……/

Autorizada la entrega del proyecto

EL DIRECTOR DEL PROYECTO

Iota

Amund
a 06
Fdo.: Fecha: ……/ ……/ ……
 ESCUELA TÉCNICA SUPERIOR DE INGENIERÍA (ICAI)
Grado en Ingeniería en Tecnologías Industriales (GITI)

PROYECTO FIN DE GRADO

CONTROL DE UN VEHÍCULO
EQUILIBRISTA MEDIANTE UNA
RASPBERRY PI

Autora: Carmen Jiménez Cortés

Director: Prof. Dr. Juan Luis Zamora Macho

MADRID
junio 2019
Copyright © 2019 by Carmen Jiménez Cortés

This dissertation was typeset with LATEX and compiled in TEXstudio.

 Agradecimientos
Este trabajo de fin de grado representa el final de la primera etapa de mi paso por la
universidad. Llegar hasta aquí no ha sido fácil y desde luego, no habría sido posible sin todas
aquellas personas con las que he tenido la suerte de compartir estos años. Sería injusto no
reconocer que mis logros son en gran parte gracias a vosotros.

En primer lugar, agradecer el apoyo incondicional de mis padres, que confiaron siempre en
mí, incluso en esas primeras semanas de carrera, cuando yo me creía incapaz de sobrevivir ni
siquiera a los primeros exámentes intercuatrimestrales.

A todos los amigos y amigas que esta carrera me ha dado la oportunidad de hacer. Hemos
sido compañeros de fatigas en los momentos más duros, y también compañeros de celebración
cada vez que despedíamos un curso más.

A todo el personal docente que me han formado tanto personal como profesionalmente a lo
largo de estos cuatro años, para que pueda convertirme en una gran profesional en el futuro.
En especial a mi director de proyecto, Dr. Juan Luis Zamora Macho, que desde que comenzamos
con este trabajo de fin de grado, no ha dejado de plantearme retos para mostrarme todo aquello
que soy capaz de hacer. Trabajar con él ha hecho que este proyecto sea una gran experiencia,
de la cual me llevo infinidad de recuerdos, anécdotas y conocimientos, y ha hecho de él la
despedida perfecta para cerrar esta primera etapa.

Gracias también a mi familia de Crossfit G2, por proporcionarme esa vía de escape que
necesitaba en los momentos de mayor estrés. Gracias a mi entrenador, Pedro, que ha estado al pie
del cañón en cada temporada de exámenes, para asegurarse de que no perdía la cabeza.

No puedo olvidarme de José y Antonio, ya que sin ellos aún seguiría intentando averiguar
como ensamblar las piezas del vehículo. Ni tampoco puedo terminar estas palabras sin
mencionar a Javier García Aguilar, que siempre que ha tocado echar horas extra ha estado
presente, dispuesto a ayudar en lo que fuera posible.

Una última vez, gracias a todos, espero estar un pasito más cerca de convertirme en esa
ingeniera de quién podáis sentiros orgullosos.

Control de un Vehículo Equilibrista mediante una Raspberry Pi VII

Carmen Jiménez Cortés
 Abstract

The aim of this project has been to develop all the tools required for being able to control
the Balboa32U4 balancing vehicle, manufactured by the american company Pololu.

All the controls required both for the stabilization and navigation of the vehicle have
been developed. To achieve this goal, the control logic, and the specific drivers used by
the vehicle hardware have been programmed using Matlab-Simulink. Then, the controls
have been optimized and eventually implemented in the vehicle.

Introduction
A balancing vehicle is one of the examples of the classic control problem known as the
“Inverted Pendulum”. The vehicle balance is unstable, because if a perturbation shifts the vehicle
from its equilibrium position it drops, unless an external action prevents it.

The hardware elements used to build the vehicle are:

An ATMega32U4 microcontroller: Programmed using the Arduino IDE, it is responsible of

performing the lower level tasks, such as writting of the motors voltage and reading of
the sensors.

A Raspberry Pi 3B+: Programmed using Matlab-Simulink, it is responsible of performing

the most demmanding tasks, such as executing the control logic and the communication
management.

An Inertial Measurement Unit (IMU): Includes a three-axis gyroscope and accelerometer,

that provide respectively the angular velocity and the linear acceleration of the vehicle
along each axis.

Two magnetic encoders: Located at each motor shaft. These sensors have been the most
critical hardware element, as they are magnetic instead of optical encoders, they are less
accurate, and when the vehicle moves provide a too noisy signal, that made it necessary
to program advanced filtering and processing tools to get an usable signal.

Two lateral sensors: Using the FlightSense™ technology that measures the distance to an
object with 1 mm of precision. Using an infrared sensor, a distance sensor and an ambient
light sensor, estimates the distance to the object based on the amount of reflected light
and time of flight.

Two 6V DC motors, that are the main vehicle actuators.

Control de un Vehículo Equilibrista mediante una Raspberry Pi IX

Carmen Jiménez Cortés
Abstract

Three pushbuttons A, B and C: Used to go from one state to another within the state
machine.

Figure 1 shows how the vehicle looks like when all sensors, the microcontrollers and actuators
previously described are installed.

Figura 1. Balboa 32U4 with Raspberry Pi.

Objectives
The main objectives of this project have been:

Hardware configuration and drivers programming required to be implemented in the

vehicle.

Control system design: To develop a precise dynamic model of the vehicle, and to optimize
the control system and the state estimator.

Implementation in the vehicle: To design and to program the controls, the state estimator
and the state machine which enable the vehicle control.

Solution
The vehicle dynamic model that has been used is an state space model. To develop the model
multiple tests were performed of both with the hardware elements standalone, and with them
integrated in a balanced vehicle. For the latter, as a model was not available yet, an stabilization
control was tuned using many trial and error tests.

Only then, it was possible to identify the unknown dynamic parameters using simula-
tion.

X Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 Abstract

As translation and pitch are an unstable dynamic, they have been uncoupled of the yaw
turning movement.

All controls have been designed as state feedback controls, but eventually, they become
cascade PIDs for their implementation.

In the control system, it has been a must to add an state estimator, due to the fact that one
of the state variables is impossible to be measured: the pitch angle. An extended Kalman filter
has been the estimator used.

All the blocks that build the control system have been integrated in one unique Simulink
diagram, which runs in the Raspberry Pi and enables the vehicle monitorization while it is
operating. Figure 2 shows this diagram.

Figura 2. Block diagram of the Simulink subsystems.

Results
Figure 3 shows the results obtained from the identification of the vehicle translation and
pitch movement. The results are very satisfactory, as the simulated variables accurately match
the test actual measurements.

On Figure 3, the actual value, red colored, the simulated one, blue colored, are compared
for the pitch rate on the upper figure, and on the lower one, for the forward speed.

Similarly, the controls optimizing for translation and pitch, and yaw turn were also very
good.

Figure 4 shows the state variables for the PI-PD control simulation for the optimum
translation and pitch control. This control was implemented in the vehicle as well.

The first graphic shows the forward speed in cm/s, the second one shows the pitch rate in
°/s and the last one shows the pitch angle in °.

Additionally, the yaw turn control was optimized and implemented in the vehicle. The wall
follower control was optimized but due to time restrictions it was not possible to implement in
the vehicle.

Control de un Vehículo Equilibrista mediante una Raspberry Pi XI

Carmen Jiménez Cortés
Abstract

Figura 3. Actual measurement, the simulated one and the error for the pitch rate and the forward speed.

Figura 4. State vector in simulation.

Contributions
The main contributions of this project are:

Hardware implementation: An operating vehicle has been set to work, it can be used in
the Digital Control and Advanced Control laboratories.

XII Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 Abstract

Software programming: The developed software can be used in future projects that use
the same sensors and actuators.

Optimization: Complex and sofisticated tools for the model identification and for the
optimization of the control system and state estimator have been coded. Such tools might
be useful in future projects.

Implementation: Most of the designed controls have been implemented in the vehicle.

Control de un Vehículo Equilibrista mediante una Raspberry Pi XIII

Carmen Jiménez Cortés
 Resumen

Este trabajo de fin de grado ha consistido en desarrollar todas las herramientas necesarias
para poder controlar el vehículo equilibrista Balboa32U4, de la empresa americana
Pololu.

Se han programado controles que permiten controlar tanto la estabilización del vehículo
como su navegación. Para ello ha sido necesario programar en Matlab-Simulink toda
la lógica de control, los drivers específicos para el hardware que compone el vehículo,
optimizar los controles y finalmente implantarlos en el vehículo.

Introducción
Un vehículo equilibrista constituye un ejemplo más del problema clásico de control
conocido como “Péndulo invertido”. El equilibrio del vehículo es inestable, porque si alguna
perturbación desplaza al vehículo de dicha posición se cae, salvo que se actúe externamente
para impedirlo.

Los elementos hardware que componen el vehículo son:

Un mircrocontrolador ATMega32U4: Programado con la IDE de Arduino, y responsable

de las tareas de menor nivel, como la escritura de la tensión en los motores y la lectura de
los sensores.

Una Raspberry Pi 3B+: Programada con Matlab-Simulink, y responsable de las tareas de

mayor complejidad, tales como la ejecución de la lógica de control y la gestión de las
comunicaciones.

Una unidad de medidas inerciales (IMU): Compuesta por un giróscopo y un acelerómetro

de tres ejes, que permiten medir la velocidad angular y la aceleración lineal del vehículo
en cada eje.

Dos encoders magnéticos: Ubicados en el eje de cada motor. Estos sensores han sido
el elemento hardware más problemático, ya que al tratarse de encoders magnéticos y
no ópticos, son menos precisos, y durante el movimiento del vehículo proporcionaban
una medida demasiado ruidosa, por lo que fue necesario programar herramientas de
procesado y filtrado avanzadas para obtener una señal útil.

Dos sensores laterales: Que mediante la tecnología FlightSense™, permiten medir la

distancia a un objeto con una precisión de 1 mm. Mediante un emisor de infrarrojos, un
sensor de distancia y un sensor de luz ambiente, estima la distancia al objeto en base a la
cantidad de luz reflejada y el tiempo de vuelo.

Control de un Vehículo Equilibrista mediante una Raspberry Pi XV

Carmen Jiménez Cortés
Resumen

Dos motores de corriente continua de 6V, que son los principales actuadores del vehículo.

Tres pulsadores A, B y C: Empleados para cambiar de un estado a otro en la máquina de

estados.

El aspecto del vehículo con todos los sensores, los dos microcontroladores y actuadores
previamente descritos se puede ver en la Figura 5.

Figura 5. Balboa 32U4 con Raspberry Pi.

Objetivos
Los objetivos fundamentales de este proyecto han sido:

Configurar el hardware y programar los drivers necesarios para implantarlos en el vehículo.

Diseñar el sistema de control: Obtener un modelo dinámico preciso de la planta, optimizar

el sistema de control y el estimador de estado.

Implantación en el vehículo: Diseñar y programar los controles, el estimador de estado y

la máquina de estados que permitieran controlar el vehículo.

Solución
El modelo dinámico del vehículo que se ha empleado es un modelo en espacio de estado.
Para obtener el modelo fue necesario llevar a cabo ensayos de los elementos hardware aislados,
y también del vehículo completo en equilibrio. Para este segundo tipo de ensayos, al no
disponerse de un modelo, fue necesario ajustar un control de estabilización por tanteo y
mediante numerosas pruebas.

XVI Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 Resumen

Una vez realizados todos los ensayos, fue posible identificar los parámetros dinámicos
desconocidos mediante simulación.

Se han desacoplado los movimientos de avance y cabeceo del movimiento de giro, ya que el
primero se trata de una dinámica inestable. Todos los controles se han diseñado como controles
con realimentación del estado, pero posteriormente se transformaban en PIDs en cascada para
su implementación.

En el sistema de control ha sido indispensable incluir un estimador de estado, ya que una de

las variables de estado no es medible: el ángulo de cabeceo. El estimador de estado empleado
ha sido un filtro extendido de Kalman (EKF).

Todos los elementos que componen el sistema de control se han integrado en un único
diagrama de Simulink, el cual se ejecuta en la Raspberry Pi y permite monitorizar el vehículo
durante su funcionamiento. Este diagrama se muestra en la Figura 6.

Figura 6. Esquema de los subsistemas que contiene el diagrama de Simulink.

Control de un Vehículo Equilibrista mediante una Raspberry Pi XVII

Carmen Jiménez Cortés
Resumen

Resultados
Los resultados que se obtuvieron de la identificación del movimiento de avance y cabeceo del
vehículo se pueden ver en la Figura 7. Los resultados son muy satisfactorios, ya que las variables
simuladas se corresponden de una forma bastante precisa con las reales medidas durante el
ensayo.

Figura 7. Medida real, la generada por el simulador y el error entre ambas para la velocidad angular de
cabeceo y la velocidad lineal de avance.

Lás gráficas que aparecen en la Figura 7 son las siguientes: En la figura superior se comparan
el valor real, en color rojo, y el simulado, en color azul, de la velocidad angular de cabeceo, y
en la figura inferior, los valores real y simulados de la velocidad lineal de avance.

Análogamente, la optimización de los controles de avance y cabeceo y de guiñada fueron

también muy buenos.

En la Figura 8 se muestra el valor de las variables de estado en simulación del control PI-PD
de avance y cabeceo óptimo. Este control también se llegó a implementar en el vehículo.

La primera gráfica se corresponde con la velocidad lineal de avance en cm/s, la segunda con
la velocidad angular de cabeceo en º/s y la tercera con el ángulo de cabeceo en º.

Adicionalmente, se optimizó el control de guiñada y pudo implantarse en el vehículo. El

control de seguimiento de pared también pudo optimizarse, pero por cuestiones de tiempo no
pudo implantarse finalmente.

XVIII Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 Resumen

Figura 8. Vector de estado en simulación.

Aportaciones
Las principales aportaciones de este proyecto son:

Implantación del hardware: Se ha puesto en marcha el vehículo, que podrá utilizarse en

los laboratorios de Control Digital y Control Avanzado.

Programación del software: Que podrán resultar de utilidad para futuros proyectos que
utilicen los mismos sensores y actuadores.

Optimización: Se han programado herramientas sofisticadas, para la identificación del

modelo, y para la optimización para el control y del estimador de estado, que podrían
emplearse en otros proyectos.

Implementación: Se han implementado la mayoría de los controles diseñados en el propio

vehículo.

Control de un Vehículo Equilibrista mediante una Raspberry Pi XIX

Carmen Jiménez Cortés
 Índice general
Abstract IX

Resumen XV

Parte I. Memoria descriptiva XXIX

1. Introducción 1
1.1. Motivación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2. Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3. Recursos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.4. Metodología . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2. Estado del Arte 7
2.1. El Péndulo Invertido . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2. Soluciones al problema de control . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3. El EKF y la navegación autónoma . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3. Hardware 11
3.1. Descripción del vehículo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.2. Raspberry Pi 3B+ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.3. ATMega32U4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.3.1. Descripción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.3.2. Comunicación entre los dos microcontroladores . . . . . . . . . . . . . . . 17
3.4. IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.4.1. Descripción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.4.2. Modo de empleo de la IMU . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.4.3. Procesamiento de la información . . . . . . . . . . . . . . . . . . . . . . . 18
3.5. Motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.5.1. Descripción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.5.2. Funcionamiento del driver DRV8838 . . . . . . . . . . . . . . . . . . . . . 21
3.6. Sensores laterales . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.6.1. Descripción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.6.2. Comunicación con la Raspberry Pi . . . . . . . . . . . . . . . . . . . . . . . 24
3.6.3. Modo de empleo de la información de distancia . . . . . . . . . . . . . . . 24
3.7. Encoders . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.7.1. Descripción y funcionamiento . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.7.2. Estimación de la velocidad de avance y velocidad angular de guiñada . . . 26
3.7.3. Corrección de la medida de los encoders . . . . . . . . . . . . . . . . . . . 28

Control de un Vehículo Equilibrista mediante una Raspberry Pi XXI

Carmen Jiménez Cortés
Índice general

3.7.3.1. Filtro no lineal para eliminar el efecto del rebose . . . . . . . . . 28

3.7.3.2. Filtro no lineal para eliminar errores de medida . . . . . . . . . . 29
3.7.3.3. Reconstrucción de la medida del encoder . . . . . . . . . . . . . 29
3.7.3.4. Filtrado lineal paso bajo . . . . . . . . . . . . . . . . . . . . . . . 30
3.7.3.5. Aplicación de los filtros . . . . . . . . . . . . . . . . . . . . . . . 30
3.8. Fabricación piezas 3D . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
4. Modelado y obtención de parámetros 35
4.1. Modelo matemático del vehículo . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
4.1.1. Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
4.1.2. Parámetros geométricos y mecánicos . . . . . . . . . . . . . . . . . . . . . 36
4.1.3. Variables de estado y variables auxiliares . . . . . . . . . . . . . . . . . . . 36
4.1.4. Ecuaciones del motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
4.1.5. Movimiento de avance y cabeceo . . . . . . . . . . . . . . . . . . . . . . . 38
4.1.6. Movimiento de giro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
4.2. Identificación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.2.1. Caracterización de los motores . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.2.2. Algoritmo de optimización Gauss-Newton . . . . . . . . . . . . . . . . . . 43
4.2.3. Identificación del modelo de avance y cabeceo . . . . . . . . . . . . . . . . 44
4.2.4. Identificación del modelo de giro . . . . . . . . . . . . . . . . . . . . . . . 45
4.2.5. Identificación del ruido . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
4.2.6. Retardo del lazo de control . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
5. Sistema de Control 49
5.1. Control por realimentación del estado . . . . . . . . . . . . . . . . . . . . . . . . 50
5.1.1. Introducción a la estrategia de control . . . . . . . . . . . . . . . . . . . . 50
5.1.2. Diseño del control por realimentación del estado para la estabilización del
vehículo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
5.1.3. Transformación del control por realimentación del estado en un control
P-PD en cascada . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.1.4. Transformación del control por realimentación del estado en un control
PI-PD en cascada . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
5.1.5. Control de navegación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.1.6. Control para el seguimiento de pared . . . . . . . . . . . . . . . . . . . . . 56
5.2. Optimizador del Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.3. Máquina de estados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
5.3.1. Variables que determinan el comportamiento del vehículo equilibrista . . . 59
5.3.2. Funcionamiento de la máquina de estados . . . . . . . . . . . . . . . . . . 62
5.4. Estimador de estado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
5.4.1. Motivación para emplear un estimador de estado . . . . . . . . . . . . . . 63
5.4.2. Filtro complementario . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
5.4.3. Introducción al filtro de Kalman . . . . . . . . . . . . . . . . . . . . . . . . 64
5.4.4. Filtro extendido de Kalman (EKF) . . . . . . . . . . . . . . . . . . . . . . . 65
5.4.5. Diseño y optimización del EKF . . . . . . . . . . . . . . . . . . . . . . . . . 68
5.4.6. Integración con el sitema de control . . . . . . . . . . . . . . . . . . . . . . 68
5.5. Simulink para el control del vehículo . . . . . . . . . . . . . . . . . . . . . . . . . 69

XXII Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 Índice general

6. Resultados 73
6.1. Identificación de los parámetros de los motores . . . . . . . . . . . . . . . . . . . 74
6.1.1. Medida de la resistencia interna del motor . . . . . . . . . . . . . . . . . . 74
6.1.2. Medida de la relación Ganancia-Tensión-Incremento de encoder . . . . . . 74
6.2. Resultados de la identificación del movimiento de avance y cabeceo . . . . . . . . 78
6.3. Resultados de la identificación del movimiento de giro . . . . . . . . . . . . . . . 80
6.4. Resultados de la identificación del ruido . . . . . . . . . . . . . . . . . . . . . . . 81
6.5. Iteraciones del control del vehículo . . . . . . . . . . . . . . . . . . . . . . . . . . 82
6.5.1. Control P-PD para el avance y cabeceo . . . . . . . . . . . . . . . . . . . . 82
6.5.2. Control PI-PD para el avance y cabeceo . . . . . . . . . . . . . . . . . . . . 82
6.6. Controles optimizados mediante simulación . . . . . . . . . . . . . . . . . . . . . 83
6.6.1. Control P-PD para los movimientos de avance y cabeceo . . . . . . . . . . 83
6.6.2. Control PI-PD para los movimientos de avance y cabeceo . . . . . . . . . . 83
6.6.2.1. Caso 1: Velocidad de avance nula . . . . . . . . . . . . . . . . . . 83
6.6.2.2. Caso 2: Escalones en la referencia de velocidad de avance . . . . 85
6.6.3. Control de guiñada para “Navegación Libre” . . . . . . . . . . . . . . . . . 88
7. Conclusiones, aportaciones y futuros desarrollos 91
7.1. Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
7.2. Aportaciones y objetivos alcanzados . . . . . . . . . . . . . . . . . . . . . . . . . 92
7.3. Futuros desarrollos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
7.3.1. Optimización del sistema de control . . . . . . . . . . . . . . . . . . . . . . 93
7.3.2. Control para seguimiento de linea . . . . . . . . . . . . . . . . . . . . . . . 93
7.3.3. Navegación autónoma mediante cámaras . . . . . . . . . . . . . . . . . . . 93
7.3.4. Control para elevar al vehículo desde posición horizontal hasta la de equilibrio 93
7.3.5. Diseño 3D de un nuevo soporte para los sensores . . . . . . . . . . . . . . 94
A. Filtro de Kalman 95
A.1. Modelado en Espacio Estado Lineal . . . . . . . . . . . . . . . . . . . . . . . . . . 95
A.2. Filtro de Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96
A.3. Proceso de cálculo del Filtro de Kalman . . . . . . . . . . . . . . . . . . . . . . . 98
B. Configuración de la Raspberry Pi 3B+ 99
B.1. Raspi-Config para habilitar las comunicaciones . . . . . . . . . . . . . . . . . . . 99
B.1.1. Comunicación I2C a 400 kHz . . . . . . . . . . . . . . . . . . . . . . . . . 100
C. Librerías de Arduino 101
C.1. Librerías a instalar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
Referencias 103

Parte II. Presupuesto 107

Control de un Vehículo Equilibrista mediante una Raspberry Pi XXIII

Carmen Jiménez Cortés
 Índice de figuras
Figura 1. Balboa 32U4 with Raspberry Pi. . . . . . . . . . . . . . . . . . . . . . . . . X
Figura 2. Block diagram of the Simulink subsystems. . . . . . . . . . . . . . . . . . . XI
Figura 3. Actual measurement, the simulated one and the error for the pitch rate and
the forward speed. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . XII
Figura 4. State vector in simulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . XII
Figura 5. Balboa 32U4 con Raspberry Pi. . . . . . . . . . . . . . . . . . . . . . . . . . XVI
Figura 6. Esquema de los subsistemas que contiene el diagrama de Simulink. . . . . . XVII
Figura 7. Medida real, la generada por el simulador y el error entre ambas para la
velocidad angular de cabeceo y la velocidad lineal de avance. . . . . . . . . XVIII
Figura 8. Vector de estado en simulación. . . . . . . . . . . . . . . . . . . . . . . . . . XIX
Figura 3.1. Balboa 32U4 con Raspberry Pi. . . . . . . . . . . . . . . . . . . . . . . . . . 12
Figura 3.2. Balboa 32U4 con todos los elementos hardware empleados. . . . . . . . . . 13
Figura 3.3. Raspberry Pi 3B+. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
Figura 3.4. ATMega32U4. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
Figura 3.5. Unidad inercial de medidas. . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
Figura 3.6. Sistema de referencia del vehículo. . . . . . . . . . . . . . . . . . . . . . . . 19
Figura 3.7. Motor HPCB. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
Figura 3.8. Driver el motor. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
Figura 3.9. Curva de caracterización del motor 75:1 HPCB 6V. . . . . . . . . . . . . . . 22
Figura 3.10. "Timing"de las entradas y salidas del driver DRV8838. . . . . . . . . . . . . 22
Figura 3.11. Aplicación del driver DRV8838 a un motor de corriente continua. . . . . . . 23
Figura 3.12. Sensor de distancia VL6180X. . . . . . . . . . . . . . . . . . . . . . . . . . . 23
Figura 3.13. Multiplexador I2C TCA9548A. . . . . . . . . . . . . . . . . . . . . . . . . . 24
Figura 3.14. Imagen auxiliar del cálculo del ángulo y la distancia respecto a la pared. . . 25
Figura 3.15. Imagen de la disposición de los discos magnéticos en el motor. . . . . . . . 26
Figura 3.16. Lectura de los canales de los encoders. . . . . . . . . . . . . . . . . . . . . . 26
Figura 3.17. Incremento original de encoder. . . . . . . . . . . . . . . . . . . . . . . . . . 28
Figura 3.18. Incremento de encoder tras el primer filtrado no lineal. . . . . . . . . . . . . 29
Figura 3.19. Incremento definitivo de encoder. . . . . . . . . . . . . . . . . . . . . . . . . 30
Figura 3.20. Incremento definitivo de encoder sin acumular el incremento de segundo
orden. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
Figura 3.21. Incremento definitivo de encoder. . . . . . . . . . . . . . . . . . . . . . . . . 31
Figura 3.22. Velocidad común. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
Figura 3.23. Velocidad de guiñada. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
Figura 3.24. Separador para el soporte de los sensores. . . . . . . . . . . . . . . . . . . . 33

Control de un Vehículo Equilibrista mediante una Raspberry Pi XXV

Carmen Jiménez Cortés
Índice de figuras

Figura 3.25. Soporte de los sensores laterales. . . . . . . . . . . . . . . . . . . . . . . . . 33

Figura 4.1. Modelo electromecánico de un motor. . . . . . . . . . . . . . . . . . . . . . 37
Figura 4.2. Fuerzas y aceleraciones en un sistema de referencia solidario al vehículo . . 40
Figura 4.3. Representación del giro del vehículo alrededor de un eje vertical . . . . . . . 41
Figura 4.4. Diagrama de Simulink empleado en la identificación de los parámetros del
avance y cabeceo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
Figura 4.5. Diagrama de Simulink empleado en la identificación de los parámetros de giro. 46
Figura 5.1. Esquema teórico de la implantación del control integral por realimentación
del estado. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
Figura 5.2. Esquema ilustrativo de la implantación de los controles PID en cascada. . . 54
Figura 5.3. Relación entre las variables de estado y el mando. . . . . . . . . . . . . . . 54
Figura 5.4. Equivalencia entre ganancias del regulador y control P-PD en cascada. . . . 55
Figura 5.5. Equivalencia entre ganancias del regulador y control PI-PD en cascada. . . . 56
Figura 5.6. Circuito del laboratorio que debe completar el vehículo. . . . . . . . . . . . 57
Figura 5.7. Equivalencia entre ganancias del regulador y control P-PD en cascada para el
movimiento de giro. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
Figura 5.8. Máquina de estados. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
Figura 5.9. Estructura del diagrama de Simulink. . . . . . . . . . . . . . . . . . . . . . 70
Figura 5.10. Esquema de los subsistemas que contiene el diagrama de Simulink. . . . . . 71
Figura 6.1. Resultados Ganancia-Tensión para el motor izquierdo. . . . . . . . . . . . . 75
Figura 6.2. Recta de calibración Ganancia-Tensión para el motor izquierdo, ganancias
positivas. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
Figura 6.3. Recta de calibración Ganancia-Tensión para el motor izquierdo, ganancias
negativas. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
Figura 6.4. Resultados Ganancia-Tensión para el motor derecho. . . . . . . . . . . . . . 77
Figura 6.5. Recta de calibración Ganancia-Tensión para el motor derecho, ganancias
positivas. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
Figura 6.6. Recta de calibración Ganancia-Tensión para el motor derecho, ganancias
negativas. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
Figura 6.7. Medida real, la generada por el simulador y el error entre ambas para la
velocidad angular de cabeceo y velocidad lineal de avance. . . . . . . . . . 79
Figura 6.8. Medida real, la generada por el simulador y el error entre ambas para la
velocidad angular de guiñada. . . . . . . . . . . . . . . . . . . . . . . . . . 80
Figura 6.9. Ensayo para la identificación del ruido en la IMU. . . . . . . . . . . . . . . . 81
Figura 6.10. Ensayo para la identificación del ruido en los encoders. . . . . . . . . . . . . 81
Figura 6.11. Optimización PI-PD sin velocidad de avance. . . . . . . . . . . . . . . . . . 84
Figura 6.12. Optimización PI-PD con escalones en la referencia de la velocidad de avance. 85
Figura 6.13. Vector de estado en simulación. . . . . . . . . . . . . . . . . . . . . . . . . . 86
Figura 6.14. Velocidad de avance en simulación. . . . . . . . . . . . . . . . . . . . . . . . 87
Figura 6.15. Ángulo de cabeceo en simulación. . . . . . . . . . . . . . . . . . . . . . . . 87
Figura 6.16. Optimización PI con escalones en la referencia de la velocidad de guiñada. . 88
Figura 6.17. Optimización PI con escalones en la referencia de la velocidad de guiñada. . 89
Figura 7.1. Sistema de cámaras MCS empleado en drones. . . . . . . . . . . . . . . . . 94

XXVI Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 Siglas
CI Circuito Integrado

DPS Grados por segundo (Degrees per second)

EKF Filtro Extendido de Kalman (Extended Kalman Filter)

FET Transistor de efecto campo (Field Effect Transistor)

HPCB Motores de corriente continua con escobillas de carbono de alta potencia

HZ Hercio (Hertz)

IMU Unidad de Medida Inercial

IR Infrarrojos

LSB Byte menos singnificativo (Least Significant Byte)

MCS Cámaras de captura en movimiento (Motion Capture System)

MSB Byte más singnificativo (Most Significant Byte)

PID Proporcional-Integral-Derivativo

PWM Modulación por ancho de pulso

RPM Revoluciones por Minuto

XYZ Ejes de coordenadas

Control de un Vehículo Equilibrista mediante una Raspberry Pi XXVII

Carmen Jiménez Cortés
 PARTE I
ML
MEMORIA
DESCRIPTIVA
 1
Introducción

En este capítulo de introducción se presenta el proyecto fin de grado que lleva por
título Control de un vehículo equilibrista mediante una Raspberry Pi. En la Sección 1.1 se
exponen los motivos por los cuales se decidió realizar este proyecto. A continuación, en
la Sección 1.2, se plantean cuáles han sido los objetivos fundamentales del proyecto y
en qué ha consistido cada uno de ellos. Posteriormente en la Sección 1.4 se describen
cuáles han sido las tareas para llegar a alcanzar dichos objetivos. En la Sección 1.3 se
expresan qué recursos hardware y software han sido utilizados a lo largo del proyecto,
tanto para constituir el vehículo como para programar el código que se ejecuta en los
dos microcontroladores disponibles.

1.1. Motivación
Las motivaciones para realizar este trabajo de fin de grado son ampliar los conocimientos
adquiridos en asignturas previas, participar en un proyecto totalmente multidisciplinar y
proporcionar una nueva herramienta que pueda ser empleada en los laboratorios de asignatura
del área de control en la universidad.

El vehículo equilibrista es un ejemplo más del problema clásico de control conocido como
Problema del péndulo invertido. Este problema consiste en mantener la verticalidad de un objeto
en una posición de equilibrio inestable. Por equilibrio inestable se entiende aquella situación
en la que si cualquier perturbación desplaza al objeto de dicha posición de equilibrio, este es
incapaz de recuperar dicha posición salvo que haya alguna actuación externa.

Para este trabajo de fin de grado se ha elegido como planta un vehículo equilibrista, que
mediante la acción de los motores debe ser capaz de corregir la inclinación del vehículo para que
permanezca en posición vertical. El vehículo elegido es el Balboa32U4 de la empresa americana
Pololu. Este vehículo no se había utilizado antes en la universidad, lo que suponía un desafio
atractivo.

Además de las asignaturas de Control Digital, Regulación Automática y Sistemas Dinámicos,

que son el pilar de este proyecto, también ha sido necesario emplear las herramientas adquiridas

Control de un Vehículo Equilibrista mediante una Raspberry Pi 1

Carmen Jiménez Cortés
Capítulo 1. Introducción

en otras asignaturas cursadas a lo largo de estos cuatro años, entre las que se podrían destacar
las siguientes:

Microcontroladores y Electrónica Digital: Se han programado dos microcontroladores

distintos, en dos lenguajes diferentes de programación y se han integrado para que
trabajen de manera conjunta. Estos microcontroladores son una Raspberry Pi 3B+,
programada mediante Matlab-Simulink, y un ATMega32U4, programado con Arduino.

Mecánica, Ecuaciones Diferenciales y Cálculo: En el proceso de optimización del control,

del Filtro Extendido de Kalman y de poder llevar a cabo simulaciones, ha sido necesario
obtener un modelo matemático de la planta. Para ello, se han requerido conocimientos
de Mecánica, que permitieran comprender las dinámicas del vehículo, conocimientos de
Sistemas Dinámicos para poder modelar el comportamiento de los motores. Finalmente
estas dinámicas se expresaron en una serie de ecuaciones diferenciales que constituyen el
modelo matemático del vehículo.

Electrónica de Potencia y Circuitos Microelectrónicos: Para poder obtener una respuesta

satisfactoria del vehículo es indispensable conocer todos los elementos que lo componen.
Esto abarca desde conocer el driver utilizado para controlar los motores de corriente
continua, hasta el diseño de filtros que mejoraran la calidad de las medidas de los
sensores.

Finalmente, una motivación adicional para llevar a cabo este proyecto fue el poder
proporcionar una nueva herramienta para los laboratorios de las asignaturas de Regulación
Automática, Control Digital y Control Avanzado, e incluso servir de punto de partida para
futuros trabajos de fin de grado y máster.

Actualmente, el vehículo que se emplea en los laboratorios de dichas asignaturas está

construido con el kit de Lego Mindstorm, que dejó de recibir soporte de Matlab-Simulink
en 2015, mientras que los recursos para Raspberry Pi no dejan de aumentar en cada nueva
actualización. Además, la Raspberry Pi es un microcontrolador de gran potencia y orientada a la
docencia de mecatrónica, programación e informática, por lo que puede emplearse en proyectos
que demanden de una mayor carga computacional.

1.2. Objetivos
Los tres grandes objetivos planteados para este proyecto han sido:

Identificación del modelo de la planta y configuración del hardware

Partiendo de la información proporcionada en las fichas técnicas del vehículo y los

componenetes hardware adicionales, se obtuvieron la mayoría de los parámetros necesarios
para construir el modelo, tales como las dimensiones del vehículo, la separación entre sus ruedas
o las características eléctricas de los motores, y los datos necesarios para poder interactuar con
los sensores y actuadores.

Al tratarse de un vehículo equilibrista que no se había utilizado con anterioridad, no existía

ninguno de los drivers que permitieran utilizar los elementos hardware que lo componen, por lo
que fue necesario programarlos todos en Matlab-Simulink.

A partir de la teoría de Sistemas Dinámicos se modelaron las ecuaciones diferenciales

que recogieran el comportamiento mecánico del vehículo. Algunos de los parámetros que

2 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 1.3. Recursos

intervienen en dichas ecuaciones no aparecen en las fichas técnicas de los componentes ni del
vehículo, por lo que fue necesario llevar a cabo diferentes ensayos que permitieran caracterizar
completamente a este. Tanto las ecuaciones como los ensayos realizados se encuentran recogidos
en el Capítulo 4 de este documento.

Diseño del sistema de control

Este segundo objetivo aúna el diseño de todos los elementos, que junto con la planta, forman
el lazo de control.

El diseño del control empleado se basa en la realimentación del estado, que posteriormente
se transforma en dos controles PID en cascada para su implantación, como se explica en el
Capítulo 5.

Como estimador de estado, se ha empleado un Filtro Extendido de Kalman, o EKF, ya que es

la mejor herramienta para la estimación de variables en sistemas de navegación autónomos.
Dentro del Capítulo 5, se dedica la Sección 5.4 a explicar someramente en qué consiste la teoría
del filtro de Kalman y cómo se ha integrado con el sistema de control.

Finalmente, otro elemento fundamental en el sistema de control es la máquina de estados,

que se encarga de controlar que todos los procesos se lleven a cabo de manera ordenada,
habilitando y deshabilitando los diferentes elementos del archivo de Matlab-Simulink que
sean necesarios en cada momento de la ejecución. La Sección 5.3 del Capítulo 5 detalla su
funcionamiento.

Implantación del control

Una vez diseñado el lazo de control, ha sido necesario realizar numerosos ensayos que
permitieran analizar las discrepancias entre el comportamiento real y simulado del vehículo,
para así corregir y mejorar el modelo, o modificar las dinámicas en lazo cerrado fijadas en el
control para su optimización. Los ensayos y los resultados obtenidos se encuentran recogidos
en el Capítulo 6.

1.3. Recursos
Al tratarse de un proyecto de mecatrónica emplea numerosos elementos hardware, que
necesitan ser integrados con el software que permita su utilización y la implantación de la lógica
de control.

Recursos hardware

Debido a la complejidad de la planta del vehículo equilibrista han sido necesarios numerosos
elementos hardware que permitieran controlar el vehículo de la forma deseada. Estos recursos
han sido, fundamentalmente, los siguientes:

Raspberry Pi 3B+: Microcontrolador encargado de las tareas de alto nivel y de alta

demanda computacional. Su uso se detalla en la Sección 3.2 del Capítulo 3.

ATMega32U4: Microcontrolador responsable de las tareas de bajo nivel, que consisten

básicamente en interacturar con sensores y actuadores. Se describe en la Sección 3.3.

La unidad de medidas inerciales, o IMU (Inertial Measurement Unit). En este proyecto se

utiliza el circuito integrado LSM6DS33, que consiste en un giróscopo y acelerómetro de 3
ejes, Sección 3.4. Este CI se encuentra ubicado en la propia placa del ATMega32U4.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 3

Carmen Jiménez Cortés
Capítulo 1. Introducción

Dos motores 75:1 Micro Metal Gear Motor HPCB 6V, Sección 3.5. Básicamente son motores
de corriente continua con escobillas de carbono. En la Sección 3.5.2 se describe el
funcionamiento del driver de estos motores.

Dos sensores de distancia VL6180X, Sección 3.6, que permiten medir distancias a objetos
ubicados entre 5 y 200 milímetros.

Multiplexador de canales I2C TCA9548A, necesario para poder leer los dos sensores de
distancia, ya que tienen la misma dirección en el bus de I2C. El uso de este multiplexador
se detalla en la Sección 3.6.

Dos encoders magnéticos, o codificadores de pulsos incrementales, Sección 3.7, con sus
respectivos sensores de efecto Hall. Se trata de dos encoders absolutos, con dos canales
ubicados en el eje de giro de los motores, de tal forma que permiten determinar así la
velociad y sentido de giro de cada motor.

Piezas auxiliares fabricadas mediante impresión 3D. Para poder sujetar los sensores de
distancia en el lateral del vehículo, fue necesario fabricar un separador y un soporte, que
permitieran conectar ambos sensores al multiplexador de I2C y este a la extensión de
placa de la Raspberry Pi.

Recursos software

Las tres herramientas fundamentales de programación han sido:

Matlab-Simulink: Donde se han programado todos los scripts de configuración de los

sensores y actuadores, scripts de la lógica de control, del EKF, de la máquina de estados, etc.
Y donde se ha diseñado también el diagrama de Simulink que se vuelca en la Raspberry
Pi para que el vehículo funcione de forma autónoma.

Arduino IDE: Se ha empleado para programar la estructura que intercambian, a través del
bus I2C, la Raspberry Pi y el ATMega32U4. En la Sección 3.3.2 se explican los diferentes
campos que componen la estructura.

SolidWorks: Para el diseño de las piezas en 3D y la obtención los ficheros .STL para su
posterior impresión.

Recursos adicionales

Además de los recursos hardware y software propiamente empleados en el control del

vehículo, también han sido necesarias numerosas fuentes de documentación:

Para obtener todos los parámetros físicos del vehículo y poder utilizar el resto de sensores
y actuadores, se han requerido todas las fichas técnicas disponibles del vehículo y los
componentes.

Finalmente, para conocer el estado del arte del problema de control del Péndulo Invertido,
Capítulo 2, se han utilizado numerosos documentos de IEEE, revistas de Regulación
Automática y Control de Sistemas, presentaciones en conferencias, y publicaciones de
otras universidades, que se entuentran el la Bibliografía de este documento.

4 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 1.4. Metodología

1.4. Metodología
Dada la extensión del proyecto, para satisfacer todos los objetivos planteados en la
Sección 1.2, ha sido necesario tener una planificación clara y ordenada desde el primer momento.
Aún así fue necesario reajustarla conforme iban apareciendo inconvenientes y dificultades.

Modelo del vehículo

La tarea más extensa ha sido la obtención del modelo de la planta, pues se comenzó con
ello en los primeros días del proyecto y hasta el último momento, no se ha dejado de mejorar y
perfeccionar dicho modelo. Durante el primer mes se reunió toda la documetación disponible
tanto del vehículo como de los sensores y los actuadores, para poder empezar a caracterizar
parámetros clave del modelo. En los meses siguientes se comenzaron a realizar ensayos para
obtener los parámetros pendientes de identificar, empezando por ensayos a elementos hardware
aislados, como los necesarios para obtener las relaciones tensión-velocidad de giro o ganancia
aplicada-tensión de los motores, hasta que finalmente se pudieron realizar pruebas con el
vehículo en equilibrio para conseguir los valores de las inercias en los tres ejes y los valores de
las fricciones viscosas. Todos estos ensayos se encuentran recogidos en el Capítulo 6.

Integración del hardware en una única planta

Una vez caracterizados los elementos hardware que componían el vehículo, se pasó a
integrarlos y caracterizar los parámetros del robot completo, como las inercias del cuerpo en
los ejes Y y Z. Para obtener datos que permitieran conocer algunos de estos parámetros, era
necesario mantener al vehículo en equilibrio y registrar las medidas de la IMU y los encoders.
Por lo tanto, se comenzó a diseñar la máquina de estados, que controla las tareas que deben
llevarse a cabo en cada momento, desde el encendido del robot hasta que este se deja libre en
equilibrio.

Primera aproximación al sistema de control

Como en esta etapa del proyecto aún no se disponía de un modelo lo suficientemente preciso
como para poder utilizarlo para simular y optimizar el control, se recuperaron los archivos
utilizados durante el laboratorio de Control Digital, se estudiaron los valores de las ganancias
del regulador que habían dado un buen resultado con el vehículo de Lego y por observación y
comparación se dedujeron unas primeras ganancias que pudieran funcionar para el Balboa32U4.
Evidentemente este control inferido a partir de otro robot tan distinto, no era adecuado, pero
proporcionó un punto de partida para reajustar dichas ganancias mediante ensayos hasta que el
vehículo fuera capaz de equilibrarse, tanto con velocidad de avance nula o constante.

Una vez se consiguió tener el vehículo equilibrado, se pudieron registrar los últimos ensayos
necesarios para identificar la dinámica del vehículo por completo.

Diseño del estimador de estado

Mientras se seguía ampliando y mejorando el modelo con la nueva información, se trató de

mejorar la respuesta del control con ayuda del un estimador de estado, y atenuar así el ruido
presente en las medidas de los sensores analógicos. En un primer momento se empleó un filtro
complementario, el cual se encuentra descrito en la Sección 5.4.2.

Posteriormente, se optó por una herramienta más sofisticada para la estimación del estado:
un filtro extendido de Kalman, Sección 5.4.4. La implementación del EKF se realizó de
forma escalonada, ya que los pesos de la matriz de corrección y los ruidos eran inicialmente

Control de un Vehículo Equilibrista mediante una Raspberry Pi 5

Carmen Jiménez Cortés
Capítulo 1. Introducción

desconocidos. En primer lugar se ajustaron el ángulo y la velocidad angular de cabeceo. Se

empezó con unos valores genéricos y mientras el vehículo se equilibraba se monitorizaron
las medidas de la IMU y las generadas por el EKF, modificando los pesos hasta que ambas
medidas coincidían, siendo la del EKF menos ruidosa. De la misma forma se fueron incluyendo
también las estimaciones de la velocidad de avance, del ángulo de guiñada y de la velocidad de
guiñada.

Control de navegación y seguimiento de pared

Otro de los grandes objetivos de este proyecto era conseguir que el vehículo completara una
vuelta a uno de los circuitos disponibles en los laboratorios de la universidad, lo que planteaba
nuevas dificultades: el vehículo debía ser capaz de seguir una referencia de distancia y ángulo
respecto de la pared, para poder seguir la pared, y además debía ser capaz de subir y bajar las
rampas de forma equilibrada.

Como paso previo, fue necesario diseñar un control de navegación. Este control permite que
el vehículo se desplace en línea recta o gire siguiendo una determinada referencia de guiñada.
Se diseñaron dos controles:

Control P: Pese a tener error de seguimiento, proporcionaba un punto de partida para el

diseño del control de guiñada del vehículo.

Control PI: Para garantizar error de seguimiento a referencia nulo en régimen permanente.

Para que el vehículo siguiera la pared, primero se instalaron los dos sensores de distancia en
el lateral izquierdo del vehículo. Esto supuso tener que diseñar y fabricar las piezas en 3D que
sujetaban los sensores al lateral.

Una vez instalados los sensores, se amplió el diagrama de simulink, la máquina de estados
y la lógica de control para que a partir de sus medidas, se estimara la distancia y el ángulo
respecto de la pared. De nuevo se utilizó un control por realimentación del estado, que se
transformó en dos PIDs en cascada para su implementación. En esta ocasión se ha utilizado un
control P-PD en lugar de un PI-PD porque cuando el vehículo toma las curvas, es conveniente
que se pegue algo más a la pared que cuando baja por la rampa.

Finalmente, en las últimas semanas del proyecto, se programaron herramientas de

optimización por mínimos cuadrados que permitieron optimizar todos los controles y el EKF. Los
resultados de esta última optimización se encuentran también recogidos en el Capítulo 6.

Antes de finalizar esta sección de metodología, cabe mencionar que una parte no despreciable
de la duración de este proyecto se ha invertido en desarrollar esta memoria descriptiva y los
anexos previos que se tuvieron que entregar.

6 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 2
Estado del Arte

A lo largo de este capítulo se describe el estado del arte del proyecto. En primer lugar,
en la Sección 2.1, se introduce el problema de control, diferentes sistemas en los que se
plantea y una breve reflexión acerca de su importancia dentro de la teoría de control.

A continuación, en la Sección 2.2, se presentan diferentes soluciones que se han

encontrado para intentar controlar un péndulo invertido, distintas a la propuesta en este
proyecto.

Finalmente, en la Sección 2.3 se muestran otras posibles aplicaciones del Filtro Extendido
de Kalman dentro de la navegación autónoma de vehículos.

2.1. El Péndulo Invertido

El control de la verticalidad de un vehículo equilibrista constistuye un ejemplo más del
problema clásico de control conocido como “Péndulo Invertido”.

Este problema consiste en controlar un sistema dinámico inestable que tiene una posición
estable, cuando el péndulo se ha caído o se encuentra en posición horizontal, y una posición
de equilibrio inestable, que es la que se desea mantener, cuando el péndulo se encuentra
en vertical. Por equilibrio inestable se quiere expresar aquella situación en la que si una
perturbación desplazara al cuerpo, este saldría de dicha posición de equilibrio y no sería capaz
de recuperarla salvo que se actuara externamente.

Este problema se presenta en plantas muy diversas, desde una simple varilla sujeta por uno
de sus extremos [1], o la propia acción de caminar de un humano, hasta plantas mucho más
complejas, como triples péndulos, que suponen controlar tres brazos articulados entre sí [2], o
también en aplicaciones industriales, como en el equilibrado de un cohete durante la fase de
lanzamiento, en el cual mediante las toberas laterales se corrigen las posibles desviaciones en el
ángulo.

Dentro del sector docente, la empresa americana Quanser, líder mundial en equipamiento
para laboratorios de control, mecatrónica y robótica oferta dos péndulos invertidos distintos.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 7

Carmen Jiménez Cortés
Capítulo 2. Estado del Arte

El primero se trata de una varilla sujeta sobre un carro, que se mueve mediante un motor de
corriente continua y un sistema de piñón-cremallera, usando exclusivamente el movimiento de
traslación para equilibrarlo, y el segundo consiste en cambio, en un servo rotatorio al cual se le
ha acoplado un eje y mediante movimientos de rotación trata de mantenerlo en equilibrio. Los
sensores fundamentales empleados en ambos sistemas son encoders, que permiten controlar
tanto la rotación de la varilla como el avance del carro o la rotación del servo.

Pese a todo ello, la planta más extendida para el estudio del problema del Péndulo Invertido
actualmente consiste en vehículos equilibristas, como el planteado en el libro [3], donde
se recogen todas las posibles estrategias de control que podrían ser útiles para este tipo de
vehículos en exclusivo.

En [4] se pone de manifiesto por qué pese a ser un problema tan ampliamente estudiado,
el control de un péndulo invertido sigue actualmente siendo un objetivo desafiante para
investigadores y educadores. Y es que permite ilustrar grandes retos vigentes en la teoría de
control, tales como:

Conseguir sacar un cuerpo de su posición de equilibrio estable, para llevarlo a una posición
de equilibrio inestable. La transición entre estas dos posiciones no es linealizable, por
lo que la matemática requerida para su modelado dificulta notablemente el diseño del
control.

Conseguir mantener el equilibrio inestable del péndulo, que permite explicar a estudiantes
universitarios en qué consiste la linealización de sistemas inestables en torno a un punto
de operación y el control por posicionamiento de polos.

Conseguir seguir una referencia de velocidad o posición frente a perturbaciones

estocásticas debidas al movimiento de balanceo del péndulo para mantener su verticalidad.

Integrar observadores en el lazo de control, que en sistemas no lineales es muy necesario,

ya que existen variables de estado a controlar que no son medibles, como por ejemplo el
ángulo de cabeceo. Además, permite demostrar la teoría de separación, que expresa que
el control y el observador pueden ser diseñados de forma independiente.

Gracias al problema del péndulo invertido, en robótica se han conseguido grandes avances
en los últimos años: El desarrollo de vehículos autoequilibrados y el modelado y control de la
acción de caminar en humanoides.

Vehículos autoequilibrados

Entre los cuales cabe destacar el famoso “Segway”, que plantea una nueva alternativa de
movilidad en las ciudades, y también un vehículo auxiliar para los cuerpos de seguridad en
interiores.

También son conocidos vehículos equilibristas como el JOE [5], desarrollado por la
universidad de Lausanne en Suiza o el I-PENTAR [6], que es además un pseudo asistente
humanoide.

Humanoides

Basándose en los principios empleados para mantener en equilibrio péndulos invertidos y

relacionándolos con las mecánicas que intervienen en la acción de caminar de los humanos,
ha sido posible desarrollar modelos tridimensionales de péndulos invertidos, denominados
LIPM (linear inverted-pendulum models), [7]. Algunos de los humanoides más reconocidos

8 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 2.2. Soluciones al problema de control

que responden dinámicamente a estos modelos son el WABOT-1, que fue el primer humanoide
existente, desarrollado por la universidad de Waseda en 1973, la Androide HRP-4C de la
empresa japonesa Honda [8], o el WABIAN-2R [9], desarrollado también por la universidad de
Waseda y que se trata del humanoide más complejo diseñado hasta la fecha.

2.2. Soluciones al problema de control

En este trabajo de fin de grado, se ha optado por modelar el vehículo en el espacio de estado,
para así utilizar estrategias de control por realimentación del estado, que posteriormente se
transformaban en dos PIDs en cascada, como se explica en el Capítulo 5. También plantea esta
solución [2] para su control del tripe péndulo invertido.

El control por realimentación del estado no es la única solución posible al problema, desde
aproximadamente 2010 hasta la actualidad la mayoría de las publicaciones se centran en las
siguientes dos corrientes: el uso de redes neuronales en el lazo de control junto con controladores
clásicos, tales como el PID, y sobre todo, el diseño de controles de lógica difusa. Esta tendencia
dentro de las estrategias de control es un claro ejemplo de la línea evolutiva de la investigación
tecnológica de los últimos años, que busca soluciones basadas en Machine Learning, Inteligencia
Artificial, etc., para conseguir sistemas cada vez más robustos y autónomos.

Los controles de lógica difusa [10] suponen un acercamiento al problema completamente

distinto al aquí planteado. Esta estrategia tiene como principal objetivo controlar procesos
complejos mediante las normas de la experiencia humana. Mientras que los sistemas de control
convencionales (no difusos) se centran en problemas que requieren de la elaboración de un
modelo, lo cual consume tiempo y es susceptible a numerosos errores, bien por las sucesivas
simplificaciones que supone la abstracción de la realidad para elaborar el modelo, como por
los ruidos en las medidas de los sensores a emplear o las dificultades para identificar todos los
parámetros que intervienen. Los modelos difusos, en cambio, pretenden prescindir de modelos
y actuar sobre el entorno a partir de los cálculos heurísticos llevados a acabo en el sistema
de control, utilizando como entradas variables del medio físico (modificado por el sistema de
control) tomadas mediante sensores.

De entre los numerosos ejemplos de la aplicación de esta estrategia de control para el

problema del péndulo invertido cabe destacar dos aplicaciones completamente distintas entre
sí: en [11] se utilizaron tres controladores distintos para controlar un péndulo invertido de
dos ruedas: Un control difuso de estabilización, un control difuso de navegación y posición y
un último control difuso de guiñada. Operando de esta forma, el control de estabilización se
puede resolver mediante la aproximación del problema a un sistema matricial de inecuaciones.
La otra aplicación [12], propone utilizar un control adaptativo de lógica difusa para evitar las
dinámicas inestables que aparecen al tratar el problema del péndulo invertido como un sistema
de segundo orden cuando se linealiza. Se añadieron un término amortiguado y un control
difuso para asegurar la estabilidad asintótica incluso en presencia de perturbaciones.

Entre el uso de redes neuronales destacan estudios como el de [13]. Que en lugar de invertir
tiempo y recursos en conocer en profundidad las dinámicas de la planta, para implantar un
controlador PID convencional, proponen utilizar una red neuronal que controle un sistema de
única entrada y múltiples salidas para aplicarlo a la estabilización del péndulo invertido y así
prescindir del modelo del vehículo.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 9

Carmen Jiménez Cortés
Capítulo 2. Estado del Arte

También se han utilizado redes neuronales para compensar el desconocimiento e incertidum-

bres de la dinámica de la planta en [14]. Como controlador en este estudio, de nuevo se optó
por un PID.

Se pueden encontrar trabajos basados en el modelo predictivo grey para controlar el péndulo
[15]. La teoría del control predictivo grey se emplea para mejorar la labor del controlador en
sistemas sin la suficiente información o con propiedades altamente no lineales. El objetivo de
esta investigación es mediante la combinación del modelo predictivo grey y un controlador
Proporcional-Derivativo (PD), conseguir elevar el péndulo desde su posición estable (vertical
hacia abajo) hasta la posición vertical invertida.

Otro acercamiento a este problema, que dista mucho de los anteriormente planteados
y del elegido para este proyecto de fin de grado, es [16] quienes decidieron controlar un
doble péndulo invertido mediante las técnicas de transformación lineal fraccionaria y técnicas
politópicas, que son parte de los denominados controles por planificación de ganancias (Linear
Parameter- Varying control, LPV). Este tipo de controles constituyen una de las formas más
simples e intuitivas de control adaptativo.

2.3. El EKF y la navegación autónoma

Por todos los estudios previamente expuestos podemos observar como no parece haber
una única solución óptima para este problema de control. Lo que si podemos concluir es que
es indispensable combinar la acción de un controlador con herramientas más complejas que
aporten una mayor robustez, bien porque permitan una estimación más precisa de las variables
(redes neuronales), alternar entre diferentes controladores (controles adaptativos), o inferir los
parámetros del control más adecuados mediante decisiones heurísticas (lógica difusa). En este
trabajo de fin de grado se ha optado por la utilización de un controlador por realimentación de
estados junto con un filtro extendido de Kalman (EKF).

El uso del EKF está altamente extendido en proyectos de navegación autónoma. En [17] se
utilizó junto con las técnicas SLAM (Simultaneous Localization and Mapping) para el control
de la navegación autónoma de un coche. En este proyecto no se conoce con anterioridad el
mapa del recinto por el que se va a navegar, por lo que el EKF estima puntos de referencia y
localizaciones seguras a medida que el vehículo se desplaza.

Otro ejemplo sería [18], donde se empleó un EKF para la navegación autónoma de un camión
de carga y descarga, bajo tierra. El EKF se encargaba de estimar el movimiento cinemático del
vehículo, incluyendo el deslizamiento con el suelo, a partir de las medidas de un sensor láser
ubicado en el cojinete y los vértices del mapa de polilínea del terreno.

10 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3
Hardware

En este capítulo se describen cuáles han sido los elementos hardware empleados en el
proyecto, su funcionalidad y cómo se utilizan en el vehículo.

En primer lugar, en la Sección 3.1 se realiza una breve introducción al robot y a todos
los elementos hardware que lo componen. A continuación, se explican en detalle
el funcionamiento de los dos microcontroladores, comenzando por la Raspberry Pi,
Sección 3.2, que es la tarjeta encargada de las tareas de mayor carga computacional,
tales como la gestión de las comunicaciones y la lógica de control. Después se describe el
ATMega32U4, Sección 3.3, cuya función consiste en interactuar con los sensores y los
actuadores.

Posteriormente, se describen el resto de elementos hardware:

La IMU, Sección 3.4, proporciona las medidas de las aceleraciones lineales y las
velocidades angulares necesarias para el control.

Los motores, Sección 3.5, su driver y la aplicación de los valores del mando
calculados por el control.

Los sensores laterales, Sección 3.6, y cómo estimar a partir de sus medidas la
distancia a la pared y el ángulo respecto de la misma.

Los encoders, Sección 3.7, responsables del cálculo de la velocidad de avance y la

guiñada del vehículo. De esta última sección se ha dedicado gran parte a explicar el
proceso de filtrado empleado, ya que al tratarse de encoders magnéticos, la medida
presentaba numerosas irregularidades.

Finalmente, en la Sección 3.8, se muestran las piezas que se fabricaron mediante

impresión 3D, para la sujección de los sensores de distancia en el lateral del vehículo.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 11

Carmen Jiménez Cortés
Capítulo 3. Hardware

3.1. Descripción del vehículo

El Balboa32U4 es un vehículo equilibrista de pequeño tamaño, Figura 3.1, formado, desde
el punto de vista electrónico, por dos microcontroladores: un ATMega32U4 y una Raspberry
Pi 3B+. El ATMega se encarga de las tareas de bajo nivel, tales como la lectura de sensores
y escritura en los actuadores. La Raspberry en cambio, se encarga de las tareas de alto nivel,
como la gestión de las comunicaciones, la integración de todos los elementos hardware, y la
lógica de control.

Figura 3.1. Balboa 32U4 con Raspberry Pi.

Adicionalmente, se ha soldado una extensión de placa que permite conectar dos sensores
laterales, indispensables para que el vehículo sea capaz de seguir la pared. Los sensores se
encuentran sujetos al lateral del vehículo. El aspecto del vehículo una vez se han incorporado
todos los elementos adicionales se muestra en la Figura 3.2.

12 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3.1. Descripción del vehículo

Figura 3.2. Balboa 32U4 con todos los elementos hardware empleados.

Los sensores que incorpora el robot son:

Una unidad de medida inercial (IMU), formada por un giróscopo y un acelerómetro de 3

ejes. Ambos se encuentran en el circuito integrado LSM6DS33, Figura 3.5. El giróscopo
mide las velocidades angulares en cada uno de los tres ejes, y el acelerómetro la aceleración
lineal del cuerpo junto con la proyección de la gravedad en cada eje.

Dos encoders magnéticos, ubicados en los ejes de los motores, Figura 3.15. Estos encoders
permiten medir la velocidad y sentido en el que giran los motores, para posteriormente
estimar la velocidad de avance y la velocidad de guiñada del vehículo.

Dos sensores de distancia VL6180X, Figura 3.12, que devuelven la distancia en milímetros
en un rango de 5 a 200 milímetros. Gracias a esta información se puede calcular tanto la
distancia a la pared del circuito como el ángulo respecto de la misma.

Tres pulsadores ubicados en la parte inferior de la placa ATMega, como se puede ver en la
Figura 3.4. Estos pulsadores se emplean como señales digitales de entrada en la máquina
de estados.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 13

Carmen Jiménez Cortés
Capítulo 3. Hardware

Los actuadores y señalizadores disponibles son:

Dos motores de corriente continua con escobillas de carbono ("High Power Long Life
Carbon Brushes", HPCB) de 6 V, Figura 3.7.

Tres leds: amarillo, verde y rojo. Ubicados también en el ATMega, Figura 3.4.

Un buzzer: Que se gestiona directamente en el programa de Arduino para indicar que se

ha volcado correctamente el programa en el ATMega32U4.

Las comunicaciones entre los dos microcontroladores, los sensores y los actuadores se realiza
a través del bus de comunicaciones I2C, siendo la Raspberry Pi el máster de la comunicación, y
todo el resto de elementos esclavos I2C.

3.2. Raspberry Pi 3B+

Una Raspberry Pi es el nombre que reciben una serie de ordenadores de placa única creados
por Raspberry Pi Foundation, una organización benéfica originaria de Reino Unido, cuyo objetivo
es favorecer el acceso a la educación en informática.

Cabe destacar que la Raspberry Pi aunque comúnmente es reconocida como microcontrolador

en robótica, es un ordenador en sí mismo, ya que contiene un chip Broadcom BCM2837, que
integra la CPU, GPU, DSP, SDRAM y el puerto USB.

Desde 2012, cuando la primera Raspberry Pi salió al mercado con un único núcleo, 700 MHz
de CPU y 256 MB de RAM, hasta hoy en día el dispositivo ha evolucionado exponencialmente,
y es por ello que se pueden encontrar en todo tipo de proyectos de electrónica, automática y
domótica, tanto a nivel docente como industrial.

En este proyecto se emplea el último modelo disponible: la Raspberry Pi 3B+, Figura 3.3,
que cuenta con un procesador de cuatro núcleos de 64 bits, 1.4 GHz de CPU y 1GByte de
memoria SDRAM. Estas especificaciones, sus prestaciones de conectividad de red, y su pequeño
tamaño (85.60mm ⇥ 53.98mm), convierten a la Raspberry Pi 3B+ en el microcontrolador ideal
para soportar la gran parte de la carga computacional del proyecto.

Figura 3.3. Raspberry Pi 3B+.

14 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3.3. ATMega32U4

Las tareas de la Raspberry Pi consisten en:

Máster en la comunicación I2C con el ATMega32U4 y la IMU

Carga computacional de la lógica de control

Integración de los diferentes elementos Hardware

Comunicaciones con la estación base

El código de todas ellas se encuentra recogido en diferentes scripts de Matlab, que se integran en
un mismo Simulink, el cual es volcado en la Raspberry Pi, donde se ejecutará constantemente
hasta que el usuario decida finalizar el proceso. En el proceso de compilado del diagrama de
Simulink se genera el código C que interpretará la Raspberry Pi.

3.3. ATMega32U4
3.3.1. Descripción
El microcontrolador ATMega32U4 es un microcontrolador de la familia AVR, de la empresa
americana Atmel. La línea ATmega se caracteriza por tener una memoria flash programable,
entre 28 y 100 pines y numerosos conjuntos de instrucciones y periféricos. Concretamente, el
ATMega32U4, Figura 3.4, empleado en este proyecto dispone de 32 KBytes de memoria flash y
2.5 KBytes de RAM a 16 Mhz. Por ello, al tratarse de un dispositivo de menor capacidad que la
Raspberry Pi, se le han asignado las tareas más simples:

Interacción con los los encoders, Figura 3.15, aunque el procesamiento de su información
se realiza en la Raspberry Pi.

También se encarga de la lectura del estado de los pulsadores, llamados A, B y C, ubicados

en la parte inferior de la placa.

Escritura de la tensión en cada uno de los motores.

Figura 3.4. ATMega32U4.

Estas tareas se programan en Arduino y se ejecutan periódicamente en el microcontrolador

ATMega, con una frecuencia de 100 Hz, por lo tanto, el tiempo de muestreo empleado por
este microcontrolador es de diez milisegundos. Todas las funciones de Arduino empleadas

Control de un Vehículo Equilibrista mediante una Raspberry Pi 15

Carmen Jiménez Cortés
Capítulo 3. Hardware

pertenecen a dos librerías propias del fabricante, Pololu, que han sido expresamente diseñadas
para el Balboa 32U4. Estas librerías son:

Balboa32U4.h: Recoge las funciones necesarias para poder interacturar con el hardware
implementado en la tarjeta del Balboa.

PololuRpiSlave.h: Contiene las funciones encargadas de establecer al ATMega como esclavo

I2C de la Raspberry Pi.

El programa consiste en definir, escribir y leer una estructura que recoge toda la información
necesaria para interacturar con el resto de elementos hardware del vehículo. Los campos que se
han incluido en esta estructura son:

Batería en mV: Campo de tipo int16 escrito por el ATMega, que registra el valor en tiempo
real de la tensión proporcionada por las seis baterías que utiliza el robot. Es un dato de
gran importancia ya que al disponer de tantos elementos hardware, este proyecto tiene
un consumo bastante elevado. Permite así disponer de una alarma por batería baja, que
detenga la operación del vehículo equilibrista.

Cuentas del encoder izquierdo: Elemento de tipo int16 escrito por el ATMega. Almacena
el valor absoluto de las cuentas del encoder izquierdo. El proceso de lectura del mismo se
explica en detalle en la Sección 3.7.

Cuentas del encoder derecho: Elemento de tipo int16 escrito por el ATMega. Almacena
el valor absoluto de las cuentas del encoder derecho. El proceso de lectura del mismo se
explica en detalle en la Sección 3.7.

Error de lectura del encoder izquierdo: Dato de tipo boolean escrito por el ATMega,
habitualmente denominado "Flag". Se pone a uno cuando se ha producido algún error
en la medida del encoder izquierdo, como por ejemplo, que ambos canales del encoder,
Figura 3.16, hayan cambiado de valor a la vez y por tanto no es posible determinar en
qué dirección se encuentra girando el motor.

Error de lectura del encoder derecho: Dato de tipo boolean escrito por el ATMega, habi-
tualmente denominado "Flag". Se pone a uno cuando se ha producido algún error en la
medida del encoder derecho.

Pulsadores A, B y C: Campos de tipo boolean escrito por el ATMega, que se activan

mientras se presione el pulsador correspondiente.

Leds amarillo, verde y rojo: Elementos de tipo boolean escritos por la Raspberry Pi. El
ATMega encenderá los leds para los que la Raspberry Pi haya escrito un 1 en su posición
dentro de la estructura.

Tensión a aplicar al motor izquierdo: Dato tipo int16 entre -300 y +300. Multiplicando
este dato por 0.018, se obtiene la tensión que el ATMega debe aplicar al motor izquierdo.

Tensión a aplicar al motor derecho: Dato tipo int16 entre -300 y +300. Multiplicando
este dato por 0.018, se obtiene la tensión que el ATMega debe aplicar al motor derecho.

16 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3.4. IMU

3.3.2. Comunicación entre los dos microcontroladores

Como se ha mencionado previamente en la Sección 3.1, el microcontrolador ATMega actúa
como esclavo I2C de la Raspberry Pi. La comunicación se basa en intercambiar la estructura
previamente descrita. La comunicación I2C se realiza a una velocidad de 400 kHz.

Pese a que la estructura definida en Arduino dispone de diferentes tipos de elementos, al ser
enviada a la Raspberry Pi, es recibida como una estructura de dieciocho elementos tipo uint8,
por lo que la información resulta organizada de la siguiente forma:
Nº Byte Campo de la estructura
0 Batería mV LSB
1 Batería mV MSB
2 Encoder izquierdo LSB
3 Encoder izquierdo MSB
4 Encoder derecho LSB
5 Encoder derecho MSB
6 Error lectura encoder izquierdo
7 Error lectura encoder derecho
8 Pulsador A
9 Pulsador B
10 Pulsador C
11 Led amarillo
12 Led verde
13 Led rojo
14 Tensión motor izquierdo LSB
15 Tensión motor izquierdo MSB
16 Tensión motor derecho LSB
17 Tensión motor derecho MSB
Esta nueva distribución de la información pone de manifiesto la necesidad de reconstruir los
datos del tipo int16 para obtener el valor real. Con este fin se ha utilizado la función typecast de
Matlab, que recibe como entrada los dos bytes que definen el dato, concatenados en un único
vector [LSB MSB], y devuelve el número resultante en 16 bits.

3.4. IMU
3.4.1. Descripción
La unidad de medidas inerciales, o más comúnmente llamada IMU(Inertial Measurement
Unit), consiste en un giróscopo y un acelerómetro de tres ejes. Ambos se encuentran integrados
en el chip LSM6DS33. Este CI dispone de múltiples opciones de escala:

±2/ ± 4/ ± 8/ ± 16 g’s para la gravedad

±125/ ± 245/ ± 500/ ± 1000/ ± 2000 dps para la velocidad angular

En este proyecto se han utilizado las escalas de ±2 g y ±125 dps, para así obtener la máxima
resolución.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 17

Carmen Jiménez Cortés
Capítulo 3. Hardware

Figura 3.5. Unidad inercial de medidas.

El CI LSM6DS33 también incluía una serie de filtros digitales paso bajo, que se configuraron a
la frecuencia de corte menos restrictiva (mayor frecuencia de corte), para no alterar las medidas
crudas y así poder filtrar las mismas con los filtros programados en Matlab-Simulink.

3.4.2. Modo de empleo de la IMU

El acelerómetro permite medir la aceleración lineal y la proyección de la gravedad en cada
uno de los tres ejes. El giróscopo, en cambio, proporciona la velocidad angular en cada uno de
los tres ejes.

La información de estas medidas se recibe a través de I2C, ya que el chip LSM6DS33 es un

esclavo I2C de la Raspberry Pi. Los datos se obtienen leyendo los registros del 22 al 27 para el
giróscopo, y del 28 al 33 para el acelerómetro. Cada pareja de registros se corresponde con la
medida en cada uno de los ejes, por lo que la medida real resulta de componer ambos datos
como uno único de tipo int16.

La IMU se muestrea cada milisegundo, para así poder calcular un valor medio de cada
medida con diez muestras. Este dato será de gran utilidad al control.

En la Figura 3.5 aparece también en el circuito integrado LIS3MDL, un magnetómetro de

tres ejes. Este sensor mide el campo magnético de la tierra en cada uno de los ejes, y permite así
conocer el rumbo en la navegación como si se tratara de una brújula. Se ha decidido no utilizarlo
en este proyecto porque el vehículo está destinado a funcionar en interiores, lo que dificulta su
calibración y su uso, debido a todos los campos magnéticos parásitos presentes.

3.4.3. Procesamiento de la información

Una vez se han recibido las medidas, a la procedente del acelerómetro se le aplica una
ganancia de 9.81/1000 para obtener el dato en m/s2 , y a la procedente del giróscopo una
de ⇡/180/1000, para que la medida esté en rad/s. A continuación, ambas medidas se filtran
mediante un filtro lineal paso bajo con frecuencia de corte igual a 5 Hz.

18 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3.4. IMU

Figura 3.6. Sistema de referencia del vehículo.

Como se puede ver en la Figura 3.5, los ejes por defecto de la IMU no se corresponden con
los ejes del sistema de referencia del vehículo, Figura 3.6, por lo que las medidas recibidas a
través del bus I2C deben ser transformadas para obtener las medidas en los ejes del vehículo.
Esta transformación consta de los siguientes dos pasos:

Cambio en la orientación del sistema de referencia En primer lugar, se intercambian los

ejes X y Z entre sí, manteniendo así un triedro ortonormal, según las matrices:

Acelerómetro

2 3 2 3 2 3
ax 0 0 1 ax
6 7 6 7 6 7
4 ay 5 = 4 0 1 05 · 4 a y 5 (3.1)
az 1 0 0 az
Balboa0 IM U

Giróscopo

2 3 2 3 2 3
gx 0 0 1 gx
6 7 6 7 6 7
4 gy 5 = 4 0 1 05 · 4 g y 5 (3.2)
gz 1 0 0 gz
Balboa0 IM U

Siendo [ax ay az ]Balboa0 y [gx gy gz ]Balboa0 las medidas de la IMU expresadas en el sistema de
referencia del vehículo si estuviera en equilibrio en posición vertical.

Como se puede observar, el elemento (3,1) de la matriz de rotación del acelerómetro y

del giróscopo no son el mismo. Esto se debe a que el código para la implementación del
EKF estaba ya previamente diseñado con este sistema de referencia.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 19

Carmen Jiménez Cortés
Capítulo 3. Hardware

Rotación del sistema de referencia Entre la posición de equilibrio del vehículo y la posición
vertical de la IMU existe un ángulo de 15º. Por lo tanto, se aplica la siguiente matriz de
rotación tanto al acelerómetro como al giróscopo:

2 3 2 3T 2 3
X cos(✓) 0 sen(✓) X
6 7 6 7 6 7
4Y 5 =4 0 1 0 5 · 4Y 5 (3.3)
Z sin(✓) 0 cos(✓) Z
Balboa Balboa0

Siendo [XY Z]Balboa los valores de la lectura de la IMU en el sistema de referencia definitivos
del vehículo, y ✓ el ángulo de inclinación de la IMU con respecto a la posición de equilibrio
(15º).

Una vez se han filtrado y transformado las medidas, se almacenan en el bus de control, para
que puedan ser empleadas por el EKF y el resto de la lógica del control.

3.5. Motores
3.5.1. Descripción
Los actuadores del Balboa 32U4 son dos motores HPCB de 6V, Figura 3.7. Entre todos los
modelos disponibles se ha elegido el modelo 75:1, que proporciona una relación de transmisión
de 75.81:1, y que tiene una resistencia interna de 4 ⌦.

Entre las especificaciones clave destaca que sin carga, alimentado a 6 V, gira a una velocidad
de 430 rpm, consumiendo una corriente de 100 mA. Con el motor bloqueado ofrece un par de
1.1 kg ⇤ cm y consume una corriente de 1.5 A.

Figura 3.7. Motor HPCB.

El driver empleado para el control de los motores es el DRV8838 de la empresa americana

"Texas Instruments". El ATMega emplea los siguientes pines para el control del driver:

Pin digital 15: Controla la dirección de giro del motor derecho. A nivel bajo, mueve el
motor en sentido positivo y a nivel alto en dirección contraria.

Pin digital 16: Controla la dirección de giro del motor izquierdo. A nivel bajo, mueve el
motor en sentido positivo y a nivel alto en dirección contraria.

Pin digital 9: Controla la velocidad de giro del motor derecho, es el encargado de modular
el PWM generado con el Timer 1 del ATMega32U4.

20 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3.5. Motores

Pin digital 10: Controla la velocidad de giro del motor izquierdo mediante la modulación
del PWM generado con el Timer 1 del ATMega32U4.

El setido positivo de giro del motor se corresponde con aquella que haría avanzar al vehículo en
el sentido en el que se encuentra ubicada la tapa de la batería.

En la Figura 3.8 aparece el driver que controla uno de los dos motores. Se trata de un Puente
en H formado por cuatro FETs de pequeña resistencia parásita, alrededor de 2.8 m⌦, y con un
diodo de recirculación. Soportan una corriente máxima de 1.8 A de corriente directa. Este driver
está especialmente indicado para alimentar cargas tales como cámaras, lentes DSLR, robótica,
juguetes, equipos médicos y cargas similares que se comporten como solenoides.

Figura 3.8. Driver el motor.

La lógica de control de los cuatro FETs se integran en un único circuito. Además, dispone
de elementos adicionales de protección frente a bloqueos por infratensión, sobrecorriente o
apagado por sobretemperatura. Estas protecciones saltan, respectivamente, cuando la tensión
de alimentación (VCC ) es menor a 1.7 V, IOU T es superior a 1.9 A, y cuando la temperatura
excede los 150ºC.

En la Figura 3.9 se puede observar la curva del motor empleado.

3.5.2. Funcionamiento del driver DRV8838

Las salidas se controlan utilizando una interfaz Fase/Habilitar en lugar del habitual PWM.
La principal ventaja que presenta esta intefaz es que mientras que en un PWM es necesario
controlar cada salida con un pin de entrada, con una interfaz del tipo Fase/Habilitar basta con
solo usar dos pines: El pin xPHASE se encarga de controlar la dirección de la corriente y el pin
xENBL para habilitar o deshabilitar el puente en H, Figura 3.10.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 21

Carmen Jiménez Cortés
Capítulo 3. Hardware

Figura 3.9. Curva de caracterización del motor 75:1 HPCB 6V.

Figura 3.10. "Timing"de las entradas y salidas del driver DRV8838.

En la Figura 3.10 se puede observar el funcionamiento de este dispositivo. Básicamente,

cuando el pin de habilitación se encuentra a nivel alto, y el pin de de fase a nivel bajo, el pin
xOUT1, Figura 3.8, se pone a nivel alto y el pin xOUT2 a nivel alto, haciendo que la corriente
llegue al motor en sentido directo. Cuando tanto el pin de habilitación, como el de fase, se
encuentran a nivel alto, el pin xOUT1 se pone a nivel bajo y el pin xOUT2 a nivel bajo, resultando
el motor alimentado con corriente inversa.

Cuando el pin de habilitación se encuentra a nivel bajo, independientemente del estado

del pin de fase, ambos pines xOUTx se ponen a nivel bajo y la corriente que llega al motor se
interrumpe.

22 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3.6. Sensores laterales

Los diferentes círculos de color rosa que aparecen en la Figura 3.10 hacen referencia a todos
los retrasos que se producen en la conmutación del estado de los diferentes pines.

Una implementación de este driver en una placa, como la del ATMega32U4 se puede observar
en la Figura 3.11

Figura 3.11. Aplicación del driver DRV8838 a un motor de corriente continua.

3.6. Sensores laterales

3.6.1. Descripción
Dado que uno de los objetivos de este proyecto consiste en conseguir que el vehículo complete
una vuelta al circuito cerrado empleado en el laboratorio de control, se han utilizado un par
de sensores de distancia, que ubicados en el lateral izquierdo del vehículo, proporcionaran la
información necesaria para poder calcular la distancia a la pared y el ángulo respecto de la
misma.

Como sensores se han empleado los VL6180X, de la empresa STMicroelectronics, Figura 3.12.
Estos sensores incorporan una tecnología patentada por STMicroelectronics, denominada
FlightSense™. Esta tecnología permite medir distancias absolutas independientemente de la
reflectancia del objeto a medir, en lugar de estimar la distancia midiendo la cantidad de luz
reflejada desde el cuerpo, se mide el tiempo que tarda la luz en viajar al cuerpo y volver al
sensor. Este tiempo es conocido como tiempo de vuelo. Gracias a ello, el color o la superficie del
objeto dejan de ser posibles fuentes de error en la medida.

Para llevar a cabo el proceso de medida previamente descrito, VL6180X integra en un mismo
CI un emisor de infrarrojos, un sensor de distancia y un sensor de luz ambiente.

Figura 3.12. Sensor de distancia VL6180X.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 23

Carmen Jiménez Cortés
Capítulo 3. Hardware

Cada sensor nos proporciona la distancia al objeto más cercano en milímetros, en un rango
de 5 a 200 milímetros, con una incertidumbre de ±1 milímetro.

3.6.2. Comunicación con la Raspberry Pi

La comunicación con la Raspberry Pi vuelve a realizarse a través del bus I2C, siendo los
sensores VL6180X esclavos I2C de la Raspberry Pi.

El principal inconveniente es que ambos sensores tienen la misma dirección en el bus I2C,
por lo que ha sido necesario utilizar adicionalmente un multiplexador I2C. El multiplexador
empleado es el TCA9548A de Texas Instruments, que dispone de ocho canales para conectar
diferentes dispositivos I2C, Figura 3.13

Figura 3.13. Multiplexador I2C TCA9548A.

Gracias al TCA, en vez de tener dos dispositivos con la misma dirección en el bus de I2C,
aparece el propio multiplexador como esclavo. Cada vez que se quiera utilizar uno de los
sensores, basta con escribir un 1 en el registro de control correspondiente al canal que se quiere
habilitar.

3.6.3. Modo de empleo de la información de distancia

Para poder calcular la distancia y el ángulo del vehículo respecto de la pared, se realizan las
siguientes operaciones:

Ángulo respecto de la pared

d1 d2
✓ = atan( ) (3.4)
distsens · cos( )
Siendo:

✓: Ángulo del vehículo respecto la pared, en radianes.

d1 : Distancia medida por el sensor ubicado en la parte frontal del vehículo.

d2 : Distancia medida por el sensor ubicado en la parte trasera del vehículo.

distsens : Distancia que separa los sensores entre sí.

d: Distancia a la pared en milímetros.

: Ángulo de inclinación de la posición de equilibrio del vehículo respecto de la vertical

(15º).

24 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3.7. Encoders

Estos parámetros se encuentran representados en la Figura 3.14.

La distancia entre los sensores se multiplica por el coseno de 15º porque en la ecuación
interviene la distancia que separa a los sensores, proyectada sobre el eje horizontal, y la posición
de equilibrio del vehículo se corresponde con una inclinación de 15º hacia atrás.

Distancia a la pared
d1 + d2
d= · cos(✓) (3.5)
2

Figura 3.14. Imagen auxiliar del cálculo del ángulo y la distancia respecto a la pared.
Una vez obtenidos estos dos datos, se almacenan en el bus de control para que puedan ser
empleados por la lógica del control de seguimiento de la pared.

3.7. Encoders
3.7.1. Descripción y funcionamiento
El Balboa cuenta con dos encoders de cuadratura magnéticos, situados en cada uno de los
ejes de los motores, Figura 3.15. Estos encoders consisten en dos discos magnéticos sujetos a
una extensión del eje de giro del motor y un par de sensores de efecto Hall, ubicados en la
placa.

Los sensores de efecto Hall son un tipo de transductores que en presencia de un campo
magnético variable, generan una tensión de salida. Los encoders de cuadratura funcionan gracias
al principio del efecto Hall, generando dos señales cuadradas desfasadas 90º entre sí.

Los encoders son incrementales y poseen una resolución de 12 cuentas por revolución del eje
del motor.

Los encoders de cuadratura generalmente se controlan monitorizando ambos canales, canales

A y B en la Figura 3.16, directamente, pero en el caso del Balboa se realiza un XOR entre ambos
canales, resultando así un único pin de interrupción. Adicionalmente, se conecta un segundo
pin al canal B, lo que permite reconstruir la señal del canal A mediante una operación XOR
entre ambos pines, Ecuación 3.6.

(A xor B) xor B = A (3.6)

Control de un Vehículo Equilibrista mediante una Raspberry Pi 25

Carmen Jiménez Cortés
Capítulo 3. Hardware

Figura 3.15. Imagen de la disposición de los discos magnéticos en el motor.

Figura 3.16. Lectura de los canales de los encoders.

Este proceso de lectura y obtención de la medida cruda de los encoders se realiza

en el ATMega32U4, para a continuación incluirla dentro de la estructura descrita en la
Sección 3.3.

3.7.2. Estimación de la velocidad de avance y velocidad angular de

guiñada
Como se ha explicado en la Sección 3.3, la Raspberry Pi recibe dos bytes con las cuentas
de cada uno de los dos encoders. Esta medida cruda se procesa para obtener las velocidades
lineales común y diferencial.

En primer lugar se transforma el valor recibido a través del bus I2C de cuentas de encoder a
grados. Para ello, a partir de la información de los encoders del datasheet del Balboa, se calcula
la resolución del encoder según la Ecuación 3.7:

26 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3.7. Encoders

renc = 12 · mrt · rrt (3.7)

Siendo:

renc : La resolución del encoder.

mrt : La relación de transmisión del motor.

rrt : La relación de transmisión de la rueda.

A continuación, el valor de la medida se puede transformar a grados a partir de la

Ecuación 3.8:

encg = encc · 360/renc (3.8)

Siendo:

encg : El valor del encoder absoluto en grados.

encc : El valor del encoder en cuentas.

renc : La resolución del encoder, calculada mediante la Ecuación 3.7.

La velocidad común se puede calcular entonces mediante la Ecuación 3.9:

4encl + 4encr 1 ⇡
vc = · · ·R (3.9)
2 ts 180

Siendo:

vc : La velocidad común, y por tanto de avance, del vehículo.

4encl : El incremento del encoder izquierdo.

4encr : El incremento del encoder derecho.

R: El radio de la rueda en metros.

ts: El tiempo de muestreo en segundos.

La velocidad diferencial o de guiñada se calcula mediante la Ecuación 3.10:

1 ⇡ 1
vd = (4encl 4encr ) · · ·R· (3.10)
ts 180 D

Siendo:

vd : La velocidad de guiñada, y por tanto diferencial, del vehículo.

4encl : El incremento del encoder izquierdo.

4encr : El incremento del encoder derecho.

R: El radio de la rueda en metros.

ts: El tiempo de muestreo en segundos.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 27

Carmen Jiménez Cortés
Capítulo 3. Hardware

D: La distancia que separa las ruedas, en metros.

Se ha definido la velocidad de guiñada como la diferencia entre el incremento del encoder

izquierdo menos el derecho para que así cuando el vehículo gire en sentido horario, sentido
positivo en el eje z del sistema de referencia del vehículo, la velocidad de guiñada resultara
positiva.

3.7.3. Corrección de la medida de los encoders

La principal desventaja de usar encoders magnéticos es que su precisión es menor que la de
los encoders ópticos. Lamentablemente, los encoders ópticos son muy delicados, en caso de fallo,
fallan catastróficamente y tienen un rango limitado de temperaturas para su funcionamiento, lo
que justifica la popularidad de los encoders magnéticos en proyectos de mecatrónica.

Debido a esta falta de precisión en la medida, ha sido necesario implementar tres filtros
consecutivos para conseguir una medida de la velocidad que fuera útil para el control. Los
dos primeros son filtros no lineales, encargados de eliminar los picos tan característicos de
la Figura 3.17. El tercero en cambio es un filtro paso bajo habitual, para eliminar ruido
parásito.

Para el correcto filtrado, es necesario utilizar un buffer de N muestras, de modo que se

puedan descartar medidas por comparación, en los filtros no lineales. Tras varios ensayos, se
obtuvo que un tamaño adecuado para este buffer eran 15 muestras. Existe un compromiso entre
el número de muestras y la velocidad de cálculo, ya que ha de ser un valor suficientemente
grande para albergar como mínimo una medida válida, pero lo suficientemente pequeño como
para no comprometer el tiempo de ejecución.

20

10

-10

-20

-30

-40

0 5 10 15 20 25 30 35

Figura 3.17. Incremento original de encoder.

3.7.3.1. Filtro no lineal para eliminar el efecto del rebose

El primer filtro es un filtro no lineal. Este filtro se encarga de eliminar los picos que se
producen en la medida del encoder incremental debido al rebose del tipo de dato int16. A
medida que el motor gira, en sentido positivo, las cuentas se incrementan y en el momento que

28 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3.7. Encoders

estas alcanzan el valor de 32767, se reinician a −32768, provocando un falso incremento de

encoder de 65535 cuentas. Esto resulta en un valor de velocidad imposible.

Para llevar a cabo este filtrado, basta con comparar el valor actual del incremento del encoder
con un umbral de 180º. Si el valor es superior a 180º, entonces se descarta y se emplea como
valor actual del encoder el último dato válido que se haya almacenado en el buffer. Tras esta
operación, se obtiene un incremento de encoder que presenta el aspecto de la Figura 3.18.

25

20

15

10

0
0 5 10 15 20 25 30 35 40

Figura 3.18. Incremento de encoder tras el primer filtrado no lineal.

3.7.3.2. Filtro no lineal para eliminar errores de medida

El segundo filtro también es un filtro no lineal, responsable de eliminar los picos provocados
por un aumento desmesurado en el incremento del encoder en un tiempo de muestreo, es decir,
si el vehículo se desplaza a una velocidad constante de 5 cm/s, que de pronto se produzca un
aumento de esta velocidad a 20 cm/s denota claramente que se ha producido una anomalía en
la medida de la misma.

Para eliminar este conjunto de medidas erróneas se definió un valor umbral para los
incrementos de segundo orden de las cuentas del encoder. Por incremento de segundo orden
se entiende la diferencia entre el incremento de encoder actual y el incremento calculado
anteriormente. El valor de este umbral se dedujo de manera experimental, sometiendo a los
motores a diferentes variaciones de velocidad, y finalmente se fijó a un valor de 10 cm/s2 .
Todos aquellos incrementos de segundo orden que resulten por debajo de este valor umbral,
son almacenados en un vector auxiliar que permitirá reconstruir finalmente la medida del
encoder.

3.7.3.3. Reconstrucción de la medida del encoder

Finalmente, tras haber eliminado todas las muestras que generaban los picos, se calcula
la medida incremental del encoder que será posteriormente empleada en el cálculo de las
velocidades lineales común y diferencial.

El incremento definitivo de cada encoder se realiza en tres pasos:

Control de un Vehículo Equilibrista mediante una Raspberry Pi 29

Carmen Jiménez Cortés
Capítulo 3. Hardware

Se ordenan de menor a mayor los elementos del vector auxiliar con los incrementos del
encoder.

Se toma la mediana de la ordenación anterior como valor actual.

Al incremento de encoder actual, se le suman todos los incrementos de segundo orden

válidos desde la muestra correspondiente a la mediana, hasta la muestra actual.

El resultado tras este segundo filtro no lineal se puede observar en la Figura 3.19

0
0 5 10 15 20 25 30 35 40

Figura 3.19. Incremento definitivo de encoder.

El tercer paso de la reconstrucción de la medida puede resultar poco intuitivo, pero la

realidad demuestra que es muy necesario, si no en lugar de obtener una velocidad como la
mostrada en la Figura 3.19, resultaria la de la Figura 3.20, que no se corresponde con el aspecto
que cabe esperar de una medida analógica.

3.7.3.4. Filtrado lineal paso bajo

Finalmente, se aplica al incremento definitivo de encoder un filtro lineal paso bajo que
permita eliminar ruidos parásitos. Esto provoca un error de cuantización, dado que el número
de valores posibles para la tensión es finito, pero aún así, la medida posee la resolución suficiente
cómo para estimar las velocidades común y diferencial del vehículo con precisión.

El resultado final se muestra en la Figura 3.21

3.7.3.5. Aplicación de los filtros

Para verificar la eficiencia de los filtros, se estudiaron las velocidades común y diferencial en
dos ensayos completamente distintos.

Medida de la velocidad común

En este ensayo se aplicó una tensión común cuadrada de amplitud 0.15 V y valor medio 0.45
V. El valor de la tensión diferencial aplicada era de 0 V. El vehículo no se estaba equilibrando,
sino que se desplazaba en posición horizontal sobre su tercer punto de apoyo. El resultado se
puede observar en la Figura 3.22.

30 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3.7. Encoders

0
0 5 10 15 20 25 30 35 40

Figura 3.20. Incremento definitivo de encoder sin acumular el incremento de segundo orden.

0
0 5 10 15 20 25 30 35 40

Figura 3.21. Incremento definitivo de encoder.

Medida de la velocidad de guiñada

En este ensayo se aplicó una tensión diferencial cuadrada de amplitud 0.4 V y valor medio
0 V. El valor de la tensión común aplicada era de 0 V. Pero en esta ocasión el vehículo si se
encontraba en equilibrio mientras giraba sobre su eje z, por eso la medida resulta más ruidosa.
El resultado se muestra en Figura 3.23.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 31

Carmen Jiménez Cortés
Capítulo 3. Hardware

Ensayo: Referencia de velocidad cuadrada para la velocidad común

0.7

0.6

0.5

Velocidad de avance (m/s)

0.4

0.3

0.2

0.1

0
0 5 10 15 20 25 30 35 40
Tiempo (s)

Figura 3.22. Velocidad común.

Ensayo: Seguimiento a referencia de velocidad de guiñada

0.6

0.4
Velocidad de guiñada (rad/s)

0.2

-0.2

-0.4

-0.6

40 42 44 46 48 50 52 54 56 58 60
Tiempo (s)

Figura 3.23. Velocidad de guiñada.

3.8. Fabricación piezas 3D

Para poder disponer los sensores laterales y el multiplexador de canales I2C en el lateral del
vehículo, fue necesario diseñar e imprimir en 3D tanto un separador como un soporte para los
sensores.

En la Figura 3.24 se pueden ver el modelo 3D del separador, que sujeta el soporte al
vehículo.

32 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 3.8. Fabricación piezas 3D

Figura 3.24. Separador para el soporte de los sensores.

En la Figura 3.25 se muestra el modelo 3D del soporte.

Figura 3.25. Soporte de los sensores laterales.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 33

Carmen Jiménez Cortés
 4
Modelado y obtención de
parámetros

El objetivo de esta sección es obtener un modelo matemático no lineal en tiempo continuo

que describa las dinámicas del vehículo y permita así evaluar diferentes estrategias de
control.

En la Sección 4.1 se describen en primer lugar todos los parámetros geométricos y

mecánicos que intervienen en las ecuaciones, Sección 4.1.2. A continuación, las variables
de estado que se han elegido para el diseño del control por realimentación del estado,
Sección 4.1.3. Después, las ecuaciones que caracterizan el modelo electromecánico
de un motor de corriente continua, Sección 4.1.4, y finalmente se describen todas las
ecuaciones de los movimientos de avance y cabeceo, Sección 4.1.5, y de giro del vehículo,
Sección 4.1.6.

Para terminar este capítulo, se explica cómo se ha llevado a cabo la caracterización del
vehículo, Sección 4.2. Fueron necesarios dos procesos independientes de identificación:
uno para los parámetros que intervienen en las ecuaciones de avance y cabeceo,
Sección 4.2.3, y otro para los parámetros del movimiento de giro, Sección 4.2.4.

Adicionalmente, se ha descrito también cómo se identificó el ruido presente en la IMU

y en los encoders, que se incluyen en el simulador para que las simulaciones sean más
representativas del comportamiento real del vehículo, Sección 4.2.5.

4.1. Modelo matemático del vehículo

4.1.1. Introducción
Al tratarse de una planta tan compleja, el estudio de las dinámicas de su movimiento supone
un compromiso entre la precisión y exactitud del modelo y la sencillez de cálculo.

Para una mayor simplicidad matemática, se ha optado por desacoplar los movimientos que
intervienen cuando el vehículo se encuentra desplazándose por el circuito:

Control de un Vehículo Equilibrista mediante una Raspberry Pi 35

Carmen Jiménez Cortés
Capítulo 4. Modelado y obtención de parámetros

Avance y cabeceo: Que es el movimiento responsable de que el vehículo se mantenga en

equilibrio.

Giro: Que corrige la guiñada para que el vehículo pueda desplazarse en línea recta,
siguiendo una pared, o una referencia de guiñada.

4.1.2. Parámetros geométricos y mecánicos

Los parámetros que intervienen en las ecuaciones que se desarrollan en la Sección 4.1.5 y la
Sección 4.1.6 son los siguientes:

Parámetros del motor

Tensión del motor Vbat [V]

Resistencia del motor Rm [⌦]

Fricción viscosa Dm [Nms/rad]

Constante mecánica Kt [Nm/A]

Constante eléctrica Ke [Vs/rad]

Relación de transmisión del motor mrt

Par de fricción máxima Trmax [Nm]

Caida del voltaje del motor Vdrop [V]

Retraso en la conversión del motor delaym [s]

Parámetros del vehículo

Distancia del eje de la rueda al centro de gravedad del vehículo h [m]

Relación de transformación de la rueda rrt

Masa del chasis M [kg]

Masa de cada rueda, incluyendo el rotor del motor m [kg]

Momento de inercia de cabeceo del chasis respecto a un eje principal que pasa por su cdg
I✓ [kgm2 ]

Momento de inercia de giro del vehículo respeto al eje vertical o guiñada I [kgm2 ]

Radio de la rueda R [m]

Fricción viscosa de giro respecto al eje vertical o guiñada D [Nms/rad]

Momento de inercia del conjunto motor y rueda Imr [kgm2 ]

Separación entre las ruedas W [m]

4.1.3. Variables de estado y variables auxiliares

Las variables de estado empleadas para estudiar el movimiento completo del vehículo
son:

Velocidad común lineal de las ruedas vRc [m/s]

36 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 4.1. Modelo matemático del vehículo

Velocidad angular de cabeceo !✓ [rad/s]

Ángulo de cabeceo ✓ [rad]

Velocidad angular de guiñada !

Ángulo de guiñada [rad]

Conocidas estas variables, se pueden definir una serie de variables auxiliares que serán útiles
en la definición de las ecuaciones del modelo:

Velocidad angular común de la rueda !Rc = vRc /R [rad/s]

Velocidad angular común del motor !mc = rrt · !Rc + !✓ [rad/s]

Velocidad diferencial lineal de las ruedas vRd = W · ! [m/s]

Velocidad angular diferencial de las ruedas !Rd = vRd /R [rad/s]

Velocidad angular diferencal del motor !md = rrt · !Rd [rad/s]

Velocidad del motor izquierdo !ml = !mc + !md /2 [rad/s]

Velocidad del motor derecho !mr = !mc !md /2 [rad/s]

Tensión del motor izquiero ul [V]

Tensión del motor derecho ur [V]

4.1.4. Ecuaciones del motor

En la Figura 4.1 se muestra el modelo electromecánico de un motor de corriente
continua.

Figura 4.1. Modelo electromecánico de un motor.

Las ecuaciones que definen el modelo del motor son las siguientes:

Corrientes en los motores

el = Ke · !ml (4.1)

er = Ke · !mr (4.2)

Control de un Vehículo Equilibrista mediante una Raspberry Pi 37

Carmen Jiménez Cortés
Capítulo 4. Modelado y obtención de parámetros

ul sig(ul el ) · Vdrop el
il = (4.3)
Rm

ur sig(ur er ) · Vdrop er
ir = (4.4)
Rm

En la Ecuación 4.3 y en la Ecuación 4.4 es importante destacar que el signo de Vdrop debe
ser el resultante de la diferencia ul el o ur er , según corresponda.

Tensión común de los motores

ul + ur
uc = (4.5)
2

Tensión diferencial de los motores

ud = ul ur (4.6)

4.1.5. Movimiento de avance y cabeceo

En primer lugar se calculan los pares motores en el eje de cada motor.

Tmc = Kt · ic (4.7)

Tmd = Kt · id (4.8)

Tml = Tmc + Tmd /2 (4.9)

Tmr = Tmc Tmd /2 (4.10)

A continuación, se determinan los pares resistentes en cada rueda:

Trlim = Tmr · (|Tmr | < Trmax ) Trmax · (Tmr <= Trmax ) + Trmax · (Tmr >= Trmax ) (4.11)

Trr = Trlim · (|!mr | == 0) + Trmax · sig(!mr ) · (|!mr | > 0) (4.12)

Tllim = Tml · (|Tml | < Trmax ) Trmax · (Tml <= Trmax ) + Trmax · (Tml >= Trmax ) (4.13)

Trl = Tllim · (|!ml | == 0) + Trmax · sig(!ml ) · (|!ml | > 0) (4.14)

38 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 4.1. Modelo matemático del vehículo

De nuevo, en la Ecuación 4.12 y Ecuación 4.14 intervienen el signo de wr y wl

respectivamente.

Trl + Trr
Trc = (4.15)
2

Trd = Trl Trr (4.16)

Y se obtienen finalmente los pares netos común y diferencial en el eje de cada motor:

Tc = Tmc Dm · !mc Trc (4.17)

Td = Tmd Dm · !md Trd (4.18)

Conocidos todos los pares, se puede estudiar el movimiento de avance y cabeceo de la

siguiente forma:

La ecuación dinámica de rotación de las ruedas en modo común resulta:

d!Rc Imr dvRc

mrt · Tc FRc · R = Imr · = · (4.19)
dt R dt

Donde FRc es la fuerza de rozamiento común de las ruedas con el suelo.

Despejando de esta ecuación FRc y multiplicando por 2 se obtiene:

2 · mrt · Tc 2 · Imr dvRc

2 · FRc = · (4.20)
R R2 dt

La ecuación dinámica de traslación del conjunto del sistema de tracción y el chasis debido al
mando común resulta entonces:

dvRc d!✓
2 · FRc = (2 · m + M ) · +M ·h· · cos(✓) M · !✓2 · h · sen(✓) (4.21)
dt dt

En la ecuación anterior se considera que la aceleración del centro de masas del sistema de
tracción es:

dvRc
= aRc
dt

Y que la aceleración en la dirección de avance del centro de gravedad del chasis es:

dvRc d!✓
+h· · cos(✓) !✓2 · h · sen(✓)
dt dt

Siendo !✓ la velocidad angular de cabeceo del chasis.

Sustituyendo el valor de 2 · FRc en la Ecuación 4.21 resulta:

Control de un Vehículo Equilibrista mediante una Raspberry Pi 39

Carmen Jiménez Cortés
Capítulo 4. Modelado y obtención de parámetros

2 · mrt · Tc Imr dvRc d!✓

= (2 · (m + 2 ) + M ) · +M ·h· · cos(✓) M · !✓2 · h · sen(✓) (4.22)
R R dt dt

Finalmente, se plantean las ecuaciones que gobiernan el cabeceo del chasis. El cuerpo se
encuentra sometido a su propio peso, las fuerzas totales de reacción Nx , Ny y al par neto de
reacción del motor 2 · Tc (dos veces la componente común del par). Las fuerzas y aceleraciones
que intervienen en estas ecuaciones se pueden ver en la Figura 4.2

Figura 4.2. Fuerzas y aceleraciones en un sistema de referencia solidario al vehículo .

Ecuación de traslación en el eje X:

dvRc d!✓
Nx + M · g · sen(✓) = M · · cos(✓) + M · h · (4.23)
dt dt

Ecuación de rotación con respecto al centro de masas del chasis:

d!✓
h · Nx 2 · Tc = I ✓ · (4.24)
dt

Sustituyendo el valor de la fuerza de reacción Nx de ambas ecuaciones resulta:

dvRc d!✓ d!✓

h · (M · · cos(✓) + M · h · M · g · sen(✓)) 2 · Tc = I ✓ · (4.25)
dt dt dt

dvRc d!✓
2 · Tc + M · g · h · sen(✓) = M · h · · cos(✓) + (I✓ + M · h2 ) · (4.26)
dt dt

Una vez eliminadas las variables intermedias de las ecuaciones previas ,se obtiene el siguiente
modelo no lineal:

40 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 4.1. Modelo matemático del vehículo

Kt mrt · vRc
Tmc = · (uc Ke · ( !✓ )) (4.27)
Rm R

!mc
mrt · Tc = mrt · Tmc Trc Dm · (4.28)
mrt

2 · mrt · Tc Imr dvRc d!✓

= (2 · (m + 2 ) + M ) · +M ·h· · cos(✓) M · !✓2 · h · sen(✓) (4.29)
R R dt dt

dvRc d!✓
2 · Tc + M · g · h · sen(✓) = M · h · · cos(✓) + (I✓ + M · h2 ) · (4.30)
dt dt

Las ecuaciones anteriores, Ecuación 4.29 y Ecuación 4.30, forman un sistema de ecuaciones
no lineal que permiten despejar las derivadas temporales de la velocidad de avance vRc y la
velocidad angular de cabeceo !✓ .

4.1.6. Movimiento de giro

En la Figura 4.3 se representa el giro del vehículo con respecto a un eje vertical
(guiñada).

Figura 4.3. Representación del giro del vehículo alrededor de un eje vertical .

Sabiendo que la fuerza de rozamiento diferencial con el suelo, FRd es la única fuerza externa
que provoca una aceleración angular, la ecuación dinámica del giro del vehíchulo con respecto
a un eje vertical resulta:

W d!
FRd · =I · +D ·! (4.31)
2 dt

Eliminando FRd a partir de la ecuación en modo diferencial de los motores se obtiene:

W mrt · Td Imr · W d! W d!
FRd · =( 2
· )· =I · +D ·! (4.32)
2 R R dt 2 dt

W Imr · W 2 d!
mrt · Td · = (I + )· +D ·! (4.33)
2·R 2 · R2 dt

Control de un Vehículo Equilibrista mediante una Raspberry Pi 41

Carmen Jiménez Cortés
Capítulo 4. Modelado y obtención de parámetros

mrt · Kt W m2rt · Kt · Ke W Imr · W 2 d!

( ·ud TRd ·( +Dm )·! )· = (I + )· +D ·! (4.34)
Rm R Rm 2R 2 · R2 dt

mrt · W · Kt W Imr · W 2 d! W2 m2rt · Kt · Ke

· ud · TRd = (I + ) · + ( · ( + Dm ) + D ) · !
2 · R · Rm 2·R 2 · R2 dt 2 · R2 Rm
(4.35)

Para poder llegar a la Ecuación 4.35 se ha considerado que el ángulo de cabeceo es

despreciable, lo cual no es nada precipitado, ya que es el objetivo que debe garantizar el
sistema de control del cabeceo.

Finalmente, agrupando todos los parámetros resulta una única ecuación diferencial de
primer orden:

d! 1 Km Rm
= ·! + · (ud · TRd ) (4.36)
dt ⌧m ⌧m mrt · Kt

Donde los términos ⌧m y Km son:

Imr ·W 2
I + 2·R2
⌧m = m2rt ·Kt ·Ke
(4.37)
W2
2·R2
· ( Rm + Dm ) +D

mrt ·W ·Kt
2·R·Rm
Km = m2rt ·Kt ·Ke
(4.38)
W2
2·R2
· ( Rm + Dm ) +D

Como la Ecuación 4.38 es una ecuación diferencial de primer orden, no resulta complicado
resolverla y obtener así el valor de la derivada de la velocidad angular de guiñada ! .

4.2. Identificación
Todos los parámetros que se describen en la Sección 4.1.2 se han obtenido a partir de las
hojas técnicas de los diferentes componentes y también mediante el método de optimización
Gauss-Newton, empleando datos registrados en diferentes ensayos.

Conseguir identificar los parámetros del vehículo no es trivial, ya que al tratarse de un

sistema inestable, es necesario poder realizar ensayos con el vehículo en equilibrio para poder
analizar las variables del sistema y poder extraer conclusiones. Al no disponer de un modelo lo
suficientemente preciso, la única alternativa posible para diseñar el control es mediante prueba
y error, lo que resulta complejo, muy poco intuitivo y requiere una cantidad no despreciable de
tiempo.

4.2.1. Caracterización de los motores

Como paso previo a la identificación de los parámetros dinámicos y mecánicos del vehículo
en su conjunto, fue necesario obtener los parámetros propios del motor, que intervienen en las
ecuaciones de la Sección 4.1.4. De no haber operado así, no habría sido posible equilibrar el
vehículo.

42 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 4.2. Identificación

Los ensayos que se llevaron a cabo fueron los siguientes cuatro:

Medida de la resistencia interma del motor con ayuda de un polímetro Rm [⌦].

Medida de la tensión aplicada a cada motor para diferentes valores de ganancia. Con este
ensayo pudimos verificar que la relación ganancia-tensión era realmente de la ecuación
teórica: Vbat /mg = 6/300 = 0.02. Donde Vbat es la tensión nominal del motor (6V) y mg la
ganancia máxima que se puede aplicar al motor, 300, que se correspondería a una tensión
de 6V.

Medida de la velocidad con los encoders para diferentes valores de ganancia. Con este
ensayo se puede verificar el valor teórico de la resolución del encoder.

Tiempo del transitorio desde que se retira la tensión del motor hasta que el mismo se
detiene, para determinar su incercia Imr [kgm2 ]. Este dato finalmente no se ha utilizado,
y en su lugar en el modelo aparece el obtenido en la Sección 4.2.3.

En la Sección 6.1 se muestran los resultados de estos ensayos en detalle, para los motores de
50:1, que eran los que inicialmente se querían haber empleado. Como finalmente se usaron los
motores de 75:1 se repitieron sendos ensayos, pero de una forma menos elaborada y por tanto
se ha preferido no incluirlos en este documento.

4.2.2. Algoritmo de optimización Gauss-Newton

El algoritmo de Gauss-Newton se emplea para resolver problemas no lineales de mínimos
cuadrados. Es una simplificación realizada por Carl Firedrich Gauss del método de optimización
de Newton, que omite el uso de segundas derivadas, [19].

El problema que se pretende resolver es la minimización de la suma:

m
X
S(p) = (fi (p))2 (4.39)
i=1

Siendo:

m funciones f1 , ..., fm

n parámetros p1 , ..., pn con m n. Estos parámetros se encuentran recogidos en el vector

p.

Como el algortimo de Gauss-Newton es un procedimiento iterativo, es necesario partir de

una estimación incial del vector de parámetros, p0 .

Las estimaciones posteriores del vector pk se calculan mediante la siguiente relación

recurrente:

pk+1 = pk (Jf (pk )T · Jf (pk )) 1

Jf (pk )T · f (pk ) (4.40)

Donde Jf (p) denota el Jacobiano de la función f en el parámetro p.

En la práctica, no es habitual calcular expresamente la matriz inversa. en su lugar se

utiliza:

Control de un Vehículo Equilibrista mediante una Raspberry Pi 43

Carmen Jiménez Cortés
Capítulo 4. Modelado y obtención de parámetros

pk+1 = pk + k
(4.41)

Y la actualización de k se obtiene a partir del sistema lineal:

Jf (pk )T · Jf (pk ) k
= Jf (pk )T · f (pk ) (4.42)

Si se desea una mejor implementación del algoritmo de Gauss-Newton, se suele utilizar un

algoritmo de búsqueda lineal. En lugar de emplear pk+1 se utiliza:

pk+1 = pk + ↵k · k
(4.43)

Donde ↵k es de algún modo óptimo.

4.2.3. Identificación del modelo de avance y cabeceo

En este proceso de identificación se obtienen los valores de nueve parámetros que intervienen
en el movimiento de avance y cabeceo del vehículo:

Momento de inercia de cabeceo del chasis respecto a un eje principal que pasa por su cdg
I✓ [kgm2 ]

Momento de inercia del conjunto motor y rueda Imr [kgm2 ]

Distancia del eje de la rueda al centro de gravedad del vehículo h [m]

Constante eléctrica Ke [Vs/rad]

Constante mecánica Kt [Nm/A]

Resistencia del motor Rm [⌦]

Par de fricción máxima Trmax [Nm]

Fricción viscosa Dm [Nms/rad]

Caída del voltaje del motor Vdrop [V]

Se registraron el valor de las variables de estado en dos ensayos de equilibrio del vehículo
distintos: equilibrándolo mediante un control P-PD y mediante un PI-PD, ambos con referencia
nula de velocidad.

Una vez registrados ambos ensayos, el proceso para llevar a cabo la optimización consta de
las siguientes etapas:

Preparación de los datos

Con ayuda de Matlab, se selecciona una parte del ensayo, que será la empleada en la
optimización. Es importante seleccionar una parte del ensayo representativa del comportamiento
del vehículo cuando trata de equilibrarse, pero también se debe evitar seleccionar partes con
anomalías que pudieran corromper los resultados obtenidos.

Optimización

44 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 4.2. Identificación

El algoritmo se ejecuta en un bucle while mientras la variación de la función objetivo, en

tanto por ciento, sea mayor que la tolerancia definida, o cuando el máximo incremento de
los parámetros, en tanto por ciento, sea mayor que su tolerancia. Cuando alguna de estas
dos condiciones deja de cumplirse, el programa se detiene, y se habría obtenido por tanto el
resultado óptimo.

En el proceso de optimización, el script de Matlab, simula dentro de un bucle while la salida

obtenida, para diferentes combinaciones de parámetros. Se utiliza un diagrama de Simulink
específicamente diseñado para la caracterización del modelo. Una vez se han conseguido unos
parámetros que resultan en un mejor valor de la función de coste, los muestra por pantalla
y queda a la espera de que se lance una nueva iteración, que comenzará con estos últimos
parámetros.

Cabe destacar que el algoritmo empleado no calcula el valor absoluto de los parámetros
descritos al inicio de esta sección, sino los nueve coeficientes que multiplican al valor inicial del
parámetro.

Diagrama de Simulink empleado en la optimización

Para la obtención de los parámetros del modelo de avance y cabeceo no fue suficiente con
simular la respuesta del vehículo a partir del modelo no lineal descrito en Sección 4.1. Fue
necesario duplicar este modelo, y trabajar de forma separada con la velocidad de avance y la
velocidad angular de cabeceo. Este modelo se puede ver en la Figura 4.4.

Los modelos no lineales son idénticos:

Ambos se encuentran parametrizados, como se ha expresado en el apartado anterior. Los

coeficientes se actualizan en cada iteración del algoritmo Gauss-Newton.

Las ecuaciones que contienen son las expresadas en la Sección 4.1.5.

Y sus principales diferencias:

Cada modelo contribuye a la salida del sistema con una variable distinta: El primer modelo
se encarga de la velocidad lineal de avance, y el segundo de la velocidad angular de
cabeceo.

Las variables que no intervienen en el cálculo de la salida de cada modelo, en lugar de

simularse, se sustituyen por las medidas registradas durante el ensayo. Serían la velocidad
angular de cabeceo para el modelo que calcula la velocidad de avance, y la velocidad de
avance para el modelo que calcula la velocidad angular de cabeceo.

Se llegó a la conclusión de que era necesario tratar de forma separada las velocidades
lineal de avance y angular de cabeceo tras varios intentos de obtener los parámetros con el
modelo completo. El resultado que se obtenía con la optimización no era admisible y por eso se
comenzaron a investigar nuevas alternativas.

El resultado del proceso de la identificación se ha detallado en la Sección 6.2.

4.2.4. Identificación del modelo de giro

En este proceso de identificación se obtienen los valores de tres parámetros que intervienen
en el movimiento de giro del vehículo:

Momento de inercia de giro del vehículo respeto al eje vertical o guiñada I [kgm2 ]

Control de un Vehículo Equilibrista mediante una Raspberry Pi 45

Carmen Jiménez Cortés
Capítulo 4. Modelado y obtención de parámetros

Figura 4.4. Diagrama de Simulink empleado en la identificación de los parámetros del avance y cabeceo.

Fricción viscosa de giro respecto al eje vertical o guiñada D [Nms/rad]

Caida del voltaje del motor Vdrop [V]

Se registraron los valores de las variables de estado en un ensayo con el vehículo en equilibrio,
siguiendo una referencia cuadrada en la velocidad de guiñada, lo que hacía que el vehículo
fuera girando alrededor de su eje z en ambos sentidos alternativamente.

Una vez registrado este ensayo el proceso de optimización de los parámetros es idéntico al
descrito en la Sección 4.2.3, salvo en el modelo empleado para las simulaciones en el proceso
de optimización.

Diagrama de Simulink empleado en la optimización

En esta ocasión sí fue suficiente con utilizar un único modelo que recogiera todas las
ecuaciones que describen el movimiento de giro del vehículo. Estas ecuaciones son las
expresadas en la Sección 4.1.6. Este modelo se muestra en la Figura 4.5.

Figura 4.5. Diagrama de Simulink empleado en la identificación de los parámetros de giro.

46 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 4.2. Identificación

4.2.5. Identificación del ruido

La caracterización de los ruidos de medida de los sensores, IMU y encoders, no es tan
representativo para describir matemáticamente el vehículo, pero si es muy importante para
incluirlos en el simulador, así cuando se esté tratanto de optimizar el control, el resultado de las
simulaciones serán mucho más similares a las que se obtendrían en la realidad.

Identificación del ruido de la IMU

Para poder conocer el ruido de la IMU es necesario realizar un ensayo en el que, sin necesidad
de que se esté equilibrando el vehículo, sencillamente los motores giren un determinado tiempo
en un sentido y después en el contrario, a diferentes velocidades. Así aparecerán ruidos en la
IMU, debidos a su propio error de medida y también a las vibraciones del vehículo.

Con esta información y con ayuda de un osciloscopio de Matlab, se mide la amplitud del
ruido respecto del valor que debería tener la medida en régimen permanente. Esta varianza del
ruido será la que se utilice dentro del simulador.

Identificación del ruido del encoder

Para caracterizar el ruido del encoder se emplea el mismo ensayo que para identificar el
ruido de la IMU.

De nuevo, se analiza la amplitud del ruido respecto del valor de la medida en régimen
permanente y se registra su varianza para incluirla en la simulación.

Los resultados de la identificación de ambos ruidos se encuentran recogidos en la

Sección 6.4.

4.2.6. Retardo del lazo de control

Como final a esta sección, es indispensable mencionar que se ha incluido un parámetro
adicional en el modelo, motordelay = 30 [ms], que recoge todos los retardos existentes en el lazo
de control. Este parámetro es fundamental si se quiere estudiar en simulación el comportamiento
real del vehículo. Si se omite, controles óptimos en simulación pueden ser inestables en la
realidad.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 47

Carmen Jiménez Cortés
 5
Sistema de Control

A lo largo de este capítulo se describe cuál ha sido la estrategia de control elegida para
el vehículo equilibrista, Sección 5.1; el optimizador del control, Sección 5.2; la máquina
de estados que gobierna los procesos, Sección 5.3 y el estimador de estado empleado,
Sección 5.4.

En la Sección 5.1.1, se explica por qué se ha escogido un control por realimentación

del estado y en qué consiste el mismo. En las secciones 5.1.3 y 5.1.4, se explica su
transformación en dos PIDs en cascada.

El optimizador del control, descrito en la Sección 5.2, ha sido la herramienta que ha

permitido obtener los mejores controles para el vehículo.

De la máquina de estados se han detallado todas las variables que intervienen en los
diferentes estados y transiciones, Sección 5.3.1, determinando así el comportamiento del
vehículo. También se ha incluido un esquema y se ha explicado su funcionamiento en
detalle, Sección 5.3.2.

El estimador de estado que se decidió implementar fue un Filtro Extendido de Kalman,

Sección 5.4.4, pero anteriormente se utilizó un filtro complementario, Sección 5.4.2. En
la Sección 5.4.1 se expresa el motivo por el cual es necesario utilizar un estimador de
estado en este proyecto y cómo se ha integrado en el lazo de control, Sección 5.4.6.

Finalmente, se describe cómo es el diagrama de Simulink que integra todo el sistema de

control, en la Sección 5.5.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 49

Carmen Jiménez Cortés
Capítulo 5. Sistema de Control

5.1. Control por realimentación del estado

5.1.1. Introducción a la estrategia de control
Pese a que el control PID es el más extendido y empleado en la industria, ya que da solución a
la gran mayoría de los problemas de control que se plantean en la realidad, no proporciona una
solución satisfactoria cuando se debe trabajar con plantas con dinámicas complejas, como ocurre
con el vehículo equilibrista, que posee una planta con doble integrador cuando se pretende que
siga la pared.

La principal alternativa para un planta monovariable inestable como la de este proyecto

consiste en el control por realimentación de estados.

El control por realimentación se caracteriza por utilizar la información de un conjunto

de variables, denominadas variables de estado del sistema. Gracias a ellas y a las variables
controladas se pueden obtener los mandos para todos los actuadores. Dado que la mayoría de
estas variables de estado deben ser medidas por sensores, es habitual incluir un estimador de
estado en el lazo de control que minimice el efecto de los errores en las medidas de dichos
sensores e incluso permita prescindir de alguno de ellos, reduciendo así el coste del proyecto,
manteniendo la fiabilidad.

Las variables de estado de la planta del vehículo equilibrista son:

Velocidad lineal de avance [m/s]

Velocidad angular de cabeceo [rad/s]

Ángulo de cabeceo [rad]

Si además se quiere controlar el giro del vehículo se deben añadir:

Ángulo de guiñada [rad]

Velocidad angular de guiñada [rad/s]

Y finalmente, si se desea seguir la pared hay que añadir:

Distancia a la pared [m]

En el caso de querer que el vehículo siga alguna referencia, como podría ser la pared o
una línea, habría que incluir las distancias a los objetivos como variables de estado dentro del
modelo de giro.

Como estimador de estado se ha utilizado un filtro extendido de Kalman, o EKF, el cual se

explica detalladamente en la Sección 5.4.

El aspecto global de lazo de control para la estabilización del vehículo equilibrista sería el
que se muestra a continuación, en la Figura 5.1.

La principal ventaja del diseño de controles por realimentación de estado reside en que
permite fijar las dinámicas de lazo cerrado en tiempo continuo, mediante el posicionado de
los autovalores de la matriz de estado (Matriz A). Esta estrategia resulta de gran interés
cuando la planta tiene polos con parte real positiva (inestables), aparecen integradores, el
sistema es muy poco amortiguado o se trata de un sistema resonante, ya que permite eliminar
completamente estas dinámicas conflictivas. Esta ventaja es a su vez su mayor inconveniente,
ya que al fijar los polos en una determinada posición, como resultado varían los elementos de

50 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 5.1. Control por realimentación del estado

Figura 5.1. Esquema teórico de la implantación del control integral por realimentación del estado.

la matriz de ganancias del control, mientras que con un control PID se disponen de diferentes
parámetros y ganancias que resultan mucho más intuitivos de modificar y analizar su efecto en
la planta.

El diseño de un control por realimentación del estado es en un proceso iterativo.

Se comienzan fijando unos autovalores de la matriz de estado que cumplan los requisitos
de estabilidad y amortiguamiento indispensables.

Una vez alcanzado dicho objetivo, se comienza a tratar de mejorar la respuesta, haciéndola
más rápida, por ejemplo. Hasta que finalmente se obtienen la mejor combinación de polos
para el sistema.

Las primeras iteraciones para ajustar el control se llevaron a cabo por prueba y error
sobre el propio vehículo, hasta que se consiguió un modelo lo suficientemente preciso como
para optimizar el control mediante simulación. El optimizador del control se explica en la
Sección 5.2, y los resultados del ajuste del control en las secciones 6.5 para el ajuste por tanteo,
y las secciones 6.6.1 y 6.6.2 para los controles obtenidos con el optimizador.

En el Capítulo 4 se explicó que para simplificar el estudio de la planta, se han desacoplado

los movimientos de avance y cabeceo, y del giro del vehículo sobre su eje z. Por ese motivo, los
controles de estabilización y guiñada están desacoplados y se tratarán de forma separada a lo
largo de este capítulo.

5.1.2. Diseño del control por realimentación del estado para la

estabilización del vehículo
Para el control de estabilización del vehículo se han diseñado dos posibles controles:

Un control por realimentación de estados

Un control por realimentación de estados con seguimiento a referencia mediante acción

integral.

Es importante realizar esta distinción, pues la forma de construir los dos controles es
diferente. En esta sección se van a desarrollar ambos, comenzando con el regulador.

Control por realimentación del estado

En primer lugar, al tratarse de una planta no lineal, es necesaio linealizar el sistema en torno
a un punto de trabajo. El punto de trabajo será el que se corresponde con el vehículo quieto en
equilibrio. Las ecuaciones de estado linealizadas resultan:

Control de un Vehículo Equilibrista mediante una Raspberry Pi 51

Carmen Jiménez Cortés
Capítulo 5. Sistema de Control

d X dF dF
= |X ,U · X+ |X ,U · U (5.1)
dt dX 0 0 dU 0 0

dG dG
Y = |X ,U · X+ |X ,U · U (5.2)
dX 0 0 dU 0 0

d X
=A· X +B· U (5.3)
dt

Y =C· X +D· U (5.4)

Donde el vector de estado incremental es:

2 3
v v0
6 7
X = 4! !0 5 (5.5)
✓ ✓0

El vector de salidas:
h i
Y = v v0 (5.6)

Y el vector de mandos:
h i
U= u u0 (5.7)

El subíndice 0 representa el valor de dicha variable en el punto de trabajo.

A continuación, como se trata de un control digital, es necesario discretizar la planta de forma

exacta. Esto se lleva a cabo mediante la función c2d de Matlab, que utiliza una discretización
del tipo zero order hold. Se obtiene por tanto la siguiente representación de estado en tiempo
discreto:

X[k + 1] = Ad · X[k] + Bd · U [k] (5.8)

Y [k] = Cd · X[k] + Dd · U [k] (5.9)

El subíndice d indica que se trata de matrices en tiempo discreto.

Finalmente, se fijan las dinámicas deseadas en lazo cerrado y tiempo continuo. Como se
verá en la Sección 6.5, el control más adecuado para el vehículo es un control con 2 polos
dominantes, en una configuración muy similar aun Butterworth de 2º orden. Con ayuda del
comando place de Matlab, se obtiene la matriz de ganancias del regulador K.

La matriz de ganancias K garantiza que el valor de las variables de mando del control U [k]
serán aquellas que garanticen las dinámicas deseadas en tiempo contínuo y lazo cerrado:

52 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 5.1. Control por realimentación del estado

U [k] = K· X[k] (5.10)

Esta afirmación se demuestra sustituyendo la Ecuación 5.10 en la Ecuación 5.8. Se obtiene

entonces:

X[k + 1] = (Ad Bd · K) · X[k] (5.11)

Resultando la nueva matriz de estado en tiempo discreto: (Ad Bd · K). Se observa cómo
en el cálculo de los nuevos autovalores de la matriz de estado interviene K, si no fuera de esta
forma, resultaría imposible modificar las dinámicas de la planta.

Control por realimentación del estado con seguimiento a referencia mediante acción integral

Si se quiere que el vehículo tenga una determinada velocidad de avance, se debe incluir
la acción integral para el seguimiento de dicha referencia dentro del diseño del regulador.
No se ha contemplado la posibilidad de utilizar como herramienta para el seguimiento de
referencia un simple ajuste de ganancia, porque no garantiza error de seguimiento nulo en
régimen permanente, y además es muy dependiente del modelo del vehículo.

El procedimiento de construcción del regulador parte de ampliar las matrices de la

representación de estado, ya que es necesaria una nueva variable de estado auxiliar para
la acción integral:

" # " # " # " # " #

X[k + 1] Ad 03x1 X[k] Bd 03x1
= · + · U [k] + · Yref [k]
Xi [k + 1] Ts · C d 1 Xi [k] Ts · D d Ts · 1
(5.12)

" # " #
X[k + 1] X[k]
= Aad · + Duad · U [k] + Brad · Yref [k] (5.13)
Xi [k + 1] Xi [k]

Siendo en la Ecuación 5.13:

Xi es el nuevo estado debido al retardo de la acción integral.

Aad es la matriz ampliada de estado.

Buad es la matriz ampliada de entrada.

Brad es una nueva matriz, para seguir la referencia de velocidad de avance.

03x1 una vector de ceros de tres filas y una única columna.

A continuación se diseña la matriz de ganacias Ka = [Kcd, Ki ] del control, fijando las

dinámicas de lazo cerrado en tiempo discreto y usando de nuevo el comando place de Matlab.
El polo correspondiente a las variable de estado adicional para la acción integral se aleja de los
polos dominantes, para que no influya significativamente en la respuesta del sistema en lazo
cerrado.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 53

Carmen Jiménez Cortés
Capítulo 5. Sistema de Control

Como se ha comentado en la Sección 5.1.1, el diseño de controles por realimentación de

estados no es tan intuitivo ni flexible como ocurre en cambio con los controles PID. Por este
motivo, el control por realimentación del estado se transforma en dos controles PID en cascada:
El lazo interior se encarga de controlar el ángulo de cabeceo, y el lazo exterior de la velocidad
lineal de avance.

Figura 5.2. Esquema ilustrativo de la implantación de los controles PID en cascada.

En la Figura 5.2, uc es la tensión común a aplicar a los motores, x(1), x(2) y x(3) son las
variables de estado: velocidad lineal de avance, velocidad angular de cabeceo y ángulo de
cabeceo, respectivamente.

En la Sección 5.1.3 se describe cómo se relacionan los términos de la matriz de ganancias del
regulador con los parámetros de un control P-PD en cascada, y en la Sección 5.1.4, la relación
de las ganancias del control integral por realimentación de estados con un control PI-PD.

5.1.3. Transformación del control por realimentación del estado en

un control P-PD en cascada
Analizando el diagrama de la Figura 5.1, se observa cómo el mando del control no es más
que el resultado de sumar, con signo negativo, todas las variables de estado multiplicadas por
su respectiva ganancia del regulador. Por lo que si se desean relacionar dichas variables dentro
de un lazo de control, con la estructura de un P-PD, bastará con relacionar dichas ganancias y
las relaciones físicas que existen entre las propias variables con los parámetros que intervienen
en un control PID:

Figura 5.3. Relación entre las variables de estado y el mando.

54 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 5.1. Control por realimentación del estado

El lazo interior controla el ángulo de cabeceo mediante un control PD, por lo que debe
contener tanto a x(3), que es el ángulo de cabeceo, como a x(2), que es la velocidad
angular de cabeceo, su derivada.

La velocidad lineal de avance se controla en el lazo exterior mediante un control P, por lo

que el error entre la referencia y la variable de estado x(1) se multiplicará por la ganancia
del control P.

Poniendo todo esto en conjunto, resulta el diagrama de la Figura 5.4:

Figura 5.4. Equivalencia entre ganancias del regulador y control P-PD en cascada.

El principal inconveniente de usar un control P para la velocidad lineal de avance, es que

habrá un error en el seguimiento de dicha referencia en régimen permanente. Esto no es tan
importante en alguno de los modos de funcionamiento del vehículo equilibrista, por ejemplo,
cuando se quieren realizar ensayos para verificar el funcionamiento de la IMU y encoders
mientras el vehículo se equilibra.

5.1.4. Transformación del control por realimentación del estado en

un control PI-PD en cascada
Para garantizar error nulo de seguimiento a referencia en régimen permanente para la
velocidad de avance, es indispensable usar un control con acción integral. El razonamiento
para obtener la relación entre los parámetros del PI y el control integral por realimentación de
estados parte de lo representado por la Figura 5.3, únicamente añadiendo un integrador y una
ponderación de la referencia en el PI de velocidad.

La ganancia Ki recoge la constante de tiempo de la acción integral 1/T i y la propia ganancia

del control proporcional. Se encuentran relacionadas de la siguiente forma:

K1
Ti = (5.14)
K4

K4 es la gananancia del regulador que corresponde a la variable de estado adicional que

aparece al ampliar las matrices del control por realimentación de estados, para poder incluir el
seguimiento de la referencia de velocidad de avance.

K1
Ki = (5.15)
K3

1
Ki = ⇤ K1 (5.16)
Ti

Control de un Vehículo Equilibrista mediante una Raspberry Pi 55

Carmen Jiménez Cortés
Capítulo 5. Sistema de Control

El lazo de control resultante se puede observar en Figura 5.5:

Figura 5.5. Equivalencia entre ganancias del regulador y control PI-PD en cascada.

Este control es el que usa habitualmente el vehículo, ya que es conveniente que mantenga
una velocidad concreta según la aplicación.

5.1.5. Control de navegación

Para compensar la falta de simetría de los motores, que supone que el vehículo no se
desplace por defecto en línea recta, o bien para poder seguir una referencia de giro al navegar,
es indispensable implementar un control que actúe sobre la tensión diferencial de los motores,
modificando así su velocidad angular y su ángulo de guiñada.

Este control es simplemente un control PD, no existen dos controles en cascada, ya que las
variables de estado que intervienen en el giro son, como se ha dicho anteriormente:

Velocidad angular de guiñada [rad/s]

Ángulo de guiñada [rad]

Y como la velocidad angular de guiñada es la derivada del ángulo, es suficiente con un único
lazo de control.

El proceso de diseño del regulador es el mismo que se expresó en Sección 5.1.2, con las
diferencias de que el mando del control es ahora la tensión diferencial de los motores, y
que el vector de estado ahora lo componen la velocidad angular de guiñada y el ángulo de
guiñada.

5.1.6. Control para el seguimiento de pared

Circuito del laboratorio

El control para el seguimiento de la pared está orientado a que el vehículo pueda

completar una vuelta en los circuitos que se encuentran en los laboratorios de la universidad,
Figura 5.6.

El circuito consiste en dos curvas de 180º y dos rampas. Las dificultades de completar el
circuito residen en que el vehículo debe ser capaz de inclinarse hacia delante en la rampa de
subida, frenar en el instante anterior a entrar en la curva, conseguir completar dicha curva,
frenar e inclinarse hacia atrás para superar la rampa de bajada, y finalmente completar la última
curva para llegar al punto de origen. Por lo que se deduce que el control debe ser muy robusto,
para conseguir mantener al vehículo en equilibrio en condiciones tan diversas.

56 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 5.1. Control por realimentación del estado

Figura 5.6. Circuito del laboratorio que debe completar el vehículo.

Seguimiento de pared

Para el seguimiento de pared, se ha añadido al control de la Sección 5.1.5 una nueva variable
de estado: la distancia a la pared [m].

Cabría esperar que la estrategia de control elegida fuera un control integral por
realimentación de estados (que posteriormente se transformaría en un PI-PD en cascada). Pero
se ha observado que resulta más conveniente emplear un simple control por realimentación del
estado, y transformarlo en un P-PD, ya que se obtienen mejores resultados si se permite que el
vehículo se aproxime más a la pared cuando trata de tomar las curvas.

Una vez obtenido el regulador, al igual que se hizo en la Sección 5.1.3, se despejan los
coeficientes de los controles P y PD.

El razonamiento se basa de nuevo en lo planteado en la Figura 5.3, con la diferencia de que

ahora las variables de estado son: la velocidad angular de guiñada, x(1), el ángulo de guiñada,
x(2) y la distancia a la pared x(3). El mando es la tensión diferencial.

Además, se debe considierar que:

El lazo exterior debe ser el de control de la distancia, ya que la referencia del ángulo de
guiñada depende del error existente entre la referencia de distancia y la real medida.

La velocidad angular de guiñada es la derivada del ángulo de guiñada, por lo que se deben
encontrar en el mismo lazo de control y relacionadas a través de la acción diferencial.

Poniendo todo esto en conjunto, resulta un esquema como el de la Figura 5.7, en la que se
muestran las correspondencias entre las ganancias y los parámetros de los controles PID.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 57

Carmen Jiménez Cortés
Capítulo 5. Sistema de Control

Figura 5.7. Equivalencia entre ganancias del regulador y control P-PD en cascada para el movimiento de
giro.

5.2. Optimizador del Control

El optimizador del control se recoge en un script de Matlab, que consiste en utilizar de nuevo
el algoritmo de Gauss-Newton, Sección 4.2.2, para buscar la combinación de parámetros que
minimizan la función de coste, al igual que se hizo en las identificaciones de los parámetros del
modelo, secciones 4.2.3 y 4.2.4.

Este optimizador se emplea para todos los parámetros de los distintos controles que se
encuentran programados para el vehículo y también para las varianzas y los ruidos del EKF. Por
lo que en total dispone de una lista de 39 posibles parámetros.

También es posible escoger el modo de ajuste a través de la variable F itM ode , que determina
qué variables se utilizan en el proceso de optimización. Estas dependen de si lo que se pretende
ajustar es el control o el EKF y de qué control se trate. Los siete posibles modos de ajuste son
los siguientes:

1. Control - Cabeceo.

2. Control - Velocidad angular de cabeceo y cabeceo.

3. Control - Velocidad angular de cabeceo, cabeceo y velocidad lineal de avance.

4. Velocidad angular de cabeceo, cabeceo, velocidad lineal de avance y velocidad de guiñada.

5. EKF - Cabeceo.

6. EKF - Velocidad angular de cabeceo y cabeceo.

7. EKF - Velocidad angular de cabeceo, cabeceo y velocidad lineal de avance.

Las condiciones de la simulación, es decir, las referencias de velocidad de avance y guiñada,

se fijan externamente, escribiéndolas directamente en el estado 10 de la máquina de estados,
Sección 5.3. Por ejemplo, podría tratar de optimizarse el control PI-PD para que el vehículo
siguiera un tren de escalones en la velocidad de avance.

Los parámetros a optimizar se eligen al inicio del código y pueden combinarse entre sí. Una
vez ejecutado, el programa llama al Simulink de la Sección 5.5, dentro de un bucle while y
comienza a realizar simulaciones para diferentes combinaciones de los parámetros previamente
indicados, y cuando obtiene un mejor valor de la función de coste lo muestra por pantalla, y
queda a la espera de que se lance una nueva iteración. El programa se detiene cuando bien la
función de coste no es capaz de reducirse más de un 1 % o alguno de los parámetros no varía
tampoco en más de un 1 %.

58 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 5.3. Máquina de estados

De nuevo, destacar que el valor que muestra el optimizador, no es el valor absoluto del
parámetro, si no el coeficiente por el que hay que multiplicar el valor inicial del mismo en la
iteración.

Los resultados obtenidos para el control P-PD se encuentran en la Sección 6.6.1 y para el
PI-PD en la Sección 6.6.2.

5.3. Máquina de estados

Como en todo programa que se ejecute en un microcontrolador, es necesaria una máquina
de estados que, en función del valor de una serie de variables auxiliares y del estado actual del
proceso, decida cuál debe ser el siguiente estado y actualice el valor de dichas variables. La
máquina de estados es indispensable para poder gestionar todos los procesos que deben llevarse
a cabo desde el encendido del vehículo, hasta cuando pueda empezar a actuar el control y
también para su correcta detención.

5.3.1. Variables que determinan el comportamiento del vehículo

equilibrista
Dada la complejidad del vehículo, se han programado diferentes modos de funcionamiento
para el mismo. Cada uno de estos modos de funcionamiento o comportamientos viene
condicionado por los valores de una serie de variables auxiliares y el estado en el que se
encuentre la máquina de estados, Sección 5.3.2.

Modos de operación del control

Tiene asociada la variable denominada ControlM ode , que puede tomar los siguientes
valores:

ControlM ode = 0: Inicialización. Implica que el control se está inicializando y, por tanto,
no actúa.

ControlM ode = 1, Estimación. El control utiliza la información de los sensores y del EKF
para calcular los mandos, pero no los aplica.

ControlM ode = 2, Control activo. Igual que en el modo de estimación, con la diferencia de
que ahora sí se escriben los valores del mando en sus correspondientes variables.

Modos de estabilización

Tiene asociada la variable denominada StabM ode , que puede tomar los siguientes valo-
res:

StabM ode = 0, Lazo abierto. Los valores de los actuadores no se escriben en el lazo de
control, si no directamente en la máquina de estados. Se utiliza cuando no se quiere
equilibrar el vehículo, por ejemplo al hacer pruebas de hardware.

StabM ode = 1, P-PD en cascada. Equilibra al Balboa con un control proporcional para la
velocidad y porporcional-diferencial para el ángulo de cabeceo.

StabM ode = 2, PI-PD en cascada. Equilibra al Balboa con un control proporcional-integral

para la velocidad y porporcional-diferencial para el ángulo de cabeceo.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 59

Carmen Jiménez Cortés
Capítulo 5. Sistema de Control

Modos de navegación

Tiene asociada la variable denominada N avM ode , que puede tomar los siguientes valo-
res:

N avM ode = 0, Sin Navegación. Este modo de navegación omite el control de guiñada. Se
utiliza cuando se quieren realizar ensayos en los que no es relevante el control del giro
del vehículo.

N avM ode = 1, Seguidor de pared. Activa un control integral de seguimiento a referencia

de distancia a la pared del circuito.

N avM ode = 4, Navegación libre. Permite mantener el vehículo en equilibrio sin necesidad
de estar completando el circuito y además implementar un control de guiñada, que corrija
posibles desviaciones del vehículo, o seguir una referencia de velocidad de guiñada fijada
externamente, por ejemplo para que el vehículo gire sobre su eje z mientras se equilibra.

El motivo por el cual no existen los modos de navegación correspondientes a N avM ode = 2
y N avM ode = 3 ,es porque requerían de elementos hardware que no fue posible implementar
antes de la entrega del proyecto.

Modos de EKF

Tiene asociada la variable denominada EKFM ode , que puede tomar los siguientes
valores:

EKFM ode = 0, Sin EKF. Desactiva el EKF, por lo que se utilizarán las medidas reales.

EKFM ode = 1, Estabilización. El EKF estima las variables necesarias para mantener al
Balboa en equilibrio, es decir, el ángulo de cabeceo, la velocidad angular de cabeceo y la
velocidad lineal de avance.

EKFM ode = 2, Seguidor de pared. Además de las anteriores, se estiman también las
variables de estado que permiten seguir la pared: ángulo de guiñada y velocidad angular
de guiñada.

EKFM ode = 4, Navegación libre. El EKF estima las variables necesarias para mantener al
Balboa en equilibrio mientras se desplaza, es decir, el ángulo de cabeceo, la velocidad
angular de cabeceo y la velocidad lineal de avance.

En este caso, EKFM ode = 3 se correspondería con N avM ode = 3, por lo que no se llegó a
diseñar.

Modos de operación de los motores

Tiene asociada la variable denominada M otorM ode , que puede tomar los siguientes
valores:

M otorM ode = 0, Motores inhabilitados. Los motores permanecerán apagados aunque el

control escriba los valores de las tensiones a aplicar.

M otorM ode = 1, Motores habilitados. Estado que permite mover los motores.

Estados de los sensores

Tiene asociada la variable denominada SensorCalibM ode , que puede tomar los siguientes
valores:

60 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 5.3. Máquina de estados

SensorCalibM ode = 0: Sin calibrar. Es el estado por defecto al encender el vehículo, sin
corregir posibles desviaciones de las medidas.

SensorCalibM ode = 1, Calibrando. En este estado, los sensores están midiendo y se está
calculando la variación media de su medida, para después restarlo, pues se trata de una
desviación constante.

SensorCalibM ode = 2, Calibrados. Una vez que se ha corregido el posible error constante
en la medida, los sensores están listos para ser utilizados con normalidad.

De todas las variables previamente mencionadas, las que condicionan el salto de un estado a
otro dentro de la máquina de estados son únicamente: N avM ode y StabM ode .

Control de un Vehículo Equilibrista mediante una Raspberry Pi 61

Carmen Jiménez Cortés
Capítulo 5. Sistema de Control

5.3.2. Funcionamiento de la máquina de estados

Diagrama de la máquina de estados

Button A = 0 Timer < 5s

Button A = 1
start Booting SensorCalibration

Timer 5s
Button A = 1

Ready HardwareT est

Button A = 1
Button A = 1
Button A = 1

W allF ollower Stabilization F reeN avigation

Button B = 1

Button B = 1 Button B = 1
Stop

Button C = 0

start EKF &Control T uning

Figura 5.8. Máquina de estados.

62 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 5.4. Estimador de estado

Explicación de los procesos de la máquina de estados

El proceso que se lleva a cabo en la máquina de estados de la Figura 5.8 es el siguiente:

1. Inicialización: Cuando se enciende el vehículo, por defecto está en este estado de reposo.
Todas las variables de la Sección 5.3.1 permanecen a cero. Al pulsar el botón A, se pasa al
estado Calibración de Sensores.

2. Calibración de Sensores: Al entrar en este estado SensorCalibM ode pasa a valer 1. En este
estado permanece durante 5 segundos, para así eliminar el offset del girósocopo. Pasado
este tiempo, la variable SensorCalibM ode se pone a 2, y se salta al estado Listo.

3. Listo: En este estado, el valor de las variables N avM ode y StabM ode condicionará cuál será
el siguiente estado, una vez se haya pulsado el botón A.

Si se cumple que N avM ode = 0 y StabM ode = 0, entonces el siguiente estado será
Test Hardware.

Si N avM ode = 0 y StabM ode 1, entonces el siguiente estado será Estabilización.

Si se cumple que N avM ode = 1 y StabM ode 1, el siguiente estado será Seguidor de
Pared.

Si se cumple que N avM ode = 4 y StabM ode 1, el siguiente estado será Navegación
Libre.

4. Estabilización: En este estado, se actualizan las variables a los siguientes valores:

ContolM ode = 2, EKFM ode = 1 y M otorM ode = 1. El resto permanecen a cero.

5. Test Hardware: En este estado, se actualizan las variables a los siguientes valores:
ContolM ode = 1 y M otorM ode = 1. El resto permanecen a cero.

6. Navegación Libre: En este estado, se actualizan las variables a los siguientes valores:
ContolM ode = 2, EKFM ode = 1 y M otorM ode = 1. El resto permanecen a cero.

Además de todos los estados previamente descritos, existen también un par de estados
especiales:

1. Parada de emergencia: Si en cualquier momento de la ejecución del programa se pulsa el

botón B, se salta a este estado, en el que todas las variables vuelven a ponerse a 0. Para
salir de este estado y volver a inicializar el programa, basta con pulsar el botón C, que
actualiza el estado siguiente a Inicialización.

2. EKF y Control Tunning: A este estado solo es posible acceder cuando el programa en
lugar de estar ejecutándose en el Balboa, se ejecuta en el ordenador mediante simulación.
Permite modificar el control y el EKF con la ayuda de un simulador para poder optimizar
los parámetros de forma más eficiente.

5.4. Estimador de estado

5.4.1. Motivación para emplear un estimador de estado
El objeto de utilizar un estimador de estado no es otro que mejorar las medidas
proporcionadas por los sensores. Al haberse elegido un control por realimentación del estado, es
indispensable utilizar numerosos sensores que permitan medir los valores de todas las variables

Control de un Vehículo Equilibrista mediante una Raspberry Pi 63

Carmen Jiménez Cortés
Capítulo 5. Sistema de Control

de estado. Las medidas de estos sensores son ruidosas y pese a que se hayan diseñado filtros
digitales que limpian la señal, siguen sin ser tan precisas como sería deseable para obtener la
mejor respuesta del control. Además, como se trata de una planta con dinámicas inestables, es
necesario utilizar un observador con el fin de estabilizar y amortiguar dichas dinámicas, para
que así no tengan una influencia negativa sobe la respuesta del control.

En este proyecto se han usado dos estimadores de estado distintos: Un Filtro complementario,
Sección 5.4.2 , y un Filtro Extendido de Kalman o EKF, Sección 5.4.4.

5.4.2. Filtro complementario

En una primera fase de diseño del sistema de control, se utilizó un filtro complementario
como estimador de estado.

El giróscopo mide la velocidad angular del vehículo en sus tres ejes, pero tiene dos
inconvenientes:

El primero es que la medida tiene un bias inicial, que altera el valor real de la medida.
Durante la fase de calibración, se toman muestras durante 5 segundos, se calcula dicho
valor medio y se resta en las medidas siguientes.

El segundo problema es que dicho error, con el paso del tiempo, vuelve a aparecer y por
tanto la medida se vuelve cada vez menos fiable, siendo indispensable corregirla.

Como el acelerómetro es capaz de medir las proyecciones de la gavedad en los tres ejes,
cuando el vehículo se desplaza a velocidad constante se usan sus medidas para estimar los
ángulos de inclinación del vehículo en cada eje, compararlos con la medida del giróscopo y
corregir así la medida del giróscopo.

La forma de realizar dicha corrección consiste en realizar una media ponderada de las
medidas del giróscopo y acelerómetro y tomarlo como valor verdadero de la inclinación. Esto
recibe el nombre de Filtro Complementario, y es la primera aproximación que se empleó para
estimar el ángulo de cabeceo del vehículo.

Los valores elegidos para la ponderación fueron de 0.98 para la medida del giróscopo y
1 0.98 = 0.02 para el acelerómetro. No es posible asignarle un mayor peso al acelerómetro,
porque es muy poco habitual que el vehículo se desplace a velocidad constante.

5.4.3. Introducción al filtro de Kalman

El filtro de Kalman y el filtro extendido de Kalman consisten en un conjunto de ecuaciones
matemáticas, que proporcionan una solución eficiente recursiva por mínimos cuadrados. Su
gran potencia se basa en que permite realizar estimaciones de estados pasados, presentes e
incluso futuros. Todas estas estimaciones pueden ser llevadas a cabo incluso cuando no se
dispone del sistema real, sino de un modelo matemático, [20].

R.E. Kalman publicó en 1960 su descubrimiento como una solución recursiva para el filtrado
de datos discretizados linealmente. Desde entonces y gracias a los avances en computación, el
filtro de Kalman se ha convertido en una herramienta ampliamente utilizada, sobre todo en el
sector de la navegación autónoma o asistida.

Es una de las herramientas más sofisticadas para estimar el vector de estado de un sistema,
y por ello se ha decidido incluirlo en este proyecto. En concreto, se ha utilizado para mejorar

64 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 5.4. Estimador de estado

todas aquellas medidas con demasiado ruido o incluso estimar aquellas que no se pueden
medir de manera directa, como el ángulo de cabeceo. Además, ha sido posible optimizarlo por
medio de simulaciones con ayuda de Matlab-Simulink, lo que ha maximizado su eficiencia y
precisión.

En tiempo discreto, toda variable o vector de estado de un sistema se puede expresar

como:
xk+1 = F (xk , uk , wk ) (5.17)

Y las salidas de dicho sistema, como:

zk = G(xk , uk , vk ) (5.18)

Siendo:

xk : El vector de variables de estado en la muestra k

uk : El vector de entradas en la muestra k

wk : El vector de ruido en el proceso en la muestra k

zk : El vector de salidas obtenidas a partir de las medidas en la muestra k

vk : El vector de ruido en las salidas en la muestra k

Ambos ruidos, wk y vk son considerados ruidos blancos, independientes y con una

distribución normal de probabilidad, es decir, son ruidos gaussianos.

Además, la construcción de todo estimador se realiza de la siguiente forma:

x̂k+1 = F (x̂k , uk ) + K ⇤ (zk yk ) (5.19)

x̂k+1 : El vector de variables de estado estimadas en la muestra k+1

x̂k : El vector de variables de estado estimadas en la muestra k

uk : El vector de entradas en la muestra k

zk : El vector de salidas medidas en la muestra k

yk : El vector de salidas estimadas en la muestra k

K: La matriz de corrección

F (x̂k , uk ) en su conjunto se denomina predicción del modelo

K ⇤ (zk yk ) en su conjunto se denomina corrección de la predicción

5.4.4. Filtro extendido de Kalman (EKF)

Para una mejor comprensión del significado de la matriz K y su objetivo, se recomienda leer
previamente a esta sección el Apéndice A.

Todo el desarrollo matemático que se encuentra recogido en el Apéndice A es válido cuando

se está trabajando con un modelo gobernado por una ecuación diferencial estocástica, o lo que

Control de un Vehículo Equilibrista mediante una Raspberry Pi 65

Carmen Jiménez Cortés
Capítulo 5. Sistema de Control

es lo mismo, se trata de un sistema lineal. Esto no ocurre en el problema del péndulo invertido,
por lo que es necesario utilizar el Filtro extendido de Kalman, o EKF.

Para construir un EKF, en primer lugar se debe linealizar el sistema en torno a un punto de
trabajo, de la misma forma que se hizo en la Sección 5.1.2.

Tanto la Ecuación 5.17 como la Ecuación 5.18 se expresan ahora como:

xk+1 = f (xk , uk , wk ) (5.20)

Y las salidas de dicho sistema, como:

zk = h(xk , uk , vk ) (5.21)

Donde las funciones f () y h() son funciones no lineales.

Como es habitual en estos sistemas que los ruidos wk y vk no sean conocidos, se pueden
aproximar el estado y las salidas como:

x̃k+1 = f (x̂k , uk , 0) (5.22)

z̃k = h(x̃k , 0) (5.23)

Donde x̂k es una estimación a posteriori del estado en la muestra k.

Para estimar el estado del proceso no lineal, se comienzan definiendo unas nuesvas
ecuaciones que linealicen la estimación de la Ecuación 5.22 y de la Ecuación 5.23.

xk+1 ' x̃k+1 + A · (xk x̂k ) + W · wk (5.24)

zk ' z̃k + H · (xk x̃k ) + V · vk (5.25)

En estas ecuaciones:

xk+1 y zk son el vector de estado actual y el vector de salida.

x̃k+1 y z̃k son el vector de estado actual y el vector de salida aproximados de la

Ecuación 5.22 y de la Ecuación 5.23.

x̂k es la estimación a posteriori del estado en la muestra k

wk y vk son los ruidos del proceso y de la medida

Y las matrices que aparecen son los Jacobianos de las ecuaciones:

Ecuación 5.17, f () respecto xk , para obtener la matriz A.

F[i]
A0[i,j] = (x̂k , uk , 0) (5.26)
x[j]

66 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 5.4. Estimador de estado

Ecuación 5.17, f () respecto wk , para obtener la matriz W.

F[i]
W[i,j] = (x̂k , uk , 0) (5.27)
w[j]

Ecuación 5.18, h() respecto xk , para obtener la matriz H.

G[i]
H[i,j] = (x̃k , 0) (5.28)
x[j]

Ecuación 5.18, h() respecto vk , para obtener la matriz V.

G[i]
V[i,j] = (x̃k , 0) (5.29)
v[j]

Se definen también el error de predicción:

ẽk ⌘ xk x̃k (5.30)

Y el residuo de la medida:
ẽk ⌘ zk z̃k (5.31)

Usando las equaciones 5.30 y 5.31, las ecuaciones que gobiernan el error del proceso
resultan:

ẽxk+1 = A · (xk x̂k ) + ✏k (5.32)

ẽzk = H · êxk ⌘k (5.33)

✏k y ⌘k son variables aleatorias independientes, con media cero y matrices de covarianza:

W · Q · W T y V · R · V T . Q y R siguen siendo las matrices de covarianza de la distribución de
los ruidos w y v de las ecuaciones 5.17 y 5.18.

Cabe destacar que tanto la Ecuación 5.32 como la Ecuación 5.33 son lineales, por lo que
se puede utilizar el residuo actual de la medida ẽzk en un hipotético segundo filtro de Kalman,
para estimar así el error en la predicción ẽxk . Esta estimación, denominada êzk , permite obtener
una estimación a posteriori del estado para el proceso no lineal original:

x̂k = x̃k êk (5.34)

Dadas todas las aproximaciones para que êk sea cero, se obtiene una ecuación para estimar
êk

êk = Kk ẽzk (5.35)

Control de un Vehículo Equilibrista mediante una Raspberry Pi 67

Carmen Jiménez Cortés
Capítulo 5. Sistema de Control

Si se sustituye la Ecuación 5.35 en la Ecuación 5.34 resulta que no es necesario el hipotético

segundo filtro de Kaman:

x̂k = x̃k + Kk · ẽzk = x̃k + Kk · (zk z̃k ) (5.36)

La Ecuación 5.36 puede usarse para actualizar la medida y la matriz de ganancias de Kalman
Kk junto con x̃k y z̃k de las ecuaciones 5.22 y 5.23.

5.4.5. Diseño y optimización del EKF

El proceso de operación del Filtro Extendido de Kalman parte de unas estimaciones iniciales
para x̂k y Pk , y consta de dos partes claramente diferenciadas:

Actualización de la medida o Corrección

1. Para los valores iniciales de x̃k y P̃k se calcula la matriz de ganancias de Kalman en la
muestra k:
Kk = P̃k · HkT · (Hk · P̃k · HkT + Vk · Rk · VkT ) 1 (5.37)

2. Se actualiza el estado con la medida zk

x̂k = x̃k + K · (zk h(x̃k , 0)) (5.38)

3. Se actualiza la covarianza del error

Pk = (I Kk · Ck ) · P̃k (5.39)

Actualización del tiempo o Predicción

1. Se estima el estado de la siguiente muestra

x̃k+1 = f (x̂k , uk , 0) (5.40)

2. Se estima la covarianza del error en la siguiente muestra

P̃k+1 = Ak · Pk · ATk + Wk · Qk · WkT (5.41)

3. Con estas nuevas estimaciones de x̃k+1 y P̃k+1 se repiten los procesos de Corrección y
Predicción.

Todos estos cálculos se realizan mediante Matlab y es el proceso que supone una mayor
carga computacional de todo el proyecto, ya que deben realizarse para cada muestra, siendo la
frecuencia de muestreo de la IMU 1 ms.

5.4.6. Integración con el sitema de control

Como se ha explicado el la Sección 5.4.3, el filtro extendido de Kalman es de gran utilidad
en navegación autónoma, por ello se ha empleado en este proyecto como estimador de las
variables de estado responsables del equilibrio del vehículo: el ángulo de cabeceo, la velocidad
angular de cabeceo y la velocidad de avance lineal.

68 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 5.5. Simulink para el control del vehículo

Para intregrar el EKF en el lazo de control, se comenzó en primer lugar con el ángulo de
cabeceo y la velocidad de cabeceo. Mientras el vehículo se equilibraba, y mediante Matlab-
Simulink, se compararon en un mismo osciloscopio las medidas obtenidas a partir de la IMU y
las estimadas por el EKF. Se fueron modificando los diferentes pesos hasta que finalmente se
obtuvieron unas estimaciones de dichas variables de estado muy similares a las reales pero con
menos ruido.

Una vez conseguido este primer objetivo con el EKF, y cuando ya se disponía de un modelo
lo suficientemente preciso como para poder diseñar y optimizar mediante simulaciones, se
comenzaron a integrar el resto de variables a estimar.

Para el movimiento de avance y cabeceo

Ángulo de cabeceo [rad]

Velocidad angular de cabeceo [rad/s]

Velocidad lineal de avance [m/s]

Para el movimiento de giro

Ángulo de guiñada [rad]

Velocidad angular de guiñada [rad/s]

Para el seguimiento de pared

Todas las anteriormente mencionadas

Distancia a la pared [m]

En lugar de utilizar un osciloscopio, mientras el vehículo se equilibraba, para comparar los

valores de las variables de estado obtenidas a partir de las medidas de los sensores con las
estimadas por el EKF, mientras se modificaban los elementos de Kk , se utilizó un simulador
y los datos registrados en distintos ensayos con el vehículo. Esta forma de proceder redujo
significativamente el tiempo necesario para el ajuste de cada ganancia y además permitió
obtener mejores resultados que los que podrían haberse alcanzado por prueba y error.

5.5. Simulink para el control del vehículo

Todos los elementos del lazo que se han descrito a lo largo de este capítulo se integran en el
diagrama de Simulink de la Figura 5.9.

La información se almacena en buses, que son los tres Data Store Memory que aparecen en
la parte superior del diagrama. Trabajar de esta forma permite leer y escribir todas las variables
que almacena cada bus desde cualquier otra parte del programa. Para una mayor claridad, todas
las variables que contienen estos buses se inicializan desde los scripts de configuración, para así
definir claramente sus dimensiones y tipo de dato almacenado.

El bus de Control contiene todas las variables que son necesarias para la actuación del
control. Esto abarca desde las medidas de los sensores hasta los valores estimados por el
EKF, y también los valores de las salidas y mandos generados por el control.

El bus del modelo contiene todos los parámetros que identifican al vehículo, los sensores,
los actuadores e incluso al circuito del laboratorio.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 69

Carmen Jiménez Cortés
Capítulo 5. Sistema de Control

Figura 5.9. Estructura del diagrama de Simulink.

El bus de Mavlink contiene todas las variables que intervienen en las comunicaciones entre
la estación base del PC y la Raspberry Pi cuando se encuentra ejecutando el programa en
modo autónomo.

El resto de subsistemas que aparecen se comentan o descomentan según el modo de

implementación del programa. En la Figura 5.9 se puede observar que los bloques de Hardware
y Computational Load se encuentran comentados. Esto es porque el modo de implementación
que se había elegido era el de simulación, y por tanto no intervienen ni los elementos físicos ni
la carga computacional de la Raspberry Pi.

En el subsistema de Control, se encuentran la máquina de estados y el propio control,

donde a partir de las medidas, las estimaciones del EKF y el tipo de control elegido, se
calculan los mandos a aplicar.

El subsistema de Hardware está a su vez formado por otros dos subsistemas: Sensores y
Actuadores, que respectivamente contienen las diferentes funciones de Matlab encargadas
de controlar cada uno de estos elementos Hardware. Las funciones de Matlab de cada
sensor y actuador se encentran también recogidas en un subsistema propio para una
mayor claridad del diagrama.

El subsistema de Monitorización, también se encuentra formado por cuatro subsistemas:

Monitorización en modo externo, monitorización de la simulación, monitorización vía
Mavlink en modo autónomo y la ’caja negra’, que permitiría visualizar la información
almacenada en la tarjeta SD de la Raspberry Pi, aunque en este proyecto no se ha utilizado.
Cada subsistema de monitorización contiene los osciloscopios que permiten visualizar y
registrar el valor de posibles variables de interés.

Finalmente, el subsistema de Simulación. Contiene el modelo del vehículo, el de los

sensores, el de la pared e incluso el del circuito. También bloques que permiten incluir el
retardo del lazo de control y los ruidos de las medidas.

70 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 5.5. Simulink para el control del vehículo

En la Figura 5.10 se puede ver un esquema de los distintos subsistemas previamente

descritos.

Figura 5.10. Esquema de los subsistemas que contiene el diagrama de Simulink.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 71

Carmen Jiménez Cortés
 6
Resultados

A lo largo de este capítulo se recogen todos los resultados de los ensayos que se han
mencionado en secciones anteriores.

En primer lugar, en la Sección 6.1 se explican los primeros ensayos realizados a los
motores: Medición de la resistencia interna del motor y obtención de la curva de
caracterización Ganancia-Tensión.

A continuación, en las secciones 6.2, 6.3 y 6.4 se muestran los resultados obtenidos al
caracterizar los parámetros mecánicos del vehículo que intervienen en las ecuaciones de
los movimientos de avance y cabeceo, Sección 6.2, y de giro, Sección 6.3. También la
identificación de la varianza del ruido de la IMU y del encoder, útil para poder realizar
simulaciones más precisas, Sección 6.4.

Finalmente, en la Sección 6.5 se exponen los resultados y valores para el control obtenidos
en las primeras iteraciones por tanteo, hasta que se consiguió caracterizar el vehículo.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 73

Carmen Jiménez Cortés
Capítulo 6. Resultados

6.1. Identificación de los parámetros de los motores

Como se expresaba en la Sección 4.2.1, fue necesario realizar ensayos a los motores
para obtener una primera aproximación de los parámetros que los caracterizaban, de forma
independiente al vehículo.

6.1.1. Medida de la resistencia interna del motor

Es el ensayo más simple e inmediato de todos. Consiste en, con ayuda de un polímetro,
medir las resistencias de ambos motores. Se obtuvo un valor de 4.5 ⌦.

6.1.2. Medida de la relación Ganancia-Tensión-Incremento de enco-

der
Para cada motor por separado, se fueron aplicando diferentes valores de ganancia entre -300
y +300. Con un polímetro se medía la tensión real que se estaba aplicando al motor, con su
signo, y también se monitorizaba en Matlab-Simulink los valores de las cuentas en crudo que
estaba midiendo el encoder correspondiente.

Para cada motor se comenzó el ensayo con baterías nuevas y se aplicaron las mismas
ganancias, con el objetivo de que ambos ensayos se hubieran realizado en igualdad de
condiciones. Los resultados de estos ensayos se muestran en las siguientes tablas:

74 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 6.1. Identificación de los parámetros de los motores

Motor izquierdo

Los resultados del ensayo para el motor izquierdo se muestran en la Figura 6.1. Los cuadros
en rojo indican que para dicho valor de ganancia, el motor comenzó a moverse.

Figura 6.1. Resultados Ganancia-Tensión para el motor izquierdo.

Representando los datos anteriores en una gráfica, se obtienen la Figura 6.2 para valores de
ganancia positivos, y la Figura 6.3 para los negativos.

Se concluye entonces que los valores de la relación ganancia-tensión para el motor izquierdo
son:

0.01865, para ganancias positivas.

-0.01753, para ganancias negativas.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 75

Carmen Jiménez Cortés
Capítulo 6. Resultados

Relación Ganancia-Tensión, Motor Izquierdo

6

Tensión (V)

0
0 50 100 150 200 250 300
Ganancia

Figura 6.2. Recta de calibración Ganancia-Tensión para el motor izquierdo, ganancias positivas.

Relación Ganancia-Tensión, Motor Izquierdo

0

-1

-2
Tensión (V)

-3

-4

-5

-6
-300 -250 -200 -150 -100 -50 0
Ganancia

Figura 6.3. Recta de calibración Ganancia-Tensión para el motor izquierdo, ganancias negativas.

Se han separado en las gráficas las ganancias positivas de las negativas, porque realmente
fueron dos ensayos distintos. Ambos empezaban con baterías nuevas y tras un periodo de
inactividad del motor.

76 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 6.1. Identificación de los parámetros de los motores

Motor derecho

Los resultados del ensayo para el motor derecho se muestran en la Figura 6.4. Los cuadros
en rojo indican que para dicho valor de ganancia, el motor comenzó a moverse.

Figura 6.4. Resultados Ganancia-Tensión para el motor derecho.

Representando los datos anteriores en una gráfica, se obtienen la Figura 6.5 para valores de
ganancia positivos, y la Figura 6.6 para los negativos.

Se concluye entonces que los valores de la relación ganancia-tensión para el motor derecho
son:

-0.019057, para ganancias positivas.

0.01824, para ganancias negativas.

Es sorprendente cómo el motor derecho tiene la relación ganancia-tensión con el signo

opuesto al que cabría esperar. Este detalle hubo que incluirlo en el código, para que el control
generara los mandos adecuados.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 77

Carmen Jiménez Cortés
Capítulo 6. Resultados

Relación Ganancia-Tensión, Motor Derecho

0

-1

-2

Tensión (V)

-3

-4

-5

-6
0 50 100 150 200 250 300
Ganancia

Figura 6.5. Recta de calibración Ganancia-Tensión para el motor derecho, ganancias positivas.

Relación Ganancia-Tensión, Motor Derecho

6

4
Tensión (V)

0
-300 -250 -200 -150 -100 -50 0
Ganancia

Figura 6.6. Recta de calibración Ganancia-Tensión para el motor derecho, ganancias negativas.

De nuevo, se han separado en las gráficas las ganancias positivas de las negativas, porque
realmente fueron dos ensayos distintos. Ambos empezaban con baterías nuevas y tras un periodo
de inactividad del motor.

6.2. Resultados de la identificación del movimiento de

avance y cabeceo
Como se ha expresado anteriormente en este documento, una de las mayores dificultades
ha sido conseguir identificar todos los parámetros dinámicos que caracterizan al vehículo.
Tras numerosas aproximaciones, modelos y estrategias de optimización, los resultados más
satisfactorios, para el avance y cabeceo, se obtuvieron con el simulador que contenía un modelo
ducplicado de la planta, como se explica en Sección 4.2.3.

78 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 6.2. Resultados de la identificación del movimiento de avance y cabeceo

En la Figura 6.7 se muestran dos gráficas, la primera gráfica recoge la optimización de la

velocidad angular de cabeceo [rad/s], y la segunda la velocidad lineal de avance [m/s]. En
dichas gráficas aparecen:

En color rojo, la medida real registrada durante el ensayo

En color azul, la medida simulada con el modelo

En color verde, el error entre ambos gráficas.

Figura 6.7. Medida real, la generada por el simulador y el error entre ambas para la velocidad angular
de cabeceo y velocidad lineal de avance.

Se puede observar que las discrepancias entre las variables medidas y estimadas son poco más
que el propio ruido de los sensores, por lo que se concluye que los parámetros proporcionados
por el algortimo Gauss-Newton serán adecuados. El valor de estos parámetros es de:

Momento de inercia de cabeceo del chasis respecto a un eje principal que pasa por su cdg
I✓ = 1.511 · 10 4 [kgm2 ]

Momento de inercia del conjunto motor y rueda Imr = 1.405 · 10 5 [kgm2 ]

Distancia del eje de la rueda al centro de gravedad del vehículo h = 1.414 · 10 4 [m]

Constante eléctrica Ke = 0.0584 [Vs/rad]

Constante mecánica Kt = 0.0636 [Nm/A]

Resistencia del motor Rm = 4.53 [⌦]

Par de fricción máxima Trmax = 1.734 · 10 5 [Nm]

Fricción viscosa Dm = 7.95 · 10 5 [Nms/rad]

Caída del voltaje del motor Vdrop = 0.323 [V]

Control de un Vehículo Equilibrista mediante una Raspberry Pi 79

Carmen Jiménez Cortés
Capítulo 6. Resultados

Llama la atención que las constantes Ke y Kt no sean iguales, que es una aproximación
bastante habitual cuando se trata de estudiar el modelo dinámico de un motor de corriente
continua.

6.3. Resultados de la identificación del movimiento de

giro
Identificar los parámetros que intervienen en las ecuaciones de giro del vehículo tampoco
fue sencillo, pero no fue necesario emplear un modelo duplicado de la planta, como se ha
expresado en la Sección 4.2.4.

De nuevo, en la Figura 6.8 se muestra una gráfica de la velocidad angular de guiñada [rad/s],
en donde aparece:

En color rojo, la medida real registrada durante el ensayo

En color azul, la medida simulada con el modelo

En color verde, el error entre ambos gráficas.

Figura 6.8. Medida real, la generada por el simulador y el error entre ambas para la velocidad angular
de guiñada.

Dada la pequeña diferencia o error que existe entre ambas gráficas, se han considerado
buenos los parámetros obtenidos en el proceso de optimización, cuyos valores son de:

Momento de inercia de giro del vehículo respeto al eje vertical o guiñada I = 1.497 · 10 3

[kgm2 ]

Fricción viscosa de giro respecto al eje vertical o guiñada D = 5.426 · 10 2 [Nms/rad]

Caída del voltaje del motor Vdrop = 0.24 [V]

Aunque en este ensayo también se identificaba el valor de la caída de voltaje del motor, se
ha decidido mantener el dato obtenido en la identificación del avance y cabeceo.

80 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 6.4. Resultados de la identificación del ruido

6.4. Resultados de la identificación del ruido

Las desviaciones estándar resultaron:

Para el acelerómetro: 0.642.

Para el giróscopo: 0.0649.

Figura 6.9. Ensayo para la identificación del ruido en la IMU.

-1

-2
0 1000 2000 3000 4000 5000 6000 7000

Figura 6.10. Ensayo para la identificación del ruido en los encoders.

Las desviaciones estándar resultaron:

Para la velocidad de avance: 0.112.

Para la velocidad angular de giro: 0.4693.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 81

Carmen Jiménez Cortés
Capítulo 6. Resultados

6.5. Iteraciones del control del vehículo

Los primeros controles, como ya se ha expresado anteriormente, se obtuvieron mediante
tanteo y numerosos ensayos. Tras numerosas pruebas, se consiguió concluir que la ganancia
K, del control de cabeceo, era responsable de cuánto tiempo tardaba en actuar el control
desde que el vehículo salía de su posición de equilibrio, mientras que la constante de tiempo
T d, del control de cabeceo, condicionaba cómo de enérgica era esta actuación del control. La
ganania K, del control de la velocidad de avance, no parecía tener un efecto que mejorara la
estabilización y fue un parámetro que varió poco en las iteraciones.

Solo cuando se llegó a esta conclusión y se descubrió también cómo de sensibles eran los
parámetros por separado y en conjunto, fue posible llegar a equilibrar el vehículo. Los mejores
controles que se obtuvieron fueron los presentados en las seciones 6.5.1 y 6.5.2.

6.5.1. Control P-PD para el avance y cabeceo

Las ganancias del regulador que proporcionaron el mejor resultado fueron:

2 3T
10.946
6 7
Kcd = 4 4.1332 5
8.0569

Y los parámetros correspondientes al transformarlo en un P-PD en cascada:

Control PD para el cabeceo

K = 8.0569

T d = 0.513

Control P para la velocidad de avance

K = -1.3586

6.5.2. Control PI-PD para el avance y cabeceo

Partiendo de un control P estable, simplemente se añadió la acción integral de la velocidad
de avance y se modificaron ligeramente los parámetros. Resultando entonces el control integral
por realimentación del estado:

2 3T
11.625
6 4.1584 7
6 7
Kcd = 6 7
4 9.5072 5
1.6146

Y los parámetros de los PIDs en cascada:

Control PD para el cabeceo

K = 9.5072

82 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 6.6. Controles optimizados mediante simulación

T d = 0.486

Control PI para la velocidad de avance

K = -1.3586

Ti = 8

6.6. Controles optimizados mediante simulación

6.6.1. Control P-PD para los movimientos de avance y cabeceo
El control Proporcional óptimo se encuentra recogido en la Sección 6.6.2, ya que el
optimizador calculó un valor para la constante de tiempo de la acción integral tan grande, que
era equivalente a no incluir ninguna acción integral en el lazo de control de la velocidad de
avance.

6.6.2. Control PI-PD para los movimientos de avance y cabeceo

Para optimizar el control, y con él también el EKF, fue necesario realizar simulaciones de
dos casos distintos. La única diferencia entre ellos es el valor de la referencia para la velocidad
de avance. En el primer caso, Sección 6.6.2.1, esta referencia era cero, y en el segundo,
Sección 6.6.2.2, una señal cuadrada. En ambos casos la referencia para el control de guiñada
era cero.

6.6.2.1. Caso 1: Velocidad de avance nula

El proceso de optimización se realizó en tres etapas:

1. Se optimiza únicamente el control PI-PD para el avance y cabeceo.

2. Se optimizan los parámetros del EKF que intervienen en la estimación del estado para
este control, manteniendo el control óptimo obtenido en la etapa anterior.

3. Partiendo del control y parámetros del EKF óptimos, se trata de optimizar todo junto.

Durante estas tres etapas, el control de guiñada que se empleó fue un control PI calculado
previamente y que se había observado que era estable en simulación. Este control era el
siguiente:

K = 0.58504

T i = 0.050853

b = 2.5926

Cada simulación duraba 15 segundos.

Una vez completado el proceso de optimización, los resultados obtenidos fueron:

Control PD para el cabeceo

K = 16.934

T d = 0.7583

Control PI para la velocidad de avance

Control de un Vehículo Equilibrista mediante una Raspberry Pi 83

Carmen Jiménez Cortés
Capítulo 6. Resultados

K = -0.89621

T i = 82.946

b=0

Se puede observar como la constante de tiempo de la acción integral, T i, es muy grande. Por
lo que realmente el optimizador ha ajustado un control P, no un PI, como se ha expresado en
Sección 6.6.1. Esto se debe a que al no existir una variación en el valor inicial de la referencia,
no es necesario un control integral para garantizar error de seguimiento nulo en régimen
permanente.

Parámetros del EKF

Frecuencia de corte del filtro paso bajo del encoder = 8.1170 [Hz]

Frecuencia de corte del filtro paso bajo de la IMU = 16.209 [Hz]

Ganancia adaptativa del acelerómetro = 1190.4

Desviación estándar del acelerómetro = 0.998

Desviación estándar de la velocidad de avance = 0.11787

Desviación estándar del giróscopo = 0.17956

Desviación estándar del valor medio del giróscopo = 9.5772·10 5

Desviación estándar del valor medio del acelerómetro = 2.0399·10 5

La gráfica final tras la optimización se puede observar en la Figura 6.11.

El eje de tiempos (eje X) llega hasta 45 segundos, pese a que realmente se simularon
únicamente 15. Esto se debe a que en la Figura 6.11 se muestran, una a continuación de otra,
las tres variables de estado en el orden: Ángulo de cabeceo (segundos del 0 al 15), velocidad
angular de cabeceo (segundos del 15 al 30) y velocidad lineal de avance (segundos del 30 al
45).

60
Real value
Simulated value
Error

40
Simulated and real variables

20

-20

-40

-60
0 5 10 15 20 25 30 35 40 45 50
Time (s)

Figura 6.11. Optimización PI-PD sin velocidad de avance.

84 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 6.6. Controles optimizados mediante simulación

En color rojo aparece el valor real de cada variable de estado, en azul su valor simulado y en
verde el error existente entre las dos señales anteriores.

Las señales simuladas resultan tan ruidosas porque se han incluido en el modelo los ruidos
de los sensores calculados en la Sección 6.4.

6.6.2.2. Caso 2: Escalones en la referencia de velocidad de avance

En esta ocasión, la referencia empleada para la velocidad de avance fue una señal cuadrada
de periodo 20 segundos, valor medio 10 cm/s y amplitud 10 cm/s. Esta señal entraba pasados
los primeros 10 segundos de simulación, tratando de simular esos primeros segundos que
requiere el vehículo para equilibrarse tras el encendido.

La simulación tuvo una duración total de 80 segundos, y las etapas que se llevaron a cabo
fueron las mismas que las descritas anteriormente, en la Sección 6.6.2.1.

Una vez concluido el proceso de optimización, el único parámetro que varió fue la constante
de tiempo de la acción integral para la velocidad de avance:

Ti = 8

La gráfica final tras la optimización se puede observar en la Figura 6.12.

60
Real value
Simulated value
Error

40
Simulated and real variables

20

-20

-40

-60
0 50 100 150 200 250
Time (s)

Figura 6.12. Optimización PI-PD con escalones en la referencia de la velocidad de avance.

De nuevo, en la Figura 6.12, se muestran las tres variables de estado en el orden: Ángulo
de cabeceo (segundos del 0 al 80), velocidad angular de cabeceo (segundos del 80 al 160) y
velocidad lineal de avance (segundos del 160 al 240).

Puede parecer que en la Figura 6.12, el control no sigue adecuadamente la referencia.

El problema es realmente que los escalones están muy juntos y el control no tiene tiempo
suficiente para alcanzar el valor final. A continuación, en la Figura 6.13, se muestra el resultado

Control de un Vehículo Equilibrista mediante una Raspberry Pi 85

Carmen Jiménez Cortés
Capítulo 6. Resultados

de una simulación para una señal cuadrada de igual amplitud y valor medio, pero periodo 40
segundos:

Figura 6.13. Vector de estado en simulación.

La primera gráfica se corresponde con la velocidad lineal de avance en cm/s, la segunda con
la velocidad angular de cabeceo en º/s y la tercera con el ángulo de cabeceo en º.

Las gráficas se pueden ver más en detalle a las siguientes imágenes:

En la Figura 6.14 se ha representado la velocidad lineal de avance, y en la Figura 6.15 el

ángulo de cabeceo.

86 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 6.6. Controles optimizados mediante simulación

Simulación: Señal cuadrada en la referencia de la velocidad de avance

30

20

10
Velocidad de avance (cm/s)

-10

-20

-30

-40

-50

-60
0 20 40 60 80 100 120 140 160
Tiempo (s)

Figura 6.14. Velocidad de avance en simulación.

Simulación: Señal cuadrada en la referencia de la velocidad de avance

12

10

8
Ángulo de cabeceo (º)

-2

-4
0 20 40 60 80 100 120 140 160
Tiempo (s)

Figura 6.15. Ángulo de cabeceo en simulación.

De nuevo, las simulaciones resultan ruidosas porque se han incluido en el modelo los ruidos
identificados en la Sección 6.4.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 87

Carmen Jiménez Cortés
Capítulo 6. Resultados

6.6.3. Control de guiñada para “Navegación Libre”

Una vez se obtuvo un control óptimo para el cabeceo y la velocidad de avance, se optimizó
el control integral de guiñada.

Para llevar a cabo esta optimización, se utilizó un único caso, que consistía en una simulación
de 60 segundos en la cual:

En el segundo 10 se produce un escalón en la referencia de la velocidad lineal de avance

de 30 cm/s.

A partir del segundo 30 y hasta el final de la simulación, se aplica una referencia para la
velocidad de guiñada cuadrada, de periodo 2 segundos, valor medio nulo y amplitud 30º.

El control de avance y cabeceo empleado fue el óptimo calculado en la Sección 6.6.2.2.

Las etapas llevadas a cabo en el proceso de optimización fueron las mismas que se han
descrito en la Sección 6.6.2.1, y los resultados obtenidos:

Control PI para la velocidad de guiñada

K = 0.73262

T i = 0.060158

b = 2.2727

Y la gráfica final tras la optimización se muestra en la Figura 6.16.

80
Real value
Simulated value
60 Error

40
Simulated and real variables

20

-20

-40

-60

-80
0 50 100 150 200 250
Time (s)

Figura 6.16. Optimización PI con escalones en la referencia de la velocidad de guiñada.

En la Figura 6.16 se muestran en orden las cuatro variables de estado: Ángulo de cabeceo
(segundos del 0 al 60), velocidad angular de cabeceo (segundos del 60 al 120), velocidad

88 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 6.6. Controles optimizados mediante simulación

lineal de avance (segundos del 120 al 180) y velocidad angular de guiñada (segundos del 10 al
240).

Ampliando la velocidad angular de guiñada, Figura 6.17, se puede observar como sigue
mejor los escalones de referencia que cómo lo hacía el control integral de la velocidad lineal de
avance. Esto se debe a que el giróscopo ayuda a corregir la medida del encoder, mientras que en
el control de la velocidad de avance, el acelerómetro perjudicaba a la medida del encoder.

30 Real value
Simulated value
Error

20
Simulated and real variables

10

-10

-20

-30

220 221 222 223 224 225 226 227

Time (s)

Figura 6.17. Optimización PI con escalones en la referencia de la velocidad de guiñada.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 89

Carmen Jiménez Cortés
 7
Conclusiones, aportaciones y
futuros desarrollos

En este último capítulo se expresan cuáles han sido las conclusiones finales del proyecto,
Sección 7.1, las mejores aportaciones que se han realizado, Sección 7.2 y los futuros
desarrollos que podrían realizarse en próximos trabajos de fin de grado y máster,
Sección 7.3.

7.1. Conclusiones
Por todo lo anteriormente descrito en este documento, las principales conclusiones de este
proyecto son las siguientes:

El elemento hardware más problemático ha sido el encoder magnético

Como se ha explicado en la Sección 3.7.3, la medida del encoder resultaba tan ruidosa, que
no era suficiente con un filtro paso bajo habitual para poder trabajar con ella. Fue necesario
invertir varios días en exclusiva a conseguir corregirla, lo que ralentizó el avance del resto
del proyecto. Se han diseñado y programado herramientas de filtrado más sofisticadas que
pueden resultar de gran utilidad en un futuro, cuando se vuelva a trabajar con este tipo de
sensores.

Diseñar controles para plantas inestables sin un modelo no es trivial

Como el vehículo equilibrista es una planta inestable, era indispensable conseguir un primer
control que lo mantuviera en equilibrio para poder realizar ensayos más complejos. Conseguir
entender las dinámicas del vehículo por mera observación y ensayos hasta poder inferir cuál
era el camino para modificar el control, no ha sido una tarea sencilla ni liviana.

Finalmente se consiguió un punto de partida, y como se puede ver comparando las secciones
6.5 y 6.6, este no era muy distinto del control óptimo.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 91

Carmen Jiménez Cortés
Capítulo 7. Conclusiones, aportaciones y futuros desarrollos

7.2. Aportaciones y objetivos alcanzados

Las principales contribuciones que aporta este proyecto son:

Obtención e identificación de un nuevo vehículo equilibrista

Se han realizado todos los ensayos necesarios para poder caracterizar todos los elementos
hardware que forman el vehículo. Esta información es útil tanto para este proyecto como para
cualquier otro que emplee los mismos elementos hardware.

Además, este nuevo vehículo se encuentra controlado por una Raspberry Pi 3B+, y Matlab
en cada actualización amplia el soporte para este microcontrolador, por lo que cabe esperar que
pueda seguirse utilizando en los próximos años.

Programación de la Raspberry Pi 3B+ para que sea compatible con el resto de elementos hardware

Como se ha expresado en la Sección 3.2, la Raspberry Pi 3B+ es un ordenador en sí mismo,

lo que le dota de una gran capacidad computacional y versatilidad, pero también añade la
dificultad adicional de que hay que asegurarse de configurarla adecuadamente si se desea que
interactúe con elementos hardware adicionales. La configuración necesaria para poder utilizarla
en este proyecto se ha includo en el Apéndice B.

Desarrollo de herramientas de estrategias de identificación y optimización complejas

Dadas las numerosas aproximaciones que se tuvieron que realizar hasta conseguir obtener
todos los valores de los parámetros físicos del vehículo, se han terminado diseñando simuladores
y optimizadores más complejos y precisos, que a partir de ensayos son capaces de inferir todos
los parámetros que intervienen en las ecuaciones dinámicas de los mismos.

Esto proporciona una nueva herramienta para todos los futuros trabajos de fin de grado y
máster que necesiten identificar plantas complejas.

Programación en Matlab-Simulink de todos los archivos de configuración del hardware

Para poder utilizar los elementos hardware que componen el sistema ha sido necesario
programar en scripts de Matlab todos los mapas de registros y configuraciones necesarios para
su uso en el proyecto. De nuevo, estos ficheros de configuración son de gran utilidad para
futuros proyectos que empleen alguno de estos elementos.

Programación en Matlab-Simulink del sistema de control

Finalmente, la mayor aportación de este trabajo de fin de grado es haber conseguido

programar en Matlab-Simulink todos los scripts y diagramas necesarios para poder controlar el
vehículo, monitorizarlo en tiempo real, simularlo e incluso optimizar tanto el control como el
estimador de estado.

Todo el sistema de control se encuentra recogido en una interfaz intuitiva, que mediante
buses permite trabajar con toda la información necesaria de forma sencilla y ordenada. Por lo
que es adecuada para ser incorporada a proyectos de docencia.

92 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 7.3. Futuros desarrollos

7.3. Futuros desarrollos

7.3.1. Optimización del sistema de control
El principal futuro desarrollo que se espera para este proyecto es la optimización y depuración
de los controles que actualmente se han implementado. Dado que ha sido el primer año que
se trabajaba con la planta del vehículo Balboa, de la empresa americana Pololu, gran parte
del proyecto se ha invertido conseguir caracterizarla, y una vez se consiguió un modelo lo
suficientemente preciso, que permitiera optimizar el control y contrastarlo con ensayos en el
laboratorio, eran las últimas semanas del proyecto. Con la palabra optimizar, se quiere expresar
que es necesario mejorar la robustez del control, para que el vehículo sea capaz de estabilizarse
incluso en condiciones de funcionamiento más críticas.

Gracias al simulador y al optimizador que se han programado, el camino para llegar a

alcanzar este objetivo está bastante allanado.

También, con un control más óptimo se podrían ajustar de forma más precisa las covarianzas
de la matriz de Kalman, en el estimador de estado, para así obtener una mejor estimación de
las variables de estado, que pudieran contribuir en la mejora de la actuación del control.

Una vez que se lleve a cabo este objetivo, el vehículo estaría listo para incluirse en los
laboratorios de Control Digital y de Control Avanzado de la universidad.

7.3.2. Control para seguimiento de linea

De igual forma que se ha conseguido que el vehículo sea capaz de seguir una referencia de
distancia a la pared, gracias a los sensores laterales VL6180X, Sección 3.6, y mediante un control
integral por realimentación del estado, podría implementarse una cámara y un nuevo control,
que permitieran seguir una referencia de distancia a una línea pintada en el suelo.

Este desarrollo plantea un nuevo problema de control de gran interés para la docencia:
conseguir mantener en equilibrio el vehículo mientras recorre un circuito completamente
customizable.

7.3.3. Navegación autónoma mediante cámaras

Gracias a que el laboratorio dispone del sistema de cámaras de captura de movimiento
(Motion Capture System, MCS), Figura 7.1 y el software Motive instalados por Javier García
Aguilar el año pasado, para su trabajo de fin de máster, podrían emplearse para estimar tanto los
ángulos de cabeceo y guiñada como sus respectivas velocidades, la velocidad lineal de avance y
la ubicación del vehículo dentro de la sala, para que así pudiera navegar autónomamente.

Para con lograr este objetivo, bastaría con colocar varios receptores de las cámaras sobre el
chasis del vehículo y modificar el código en simulink que actualmente se utiliza para monitorizar
el vuelo de los drones.

7.3.4. Control para elevar al vehículo desde posición horizontal

hasta la de equilibrio
Este futuro desarrollo es quizás el más ambicioso de todos los planteados, pero dada la
versatilidad de la planta es perfectamente alcanzable. Consistiría en diseñar un control lo

Control de un Vehículo Equilibrista mediante una Raspberry Pi 93

Carmen Jiménez Cortés
Capítulo 7. Conclusiones, aportaciones y futuros desarrollos

Figura 7.1. Sistema de cámaras MCS empleado en drones.

suficientemente rápido y eficaz como para, partiendo con el vehículo en posición horizontal,
acelerar enérgicamente para llevarlo hasta su verticalidad, y entonces que comenzaran a actuar
todos los controles mencionados en este documento, Capítulo 5.

Este objetivo tiene sentido porque en paralelo a este proyecto, se han estado desarrollando los
controles que permitirían controlar al vehículo en horizontal sobre su tercer apoyo y funcionar
así como un coche. Concretamente, se pretende sustituir el actual coche empleado en los
laboratorios de Regulación Automática de la universidad, ya que este también utiliza el kit de
Lego Mindstorm, y como se indicó en la Sección 1.1, no existe soporte de Matlab-Simulink para
el mismo desde 2015.

7.3.5. Diseño 3D de un nuevo soporte para los sensores

Finalmente, sería interesante diseñar un nuevo soporte para los sensores laterales, que
además de mantener a los mismos en su posición, permitiera proteger al vehículo frente
a impactos o situar nuevos sensores, como la cámara mencionada anteriormente en la
Sección 7.3.2.

94 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 A
Filtro de Kalman

Este anexo se ha decidido incluir como complemento a la teoría del Filtro Extendido de
Kalman presentada en la Sección 5.4.3, ya que los conceptos teóricos y matemáticos que
lo fundamentan resultan mucho más sencillos de comprender para sistemas lineales. En
la Sección A.1 se introduce cómo es la representación de estado de un sistema lineal,
para a continuación, en la Sección A.2 explicar qué es la matriz de ganancias de Kalman
y cómo conseguir su valor óptimo. Finalmente se describen las ecuaciones que componen
el proceso de cálculo del filtro, en la Sección A.3.

A.1. Modelado en Espacio Estado Lineal

En la Sección 5.4.3 se expresó que la representación de estado de todo sistema queda
recogida según las ecuaciones:

xk+1 = F (xk , uk , wk ) (A.1)

zk = G(xk , uk , vk ) (A.2)

Si además se trata de un sistema lineal, entonces las equaciones resultan en las

siguientes:

La Ecuación A.1, para el vector de estado:

xk+1 = A · xk + B · uk + wk (A.3)

Y la Ecuación A.2, para las salidas:

zk = C · x k + D · u k + v k (A.4)

Control de un Vehículo Equilibrista mediante una Raspberry Pi 95

Carmen Jiménez Cortés
Apéndice A. Filtro de Kalman

Donde A, B, C y D son matrices de coeficientes que recogen la relacion entre las variables de
estado en la muestra actual y las variables de estado en la muestra anterior, matriz A, entre las
variables de estado en la muestra actual y el vector de entradas, matriz B, entre las salidas y las
variables de estado actuales, matriz C, y entre las salidas y el vector de entradas en la muestra
actual, matriz D.

Siendo:

xk : El vector de variables de estado en la muestra k

uk : El vector de entradas en la muestra k

wk : El vector de ruido en el proceso en la muestra k

zk : El vector de salidas obtenidas a partir de las medidas en la muestra k

vk : El vector de ruido en las salidas en la muestra k

Y todo estimador se construye como:

x̂k+1 = F (x̂k , uk ) + K · (zk yk ) (A.5)

x̂k+1 : El vector de variables de estado estimadas en la muestra k+1

x̂k : El vector de variables de estado estimadas en la muestra k

uk : El vector de entradas en la muestra k

zk : El vector de salidas medidas en la muestra k

yk : El vector de salidas estimadas en la muestra k

K: La matriz de corrección

F (x̂k , uk ) en su conjunto se denomina predicción del modelo

K ⇤ (zk yk ) en su conjunto se denomina corrección de la predicción

Ambos ruidos, wk y vk son considerados ruidos blancos, independientes y con una

distribución normal de probabilidad, es decir, son ruidos gaussianos.

A.2. Filtro de Kalman

Lo que diferencia la teoría de Kalman de la construcción de otros estimadores de estado
empleados en teoría de control, es el procedimiento a seguir para obtener la matriz de corrección
K, [21].

Si en la ecuación Ecuación A.5 se sustitye el valor de yk , resulta:

x̂k+1 = x̂k + K · (zk C · x̂k ) (A.6)

Al término (zk C · x̂k 1 ) se le denomina residuo. Estos residuos representan la diferencia

existente entre la salida estimada con el modelo yk = C · x̂k 1 y la salida obtenida a partir de
las medidas zk . Si el residuo es igual a cero, quiere decir que ambos valores coinciden.

Se definen también los siguientes dos errores y sus respectivas covarianzas:

96 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 A.2. Filtro de Kalman

Error en la estimación a priori:

êk ⌘ xk x̂k (A.7)

Error en la estimación a posteriori:

ek = xk x̂k (A.8)

Siendo entonces la covarianza del error estimado a priori:

Pk = E[êk êk T ] (A.9)

Y la covarianza del error estimado a posteriori:

Pk = E[ek eTk ] (A.10)

La matriz K de la Ecuación A.5 se elige de tal forma que minimize la covarianza del
error a posteriori, Ecuación A.10, es decir, aquella que proporcione la estimación de máxima
verosimilitud. Para ello se sustituye la Ecuación A.9 en la Ecuación A.5 y tras manipular la
ecuación, resulta la siguiente expresión para K:

Pk · CkT
Kk = (A.11)
Ck · Pk · CkT + Rk

Donde Rk es la covarianza del error en la medida.

De la Ecuación A.11 se pueden extraer las siguientes conclusiones:

Cuando la covarianza del error en la medida, Rk , tiende a cero, K hace que el residuo tenga
mayor importancia.
lı́m Kk = Ck 1 (A.12)
Rk !0

Por otro lado, cuando la covarianza del error estimado a priori, Pk 1 tiende a 0, el residuo
interviene en menor medida en la Ecuación A.5.

lı́m Kk = 0 (A.13)
Pk !0

Estos resultados permiten identificar la matriz K como un conjunto de pesos o ponderaciones,

que a medida que la covarianza del error, Rk , tiende a cero, hace que la salida calculada a partir
de las medidas zk sea más fiable y la salida estimada a partir del modelo yk sea menos fiable.
De igual forma, si la covarianza del error estimado a prior, Pk 1 , se aproxima a cero, la salida
calculada a partir de las medidas zk se considera menos fiable y K da una mayor importancia a
la salida estimada a partir del modelo yk .

En una primera iteración, los pesos de la matriz K se estiman por observación de la precisión
de los sensores y la exactitud del modelo matemático disponible. Posteriormente se realizan
ensayos donde se comparan ambas salidas yk y zk y se reajustan los pesos hasta que son

Control de un Vehículo Equilibrista mediante una Raspberry Pi 97

Carmen Jiménez Cortés
Apéndice A. Filtro de Kalman

similares. La salida estimada, yk , al ser digital estará libre de ruidos analógicos, a diferencia de
zk .

Se puede observar la gran potencia y flexibilidad que ofrece un filtro de Kalman ya

que, dentro de un mismo sistema, permite combinar los resultados estimados a partir del
modelo matemático y los obtenidos a partir de las medidas reales, para así obtener una mejor
solución.

A.3. Proceso de cálculo del Filtro de Kalman

Actualización de la medida o Corrección

1. Para los valores iniciales de x̂k y P̃k se calcula la matriz de ganancias de Kalman en la
muestra k:
Kk = Pk · HkT · (Hk · Pk · HkT + Rk ) 1 (A.14)

2. Se actualiza el estado con la medida zk

x̂k = x̂k + K · (zk Hk · x̂k ) (A.15)

3. Se actualiza la covarianza del error

Pk = (I Kk · Ck ) · P̃k (A.16)

Actualización del tiempo o Predicción

1. Se calcula el estado de la siguiente muestra

x̂k+1 = Ak · x̂k + B · uk (A.17)

2. Se calcula la covarianza del error en la siguiente muestra

Pk+1 = Ak · Pk · ATk + Qk (A.18)

3. Con estas nuevas estimaciones de x̃k+1 y P̃k+1 se repiten los procesos de Corrección y
Predicción.

Bajo las hipótesis de que Qk y Rk son constantes, tanto matriz de covarianzas del error en la
estimación Pk y la ganancia de Kalman Kk , convergerán pronto y permanecerán constantes. El
ruido del proceso, Qk , en cambio, no suele permanecer constante.

98 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 B
Configuración de la Raspberry
Pi 3B+

En este anexo se ha incluido una breve explicación de cómo debe programarse la

Raspberry Pi 3B+ antes de intentar utilizarla con el vehículo equilibrista. La gran
mayoría de acciones aquí recogidas son extensibles a cualquier proyecto que trabaje con
Raspberry Pi

B.1. Raspi-Config para habilitar las comunicaciones

En primer lugar, es altamente recomendable habilitar todas las comunicaciones disponibles
en la Raspberry Pi, ya que no suponen ningún inconveniente y de no realizarse al principio del
proyecto es altamente probable que en el momento de que sean necesarias, no se recuerde que
hay que habilitarlas.

Con la interfaz de ventanas

Hacer click en el icono de Raspberry y en el menú desplegable acceder a: Preferences -

Raspberry Pi Configuration.

Mediante la ventana de comandos

Escribir el comando linux sudo raspi-config

Una vez dentro de la ventana de configuración, seleccionar la opción número 5. Interfacing

Options y realizar las siguientes acciones:

1. Habilitar el acceso remoto SSH a la Raspberry Pi, de lo contrario será imposible controlarla
desde Matlab-Simulink.

2. Habilitar las comunicaciones SPI, si se emplea algún elemento hardware que las utilice.
En este proyecto no sería necesario.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 99

Carmen Jiménez Cortés
Apéndice B. Configuración de la Raspberry Pi 3B+

3. Habilitar las comunicaciones I2C, si se emplea algún elemento hardware que las
utilice. En este proyecto es indispensable para permitir la comunicación entre los dos
microcontroladores.

4. Deshabilitar la consola de Linux sobre el puerto serie y habilitar el puerto serie. Para ello,
a la pregunta ’Would you like a login shell to be accessible over serial?’ responder No.

Una vez hecho esto, reiniciar la Raspberry Pi para guardar todos los cambios.

B.1.1. Comunicación I2C a 400 kHz

Para comunicarse con el ATMega32U4 se emplea el protocolo de comunicación I2C, pero es
necesario que este sea a 400 kHz. Debido a un error interno de Matlab no es posible modificar
el valor por defecto de la frecuencia de la comunicación de la Raspberry, 100kHz, por lo que es
necesario modificar el fichero de configuración. El proceso para realizarlo es el siguiente:

1. Abrir el terminal de la Raspberry.

2. Escribir el comando: sudo nano /boot/config.txt, respetando todos los espacios y

minúsculas, de lo contrario generará un nuevo .txt en blanco en otra ubicación.

3. Bajar en el documento hasta encontrar la línea en la que aparece escrito: i2c_arm = on y

a continuación añadir, sin espacios y separado por una , lo siguiente: i2c_arm_baudrate
= 400000. Debería quedar la línea como: i2c_arm = on,i2c_arm_baudrate = 400000.

4. Salir del fichero pulsando Ctrl + X, guardar los cambios en el propio fichero y reiniciar la
Raspberry Pi.

Por algún motivo, desconocido actualmente, puede llegar a ocurrir que por inactividad
o por algún uso indebido de la Raspberry Pi es posible que desaparezca de la línea el
i2c_arm_baudrate = 400000, por lo que si se detecta algún fallo en la comunicación
I2C, es altamente recomendable revisar que no haya desaparecido la misma del archivo de
configuración.

100 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 C
Librerías de Arduino

En este anexo se recogen las librerías que han sido empleadas para escribir la estructura
que intercambian entre sí los dos microcontroladores: La Raspberry Pi 3B+, máster en la
comunicación, y el ATMega32U4, esclavo en el bus I2C.

C.1. Librerías a instalar

Las librerías que se han empleado en la definición del programa que se ejecuta en el
ATMega32U4 han sido las siguientes:

Servo.h

PololuRPiSlave.h

Balboa32U4.h

Destacar que debido a incompatibilidades de funciones con las librerías anteriores, no se

deben instalar las siguientes librerías pese a que las recomienden en la web de Pololu:

AStar32U4.h

LSM6.h

LIS3MDL.h

wire.h

Las funciones de I2C se gestionan directamente a través de la librería PololuRPiSlave.h, por

eso no se puede utilizar la librería habitual de Arduino wire.h.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 101

Carmen Jiménez Cortés
 Referencias
[1] S. Renou and L. Saydy, “Real time control of an inverted pendulum based on approximate
linearization,” vol. 597, no. March 1997, pp. 502–504, 2002.

[2] H. M. Z. U. Farwlg and H. Unbehauen, “Triple-Inverted Pendulum,” vol. 1, no. December

1988, pp. 157–171, 1990.

[3] Z. Li, C. Yang, and L. Fan, Advanced control of wheeled inverted pendulum systems, 2013,
vol. 9781447129.

[4] I. E. T. Control, The Inverted Pendulum in Control Theory and Robotics.

[5] F. Grasser, A. D’Arrigo, S. Colombi, and A. C. Rufer, “JOE: A mobile, inverted pendulum,”
IEEE Transactions on Industrial Electronics, vol. 49, no. 1, pp. 107–114, 2002.

[6] S. Jeong and T. Takahashi, “Wheeled inverted pendulum type assistant robot: Design
concept and mobile control,” Intelligent Service Robotics, vol. 1, no. 4, pp. 313–320, 2008.

[7] S. Kajita, F. Kanehiro, K. Kaneko, K. Yokoi, and H. Hirukawa, “The 3D linear inverted
pendulum mode: a simple modeling for a biped walking pattern generation,” pp. 239–246,
2002.

[8] K. Kaneko, F. Kanehiro, M. Morisawa, K. Miura, S. Nakaoka, and S. Kajita,

“Cybernetic human HRP-4C,” 9th IEEE-RAS International Conference on Humanoid Robots,
HUMANOIDS09, pp. 7–14, 2009.

[9] Y. Ogura, K. Shimomura, H. Kondo, A. Morishima, T. Okubo, S. Momoki, H. O. Lim, and

A. Takanishi, “Human-like walking with knee stretched, heel-contact and toe-off motion
by a humanoid robot,” IEEE International Conference on Intelligent Robots and Systems, pp.
3976–3981, 2006.

[10] C. C. Lee, “Fuzzy logic in control systems: fuzzy logic controller, Part II,” IEEE Transactions
on Systems, Man, and Cybernetics, vol. 20, no. 2, pp. 419–435, 1990.

[11] C. H. Huang, W. J. Wang, and C. H. Chiu, “Design and implementation of fuzzy control
on a two-wheel inverted pendulum,” IEEE Transactions on Industrial Electronics, vol. 58,
no. 7, pp. 2988–3001, 2011.

[12] M. I. El-Hawwary, A. L. Elshafei, H. M. Emara, and H. A. Fattah, “Adaptive fuzzy control of

the inverted pendulum problem,” IEEE Transactions on Control Systems Technology, vol. 14,
no. 6, pp. 1135–1144, 2006.

[13] S. Omatu, T. Fujinaka, and M. Yoshioka, “Neuro-PID control for inverted single and double
pendulums,” pp. 2685–2690, 2002.

Control de un Vehículo Equilibrista mediante una Raspberry Pi 103

Carmen Jiménez Cortés
Referencias

[14] Seul Jung and Sung Su Kim, “Control Experiment of a Wheel-Driven Mobile Inverted
Pendulum Using Neural Network,” IEEE Transactions on Control Systems Technology, vol. 16,
no. 2, pp. 297–303, 2008.

[15] Shiuh-Jer Huang and Chien-Lo Huang, “Control of an inverted pendulum using grey
prediction model,” IEEE Transactions on Industry Applications, vol. 36, no. 2, pp. 452–458,
2002.

[16] H. Kajiwara, P. Apkarian, and P. Gahinet, “LPV Techniques for Control of an Inverted
Pendulum,” IEEE Control Systems, pp. 44–54.

[17] J. Z. Sasiadek, A. Monjazeb, and D. Necsulescu, “Navigation of an autonomous mobile

robot using EKF-SLAM and FastSLAM,” 2008 Mediterranean Conference on Control and
Automation - Conference Proceedings, MED’08, no. 1, pp. 517–522, 2008.

[18] M. D. R. Madhavan and H. Durrant-Whyte, “Using a Combined ICP-EKF Approach,” no.

May 1998, pp. 3703–3708, 2006.

[19] F. D. Foresee and M. T. Hagan, “Gauss-Newton approximation to Bayesian regularization,”

Proceedings of the 1997 International Joint Conference on Neural Networks, vol. 3, pp.
1930–1935, 1997.

[20] G. B. Greg Welch, “An Introduction to the Kalman Filter,” Department of Computer Science,
University of North Carolina at Chapel Hill, 2018.

[21] K. Reif, S. Günther, E. Yaz, and R. Unbehauen, “Stochastic stability of the discrete-time
extended Kalman filter,” IEEE Transactions on Automatic Control, vol. 44, no. 4, pp. 714–
728, 1999.

104 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
 PARTE II
ML
PRESUPUESTO
Elementos y precios unitarios
Elemento Coste unitario [e]
Vehículo
Raspberri Pi 3B+ 35
Robot Balboa 62.05
Motores 75:1 HPCB 6V 16.81
Kit de estabilidad 7.05
Par de ruedas 90x10 mm 5.28
Sensores VL6180X 14.8
Multiplexador I2C TCA9548A 9.83
Batería recargable 2800 mAh 3.5
Software - Licencias
Matlab / Simulink 800
Horas de trabajo
Programación 18
Ensayos para la identificación 18
Pruebas de hardware e integración 15
Identificación de parámetros con simulación 15
Pruebas de comunicaciones 15
Ensayos en el laboratorio 15
Optimización del EKF y del control 15
Diseño de piezas 3D 10

Tabla C.1. Elementos empleados y precios unitarios

Control de un Vehículo Equilibrista mediante una Raspberry Pi 109

Carmen Jiménez Cortés
Cantidad de cada elemento
Elemento unidades
Vehículo
Raspberri Pi 3B+ 1
Robot Balboa 1
Motores 75:1 HPCB 6V 2
Kit de estabilidad 1
Par de ruedas 90x10 mm 1
Sensores VL6180X 2
Multiplexador I2C TCA9548A 1
Batería recargable 2800 mAh 6
Software - Licencias
Matlab / Simulink 1
Horas de trabajo
Programación 300
Ensayos para la identificación 300
Pruebas de hardware e integración 50
Identificación de parámetros con simulación 25
Pruebas de comunicaciones 25
Ensayos en el laboratorio 200
Optimización del EKF y del control 50
Diseño de piezas 3D 15

Tabla C.2. Cantidad de elementos empleados

110 Control de un Vehículo Equilibrista mediante una Raspberry Pi

Carmen Jiménez Cortés
Presupuesto final
Elemento Coste total [e]
Vehículo
Raspberri Pi 3B+ 35
Robot Balboa 62.05
Motores 75:1 HPCB 6V 33.62
Kit de estabilidad 7.05
Par de ruedas 90x10 mm 5.28
Sensores VL6180X 29.6
Multiplexador I2C TCA9548A 9.83
Batería recargable 2800 mAh 21
Software - Licencias
Matlab / Simulink 800
Horas de trabajo
Programación 5400
Ensayos para la identificación 5400
Pruebas de hardware e integración 750
Identificación de parámetros con simulación 375
Pruebas de comunicaciones 375
Ensayos en el laboratorio 3000
Optimización del EKF y del control 750
Diseño de piezas 3D 150
Presupuesto total 17 203.43e

Tabla C.3. Elementos empleados y precios unitarios

Control de un Vehículo Equilibrista mediante una Raspberry Pi 111

Carmen Jiménez Cortés