Sistema de navegación inercial para un AUV en presencia de corrientes marinas

Inertial Navigation System for an AUV in presence of sea current

Jorge Luis Lemus-Ramos^I, Luis E. Hernández-Santana^II, Yunier Valeriano-Medina^II

^IEmpresa de Tecnologías de la Información y la Automática (ATI), Villa Clara, Cuba.

^IIUniversidad Central “Marta Abreu” de las Villas (UCLV), Villa Clara, Cuba.

RESUMEN

Un AUV es un submarino que porta consigo una fuente de energía y unidad de cómputo. El HRC-AUV es un prototipo desarrollado por el “Centro de investigación y Desarrollo Naval” (CIDNAV) para la exploración del fondo marino. En este trabajo se presenta la implementación de un sistema de navegación basada en el modelo dinámico no lineal del HRC-AUV que permite la estimación de la magnitud y dirección de las corrientes en el teatro de operaciones mejorando los resultados de la navegación. Los resultados se validan mediante simulación a partir del modelo matemático de 6 grados de libertad (GDL) del HRC-AUV y utilizando conjuntos de datos obtenidos en sesiones de experimentos con el vehículo.

Palabras claves: Vehículos autónomos, Sistema de navegación, filtro.

ABSTRACT

An AUV is an underwater vehicle that carries its own power supply and computational units. The HRC-AUV is a prototype develop by “Center for Navy Researching and Development” (CIDNAV) for undersea researching. In this paper is presented the implementation of a navigation system based on the nonlinear dynamical model for the HRC-AUV which allows to estimate sea currents magnitude and direction in the operation environment improving the navigation results. The results are validated through simulation using the 6 degrades of freedom (DOF) nonlinear model obtained for the HRC-AUV and using data sets obtained in experimental sessions directly from the vehicle.

Keywords: Autonomous vehicles, Navigation system, navigation, filter.

Un Vehículo Autónomo Sumergible (AUV¹, por sus siglas en inglés) es un submarino que porta consigo una fuente de energía y unidad de cómputo, donde se ejecutan soluciones de control, que le permite cumplir tareas predefinidas sin la intervención humana [1-3]. Un ejemplo claro de la aplicación de esta tecnología lo constituye el AUV Hugin [4], desarrollado por Kongsberg Maritime y Forsvarets Forsknings Institute (FFI) de Noruega. En [5] se discute la inspección visual del fondo subacuático utilizando el AUV ICTINEU.

El HRC-AUV es un prototipo desarrollado para la exploración del fondo marino [6]. Entre sus características principales se encuentra, que es un vehículo sub-actuado, construido con una arquitectura sensorial de bajo costo. El Grupo de Automatización, Robótica y Percepción (GARP) perteneciente a la Universidad Central de Las Villas (UCLV), se ocupó del diseño computacional y sensorial del mismo [7]. Para garantizar la autonomía del vehículo es necesario dotarlo con un sistema de navegación capaz de estimar las variables necesarias para el control y el guiado.

La navegación inercial asistida ha sido reportada ampliamente en la literatura consultada [8-12]. Este método consiste en la implementación de un Sistema de Navegación Inercial (INS², por sus siglas en inglés) al cual se incorporan un conjunto de mediciones adicionales que contribuyen a reducir y corregir el error acumulativo asociado al proceso de integración doble de las mediciones de los acelerómetros.

En [13] se plantea la implementación de un INS asistido por el modelo dinámico para navegación (DNVM³, por sus siglas en inglés) del HRC-AUV. En este trabajo el modelo es linealizado alrededor de un punto de operación y por tanto la estimación de las variables es realizada mediante un Filtro de Kalman (KF). La solución reportada por Martínez [9] logra la estimación de las variables que deciden la navegación y el guiado del HRC-AUV basándose en el modelo y partiendo de una arquitectura sensorial de bajo costo. No obstante, en la estimación de la perturbación de las corrientes marinas solo logra determinar su magnitud. Ello obliga a la realización de un grupo de maniobras ajenas al estimador para conocer a priori la dirección de las corrientes.

Sobre esa base el objetivo de este trabajo es encontrar una solución que permita la estimación de las corrientes marinas junto a los parámetros necesarios para la navegación y el guiado del HRC-AUV. La estimación de las corrientes marinas tiene gran valor pues permite conocer esa perturbación que facilitaría las soluciones de guiado, altamente dependientes del conocimiento de las mismas.

