Segway - Robô Equilibrista PDF

Preview

Summary

Relatório final sobre um Segway realizado para a cadeira de Projeto I....

Description

-Meter uma secção resultados -Referências antes de anexo -Comentar resultados de simulação -Sumário antes do Índice

Universidade do Minho Escola de Engenharia Mestrado Integrado em Engenharia Eletrónica Industrial e Computadores Projeto I

Segway Robô Equilibrista Relatório final Professores: Filomena Soares Luís Louro Paulo Garrido Autores: João Guimarães, A80510 Edgar Baptista, A82131

Conteúdo 1 Sumário

1

2 Introdução 2.1 Motivação . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3 Tarefas realizadas . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2 2 2 2

3 Fundamentos Teóricos 3 3.1 Análise matemática do Modelo . . . . . . . . . . . . . . . . . . . . . 3 3.1.1 Modelo linear de um motor de corrente contínua (CC) . . . . 3 3.1.2 Modelo dinâmico linear de um pêndulo invertido com duas rodas 5 3.2 Parâmetros do robô . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 3.3 Resultados de Simulação . . . . . . . . . . . . . . . . . . . . . . . . . 7 3.4 Filtro digital para a MPU . . . . . . . . . . . . . . . . . . . . . . . . 9 3.4.1 O Filtro Complementar . . . . . . . . . . . . . . . . . . . . . . 9 3.5 Encoder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 4 Trabalho realizado 4.1 Material utilizado . . . . . . . . . 4.2 Estrutura física . . . . . . . . . . 4.3 Descrição do Hardware Utilizado 4.4 Aplicação Bluetooth . . . . . . . 4.5 Nível de equilíbrio atingido . . . . 4.6 Código desenvolvido . . . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

11 11 12 13 15 15 16

5 Conclusão e perspetivas futuras

18

A Anexo A.1 Código Simulação Matlab . . . . . . . . . . . . . . . . . . . . . . . . A.2 Código da placa ESP32 . . . . . . . . . . . . . . . . . . . . . . . . . . A.3 Diagrama de Gantt . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.4 Esquema Elétrico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.5 Outputs dos encoders no osciloscópio . . . . . . . . . . . . . . . . . . A.6 Medição das constantes dos Motores . . . . . . . . . . . . . . . . . . A.7 Despesas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.8 Código Arduino . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.8.1 main.ino . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.8.2 bluetooth.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.8.3 Encoder.cpp . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.8.4 Encoder.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.8.5 Motor.cpp . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.8.6 Motor.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.8.7 MPU_6050_config.h . . . . . . . . . . . . . . . . . . . . . . . A.8.8 SpeedControl.cpp . . . . . . . . . . . . . . . . . . . . . . . . . A.8.9 SpeedControl.h . . . . . . . . . . . . . . . . . . . . . . . . . . A.8.10 ultrasonic.h . . . . . . . . . . . . . . . . . . . . . . . . . . . .

19 19 21 22 23 24 25 26 27 27 29 30 32 33 34 34 36 38 39

i

Lista de Figuras 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16

Diagrama de blocos do modelo em estudo . . . . . . . . . . . . . . . . 7 Simulação para um ângulo inicial de 10° sem referência . . . . . . . . 7 Simulação para uma referência em degrau com amplitude 0.5 m/s com condições iniciais nulas . . . . . . . . . . . . . . . . . . . . . . . 8 Simulação para uma referência impulsional com condições iniciais nulas 8 Resultados do Filtro Complementar . . . . . . . . . . . . . . . . . . . 10 Output das saídas A(azul) e B(verde) do encoder conforme o sentido de rotação do motor . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 Estrutura física final do robô . . . . . . . . . . . . . . . . . . . . . . . 12 Arduino Mega 2560 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 Bateria 11.1V 900mAh LiPo . . . . . . . . . . . . . . . . . . . . . . . 13 Ultrassom HC-SR04 . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 MPU-6050 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 Transmotec SD3757-12-30-FEC . . . . . . . . . . . . . . . . . . . . . 14 Placa ESP32 WROOM32 . . . . . . . . . . . . . . . . . . . . . . . . 14 Driver L298N . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 Aplicação Bluetooth . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 Fluxograma . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

