UNIVERSIDAD NACIONAL DE PIURA

FACULTAD DE INGENIERA INDUSTRIAL

ESPEC. INGENIERA MECATRNICA

Curso de Ingeniera de Control II

Proyecto Final

Robot Balancn

Docente: Ing. Csar Arturo Nio Carmona. MsC

Integrantes:

Calle Jimnez Luis Rodolfo

De La Cruz Rodrguez Stuwar
Nez Navarro Cesar OBrayan Nicolas
Ortiz Ortiz Emanuel
Olaya Dvila, Cristian Pawer
Viera Castillo Ernesto David

Piura, 20 de febrero del 2016

 ndice
1 Resumen: .................................................................................................... 5

2 Procedimiento y anlisis de resultados ....................................................... 5

2.1 Objetivo: ................................................................................................ 5

2.2 Marco terico: ....................................................................................... 5

2.2.1 Descripcin del pndulo invertido ................................................... 5

2.2.2 PWM ............................................................................................... 6

2.2.3 Arduino Uno R3 .............................................................................. 7

2.2.4 Driver Puente H L298N ................................................................... 9

2.2.5 MPU-6050 ...................................................................................... 9

2.2.6 Control PID ................................................................................... 10

2.3 Equipo utilizado: .................................................................................. 12

2.4 Procedimiento: .................................................................................... 13

2.4.1 Recopilacin de Informacin ........................................................ 13

2.4.2 Simulacin .................................................................................... 14

2.4.3 Construccin del Robot Balancn.................................................. 14

2.4.4 Programacin ............................................................................... 16

2.4.5 Valores de Kp , Ki y Kd ................................................................. 22

2.5 Resultados experimentales: ................................................................ 23

2.6 Anlisis de resultados: ........................................................................ 23

3 Conclusiones............................................................................................. 24

4 Bibliografa ................................................................................................ 25
TABLA DE FIGURAS

Figura 1: Esquema del pndulo invertido ..................................................................................................... 6

Figura 2: Arduino Uno R3 ............................................................................................................................. 8

Figura 3: Driver Puente H L298N .................................................................................................................. 9

Figura 4:Mdulo Acelermetro MPU.......................................................................................................... 10

Figura 5: Diagrama de un PID ................................................................................................................... 11

1 Resumen:

El trabajo consiste en el desarrollo e implementacin de un pequeo robot

basado en Arduino, capaz de auto-balancearse, y de este modo mantener el

equilibrio sobre sus dos ruedas. Esto se consigue empleando un sensor que

nos informa en tiempo real sobre el ngulo que forma el robot con respecto al

suelo. Se usa un filtro en los valores obtenidos del sensor. El ngulo obtenido

por el sensor es gestionado por un microcontrolador (Arduino), el cual,

mediante un algoritmo de control obtiene la seal de control que se transmite a

los motores de las ruedas para su accionamiento.

PALABRAS CLAVE

Arduino, auto-balanceado, algoritmo de control PID, giroscopio, acelermetro.

2 Procedimiento y anlisis de resultados

2.1 Objetivo:

Realizar un robot balancn utilizando el principio bsico de un pndulo

invertido y de un control PID.

2.2 Marco terico:

2.2.1 Descripcin del pndulo invertido

El pndulo invertido es un servo mecanismo que consta de un riel sobre el cual

se puede deslizar un carro, sobre este montado un pndulo que puede girar

libremente.

El sistema est instrumentado, de tal suerte que se puede medir el ngulo del

pndulo con respecto a la vertical, as como la posicin y la velocidad del carro.

A travs de un motor y una banda conectada al carro, se puede hacer que este

se deslice sobre el riel (Ver Figura 1)

Figura 1: Esquema del pndulo invertido

Si se considera al pndulo separado del carro, el pndulo tiene dos puntos de

equilibrio: uno estable, abajo; y otro inestable, arriba.

El objetivo del control es cambiar la dinmica del sistema para que en la

posicin vertical, arriba, se tenga un punto de equilibrio estable.

En otras palabras, la idea es encontrar la fuerza que ha de aplicarse al carro

para que el pndulo no se caiga, incluso si se le perturba con un empujn tipo

