O robô equilibrista é um desses projetos que as pessoas leigas e programadores iniciantes olharão, mas não conseguirão enxergar toda a complexidade que há por trás. É preciso juntar diversos conceitos de física, matemática, eletrônica e programação para conseguir deixar o bichinho em pé.
Vou fazer um breve relato do que é necessário para se montar um com sucesso. Não vou passar um projeto detalhado, apenas os pontos em que você precisa prestar atenção. Mesmo por que, se você assistir ao vídeo vai perceber que eu apenas juntei as peças e coloquei pra funcionar. Depois eu deixo bonito. 🙂
Material necessário
– Motores: usei dois servos de rotação contínua, mas já fiz outro com motor DC. Obviamente o acionamento dos motores é diferente, mas não não impede o sucesso do projeto. Com servo motor fica bem melhor, há maior controle da rotação e mais suavidade.
– Arduino: usei o nano que é praticamente idêntico (não fisicamente) ao UNO. Então, nos dois vai funcionar.
– Rodas: claro, sem elas ele pára em pé mas também não sai do lugar. 🙂 Usei rodas de 6,5 cm de diâmetro. Essa informação é importante pois se você escolher rodas maiores pode ser que seu motor não forneça o torque necessário.
– MPU6050: este é o coração do projeto e o centro das complicações. Esse chip possui acelerômetro, giroscópio, termômetro e ainda um processador interno para tudo isso. Só que programar esse elemento é coisa pra gente grande. Por outro lado, obter os dados brutos dos sensores (raw values) é bem simples. No final, usar o MPU6050 no carro equilibrista é tipo matar mosquito com canhão, mas já vi alguns projetos muito bem feitos onde podemos perceber até onde dá pra ir.
Você pode achar interessante dar uma olhada no datasheet e no mapa de registradores.
Conecte o VCC no pino de 5 V do seu Arduino. O MPU6050 funciona com 3.3 V mas possui um regulador de tensão, então fica tranquileba que está tudo ok. Conecte o GND no GND do Arduino. O pino SCL (Serial Clock) deve ir no pino A5 (pino analógico 5) e o pino SDA (Serial Data) deve ir no pino A4 (pino analógico 4) do Arduino. Isto é correto para Arduino UNO e nano mas para outros Arduinos pode ser diferente. Descubra onde é o SDA e o SCL no seu Arduino!
– o resto: bom, fora o que eu já mencionei, você vai precisar de estrutura (madeira no meu caso) para sustentar tudo isso. Aí fica a seu critério o material que usará. Uma dica importante é tentar subir o centro de massa do carrinho, pois ele funciona como um pêndulo invertido e, dessa maneira, seu momento de inércia fica maior. Sendo assim, ele vai resistir mais às rotação no inicio, dando tempo do motor atuar na correção. Repare que no meu as baterias são em cima, pois são o maior peso. Depois que você montar o seu, me manda uma foto que eu quero ver. 🙂
Acelerômetro
Um acelerômetro, como o próprio nome diz, mede aceleração. Mas, a bem da verdade, ele mede força, faz um cálculo e fornece a aceleração. Para fins práticos, dá na mesma. É como uma balança de farmácia: mede o seu peso (que é uma força) e, com o cálculo feito na calibração, fornece a massa.
Pense num bloco preso numa mola em uma mesa horizontal. É mais ou menos assim: se você inclina a mesa para um lado, o bloco estica a mola que faz força no sentido contrário. Se você inclinar para o outro lado a mola vai ser comprimida e fazer força no sentido inverso.
Pois bem, é como se o MPU6050 tivesse três blocos presos em molas: um na direção X, outro na direção Y e outro na direção Z. Assim, se o MPU6050 estiver numa posição qualquer, as três molas podem estar fazendo alguma força. Porém todas essas forças estão relacionadas à força gravitacional, que é sempre vertical. Assim, o valor das forças é proporcional à inclinação em relação à vertical. Desta forma, se conhecermos as forças, podemos deduzir os ângulos de inclinação nas direções X, Y e Z.
A partir destes três ângulos podemos deduzir quanto o MPU6050 rotacionou ao redor de qualquer um dos eixos. Essas rotações são conhecidas como Roll, Pitch e Yaw, e muito utilizadas em aeronáutica. Veja um video.
Giroscópio
O giroscópio é um dispositivo que oferece resistência à mudança de direção do eixo em relação ao qual gira (queria usar essa palavra só pra enfatizar o nome giroscópio… 🙂 ). O MPU6050 fornece com que velocidade seu eixo de rotação está sendo alterado, nas direções X, Y e Z. Ora, os dados que ele nos fornece são velocidades, mas precisamos de ângulos. O que fazer? Vamos a um exemplo:
Suponhamos que você entre em um carro e comece a dirigir com velocidade de 100 km/h. Depois de duas horas, que distância você percorreu? Se você respondeu 200 km, acertou em cheio: se são 100 km percorridos a cada hora, em duas horas serão 200 km. E se fossem 5 horas? 500 km, certo? E assim por diante. O que você faz é simplesmente multiplicar a velocidade do carro pelo tempo de percurso para saber a distância. Da mesma forma, se o MPU6050 fornece a velocidade angular, para saber o deslocamento angular basta multiplicar pelo tempo da mudança.
Então anota ai: os dados do giroscópio devem ser multiplicados pelo tempo entre uma medida e outra para obtermos a variação do ângulo. Assim, você começa com um certo ângulo inicial (que pode ser o mesmo do acelerômetro) e vai acrescentando todas as variações com o passar do tempo para manter a medida no ângulo sempre atualizada. Mas não diga isso para sua namorada. Diga que a posição angular é conhecida a partir da integral temporal da velocidade angular. É bem mais elegante!
A princípio pensei que bastaria tomar os dados do acelerômetro para chegar ao equilíbrio: se o ângulo aumenta para um lado, aumenta a rotação do motor em uma direção. Se o ângulo aumenta para o outro lado, aumenta a rotação do motor no sentido contrário. Simples assim! E acreditem, eu tentei. Bastante. O começo do video é bem frustrante.
Depois de muito quebrar a cabeça descobri que o problema eram os valores que o acelerômetro fornecia. Há uma variação muito grande em seus valores e isso impede qualquer programa de trabalhar com os dados do jeito que queremos. Ao mesmo tempo descobri que trocar o acelerômetro pelo giroscópio não resolveria, já que o giroscópio tem tendência a deslocar a medida com o passar do tempo (drift), por causa da integral que fazemos.
A solução encontrada foi usar os dois sensores ao mesmo tempo para obter uma medida mais confiável dos ângulos medidos. É o que os gurus por ai chamam de fusão de sensores. Na minha época, fusão era isso:
Mas enfim, existem duas formas bastante usadas de fazer a fusão de sensores: a primeira chama-se filtro Kalman e a segunda, filtro complementar. As equações para o filtro Kalman são as seguintes:
Para o filtro complementar são essas:
ang = x*(ang+var_ang_giro)+y*(ang_acelero)
sendo x + y = 1.
Não sei muito bem explicar o que me levou a escolher o filtro complementar em lugar do filtro Kalman, mas deu certo!
Por último, as correções devem ser feitas por um algoritmo P.I.D., que é uma sigla para proporcional, integral e dirivativo. Vamos ver como funciona:
Nós queremos que a posição do robô seja sempre a mesma (em pé, de preferência…) mas ele tende a sair dessa posição (ou seja, cair…) com o passar do tempo. Sua posição é dada pelo ângulo medido no MPU6050 e, quando não for a que queremos, haverá um erro. Quanto mais longe da posição desejada, maior o erro na medida do ângulo. O que fazemos então é acionar os motores para corrigirem esse erro, de tal forma que o motor gire mais quanto maior for o erro, ou seja, fazemos uma correção proporcional ao erro medido. Esse é o P da sigla P.I.D.. Vamos colocar matematicamente:
correção = Kp*(erro)
Agora vamos ao I. Se você usar apenas a correção proporcional, vai perceber que o motor corrige o erro mas não necessariamente acerta o ponto que queremos. A correção proporcional em geral traz o seu sistema para alguma posição próxima à desejada, um pouco antes ou um pouco depois. Uma correção proporcional pequena não será suficiente para trazer o sistema de volta. E uma correção muito grande vai fazer o sistema passar do ponto. No nosso caso, uma correção grande vai fazer o robô ficar “sambando” ao redor da posição de equilíbrio. O que fazemos então é calcular a soma dos erros ao longo do tempo, já que ele está sempre errando. Assim, quanto mais tempo fora da posição desejada maior será a correção aplicada. Ora, como elegância é tudo na vida, não vamos dizer soma dos erros, vamos dizer integral do erro com o tempo. Matematicamente:
correção = Ki*∫(erro)*dt
E finalmente o D. Quando falamos que o robô fica “sambando” ao redor da posição de equilíbrio, isso se deve ao fato da correção não ser rápida o suficiente para mantê-lo no mesmo lugar. Resolvemos isso fazendo a correção proporcional à velocidade com que o erro varia. Assim, se ele é rápido para sair do lugar, haverá uma correção igualmente rápida para trazê-lo de volta. Se ele for lento para sair do lugar, a correção será menor mas ainda temos a correção proporcional… 🙂 . Podemos calcular a velocidade do erro medindo sua variação e dividindo pelo tempo em que ocorreu, da mesma forma que calculamos a velocidade de um carro nas aulas de física. Só que aqui vamos chamar de derivada do erro com o tempo (elegância é tudo…). Matematicamente:
correção = Kd*d(erro)/dt
E pronto. O cálculo P.I.D. para a correção da posição do robô será:
correção = Kp*(erro) + Ki*∫(erro)*dt + Kd*d(erro)/dt
Você vai gastar um bom tempo até encontrar os valores corretos de Kp, Ki e Kd. Seus valores são obtidos na prática para cada sistema. Chute alguns, teste e vá mudando até ficar bom. Sugiro que inicialmente deixe Ki = 0, Kd = 0 e encontre um valor de Kp em que o robô quase fique em equilibrio, sem “sambar” muito. Depois procure um Kd que reduza as oscilações. Por fim escolha um Ki que o mantenha sempre no ponto desejado.
É isso. Espero ter ajudado caso você esteja tentando montar o seu. É uma experiência fascinante. Se tiver alguma dúvida, comentário ou sugestão, não deixe de postar nos comentários. Até a próxima!
Explica melhor! 