La solución propuesta tiene como principios la utilización del modelo no lineal del HRC-AUV en el plano horizontal, modelo de navegación. Ello implica que la implementación del estimador demande la utilización del Filtro Extendido de Kalman (EKF⁴, por sus siglas en inglés). Además, se ha incorporado al vector de medición del estimador los acelerómetros de la IMU. Todo ello ha permitido estimar directamente las dos componentes de las corrientes marinas en el plano horizontal en tiempo real.

El trabajo ha quedado organizado con una sección 2, dedicada al modelo dinámico del sistema. Primeramente, se obtiene el modelo dinámico de 6 grados de libertad (GDL), base de toda la modelación y utilizado como referente en el trabajo de simulación. Luego, este modelo se reduce al plano horizontal quedando en 3 grados de libertad. Se establecen los modelos para las mediciones y finalmente se obtiene el modelo discreto para la implementación del observador.

En la sección 3 se presenta el desarrollo del observador basado en el EKF. Primeramente, se establecen las ecuaciones que componen el proceso iterativo de estimación. Luego, se calculan las derivadas parciales de las ecuaciones de transición de estado y mediciones. Finalmente se definen los términos restantes.

En la sección 4 se presenta la validación de los resultados mediante simulación, utilizando como referencia el modelo de 6 grados de libertad presentado en la sección 2 y utilizando conjuntos de mediciones registradas durante experimentos con el HRC-AUV. Finalmente se presentan las conclusiones del trabajo.

2. MODELO MATEMÁTICO

Para la implementación del observador en el sistema de navegación, es necesario establecer el modelo matemático que constituye el centro del algoritmo. En el caso del HRC-AUV, los sensores inerciales instalados en el vehículo son de bajo costo y por tanto de prestaciones limitadas. Teniendo en cuenta que en la aplicación en cuestión se conocen de antemano las características del vehículo, resulta interesante la utilización del modelo dinámico no lineal de navegación para estimar la posición a través de un sistema de navegación asistida. El uso del modelo no lineal del sistema obliga a utilizar el EKF como núcleo del estimador.

Para la obtención del modelo matemático del vehículo HRC-AUV se utiliza el método descrito en [14]. De acuerdo con la literatura consultada, resulta necesario establecer dos sistemas de coordenadas tal como se muestra en la Figura 1.

El sistema de coordenadas n se plantea fijo a un punto de referencia en el teatro de operaciones y se considera inercial despreciando los efectos de los movimientos de rotación y traslación de la tierra. Por otra parte, el sistema de coordenadas b se plantea en el centro de masa del vehículo por lo que resulta no inercial. Resulta interesante destacar que: como la IMU se instala ubicada sobre el centro de masa de vehículo [7], sus mediciones se consideran referidas al sistema de coordenadas b.

En la Tabla 1 se muestra la nomenclatura utilizada para las fuerzas y momentos, velocidades lineales y angulares, posición y orientación del vehículo [15].

(1)

(2)

(3)

La relación cinemática entre ambos sistemas de referencia se puede formular a partir de la matriz de transformación de ángulos de Euler (J(η)) [16] de la siguiente forma:

(4)

De acuerdo con la metodología descrita en [14], la dinámica para un vehículo que se desplaza en el agua a mar abierto, puede ser descrita como se muestra en (5).

(5)

En la Tabla 2 se muestran las descripciones de los parámetros relacionados en la ecuación (5).

Nótese que el efecto de las corrientes marinas, principal perturbación a que se ve sometido un vehículo subacuático en mar abierto y objetivo central del presente trabajo, queda representada en (5) en el término ν_r, el cual se puede expresar como se muestra en (6).

(6)

donde μ_c representa la velocidad de las corrientes referidas al sistema de coordenadas n. Suponiendo que las corrientes son aproximadamente constantes y despreciando las componentes rotacionales, estas se pueden representar como se muestra en (7).

(7)

En [17] se presenta la descripción del modelo matemático para el HRC-AUV acorde a la estructura presentada en la ecuación (5). Este modelo de 6 GDL ha sido utilizado para propósitos de simulación en trabajos relacionados con el HRC-AUV [6, 7, 13]. Este modelo describe el movimiento del vehículo tanto en el plano horizontal como vertical.

La aplicación está destinada para funciones de guiado y seguimiento de caminos en el plano horizontal. Por tanto el vector de estado estará compuesto por las variables del plano horizontal, unido a las variables asociadas a la estimación de las corrientes. Sobre esa base se establece el vector de estado para el sistema de navegación a implementar como se muestra en (8).

