Design and implementation of a real scale segway for the program of B.Sc. Mechatronics engineering at the Catholic University of Bolivia. #segway #mechatronics #spanish #project #engineering

1. Diseño e
implementacion de un
Segway
Ronald Butron
Edwin Calla
Luis Flores
Pablo Romero
Jose Torres
Ortiz

2. Introduccion
O El Segway Personal Transporter (Segway
PT - trasportador personal) es un vehículo de
transporte ligero giroscopio eléctrico de dos
ruedas, con autobalanceo controlado
por ordenador, inventado por Dean kamey y
presentado en diciembre de 2001.
O El Segway es el primer dispositivo de
transporte con autobalanceo. El usuario se
debe inclinar hacia la dirección que quiera
tomar (delante, atrás, derecha o izquierda). El
motor es eléctrico y silencioso, alcanzando
los 20 km/h (15 km/h en los P-series).

3. Descripcion del problema
O Diseñar un sistema segway con todas las
características de diseño e
implementación superando todas las
adversidades que presenta nuestro medio
(económicas, oferta de productos,
informáticas, etc.)

4. Objetivos
O Objetivo general:Diseñar un controlador difuso para un
sistema péndulo invertido, el cual será utilizado para el
manejo de un segway.
O Obejtivo especifico:.
O Comprobar que el controlador diseñado, resuelve el
problema de estabilización del sistema péndulo invertido (a
futuro segway).
O Comprobar que el sistema péndulo invertido es estable
dentro del rango para el cual el estudio fue hecho.
O Realizar el diseño de la estructura real del segway
utilizando herramientas computacionales
O Realizar el armado de la estructura
O Simular y aplicar los modelos elegidos en el segway para
poder cumplir con el objetivo general

5. Plan de actividades
A Definir los principales problemas
B Dividir responsabilidades al grupo
C Definir los conceptos básicos del proyecto
D Bench Marking
E Compra de algunos elementos de transmisión
F Armado del prototipo (solo estructura)
G Calibración de los sensores del prototipo
H Programación del microprocesador ARDUINO UNO del prototipo
I Armado de la etapa de potencia para los motores
J Pruebas del prototipo
K Diseño de la parte mecánica (estructura del segway)
L Cotización de la mano de obra para el maquinado de las piezas
M Maquinado de las piezas (ejes) en taller de tornería
N Compra de las piezas restantes (planchas, angulares, ruedas)
O Perforado de las planchas y otros elementos de unión
P Pedido del componente electrónico (giroscopio)
Q Estudio, simulado y programado de los filtros a usarse en el segway
R Primeras pruebas del giroscopio
S Unión de todas las partes mecánicas (de taller y compradas)

6. Tabla de los tiempos iniciales y finales
de una actividad

7. Diagrama de Gantt

8. Mapa de precedencia (Para
definir rutas críticas):

10. Presupuesto realCantidad Material/item/trabajo Precio/U
Bs.
Precio
total Bs.
4 Chicharra grande 10 40
4 Chicharras pequeñas 10 40
4 Cadenas metalicas 7 28
8 Rodamientos pequeños 5 40
2 Rodamientos medianos 5 10
1 Metro eje hierro dulce 1'' 40 40
2 Ruedas 57.5 115
2 Placas de circuito (potencia) 60 120
1 Maquinado de ejes y soldado rodamientos 1800
Planchas de 2mm 150
2 Giroscopios 140 280
Perfiles en C 30
Perfiles en O 15
Angulares (varios) 25
120 Pernos 25
Transporte 50
TOTAL Bs. 2808

11. Producto esperado
Un segway de dimensiones reales que tenga la función básica de
mantener el equilibrio y acelerar por lomenos en un solo sentido.
El producto final debería verse de la siguiente forma