escaln o impulso.

(Castaos Luna, 2003)

2.2.2 PWM

La modulacin de ancho de pulso (PWM, por sus siglas en ingls) de una seal

es una tcnica que logra producir el efecto de una seal analgica sobre una

carga, a partir de la variacin de la frecuencia y ciclo de trabajo de una seal

digital.
El ciclo de trabajo describe la cantidad de tiempo que la seal est en un

estado lgico alto, como un porcentaje del tiempo total que est, toma para

completar un ciclo completo.

La frecuencia determina que tan rpido se completa un ciclo (por ejemplo: 1000

Hz corresponde a 1000 ciclos en un segundo), y por consiguiente que tan

rpido se cambia entre los estados lgicos alto y bajo. Al cambiar una seal del

estado alto a bajo a una tasa lo suficientemente rpida y con un cierto ciclo de

trabajo, la salida parecer comportarse como una seal analgica constante

cuanto esta est siendo aplicada a algn dispositivo.

Seales de PWM son utilizadas comnmente en el control de aplicaciones. Su

uso principal es el control de motores de corriente continua, aunque tambin

pueden ser utilizadas para controlar vlvulas, bombas, sistemas hidrulicos, y

algunos otros dispositivos mecnicos.

La frecuencia a la cual la seal de PWM se generar, depender de la

aplicacin y del tiempo de respuesta del sistema que est siendo controlado.

(National Instruments, 2014)

2.2.3 Arduino Uno R3

El Arduino es una plataforma computacional fsica open-source basada en una

simple tarjeta de I/O y un entorno de desarrollo que implementa el lenguaje

Processing/Wiring. El Arduino Uno R3 puede ser utilizado para desarrollar

objetos interactivos o puede ser conectado a software de tu computadora (por

ejemplo, Flash, Processing, MaxMSP). El IDE open-source puede ser

descargado gratuitamente (actualmente para Mac OS X, Windows y Linux).

(arduino, s.f.)

Figura 2: Arduino Uno R3

2.2.3.1 Caractersticas

Microcontrolador ATmega328.

Voltaje de entrada 7-12V.

14 pines digitales de I/O (6 salidas PWM).

6 entradas anlogas.

32k de memoria Flash

Reloj de 16MHz de velocidad.

2.2.4 Driver Puente H L298N

El driver L298N es un puente H Dual, permite controlar el giro y la velocidad de

2 motores DC de hasta 2A o un Motor paso a paso

El mdulo permite controlar el sentido de giro y velocidad mediante seales

TTL que se pueden obtener de microcontroladores y tarjetas de desarrollo

como Arduino, Raspberry Pi o Launchpads de Texas Instruments.

Tiene integrado un regulador de voltaje de 5V encargado de alimentar la parte

lgica del L298N, el uso de este regulador se hace a travs de un Jumper y se

puede usar para alimentar la etapa de control.

(NAYLAMP , s.f.)

Figura 3: Driver Puente H L298N

2.2.5 MPU-6050

EL MPU6050 es una unidad de medicin inercial o IMU (Inertial Measurment

Units) de 6 grados de libertad (DoF) pues combina un acelermetro de 3 ejes y

un giroscopio de 3 ejes. Este sensor es muy utilizado en navegacin,

goniometra, estabilizacin, etc. EL mdulo Acelermetro MPU tiene un

giroscopio de tres ejes con el que podemos medir velocidad angular y un

acelermetro tambin de 3 ejes con el que medimos los componentes X, Y y Z

de la aceleracin. La direccin de los ejes est indicado en el mdulo el cual

hay que tener en cuenta para no equivocarnos en el signo de las

aceleraciones.La comunicacin del mdulo es por I2C, esto le

permite trabajar con la mayora de microcontroladores. Los pines SCL y SDA

tienen una resistencia pull-up en placa para una conexin directa al

Microcontrolador o Arduino. (NAYLAMP, s.f.)

Figura 4: Mdulo Acelermetro MPU

2.2.6 Control PID

En este trabajo, veremos los parmetros bsicos a tener en cuenta sobre el

control proporcional, integral, derivativo (PID); el objetivo de este proyecto no

