estudio, simulaciÓn e implementaciÓn del control de …
TRANSCRIPT
Autor: Antonio Rodríguez Rodríguez
Tutor: Guillermo Heredia Benot
2016
UNIVERSIDAD DE SEVILLA ESCUELA TÉCNICA SUPERIOR DE INGENIERÍA
Ingeniería Industrial
Departamento de Ingeniería de Sistemas y Automática
ESTUDIO, SIMULACIÓN E IMPLEMENTACIÓN DEL
CONTROL DE UN VEHÍCULO AUTOMATIZADO DE DOS
RUEDAS
2
1 Introducción .................................................................................................................. 5
1.1 Descripción del problema ....................................................................................... 5
1.2 Objetivos................................................................................................................ 6
1.3 Metodología ........................................................................................................... 7
2 Estado del arte .............................................................................................................. 9
2.1 Control de una bicicleta ........................................................................................ 12
2.2 Control en Espacio de Estados ............................................................................. 12
2.2.1 Regulador óptimo cuadrático .................................................................... 15
3 Vehículo automatizado de dos ruedas del proyecto...................................................... 17
4 Caracterización del vehículo automatizado de dos ruedas ............................................ 19
4.1 Modelo matemático .............................................................................................. 19
4.1.1 Modelado del vehículo automatizado de dos ruedas .................................. 19
4.1.2 Caracterización de los ángulos de inclinación y giro ................................. 26
4.1.3 Modelos matemáticos de los motores de tracción y dirección .................... 28
4.2 Parámetros del sistema ......................................................................................... 35
4.3 Estabilidad del vehículo automatizado de dos ruedas ............................................ 46
4.4 Simulación del modelo ......................................................................................... 48
5 Diseño del controlador ................................................................................................ 51
5.1 Diseño y simulación del control mediante el regulador cuadrático óptimo ............ 53
6 Implementación del control en el vehículo automatizado ............................................. 59
6.1 Instalación del módulo de bluetooth HC-05 .......................................................... 59
6.2 Configuración de la unidad de medición inercial Arduimu v2 .............................. 61
6.3 Implementación del control del vehículo automatizado ........................................ 65
6.3.1 Desarrollo de una aplicación android ........................................................ 65
6.3.2 Implementación del programa arduino ...................................................... 70
6.3.3 Implementación del control manual .......................................................... 73
6.3.4 Implementación del control automático ..................................................... 77
7 Conclusiones y trabajos futuros ................................................................................... 79
3
8 Referencias bibliográficas ........................................................................................... 80
ANEXO .......................................................................................................................... 81
Experimentos .................................................................................................................. 81
Archivos Matlab .............................................................................................................. 88
Archivos Arduino ............................................................................................................ 97
Datasheet ....................................................................................................................... 111
4
Agradecimientos
A mi tutor del proyecto Guillermo Heredia Benot por su guía, apoyo, paciencia y por compartir
sus conocimientos para la realización de este proyecto.
A mis padres y pareja por estar siempre apoyándome y creer en mí.
5
1 Introducción
Los vehículos de dos ruedas, las bicicletas, son unos de los vehículos más estudiados a lo largo
de los años. Este proyecto forma parte de uno más amplio. La primera de éste consiste en el
diseño y construcción del hardware del vehículo de dos ruedas. Esta primera parte ha sido
realizada por mi compañero Honorio Romero Rodríguez y se titula “Diseño y Construcción del
Vehículo Autónomo de Dos Ruedas” [1]. La segunda parte de este proyecto, en la que se centra
este proyecto, es la simulación y control de dicho vehículo de dos ruedas. Es decir, este proyecto
que se presenta a continuación se centra en el ámbito de la robótica, la automática y el control,
seleccionando un sistema físico muy presente en la vida cotidiana como es una bicicleta, la cual,
se trata de estudiar, simular y controlar.
El proyecto propone una forma de control distinta a la que se suele utilizar para controlar un
vehículo de esta clase. Así, se utilizará un control en espacio de estados y se verá si es una
estrategia de control factible para este tipo de vehículos.
1.1 Descripción del problema
Nuestro vehículo automatizado de dos ruedas, es una bicicleta con todo el hardware necesario. El
problema de control de una bicicleta es, como ya se ha comentado, uno de los más estudiados, ya
que es básicamente un problema de péndulo invertido aunque un poco más complejo.
Como se sabe, el problema del péndulo invertido consiste en un carro sobre el cual se monta un
péndulo que puede girar libremente. Dicho carro deberá moverse para compensar el
desplazamiento del péndulo y mantenerlo en equilibrio.
Figura 1. Esquema del péndulo invertido.
6
El problema de la bicicleta consiste también en mantener un péndulo vertical pero el péndulo
puede girar libre y perpendicularmente a la dirección del desplazamiento y no paralelo a la
dirección del desplazamiento como ocurre en el péndulo invertido.
Figura 2. Esquema bicicleta.
Otra diferencia entre los dos problemas, es que el péndulo invertido se controla sólo con el
desplazamiento, aplicándole un par y variando su velocidad y/o dirección de desplazamiento, y
la bicicleta se puede controlar de dos formas, variando su velocidad y girando la horquilla de la
rueda delantera.
1.2 Objetivos
Este proyecto presenta dos objetivos. Por un lado, se va a realizar un estudio del vehículo
automatizado de dos ruedas. Se obtendrá un modelo matemático de la bicicleta y luego se
estudiará la estabilidad del sistema. Una bicicleta, dependiendo de la velocidad que lleve puede
ser estable y no necesitar un controlador. Por tanto, se verá en que rango de velocidades el
sistema es estable y cuando no lo es. A partir de esto, se elegirá una velocidad tal que el sistema
sea inestable y se diseñará una ley de control que garantice la estabilidad del sistema.
Posteriormente, una vez terminado el diseño del controlador y la simulación, se procederá a
implementar un programa con la ley de control y ejecutarlo en el vehículo automatizado de dos
ruedas.
7
Para la realización de todos los cálculos, estudio de estabilidad y simulaciones se utilizará el
programa MatLab1. Para la implementación de programa de control se utilizará el lenguaje C, el
cual se cargará en una tarjeta arduino UNO. Para darle las distintas órdenes al vehículo se
utilizará una aplicación android diseñada específicamente para el vehículo automatizado del
proyecto.
1.3 Metodología
La metodología que se ha seguido para la realización de este proyecto ha constado de las
siguientes fases:
I. Diseño del vehículo automatizado:
Esta fase consiste en diseñar el vehículo automatizado de dos ruedas mediante el software
de CAD Solid Edge2. Se ha procedido al diseño de todos los elementos y su montaje. A
partir de este software, se ha podido obtener parámetros físicos necesarios para la
obtención del modelo matemático.
II. Modelado del sistema:
En esta fase se ha realizado el modelo de la dinámica del vehículo automatizado de dos
ruedas para, posteriormente, poder realizar el control por realimentación. Los parámetros
del vehículo se han obtenido, como se ha mencionado antes, a partir del diseño en el
software Solid Edge.
III. Control del modelo:
Aquí se ha procedido a realizar el control y simulación del modelo matemático. Este
proceso se ha realizado con MatLab y Simulink3.
1 Matlab: software matemático que ofrece un entorno de desarrollo integrado con un lenguaje de programación propio (lenguaje M). Está disponible para las plataformas Unix, Windows y Mac OS X. El paquete Matlab dispone de dos herramientas adicionales GUIDE (editor de interfaces de usuario) y Simulink (plataforma de simulación multidominio). 2 Solid Edge: sistema de diseño asistido por computador (CAD) para el modelado de máquinas y elementos mecánicos, todo guiado a la producción de dibujos. 3 Simulink: entorno de diagramas de bloque para la simulación multidominio y el diseño basado en modelos.
8
IV. Implementación en Lenguaje C:
En esta fase se ha realizado la implementación del control del vehículo automatizado de
dos ruedas en Lenguaje C para que se pueda ejecutar desde la placa de Arduino UNO del
vehículo automatizado.
V. Comprobación del controlador:
En esta fase se procede a la verificación del controlador realizado y del programa. Se
carga el programa en el vehículo automatizado y se ejecuta.
9
2 Estado del arte
Los vehículos de dos ruedas constituyen un banco de pruebas completo e interesante. Desde hace
más de un siglo existen publicaciones referentes a estudios sobre la bicicleta, entre otros muchos
están:
Carvallo, E. Publicó en 1899 – “Theorie du mouvement du Monocycle et de la Bicyclette”, en el
Journal de L’Ecole Polytechnique.
Bourlet, C. En 1899 – “Etude theorique sur la bicyclette”, en el Bulletin de la Societe
Mathematique de France.
G. R. R. Routh. 1899 – “On the Motion of a Bicycle”, The Messenger of Mathematics.
Valentine Joseph Boussinesq, 1899 – “Apercu sur la theorie de la bicyclette”, “Complement a
une etude recente concernant la theorie de la bicyclette” y “De l’effet product, surle mouvement
d’inclinaison d’une bicyclette en marche, par les deplacements lateraux que s’imprime le
cavalier”. Los tres en el Journal de Mathematique Pures et appliquees.
F. J. W. Whipple, 1901. “The Stability of the Motion of a Bicycle”.
Robert Neil Collins, 1963. “A Mathematical Analysis of the Stability of Two Wheeled Vehicles”.
Digvijai Singh, 1964. “Advanced Concepts of the Stability of Two Wheeled Vehicles –
Application of Mathematical analysis to Actual Vehicles”.
Freeman Dyson, un físico y matemático, entrevistado por Stewart Brand en Wired Newsen
febrero de 1998, dijo:
“Incluso ahora, después de que hemos estado construyéndolas durante 100 años, es muy difícil
de entender por qué una bicicleta funciona. Es incluso difícil de formular como un problema
matemático”.
Los profesores Andy Ruina[3] y Jim Papadopoulos de la Cornell University, desde 1985, y el
profesor Arend L. Schawb[4] de la Delft University of Technology, desde 2002, han dedicado su
vida al estudio de los vehículos de dos ruedas. Estos investigadores han realizado muchos
trabajos para comprender el funcionamiento de las bicicletas.
Arend L. Schawb, en la Universidad de Delf, abrió un Laboratorio de la bicicleta. Una foto de
este laboratorio se tiene a continuación:
10
Una ponencia muy interesante la dio Arend L. Schawb en TEDxDelft2012 titulada “Por qué las
bicicletas no se caen”.
Algunos ejemplos de estudios sobre la bicicleta son:
Estudio del control de una bicicleta por un
piloto humano. A la bicicleta se le añadió
una cámara de video mirando al piloto para
identificar los movimientos del piloto en
relación al bastidor trasero.
11
Ensayo que consiste en la medición de la
estabilidad de una bicicleta en un gran giro
constante.
Bicicleta impulsada. Se mide la velocidad de
inclinación, la velocidad y el ángulo de dirección.
Joep Mutsaerts, en el laboratorio de las bicicletas
en 2010, construyó una bicicleta con el equipo
Lego Mindstorms NXT.
En 2005, la compañía Murata Manufacturing Co.[5] desarrolló el
“Murata Boy”, un robot montado en una bicicleta capaz de mantener la
estabilidad incluso en parado gracias a un volante de inercia dentro del
robot, que gira en un sentido u otro dependiendo de la inclinación.
12
2.1 Control de una bicicleta
Cuando se aprende a montar en una bicicleta o vehículo de dos ruedas, el control de la
estabilidad se hace sin pensar. Si se piensa en cómo se hace ese control, se observan dos métodos
simultáneos. Por un lado, aplicando un par en el manillar en el mismo sentido en el que se inclina
en vehículo. El otro método consiste en modificar la posición del centro de masas del conjunto
piloto-vehículo de dos ruedas, esto lo consigue el piloto desplazando su cuerpo hacia los lados.
En el caso del vehículo de dos ruedas en el que se basa este proyecto, no hay un piloto (un
ciclista) que pueda modificar la posición del centro de masas. Por tanto, se utilizará únicamente
la estrategia de control de aplicar un par de giro en la horquilla (manillar) en el sentido en el que
se esté inclinando el vehículo automático de dos ruedas. La determinación de dicho par se
obtendrá mediante un control en Espacio de Estados.
2.2 Control en Espacio de Estados
Los modelos matemáticos pueden adoptar diversas formas. Una de ellas es la representación en
Espacio de estados. Debido a que los sistemas son cada vez más complejos, pueden tener
múltiples entradas y salidas y pueden ser variantes en el tiempo, se desarrollo la teoría moderna
de control [2], que es una aproximación al análisis y diseño de los sistemas de control complejo.
Esta aproximación se basa en el concepto de estado.
Estado: El estado de un sistema dinámico es el conjunto de variables más pequeño (llamadas
variables de estado), de forma que el conocimiento de estas variables en el instante inicial y las
variables de entrada en instantes posteriores, determinan completamente el comportamiento de
dicho sistema en cualquier instante de tiempo posterior al inicial.
Variables de estado: Menor conjunto de variables que determinan el estado del sistema dinámico.
Vector de estado: Vector formado por las variables de estado del sistema. Determina
unívocamente el estado del sistema para cualquier instante posterior al inicial.
Espacio de estados: Espacio n-dimensional cuyos ejes de coordenadas son las variables de
estado.
13
Ecuaciones en el espacio de estados: Sea un sistema de múltiples entradas y múltiples salidas.
Supóngase que hay r entradas u1(t), u2(t),…, ur(t); m salidas y1(t), y2(t),…, ym (t) y n variables de
estado x1(t), x2(t),…, xn(t). El sistema se puede definir como:
= ( , , … , ; , , … , ; ) = ( , , … , ; , , … , ; )
⋮ = ( , , … , ; , , … , ; )
(1)
Y las salidas del sistema se obtienen como:
= ( , , … , ; , , … , ; )= ( , , … , ; , , … , ; )
⋮= ( , , … , ; , , … , ; )
(2)
Si se define:
( ) =
( )( )⋮( )
( , , ) =
( , , … , ; , , … , ; )( , , … , ; , , … , ; )
⋮( , , … , ; , , … , ; )
( ) =
( )( )⋮( )
( , , ) =
( , , … , ; , , … , ; )( , , … , ; , , … , ; )
⋮( , , … , ; , , … , ; )
( ) =
( )( )⋮( )
Las ecuaciones (1) y (2) quedan:
( ) = ( , , )( ) = ( , , ) (3)
Donde la primera ecuación es la Ecuación de Estado y la segunda ecuación es la Ecuación de
Salida.
Una vez se obtienen estas dos ecuaciones se linealizan en torno al estado de operación, dando
lugar a las siguientes ecuaciones de estado y de salida linealizadas:
( ) = ( ) ( ) + ( ) ( )( ) = ( ) ( ) + ( ) ( ) (4)
Si las funciones f y g no involucran al tiempo t explícitamente. El sistema se denomina un
sistema invariante con el tiempo. Las ecuaciones se simplifican a:
( ) = ( ) + ( )( ) = ( ) + ( ) (5)
14
Controlabilidad: Se dice que un sistema es controlable en el tiempo t0 si se puede transferir desde
cualquier estado inicial x(t0) a cualquier otro estado, mediante un vector de control sin
restricciones, en un intervalo de tiempo finito t0 ≤ t ≤ t1. Si todos los estados son controlables, se
dice que el sistema es de estado completamente controlable. Para el sistema en tiempo continuo
= + (6)
Donde:
x = vector de estados (dimensión n)
u = señal de control (escalar)
A = matriz de n × n
B = matriz de n × 1
Sea Mcont la matriz de controlabilidad
= [ | |… | ] (7)
Si el sistema es de estado completamente controlable, entonces, dado cualquier estado inicial
x(0), se debe satisfacer que el rango de la matriz de controlabilidad Mcont (matriz n × n) sea n.
Controlabilidad de la salida. En el diseño de un sistema de control, se puede necesitar controlar
la salida en lugar del estado del sistema. Que el sistema tenga una controlabilidad completa del
estado no es condición necesaria ni suficiente para controlar la salida del sistema.
Si se tiene un sistema en espacio de estados
= + = + (8)
Donde:
x = vector de estados (dimensión n)
u = vector de control (dimensión r)
A = matriz de n × n
B = matriz de n × r
C = matriz de m × n
D = matriz de m × r
Sea la matriz de controlabilidad a la salida
= [ | |… | | ] (7)
15
El sistema es de salida completamente controlable si es posible construir un vector de control sin
restricciones u(t) que transfiera cualquier salida inicial y(t0) a cualquier salida final y(t1) en un
intervalo de tiempo finito t0 ≤ t ≤ t1. Por tanto, el sistema es de salida completamente controlable
si, y sólo si, la matriz de controlabilidad a la salida es de rango m.
Observabilidad: Se dice que un sistema es observable en el tiempo t0 si, con el sistema en el
estado x(t0), es posible determinar este estado a partir de la observación de la salida durante un
intervalo de tiempo finito.
Se denomina matriz de observabilidad a la matriz
= [ ∗ | ∗ ∗|… |( ∗) ∗] (8)
El sistema es completamente observable si la matriz de observabilidad es de rango n.
2.2.1 Regulador óptimo cuadrático
El regulador óptimo cuadrático es un método de control de sistemas. Este método se basa en la
determinación de los polos en lazo cerrado deseados estableciéndose un compromiso entre la
respuesta aceptable y la cantidad de energía de control requerida. Es un procedimiento
sistemático para calcular la matriz de ganancias de control de realimentación del estado.
El problema del regulador óptimo consiste en dada la ecuación del sistema
= + (9)
determinar la matriz K del vector de control óptimo
( ) = − ( ) (10)
con el objetivo de minimizar la función de coste
= ( ∗ + ∗ ) (11)
donde Q es una matriz hermítica definida positiva (o semidefinida positiva) o simétrica real y R
es una matriz hermítica definida positiva o simétrica real. Estas matrices determinan la
importancia relativa del error y del coste de esta energía.
16
Una vez obtenida la matriz K, el sistema controlado por el regulador óptimo presenta la forma
siguiente
Figura. Sistema controlado con regulador óptimo
La resolución de este problema, de la determinación de la matriz K, puede hacerse a través des
programa MatLab mediante la orden lqr. Para ello se deben de definir primero las matrices Q y
R. La forma más simple de elegirlas es:
= ′ ∙= 1
(12)
Una vez definidas se procede a la obtención de la matriz K de la siguiente forma:
= ( , , , ) (13)
Con lo que ya se tiene la ley de control óptimo.
17
3 Vehículo automatizado de dos ruedas del proyecto
Como se ha dicho anteriormente, este proyecto es la continuación de otro que se titula “Diseño y
Construcción del Vehículo Autónomo de Dos Ruedas” [1]. Dicho vehículo automatizado se
puede ver en las siguientes imágenes:
Está constituido por una bicicleta de tamaño niño, a la que se le han acoplado dos motores (uno
para la tracción y otro para la dirección), una batería, un encoder en la rueda delantera para medir
la velocidad, un módulo de bluetooth HC-05, potenciómetro, una unidad de medición inercial
IMU y una placa PCB que tiene integrada una placa de Arduino Uno. A continuación se presenta
una descripción de cada uno de los elementos:
- Batería: La batería es de plomo de 12 V y 7 Ah.
- Encoder: dispositivo para medir la velocidad de giro de la rueda delantera y, por tanto, la
velocidad de avance. Formado por una barrera infrarroja y un disco con acanaladuras
equidistantes que gira solidariamente con la rueda delantera. Sabiendo cuando el disco corta
el haz y el tiempo que pasa cortado se calcula la velocidad de avance.
- Potenciómetro: se encuentra instalado sobre el eje de giro de la horquilla y usa para saber la
posición respeto a la posición de referencia (rueda delantera alineada con el bastidor).
- Placa PCB: es una placa PCB de potencia que se encarga de recibir de Arduino las señales
PWM de control del motor de tracción y del motor de dirección (o giro) de la horquilla, y
enviárselas a dichos motores.
- Motor de tracción: motor de corriente continua que se encarga de impulsar al vehículo de dos
ruedas. Su caracterización se encuentra en un apartado posterior.
18
- Motor de dirección o giro de la horquilla: motor de corriente continua que se encarga de girar
la horquilla. Su caracterización se encuentra en un apartado posterior
- Arduino Uno: placa con microcontrolador basada en el ATmega328. Recibe las órdenes
desde el módulo bluetooth, recibe información de los sensores, realiza la operación y
cálculos oportunos y envía el valor de las señales PWM de los dos motores.
- Arduimu V2 (IMU): unidad de medición inercial. Mide la posición, velocidad y aceleración
de los ángulos Roll, Pitch y Yaw. Su software es de libre distribución. En este proyecto sólo
interesa el ángulo Roll, por lo que se ha modificado dicho software para que devuelva los
parámetros de dicho ángulo.
- Módulo HC-05: módulo de bluetooth que permite la conexión con el dispositivo que se
encargue de enviar las órdenes al vehículo automatizado de dos ruedas.
19
4 Caracterización del vehículo automatizado de dos ruedas
En este apartado se va a proceder a caracterizar el vehículo automatizado de dos ruedas. Se va a
dividir en tres puntos que son:
- Modelo matemático: dividido a su vez en dos puntos, el modelado del motor de dirección y
de tracción, y el modelado del vehículo automatizado.
- Parámetros del sistema
- Simulación del modelo
4.1 Modelo matemático
Para poder realizar el control del vehículo de dos ruedas se deben tener tres modelos: el modelo
del motor de dirección, el modelo del motor de tracción (para poder controlar la velocidad del
motor y que el vehículo vaya a una velocidad constante) y el modelo del vehículo, para poder
obtener el controlador que haga que, girando la rueda delantera la bicicleta, se estabilice y no se
caiga.
4.1.1 Modelado del vehículo automatizado de dos ruedas
Un modelo exacto de un vehículo de dos ruedas (una bicicleta) resulta excesivamente complejo,
por los numerosos grados de libertad del sistema y su geometría nada trivial. Por tanto, en primer
lugar han de analizarse aspectos como las partes de la bicicleta a considerar, su elasticidad y el
modelo de interacción llanta – carretera. Si no se considera la elasticidad de las llantas ni de la
estructura de la bicicleta, se puede modelar mediante cuatro sólidos rígidos: las ruedas, cuadro y
horquilla. En este documento se utiliza como modelo del vehículo de dos ruedas el modelo de
Whipple, que denomina a los cuatro sólidos rígidos como:
- Rueda trasera = Rear wheel = R
- Rueda delantera = Front Wheel = F
- Bastidor trasero = Rear frame = B
- Bastidor delantero (horquilla) = Front frame = H
20
También se considera en este proyecto que las ruedas siempre mantienen contacto con el suelo,
con lo que se elimina considerar la deformación de las llantas y las fuerzas de rozamiento.
El proceso de modelado que se ha utilizado es el de una bicicleta linealizada de Schwab,
Meijaard y Papadppoulos que aparece en el documento “A multibody dynamics benchmark on
the equations of motion of an uncontrolled bicycle” [6]. A continuación se explica el proceso de
modelado y linealización que aparece en dicho documento.
En la siguiente figura se puede observar el modelo de una bicicleta con sus cuatro sólidos
rígidos, sus ejes de coordenadas y otros parámetros.
Figura. Modelo de la bicicleta Whipple
Para modelar una bicicleta se necesitan los centros de masas de los 4 sólidos (expresados con
respecto a x y a z y con centro en el punto O), sus momentos de inercia, sus masas, los radios de
las ruedas y los valores de los parámetros α, c y w.
α = inclinación de la horquilla = head angle.
c = rastro = trail.
W = distancia entre ejes = wheel base.
21
Para el sistema completo (se identifica con el subíndice T), se calculan la masa total y el
correspondiente centro de masas con respecto al origen O como:
m = m + m + m + m (14)
x = x m + x m + wm
m (15)
z = −r m + z m + z m − r m
m (16)
También se calculan los momentos de inercia en el origen O a lo largo de los ejes globales como:
I = I + I + I + I + m r + m z + m z + m r (17)
I = I + I + I + I + m x + m x + m w (18)
I = I + I − m x z − m x z + m wr (19)
Se determinan las mismas propiedades para el ensamblaje delantero (denominado A), siendo éste
formado por la horquilla y la rueda delantera (A = F + H), como:
m = m + m (20)
x =x m + wm
m (21)
z =z m − r m
m (22)
Se calculan los momentos de las masas y los productos de inercia para el ensamblaje delantero
en el centro de masas del mismo a lo largo de los ejes globales como:
22
I = I + I + m (z − Z ) + m (r − z ) (23)
I = I − m (x − x )(z − z ) + m (w − x )(r + z ) (24)
I = I + I + m (x − x ) + m (w − x ) (25)
Se toma λ = (sin(λ), 0, cos(λ))T como el ángulo entre el eje de rotación del conjunto A, también
llamado eje de dirección, con el eje z global:
λ = π2 − α (26)
Se calcula la distancia perpendicular que el centro de masas del ensamblaje delantero tiene con el
eje de rotación:
u = (x − w − c) cos λ − z sin λ (27)
Ahora se calcula para el ensamblaje delantero (A) los momentos de masas y productos de inercia
relevantes a lo largo del eje de rotación y los ejes globales en los puntos que se cruzan:
I = m u + I (sin λ) + 2I sin λ cos λ + I (cos λ) (28)
I = −m u z + I sin λ + I cos λ (29)
I = m u x + I sin λ + I cos λ (30)
Se define la proporción entre el rastro mecánico (c), distancia perpendicular del punto de
contacto de la rueda delantera a la prolongación del eje de dirección) y la distancia entre ejes (w)
como:
μ =cw cos λ (31)
Los coeficientes girostáticos de la rueda delantera y la trasera son:
S =Ir , S =
Ir (32)
23
El coeficiente girostático total es:
S = S + S (33)
Se define un término de momento estático que aparece frecuentemente como:
S = m u + μm x (34)
Las ecuaciones de movimiento para un vehículo de dos ruedas están expresadas con los grados
de libertad:
q = q , q = [Φ, δ] (35)
Donde:
Φ = Ángulo de inclinación del vehículo.
δ = Ángulo de giro de la horquilla.
Siendo las ecuaciones de movimiento linealizadas de un vehículo de dos ruedas:
Mq + vC q + (gK + v K )q = f (36)
Con todo lo anterior se obtiene, la matriz de masas M:
M = M MM M (37)
Donde:
M = I , M = I + μI ,M = M , M = I + 2μI + μ I , (38)
La matriz K0:
K = K KK K (39)
Con:
K = m z , K = −S ,K = K , K = −S sin λ , (40)
24
La matriz K2:
K = K KK K (41)
Con:
K = 0, K =S − m z
w cos λ ,
K = 0, K =S + S sin λ
w cos λ , (42)
En las ecuaciones se usa la matriz de rigidez:
K = gK0 + v2K2 (43)
La matriz C1:
C = C CC C (44)
Con:
C = 0, C = μS + S cos λ +Iw cos λ − μm z ,
C = −(μS + S cos λ), C =Iw cos λ + μ S +
Iw cos λ ,
(45)
La matriz de amortiguamiento queda como:
C = vC1 (46)
Por tanto, las ecuaciones de movimiento se pueden expresar de la forma:
Mq + Cq + Kq = f (47)
Siendo fd:
f = [0, T ] (48)
El siguiente paso consiste en pasar de estas ecuaciones de movimiento a un modelo en Espacio
de Estados. El primer paso consiste en elegir las variables del vector de estado, se han elegido el
ángulo de inclinación o (Φ), el ángulo de giro de la horquilla o (δ) y sus derivadas:
X = Φ, δ, Φ, δ (49)
25
Como ya se sabe, un modelo en Espacios de Estados tiene la siguiente forma:
dX = AX + BUY = CX + DU (50)
Por lo que se tienen que obtener las nuevas matrices: A, B, C y D. Para ello, el primer paso
consiste en multiplicar la ecuación (47) por la inversa de la matriz M, M-1, así se tiene que:
M Mq + M Cq + M Kq = M f (51)
q + M Cq + M Kq = M f (52)
Si se nombra como:
S = M CK = M KM = M
(53)
La ecuación queda de la forma:
q + Sq + Kq = Mf (54)
A continuación se sustituyen las ecuaciones (35) y (48):
+ S qq
+ K qq
= M 0T (55)
Se despeja:
= M 0T − S q
q− K q
q (56)
Se definen las variables de estado como:
X = qX = qX = qX = q
→
X = qX = qX = qX = q
(57)
Se sustituyen en la ecuación (56) y se ordena:
⎣⎢⎢⎢⎡
⎦
⎥⎥⎥⎤
= −
0 −1 0 0
0 0 0 −1 +
0 00
0 00
0 00
0 00
000T
(58)
26
La variable de salida será el ángulo de inclinación del vehículo automatizado, con lo que se
define:
Y = Φ = X (59)
Y = [1 0 0 0]
XXXX
+ [0 0 0 0]000T
(60)
Las ecuaciones (58) y (60) definen el modelo en Espacio de Estados:
⎣⎢⎢⎢⎡
⎦
⎥⎥⎥⎤
= −
0 −1 0 0
0 0 0 −1 +
0 00
0 00
0 00
0 00
000T
Y = [1 0 0 0]
XXXX
+ [0 0 0 0]000T
(61)
Se puede expresar el modelo en Espacio de Estados un poco más reducido como:
⎣⎢⎢⎢⎡
⎦
⎥⎥⎥⎤
= −
0 −1 0 0
0 0 0 −1 +
0
0 T
Y = [1 0 0 0]
XXXX
+ [0]T
(62)
4.1.2 Caracterización de los ángulos de inclinación y giro
La caracterización de los ángulos de inclinación del vehículo y el giro de la horquilla es un paso
importante, ya que a partir de su definición, se ha diseñado la estrategia de control. Esta
caracterización consiste en definir desde donde se comienzan a medir dichos ángulos, es decir,
definir su sistema de referencia.
27
Para el ángulo de inclinación del vehículo, se ha definido el valor de 0º cuando el vehículo se
encuentre en posición vertical, tomará valores negativos conforme se incline hacia la derecha y
valores positivos cuando se incline hacia la izquierda.
Figura. Sistema de referencia del ángulo de inclinación
En el caso del ángulo de giro de la horquilla, se ha definido el origen cuando la rueda delantera
se encuentre alineada con el chasis, tomará valores negativos cuando se gire hacia la derecha y
valores positivos cuando se gire hacia la izquierda.
Figura. Sistema de referencia del ángulo de de giro de la horquilla
28
4.1.3 Modelos matemáticos de los motores de tracción y dirección
Para la caracterización de los dos actuadores (motores) se han cogido muchos de los datos
obtenidos en el proyecto del que parte este, que ya se mencionó en la introducción, y se titula
“Diseño y Construcción del Vehículo Autónomo de Dos Ruedas” [1].
4.1.3.1 Motor de dirección
El actuador de la dirección es un motor Bosh 058 brose de 12 Voltios, el cual se ha modelado
tanto la parte eléctrica como la mecánica.
Figura. Motor de dirección
Del motor de dirección se modela su parte eléctrica y su parte mecánica. El modelo eléctrico
equivalente de un motor DC es.
Figura. Modelo eléctrico de un motor DC
29
Donde:
- Rm: resistencia interna.
- Lm: inductancia
El modelo mecánico es:
Figura. Modelo mecánico de un motor DC
Donde:
- Jm: inercia mecánica
- Tr: par resistente
- Dm: fricción viscosa
Primero se mide la resistencia interna (Rm). Se hace midiendo con un multímetro los pines de
alimentación del motor en vacío. Esto da como resultado una resistencia de 1.6 Ω.
Luego se procede a identificar los parámetros que faltan. Para ello, se ha procedido a variar el
factor de servicio en ambos sentidos y anotando los datos de tensión del motor (Vmot), intensidad
que circula por el motor (Imot) y velocidad angular (ω). Para medir la corriente que circula por el
motor se ha utilizado una resistencia Shunt. Se añade también el valor de la fuerza
contraelectromotriz fem que se calcula de la siguiente forma:
= ( ∙ ) (63)
En un motor DC aparece una tensión en el inducido, cuando está en rotación, que es proporcional
al producto del flujo por la velocidad angular. Si el flujo es constante, como ocurre en este caso,
la fem es directamente proporcional a la velocidad angular
( ) = ∙ (64)
30
PWM (%) Imot (A) Vmot (V) ω (rad/s) fem (V)
100 1.46 10.87 4.51 8.53
80 1.35 9.77 3.35 7.10
60 0.96 8.61 2.35 4.47
40 0.74 6.67 1.22 2.66
0 0 0 0 0
-40 -0.58 -7.41 -4.36 -2.32
-60 -0.87 -9.31 -3.23 -4.40
-80 -1.12 -10.12 -2.18 -6.15
-100 -1.48 -10.83 -1.01 -8.69
Figura. Datos medidos del motor DC
Si se representa gráficamente la ecuación (64) se obtiene:
Figura. fem frente a velocidad angular
Con lo que se obtiene que el valor de la constante eléctrica, Ke, es de 1.9842 Nm/rad. El valor de
Kt es el mismo siempre que se utilicen las unidades del SI, 1.9842 Nm/A.
Para el cálculo de la fricción viscosa y el par resistente se utilizan las siguientes expresiones:
= ∙ (65)
− = + ∙ (66)
31
En régimen permanente queda:
= ∙ + (67)
Tomando las medidas dependiendo los valores de PWM se obtiene:
PWM (%) Tm (Nm) ω (rad/s)
100 2.89 4.51
80 2.68 3.35
60 1.90 2.35
40 1.47 1.22
0 0 0
-40 -1.15 -4.36
-60 -1.73 -3.23
-80 -2.22 -2.18
-100 -2.94 -1.01
Figura. Relación PWM – Tm - ω
Con estos datos se puede ajustar una recta de mínimos cuadrados y, así, obtener los valores de
Dm y Tr en ambos sentidos. Puesto que al motor se le pasa un valor de PWM positivo, se ha
calculado la recta de regresión para el motor girando hacia la derecha con valores positivos, es
decir, PWM positivo, Tm positivo y ω positiva.
Figura. Tm frente a velocidad angular girando a derecha e izquierda
32
Los valores obtenidos para la relación Tm – ω son:
Giro hacia la izquierda Giro hacia la derecha
Dm 0.528 0.529
Tr 0.5861 0.7819
Figura. Parámetros Tm - ω
Se puede apreciar como el valor del par resistente es muy parecido y el valor de Dm
prácticamente el mismo
Con respecto a la relación PWM – ω se tiene:
Figura. PWM frente a velocidad angular girando a derecha e izquierda
A dicha relación también se le puede obtener una recta de regresión de la forma:
= ∙ + (68)
Los valores obtenidos son:
Giro hacia la izquierda Giro hacia la derecha
A 18.0102 18.2573
B 21.4348 17.6617
Figura. Parámetros PWM - ω
33
4.1.3.2 Motor de tracción
El actuador que se encarga de la tracción es un motor Bosch 058 CEP de 12 V.
Figura. Motor de tracción
Cogiendo los valores obtenidos en el proyecto “Diseño y construcción de un vehículo autónomo
de dos ruedas” [1] se tiene que el valor de resistencia interna es de Rm = 0.5 Ω.
El ensayo de ir variando el factor de servicio se tienen como resultados:
PWM (%) Imot (A) Vmot (V) ω (rad/s) fem (V)
100 3.45 11.27 6.54 9.54
96 3.26 10.46 6.04 8.83
92 2.95 7.99 4.39 6.51
90 2.92 7.05 3.85 5.59
80 2.84 6.81 3.63 5.39
70 2.52 5.50 2.85 4.24
60 2.23 4.11 2.22 2.99
50 1.82 2.75 1.25 1.84
40 1.40 1.34 0.58 0.64
30 0.95 0.41 0 0
20 - - - 0
10 - - - 0
0 0 0 0 0
Figura. Medición motor de tracción
34
Los valores de Tm:
PWM (%) Tm (Nm) ω (rad/s)
100 5.14 6.54
96 4.85 6.04
92 4.39 4.39
90 4.35 3.85
80 4.23 3.63
70 3.75 2.85
60 3.32 2.22
50 2.71 1.25
40 2.08 0.58
30 - 0
20 - -
10 - -
0 0 0
Figura. Medición Tm motor de tracción
Dando lugar a la siguiente caracterización del motor:
Resistencia interna Rm 0.5 Ω
Constante eléctrica Ke 1.4889 Nm/rad
Inductancia Lm 0.63 mH
Constante mecánica Kt 1.4889 Nm/A
Fricción viscosa Dm 0.4848 Nms
Inercia mecánica Jm 0.1816 Kgm2
Par resistente Tr 2.1801 Nm
Figura. Caracterización motor de tracción
El estudio del tren de arrastre (formado por el plato, cadena y piñón) da lugar a la obtención de la
velocidad máxima de arrastre. El cálculo se expresa en el siguiente desarrollo. Si se denomina
“1” al plato del motor de arrastre y “2” al piñón de la rueda trasera:
∙ = ∙
35
6.54 ∙ 18 = ∙ 16
= 7.51
= ∙ = 7.51 ∙ 0.141 = 1.05 ⁄
Por tanto la velocidad de avance máxima teórica es de 1.05 m/s. Para comprobar dicho resultado
se ha medido el tiempo que tarda el vehículo en recorrer una distancia fija. Ya que la velocidad
es igual al espacio recorrido partido por el tiempo, se obtiene la velocidad de avance. Por tanto,
se ha hecho recorrer al vehículo una distancia de 3 m varias veces y se ha obtenido que el tiempo
medio de recorrer esa distancia fue de 3 s. Por lo que la velocidad de avance resulta ser de 1 m/s.
Con esto queda comprobado que el valor teórico y experimental coinciden.
4.2 Parámetros del sistema
En este apartado se va a diseñar el vehículo utilizando la herramienta CAD 3D Solid Edge V19.
Gracias a este programa, se va a poder obtener todo tipo de información acerca del vehículo:
volumen total, peso del vehículo completo, centro de masas, momentos de inercia respecto a
diferentes ejes, etc. Estos parámetros serán necesarios para el modelo matemático del robot.
Los parámetros que se necesitan son:
- Masa, centro de masas y momentos de inercia de los cuatro sólidos rígidos en los que se ha
dividido el vehículo (rueda delantera y trasera, bastidor delantero y trasero)
- Rastro mecánico (c) y distancia entre ejes (w).
En primer lugar, para poder realizar el diseño de CAD, se desmonta el vehículo y se clasifican
las piezas. Luego se realizan las medidas de todas ellas y se procede al diseño de cada una.
A continuación se muestran los diferentes subconjuntos diseñados:
36
Figura. Rueda delantera (F)
Figura. Rueda trasera (R)
37
Figura. Bastidor trasero (B)
Figura. Bastidor delantero (H)
A continuación se comienza el ensamblado de los cuatro sólidos rígidos. El vehículo diseñado
ensamblado se muestra en la siguiente figura:
38
Figura. Vehículo automatizado de dos ruedas
Una vez se tienen todas las piezas diseñadas, se procede a la obtención de los parámetros físicos
de los cuatro sólidos rígidos. Estos parámetros son obtenidos a partir del programa Solid Edge.
Las tablas que se muestran a continuación muestran los distintos parámetros obtenidos:
Parámetros del bastidor trasero (B):
Masa mB 11.688 kg
Centro de Masas
xB 0.16964 m
yB 0.00000 m
zB -0.32278 m
Momentos de Inercia
IBxx 1287256095 g·cm2
IByy 17693860.34 g·cm2
IBzz 5247300.08 g·cm2
IBxy -11663.66 g·cm2
IBxz -6633840.72 g·cm2
IByz -3617.98 g·cm2
39
Parámetros de la rueda trasera (R):
Masa mR 0.745 kg
Centro de Masas
xR 0.00000 m
yR 0.01051 m
zR -0.14442 m
Momentos de Inercia
IRxx 178731.66 g·cm2
IRyy 192705.96 g·cm2
IRzz 23370.20 g·cm2
IRxy -90.86 g·cm2
IRxz 1241.49.57 g·cm2
IRyz -11308.84 g·cm2
Radio RR 0.144 m
Parámetros de la rueda delantera (F):
Masa mF 0.518 kg
Centro de Masas
xF 0.56563 m
yF 0.00000 m
zF -0.17137 m
Momentos de Inercia
IFxx 172259.93 g·cm2
IFyy 1844839.26 g·cm2
IFzz 1676751.11 g·cm2
IFxy 1688.76 g·cm2
IFxz -501900.08 g·cm2
IFyz -530.82 g·cm2
Radio RF 0.144 m
40
Parámetros Bastidor delantero (H):
Masa mH 0.681 kg
Centro de Masas
xH 0.50256 m
yH 0.00000 m
zH -0.30711 m
Momentos de Inercia
IHxx 709469.48 g·cm2
IHyy 2433520.60 g·cm2
IHzz 1742437.01 g·cm2
IHxy -1868.52 g·cm2
IHxz -025917.89 g·cm2
IHyz -543.76 g·cm2
También se ha sacado la masa total del vehículo automatizado diseñado en el Solid Edge y la
masa del vehículo obtenida a partir de una báscula. Al compararlas, se ha obtenido que ambos
valores son prácticamente iguales. De la misma forma se ha hecho para las distintas partes del
Vehículo, obteniéndose valores similares.
Para la obtención de los valores del rastro mecánico y la distancia entre ejes, también se han
obtenido por los dos métodos para así comprobar la precisión del diseño en Solid Edge.
En las imágenes siguientes, se puede observar las mediciones obtenidas con un metro de los dos
parámetros:
41
Figura. Medición de la distancia entre ejes
Haciendo una ampliación a la medida obtenida se observa que la distancia entre ejes es de 567
mm. A continuación se presenta dicha ampliación
Figura. Ampliación de la medición
42
Para la medición del rastro mecánico se tiene la siguiente imagen:
Figura. Medición del rastro mecánico
Se puede observar que obtenemos un rastro mecánico de 58.5 mm.
A continuación, se ven sendas medidas a partir del diseño en Solid Edge:
Figura. Medición del rastro mecánico y distancia entre ejes en Solid Edge
43
Comparando ambos parámetros en una tabla se obtiene:
Medida a mano (mm) Medida en Solid Edge (mm)
Rastro mecánico 58.5 58.2
Distancia entre ejes 567 567.41
Se aprecia claramente como en ambos métodos dan valores muy parecidos.
Con todo lo visto, se puede admitir que los parámetros obtenidos con Solid Edge son lo
suficientemente precisos con lo que se utilizarán para obtener el modelo en Espacio de Estados,
calcular la ley de control y realizar el modelado en Simulink.
Una vez hecho esto, se procede a introducir todos estos parámetros en las ecuaciones del modelo
del vehículo automatizado que se han obtenido en el apartado 4 de este proyecto. Para todos
estos cálculos se utiliza el software Matlab. Para ir obteniéndose las distintas matrices y llegar
hasta el modelo en Espacio de Estados, los parámetros del vehículo se han introducido en una
función de Matlab a la que se ha llamado matricessinpc.m. Esta función devuelve las matrices M,
K0, K2 y C1, que son las matrices del modelo matemático expresado de la forma:
Mq + vC q + (gK + v K )q = f (69)
Si se ejecuta dicha función se obtiene en la ventana de MatLab lo que aparece en la siguiente
figura.
44
Figura. Matrices del modelo matemático en MatLab
El siguiente paso es obtener las matrices M, C y K, de la ecuación (47) y a partir de ellas las
matrices A, B, C, D del modelo en Espacio de Estados. Las matrices M, C y K dependen de la
velocidad de avance del vehículo. Para el control del vehículo automatizado de este proyecto se
ha definido la velocidad de avance en 1 m/s. Por ello, se han calculado dichas matrices para ese
valor de velocidad. Para hacer esto, se ha creado un programa llamado Parametrossinpcv1.m que
a partir de las matrices M, K0, K2 y C1 se obtiene las matrices A, B, C y D (en este programa a
las matrices M, S y K se les llama Mo, C y K respectivamente). Los resultados de este programa
se pueden dividir en dos partes. En la primera parte se obtienen las matrices M, C y K de la
ecuación de movimiento:
Mq + Cq + Kq = f (47)
45
Figura. Obtención matrices M, C y K
En la segunda parte, se obtienen las matrices A, B, C y D del modelo en Espacio de Estados para
una velocidad de avance de 1 m/s
46
Figura. Obtención de las matrices A, B, C y D
4.3 Estabilidad del vehículo automatizado de dos ruedas
Para poder determinar para que rango de velocidades el vehículo automatizado es estable y para
cual es inestable, se debe resolver su ecuación característica y ver que signo tienen la parte real
de sus autovalores (λ).
En primer lugar, se parte de la ecuación de movimiento:
Mq + Cq + Kq = f (47)
47
Si lo que se quiere es mirar la estabilidad del sistema sin control, se deben quitar los pares de
fuerza, es decir:
f = 0 (70)
El polinomio característico del vehículo automatizado queda:
Mλ + Cλ + K = 0 (71)
O lo que es lo mismo:
Mλ + vC λ + gK + V K = 0 (72)
Como los parámetros C y K dependen de la velocidad. Lo que se hace es para un rango de
valores de la velocidad, se calculan sus respectivos autovalores. Esta acción se ha realizado
mediante un programa en MatLab, llamado Estabilidadsinpc.m, obteniéndose la siguiente figura:
Puede observarse que se tienen cuatro autovalores en los cuales dos de ellos siempre tienen parte
real negativa, por lo que siempre son estables. Sin embargo, los otros dos tienen tanto parte real
negativa como positiva. Para que el sistema sea estable todas las partes reales de los polos deben
48
ser negativas. Si se observa la figura, el autovalor que se representa por la línea azul superior,
pasa a ser imaginario puro en torno a 8.5 m/s.
4.4 Simulación del modelo
En este apartado se va a simular el modelo en Espacio de Estados obtenido y se va a comprobar
si los resultados de la simulación son los esperados.
En primer lugar se ha simulado el sistema en bucle abierto. Al modelo se le ha dado una entrada
nula (par de giro de la horquilla nulo) que indica que se deja libre el giro de la horquilla.
También se le han añadido varios conversores de ángulos para una mayor claridad a la hora de
ver los resultados.
El modelo en Simulink es:
Figura. Modelo en Bucle abierto
Se han realizado varias simulaciones dándole diferentes condiciones iniciales. Las dos
simulaciones que se muestran a continuación, presentan como condiciones iniciales, fuera del
punto de equilibrio, un pequeño ángulo de inclinación hacia la izquierda de 5º (para indicar que
es hacia la izquierda se pone como valor positivo) y otro hacia la derecha de 5º (valor negativo).
49
Figura. Simulación bucle abierto inclinado 5º a la izquierda
Figura. Simulación bucle abierto inclinado 5º a la derecha
50
Como se puede apreciar pasa lo esperado, sacando al sistema sin control de la posición de
equilibrio (vehículo vertical con las ruedas alineadas) tiende a caerse completamente, así, una
leve inclinación hacia la derecha hace que se caiga hacia ese lado y al contrario, un leve desvío
de la posición vertical hacia la izquierda hace que se caiga hacia ese lado.
51
5 Diseño del controlador
En primer lugar se ha intentado realizar un controlador PID. Para ello, se ha realizado un
programa de MatLab, CaractSistsinpcv1.m, para obtener la función de transferencia del sistema
y obtener el controlador PID mediante la función de MatLab pidtune. Esta función sirve para
ajustar el controlador PID y, además, si se le pide, devuelve información del sistema, entre otros
datos, si el sistema se hace estable. La función de transferencia del sistema es:
Figura. Función de transferencia del sistema
El resultado que da la función pidtune de MatLab es:
Figura. Pidtune
Como se puede observar, la función devuelve la información del sistema diciendo que el sistema
no es estable (Stable: 0).
52
La función CaractSistsinpcv1.m también devuelve dos figuras. En ellas se representan la
respuesta del sistema ante un escalón tanto en bucle abierto como en bucle cerrado. En ambas
figuras se puede apreciar claramente como el sistema es inestable.
Figura. Respuesta del sistema ante entrada escalón en Bucle Abierto
Figura. Respuesta del sistema ante entrada escalón en Bucle Cerrado
Se ha probado el diseño del controlador PID y la estabilidad del sistema a partir de la función de
MatLab pidtool.
53
Figura. Pidtool
Obteniéndose lo esperado, el sistema no se consigue estabilizar.
Debido a que no se ha conseguido estabilizar el sistema con un controlador PID, se ha utilizado
otra estrategia de control, para este problema se ha utilizado el regulador cuadrático óptimo
5.1 Diseño y simulación del control mediante el regulador cuadrático óptimo
Antes de realizar el diseño del regulador cuadrático óptimo, se ha estudiado la controlabilidad y
observabilidad del sistema. Para ello, se ha creado una función de MatLab llamada
ContrObssinpc.m. Esta función estudia la controlabilidad y observabilidad del sistema para un
rango de velocidades de avance que va desde v = 0 m/s a v = 1 m/s. Para todo el rango de
velocidades se obtiene que el sistema presenta controlabilidad y observabilidad completa. En
particular, para la velocidad de 1 m/s que es la velocidad a la que se quiere controlar el sistema
se obtiene que:
54
Figura. Controlabilidad y Observabilidad
Para el diseño del regulador cuadrático óptimo también se ha realizado un programa de MatLab
llamado LQR.m. En este programa, a partir del modelo en Espacio de Estados, se calculan las
matrices Q y R tal y como se indican en la expresión:
= ′ ∙= 1
(12)
De esta forma, la matriz Q queda de la forma:
=1 00 0
0 00 0
0 00 0
0 00 0
Para obtener el vector K del control lqr sólo se tiene que ejecutar en MatLab la función lqr tal y
como se ha explicado en el apartado 2.2.1
= ( , , , ) (13)
Se pueden obtener distintos vectores K si se varían los pesos de la matriz Q. El programa de
MatLab LQR.m varía los pesos de la matriz Q de la forma:
55
=0
0 00 00 0
0 00 0
0 00 0
Siendo prb el peso que se va modificando únicamente. En este programa se prueba con los
valores prb = 1, 10 y 100. El programa, a parte del vector de ganancias K, devuelve varias
gráficas. En primer lugar, muestra la evolución de las cuatro variables de estado del sistema sin
controlar.
Figura. Variables de Estado del sistema sin control
A continuación en el Command Windows de MatLab aparece la matriz Q, el vector K para las
variables de estado en radianes y el vector K para las variables de Estado en sexagesimales (esto
último para el programa de la placa arduino). También para cada matriz Q aparecen tres figuras:
una figura que es el esfuerzo de control (par de giro de la horquilla), otra figura que son las
variables de estado del sistema controlado en radianes y una última figura que son las variables
de estado del sistema controlado en grados sexagesimales. Se deduce de las gráficas obtenidas
que conforme se aumenta el valor de prb más rápido tiende a estabilizarse el sistema. Para el
valor prb = 10 se tienen los siguientes resultados:
56
Figura. Matriz Q y vector K en rad y en dec
Figura. Esfuerzo de control. Par de giro de la horquilla con prb = 10
57
Figura. Variables de Estado del sistema controlado con prb = 10. Ángulos en grados sexagesimales
Figura. Variables de Estado del sistema controlado con prb = 10. Ángulos en radianes
58
En el anexo Experimentos se muestran los resultados para los tres valores de prb. Hay que
destacar, que un aumento del valor de prb disminuye el tiempo necesario para estabilizar el
sistema. Se ha elegido este valor de prb ya que proporciona una dinámica de control lo
suficientemente rápida.
59
6 Implementación del control en el vehículo automatizado
Una vez que se tiene diseñada la estrategia de control, el siguiente paso es la comprobación en el
vehículo automatizado. Dado que el vehículo automatizado no tiene un botón de inicio y parada,
se le ha añadido un módulo de bluetooth y se le ha diseñado una aplicación para android con la
que se le puede enviar las distintas órdenes sin tener que ir junto al vehículo en todo momento.
6.1 Instalación del módulo de bluetooth HC-05
Bluetooth es un estándar de comunicación inalámbrica que permite la transmisión de datos a
través de radiofrecuencia en la banda de 2.4 GHz. En este proyecto se utiliza el módulo bluetooth
HC-05. Su datasheet se encuentra en los Anexos de este proyecto.
Figura. Módulo de Bluetooth HC-05
Este módulo puede ser utilizado tanto como maestro como esclavo. Presenta los siguientes pines:
- Vcc: Alimentación del módulo entre 3,6V y 6V.
- GND: La masa del módulo.
- RXD: Recepción de datos a un voltaje de 3,3V.
- TXD: Transmisión de datos.
- KEY: Poner a nivel alto para entrar en modo configuración del modulo.
- STATE: Para conectar un led de salida para visualizar cuando se comuniquen datos.
La conexión del módulo HC-05 se realiza como se muestra en la siguiente imagen.
60
Figura. Conexión módulo HC-05 a Arduino Uno
El pin GND del módulo HC-05 se conecta al pin GND de la placa Arduino Uno. Los pines de
transmisión y recepción de datos (TXD y RXD) del módulo bluetooth se conectan a los pines 10
y 11 de Arduino. Estos pines, mediante la librería SoftwareSerial, se definirán como un pin de
recepción, pin 10, y uno de transmisión, pin 11 (obsérvese que se conectan cruzados: RXD de
HC-05 con TXD de Arduino y TXD de HC-05 con RXD de Arduino). Esto se hace así para tener
otro bus serie y dejar habilitado el que trae la placa Arduino Uno. Por último, se conecta el pin
de alimentación del módulo HC-05 (Vcc) con el pin de alimentación de Arduino de 5V.
Una vez conectado el módulo HC-05 se procede a su configuración. La configuración se hace a
partir de códigos AT. Lo primero que hay que hacer es cargar el sketch que permite el envío de
dichos comandos AT (este sketch se encuentra en el anexo de programas Arduino), esto se debe
hacer con el módulo bluetooth desconectado. Una vez cargado el sketch se desconecta Arduino
del PC y se conecta el módulo HC-05. Para poner el módulo bluetooth HC-05 se debe poner el
pin KEY en HIGH. Para ello el módulo HC-05 que se utiliza en este proyecto presenta un botón
que se debe tener pulsado unos segundos. Por tanto, una vez está el módulo bluetooth conectado,
se procede a pulsar dicho botón mientras se conecta la tarjeta Arduino Uno al PC. Pasados unos
segundos, el led del HC-05 cambia la velocidad de parpadeo, indicando que ha entrado en modo
configuración (modo comandos AT). Los comandos AT son:
- AT: se usa para testear el módulo. Si se envía y devuelve Ok es que el módulo se
encuentra en modo de configuración.
- AT+RESET: resetea el módulo al estado de inicio.
61
- AT+ROLE?: devuelve el modo en que se encuentra el módulo: 0 = esclavo, 1 =
maestro, 2 = esclavo – bucle.
- AT+ROLE=<parámetro>: configura el módulo como maestro o esclavo: 0 = esclavo,
1 = maestro, 2 = esclavo – bucle.
- AT+CMODE?: averigua el modo de enlace: 0 = dirección de enlace bluetooth
específica, 1 = cualquier dirección de enlace bluetooth, 2 = esclavo – bucle
- AT+CMODE=<parámetro>: sirve para configurar el modo de enlace del módulo.
- AT+PSWD?: averigua el password del módulo.
- AT+PSWD=<parámetro>: configura el password de aparejamiento.
- AT+UART?: devuelve la configuración de baudios UART (TX y RX).
- AT+UART=<parámetro>: se usa para asignar la velocidad de los pines TX y RX del
módulo.
- AT+ORGL: configura todos los parámetros con los valores de fábrica.
- AT+NAME?: averigua el nombre asignado al módulo.
- AT+NAME=<parámetro>: devuelve el nombre asignado al módulo.
- AT+ADDR?: averigua la dirección MAC del dispositivo.
- AT+BIND?: averigua la MAC del dispositivo vinculado al módulo.
- AT+BIND=<parámetro>: establece el vínculo con un dispositivo bluetooth usando la
MAC para direccionar.
Para el módulo bluetooth del vehículo automatizado de dos ruedas de este proyecto se le ha dado
el nombre de PANIUSKA, configurado como esclavo, con una velocidad de 9600 bd y su
contraseña es la que trae por defecto: 1234.
6.2 Configuración de la unidad de medición inercial Arduimu v2
La unidad de medición inercial (IMU) es la encargada de medir el ángulo y la velocidad de
inclinación del vehículo de dos ruedas. Esta se comunica con la placa Arduino UNO mediante el
protocolo de comunicación I2C.Para que se pueda comunicar mediante este protocolo de
comunicación y envie los datos que se necesitan hay que modificar el firmware. Este Firmware
es abierto, con lo que se puede encontrar en internet. Una vez descargado se abre con una IDE de
Arduino, que también puede descargarse. El firmware de la IMU consta de las siguientes
funciones.
62
Figura. Firmware Arduimy v2
De todas estas funciones sólo se modificarán dos: arduimu y output.
En la función principal, arduimu, se puede ver en la figura anterior que se ha activado la librería
wire.h. Esto se debe a que es esta librería la que permite realizar la comunicación I2C. También
se ha configurado para que imprima los ángulos de Euler.
Figura. Definición imprimir ángulos de Euler.
63
A continuación se han definido una serie de parámetros y declarado unas variables necesarias
para el envío por comunicación I2C. El siguiente paso, es la creación de una función void I2C()
que lo que hace es transmitir el ángulo y velocidad de inclinación. Esa parte del código se
muestra en la siguiente gráfica.
Figura. Función de comunicación por I2C.
.
Esta función lo que hace es enviar un vector con cuatro elementos. Dichos elementos son la parte
entera y decimal del ángulo de inclinación, la parte entera y decimal de la velocidad de
inclinación.
La función Output es la encargada del envío de los parámetros requeridos en el firmware
mediante el protocolo puerto serie. Las modificaciones son:
64
Figura. Modificación función Output.
Con estas modificaciones, si se conecta la IMU al ordenador por el puerto serie mediante un
cable FTDI se obtiene en el monitor serie:
Figura. Modificación función Output.
Cada columna significa:
- FLOATROLL: ángulo Roll en radianes
- INTENTROLL: parte entera del ángulo Roll en grados sexagesimales
65
- INTDECROLL: parte decimal del ángulo Roll en grados sexagesimales
- FLOATOMEGAROLL: velocidad angular de Roll en radianes/s
- INTENOMEGATROLL: parte entera de la velocidad angular del ángulo Roll en º/s
- INTDECOMEGAROLL: parte decimal de la velocidad angular del ángulo Roll en º/s
6.3 Implementación del control del vehículo automatizado
El programa de control del vehículo automatizado consta de dos partes. Por un lado, el diseño de
una aplicación para android con la que se mandarán las distintas órdenes al vehículo. Por otro
lado, la compilación de un programa para la placa Arduino UNO que reciba dichas órdenes y las
ejecute.
6.3.1 Desarrollo de una aplicación android
La aplicación se ha realizado con MIT App Inventor 24. En primer lugar, se ha diseñado la
pantalla (screen) de la aplicación. Esto se hace en la página Diseñador (designer). En la siguiente
figura se muestra una captura de la pantalla terminada con los componentes ya añadidos que se
explican a continuación.
Figura. Screen de la Aplicación móvil.
4 MIT App Inventor: plataforma para crear aplicaciones de software para el sistema operativo Android.
66
Los distintos componentes son:
- Bluetooth: Este pulsador es un listpicker. Cuando se pulsa, se despliega una lista con todos los
terminales con bluetooth activados. Se debe enlazar con el vehículo automatizado.
- Desconectar: Este botón, como su nombre indica, lo que hace es desconectar el enlace por
bluetooth.
- Indicador de conexión: Entre los componentes Bluetooth y Desconectar hay un componente
label (una etiqueta), lo que hace es escribir en la pantalla conectado, si el bluetooth está
conectado, y desconectado si no lo está.
- Control Automático: Botón que le indica al vehículo automatizado que realice el control de la
estabilidad.
- Control Manual: Botón que hace que el vehículo entre en modo manual, es decir, se puede
manejar por los botones de dirección y velocidad de la parte inferior
- Giro Izqda: Botón que indica al vehículo que gire la horquilla hacia la izquierda.
- Giro Drcha: Botón que indica al vehículo que gire la horquilla hacia la derecha.
- Stop Giro: Botón que hace que la horquilla pare de girar, tanto si está girando a la derecha
como si está girando a la izquierda.
- Alinear: Botón que alinea la rueda delantera con el chasis
- Rápido: Botón que pone la velocidad de tracción a la máxima posible.
- Lento: Botón que pone la velocidad de tracción lenta.
- Parado: Botón que deja parado al vehículo automatizado, velocidad de tracción nula.
Una vez se han añadido todos los componentes, se han renombrado y dado las dimensiones y la
posición que se desea que tengan, se procede a asignar las diferentes funciones de cada
componente. Para hacerlo, hay que ir a la ventana del Editor de bloques (blocks editor). El Editor
de bloques es una aplicación Java donde se configura el comportamiento de nuestro desarrollo.
67
Figura. Blocks editor de la Aplicación móvil.
Los cuatro primeros bloques están referidos a la conexión por bluetooth. Así, el primer bloque
configura la aplicación para que cuando se ejecute el programa, el indicador de conexión muestre
que la conexión Bluetooth está desconectada.
Figura. Bloque de inicio de la aplicación móvil.
Los dos siguientes bloques configuran el pulsador Bluetooth, que es un listpickers (selector de
lista).
68
Figura. Bloques Bluetooth.
Cuando se pulsa, aparece una lista con todos los dispositivos con los que previamente se ha
sincronizado nuestro dispositivo móvil mediante bluetooth y están disponibles. Se selecciona
nuestro vehículo automatizado de dos ruedas. La selección puede verse en la siguiente figura.
Figura. Selector Bluetooth.
69
Si se ha realizado la vinculación por bluetooth entonces aparece en el label indicador de
bluetooth la palabra “Conectado” en verde. Si por el contrario, no se realiza la vinculación,
aparecen las palabras “No conectado” en rojo.
El pulsador Desconectar sirve para desconectar el dispositivo móvil del vehículo de dos ruedas.
Por ello, cuando se pulsa este botón, aparece en el label indicador las palabras “No Conectado”
en rojo. Su bloque es:
Figura. Bloques Desconectar
Para el resto de los botones, control manual, control automático, alinear,… La configuración es
la misma, la aplicación envía un determinado carácter dependiendo del botón que se haya
pulsado.
Figura. Botones de Control, velocidad y dirección.
En la siguiente tabla se muestra cada botón con su respectivo carácter:
70
Botón Carácter
Control Manual m
Control Automático a
Desconectar o
Giro Izquierda i
Stop Giro s
Giro Derecha d
Alinear t
Rápido r
Lento l
Parado p
6.3.2 Implementación del programa arduino
El programa de arduino para el vehículo automatizado tiene que realizar varias operaciones:
recepción de órdenes vía Bluetooth, toma de datos del módulo arduimu, evaluación de datos,
comprobación condiciones de seguridad, determinación señal de control, envío de señales a los
motores,… Estas operaciones se muestran en el diagrama de flujo del programa, el cual se
explica a continuación.
71
Figura. Diagrama de flujo del programa principal
El programa principal está caracterizado básicamente por cuatro variables:
- MODO: indica si está realizando el control manual o automático (‘m’ o ‘a’).
- GIRO: indica la operación de giro del motor de dirección (‘i’, ‘s’, ‘d’ o ‘t’).
- VELOC: indica la velocidad que se le da al motor de tracción (‘p’, ‘l’ o ‘r’).
- TEMP: variable temporal en donde se guarda la lectura de lo que se recibe por Bluetooth.
Cuando se activa el vehículo de dos ruedas, se conecta a la batería, lo primero que hace es
inicializar las variables: MODO la pone en control manual, GIRO en ‘s’ (motor de dirección
parado) y VELOC le da el parámetro ‘p’ (motor de tracción parado). Esto es así para que el
72
vehículo cuando se encienda no haga nada, se mantenga quieto a la espera de recibir algún
carácter válido vía bluetooth.
En el momento en que recibe un carácter lo guarda en la variable temporal TEMP y evalúa dicho
carácter para saber si es una variable de control (MODO), de dirección (GIRO) o de velocidad
(VELOC). Para ello, lo primero que hace es comparar la variable TEMP preguntándose si TEMP
es igual a ‘m’ o ‘a’. En caso afirmativo, ya sabe que es una orden de MODO. Ahora se pregunta
si TEMP es igual al carácter ‘a’. Si es así, asigna a MODO el carácter ‘a’ y ejecuta el control
automático (ejecuta la función de control automático). En el caso de que TEMP no fuese igual al
carácter ‘a’, entonces es igual al carácter ‘m’. En este momento pueden pasar dos situaciones:
que esté en modo manual y se haya vuelto a pulsar el botón de modo manual (control manual), o
que esté en modo automático y se haya pulsado el modo manual. Para saber en cual de las dos
situaciones se encuentra, el programa realiza el bloque de decisión ¿MODO = TEMP? Si es
afirmativo, se encuentra en la primera situación, con lo que el programa no hace nada y se va a la
espera de recibir otra orden por bluetooth. Si es negativo, es que ha cambiado de automático a
manual, con lo que debe inicializarse, es decir, poner modo manual, motor de dirección y de
tracción parado y esperar a recibir la siguiente orden por bluetooth.
En el caso que la comparación ¿TEMP = ‘a’ o ‘m’? fuese negativa, se pregunta si TEMP es una
orden GIRO o VELOC (¿TEMP = ‘i’, ‘s’, ‘d’, ‘t’, ‘r’, ‘l’ o ‘p’?). Si es así, tiene que comprobar si
está en modo control manual ya que esas órdenes sólo se pueden ejecutar en ese modo. Si está en
modo manual entonces ejecuta el control manual. Si no está en modo manual se va a la espera de
la siguiente orden que le llegue.
En el caso de que la comprobación de si TEMP es una orden GIRO o VELOC fuese negativa,
comprueba si es el carácter ‘o’. El carácter ‘o’ significa que se ha pulsado en el programa de
android el botón desconectar, con lo que el vehículo debe detenerse. Se ha programado arduino
de tal forma que cuando eso pase, se reinicializa el sistema, es decir, se paran los motores y entra
en modo control manual.
Si TEMP tampoco fuese el carácter ‘o’, el programa entiende que no ha recibido una orden
correcta y se va a la espera de la siguiente lectura por bluetooth.
El programa arduino se encuentra en el apartado Anexos y se llama MANDOaMSaP.ino.
73
6.3.3 Implementación del control manual
El control manual se basa en una serie de órdenes que se le mandan al vehículo y éste realiza.
Este subprograma se ejecuta cuando el programa principal ha verificado que la lectura de
Bluetooth es una orden para el motor de dirección o tracción y que se encuentra en modo manual
(modo de control manual MODO = ‘m’). Su diagrama de flujo es el siguiente.
Figura. Diagrama de flujo del subprograma control manual.
Cuando se ejecuta este subprograma, arduino lo primero que hace es identificar si es una orden
para el motor de tracción o el motor de dirección. Para ello se hace primero la pregunta de si lo
que ha recibido por bluetooth es una orden para el motor de dirección, es decir, ¿TEMP = ‘i’, ‘s’,
‘d’ o ‘t’? Si la respuesta es afirmativa, guarda el carácter que contiene TEMP en la variable
74
GIRO. En caso contrario, es una orden para el motor de tracción lo guarda en la variable
VELOC.
Si es una orden para el motor de tracción, tiene que identificar cual de las tres opciones es. Para
ello, va comprobando y descartando opciones. Primero se pregunta si VELOC es igual al
carácter ‘r’ (velocidad rápida), en caso afirmativo le da la orden al motor de tracción que se
ponga a máxima velocidad. Si no es igual al carácter ‘r’ comprueba si es igual a ‘l’ (velocidad
lenta), en caso de que sea así le da la orden al motor de tracción para que se ponga a velocidad
media. En el caso de que tampoco sea el carácter ‘l’ sólo puede ser el carácter ‘p’ con lo que le
manda al motor de tracción la orden de pararse. Una vez le ha dado la orden al motor de tracción
termina el subprograma de control manual.
Si en una orden para el motor de dirección, el proceso de identificación de que carácter y, por
consiguiente, que orden ha recibido es igual. Primero comprueba si GIRO es igual al carácter ‘i’
(girar hacia la izquierda). Si es así, antes de dar la orden de que gire hacia la izquierda debe
comprobar si puede hacerlo ya que la horquilla no puede girar todo lo que quiera hacia la
izquierda por problemas de diseño (los cables conectados a la horquilla se soltarían y el
potenciómetro, que mide el giro de la horquilla, tiene un número de vueltas determinado hacia un
lado u otro). Por ello, por seguridad, se le ha dado un valor del potenciómetro límite de giro
hacia la izquierda. Por lo tanto, primero lee el valor del potenciómetro y comprueba si es menor
al valor límite izquierdo. Si lo es, le da al motor de dirección la orden de girar hacia la izquierda.
Si no lo es, entonces hace GIRO = ‘s’ y le da la orden de que se pare al motor.
Si GIRO no es igual al carácter ‘i’, comprueba si es igual al carácter ‘s’. Si es así le da la orden
al motor de dirección de que pare.
Si Giro no es ‘s’, comprueba si es igual a ‘d’. En este caso es todo igual al caso de girar a la
izquierda salvo que, obviamente, giraría hacia la derecha y que la condición giro límite es que no
sea menor que el valor de potenciómetro límite de giro hacia la derecha.
Si tampoco es igual a ‘d’, sólo queda la opción que sea la orden de alinear la horquilla, carácter
‘t’. Alinear la horquilla es una subfunción dentro del control manual. Su diagrama de flujo se
muestra a continuación.
75
Figura. Diagrama de flujo del subprograma alinear horquilla.
Antes de explicar el subprograma de alineación se van a definir los parámetros LIMDCHA,
LIMIZQDA, REFDCHA y REFIZQDA. LIMDCHA y LIMIZQDA son los valores de seguridad
para el potenciómetro. REFDCHA y REFIZQDA son dos valores que se toman a derecha e
izquierda respectivamente del valor de alineación. Debido a que el potenciómetro es muy
sensible y con cualquier vibración afecta a su valor, con lo que la alineación
76
El proceso de alineación es una función simple. Primero realiza una lectura del potenciómetro
del motor de dirección. Luego evalúa si está dentro de los márgenes de seguridad del
potenciómetro. Si no está dentro de los márgenes de seguridad, le da al motor de dirección la
orden de que pare de girar. Si está dentro de los márgenes de seguridad ejecuta la alineación en
relación a dos parámetros: REFDCHA y REFIZDA. Estos dos parámetros son dos valores del
potenciómetro, uno justo a la derecha de la alineación (REFDCHA) y otro a la izquierda
(REFIZDA). Se necesitan estos valores ya que es muy complicado obtener el valor exacto en el
que la rueda de dirección está alineada debido a la precisión del potenciómetro.
Figura. Márgenes de seguridad y alineación
Si la horquilla se encuentra dentro del margen de seguridad y a la derecha de REFDCHA
entonces se le da la orden al motor de dirección de que gire hacia la izquierda. A continuación
vuelve a medir el valor del potenciómetro y a repetir el programa hasta que haya alcanzado el
valor de REFDCHA con lo que se considera alineado y para el motor de dirección, le asigna a la
variable GIRO el carácter ‘s’ para que no vuelva a realizar el proceso de alineación y se sale del
subprograma al principal. Si la horquilla se encuentra dentro del margen de seguridad y a la
izquierda de REFIZQDA realiza el mismo proceso pero girando hacia la derecha hasta alcanzar
el valor de REFIZQDA.
77
6.3.4 Implementación del control automático
El control automático se basa en el control cuadrático óptimo que se ha calculado
posteriormente. Una vez el programa, que se encuentra en la placa Arduino Uno, recibe la orden
de realizarlo procede de la forma. Lo primero que hace es poner el motor de tracción a máxima
velocidad que, como se ha visto, es de 1 m/s. A continuación obtiene los valores de las variables
de estado necesarias para el cálculo de la ley de control. Lee de la unidad de medición inercial la
inclinación y la velocidad de inclinación. Con la lectura del potenciómetro se obtiene el ángulo al
que se encuentra girada la horquilla con respecto a la posición vertical. Falta obtener la velocidad
de giro de la horquilla cuyo valor, en el momento de ejecutar por primera vez el control
automático es cero. Una vez obtenidas las variables de estado se calcula la ley de control a partir
de la ecuación
( ) = − ( ) (10)
En este instante se tiene el par que hay que aplicarle a la horquilla a través del motor de
dirección. Por ello, a partir de la caracterización de dicho motor, se puede obtener el valor de la
velocidad de la horquilla que hay que aplicarle a partir de la ecuación
= ∙ + (67)
A continuación, se comprueba que el potenciómetro se encuentra dentro de los límites de
seguridad. Si no está dentro de los límites se para el motor de dirección, se para el motor de
avance y se para el control automático. Si está dentro de los límites comprueba si está en la
posición de equilibrio. Si no está en la posición de equilibrio comprueba si está inclinado hacia la
derecha o izquierda. Dependiendo hacia donde esté inclinado se sabe el sentido al que tiene que
girar el motor de dirección por lo que se calcula el valor de PWM que hay que aplicarle con la
ecuación
= ∙ + (68)
Todo este proceso se encuentra representado en el siguiente diagrama de flujo.
78
Figura. Diagrama de flujo control automático.
79
7 Conclusiones y trabajos futuros
El objetivo de este proyecto era el estudio, simulación y diseño de una ley de control para un
vehiculo automatizado de dos ruedas. Dicho vehículo consta de una unidad de medición inercial,
un potenciómetro, un motor para la dirección y otro para el avance, una tarjeta Arduino Uno y un
módulo de bluetooth.
El vehículo automatizado se diseñado en un programa de CAD para poder obtener los
parámetros físicos necesarios para introducirlos en el módelo y poder realizar la simulación. Con
dicho modelo, posteriormente se ha podido diseñar una ley de control que estabilice el vehículo
de dos ruedas en torno a una posición de equilibrio. Una vez obtenida la ley de control, se ha
diseñado un programa para instalarlo en la placa Arduino Uno del vehículo y se ha diseñado una
aplicación android para poder enviarle las distintas órdenes al vehículo. Dicha aplicación android
se conecta vía bluetooth con el módulo que está instalado en el vehículo de dos ruedas, la orden
recibida se la pasa a la placa Arduino Uno que, dependiendo la orden recibida, gobernará los
motores de tracción y dirección de una forma u otra. Al mandar la orden de que se active el
control automático, el programa instalado en Arduino Uno realiza la lectura del potenciómetro y
de la unidad de medición inercial y a partir de esos valores le manda la orden al motor de
dirección para que estabilice el vehículo.
Las pruebas realizadas mostraron buenos resultados a la hora de la estabilización aunque debido
a una avería en la unidad de medición inercial no ha permitido el ajuste completo de la
estabilización.
Con este proyecto he utilizado muchos de los conocimientos que he adquirido en la carrera como
son: dibujo técnico y representación gráfica por ordenador, electrónica, técnica de soldar
componentes, control automático, programación…
Un posible trabajo futuro puede ser cambiar el sistema de comunicación entre el dispositivo de
envío de órdenes y el vehículo autónomo de dos ruedas, pasar de una comunicación mediante
bluetooth a comunicación mediante wi-fi.
80
8 Referencias bibliográficas
[1] Proyecto Fin de Carrera “Diseño y construcción de un vehículo autónomo de dos ruedas” de
Honorio Romero Rodríguez.
[2] Libro “Ingeniería de control moderna” de Katsuhiko Ogata.
[3] Página web de Andy Ruina – Biorobotics and Locomotion Lab – Cornell University:
http://ruina.tam.cornell.edu/research/logolink.php
[4] Página web de Arend. L. Schwab – Laboratory for Engineering Mechanics – Delft University
of Technology:
http://bicycle.tudelft.nl/schwab/
[5] Página web de Murata Manufacturing Co. “Murata Boy”:
http://www.murata.com/corporate/boy_girl/boy/index.html
[6] Documento: “A multibody dynamics benchmark on the equations of motion of an
uncontrolled bicycle” de Schwab, Meijaard y Papadopoulos.
81
ANEXO
Experimentos
En este apartado se van a mostrar los resultados y las simulaciones del archivo de MatLab
LQR.m. En este archivo lo que se hace es modificar uno de los pesos de la matriz Q,
denominado prb. Para cada valor de prb el programa devuelve la matriz Q, el vector de
control K (en MatLab denominado Kc), una figura que muestra el esfuerzo de control y otras
figuras que muestran la evolución de las variables de estado del sistema, una con los ángulos
en radianes y otra con los ángulos en sexagesimales para una mayor facilidad de interpretar
los resultados.
Si prb = 1 se obtiene por pantalla:
82
Figura. Esfuerzo de control: Par de giro de la horquilla
Figura. Regulador de estados en dec.
83
Figura. Regulador de estados en rad.
Si prb = 10:
84
Figura. Esfuerzo de control: Par de giro de la horquilla
Figura. Regulador de estados en dec.
85
Figura. Regulador de estados en rad.
Si prb = 100:
86
Figura. Esfuerzo de control: Par de giro de la horquilla
Figura. Regulador de estados en dec.
87
Figura. Regulador de estados en rad.
Se puede apreciar en estos resultados como un aumento del valor de prb hace que el sistema
se estabilice más rápidamente pero la velocidad angular de la horquilla también aumenta.
88
Archivos Matlab
matricessinpc.m:
function [M, K0, K2, C1] = matricessinpc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%% Parámetros del vehículo automatizado de dos ruedas sin PC %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% g = 9.81; % Gravedad (m/s^2) %%%%% Bicicleta %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% w = 0.56741; % distancia entre ejes (wheel base) (m) c = 0.0582; % rastro (trail) (m) alpha = 68*pi/180; % inclinación de la horquilla 68º %%%%% Cuerpo = body = B %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% mB = 11.688; % masa cuerpo (kg) xB = 0.16964; % componente x del centro de masas (m) yB = 0; % componente y del centro de masas (m) zB = -0.32278; % componente z del centro de masas (m) IxxB = 12872560.95e-7; % momento de inercia en xx (kg/m^2) IyyB = 17693860.34e-7; % momento de inercia en yy (kg/m^2) IzzB = 5247300.08e-7; % momento de inercia en zz (kg/m^2) IxzB = -6633840.72e-7; % momento de inercia en xz (kg/m^2) %%%%% Rueda trasera = rear wheel = R %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% mR = 0.745; % masa rueda trasera (kg) rR = 0.144; % Radio (m) IxxR = 178731.66e-7; % momento de inercia en xx (kg/m^2) IyyR = 192705.96e-7; % momento de inercia en yy (kg/m^2) IzzR = 23370.20e-7; % momento de inercia en zz (kg/m^2) IxzR = 1241.49e-7; % momento de inercia en xz (kg/m^2) %%%%% Rueda delantera = front wheel = F %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% mF = 0.518; % masa rueda delantera (kg) rF = 0.144; % Radio (m) IxxF = 172259.93e-7; % momento de inercia en xx (kg/m^2) IyyF = 1844839.26e-7; % momento de inercia en yy (kg/m^2) IzzF = 1676751.11e-7; % momento de inercia en zz (kg/m^2) IxzF = -501900.08e-7; % momento de inercia en xz (kg/m^2) %%%%% Horquilla = front trame = H %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% mH = 0.681; % masa horquilla (kg) xH = 0.50256; % componente x del centro de masas (m) yH = 0; % componente y del centro de masas (m)
89
zH = -0.30711; % componente z del centro de masas (m) IxxH = 709469.48e-7; % momento de inercia en xx (kg/m^2) IyyH = 2433520.60e-7; % momento de inercia en yy (kg/m^2) IzzH = 1742437.01e-7; % momento de inercia en zz (kg/m^2) IxzH = -1025917.89e-7; % momento de inercia en xz (kg/m^2) %%%%% Vehiculo total = T %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% mT = mR + mB + mH + mF; xT = (xB*mB + xH*mH +w*mF)/mT; zT = (-rR*mR + zB*mB + zH*mH - rF*mF)/mT; IxxT = IxxR + IxxB + IxxH + IxxF + mR*rR^2 + mB*zB^2 + mH*zH^2 + mF*rF^2; IxzT = IxzB + IxzH - mB*xB*zB - mH*xH*zH + mF*w*rF; IzzT = IzzR + IzzB + IzzH + IzzF + mB*xB^2 + mH*xH^2 + mF*w^2; %%%%% Conjunto H+F = A %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% mA = mH + mF; xA = (xH*mH + w*mF)/mA; zA = (zH*mH - rF*mF)/mA; IxxA = IxxH + IxxF + mH*(zH - zA)^2 + mF*(rF + zA)^2; IxzA = IxzH - mH*(xH - xA)*(zH - zA) + mF*(w - xA)*(rF + zA); IzzA = IzzH + IzzF + mH*(xH - xA)^2 + mF*(w - xA)^2; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%% Obtención de las Matrices de la Ec de movimiento %%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% lambda = 22*pi/180; % Ángulo entre la horquilla y el eje vertical 22º uA = (xA - w - c)*cos(lambda) - zA*sin(lambda); % Distancia perpendic % del CM de A con el % eje de rotaciön IllA =mA*uA^2+IxxA*(sin(lambda))^2+2*IxzA*(sin(lambda))*(cos(lambda))+
IzzA*(cos(lambda))^2; IlxA = -mA*uA*zA + IxxA*(sin(lambda)) + IxzA*(cos(lambda)); IlzA = mA*uA*xA + IxzA*(sin(lambda)) + IzzA*(cos(lambda)); nu = (c/w)*(cos(lambda)); SR = IyyR/rR; % Coeficiente girostático de la rueda trasera SF = IyyF/rF; % Coeficiente girostático de la rueda delanter ST = SR + SF; % Suma de los coef. girostáticos SA = mA*uA + nu*mT*xT; % Momento estático %%%%% Matriz de Masas %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% M(1,1) = IxxT; M(1,2) = IlxA + nu*IxzT; M(2,1) = M(1,2); M(2,2) = IllA + 2*nu*IlzA + (nu^2)*IzzT; %%%%% Matriz de Rigidez %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
90
K0(1,1) = mT*zT; K0(1,2) = -SA; K0(2,1) = K0(1,2); K0(2,2) = -SA*sin(lambda); K2(1,1) = 0; K2(1,2) = ((ST - mT*zT)/w)*cos(lambda); K2(2,1) = 0; K2(2,2) = ((SA + SF*sin(lambda))/w)*cos(lambda); %%%%% Matriz de Amortiguación %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% C1(1,1) = 0; C1(1,2) = nu*ST + SF*cos(lambda) + (IxzT/w)*cos(lambda) - nu*mT*zT; C1(2,1) = -(nu*ST + SF*cos(lambda)); C1(2,2) = (IlzA/w)*cos(lambda)+ nu*(SA + (IzzT/w)*cos(lambda));
Parametrossinpcv1.m:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%% Parámetros del vehículo automatizado de dos ruedas sin PC %%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc clear all syms v [M, K0, K2, C1] = matricessinpc; g = 9.81; % Gravedad (m/s^2) v = 1; % m/s %%%%% Matriz de Masas %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% disp('La Matriz de Masas es:') M %%%%% Matriz de Rigidez %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% K = K0.*g + K2.*v^2; fprintf('La Matriz de Rigidez para una v = %g m/s es:',v) K %%%%% Matriz de Amortiguación %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% C = C1.*v;
91
fprintf('La Matriz de Amortiguación para una v = %g m/s es:',v) C disp(' ') disp(' ') %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%% Obtención de las Matrices para modelo en Espacio de Estados %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% disp('-----------------------------------------------------------------') disp('Los parámetros en Espacio de Estados del vehículo de 2 ruedas, sin') disp('PC incorporado, son:') disp('------------------------------------------------------------------') disp(' ') disp(' ') disp('dx/dt = Ax + Bu') disp(' y = Cx + Du') disp(' ') Mo = inv(M); fprintf('La Matriz A del modelo en Esp. Estados con v = %g m/s es:',v) K = Mo*(K0.*g + K2.*v^2); S = Mo*(C1.*v); A = -[0, -1, 0, 0; K(1,1), S(1,1), K(1,2), S(1,2); 0, 0, 0, -1; K(2,1), S(2,1), K(2,2), S(2,2)] B = [0 0 0 0; 0 Mo(1,1) 0 Mo(1,2); 0 0 0 0; 0 Mo(2,1) 0 Mo(2,2)]; fprintf('La Matriz B del modelo en Esp. Estados para cualquier v es:') B fprintf('La Matriz C del modelo en Esp. Estados para cualquier v es:') C = [1 0 0 0] fprintf('La Matriz D del modelo en Esp. Estados para cualquier v es:') D = [0 0 0 0]
Estabilidadsinpc.m:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%% Estabilidad Vehiculo 2 ruedas %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc
92
clear all syms y % Autovalores syms v % Velocidad lineal bicicleta [M, K0, K2, C1] = matricessinpc; g = 9.81; T = M*(y^2) + v*C1*y + g*K0 + (v^2)*K2; for v=0:0.05:10; Y=det(T); G=eval(Y); polos=solve(G); t=[v v v v]; grid on plot(t,real(polos),'.b',t,abs(imag(polos)),'.g'); xlabel('velocidad m/s'); ylabel('autovalores'); title('Gráfica Autovalores - Velocidad'); legend('Parte real','Parte imaginaria','Location','NorthWest'); hold on % Parte real azul, imaginaria verde end % con leyenda a la derecha
CaractSistsinpcv1.m:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%% Caracterización del sistema sin PC incorporado con v = 1 %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc clear all [M, K0, K2, C1] = matricessinpc; g = 9.81; Mo = inv(M); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%% Funciones de Transferencia y Espacio de Estados %%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% v = 1; K = Mo*(K0.*g + K2.*v^2); S = Mo*(C1.*v); A = -[0, -1, 0, 0; K(1,1), S(1,1), K(1,2), S(1,2); 0, 0, 0, -1; K(2,1), S(2,1), K(2,2), S(2,2)];
93
B = [0; Mo(1,2); 0; Mo(2,2)]; C = [1, 0, 0, 0]; D = [0]; fprintf('La Función de Transferencia u1 con v = %g m/s es:',v) disp(' ') disp(' ') [num, den] = ss2tf(A,B,C,D); sys = ss(A,B,C,D); ftba = tf(sys) % F.T. de orden 4 disp('El modelo en Espacios de Estados es:') sys disp('---------------------------------------------------------------') figure(1) stepplot(sys) grid on title('Respuesta en B.A. ante entrada Escalón') axis([0 2 -1.5708 1.5708]) ftbc = feedback(sys,1); figure(2) stepplot(ftbc) grid on title('Respuesta en B.C. ante entrada Escalón') axis([0 2 -1.5708 1.5708]) fprintf('Los polos y ceros de la F.d.T. en BC con v = %g m/s es:',v) ftbc = feedback(ftba,1); [p, z] = pzmap(ftbc) [z1, p1, k] = tf2zp(num,den) disp('---------------------------------------------------------------') [C_PID, info] = pidtune(sys,'pid')
ContrObssinpc.m:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%% Controlabilidad y observabilidad Vehiculo sin PC v 0 1 m/s %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc clear all [M, K0, K2, C1] = matricessinpc; g = 9.81; Mo = inv(M); for v = 0:0.25:1
94
% Calculo Funcion de transferencia fprintf('Controlabilidad y Observabilidad del Sistema con v = %g m/s:\n',v) disp('---------------------------------------------------------------') disp(' ') K = Mo*(K0.*g + K2.*v^2); S = Mo*(C1.*v); A = -[0, -1, 0, 0; K(1,1), S(1,1), K(1,2), S(1,2); 0, 0, 0, -1; K(2,1), S(2,1), K(2,2), S(2,2)]; B = [0; Mo(1,2); 0; Mo(2,2)]; B1 = [0 0 0 0; 0 Mo(1,1) 0 Mo(1,2); 0 0 0 0; 0 Mo(2,1) 0 Mo(2,2)]; C = [1, 0, 0, 0]; D = [0]; D1 = [0, 0, 0, 0]; [num, den] = ss2tf(A,B,C,D,1); sys1 = ss(A,B,C,D); ftba = tf(sys1); % Controlabilidad completa del estado del sistema en tiempo continuo MContc = ctrb(A,B) % MCont = ctrb(A,B2) % MCont = [B2, A*B2, A*A*B2, A*A*A*B2]; rmcc = rank(MContc); disp(sprintf('El rango de la matriz de controlabilidad es: %d', rmcc)) if rmcc == 4 disp('El sistema presenta controlabilidad completa') else disp('El sistema no presenta controlabilidad completa') end % Controlabilidad a la salida MContSalc = [C*B, C*A*B, C*A*A*B, C*A*A*A*B, D]; %MContSal = [C*B2, C*A*B2, C*A*A*B2, C*A*A*A*B2, D]; rmcsc = rank(MContSalc); disp(sprintf('El rango de la matriz de controlabilidad a la salida es: %d', rmcc)) if rmcc == 4 disp('El sistema presenta controlabilidad completa a la salida') else disp('El sistema no presenta controlabilidad completa a la salida') end % Matriz de Observabilidad % Observabilidad completa del estado del sistema en tiempo continuo MObsc = obsv(A,C) % MObs = [C', A'*C', A'^2*C', A'^3*C']; rmoc = rank(MObsc); disp(sprintf('El rango de la matriz de observabilidad es: %d', rmoc)) if rmcc == 4 disp('El sistema presenta observabilidad completa') else disp('El sistema no presenta observabilidad completa') end disp(' ') disp('###############################################################') disp(' ') end
95
LQR.m:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%% Controlabilidad y observabilidad Vehiculo sin PC v 0 1 m/s %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc clear all [M, K0, K2, C1] = matricessinpc; g = 9.81; Mo = inv(M); v = 1; K = Mo*(K0.*g + K2.*v^2); S = Mo*(C1.*v); A = -[0, -1, 0, 0; K(1,1), S(1,1), K(1,2), S(1,2); 0, 0, 0, -1; K(2,1), S(2,1), K(2,2), S(2,2)]; B = [0; Mo(1,2); 0; Mo(2,2)]; C = [1, 0, 0, 0]; D = [0]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%% Espacio de estados Continuo %%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% sysc = ss(A, B, C, D); % Espacio estados continuo %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%% Control optimo cuadratico %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% i = 1; t = linspace(0,15); x0 = [0.1 0 0 0]; [y,x,t] = initial(A,B,C,D,x0,t); h = figure(i); subplot(221),plot(t,x(:,1)),title('x_1: Posición angular inclinación (rad)'), grid on, axis([0 1 -1.5708 1.5708]) subplot(222),plot(t,x(:,2)),title('x_2: Velocidad angular inclinación (rad/s)'), grid on, axis([0 1 -1.5708 10]) subplot(223),plot(t,x(:,3)),title('x_3: Posición angular horquilla (rad)'), grid on, axis([0 1 -1.5708 1.5708]) subplot(224),plot(t,x(:,4)),title('x_4: Velocidad angular horquilla(rad/s)'), grid on, axis([0 1 -1.5708 10]) set(h,'name','Planta sin control','numbertitle','off')
96
for prb=0:1:2 prb = 10^prb; disp('La matriz Q es:'); Q = [prb 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0] R = [1]; disp('EL vector Kc del control LQR es:') kc = lqr(A,B,Q,R) disp('Kc con datos en dec es:') kc_dec = kc*pi/180 Ac = [(A - B*kc)]; [y,x,t] = initial(Ac,B,C,D,x0,t); u = kc*x'; h = figure(i+1); plot(t,u) hold on grid on set(h,'name','Esfuerzo de control: Par de giro de la horquilla','numbertitle','off') h = figure(i+2); subplot(221),plot(t,x(:,1)), title('x_1: Posición angular inclinación (rad)'), hold on, grid on subplot(222),plot(t,x(:,2)), title('x_2: Velocidad angular inclinación (rad/s)'), hold on, grid on subplot(223),plot(t,x(:,3)), title('x_3: Posición angular horquilla (rad)'), hold on, grid on subplot(224),plot(t,x(:,4)), title('x_4: Velocidad angular horquilla (rad/s)'), hold on, grid on set(h,'name','Regulador de estados en rad','numbertitle','off') h = figure(i+3); subplot(221),plot(t,x(:,1)*180/pi), title('x_1: Posición angular inclinación (º)'), hold on, grid on subplot(222),plot(t,x(:,2)), title('x_2: Velocidad angular inclinación (rad/s)'), hold on, grid on subplot(223),plot(t,x(:,3)*180/pi), title('x_3: Posición angular horquilla (º)'), hold on, grid on subplot(224),plot(t,x(:,4)), title('x_4: Velocidad angular horquilla (rad/s)'), hold on, grid on set(h,'name','Regulador de estados en dec','numbertitle','off') i = i + 4; disp('###############################################################') disp(' ') end
97
Archivos Arduino
CONFIGURACIONBT.ino
#include <SoftwareSerial.h>
#define RxD 10
#define TxD 11
#define KEY 4
SoftwareSerial BTSerial(RxD, TxD);
void setup()
{
pinMode(KEY, OUTPUT);
digitalWrite(KEY, HIGH);
delay(500);
BTSerial.flush();
delay(500);
BTSerial.begin(38400);
Serial.begin(9600);
Serial.println("Enter AT commands:");
BTSerial.print("AT\r\n");
delay(100);
}
void loop()
{
98
if (BTSerial.available())
Serial.write(BTSerial.read());
if (Serial.available())
BTSerial.write(Serial.read());
}
MANDOaMSaP.ino
//**************************************************************************
//*********************************Mando Paniuska 1.0 ************************
// *************************************************************************
#include <SoftwareSerial.h>
SoftwareSerial BT(10,11); // Rx 10, Tx 11
#include <Servo.h> // transformacion de señal digital a analogica
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define INTERNAL INTERNAL1V1
#endif
//**************************************************************************
#include <Wire.h> // libreria del puerto i2c
char EBT; // Entrada por bluetooth
char ModoControl; // Modo control dado por mando
char AcGiro; // Accion giro dada por mando
char AcVeloc; // Accion velocidad dada por mando
99
byte angulo[] = {0,0,0,0}; // variable a almacenar el angulo en dos partes, entera y decimal
int inclinacion = 0; // angulo inclinacion º
int velinclin = 0; // velocidad angular inclinacion º/s
int torque = 0; // par
int anghorq = 0; // angulo horquilla º
int velhorq = 0; // velocidad horquilla rad/s
int inclinfinal = 0;
int valorpoten = 0;
int PWMd = 0; // % que hay que darle al PWM motor horquilla drcha
int PWMi = 0; // % que hay que darle al PWM motor horquilla izqda
int Poten; // valor del potenciometro
int PotenRecIz = 553; // valor del potenciometro recto 538
int PotenRecDr = 528; // valor del potenciometro recto 538
int PinIzq = 6; // Pin 6 giro hacia la izquierda
int PinDrch = 3; // Pin 3 giro hacia la derecha
int PinAvance = 5; // Pin 5 motor traccion
int PinLed = 13; // Pin 13 Led Arduino
int pt = 0; // Puntero indicador alineacion
int pma = 0; // Puntero indicador manual o automatico
int pa = 0; // Puntero indicador alineacion
int pg = 0; // Puntero indicador giro horquilla
int pv = 0; // Puntero indicador velocidad
int pm = 0; // Puntero indicador si se pulsa manual o esta en manual
// Si pm = 0 cambia a manual, pm = 1 esta en manual
int pal = 0; // Puntero indicador inicio modo automatico
Servo futaba ; // variable para nombrar al servo
// mi Arduimu slave sera el 4
void setup()
{
100
BT.begin(9600); // Inicia la comunicacion con Bluetooth
BT.flush();
delay(500);
Serial.begin(9600);
Wire.begin(); // Inicia la comunicacion i2c con los esclavos
// Arduimu como esclavo 4
pinMode(PinAvance, OUTPUT);
pinMode(PinIzq, OUTPUT);
pinMode(PinDrch, OUTPUT);
pinMode(PinLed, OUTPUT); // El pin 13 será una salida digital
futaba.attach(2); // inicio el servo en el pin 2
// ***** Condiciones iniciales. Motores parados *****
analogWrite(PinIzq,0);
analogWrite(PinDrch,0);
analogWrite(PinAvance,0);
valorpoten = analogRead(0);
ModoControl = 'm';
AcGiro = 's';
AcVeloc = 'p';
}
void loop()
{
// ***** Lectura de entrada desde Bluetooth *****
if(BT.available())
{
EBT = BT.read();
101
}
// ***** Clasificacion *****
if(EBT == 'm' || EBT == 'a') // if clasifica si es manual o automatico
{ // y los else si son acciones de giro o
ModoControl = EBT; // de velocidad
}
else if(EBT == 't' || EBT == 'i' || EBT == 's' || EBT == 'd')
{
AcGiro = EBT;
}
else if(EBT == 'r' || EBT == 'l' || EBT == 'p')
{
AcVeloc = EBT;
}
// ***** Control manual *****
if(ModoControl == 'm') // Se pulsa manual
{
pma = 0;
if(pm == 0) // Si se acaba de pulsar manual
{
pm = 1;
AcVeloc = 'p';
pv = 0;
AcGiro = 's';
pg = 0;
pal = 0; // pal = 0 para que cuando cambie a automatico
// ponga velocidad rapida
analogWrite(PinIzq,0); // Para motor a la izquierda
analogWrite(PinDrch,0); // Para motor a la derecha
analogWrite(PinAvance,0); // Para motor avance
102
}
if(pm == 1) // Si manual ya pulsado
{
if(AcGiro == 't') // Pulsado boton alinear
{
pg = 3;
Poten = analogRead(0);
if (Poten < PotenRecIz && Poten > PotenRecDr) // En rango recto
{
pt = 0;
pg = 0;
analogWrite(PinIzq,0);
analogWrite(PinDrch,0);
AcGiro = 's';
}
else if(Poten > PotenRecIz) // Girado hacia izquierda
{
pt = 1;
analogWrite(PinIzq,0);
analogWrite(PinDrch,90);
}
else if(Poten < PotenRecDr) // Girado hacia derecha
{
pt = 2;
analogWrite(PinDrch,0);
analogWrite(PinIzq,80);
}
}
if(AcGiro == 'i') // Pulsado boton giro izquierda
{
if(valorpoten > 950) // Seguridad potenciometro
{
analogWrite(PinIzq,0);
AcGiro = 's';
103
pg = 0;
}
else
{
pg = 1;
analogWrite(PinDrch,0);
analogWrite(PinIzq,90);
valorpoten = analogRead(0);
}
}
if(AcGiro == 's') // Pulsado boton stop giro
{
pg = 0;
analogWrite(PinIzq,0);
analogWrite(PinDrch,0);
}
if(AcGiro == 'd') // Pulsado boton giro derecha
{
if(valorpoten < 130) // Seguridad potenciometro
{
analogWrite(PinDrch,0);
AcGiro = 's';
pg = 0;
}
else
{
pg = 2;
analogWrite(PinIzq,0);
analogWrite(PinDrch,100);
valorpoten = analogRead(0);
}
}
if(AcVeloc == 'r') // Pulsado boton Avance rapido
{
pv = 2;
104
analogWrite(PinAvance,255);
}
if(AcVeloc == 'l') // Pulsado boton Avance lento
{
pv = 1;
analogWrite(PinAvance,180);
}
if(AcVeloc == 'p') // Pulsado boton avance parado
{
pv = 0;
analogWrite(PinAvance,0);
}
}
}
// ***** Control Automatico *****
if(ModoControl == 'a')
{
if(pal == 0)
{
pal = 1;
pma = 1;
pv = 2; // Velocidad rapida
analogWrite(PinAvance,255);
delay(1000);
pm = 0; // pm = 0 para que cuando cambie a manual se pare
}
if(pal == 1)
{
lecturaImu();
valorpoten = analogRead(0);
if(velinclin > 100)
{
105
velinclin = velinclin-255;
}
else
{
velinclin = velinclin;
}
inclinacion = inclinacion - 90;
anghorq = (analogRead(0)-538)/4;
torque = 18.8*inclinacion + 9*velinclin + anghorq; // - 0.43*velhorq;
velhorq = ((torque - 1.1)/0.1) + 11; // +11 para ajustar el equilibrio
if(anghorq < 90 && anghorq > -90) // Dentro limites seguridad potenciometro
{
if(inclinacion < 0) // Inclinado a la derecha. Girar a la derecha
{
pg = 2;
pa = 2;
velhorq = velhorq*(-1);
PWMd = 18.2573*velhorq*0.40 + 17.6617; //*0.01 coef ajuste
if (PWMd > 100)
{
PWMd = 100;
}
if (PWMd < 0)
{
PWMd = 0;
}
analogWrite(PinIzq,0);
analogWrite(PinDrch,PWMd);
}
if (inclinacion > 0) // Inclinado a la izquierda. Girar a la izquierda
{
pg = 1;
106
pa = 1;
PWMi = 18.0102*velhorq*0.40 + 21.4348; //*0.01 coef ajuste
if (PWMi > 100)
{
PWMi = 100;
}
if (PWMi < 0)
{
PWMi = 0;
}
analogWrite(PinDrch,0);
analogWrite(PinIzq,PWMi);
}
if (inclinacion == 0) // Equilibrio
{
pg = 0;
pa = 0;
PWMd = 0;
PWMi = 0;
analogWrite(PinDrch,PWMd);
analogWrite(PinIzq,PWMi);
}
}
if (valorpoten > 950 || valorpoten < 130) // Fuera limites seguridad potenciometro
{
pv = 0;
pg = 0;
pa = 3;
PWMd = 0;
PWMi = 0;
analogWrite(PinDrch,PWMd);
analogWrite(PinIzq,PWMi);
analogWrite(PinAvance, 0);
107
}
}
}
Serial.write(ModoControl);
Serial.print(" ");
Serial.write(AcVeloc);
Serial.print(" ");
Serial.write(AcGiro);
Serial.print(" ");
if(pma == 1)
{
Serial.print("Modo Automatico");
Serial.print(" ");
if(pa == 0)
{
Serial.println("Sistema en equilibrio");
}
if(pa == 1)
{
Serial.print("Inclinado Izqda. Giro Horquilla a Izquierda. ");
Serial.print("Vel horq:" );
Serial.print(velhorq);
Serial.print(" ");
Serial.print("PWMi:" );
Serial.println(PWMi);
}
if(pa == 2)
{
Serial.print("Inclinado Drcha. Giro Horquilla a Derecha. ");
Serial.print("Vel horq:" );
Serial.print(velhorq);
Serial.print(" ");
108
Serial.print("PWMd:" );
Serial.println(PWMd);
}
if(pa == 3)
{
Serial.println("Fuera limites potenciometro. No controlable");
}
}
if(pma == 0)
{
Serial.print("Modo Manual ");
Serial.print(" ");
if(pv == 0)
{
Serial.print("Parado");
Serial.print(" ");
}
if(pv == 1)
{
Serial.print("Lento ");
Serial.print(" ");
}
if(pv == 2)
{
Serial.print("Rapido");
Serial.print(" ");
}
if(pg == 3)
{
Serial.print("Alinear");
Serial.print(" ");
if(pt == 0)
{
Serial.println("alineado");
}
109
if(pt == 1)
{
Serial.println("Horquilla Girada Izquierda");
}
if(pt == 2)
{
Serial.println("Horquilla Girada Derecha");
}
if(pt == 3)
{
Serial.println("Fuera de Limites del Potenciometro");
}
}
if(pg == 1)
{
Serial.println("Izquierda");
}
if(pg == 0)
{
Serial.println("Stop Giro");
}
if(pg == 2)
{
Serial.println("Derecha");
}
}
}
// ***** Funcion lectura de Imu *****
int lecturaImu()
{
// Recepcion de paquete de 2 bytes conteniendo parte entera y decimal del angulo y veloc
Wire.beginTransmission(4); //inicia la transmision de Arduimu
110
Wire.requestFrom(4, 4); // requiere 2 bytes de Arduimu
int j = 0;
while(Wire.available()) //Arduimu puede enviar
{
angulo[j] = Wire.read(); // recibe un byte
j++;
}
Wire.endTransmission(4); // finaliza la transmision
inclinacion = angulo[0];
velinclin = angulo[2];
}
111
Datasheet
112
113
114
115
116