12. Modelo matematico
O El carrito con un péndulo invertido, se
muestra abajo, es "empujado" con una fuerza
impulsiva, F. Determinemos las ecuaciones
dinámicas de movimiento del sistema, y
linealicemos cerca del ángulo del péndulo,
theta = Pi (en otras palabras, asumamos que
péndulo no se aparta más que unos pocos
grados de la vertical, elegida en un ángulo de
Pi). Encontremos un controlador para
satisfacer todos los requerimientos de diseño
dados arriba.

13. Modelo matematico
M masa del carro 60 kg
m masa del péndulo 54 kg
b fricción del carro 0.1 N/m/seg
l
longitud al centro de
masa del péndulo
0.3 m
I inercia del péndulo 0kg*m^2
F
fuerza aplicada al
carro
x
coordenadas de
posición del carro
theta
ángulo del péndulo
respecto de la
vertical

14. Modelo matematico
O Sumando las fuerzas en el diagrama de
cuerpo libre del carro en la dirección
horizontal, se obtiene la siguiente
ecuación del movimiento:
O Sumando las fuerzas en el diagrama de
cuerpo libre del péndulo en la dirección
horizontal, puede obtener la ecuación
para N:

15. Modelo matematico
O Si sustituye esta ecuación en la primera ecuación, se
obtiene la primera ecuación del movimiento de este
sistema:
O para obtener la segunda ecuación de movimiento,
sume las fuerzas perpendiculares al péndulo. Si
resuelve el sistema a lo largo de este eje se
ahorrará un montón de álgebra. Debería obtener la
siguiente ecuación:

16. Modelo matematico
O Para librarse de los términos P y N en la
ecuación anterior, sume los momentos
sobre el centroide del péndulo para
obtener la siguiente ecuación:
O Combinando estas dos últimas
ecuaciones, se obtiene la segunda
ecuación dinámica:

17. Modelo matematico
O Como Matlab solo puede trabajar con funciones lineales,
este conjunto de ecuaciones debería ser linealizado
alrededor de theta = Pi. Asuma que theta = Pi + ø(ø
representa un pequeño ángulo en la dirección vertical). Por
lo tanto, cos(theta) = -1, sin(theta) = -ø, y (d(theta)/dt)^2 =0.
Luego de la linealización las dos ecuaciones de
movimiento serán :
O Para obtener analíticamente la función de transferencia de
las ecuaciones del sistema linealizado , debemos tomar
primero la transformada de Laplace de las ecuaciones del
sistema. Las transformadas de Laplace son:

18. O Como como estamos mirando al ángulo
Phi como la salida de interés, resuelva la
primera ecuación para X(s),
O y entonces sustitúyala en la segunda
ecuación:

19. Modelo matematico
O Re-ordenando, la función de transferencia
es:
O donde,

20. Tipos de control de estabilidad
O PID
O Proporcional
O Integral
O Derivativo
O Logica difusa

21. Estabilidad
O Se procedio a simular el balance del
segway a lazo abierto , en el programa de
matlab.
O A continuacion se procedio a simular la
estabilidad del mismo.

22. Matlab
O Primera simulacion
M = 60;
m = 54;
b = 0.1;
i = 0;
g = 9.8;
l = 0.3;
q = (M+m)*(i+m*l^2)-(m*l)^2; %simplifica entrada
num = [m*l/q 0]
den = [1 b*(i+m*l^2)/q -(M+m)*m*g*l/q -b*m*g*l/q]
t=0:0.01:5;
impulse(num,den,t)
axis([0 1 0 60])

23. Como se puede observar en el grafico la respuesta no es satisfactoria,
muestra que no es estable a lazo abierto. Por tanto el segway es
inestable a dicho impulso.
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
10
20
30
40
50
60
Impulse Response
Time (sec)
Amplitude

24. Para encarar dicho problema modificamos el programa de la
siguiente manera:
M = 20;
m = 54;
b = 0.1;
i = 0;
g = 9.8;
l = 0.3;
p = i*(M+m)+M*m*l^2; %denominador para las matrices A y B
A = [0 1 0 0;
0 -(i+m*l^2)*b/p (m^2*g*l^2)/p 0;
0 0 0 1;
0 -(m*l*b)/p m*g*l*(M+m)/p 0]
B = [ 0;
(i+m*l^2)/p;
0;
m*l/p]
C = [1 0 0 0;
0 0 1 0]
D = [0;
0]
T=0:0.05:10;
U=0.2*ones(size(T));
[Y,X]=lsim(A,B,C,D,U,T);
plot(T,Y)
axis([0 2 0 100])

