UNIVERSIDAD CATOLICA DE CUENCA
CONSTRUCCIÓN Y CONTROL DE UN
PROTOTIPO DE SEGWAY PARA USO DE
LA EMOV UTILIZANDO LA PLATAFORMA
LEGO MINDSTORMS EV3
Alberto Sares Reyes
rafaelsares@hotmail.com

Resumen—Se realizará un modelo de un segway en la
plataforma Lego Mindstorms Ev3, se realizara un modelo
matemático utilizando ecuaciones de Newton como inicio se
realizara la sincronización de un PID y se desarrollará un
programa en el software ROBOTC para poderlo controlar.
Palabras claves—Control, Lego, Péndulo Invertido
I. INTRODUCCIÓN
El péndulo invertido es conocido por ser uno de los
problemas más importantes y clásicos de la teoría del control.
Es un control inestable y no lineal. A menudo se utiliza como
un ejemplo académico, principalmente porque es un sistema
de control más accesible, y por otro lado, permite mostrar las
principales diferencias de control de bucle abierto y su
estabilización a bucle cerrado. Aunque existen diferentes
técnicas al diseñar el regulador óptimo capaz de estabilizar el
péndulo, no todas representan la mejor opción, uno de los
principales usos es el segway que es un dispositivo de
movilidad que se utiliza para el desplazamiento en distancias
cortas como por ejemplo dentro del centro histórico de la
ciudad de cuenca.
 diagrama:
II.CONSTRUCCIÓN DEL SEGWAY MEDIANTE EL
PRINCIPIO DE PÉNDULO INVERTIDO SOBRE
DOS RUEDAS
En esta sección se muestra la construcción del prototipo de
segway que se construirá usando la plataforma el lego
mindstrorms ev3, el segway armado cuenta con un
controlador, sensores, actuadores y piezas de armado que
conforma el paquete de lego mindstrorms ev3 versión
education.
A. Controlador
La tercera generación EV3, evolution 3 el bloque EV3
actúa como centro de control y fuente de potencia del robot
cuenta con:
 4 puertos de entrada rj12 modificada
 4 puertos de salida rj12 modificada
 Un puerto mini USB para PC
 Un puerto para tarjetas Micro SD
 Un altavoz integrado
 Receptor Bluetooth y Wi-Fi
B. Sensor
El sensor giroscópico EV3 Gyro de Hitechnic® detecta
velocidad angular y retorna un valor que representa el
número de grados de rotación por segundo, de la misma
forma que indica la dirección de rotación. El sensor Gyro
puede medir rotación en una escala de  360°.
C. Actuadores
Los servomotores de corriente DC están construidos en
base a una gran cantidad de engranajes internos. Éstos
pueden rotar hasta alcanzar 170 rpm (revoluciones por
minuto), trabajan en un rango de -9V a +9V .
III. MODELADO DEL SISTEMA FÍSICO
Primeramente tenemos que tener el esquema del segway
para luego proceder con nuestro diagrama de cuerpo libre,
utilizando ecuaciones de Newton planteamos nuestro sistema
de ecuaciones generalizada después dela cual podemos
encontrar nuestra función de trasferencia y remplazar
nuestros parámetros físicos de nuestro sistema.
Como ya lo comentamos lo primero es tener nuestro
 UNIVERSIDAD CATOLICA DE CUENCA

Procedemos inicialmente a plantear nuestras coordenadas

de punto medio geométrico y descomponer nuestras
componentes:

( xg , y g )  Cordenadas al centro de gravedad del pendulo

xg  x  l sin  
y g  l cos 
En el eje de las x
xg  x  l sin  
x g  x  l cos   Primera derivada (VELOCIDAD)
x  x  l cos   l sin   2  Segunda derivada (ACELERACION)
g

En el eje de las y
y g  l cos 
y g  l sin    Primera derivada

FIG.1 DIAGRAMA FISICO DEL PENDULO   del cos

y g  l sinLuego a ver
 2 descompuesto
 Segunda derivada
nuestros componentes
procedemos a realizar la sumatoria de fuerzas en los ejes X, Y
Luego de haber planteado nuestro diagrama se nos momentos de inercia y el movimiento horizontal del péndulo
resultara mucho más fácil realizar nuestro diagrama de recordando que las sumatorias en cada eje se igualan para
cuerpo libre en el cual se observa todas las fuerzas que se poder obtener las ecuaciones que necesitamos mas adelante.
aplican en el modelo que se esta usando. FIG. 2

Sumatoria de fuerzas en x
F x  mxg
F x H
H  mxg
 
H  m x  l cos   l sin   2
H  mx  ml cos    ml sin   2
Sumatoria de fuerzas en y
F y  my g
F y  V  mg
V  mg  my g

V  mg  m  l sin    l cos  2 
 
V  m  l sin    l cos  2  mg
V   ml sin    ml cos   mg
2

FIG. 2 DIAGRAMA DE CUERPO LIBRE