(8)

La diferencia fundamental de la estructura descrita en (8) con la presentada en [9] resulta la estimación de la velocidad de las corrientes como componentes referidas al plano de navegación en lugar de solamente estimar su intensidad.

Al trabajar solo en plano horizontal se pueden considerar valores pequeños de z, Φ, θ, w, p q y μ_D en la ecuación (5) y se pueden despreciar las variables que describen el movimiento de vehículo en el plano vertical, obteniéndose un modelo reducido de 3GDL que describe el comportamiento del sistema en el plano horizontal.

(9)

Las ecuaciones (4) y (5) se pueden reescribir, agrupando términos semejantes y despejando la derivada del vector velocidad () como:

(10)

Los términos ω_η y ω_ν se consideran ruidos blancos y representan la incertidumbre asociada a los procesos de aproximación y reducción de estados llevados a cabo durante el modelado.

Así, los términos de la ecuación (10) quedan definidos como se muestra en las ecuaciones de la (11) a la (15).

(11)

(12)

(13)

(14)

(15)

donde:

En el caso específico del HRC-AUV, se cuenta con mediciones de los valores de velocidad de rotación del rotor y posición del timón, los cuales se utilizan como entradas para el algoritmo de navegación como se muestra en (16).

(16)

donde:

Para utilizar estas mediciones como entradas es necesario tomar en consideración la incertidumbre asociada a la medición de las señales de mando, como se muestra en (17).

(17)

siendo:

Sustituyendo (17) en (14), resolviendo y eliminando los términos de segundo orden asociados a los parámetros de incertidumbre, se obtiene:

(18)

(19)

Sustituyendo (18) en (10) se obtiene:

(20)

con:

(21)

(22)

siendo ω_c un ruido blanco que representa la incertidumbre asociada a las variaciones que puedan experimentar las corrientes durante el desarrollo de la misión.

Nótese que las ecuaciones (20) y (22) representan el modelo dinámico no lineal para el HRC-AUV en tiempo continuo.

Ateniendo a las mediciones disponibles a partir del esquema sensorial del HRC-AUV presentado en [7], se puede establecer el vector de mediciones como se muestra en la ecuación (23);

(23)

donde:

Las mediciones de posición se representan como se muestra en (24), donde ξ_xyξ_y constituyen las incertidumbres asociadas al proceso de medición.

(24)

Por otra parte las mediciones de dirección y velocidad de giro se presentan como se muestra en (25) con la incertidumbre representada por ξ_ψ y ξ_rrespectivamente

(25)

(26)

Las ecuaciones de la (24) a la (26) representan el modelo de las mediciones para el sistema de navegación.

Hasta el momento, las ecuaciones que describen la dinámica del sistema en cuestión se han presentado en tiempo continuo. Sin embargo, para la implementación del EKF, es necesario expresar el modelo en tiempo discreto. En la ecuación (27) se presenta la estructura general de un modelo dinámico no lineal en tiempo discreto [18].

(27)

Utilizando un integrador cuadrado, se puede definir la ecuación de transición de estado como se muestra en la ecuación (28), donde T representa el período de muestreo.

(28)

Atendiendo a la definición del estado presentada en (8) y de acuerdo con las ecuaciones (20) y (22), queda definida como se muestra en la ecuación (29).

(29)

Las ecuaciones de la (30) a la (34) representan la estructura de los términos descritos en (29), los valores numéricos de esas ecuaciones se obtienen de [13].

(30)

(31)

(32)

(33)

(34)

De las ecuaciones (20) y (23) , se define el término de incertidumbre ω_t como se muestra en la ecuación (35).

(35)

Sustituyendo (29) en (28) se obtiene la ecuación de transición de estado en tiempo discreto como se muestra en (36).

(36)

Los valores numéricos presentados en las ecuaciones (31), (33) y (34) fueron encontrados en [15].

(37)

Los términos no descritos en la ecuación (37) se definen en las ecuaciones de la (38) a la (40).

(38)

(39)

(40)

Sustituyendo (29) en (37) y comparando miembros, se obtiene la definición de la función de medición y su incertidumbre asociada como se muestra en la ecuación (41).

(41)

donde Γ queda definido como se muestra en la ecuación (42).

(42)

3. FILTRO EXTENDIDO DE KALMAN

