Desarrollo de Algoritmos de Control y Navegación "In Home" para Vehículos Autónomos.
Development of Control and Navigation Algorithms "In Home " for Autonomous Vehicles.
Gustavo Pérez Parra1, Mirko Montesinos Cortez1, Erlan Medrano Cruz1 y Francisco J. Triveño Vargas2.
1Facultad de Ciencias y Tecnología, Universidad Mayor de San Simón, Cochabamba Bolivia.
2Universidade de Araraquara, UNIARA, Rua Carlos Gomes, 1338, Centro Araraquara-SP, Brasil.
Recibido: 26 de noviembre 2019
Aceptado: 23 de agosto 2020
Resumen. Este proyecto presenta una contribución al estudio de sistemas de control y navegación de vehículos autónomos. Son presentados los materiales utilizados para la realización de la investigación como sensores, microprocesadores, actuadores y los vehículos empleados. También son presentadas las estructuras de los algoritmos implementados "In Home". Finalmente, son presentados los resultados iniciales obtenidos. El proyecto contempló el montaje de los circuitos electrónicos, proyecto mecánico, teoría de control y navegación, e integración de tal forma que garanticen que las acciones deseadas sean ejecutadas con éxito. El desarrollo fue posible debido al uso de diferentes tipos de software. Es el primer trabajo de esta magnitud en toda Bolivia.
Palabras clave: Navegación y Control, Vehículos autónomos, Sensores, Actuadores, Microprocesadores.
Abstract. This project proposes a contribution for the study of control and navigation systems for autonomous vehicles. The materials used in the research as microprocessor, sensors, actuators and the vehicles are presented. All algorithms implemented "In Home" is presented also. Finally, are presented the initial results obtained successfully. The project included assembly of the electronic circuits, mechanical project, control and navigation theory and integration in a way that guarantees that the desired actions are executed successfully; the development was possible due to the use of different software. It is the first work of this magnitude in all Bolivia.
Keywords: Navigation and Control, Autonomous Vehicles, Sensors, Actuators, Microprocessors.
]]>
1 INTRODUCCIÓN - ANTECEDENTES.
En estos últimos años, el desarrollo de vehículos autónomos ya sea terrestres, acuáticos o aéreos ha tenido un avance significativo, por ejemplo, los vehículos aéreos no tripulados (VANTs) o UAV (Unmanned Aerial Vehicle) son ampliamente utilizados para una serie de aplicaciones civiles y militares. Entre las aplicaciones militares se destacan, tareas de vigilancia, monitoreo, interceptación de comunicaciones, lucha contra el narcotráfico y en algunos casos la entrega o despejo de armas. Nuevos paradigmas están siendo propuestos en el área militar por lo que, su uso tiende a aumentar. Paralelamente, en aplicaciones civiles son usados en misiones humanitarias o para el alivio de desastres naturales, monitoreo de áreas de preservación o cultivos específicos, vigilancia de oleoductos gasoductos o mantenimiento de líneas eléctricas entre otras (Endri R. and Radzuan R., 2011).
Las investigaciones relacionadas a los vehículos aéreos no tripulados se han incrementado, principalmente debido a sus dimensiones compactas, bajo costo y riesgo limitado de daños o perjuicios en caso de errores de proyecto. Como plataformas de ensayos en vuelo, estos vehículos se tornaron populares para investigación de áreas como: control, aerodinámica, motores, estructuras y software embarcado entre otros.
De la misma forma vehículos terrestres autónomos vienen siendo utilizados en tareas donde el entorno es de difícil acceso o representa algún peligro, como, por ejemplo: tareas de búsqueda de sobrevivientes de desastres, estudio de volcanes, etc. Los vehículos autónomos son considerados los vehículos del futuro por eso varias empresas realizan investigaciones e inversiones en el área de navegación autónoma. Algunos ejemplos son: General Motors, Toyota, Audi y Volvo. Los primeros prototipos de estos vehículos robotizados, capaces de reaccionar a su entorno, se probaron en 2007 en el DARPA Urban Challenge, en el marco de un concurso organizado por la DARPA (Agencia de Investigación Avanzada en Defensa) Hai Yang C, et al.,2010).
En este contexto, este proyecto tiene como objetivo, la implementación de algoritmos propios de control y navegación tanto en vehículos autónomos terrestres y aéreos. Esto permitirá la adquisición de conocimientos sobre mecánica vuelo, aerodinámica, software embarcado, telemetría, control y otros. Paralelamente por tratarse de vehículos de configuración abierta, es posible la implementación de algoritmos propios y la absorción completa de esta tecnología a través de la implementación de las estaciones de tierra que serán enteramente desarrolladas por primera vez en Bolivia.
2 MATERIALES
2.1 Sensores y tratamiento de señales.
El desarrollo de algoritmos de control y navegación es posible gracias al desarrollo de microcontroladores, que ofrecen la posibilidad de hacer cálculos muy complejos en poco espacio, a la miniaturización de los sensores (MEMS - Micro Electromechanical Systems) así como las mejoras en los sistemas de almacenamiento de energía. La Figura 1 presenta algunos de los sensores empleados en este trabajo.
]]>
El cerebro empleado en este trabajo corresponde al Arduino 2560. Algunas de sus especificaciones son presentadas en la Tabla 1.
Uno de los sensores empleados en este trabajo corresponde al MPU9250 que contiene acelerómetros, giroscopios, magnetómetros y un barómetro, estos sensores hacen parte de los sistemas inerciales de navegación (INS Inertial Navigation Systems), empleados para determinar la orientación del vehículo con respecto a un sistema de referencia, determinar velocidades lineales y altitudes. La Tabla 2 presenta algunas de las especificaciones de este sensor:
Paralelamente, la combinación de sensores para mejorar la precisión y la salida de estos es una práctica común en diferentes industrias. Algunos métodos para aplicar la fusión de sensores están disponibles con diversos grados de complejidad. Un filtro complementario es una forma sencilla de combinar sensores. El principio de este filtro es presentado en la Figura 2.
]]>
2.2 Variables o parámetros.
La rotación de un cuerpo rígido con respecto a un espacio en marco de referencia fijo se puede representar con los ángulos de Euler como presentado en la Figura 3. En este diagrama podemos verificar los ángulos de Euler (ver Tabla 3) así como las velocidades lineales que dan a un vehículo en forma general 6 grados de libertad.
2.3 Vehículo Terrestre.
El vehículo terrestre empleado para la implementación de los algoritmos de control y navegación es presentado en la Figura 4 (este vehículo fue adquirido con recursos propios y donaciones de las empresas Ecojet y Neuron de la ciudad de Cochabamba y Santa Cruz respectivamente).
]]>
2.4 Vehículos Aéreos.
Los dos vehículos aéreos de estudio un Cuadri-rotor y un avión de ala fija.
Para la modelación 3D del Cuadri-rotor presentado en la Figura 5 se empleó el Software SolidWorks 3D CAD (representación virtual), la configuración del Cuadri-rotor es del tipo "X", este tipo de configuración contempla una simetría con respecto al centro de masas, centro geométrico, permitiendo solo tomar momentos inerciales diagonales de la matriz de inercias, los motores por otro lado generar momentos inerciales, sin embargo, este se soluciona compensando la rotación de dos motores en sentido horario y otros dos en sentido antihorario, variando ajustadamente la velocidad de los motores se puede rotar el cuerpo del Cuadri-rotor en vuelo con respecto al eje "Z" conocido como movimiento de guiñada o yaw.
Los materiales empleados en la construcción del Cuadri-rotor son partes prototipadas en impresora 3D y madera balsa (usado en la construcción de aeromodelismo) su baja densidad nos permite reducir el peso y por tanto introducir menor potencia en vuelo, mejorado así la autonomía del Cuadri-rotor.
El vehículo aéreo empleado para la implementación de los algoritmos de control y navegación es presentado en la Figura 6. Este vehículo fue una donación importada de la china.
]]>
2.5 Telemetría y Estación de Tierra
La telemetría y la estación de tierra están encargadas del recibimiento y procesamiento de las señales de los sensores embarcados en los vehículos y paralelamente del envió de comandos y variables a dichos vehículos. La estación de tierra permite el gerenciamiento de las tareas del vehículo de forma más amigable. La Figura 7 presenta algunas de las antenas empleadas, así como una de las estaciones de suelo desarrolladas.
3 MÉTODOS-ALGORITMOS DE NAVEGACIÓN Y CONTROL
El piloto automático puede funcionar de forma independiente, controlando la proa y la altitud, o puede acoplarse a un sistema de navegación y ejecutar un curso programado o una aproximación. Las principales funciones del piloto automático se pueden dividir en dos: orientación y control (Vargas, F. J. T. y Paglione, P., 2015):
Orientación: la acción de determinar el rumbo y la velocidad, en relación con algún sistema de referencia, que debe seguir el vehículo.
]]> Control: El desarrollo y la aplicación de fuerzas y momentos apropiados para el vehículo, para:• Establecer algún estado de equilibrio del movimiento del vehículo,
• Restaurar un vehículo perturbado a su estado de equilibrio (punto de operación) y/o regular, dentro de los límites deseados, su salida de las condiciones del punto de operación.
Los lazos de control aseguran una respuesta rápida y estable de la aeronave a los comandos, creados por los bucles de guiado. También deben eliminar la influencia de perturbaciones tales como la turbulencia atmosférica. La estructura de control implementada es presentada en la Figura 8.
El sistema de piloto automático de dos ejes se instala en la mayoría de los controles generales de la aeronave (cabeceo θ y balanceo Φ).
3.1 Algoritmo de Control
Una de las técnicas más difundidas en el área de control y automatización es la estructura PID (Proporcional Integral Derivativo) (Samir Bouabdallah, A. N. ,2013). Este controlador es una de las técnicas más populares cuando se desea realizar el control de una determinada planta. El control PID consiste en un algoritmo matemático, que tiene por función el control preciso de una variable en un sistema, permitiendo al sistema operar de forma estable en el punto de ajuste deseado, frente a variaciones o disturbios que afecten su estabilidad. La Figura 9 ilustra el controlador implementado en su forma digital (adicionar que esta no es la única estructura, existen modificaciones en esta estructura).
]]>
3.2 Algoritmo de Navegación
La navegación se ha vuelto mucho menos laboriosa con el advenimiento del GPS. La forma más segura de navegar utilizando el GPS es a través de rutas planificadas por adelantado, pues así en el transcurso de una determinada ruta se dispondrá de varias informaciones para llegar destino. La navegación por waypoints es empleada en diferentes tipos de vehículos ya sea terrestre o aéreo (Austin M. Murch, Y. C, 2013). La Figura 10 presenta el algoritmo de navegación implementado, donde se verifica que el mismo depende de la velocidad de tierra, la proa del vehículo y el Cross Track error, (XTE) esta última variable depende de la geometría terrestre, así como de la latitud y la longitud leídas a partir del GPS.
Cross Track Error (XTE) se define como: "la distancia desde la posición actual del vehículo al punto más cercano en una línea entre los puntos de referencia de origen y destino de la ruta activa." (Kleber Lima da Silva, A. S., 2013). La Figura 11 presenta este concepto.
]]> 4 RESULTADOS INICIALES
4.1 Tratamiento de señales
La comunicación entre el sensor y el controlador es mediante I2C, en Fast-Mode según el Datasheet para la comunicación con todos los registros es de 400 kHz. Los sensores se configuran de acuerdo a las necesidades de trabajo, según el Datasheet de cada sensor las lecturas vienen acompañadas de una resolución ADC 16 bits (para cada eje del giroscopio y acelerómetro), sensibilidad ajustable y frecuencia de 8kHz para el giroscopio y 1kHz para el acelerómetro. La frecuencia de muestre o la solicitud de datos a los registros del controlador del sensor se determinó a 200Hz.
La sensibilidad escogida para el acelerómetro fue de +8, por tanto, corresponde a 4096 LSB/g, dividiendo los valores raw (brutos) que obtenemos de los registros del acelerómetro y dividió entre 4096 obtenemos la aceleración en unidades m/s^2, de la misma manera para el giroscópico, se escogió una sensibilidad intermedia de 65.5 LSB/(°/s), dividiendo los valores raw obtenidas de los registros entre 65.5 LSB/(°/s) obtenemos la velocidad angular.
La aceleración obtenida de acelerómetro es lineal, ubicada en el centro geométrico y centro de masa del Cuadri-rotor estos datos representaran la aceleración lineal del cuerpo, para poder obtener la rotación efectuada del cuerpo en términos del marco de referencia inercial empleamos matrices de rotaciones sucesivas.
El orden de multiplicación de las ecuaciones 1, 2 y 3 llevan distintos nombres, sin embargo, debido a que el acelerómetro mesura la aceleración de la tierra, es decir solo aceleraciones en el eje Z podemos obtener la inclinación del Cuadri-Rotor, considerando la siguiente matriz Rg = (Rgx Rgy Rgz), donde Rgx, Rgy = 0, y la simplicidad que presenta los ángulos de Tait-Bryan multiplicando 1, 2 y 3 se obtiene.
Relacionando los ángulos Φ y θ con las aceleraciones muestreadas por el acelerómetro en los 3 ejes obtenemos la siguiente expresión.
En el caso del giroscopio, se tiene la velocidad angular en los 3 ejes, en tiempo discreto para obtener la posición angular debemos integrar, mediante la siguiente expresión.
De este modo obtenemos los ángulos mediante el giroscopio.
4.2 Calibración Sensores.
Todo instrumento de medición debe ser calibrado y puesto a prueba en diferentes condiciones para considerar cuando los datos deben ser considerados erróneos y como tratarlos, una característica de los instrumentos de medición electrónico es el ruido térmico, sumado a otros errores de obtención indirecta de los datos de interés el controlador entrar en un ciclo de oscilación desestabilizando los sistemas (vehículos).
Tanto el giroscópico como el acelerómetro se calibraron en condiciones de medición en estado de reposo, con excepción del eje Z del acelerómetro, este debe corresponder a las aceleraciones de la gravedad.
La calibración consiste en llegar a un valor de comparación entre el instrumento a calibrar y el patrón de medida, en este caso una manera de ajustar el valor de patrón para el giroscopio es calibrar una estación de soporte para el sensor con instrumentos de nivel, sin embargo, bajo el contexto en el que se desarrolla dicho experimento in-home la calibración de la estación de soporte se realiza con sensores de móviles.
El giroscopio para sus 3 ejes se puede observar que presenta valores fuera de la media ajusta al soporte 0o, para eliminar este error se obtiene la media del conjunto de datos muestreados y este valor seria como valor de calibración, en la Figura 12 se tiene para el caso del ángulo θ del giroscopio.
]]>
Para el caso del acelerómetro se empleó la siguiente metodología de normalización por ganancia.
Obteniendo los valores brutos del acelerómetro obtenemos el offset.
Para este caso de normalización nos interesa más cuando la aceleración sea positiva, por tanto.
De esta manera normalizamos cualquier dato obtenido por el acelerómetro, en la Figura 14 se tiene para el caso del eje X.
]]>
La calibración de los sensores, previo a su uso para el cálculo de los ángulos nos permite redundar en los datos, tanto del giroscopio como del acelerómetro, para el caso del giroscópico es necesario encontrar un valor medio correcto y calibrar cada vez que el sistema comience a trabajar, esto debió a que esos valores pueden cambiar en el tiempo, recordando también que dichos datos son integrados en tiempo discreto, lo que podía incurrir en una deriva del valor real.
Los datos obtenidos por ambos tipos de sensores, posterior a la obtención de los ángulos, son filtrados y combinados por medio de un filtro complementario. Se obtiene las siguientes lecturas haciendo uso del software LabVIEW, Figura 15.
Para la obtención de estos resultados fueron empleados el filtro complementar presentado y el filtro de Kalman (Faragher, R., 2012.) a ser presentado en el trabajo final, la nomenclatura presentada y diferentes matrices de rotación.
]]>
4.3 Control Cuadri-Rotor
El modelo matemático desarrollado en Matlab-Simulink® para la simulación del cuadri-rotor así como para el cálculo de las ganancias es ilustrado en la Figura 18.
Las ganancias iniciales son obtenidas generando un escalón como entrada para cada uno de los ángulos de orientación. Las Figuras 19-21 ilustran los resultados obtenidos.
4.4 Código embarcado
Los algoritmos de control fueron embarcados en la placa 2560, posterior a eso se hizo un balance de hélices para evitar mayor vibración y error en las lecturas. Se construyó un banco de pruebas donde se hizo el teste de equilibrio obteniendo el resultado previamente descrito con el modelo matemático. La Figura 22 presenta los resultados de teste u ensayo en vuelo.
4.5 Navegación y estación de tierra
El mapa implementado para realizar la navegación vía waypoints y una de las estaciones de tierra implementadas es ilustrado en la Figura 23.
]]>
En la Figura 23 se puede verificar el camino programado a partir de la estación de tierra para que el vehículo terrestre realice una determinada tarea, al lado se puede observar la estación de tierra con el reloj de orientación y nivel artificial. Es importante resaltar que todos los algoritmos presentados hasta aquí fueron desarrollados enteramente en Bolivia.+
5 CONCLUSIONES
El proceso de calibración de los sensores de mpu10DOF fue uno de los retos más fuertes, debido a la calibración del magnetómetro (sensor gy-87), la mayor dificultad era poder usar el algoritmo de calibración para el magnetómetro y comprender los registros a cuál se debería llamar para su correcto funcionamiento.
La fusión sensorial del acelerómetro y el giroscopio para el desarrollo del roll, pitch, con el filtrado de las señales, por medio del filtro de Kalman el cual nos dio lecturas aceptables.
Para la corrección de error del GPS se realizó un filtrado y unión sensorial entre el dato del GPS con el acelerómetro demás de darle precisión, se modificó los registros del GPS para la obtención de datos en el tiempo necesario para la correcta adquisición de datos.
Se realizó bastantes conexiones seriales i2c para poder tener la comunicación sensorial, y por el bajo rendimiento de Arduino se procedió a usar dos Arduinos que trabajan conjuntamente con toda la lectura sensorial.
La comunicación de las antenas apc220v3 su mayor dificultad fue poder enlazarlas y poder tener los mismos registros del receptor y del emisor y evitar la menor cantidad de datos perdidos durante él envió.
]]> El problema más grande el cual se tuvo fue en la elaboración del piloto automático, el algoritmo y su implementación además de tener que modificar todos los distintos tiempos de muestreo de datos ya que no coincidían, ya que el Arduino es una placa de desarrollo de alto nivel el cual para poder modificar a gusto algunas cosas se debe entrar a los registros más profundos modificamos el reloj de cuarzo de lectura y los puertos por donde no se debería de procesar mientras se realiza alguna tarea.Aún se está trabajando en una placa stm32f3 que incluye un acelerómetro, giroscopio y magnetómetro también.
Para la interface de vuelo, es aplaudible tener un novedoso instrumento de vuelo 3D-tridimendional, ya que de esa manera la alteración en el cuerpo regido por los controles mecánicos se refleja con mayor interactividad al usuario, saliendo del clásico 2D en instrumentación.
Como mencionado en la Introducción es el resultado de aproximadamente 3 años de investigación para implementar nuestros propios algoritmos. Este es el primer trabajo de investigación de esta magnitud en toda Bolivia
Agradecimientos
Agradecer de sobre manera a todos aquellos que contribuyeron de alguna manera para la realización de esta investigación.
Referencias
[1] Vargas, F. J. T. e Paglione, P. (2015). Ferramentas de Álgebra Computacional: Aplicações em Modelagem, Simulação e Controle para Engenharia. (1era Edição) LTC, Brasil.
]]> [2] Endri Rachman and RAdzuan Razali (2011). A Mathematical Modeling for design and Development of Control Laws For Unmanned Aerial Vehicle (UAV). International Journal of Applied Science and Technology. Vol. 1 No. 4.[3] Amahah, J. (2009). The Design of an Unmanned Aerial Vehicle Based on the ArduPilot. [ Links ]
[4] Brandon McCarron. (2013). Low-Cost IMU Implementation via Sensor Fusion Algorithms in the Arduino Environment. Bachelor in Science Work.
[5] HaiYang Chao, YongCan Cao, and YangQuan Chen (2010). Autopilots for Small Unmanned Aerial Vehicles: A Survey. International Journal of Control, Automation, and Systems
[6] Samir Bouabdallah, A. N. (2013). PID vs LQ Control Techniques Applied to an. 6. [ Links ]
[7] Faragher, R. (2012). Understanding the Basis of the Kalman Filter. IEEE signal Processing Magazine. [ Links ]
[8] Kleber Lima da Silva, A. S. (2013). Hardware para Controle Avançado de um Veículo Aéreo Não Tripulado do tipo Quadricóptero. XI CEEL, Minas Gerais. [ Links ]
[9] Karwoski1, K. (s.f.). Quadcopter Control Design and Flight Operation. NASA USRP – Internship Final Report.
[10] Ying-Chih Lai, Shau-Shiun Jan and Fei-Bin Hsiao. (2010). Development of a Low-Cost Attitude and Heading Reference. Sensors Journal.
]]>