Lista de Tabelas 1 2

Nomenclatura . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Parâmetros do robô . . . . . . . . . . . . . . . . . . . . . . . . . . . .

ii

3 6

1

Sumário

O presente relatório apresenta um robô equilibrista desenvolvido no âmbito da UC de Projeto I. O objetivo principal do projeto foi desenvolver um robô que se equilibrasse sozinho, evitasse a colisão com obstáculos e permitisse ser comandado por uma referência transmitida por comunicação sem fios. Com a finalidade de atingir o objetivo pricipal, subdividiu-se o projeto em várias tarefas, com a seguinte ordem: Primeiramente, construiu-se a estrutura física do robot, constituída por três andares com placas de madeira. Testaram-se os componentes e, depois de comprovado o seu correto funcionamento, foram colocados na estrutura física. Concluída a estrutura, simulou-se o modelo do robot em ambiente Matlab, tendo por base um modelo adequado. Procedeu-se ao desenvolvimento e implementação do código que permitisse balançar o robot. Atingindo um equilíbrio satisfatório, utilizou-se uma aplicação no smartphone que permitisse o controlo de direção do robot via comunicação sem fios Bluetooth, e fez-se a sua integração com o código existente. Por fim, adicionou-se a componente de deteção e repulsão de obstáculos recorrendo aos sensores ultrassónicos instalados.

1

2

Introdução

2.1

Motivação

O problema do pêndulo invertido é um "clássico"problema da engenharia de controlo devido à sua instabilidade intrínseca. Bastantes projetos têm por base este problema, sendo um dos projetos mais conhecidos o Segway, um veículo de duas rodas de transporte individual que foi bastante popular. O grupo teve curiosidade na resolução do problema do pêndulo invertido a nível de hardware e implementação em microcontrolador o que, aliado ao interesse na construção de um robô, culminou na escolha deste projeto.

2.2

Objetivos

O projeto consiste no desenvolvimento de um robô com duas rodas que contemple as seguintes três funcionalidades, que também são os objetivos principais do projeto: • Equilíbrio autónomo; • Evitar colisões com obstáculos; • Controlo da direção e velocidade por comunicação sem fios.

2.3

Tarefas realizadas

De modo a atingir os objetivos propostos, subdividiram-se os objetivos em várias tarefas mais simples. De seguida, fez-se o planeamento do projeto de acordo com essas tarefas. O Diagrama de Gantt que reflete as tarefas e os seus respetivos prazos encontra-se no Anexo do Relatório.

2

3

Fundamentos Teóricos

O sistema consiste numa estrutura, alta e estreita, que se pode deslocar angularmente de forma livre, possuindo duas rodas nas suas extremidades inferiores. A estrutura tende a cair devido ao efeito da gravidade, sendo por isso considerado um sistema naturalmente instável. Para evitar isso, é adicionado um sistema de controlo que analisa a posição angular do pêndulo e move o robô horizontalmente para a frente ou para trás, de modo a que a ação da força gravítica na rotação da estrutura seja compensada.

3.1

Análise matemática do Modelo

O modelo matemático que se aproxima ao comportamento dinâmico real do robô construído foi baseado, na sua maioria, na tese de um aluno de uma universidade australiana, no âmbito da engenharia mecatrónica[1]. Nesta secção descreve-se as equações de movimento do pêndulo invertido bem como o modelo linear de um motor de corrente contínua. Variável i (A) ω (rad/s) θ (rad) ke (V·s/rad) km (Nm/A) Va (V) Ve (V) R (Ω) L (H) Ir (kg.m 2 ) kf (N ms/rad) τa (N m) τm (N m)