A partir de las ecuaciones: (27), (36) y (41), se abordan los pasos del diseño e implementación del EKF para la estimación de las variables de estado (8) que forman parte del modelo no lineal del plano horizontal.

Las ecuaciones que describen el proceso iterativo de estimación [18] se definen como sigue:

(43)

(44)

(45)

(46)

(47)

El término F_k representa la derivada parcial de la función de transición de estado definida en (36) con respecto al estado, evaluada en el estado estimado y se obtiene como se muestra en (48).

(48)

Los términos de quedan definidos como se muestra en las ecuaciones de la (49) a la (54).

(49)

(50)

(51)

(52)

(53)

(54)

Q_k se define según la ecuación (55).

(55)

Por otra parte, el término M_k aparece debido a las correlaciones existentes entre ξ_k y ω_k descritas en la ecuación (41), y se define como se muestra en la ecuación (56).

(56)

Además, H_k es la derivada parcial de la función de medición definida en (41) con respecto al estado y se obtiene como se muestra en la ecuación (57).

(57)

En la ecuación (58) aparece definida R_k. Este término representa la covarianza asociada a la función de medición y depende de R₀, definido en (59), el cual representa la covarianza de los errores asociados a los procesos de medición.

(58)

(59)

4. VALIDACIÓN DE LOS RESULTADOS

Para la validación de los resultados obtenidos se utiliza el software MATLAB con su herramienta SIMULINK. A partir del modelo descrito en el epígrafe 2, se aplica un mando constante de 0.1 rad a la deflexión del timón(δ_T) generándose una trayectoria circular. En este escenario, de no existir corrientes, se originaría una trayectoria formada por círculos concéntricos. En la realidad, el centro de los círculos se desplaza debido al efecto de las corrientes. Para la simulación se establecen parámetros de intensidad de corrientes consistentes con los valores observados durante las pruebas realizadas con el HRC-AUV en varias direcciones. Todas las mediciones simuladas, pasadas al observador, se contaminan con señales ruidosas generadas aleatoriamente a partir de los parámetros de incertidumbre de los sensores presentes en el HRC-AUV.

En la Figura 2 se presenta el resultado de la simulación para una corriente con dirección norte (0°) e intensidad 0.5 m⁄s . En la Figura 2 a) se muestra la posición, en la cual se aprecia como la estimación consolida la posición mejorando los elevados niveles de incertidumbre asociados al sistema de posicionamiento global (GPS). En la Figura 2 b) se presenta la estimación de las corrientes, observándose la convergencia del observador a los valores establecidos para la simulación.

En la Figura 3 se presenta el resultado de la simulación para una corriente con dirección este (90°) e intensidad 0.5 m⁄s . En la Figura 3 a) se muestra la estimación de la posición y en la Figura 2 b) se presenta la estimación de las corrientes, observándose la convergencia del observador a los nuevos valores simulados.

En la Figura 4 se muestra el recorrido realizado por el vehículo durante una sesión de experimentos. La estimación de la velocidad de las corrientes se muestra mediante vectores desplegados sobre la trayectoria, coincidiendo la dirección de las corrientes con las desviaciones experimentadas por el vehículo. Además, en el acercamiento se observa como la trayectoria estimada resulta mucho más precisa que las mediciones de GPS. Por otra parte, en la Figura 5 se presenta la estimación de las componentes de la velocidad de las corrientes a lo largo de la trayectoria, observándose cómo convergen rápidamente y varían suavemente con el desplazamiento del vehículo.

5. CONCLUSIONES

En esta investigación se presenta un sistema de navegación inercial basado en el modelo dinámico no lineal en el plano horizontal del vehículo subacuático HRC-AUV, con el cual se logra de las corrientes marinas que afectan la navegación en el teatro de operaciones. La implementación del sistema de navegación utiliza un EKF para la fusión de la información de las mediciones con las predicciones del modelo. El algoritmo que se propone es capaz de estimar tanto intensidad como dirección de las corrientes marinas. Tanto los resultados de simulación como los resultados obtenidos con los datos reales demuestran que las estimaciones de las corrientes realizadas por el observador mejoran la calidad de las mediciones de posición obtenidas a partir de los sensores instalados a bordo de la embarcación.

NOTAS

¹Autonomous Underwater Vehicle

³Dynamic Navigation Vehicle Model

⁴Extended Kalman Filter

6. REFERENCIAS