es introducirnos en los anlisis tericos del PID, sino ver su aplicacin en un

sistema real utilizando un Arduino uno r3 y extensiones de este mismo.

2.2.6.1 La Ecuacin del PID

Para tener una idea ms clara, recurrimos al siguiente diagrama

Figura 5: Diagrama de un PID

De la ecuacin, podemos hacer las siguientes afirmaciones:

e(t) es el error de la seal.

u(t) salida del controlador y entrada de control al proceso.

Kp es la ganancia proporcional.

Ti es la constante de tiempo integral

Td es la constante de tiempo derivativa.

Del diagrama de flujo determinamos lo siguiente:

El primer bloque de control (proporcional) consiste en el producto entre

la seal de error y la constante proporcional, quedando un error en

estado estacionario casi nulo.

El segundo bloque de control (integral) tiene como propsito disminuir y

eliminar el error en estado estacionario, provocado por el modo

proporcional. El control integral acta cuando hay una desviacin entre

la variable y el punto de consigna, integrando esta desviacin en el

tiempo y sumndola a la accin proporcional.

El tercer bloque de control (Derivativo) considera la tendencia del error y

permite una repercusin rpida de la variable despus de presentarse

una perturbacin en el proceso.

(Moyano)

2.3 Equipo utilizado:

La mecnica de un robot de este tipo requiere de los siguientes componentes

fsicos:

1 Chasis, (de madera)

Motores dc con reduccin (6v)

2 Llantas para motor DC

1 Arduino UNO

1 Giroscopio/acelermetro (MPU6050)

1 Controlador de motores (L298n)

8 Pilas AA que sirva de fuente de alimentacin para el sistema

1Lcd 2x16
 1Protoboard

Cables de Conexin

Software:

IDE Arduino

ISIS PROTEUS

2.4 Procedimiento:

El siguiente trabajo basado en un controlador PID se realiz de la siguiente

manera.

2.4.1 Recopilacin de Informacin

El primer pas e indispensable es la indagacin de informacin la cual ser

base para poder realizar un proyecto de esta envergadura. Teniendo como

consideracin los temas de pndulo invertido, control PID digital.

Para entender, comprender y analizar temas que involucran a este proyecto se

debe adquirir conocimientos previos de arduino, ingeniera de control,

manipulacin de dispositivos que permitan la comunicacin del arduino y

sensores, entre otros.

2.4.2 Simulacin

2.4.3 Construccin del Robot Balancn

En la etapa del proceso de dicho procedimiento, se arm un primer

prototipo en madera por motivo de las propiedades de este material,

pero se cambia a otro material sugerido por el docente obtenindose el

prototipo actual.

2.4.3.1 Primer Prototipo

Figura 6: Corte de la madera para la silueta del prototipo

Figura 7: Silueta de la maqueta con los servomotores

Figura 8: Primer prototipo terminado

 Figura 10: Posicin del sensor giroscopio

2.4.4 Programacin

En este proceso se ver la programacin que se realiz en la plataforma

de desarrollo en Arduino, adjuntndose las libreras necesarias para

obtener control PID. El MPU-6050 dar unos valores en bruto que

despus habr que refinar (dividir por una constante) a fin de conseguir

valores utilizables. La web oficial de Arduino dispone de un ejemplo para

leer datos de la MPU-6050, aunque habr que modificarla

considerablemente ya que slo da los valores sin refinar del Giroscopio y

el Acelermetro.

2.4.4.1 Declaracin de variables y librera:

La primera lnea incluye la librera Wire.h, necesaria para la interaccin

va protocolo I2C. #define MPU 0x68 es la direccin I2C de la IMU que

se especifica en la documentacin oficial. Los ratios de conversin son

los especificados en la documentacin. Deberemos dividir los valores

que nos d el Giroscopio y el Acelermetro entre estas constantes para

obtener un valor coherente. RAD_A_DEG es la conversin de radianes a

grados. La IMU da los valores en enteros de 16 bits. Como Arduino los

guarda en menos bits, hay que declarar las variables que almacenarn

los enteros provenientes de la IMU como un tipo de enteros especiales.

int16_t AcX, AcY, AcZ, GyX, GyY son, pues, los raw_values de la IMU.