25. La línea azul representa la posición del carro y la línea verde representa el
ángulo del péndulo. De esta figura y de la anterior es obvio que para mejorar la
dinámica del sistema alguna especie de control tendrá que ser diseñada.
En este caso fue elegido un PID o un LQR.
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
0
10
20
30
40
50
60
70
80
90
100

26. PID
O M = 60;
O m = 54;
O b = 0.1;
O i = 0.006;
O g = 9.8;
O l = 0.3;
O
O q = (M+m)*(i+m*l^2)-(m*l)^2; %simplifica entrada
O
O num = [m*l/q 0]
O den = [1 b*(i+m*l^2)/q -(M+m)*m*g*l/q -b*m*g*l/q]
O kd = 200;
O k = 1000;
O ki = 1;
O numPID = [kd k ki];
O denPID = [1 0];
O numc = conv(num,denPID)
O denc = polyadd(conv(denPID,den),conv(numPID,num))
O t=0:0.01:20;
O impulse(numc,denc,t)
O axis([0 7 0 0.05])

27. PID
O Como se puede observar se logro la
estabilidad del segway en un tiempo
menor a 1 seg.
0 1 2 3 4 5 6 7
0
0.005
0.01
0.015
0.02
0.025
0.03
0.035
0.04
0.045
0.05
Impulse Response
Time (sec)
Amplitude

28. PID
CONTROL MANUAL Y AUTOMÁTICO DE
PÉNDULO INVERTIDO
Este script ejecuta la simulación de un péndulo que
puede ser controlado manualmente (accionando una
barra deslizante o "slider") o de forma automática,
mediante lazos de control PID.
El control automático incorpora un doble lazo
incluyendo un lazo con control del ángulo del
péndulo (para que se sitúe en posición vertical) y
otro lazo más lento para hacer que la posición de la
base se sitúe en un punto dado en el estado de
equilibrio.

29. PIDclear;
close all;
clc;
disp('Instrucciones:');
disp('- Pulsar ''0'' para desactivar el sistema de control');
disp('- Pulsar otra tecla para restaurar el sistema de control');
disp(' ');
disp('Control manual: actuar sobre la barra deslizante para modificar la base del péndulo.');
pause(2);
% PARÁMETROS DEL PÉNDULO
l = 1.70; % Longitud del péndulo
m = 54; % Masa del péndulo
M=60; %Masa de la caja
J = m*l^2; % Momento de inercia referido al eje
B = 1; % Coeficiente de fricción
g = 9.8; % Aceleración de la gravedad
% ESTADO INICIAL DEL PÉNDULO
x = [pi-0.1;0]; % Para que se vea el efecto del control, empezamos
% con el péndulo casi vertical (theta = pi +/- "algo")

30. % DEFINICIÓN DE UN "SLIDER" PARA CONTROLAR MANUALMENTE EL PÉNDULO
f = figure(1);
set(f,'pos',[200,200,700 700]);
h = uicontrol('style','slider','pos',[20 20 680 20],'min',-4,'max',4);
Tm = 0.01; % Período de muestreo
e0 = zeros(2,1); % Condiciones iniciales del control de ángulo
ep0 = zeros(2,1); % Condiciones iniciales del control de posición
x0 = [0;0]; % Condiciones iniciales del péndulo
a0 = [0;0];
xmin = -2;
xmax = +2;
y = x(1);
% PARÁMETROS DEL CONTROL PID DEL ÁNGULO
Kp = 10;
Ki = 100;
Kd = .1;
s = tf('s');
Cth = c2d(Kp + Ki/s + Kd*s/(0.01*s+1),Tm,'tustin');