1. Fjellstad OE. Control of unmanned underwater vehicles in six degrees of freedom a quaternion feedback approach. Tesis de Doctorado. Noruega: NTNU; 1994. Disponible en: http://fossen.biz/home/PhD/thesis/fjellstad.pdf.

2. Blidberg DR. The Development of Autonomous Underwater Vehicles (AUVs); A Brief Summary. In: IEEE International Conference on Robotics and Automation (ICRA). IEEE. Seoul, Korea.; 2001. p. 21-5.

3. Antonelli G. Fault Detection Tolerance Strategies for AUVs and ROVs. In: Antonelli G, editor. Underwater Robots. Tracts in Advanced Robotics. Berlin, Alemania: Springer, Berlin, Heidelberg; 2006. p. 79-91.

4. Gorset JE. Nonlinear model-based control of slender body AUVs Tesis de Doctorado. Noruega: NTNU; 2007. Disponible en: http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.210.3536&rep=rep1&type=pdf.

5. Carreras M, Ridao P, García R, Ribas D, Palomeras N. Inspección visual subacuática mediante robótica submarina. Revista Iberoamericana de Automática e Informática industrial. 2012;9(1):34-45.

6. Valeriano Medina Y, Hernández Julián A, Hernández L. Controlador I LOS para el segiuimiento de caminos en linea recta de un vehículo autónomo subacuático. Revista de Ingeniería Electrónica, Automática y Comunicaciones. 2015;XXXVI(2):15-28.

7. Martinez A, Rodriguez Y, Hernandez L, Guerra C, Lemus J, Sahli H. Diseño de AUV. Arquitectura de hardware y software. Revista Iberoamericana de Automática e Informática Industrial RIAI. 2013;10(3):333-43.

8. Grewall M, editor. Global Positioning Systems, Inertial Navegation, and Integration: John Wiley \& sons; 2001.

9. Martinez A, Hernandez L, Sahli H, Valeriano Y, Orozco M, Garcia D. Model aided navigation with sea current estimation for autonomous underwater vehicle. International Journal of Advanced Robotic Systems. 2015.

10. Grewal M, Andrews AP. KALMAN FILTERING,Theory and Practice Using MATLAB. Third ed. Hoboken, New Jersey: John Wiley and Sons; 2008.

11. Hegrenaes OB, E.; Hallingstad,O. . Model-aided inertial navigation for underwater vehicles In: IEEE International Conference on Robotics and Automation (ICRA). IEEE. Pasadena, Estados Unidos; 2008. p. 1069-76.

12. Hegrenaes O, Hallingstad O. Model-Aided INS With Sea Current Estimation for Robust Underwater Navigation. IEEE Journal of Oceanic Engineering. 2011;36(2):316 - 37.

13. García García D, Valeriano Medina Y, Portal Linarez JA, Hernandez L. Sistema de navegación basado en modelo dinámico no lineal de vehículo autónomo sumergible. Revista de Ingeniería Electrónica, Automática y Comunicaciones. 2015;XXXVI(2):83-97.

14. Fossen TI. Handbook of marine craft hydrodynamics and motion control: John Wiley & Sons; 2011.

15. Valeriano Y. Modelo Dinámico para Vehículo Autonomo Sumergible Tesis de Maestría. Santa Clara, Villa Clara, Cuba: Univercidad Central "Martha Abreu" de las Villas; 2013. http://dspace.uclv.edu.cu/bitstream/handle/123456789/6863/Yunier%20Valeriano%20Medina.pdf?sequence=1&isAllowed=y.

16. Fossen TI, Ross A. Nonlinear Modelling, identification and control of UUVs. In: Sutton R, editor. Advances in unmanned marine vehicles. London, Great Britain: Peter Peregrinus LTD; 2006. p. 13-42.

17. Valeriano Y, Martinez A, Hernandez L, Rodriguez Y, Sahli H, Cañizares JR. Dynamic model for an autonomous underwater vehicle based on experimental data. Mathematical and Computer Modelling of Dynamical Systems. 2013;19(2):175-200.

18. Simon D. Optimal State Estimation. United States of America.: JOHN WILEY & SONS, INC; 2006.

Recibido: 30/1/2018

Aceptado: 15/5/2018

Jorge Luis Lemus Ramos, Ing. Especialista A en automática de la “Empresa de Tecnologías de la Información y la Automática” (ATI), Villa Clara, Cuba, jlemus@ativc.une.cu.