Sumatoria de momentos de inercia
Se muestra también el nombre y significado de cada una de  M  Vl sin    Hl cos 
nuestras variables y constantes:
V=fuerza Vertical  M  I
Encontramos nuestras primeras ecuaciones del sistema,
H=fuerza Horizontal I que
tenemos Vl sin   en
tomar  Hl cosque
cuenta   para cumplir con nuestro
mg=fuerza de peso del péndulo
l=distancia media propósito tenemos que tener unas restricciones las cuales
x=distancia horizontal a la base del péndulo tienen como objetivo
Movimiento que el robot
horizontal del permanezca
Pendulo en posición
Y=distancia vertical a la base del péndulo vertical. 2
d x
XG=distancia horizontal al centro de gravedad M 2 uH
YG=distancia vertical al centro de gravedad dt
Mx  u  H
H  u  Mx
 UNIVERSIDAD CATOLICA DE CUENCA

cos   1 Teniendo en cuenta que I (momento de inercia del cuerpo del

péndulo) es despreciable ya que por lo general es muy
 2  0 pequeño en comparación con m y l.

sin    
Con estas restricciones las ecuaciones quedarían de manera
reducida de la siguiente manera:


 ml
ml x  mgl 2

ml 
 u  ml  
  ml

  M  m 
 
 u  ml  
   l   g
 
  M  m 
 

E
H
n

x
mx
 
u  ml    M
ml  M
co  m

s
 m l

 


  
u  ml   Ml   m
H mx
 ml

u  Ml  m
En y
V  m  l s in        M 

  Ml 
V   ml s i n     m

  
V  mg    M  m g


E s tos do s res ul tad o

 
I   Vl s in  Hl c

 
I   Vl  Hl
I   m gl   mx
 
I   m gl  lmx
 
I   ml 2   l mx
 
 
 
IV. 
FUNCION DE TRANSFERENCIA 
2
I ml ml
x

Hemos obtenido nuestro modelo matemático que obedece a

nuestro robot pero necesitamos la función de trasferencia del
mismo por lo que realizamos LA PLACE en nuestra ecuación
para obtener dicho función:
Ml 
   M  m g   u

Ml s 
2
( s)   M  m g  (s)  U ( s )
 (s)  Ml s 2
  M  m g   U ( s )
 ( s) 1
 U ( s)

Ml s 2
  M  m g

H  m
x  ml   Luego de 
encontrar la función de trasferencia del sistema

u  M
x  mx  ml 
    procedemos a remplazar los parámetros del robot:

u m
x ml Mx
u  x
 M  m   ml 

Encontramos las ecuaciones que describen el movimiento del

péndulo invertido las cuales son: TABLA 1
 (s) 1

u  x M  m   ml  U ( s) Mls   M  m  g
2

 I  ml 2   mlx  mgl  (s) 1


 U ( s ) (0.124)(0.145) s 2  (0.124  0.8788)(9.81)
Realizando operaciones matemáticas despejando la variable x
 (s) 1
y remplazándola en la segunda ecuación obtenemos: 
 U ( s ) 0.01798s  9.837468
2

u  
x M  m   ml 

55.61
u  ml G(s) 
x
 
 M  m s  547.13
2

Esto remplazamo s en nuestra ecuacion : 

 I  ml 2    ml
x  mgl
Observando la función de trasferencia a simple vista se
 UNIVERSIDAD CATOLICA DE CUENCA

denota que es inestable y se lo comprueba simulando nuestra 4.4479

función en MATLAB. C ( s )  10.772938  0.75258468s 
s

Y las gráficas FIG. 5 corregidas con la funcion:

FIG. 3 POLOS Y CEROS DE LA FUNCION

La grafica de polos y ceros de la función describe un

sistema inestable ya que tiene un polo en el lado derecho del
eje.

V. PID
Utilizando Matlab y usando el commando SISOTOOL FIG. 4
realizamos una compensación de la función de transferencia
para que el sistema pase a ser estable:

FIG. 5 POLOS CEROS Y RESPUESTA CON FUNCION

COMPENSADA

Observamos que los polos se han convertido a críticamente

estables y que tenemos una función de trasferencia que se
estabiliza en el tiempo, con esta función de transferencia ya
podemos encontrar las constantes proporcional, integrativa y
derivativa teniendo en cuenta que:

FIG. 4 COMPENSACION DE FUNCION DE

TRASFERENCIA
TABLA 2
C ( s )  4.4479
1  0.072s 1  2.3s 
s
Luego de realizar la compensación la función de traferencia 4.4479
nos queda de la siguiente manera: C ( s )  10.772938  0.75258468s 
s
I
C ( s )  P  Ds 
s
P  10.772938
I  4.4479
D  0.75258468
 UNIVERSIDAD CATOLICA DE CUENCA

#pragma config(Sensor, S4, boton,

