miércoles, 25 de noviembre de 2015

Robot equilibrista con arduino, imu, pid, filtro kalman, bluetooth y rf.

El proyecto consiste en la fabricacion de un robot de dos ruedas que consiga mantener el equilibrio por si mismo con ayuda de un acelerometro y un giroscopio, ademas de un filtrado digital de la señal y un control PID y que pueda ser controlado por bluetooth desde el smartphone.











Gantproyect del tiempo estimado para cada una de las partes y tareas de este proyecto:





Los motores utilizados para este proyecto seran dos servos que habra que modificar para conseguir que puedan girar 360º.
En internet hay gran cantidad de tutoriales que enseñan como se realiza la modificacion, como por ejemplo este:  enlace o este otro enlace.

Fotos del proceso:











Para controlar los motores utilizare un puente en H.  El puente se basara en un integrado L298 que soporta 2 amperios de trabajo continuo y picos de hasta 3, y una tension de hasta 46 voltios.











Para realizar las primeras pruebas de estabilidad he construido un prototipo de chasis muy basico para probar los motores, ruedas, etc.  Proximamente el chasis ira evolucionando ya que esto es solo una pequeña prueba.











Para la parte de programacion lo primero que hare es implementar un filtrado digital de la señal del acelerometro y el giroscopio.     El sensor que utilizare es el GY-521, basado en el chip MPU-6050 que incluye acelerometro y giroscopio en el mismo integrado.





El filtrado digital consistira en un filtro de Kalman que hara mas estables las medidas y evitara medidas erroneas puntuales. El filtro de Kalman trabaja en dos etapas, en la primera realiza una estimación y en la segunda etapa corrige el error. En este caso para corregir la deriva del giróscopio se utiliza el acelerómetro o a la inversa.

 Aqui se puede ver una grafica hecha en procesing que muestra los ejes del acelerometro, del giroscopio y la salida del filtro de kalman con las 2 medidas ya combinadas en una que da un eje x y un eje y.

video

El codigo para implementar el filtro lo podeis encontrar aqui.



Ademas del filtro de Kalman tambien sera necesario implementar un control PID para el control de los motores.
Un PID es un mecanismo de control por realimentación que calcula la desviación o error entre un valor medido y el valor que se quiere obtener, para aplicar una acción correctora que ajuste el proceso. El algoritmo de calculo del control PID se da en tres parámetros distintos: el proporcional, el integral, y el derivativo.
La libreria PID para arduino que he utilizado se puede descargar de aqui.





Tras un rato de pruebas y otro buen rato de lucha con el pid probando diferentes valores de kp, ki y kd el robot ya se mantiene de pie aunque no es muy estable aun.

video





La estructura final ha sufrido varios cambios y ha quedado asi:






Actualmente es bastante estable como se puede ver en los siguientes videos:

video


video


video





El siguiente paso es intentar controlar la direccion y el giro mediante un modulo bluetooth y una aplicacion en android.





Tambien tengo la intencion de controlar el robot desde procesing con un modulo rf a la vez que se reciben los datos del acelerometro y el giroscopio y se representan en una grafica.






Despues de la implementacion del modulo bluetooth y de un poco mas de programacion el robot ya se puede controlar desde el telefono.

video




Despues de todo esto aun me sobro un poco de tiempo antes de la fecha de entrega del proyecto asique se me ocurrio ponerle 2 servos, uno a cada lado del robot y acoplarles un trozo del mismo material que la base a modo de "brazos", con la idea de poder hacer que se levantase solo si se cae.
El resultado es este:

video


Tras esto doy por finalizado el proyecto, yo diria que con bastante exito, al menos mas del que yo mismo esperaba.