31. % PARÁMETROS DEL CONTROL PID DE LA POSICIÓN DE LA BASE
Cpos = c2d(.04 + 0.0001/s + s*0.0001/(0.01*s+1),Tm,'tustin');
pos = 0; % Valor inicial de la posición de la base del péndulo
k = 2; % Empezamos en k=2 para tener acceso al menos a dos muestras anteriores
while 1,
k = k + 1;
% BUFFER CON LA POSICIÓN (X(3) TIENE LA POSICIÓN ACTUAL)
% X(k) = get(h,'Value');
e(k) = pi-y;
% RESTRINGIMOS EL VALOR DEL ERROR AL INTERVALO [-pi,pi]
e(k) = mod(e(k)+pi,2*pi)-pi;
% CONTROL DE LA POSICIÓN DE LA BASE DEL PÉNDULO
ep = 0-pos;
[dpos,ep0] = filter(Cpos.num{1},Cpos.den{1},ep,ep0);
e(k) = e(k) - dpos;
% SELECCIÓN DE CONTROL MANUAL / CONTROL AUTOMÁTICO
tecla = get(f,'currentchar');
switch tecla
case '0'
pos = get(h,'value');
otherwise
[pos,e0] = filter(Cth.num{1},Cth.den{1},e(k),e0);
end

32. % Suavizado de la aceleración (muy conveniente, porque el movimiento del
% objeto "slider" con un ratón se produce a saltos, dando lugar a segundas
% derivadas muy elevadas)
[X(k),x0] = filter(.01,poly([.9 .9]),pos,x0);
[a,a0] = filter((1/Tm^2)*[1 -2 1],[1 0 0],X(k),a0);
A(k) = a;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%
% ECUACIONES EN ESPACIO DE ESTADOS (NO LINEALES) DEL PÉNDULO
u = -a; % Asignamos la entrada
% Ecuación de estados
x(1) = x(1) + Tm*x(2);
x(2) = x(2) + Tm*(1/J*(-B*x(2)-m*g*l*sin(x(1))+m*u*l*cos(x(1))));
% Ecuación de salida
y = x(1);
th = y; % Asignamos la salida
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%
% REPRESENTACIÓN GRÁFICA DE LA SIMULACIÓN
figure(1);
plot(X(k),0,'.');
hold on;
p1 = X(k);
p2 = X(k)+l*exp(j*(th-pi/2));
line(real([p1,p2]),imag([p1,p2]));
plot(real(p2),imag(p2),'.','markersize',40);
hold off;

33. % Sugerencia: pueden dibujarse también otras flechas indicando en tiempo real las fuerzas
reales
% o de inercia que actúan en cada elemento del sistema
% Centrado automático de la perspectiva sobre el objeto de control
if X(k)>xmax-1
xmin = xmin + 0.1;
xmax = xmax + 0.1;
elseif X(k)<xmin+1
xmin = xmin - 0.1;
xmax = xmax - 0.1;
end
grid on;
axis([xmin-3 xmax+3 -5 5]);
% Refresco de la imagen
drawnow;
end

34. Pid
-5 -4 -3 -2 -1 0 1 2 3 4 5
-5
-4
-3
-2
-1
0
1
2
3
4
5

35. O Diagrama de bode Cpos
-30
-20
-10
0
10
Magnitude(dB)
10
-4
10
-3
10
-2
10
-1
10
0
10
1
10
2
10
3
-90
-45
0
45
Phase(deg)
Bode Diagram
Frequency (rad/sec)

36. O Diagram de bode de cth (angulo inicial)
20
30
40
50
60
70
Magnitude(dB)
10
-1
10
0
10
1
10
2
10
3
-90
-45
0
45
Phase(deg)
Bode Diagram
Frequency (rad/sec)

37. O Diagrama de bode al encontrar
estabilidad
-5
0
5
10
15
20
Magnitude(dB)
10
0
10
1
89
89.5
90
90.5
91
Phase(deg)
Bode Diagram
Frequency (rad/sec)