Significado Corrente da armadura Velocidade angular Ângulo Constante de tensão Constante de binário Tensão aplicada ao motor Tensão eletromotriz inversa Resistência da armadura Indutância do rotor Inércia do rotor Coeficiente de atrito do rotor Binário resistivo Binário do motor

Tabela 1: Nomenclatura 3.1.1

Modelo linear de um motor de corrente contínua (CC)

O robô equilibrista é atuado por dois motores CC. Quando é aplicada uma tensão aos terminais do motor é gerada uma corrente na sua armadura que produz um binário proporcional: τm = km ∗ i (1) Como os enrolamentos do rotor se movem num campo magnético é produzida uma força eletromotriz inversa, que pode ser aproximada como como função linear da velocidade angular do rotor: V e = ke ∗ ω

3

(2)

Aplicando a Lei de Kirchhoff podemos agora escrever a equação diferencial linear do circuito elétrico do motor CC: Va−R∗i−L∗

di =0 dt

(3)

Segundo a 2ª lei de Newton, a soma de todas as forças produzidas no eixo do motor é diretamente proporcional à aceleração do mesmo. Posto isto: X

F =

τa dω τm kf − − = dt Ir Ir Ir

(4)

Substituindo as equações 1 e 2 nas equações 3 e 4 obtem-se as duas equações fundamentais, em função da velocidade angular e da corrente, que governam o comportamento de um motor CC: Va R ke di ∗ω = − ∗i− L dt L L

(5)

kf τa dω km (6) = ∗i− ∗ω− Ir dt Ir Ir Para este caso é necessário apenas um modelo simples do motor, por isso desprezase o valor da indutância do motor: i=

V a ke − ∗ω R R

(7)

Juntando as equações 6 e 7 obtem-se uma equação apenas em função da velocidade angular do eixo do motor:   km ∗ V a km ∗ ke kf τa dω (8) = + − ∗ω− Ir dt Ir ∗ R Ir ∗ R Ir Representando esta dinâmica através de um modelo em espaço de estados obtemse as seguintes equações de estado e de saída:    dθ       0 1 Va 0 0 θ dt   + km (9) ∗ kf 1 dω = km∗ke − + ω τa 0 − Ir∗R Ir∗R Ir dt Ir        Va θ y= 1 0 ∗ + 0 0 τa ω 

4

(10)

3.1.2

Modelo dinâmico linear de um pêndulo invertido com duas rodas

O vetor de estado considera o ângulo da estrutura θ e a sua velocidade angular ω, bem como a velocidade do motor v como sendo as variáveis de estado.   v (11) x = θ  ω A equação de estado e as matrizes do modelo (14), onde u é a tensão aplicada aos motores.  A(1, 1) A(1, 2) 0 A= 0 A(3, 1) A(3, 2)   B(1) B= 0  B(3)

são apresentadas em (12), (13) e  0 1 0

(12)

(13)

dx =A∗x+B ∗u dt

(14)

Onde, 2 ∗ (Mp ∗ l ∗ r − Ip − Mp ∗ l2 ) ∗ A (1, 1) = α ∗ r2



km ∗ ke + kf R

M p2 ∗ g ∗ l2 α   km ∗ ke 2 ∗ (r ∗ β − Mp ∗ l) ∗ + kf A (3, 1) = α ∗ r2 R A (1, 2) =

A (3, 2) = B(1) =

Mp ∗ g ∗ l ∗ β α

2 ∗ km ∗ (Ip + Mp ∗ l2 − Mp ∗ l ∗ r) R∗r∗α

2 ∗ km ∗ (Mp ∗ l − r ∗ β) R∗r∗α   Iw 2 α = Ip ∗ β + 2 ∗ Mp ∗ l ∗ Mw + 2 r B(2) =



(15)

(16) (17) (18) (19) (20) (21)

2 ∗ Iw + Mp (22) r2 Neste modelo assumiu-se que as rodas estarão sempre em contacto com o chão e que não existe derrapagem das rodas. β = 2 ∗ Mw +

5

3.2