Finalmente tenemos tres arrays (Acc[], Gy[], Angle[]) que guardan el

ngulo X, Y del Acelermetro, el Giroscopio y el resultado del Filtro

respectivamente. [0] se corresponde a X y [1] a Y.

1 #include <Wire.h>
2
3 //Direccin I2C de la IMU
4 #define MPU 0x68
5
6 //Ratios de conversion
7 #define A_R 16384.0
8 #define G_R 131.0
9
10 //Conversin de radianes a grados 180/PI
11 #define RAD_A_DEG = 57.295779
12
//MPU-6050 da los valores en enteros de 16
13 bits
14 //Valores sin refinar
15 int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
16
17 //Angulos
18 float Acc[2];
19 float Gy[2];
20 float Angle[2];
Se declaran las variables utilizadas en el control y los pines de los motores.

22 int TiempoMuestreo = 1; // tiempo de muestreo Se encuentra en milisegundos

unsigned long pasado = 0; // tiempo pasado (Se hace para asegurar tiempo de
23 muestreo)
24 unsigned long ahora;
25 double error;
26 double errorPass = 0; // Suma del error para la parte integral
27 double errorAnt = 0; // error anterior para la derivada
28 double U; // Seal control
29
30 // constantes del controlador
31 int in1 = 5;
32 int in2 = 4;
33 int in3 = 7;
34 int in4 = 6;
35 float Kp = 1;
36 float Ki = 0.01;
37 float Kd = 0.01;
38
39 int llantaI = 10; // llanta Izquierda
40 int llantaD = 11; // llanta derecha

2.4.4.2 Inicio de comunicacin entre el sensor y el arduino:

Se inicia la comunicacin por I2C con el dispositivo MPU, y se activa

enviando el comando 0. Tambin se declara los pines de los motores

como OUTPUT.

41 void setup()
42 {
43 Wire.begin();
44 Wire.beginTransmission(MPU);
45 Wire.write(0x6B);
46 Wire.write(0);
47 Wire.endTransmission(true);
48 pinMode(llantaI, OUTPUT);
49 pinMode(llantaD, OUTPUT);
50 pinMode(in1, OUTPUT);
51 pinMode(in2, OUTPUT);
52 pinMode(in3, OUTPUT);
53 pinMode(in4, OUTPUT);
 55 }

2.4.4.3 Adquisicin de datos del sensor y uso del filtro :

Se leen y guardan los datos del sensor MUP0650. Se calcula el ngulo,

como se conoce que la gravedad es 9.8 m/s, y tambin se conoce que

mesura dan los tres ejes del acelermetro, por trigonometra es posible

calcular el ngulo de inclinacin de la IMU usando la frmula de tangentes:

Acc[1] = atan(-1 * (AcX / A_R) / sqrt(pow((AcY / A_R), 2) + pow((AcZ / A_R), 2))) *

RAD_TO_DEG.

Luego de obtener los angulos, se usa un filtro. El filtro usado es conocido

como Filtro Complementario o Complementary Filter en ingls. Es ideal para

implementar con Arduino: fcil de utilizar, bajo coste de procesamiento y

con una precisin muy buena. En qu consiste exactamente? El Filtro

Complementario es en realidad una unin de dos filtros diferentes: un High-

pass Filter para el giroscopio y un Low-pass Filter para el Acelermetro. El

primero deja pasar nicamente los valores por encima de un cierto lmite, al

contrario que el Low-pass filter, que slo permite a los que estn por debajo.