19 comentarios:

  1. Está claro que para este proyecto te hará falta un controlador PID y algún tipo de filtrado digital.

    ResponderEliminar
  2. Muy buen proyecto, te agradecería si me puedes ayudar a interpretar los datos que me entrega el código para el MPU6050 con filtro de kalman que incluyes. Lo he ejecutado en mi arduino e imprime los datos por monitor serial según esto:

    Serial.print(roll); Serial.print("\t");
    Serial.print(gyroXangle); Serial.print("\t");
    Serial.print(compAngleX); Serial.print("\t");
    Serial.print(kalAngleX); Serial.print("\t");

    Serial.print("\t");

    Serial.print(pitch); Serial.print("\t");
    Serial.print(gyroYangle); Serial.print("\t");
    Serial.print(compAngleY); Serial.print("\t");
    Serial.print(kalAngleY); Serial.print("\t");

    Hasta donde entiendo son los los ángulos en X y Y sin filtro y con el filtro, pero físicamente no parece tener mucha relación, inclino en x aproximadamente a 90 y por el monitor serial muestra un dato muy cercano, y creo no haber problema, ahora en Y hago lo mismo pero nunca llega a 90, si trato de nivelar el sensor tampoco, los valores seriales en 0 (cero) no corresponden con la realidad, que podría estar mal, sera que estoy interpretando mal los datos.

    ResponderEliminar
  3. Enhorabuena por tu trabajo, está realmente bien.

    La librería del filtro de Kalman que has puesto está muy bien organizada y es fácil de usar. La salida con ésta me sale mejor que con otro filtro que probé, pero tarda un poco en converger hasta 0 cuando vuelvo a la posición inicial, ¿Esto es normal o quizás está mal el giróscopo que utilizo?

    Saludos desde la UPCT

    ResponderEliminar
  4. Gracias por compartir este gran proyecto amigo, tengo algunos componentes que mencionas en tu articulo y tengo planeado armar uno. Soy estudiante de ing. electronica y bueno la idea es realizar un proyecto de robot equilibrista con sensores de obstaculos. Nueva cuenta gracias amigo !!!

    ResponderEliminar
  5. Hola me encantaria que me pasaras el codigo completo del arduino y todos los componentes por favor estoy intentando realizar un proyecto y este esta muy bien

    ResponderEliminar
    Respuestas
    1. Tienes los enlaces para poder descargar todo el código y construir tu propio robot.
      Un saludo.

      Eliminar
    2. Yo sólo he encontrado el código de Kalman y librería de PID, pero el código que controla ambas cosas no lo encuentro.

      Gracias de antemano, y gran trabajo!

      Eliminar
  6. Este comentario ha sido eliminado por un administrador del blog.

    ResponderEliminar
  7. Hola me podrías decir que es un filtro de Kalman, si es un componente mas que hay que incorporar al arduino o es solo la programación?

    ResponderEliminar
    Respuestas
    1. Muy bien gracias. Tambien me gustaria saber que tipo de motores has utilizado, modelo para saber cuales comprar. Y si no es mucho pedir y me pudiera mandar el codigo de arduino, todo en un archivo, es que la verdad no tengo muchos conocimientos sobre esto y me cuesta trabajo interpretar donde va cada cosa Gracias. Un saludo.

      Eliminar
    2. Parece que ha tenido éxito el proyecto. Hace mucho tiempo que no lo miro y no esperaba encontrarme tantos comentarios y preguntas.

      Eliminar
    3. Sí ha tenido mucho éxito. A ver si te da algún fruto ;)

      Eliminar
    4. Si... voy a vender la patente y de esta me jubilo :)

      Eliminar
  8. Hola me podrías decir si el filtro de Kalman es un componente mas que se incorpora al arduino o solo es programación?

    ResponderEliminar
  9. Hola me podria decir si es usted tan amable el tipo de motores que utiliza para comprar los mismos que usted.

    ResponderEliminar
    Respuestas
    1. Use unos similares a estos: http://www.complubot.com/shop/index.php?id_product=96&controller=product&id_lang=1

      Eliminar
  10. Hola mire tengo una mpu 6050 igual a la de a foto de este blog. La conexion la tengo por cuatro patillas que son:
    VCC = 5V
    GND = TIERRA
    SCL = A5
    SDA = A4
    Me gustaria saber si para implementar el filtro kalman que usted utilizó me faltaria conectar alguna patillas mas y si es así me podria decir cual es. Gracias.

    ResponderEliminar