Parâmetros do robô

Na tabela seguinte são apresentados alguns parâmetros do robô construído: Parâmetro l H L r Mp Mw ke km kf R Ip Iw g

Descrição Distância entre o centro de massa e o eixo Altura Largura Raio da roda Massa da estrutura estática Massa das rodas Constante de tensão do motor Constante de binário do motor Coeficiente de atrito do motor Resistência de armadura do motor Momento de inércia da estrutura Momento de inércia da roda Aceleração gravítica

Valor

Unidade

0.045

m

0.3 0.44 0.0625

m m m

1.55

kg

0.3

kg

0.6

V·s/rad

0.6

Nm/A

0.008

—

1.2

Ω

0.0319

kg·m2

0.0003

kg·m2

9.8

m/s2

Tabela 2: Parâmetros do robô

Mp ∗ (H 2 + L2 ) Mw ∗ r 2 (23) Iw = 2 12 A resistência de armadura (R) foi medida experimentalmente para diferentes posições angulares do rotor, uma vez que as posições das escovas podem provocar variações do seu valor. As constantes de tensão, de binário e de atrito do motor também foram calculadas experimentalmente, tendo por base as seguintes equações: Ip =

km ∗ i − kf ∗ w V a − R ∗ i − ke ∗ w dω di (24) = = L Ir dt dt Aplicando ao motor uma tensão constante, a corrente consumida torna-se constante, assim como a velocidade angular. Isolando as variáveis a ser calculadas obtemos as seguintes expressões, com as quais conseguimos determinar os seus valores: km = ke =

R∗i+Va ω

6

kf =

km ∗i ω

(25)

3.3

Resultados de Simulação

Figura 1: Diagrama de blocos do modelo em estudo

Figura 2: Simulação para um ângulo inicial de 10° sem referência

7

Figura 3: Simulação para uma referência em degrau com amplitude 0.5 m/s com condições iniciais nulas

Figura 4: Simulação para uma referência impulsional com condições iniciais nulas

8

3.4

Filtro digital para a MPU

A MPU 6050 utilizada possui 6 graus de liberdade (3 acelerómetros e 3 giroscópios) e o seu objetivo é medir a posição angular (θ) do robô. A MPU fornece os seguintes valores, que são lidos pelos sensores e guardados nos seus registos: • Aceleração medida em m/s2 , em 3 eixos • Velocidade angular (ω) medida em °/s em 3 eixos. Integrando a velocidade angular (ω) ao longo do tempo conseguimos o ângulo (θ) dessa componente, enquanto que para a aceleração basta usar, por exemplo, a função atan2 sobre os vetores aceleração para obter o ângulo. Temos, então, duas componentes que fornecem o ângulo (θ), porém em ambos os casos existe um problema: • Acelerómetro - O facto de o robô se mover constantemente para a frente e para trás dificulta bastante a medida da aceleração gravitacional, pelo que as medidas vão ser bastante ruidosas. Sendo assim, só podemos confiar nos dados do acelerómetro a longo termo. • Giroscópio - Devido à integração ao longo do tempo, as suas medidas tendem a afastar-se (drift ), não retornando a zero quando o sistema volta à posição original. Sendo assim, as medidas do giroscópio apenas são confiáveis a curto prazo. Teremos, então, que aplicar um filtro digital que mitigue estes dois problemas: a imprecisão a curto prazo do acelerómetro e o drift a longo prazo do giroscópio. Investigou-se o filtro de Kalman e o filtro Complementar, optando-se por escolher o último por apresentar resultados idênticos ao filtro de Kalman neste caso, para além de ser um filtro mais fácil de compreender, implementar e mais económico em termos de recursos do microcontrolador. 3.4.1

O Filtro Complementar