57 void loop() {
58
59 Wire.beginTransmission(MPU);
60 Wire.write(0x3B); //Pedir el registro 0x3B - corresponde al AcX
61 Wire.endTransmission(false);
62 Wire.requestFrom(MPU, 6, true); //A partir del 0x3B, se piden 6 registros
63 AcX = Wire.read() << 8 | Wire.read(); //Cada valor ocupa 2 registros
64 AcY = Wire.read() << 8 | Wire.read();
65 AcZ = Wire.read() << 8 | Wire.read();
66
67 //A partir de los valores del acelerometro, se calculan los angulos Y, X
68 //respectivamente, con la formula de la tangente.
Acc[1] = atan(-1 * (AcX / A_R) / sqrt(pow((AcY / A_R), 2) + pow((AcZ / A_R), 2))) *
69 RAD_TO_DEG;
 Acc[0] = atan((AcY / A_R) / sqrt(pow((AcX / A_R), 2) + pow((AcZ / A_R), 2))) *
70 RAD_TO_DEG;
71 //Leer los valores del Giroscopio
72 Wire.beginTransmission(MPU);
73 Wire.write(0x43);
74 Wire.endTransmission(false);
Wire.requestFrom(MPU, 4, true); //A diferencia del Acelerometro, solo se piden 4
75 registros
76 GyX = Wire.read() << 8 | Wire.read();
77 GyY = Wire.read() << 8 | Wire.read();
78
79 //Calculo del angulo del Giroscopio
80 Gy[0] = GyX / G_R;
81 Gy[1] = GyY / G_R;
82
83 //Aplicar el Filtro Complementario
84 Angle[0] = 0.98 * (Angle[0] + Gy[0] * 0.010) + 0.02 * Acc[0];

2.4.4.4 PID:

La variable ahora guarda el tiempo de ejecucin. El CambioTiempo es la

diferencia entre el tiempo actual y el tiempo anterior. El punto de

equilibrio es -1.84, se escoge este valor luego de hacer varias pruebas.

El proporcional es igual al error multiplicado por el valor de Kp, el integral

es la suma de los erros multiplicado por el valor de Ki y el derivativo es

la diferencia de errores multiplicado por el valor de Kd.

86 Ahora = millis(); // tiempo actual

87
88 int CambioTiempo = ahora - pasado;
89 if (CambioTiempo >= TiempoMuestreo)
90 {
91 error = -1.84 - Angle[0];
92 errorPass = error * TiempoMuestreo + errorPass;
93 double errorD = (error - errorAnt) / TiempoMuestreo;
94 float P = Kp * error;
95 float I = Ki * errorPass;
96 float D = Kd * errorD;
 97
98 U = P + I + D; // Seal de control
99
100 pasado = ahora; // actualizar tiempo
101 errorAnt = error; // actualizar el error

2.4.4.5 Motores:

La funcin motores depende del valor obtenido en el PID. Si el robot

balancn se inclina as un lado, los motores van a girar hacia ese mismo

lado. El lmite de saturacin de la seal de control es U > 0 y U < 0. El

PWM que se le ingresa a los motores es de 220 (los motores giran al

mismo tiempo)

103 int atras = 0;

104 int adelante = 0;
105
106 if (U < 0) // lmites de saturacin de la seal de control
107 {
108 atras = 220;
109 }
110 if (U > 0) // lmites de saturacin de la seal de control
111 {
adelante = 220; // PWM de los motores atras y adelante (las dos ruedas funcionan
112 al tiempo)
113
114 }
115
116 if (atras != 0) {
117 if (adelante == 0) {
118 analogWrite(llantaI, atras);
119 digitalWrite(in2, HIGH);
120 digitalWrite(in1, LOW);
121
122 analogWrite(llantaD, atras);
123 digitalWrite(in4, HIGH);
124 digitalWrite(in3, LOW);
125 }
126 }
127
128 if (adelante != 0) {
 129 if (atras == 0) {
130 analogWrite(llantaI, adelante);
131 digitalWrite(in1, HIGH);
132 digitalWrite(in2, LOW);
133
134 analogWrite(llantaD, adelante);
135 digitalWrite(in3, HIGH);
136 digitalWrite(in4, LOW);
137 }
138 }
139 delay(4.0);
140 }

2.4.5 Valores de Kp , Ki y Kd

Se inicia con Kp, Ki, Kd igual a 0 y al final del Void loop un delay de 4.00. Se

trabaja primero con el Kp, establecindolo primero en 1. El objetivo del robot es

que mantenerse en vertical, incluso si es muy inestable. Si el robot avanza un

poco y cae, se debe reducir el valor del Kp. Si la respuesta del robot parece

lenta, se aumenta el valor del Kp. Una vez que el robot es capaz de

