RESUMEN : En los últimos años ha crecido el interés por el desarrollo de vehículos autónomos por las posibilidades que brindan para el cumplimiento de misiones en lugares de difícil acceso, en tareas de reconocimiento, estudio de ecosistemas; y en otras ramas importantes como la agricultura. En Cuba, el Grupo de Automática, Robótica y Percepción de la Universidad Central de las Villas, de conjunto con otras instituciones, tiene como objetivo desarrollar autopilotos para vehículos autónomos. En este artículo se presenta una solución de navegación basada en el Filtro Extendido de Kalman. Se fusionan de forma indirecta las mediciones de sensores inerciales de bajo costo con mediciones de GPS para estimar la estimar la posición y velocidad de un vehículo autónomo. Se abordan como valor agregado las ecuaciones fundamentales para la implementación de un filtro complementario para estimar la orientación del vehículo, de importancia notable para la navegación. La simulación se realizó en Matlab, haciendo uso de datos reales de navegación de un vehículo subacuático. Los resultados muestran que la estimación de los parámetros de navegación es aceptable para este tipo de aplicación.
Palabras clave: Filtro de Kalman ExtendidoFiltro de Kalman Extendido,integración INS/GPSintegración INS/GPS,parámetros de navegaciónparámetros de navegación,estimación de orientaciónestimación de orientación.
ABSTRACT : Interest in the development of autonomous vehicles has grown in recent years, due to the possibilities they offer for the accomplishment of missions in places difficult to reach, in reconnaissance tasks, in the study of ecosystems, and in other important branches such as agriculture . In Cuba, the Group of Automatation, Robotics and Perception of the Universidad Central Marta Abreu de las Villas, together with other institutions, it is intended to develop autopilots for autonomous vehicles. This paper presents a navigation solution based on the Kalman Extended Filter. Low-cost inertial sensor measurements are indirectly fused with GPS measurements to estimate the position and speed of an autonomous vehicle. The fundamental equations for the implementation of a complementary filter to estimate the orientation of the vehicle, very important for navigation, are addressed as added value. The simulation was performed with real data of an underwater vehicle, using Matlab. The results show that the estimation of the navigation parameters is feasible for this type of application.
Key words: Extended Kalman Filter, INS/GPS integration, navigation parameters, attitude and heading estimation.
Artículo Original
Algoritmo de navegación integrada para vehículos autónomos con tecnología de bajo costo.
Integrated navigation algorithm for autonomous vehicles with low cost technology.
Editorial Ediciones Futuro
Recepción: 22 Abril 2018
Aprobación: 09 Julio 2018
En los últimos años ha crecido el interés por el desarrollo de vehículos autónomos, debido a las posibilidades que brindan para el cumplimiento de misiones en lugares de difícil acceso, o tareas de reconocimiento, supervisión y vigilancia; estudio de ecosistemas, además de las aplicaciones tradicionales en el campo militar (Abraham et al., 2017),(Amer et al., 2017), (Bayat et al., 2017), (Mandujano, 2017), (Ibáñez, 2017). En la agricultura se reportan importantes aplicaciones, como la fotogrametría aérea, la agricultura de precisión, donde combinan vehículos aéreos y terrestres (Conesa-Muñoz et al., 2015),(Santana et al., 2017), (Gutierrez et al., 2018), (Valverde et al., 2018). El desarrollo de sistemas y algoritmos de navegación es un área relevante que atrae a muchos investigadores, por la necesidad de estimar con precisión la posición y orientación en cada instante de tiempo. Existen numerosos productos en el mercado internacional con capacidad de brindar soluciones de navegación, pero son en su mayoría de código propietario e inaccesibles por sus altos costos.
Los vehículos autónomos típicamente dependen del sistema de posicionamiento global (Global Positioning Systems, GPS) para proveer información de posición y velocidad para la navegación. Su uso ha sido ampliamente estudiado en los últimos años (Barth and Farrell, 1999). Hay situaciones en las que el GPS no está disponible, por interferencias, por los efectos de apantallamiento o multitrayecto, por reflexión de la señal, siempre en dependencia del medio de navegación (Afia et al., 2015),(Ryu et al., 2016). La baja frecuencia de muestreo es un problema que afecta la disponibilidad de información de navegación para las acciones de control sobre el vehículo. En otro sentido el desarrollo de la tecnología de los sistemas micro-electro-mecánicos (Micro-electromechanical systems, MEMs) ha hecho posible la construcción de sensores inerciales y unidades de medición inercial (Inertial Measurement Units), cada vez más baratos y rápidos. Los sistemas de navegación inercial (Inertial Navigation Systems, INS), si bien tienen altas tasas de muestreo, y alta disponibilidad, la solución de navegación con esta tecnología diverge polinomialmente en el tiempo, por la integración sucesiva de errores en las mediciones, y por imprecisiones en las condiciones iniciales (Salychev, 2012).
Es muy popular la integración de estas dos tecnologías para mitigar mutuamente sus deficiencias, y garantizar mayor precisión en la navegación (España, 2010), (Quan et al., 2015), (Kang et al., 2018). La navegación integrada como tecnología, permite la fusión de múltiples fuentes de información para incrementar la precisión en la navegación. Varios autores han contribuido al desarrollo de algoritmos de navegación integrada, con un conjunto mínimo de sensores para mantener las condiciones de bajo costo. Para esta integración se ha recurrido fundamentalmente al Filtro de Kalman y sus extensiones, herramienta de estimación estocástica óptima de mayor relevancia (Grewal and Andrews, 2015).
En (Zhao et al., 2014) se propone una solución de navegación integrada de baja latencia que resuelve las deficiencias del Filtro de Kalman Extendido ante no linealidades fuertes, y mediciones con elevados niveles de ruido; para ello utiliza un esquema de fusión fuertemente acoplado entre un GPS diferencial y un INS, que implica acceder al código de la unidad de medición inercial. En (Sokolovi´c et al., 2015), se propone un esquema de fusión basada en el Filtro de Kalman Extendido, con la peculiaridad de estimar la deriva en el giróscopo, mediante el uso de magnetómetros y de un controlador PI. Logra además la compensación de errores en el canal horizontal del sistema de navegación utilizando señales adaptativas; en cambio, para su funcionamiento, requiere la incorporación de un barómetro, lo cual encarece la solución. Por su parte, en (Van and Van, 2015) se presenta el diseño de un Filtro de Kalman Extendido que integra INS/GPS de forma flexible respecto a la filosofía de integración. Compara los métodos de feedback y forward para la integración, cuya conclusión a favor de los métodos feedback es utilizada en este trabajo. Evalúa la propuesta con datos reales obtenidos con sensores inerciales y GPS. En cambio no se incluyen magnetómetros para mejorar la estimación del rumbo. El trabajo presentado en (Ryu et al., 2016) propone una comparación entre el Filtro de Kalman Extendido, y el Filtro de Kalman de tipo Unscented, utilizados para la fusión entre INS y GPS, con un modelo dinámico no lineal, y un vector de estados compuesto por las velocidades lineales en direcciones Norte y Este, y la velocidad de cambio del ángulo de rumbo. No se estiman los ángulos de balanceo y cabeceo, tampoco la altura. Esta solución no es generalizable a otros vehículos autónomos que requieren de más información sobre los parámetros de navegación.
En Cuba se inició el desarrollo de estas tecnologías en el ámbito académico, fundamentalmente por el Grupo de Automática, Robótica y Percepción de la Universidad Central de las Villas (GARP), de conjunto con el Centro de Investigación y Desarrollo Naval (CIDNAV). Se ha trazado como objetivo el desarrollo de autopilotos para vehículos autónomos subacuáticos y aéreos, con resultados importantes, expuestos en los trabajos (Martinez et al., 2013), (Valerianomedina et al., 2013), (Martinez et al., 2015) y (Garcia et al., 2015). No obstante, los sensores disponibles para estos desarrollos son de gama comercial, de código cerrado, considerados de bajo costo, poco fiables, ruidosos. No es posible acceder a los algoritmos implementados para obtener los parámetros de navegación (posición, velocidad y orientación), ni modificarlos para obtener alguna mejora ante nuevas aplicaciones.
Se requiere entonces de un algoritmo que permita estimar con precisión los parámetros de navegación a partir de las mediciones en bruto de un conjunto mínimo de sensores, con tecnología de bajo costo. En este artículo se presenta una solución de navegación que se basa en el uso del Filtro de Kalman Extendido para la fusión de información entre sensores inerciales, magnetómetros, y GPS como referencia externa, para estimar la orientación, posición y velocidad de un vehículo autónomo. Una peculiaridad de la propuesta es la modularidad alcanzada al separar la estimación de orientación de la posición. Se abordan las ecuaciones fundamentales de la implementación del Filtro Extendido de Kalman tanto para la estimación de la orientación como para la fusión de INS con GPS. Esta propuesta fue implementada y ejectuada en Matlab. Para su evaluación se utilizaron datos reales registrados en un experimento conjunto entre el GARP y el CidNav, en el año 2014.
Según (Castillo, 2012) un marco de referencia es un sistema cartesiano de tres ejes ortogonales orientados de forma positiva, con su origen en algún punto de interés para la navegación del vehículo. La posición, velocidad y orientación, así como las ecuaciones diferenciales que relacionan estos parámetros para la navegación, tienen forma vectorial, que exige su representación respecto a cierto marco de referencia. Los marcos de interés para este trabajo se describen a continuación:
Marco de referencia centrado en el cuerpo: El marco del cuerpo (b-frame) es no inercial, tiene su origen en el centro de masa del vehículo, y se alinea con los ejes de rotación del vehículo.
Los ángulos de Euler que describen la rotación del vehículo son: balanceo, cabeceo y guiñado, las rotaciones sobre los ejes X, Y y Z respectivamente, como se muestra en la Figura 1B.
Marco de referencia de navegación: El marco de navegación (n-frame) tiene su origen en la ubicación del sistema de navegación, en un punto arbitrario sobre la superficie de la Tierra. Sus ejes se alinean en las direcciones Norte - Este - Abajo (hacia el centro de la Tierra).
En la Figura 1 se muestran los marcos abordados. En la Figura 1A se muestran los ejes del arco de navegación, y en la Figura 1B los ejes del marco del cuerpo. Puede consultarse en (Titterton and Weston, 2004) y (Groves, 2008) para mayor profundidad.
Se plantea una estrategia de navegación integrada basada en la fusión de mediciones provenientes de sensores inerciales de bajo costo, del GPS como fuente externa de información, y el complemento de magnetómetros para mejorar la estimación del rumbo (ángulo de guiñada). En la Figura 2 se ilustra gráficamente los componentes de la solución.
Las mediciones de aceleración 
, y velocidad angular 
proveniente de los sensores inerciales, se acondicionan en el bloque “Filtrado de señales”. Con estas mediciones y las lecturas del magnetómetro 
, se estima la orientación 
del vehículo necesaria para la mecanización inercial, donde se resuelven numéricamente las ecuaciones diferenciales de navegación. Un Filtro Extendido de Kalman estima el error en posición y velocidad 
, con ayuda de la medición de un receptor GPS
. Con este error se corrige la posición y velocidad calculadas en la mecanización. Esta configuración para la navegación integrada se conoce en la literatura como esquema de retroalimentación o feedback, con el que se consigue mitigar las dificultades de navegar exclusivamente con una u otra fuente de información (España, 2010).
Las próximas secciones describen los componentes de la solución.
Fig.2
Propuesta de solución para navegación. Fuente: elaboración de los autores.
La naturaleza ruidosa de los sensores inerciales de bajo costo de tipo MEMs sugiere emplear un mecanismo de filtrado que permita eliminar parte del ruido presente en las mediciones. Para lograr este fin se propone un Filtro de Kalman lineal en tiempo discreto, cuya función es estimar óptimamente las señales de los acelerómetros y de los giróscopos, tal como se propuso en (Quesada Navarro, 2014). Esta función se realiza en el bloque Filtrado de Señales de la Figura 2.
El filtro de Kalman posee dos etapas: predicción y corrección (Simon, 2006).
La etapa de predicción:
La etapa de corrección:
Para esta aplicación, el vector de estados se define como 
, cuyas componentes son las señales estimadas de los acelerómetros y los giróscopos. La matriz del sistema es A = I6x6, y la matriz de salida C = I6x6. Las matrices P− y P+ son respectivamente la covarianza del error en la predicción y en la etapa de corrección. Q es la matriz de covarianza del error en el modelo. Kk es la matriz de ganancias de Kalman, y R es la matriz de covarianza del error en las mediciones. Por su parte, el vector 
se construye tomando las lecturas directamente de los sensores inerciales.
Por las características de las matrices A y C, cada nueva estimación estará en función de la estimación del instante anterior bajo el efecto del factor de corrección K.
Las desviaciones estándar de los sensores se obtienen de las hojas de datos del fabricante, y se utilizan para conformar la matriz de covarianza del error de las mediciones R.
Con las señales filtradas de los acelerómetros y giróscopos 
, en el período de muestreo en curso, se procede a la estimación de la orientación del vehículo, como paso previo a la mecanización inercial para obtener los parámetros de navegación.
Esta etapa consiste en estimar la matriz de rotación 
que describe la orientación del vehículo, necesaria para la etapa de mecanización inercial. Varios autores han propuesto alternativas para esta tarea, dada la imposibilidad de estimar adecuadamente la orientación solo con la integración de la velocidad angular, medida con giróscopos de bajo costo. Entre ellos, (Madgwick, 2010) propuso un filtro complementario basado en el método del gradiente descendente, el cual consigue corregir las mediciones del magnetómetro ante la presencia de perturbaciones magnéticas. En (Marmion, 2006) se planteó un Filtro de Kalman Extendido diseñado para estimar la orientación, partiendo de la cinemática de la rotación basada en cuaterniones, y luego con las mediciones de los acelerómetros y magnetómetros, corrige la estimación del rumbo. Por su parte (Valenti et al., 2015) propuso un filtro complementario con menor carga computacional que las propuestas anteriores, dado que no requiere el cálculo de Jacobianos, ni precisa la inversión de matrices.
Estas propuestas fueron tomadas en cuenta porque se basan en el álgebra de cuaterniones, cuyas ventajas se abordan ampliamente en (Rogers, 2003) y (Diebel, 2006). A continuación se detallan las ecuaciones principales para estimar la orientación, siguiendo los desarrollos de (Valenti et al., 2015).
Etapa de predicción: Las ecuaciones 3 y 4 conforman la etapa de predicción. El vector 
es un cuaternión puro que contiene la medición de los giróscopos; 
es la velocidad de cambio de la orientación, 
son la orientación estimada los tiempos de muestreo k − 1 y k respectivamente. Se sugiere revisar (Rogers, 2003) para la definición de las operaciones algebraicas con cuaterniones.
Etapa de corrección:
La corrección del cuaternión estimado en la etapa anterior se realiza multiplicando primero por un cuaternión que corrige las componentes de balanceo y cabeceo, y luego por otro cuaternión que corrige la componente de rumbo. Para ello se utilizan las mediciones de los acelerómetros y los magnetómetros, previamente normalizadas.
representa la desviación del vector de gravedad estimada 
que se obtiene al rotar el vector de aceleraciones del marco del cuerpo al marco de navegación, con respecto al vector de gravedad real g = [0,0,1]T . Para calcularlo se hace uso de la matriz de rotaciones basada en el cuaternión 
según las ecuaciones:
De la ecuación 7, siguiendo el desarrollo planteado en (Valenti et al., 2015), se obtiene la solución:
Dado que esta estimación está contaminada con ruido de alta frecuencia, se aplica un método de interpolación con el cuaternión unitario qI = [1,0,0,0]T . Tendrá dos expresiones, en función del valor de la parte real del quaternion 
(Valenti et al., 2015):
Donde el parámetro α ∈ [0,1] es la frecuencia de corte del filtro, Ω es el ángulo entre el cuaternión unitario puro, y el 
, obtenido del producto escalar entre ambos cuaterniones, según 10.
Al culminar este proceso, es necesario normalizar el cuaternión obtenido de la forma expresada en 11.
Para obtener el quaternión 
, se procede de manera similar. Se rota la medición de los magnetómetros al marco de navegación para obtener el vector 
luego se rota hasta el semiplano xz, según el sistema 12b.
De este modo el cuaternión 
describe solo una rotación alrededor del eje Z, alineado con el eje X en dirección positiva con el norte magnético.
De la ecuación 12b, se obtiene como solución la ecuación 13:
Donde β establece la frecuencia de corte para el filtro. ΩM es el ángulo entre el cuaternión unitario, y el
, obtenido del producto escalar entre estos, según 15.
El cuaternión debe normalizarse para realizar la corrección: 
Luego de estimada y corregida la orientación 
, solo resta obtener la matriz de rotaciones que se requiere para la mecanización, y la obtención de los ángulos de balanceo (φ), cabeceo (θ)y rumbo (ψ), respectivamente, tal como se expresa en 16 y 17.
Los vectores 
están expresados en el marco de navegación, ϕ es la latitud, λ es la longitud, expresadas en grados, y h la altura, en metros. Las ecuaciones diferenciales 18a y 18b se expresan en el marco de navegación según (Titterton and Weston, 2004):
donde 
es la matriz de rotación evaluada en la orientación estimada, fb es el vector de las aceleraciones filtradas. El producto cartesiano es la corrección del efecto de la fuerza de Coriolis, y las expresiones los vectores 
son respectivamente la velocidad de rotación de la Tierra en el marco de navegación, y la velocidad angular causada por la rotación del marco de navegación respecto a la Tierra, según las ecuaciones 19 y 20:
donde 
es la velocidad de rotación de la Tierra.
Se utiliza el método de perturbaciones para hacer el análisis del error en la mecanización inercial. Para la posición, luego de la integración se plantea en la expresión 21 la existencia de un error en el cálculo realizado. Luego en la ecuación 22 se plantea la dinámica del error de posición, que depende también del error en velocidad.
Donde, las matrices Frr y Frv se detallan a continuación:
Otro tanto ocurre con la velocidad:
Las matrices 26a y 26b son respectivamente:
donde 
y γ0 es la gravedad de la Tierra en h = 0.
En lo adelante se abordará la estrategia de fusión para corregir la estimación de velocidad y posición del vehículo.
Esta estrategia de fusión implica tener en cuenta no linealidades en las ecuaciones de navegación, así como la naturaleza de las perturbaciones presentes en las mediciones; por ello se elige el diseño de Filtro de Kalman Extendido.
Partiendo de definir el estado a estimar 
, como la medida del error entre las mediciones de posición y velocidad del INS y el GPS, se define la etapa de predicción del filtro con las ecuaciones 27a y 27b.
Donde Φ es la matriz de transición de estados, que se obtiene de la discretización de la matriz 
.
Esta es la matriz que describe la dinámica del error en posición y velocidad, según el desarrollo propuesto por (Shin and El-Sheimy, 2003) y (Falco et al., 2012). Φse calcula tomando los tres primeros términos de la serie de Taylor, según (Hu et al., 2015).
En la etapa de corrección:
De esta ecuación se destaca la matriz de salida Hk,
El vector de mediciones se conforma con la diferencia entre la salida de la solución inercial, y la información que proviene del GPS. Se adopta la modificación de multiplicar las diferencias en latitud y longitud por los radios de la Tierra, dado que están expresadas en radianes, son números pequeños que pueden generar inestabilidad numérica en la ejecución del filtro. (Hu et al., 2015)
Finalmente se actualiza el vector de estados, y la matriz de covarianza.
Esta solución fue implementada en Matlab con fines de simulación y evaluación. Se utilizaron los datos registrados de un experimento realizado por el GARP y el CIDNAV en el año 2014. Los datos de navegación fueron obtenidos con una IMU Mti-G, del fabricante Xsens, a bordo del vehículo autónomo subacúatico HRC400. Se tomaron los datos en bruto de los acelerómetros, giróscopos y magnetómetros en los tres ejes (aceleración, velocidad angular e intensidad del campo magnético). Como valores de referencia se tomaron los ángulos de Euler que describen la orientación del vehículo calculados por la IMU, así como la información sobre posición y velocidad obtenidos del GPS, expresados en latitud, longitud, altura, y la velocidad en los tres ejes. Los valores iniciales de los ángulos de orientación se tomaron de la referencia. Los valores iniciales para la posición y la velocidad fueron tomados de las lecturas iniciales del GPS.
Los resultados obtenidos en el acondicionamiento de las señales inerciales permitieron lograr una mayor estabilidad en la estimación. Se logra un adecuado nivel de atenuación del ruido presente en las señales de los instrumentos, como se aprecia en las Figura 3 y Figura 4. En la velocidad angular persiste inestabilidad dado que no se consideran fenómenos físicos propios del entorno donde se realizó el experimento, como el oleaje. Esto incide en la estimación de la orientación.
Fig.3
Aceleraciones en bruto y filtradas.
Fig.4
Velocidades angulares en bruto y filtradas.
El estimador de orientación muestra resultados aceptables en los ángulos de balanceo, cabeceo, manteniendo con un error medio cuadrático de 1,04◦ y 0,71◦ respectivamente. El rumbo exhibe 4,6◦ de error, debido a perturbaciones en las lecturas de los magnetómetros. Estos valores se obtuvieron fijando el valor de los parámetros α0 = 0,6, y β = 0,01. El uso de las lecturas de los acelerómetros y los magnetómetros contribuye a mejorar la estimación de los ángulos. No obstante, la modularidad de la propuesta permite evaluar varias estrategias sin dependencia directa del proceso de estimación de posición y velocidad. En la Figura 5 se muestra la evolución temporal de la orientación expresada en ángulos de Euler.
Fig.5
Resultado de los ángulos de Euler
Fig.6
Resultado de la trayectoria seguida por el vehículo.
Para el ajuste del Filtro de Kalman Extendido, empíricamente se definieron las matrices Q y R, con los valores:
La posición se expresa en latitud, longitud y altura. Los errores medios cuadráticos son 6,3 * 10−6◦, 1,1 * 10−5◦, y 0,0053m, respectivamente. En la Figura 6 se observa el comportamiento de estos tres parámetros. En la Figura 7 se muestra el segmento de trayectoria de los primeros 100s del experimento en dos dimensiones. Por su parte, las velocidades en los tres ejes fueron estimadas con errores de 0,0140m/s, 0,0112m/s, y 0,0101m/s respectivamente; se muestran en la Figura 8 . En todos los casos se observa un comportamiento cercano a los valores de referencia considerados como válidos para este experimento.
Fig.7
Resultado de la trayectoria seguida por el vehículo en dos dimensiones.
Fig.8
Resultado de la velocidad en los tres ejes.
Se destaca como elemento distintivo la incorporación de un estimador de orientación independiente de la mecanización inercial, lo cual provee mayor modularidad y posibilidad de mantenimiento. Con la solución propuesta es posible obtener la orientación con precisión comparable a las especificaciones técnicas de IMUs comerciales. La propuesta permite utilizar las mediciones inerciales en bruto, con ayuda de magnetómetros y de referencia externa como el GPS, para garantizar una solución de navegación eficaz.
Como trabajo futuro se debe trabajar en una solución más robusta para la estimación del rumbo, así como evaluar la incorporación del modelo dinámico del vehículo para aumentar independencia de la calidad o disponibilidad de otros sensores. La elección de las matrices Q y R son críticas para la sintonía del filtro, en trabajos posteriores es preciso contar con los sensores para aplicar técnicas de calibración que permitan estimar con mayor eficacia estas matrices.
Los autores agradecen la colaboración prestada por los investigadores del Grupo de Automática, Robótica y Percepción de la Universidad Central Marta Abreu de Las Villas, DrC Alain Martínez Laguardia, y Msc Valeriano Medina.
*Autor para la correspondencia: ddorvigny@uci.cu
Fig.2
Propuesta de solución para navegación. Fuente: elaboración de los autores.
Fig.3
Aceleraciones en bruto y filtradas.
Fig.4
Velocidades angulares en bruto y filtradas.
Fig.5
Resultado de los ángulos de Euler
Fig.6
Resultado de la trayectoria seguida por el vehículo.
Fig.7
Resultado de la trayectoria seguida por el vehículo en dos dimensiones.
Fig.8
Resultado de la velocidad en los tres ejes.