38. Ecuación de estado
• El espacio de estados es otro método que
permite modelar un sistema físico.
• Una forma general de expresar la din´amica
de un sistema lineal es:
x = Ax + Bu
y = C x + Du

39. • Las ecuaciones de estado para el problema
son:

40. Respuesta a lazo abierto
• Es necesario comprobar si el sistema es estable a lazo
abierto.
• Por lo tanto, los requerimientos de disen˜o a tener en
cuenta son:
Tiempo de establecimiento de x y θ menor a 5
segundos.
Tiempo de subida para x menor que 0.5 segundos.
Sobrepico de θ menor que 20 grados

41. • En primer lugar hay que calcular las matrices
A,B,C y D a partir de la expresion obtenida

44. Como resultado se obtiene:

45. • Hay que introducir una entrada que en este
caso ser un escalon de 0,2 metros.

46. • Se observa la salida donde la lınea azul
representa la posicion del carro y la lınea
verde el angulo del pendulo. Para mejorar la
dinamica del sistema es necesario incluir
alguna clase de control.

47. Regulador LQR

48. • El paso siguiente en el proceso de diseño es
asumir que tenemos realimentación completa
de estados.
• Y encontrar el vector K que determina la ley
de control para la realimentación.
• Esto puede hacerse usando la funcion lqr en
matlab que nos permite elegir dos
parámetros, R y Q.
• El caso más simple es considerar R=1, y
Q=C'*C

49. Diseño en espacio de estados
• Se tomaran los siguientes criterios de diseño:
Tiempo de establecimiento de x y theta menor que 5
segundos.
Tiempo de Subida para x menor que 0.5 segundos.
Sobrepico de theta menor que 20 grados (0.35 radianes).
A los cuales ademas se incluye un cuarto criterio, un error
estacionario entre el 2 %. Las caracter´ısticas del p´endulo siguen
siendo las mismas:

51. • Para la obtencion del regulador se va a
utilizar la funcion “lqr” que dara el
controlador optimo.
• Para obtener la matriz Q es suficiente con
introducir el comando C’*C.

52. • Ahora que se conoce la matriz Q ya se puede
calcular el valor de K y mostrar graficamente
el resultado.

54. • Para valores x=5000; y=100 se obtiene la
siguiente respuesta:

55. Observadores
• Se considera entonces el sistema:
x˙ (t) = Ax(t) +B
y(t) = C x(t)

56. • Observador a lazo abierto:
Conociendo A y B, es posible duplicar la
ecuación de estados original construyendo el
sistema:

57. Arduino#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include <PID_v1.h>
MPU6050 accelgyro;
int16_t gx1, gy1, gz1;
int16_t ax1, ay1, az1;
float gx2=0, gy2=0, gz2=0;
float ax2=0, ay2=0, az2=0;
int16_t i=0;
double Setpoint, Input, Output;
double Setpoint1, Input1, Output1;
double Salida;
float T_Now =0;
float T_Last=0;
float gx, gy, gz;
float ax, ay, az;