mantenerse un tiempo aunque sea breve en vertical, asignar un valor de 0.01 a

Kd. Intentar aumentar este valor hasta que se logre observar menos

oscilaciones. Luego de que el robot sea ms estable, asignar un valor de 10

a Ki. Si el valor de Ki es demasiado alto, el robot se sacudir izquierda y

derecha rpidamente. Si es demasiado baja, no se ver ninguna diferencia

perceptible. El Integral es acumulativo por lo tanto el valor Ki tiene un impacto

significativo.

Una vez que el robot sea lo suficientemente estable, se puede aumentar o

disminuir un poco el valor delay y ver si todava es suficiente o ms estable. El

valor delay afecta la estabilidad y es un factor importante a considerar.

2.5 Resultados experimentales:

En esta etapa los resultados del sensor acelermetro ms giroscopio se

muestran en una grfica en el software Matlab. (figura ..).

2.6 Anlisis de resultados:

Al principio no se obtuvo una estabilidad en el prototipo debido a que el sensor

se colocaba en una posicin errnea.

En la prueba cada vez que se inclinaba el prototipo no tena una respuesta

rpida en las llantas haciendo que este cayera, para ello se puso un rango de

respuesta en la programacin, obtenindose una mayor respuesta en el giro de

las llantas.

3 Mejoras al Robot

Se ha conseguido desarrollar un robot, totalmente autnomo, capaz de

auto-balancearse con sus dos ruedas. Sin embargo, an se podra seguir

aadindole futuras posibles mejoras o desarrollos, las cuales se pasan a

describir a continuacin:

Este proyecto servira como punto de partida para un proyecto ms

grande acerca de un vehculo inteligente de transporte basado en el

mismo concepto de mantener el equilibrio con dos ruedas.

Se podra conseguir un mejor comportamiento en la estabilidad del

sistema empleando tcnicas modernas de control, estas tcnicas se

basan en la lgica difusa con la que se puede implementar controladores

conocidos con el nombre de fuzzy o borrosos.

 Realizando un modelado preciso del sistema y posteriormente un

simulador capaz de asemejarse mucho a la realidad, se conseguira

obtener mejores resultados en lo respecto al control del robot.

Se puede establecer medio de comunicacin con dispositivos mviles

mediante mdulo bluetooth para controlar la direccin del robot.

El uso de encoders en las ruedas, dara la capacidad de poder

implementar un lazo de control en cascada, uno de control de velocidad

y otro de control de la posicin vertical del robot. De esta manera se

consigue un mejor comportamiento con respecto a la estabilidad.

Incorporando una pequea pantalla LCD al robot, se dara la posibilidad

de visualizar continuamente las variables importantes del sistema.

4 Conclusiones

Filtrar la seal que nos enva el sensores MPU6050, es muy importante

ya que el uso de los ngulos obtenidos puede presentar problemas o

inconvenientes como offsets, los ruidos de las medidas con los

acelermetros, que pueden indicar valores falsos, o derivas producidas

al tener que integrar los valores del girscopo. Para poder obtener

medidas que se ajusten lo mximo posible a la realidad del sistema,

necesitaremos de algn otro medio como el filtro complementario.

5 Bibliografa

arduino. (s.f.). Obtenido de http://arduino.cl/arduino-uno/

Castaos Luna, F. (3 de Febrero de 2003). Levantamiento y Estabilizacion del

Pendulo.

Moyano, E. (s.f.). Arduino PID - Gua de uso de libreias. Obtenido de

http://brettbeauregard.com/blog/wp-

content/uploads/2012/07/Gu%C3%ADa-de-uso-PID-para-Arduino.pdf

National Instruments. (09 de 12 de 2014). Obtenido de

http://digital.ni.com/public.nsf/allkb/AA1BDEA4AA224E3E86257CE4007

07527

NAYLAMP. (s.f.). Obtenido de MECHATRONICS:

http://www.naylampmechatronics.com/blog/45_Tutorial-MPU6050-

Aceler%C3%B3metro-y-Giroscopio.html

NAYLAMP . (s.f.). Obtenido de MECHATRONICS:

http://www.naylampmechatronics.com/drivers/11-driver-puente-h-

l298n.html