sensorEV3_Touch)
#pragma config(Motor, motorA, derecha,
tmotorEV3_Large, PIDControl, driveRight, encoder)
#pragma config(Motor, motorD, izquierda,
tmotorEV3_Large, PIDControl, driveLeft, encoder)
//*!!Code automatically generated by 'ROBOTC'
configuration wizard !!*//
Si bien es cierto se han encontrado el PID de mi sistema,
puedo yo aun regularlo para que este lo mas estable posible //+++++++++++++++++++++++++++++
por lo cual usamos el comando PIDTUNER, y regulamos //Programa base para un PID
nuestro pid lo mas estable posible en nuestro caso en //+++++++++++++++++++++++++++++
particular logramos obtener un sistema casi ideal con los //Rutina de Inicializacion=====
valores de la FIG.6
float ang_sen = 0.000; //dato de lectura del sensor
float ang_sen_anterior=0; //dato de lectura del sensor
anterior
float error_pid = 0;//error del sistema de control
float ang_referencia = 0.000;//angulo de referencia
int valor_pid=0;//Valor calculado en la rutina de pid

float kp=142.276;//Constante proporcional

float ki=1482.898;//Constante de Integracion
float kd=3.4127;//Constante Derivativa

float error_presente=0; //Declaramos una variable llamada

"error_presente" con inicializacion 0
float error_pasado=0; //Declaramos una variable llamada
"error_pasado" con inicializacion 0

//Rutina de Inicializacion=====

//1. Subrutina Sensor Giroscopo=============

void giro()
{

FIG. 6 PARAMETROS DE CONTROL PIDTUNER //ang_sen =getGyroRate(S1);

ang_sen = getGyroHeading(S1);
La grafica con los cambios de kp ki y kd es la que se muestra //ang_sen = getGyroDegrees(S1);
en la FIG 7. displayBigTextLine(10,"%f",ang_sen);
ang_sen = ang_sen - ang_sen_anterior;
if (ang_sen < -90)//Limite de angulo
{
ang_sen=0.000;
}

displayBigTextLine(12,"%d",ang_sen);
ang_sen_anterior= ang_sen; //EL VALOR DEL ANGULO
DEL SENSOR SE LE ASIGNA AL VALOR DEL ANGULO
ANTERIOR

}
FIG. 7 PID SINTONIZADO //1. Subrutina Sensor Giroscopo=============

//2. Subrutina Calculo del Error============

void error()
{
VI. PROGRAMACION ROBOTC
error_pid= ang_referencia;
La programación se la realizo en Robotc, en el cual se error_pid= ang_sen-error_pid;
ingresan los valores PID obtenidos. error_presente= error_pid;
displayBigTextLine(14,"%d",error_pid);
 UNIVERSIDAD CATOLICA DE CUENCA

}  Los valores de los parámetros de nuestra planta son

//2. Subrutina Calculo del Error============ importantes para que la función de trasferencia sea
correcta y que en el momento de colocar valores en
//3. Subrutina PID========================== nuestro programa sean los idóneos para que el
void pid() péndulo funcione.
{  Las herramientas informáticas como MATLAB son
float proporcional=0; indispensables en el cálculo del PID ya que nos
float integral=0; ayudan a calcularlos de manera más rápida.
float derivativa=0;

proporcional= kp*error_presente; VIII.BIBLIOGRAFIAS Y REFERENCIAS

integral = ki*(error_presente+error_pasado);
K. Ogata. “Sistemas de Control en Tiempo Discreto”. Ed. Prentice Hall
derivativa = kd*(error_presente-error_pasado); Hispanoamericana S.A., 1996
valor_pid = proporcional+ integral +derivativa;
B. Sherman. “Balibot, an inverted pendulum robot”, 2003. Disponible
en http://home.comcast.net/~botronics/balibot.html.
//Limitar valor_pid
if (valor_pid > 100) Página oficial de Lego Mindstorms NXT®. http://www.mindstorms.
{ lego.com
valor_pid = 50; .
Página oficial de Hitechnic®. http://www.hitechnic.com.
}
A. Patete. “Desarrollo de un mímico de un sistema de control en
if (valor_pid < -100) Mathematica®”. Proyecto de grado, Facultad de Ingeniería, ULA,
{ Mérida, 2002.
valor_pid = -50;
}

error_pasado = error_presente;//Guardo el error anterior

}
//3. Subrutina PID==========================

//4. Subrutina Motores======================

void motores(int velocidad)
{
setMotorSpeed(motorA, velocidad);
setMotorSpeed(motorD, velocidad);
}
//4. Subrutina Motores======================
/////////////////////////////////////////////////////////////////
short repetir=0;// Declaramos la variable "repetir"
/////////////////////////////////////////////////////////////////
task main()
{
resetGyro(S1);

//repetir

while (repetir==0)
{
//1.
giro(); //Llamamos a la subrutina "giro"
//2.
error();
//3.
pid();
//4.
motores(valor_pid);
}
//repetir
}

VII. CONCLUCIONES Y RECOMENDACIONES