O Filtro complementar é um filtro que mitiga os dois problemas mencionados anteriormente de uma forma bastante simples: aplica um filtro passa-alto digital ao acelerómetro e um filtro passa-baixo digital ao giroscópio. A equação é a seguinte: aˆngulo_atual = α.(ˆ angulo_anterior − aˆngulo_gyro) + (1 − α).ˆ angulo_acc (26) O parâmetro α determina o peso de cada componente e será determinado experimentalmente. Veja-se o exemplo seguinte: 0.75 τ = 0.9934 (27) = 0.75 + 0.005 τ + dt Para este caso escolheu-se um período de amostragem dt de 5ms e τ = 0.75s, que determina o intervalo de tempo a partir do qual o filtro passa-baixo deixa passar o sinal e até o qual o filtro passa-alto deixa passar o sinal. Para estas escolhas, o parâmetro α resultante é de 0.9934. Porém este resultado foi testado e não foi suficiente para mitigar o drift a longo prazo do giroscópio, optando-se por escolher um valor de α de 0.95 depois de efetuados mais testes. α=

9

Os resultados do filtro e os problemas mencionados podem ser verificados no gráfico que se segue:

Figura 5: Resultados do Filtro Complementar

3.5

Encoder

O encoder incremental tem duas saídas A e B, uma para cada sensor de efeito Hall. Sempre que um polo magnético passa pelo sensor de efeito Hall, aparece um pulso na saída do encoder, também chamado de “tick”. Os sensores estão separados 90º entre si, o que permite determinar a magnitude e direção da rotação do motor. Assim, se a saída A está adiantada da saída B (Fig. a), o motor está a rodar para a frente, enquanto que se a saída A está atrasada em relação à saída B (Fig. b), o motor está a rodar para trás. O microcontrolador irá contar todos os pulsos através de uma interrupção desencadeada pela transição de 0–>1 (rising edge).

(b) Saída A atrasada de B

(a) Saída A adiantada de B

Figura 6: Output das saídas A(azul) e B(verde) do encoder conforme o sentido de rotação do motor

10

4

Trabalho realizado

4.1

Material utilizado

Para este projeto utilizou-se vários materiais, os quais se seguem. O Diagrama elétrico do circuito total pode ser consultado no Anexo do Relatório. • Bateria Li-Po 11.1V 900mAh • Arduino Mega 2560 • ESP32 (WROOM-32) • 4 sensores ultrassom HC-SR04 • 2 motores Transmotec SD3757-12-30-FEC com encoder • MPU 6050 (GY-521) • Driver de motor L298N • Interruptor on-off • Resistências:4x 1KΩ; 1x 1,1KΩ; 1x 2,2KΩ • 2 Pontas crocodilo • Ligações roda-motor • 2 rodas • Madeira e 4 hastes metálicas • Breadboard e fios de ligação

11

4.2

Estrutura física

O robô possui uma estrutura dividida três andares feitos com placas de madeira e suportados por hastes metálicas: • No primeiro andar encontra-se a maioria do hardware. Por baixo, encontrase a ponte-H, a bateria e os motores com encoder e rodas acopladas. Por cima, encontra-se o microcontrolador, a MPU e o interruptor on-off. Colados na parte lateral da placa de madeira, encontram-se os quatro sensores de ultrassom, dois à frente e dois atrás • No segundo andar encontra-se a breadboard para ligações com as resistências, bem como o microcontrolador ESP32 • O terceiro andar, fora umas esponjas para proteção do robô em caso de contacto, encontra-se vazio e serve para porventura colocar algum objeto ou aplicar alguma perturbação no robô com a finalidade de testar a sua estabilidade. Na imagem que se segue pode-se visualizar a estrutura física final do robô:

Figura 7: Estrutura física final do robô

12

4.3

Descrição do Hardware Utilizado

Microcontrolador principal Para o microcontrolador que irá processar a informação recebida dos sensores e bluetooth e aplicar o algoritmo para o equilíbrio do robô, utilizou-se o Arduino Mega 2560. Devido à sua ubiquidade, foi mais fácil encontrar bibliotecas desenvolvidas para o hardware utilizado, para além de ser uma placa bastante acessível e com muitos pinos...