58. float gx_offest=0, gy_offest=0, gz_offest=0;
float ax_offest=0, ay_offest=0, az_offest=0;
float Angle_Z,Angle_AG,Angle_GG,Angle_G;
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
PID myPID1(&Input1, &Output1, &Setpoint1,2,5,1, DIRECT);
void setup() {
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();
pinMode(13, OUTPUT);
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection
failed");
Setpoint = 0;
Setpoint1=0;
myPID.SetMode(AUTOMATIC);
myPID1.SetMode(AUTOMATIC);
for(i=0;i<1000;i++)
{
digitalWrite(13, LOW);
delay(10);
accelgyro.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);
gx=gx1/131.00;gy=gy1/131.00;gz=gz1/131.00; //FS_SEL=0
ax=ax1/16384.00;ay=ay1/16384.00;az=az1/16384.00; //AFS_SEL=0
// Serial.print(gx);Serial.print(",");
// Serial.print(gy);Serial.print(",");
// Serial.print(gz);Serial.print(",");
// Serial.print(ax);Serial.print(",");
// Serial.print(ay);Serial.print(",");

59. // Serial.print(az);Serial.print(",");
// Serial.print("n");
gx_offest+=gx; gy_offest+=gy;gz_offest+=gz;
ax_offest+=ax; ay_offest+=ay;az_offest+=az;
digitalWrite(13, HIGH);
}
gx_offest/=i; gy_offest/=i; gz_offest/=i;
ax_offest/=i; ay_offest/=i; az_offest/=i;
// Serial.print(gx_offest);Serial.print(",");
// Serial.print(gy_offest);Serial.print(",");
// Serial.print(gz_offest);Serial.print(",");
// Serial.print(ax_offest);Serial.print(",");
// Serial.print(ay_offest);Serial.print(",");
// Serial.print(az_offest);Serial.print(",");
// Serial.print("n");
T_Last=millis()/1000.0;
} void loop() {
accelgyro.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);
gx=gx1/131.00;gy=gy1/131.00;gz=gz1/131.00; //FS_SEL=0
ax=ax1/16384.00;ay=ay1/16384.00;az=az1/16384.00; //AFS_SEL=0
float Angle_Z = gz-gz_offest;
T_Now=millis()/1000.0;
Angle_GG = Angle_GG + Angle_Z*(T_Now-T_Last);
T_Last=millis()/1000.0;
// Serial.print(gx-gx_offest);Serial.print(",");
// Serial.print(gy-gy_offest);Serial.print(",");
// Serial.print(gz-gz_offest);Serial.print(",");
// Serial.print(ax-ax_offest);Serial.print(",");
// Serial.print(ay-ay_offest);Serial.print(",");
// Serial.print(az-az_offest);Serial.print(",");
delay(50);
Serial.print(Angle_GG);
Input=Angle_GG;
Input1=(-1)*(Input);
Serial.print("-");
myPID.Compute();

60. myPID1.Compute();
Salida=Output-Output1;
if(Salida<0){
Salida=(-1)*(Salida);
analogWrite(9, Salida);
analogWrite(10, 0);
analogWrite(5, Salida);
analogWrite(6, 0);
Serial.println("izquierda");
Serial.print("-");
Serial.println(Salida);
}
else{
analogWrite(10, Salida);
analogWrite(9, 0);
analogWrite(6, Salida);
analogWrite(5, 0);
Serial.println("derecha");
Serial.print("-");
Serial.println(Salida);
}
Serial.print("n");
}

61. Giroscopio
La comunicación realizada por el giroscopio
MPU6050 sigue el protocolo de comunicación I2C
donde explicaremos algunos términos de este
El bus I2C, un estándar que facilita la comunicación
entre microcontroladores, memorias y otros
dispositivos con cierto nivel de "inteligencia", sólo
requiere de dos líneas de señal y un común o masa.
Fue diseñado a este efecto por Philips y permite el
intercambio de información entre muchos
dispositivos a una velocidad aceptable, de unos 100
Kbits por segundo, aunque hay casos especiales en
los que el reloj llega hasta los 3,4 MHz.

62. Descripcion de las senhales:
O SCL (System Clock) es la línea de los pulsos
de reloj que sincronizan el sistema.
O SDA (System Data) es la línea por la que se
mueven los datos entre los dispositivos.
O GND (Masa) común de la interconección
entre todos los dispositivos "enganchados" al
bus.

63. Protocolo de comunicacion del
bus I2C
Habiendo varios dispositivos conectados sobre el bus, es lógico
que para establecer una comunicación a través de él se deba
respetar un protocolo. Digamos, en primer lugar, lo más
importante: existen dispositivos maestros y dispositivos
esclavos. Sólo los dispositivos maestros pueden iniciar una
comunicación.

64. Diagrama del circuito

65. Funcionamiento

66. Placa (etapa de potencia)

69. Diseno mecanico

70. Diseno mecanico

72. Implementacion

73. Implementacion