Software de navegación y guiado en tiempo real para vehículo autónomo sumergible

Texto completo

(1)Universidad Central “Marta Abreu” de Las Villas Facultad de Ingenierı́a Eléctrica Departamento de Automática y Sistemas Computacionales. Trabajo de Diploma. Software de navegación y guiado en tiempo real para Vehı́culo Autónomo Sumergible. Autor: Jorge Luis Lemus Ramos Tutores: M.Sc. Alain Martı́nez Laguardia Ing. Richard Sosa López. Santa Clara 2011 “Año 53 de la Revolución”.

(2) Universidad Central “Marta Abreu” de Las Villas Facultad de Ingenierı́a Eléctrica Departamento de Automática y Sistemas Computacionales. Trabajo de Diploma Software de navegación y guiado en tiempo real para Vehı́culo Autónomo Sumergible Trabajo de Diploma presentado en opción al Tı́tulo Académico de Ingeniero en Automática. Autor: Jorge Luis Lemus Ramos email: [email protected]. Tutores: M.Sc. Alain Martı́nez Laguardia Dpto. de Automática, Facultad de Ing. Eléctrica, UCLV email: [email protected]. Ing. Richard Sosa López Centro de Investigación y Desarrollo Naval (CIDNAV) email: [email protected]. Santa Clara 2011 “Año 53 de la Revolución”.

(3) Hago constar que el presente Trabajo de Diploma fue realizado en la Universidad Central “Marta Abreu” de Las Villas como parte de la culminación de estudios de la especialidad de Ingenierı́a en Automática, autorizando a que el mismo sea utilizado por la Institución, para los fines que estime conveniente, tanto de forma parcial como total y que además no podrá ser presentado en eventos, ni publicados sin autorización de la Universidad.. Jorge Luis Lemus Ramos Autor. Fecha. Los abajo firmantes certificamos que el presente trabajo ha sido realizado según acuerdo de la dirección de nuestro centro y el mismo cumple con los requisitos que debe tener un trabajo de esta envergadura referido a la temática señalada.. Jorge Luis Lemus Ramos Autor. Fecha. Boris Luis Martı́nez Jiménez, Dr.C Jefe del Departmento. Fecha. Responsable ICT o J’ de Carrera, (Dr.C., M.Sc. o Ing.) Responsable de Información Cientı́fico-Técnica. Fecha.

(4) PENSAMIENTO. “Son vanas y están plagadas de errores las ciencias que no han nacido del experimento, madre de toda certidumbre.”. Leonardo Da Vinci. i.

(5) DEDICATORIA. A mis padres, Lisset y Jorge Luis, por su apoyo y por lo que han logrado hacer de mi persona.. A mis abuelos, Lourdes y Omar, por su cariño sincero e incondicional.. A mis hermanos, Iliana y Miguel, todo mi esfuerzo va encaminado a ser un buen ejemplo para ellos.. A mi novia, Magyoly, su cariño ha sido un apoyo insustituible en los últimos dos años.. ii.

(6) SÍNTESIS. Los vehı́culos sumergibles autónomos son muy utilizados e investigados a nivel mundial debido a sus variadas aplicaciones. Instituciones de nuestro paı́s han mostrado interés en el tema en los últimos años. El Grupo de Automatización, Robótica y Percepción (GARP) de la Universidad Central “Marta Abreu” de las Villas (UCLV ), en combinación con el Centro de Investigación y Desarrollo Naval (CIDNAV ), llevan la delantera en el tema, con varias publicaciones en este campo y el desarrollo de un prototipo en fase experimental. Este trabajo se enmarca en el desarrollo de un software que ejecute las funciones de navegación y guiado para dicho prototipo. Los algoritmos desarrollados por GARP con este objetivo presentan requerimientos de tiempo real. Además, el software desarrollado implementa un sistema de comunicación basado en el protocolo RS-232 que posibilita el intercambio seguro de datos entre los diferentes nodos que integran el sistema. Finalmente se validan los resultados mediante simulación.. iii.

(7) TABLA DE CONTENIDO Página PENSAMIENTO . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. i. DEDICATORIA . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. ii. SÍNTESIS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. iii. Índice de cuadros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii Índice de figuras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . viii INTRODUCCIÓN . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 1. 1.. 5. MARCO TEÓRICO . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.1.. Introducción. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 5. 1.1.1. Definición. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 6. 1.1.2. Aplicaciones. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 6. Precedentes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 7. 1.2.1. MARES AUV . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 8. 1.2.2. Cormorán AUV . . . . . . . . . . . . . . . . . . . . . . . . . . .. 9. 1.2.3. HRC-AUV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 10. 1.2.4. Ventajas y Desventajas . . . . . . . . . . . . . . . . . . . . . . .. 14. Sistemas de Tiempo Real. . . . . . . . . . . . . . . . . . . . . . . . . .. 14. 1.3.1. Clasificaciones de los Sistemas de Tiempo Real . . . . . . . . . .. 15. 1.3.2. Requerimientos de los sistemas de tiempo real . . . . . . . . . .. 16. Sistema Operativo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 17. 1.4.1. Potencialidades de Linux para el trabajo en tiempo real . . . . .. 18. Conclusiones parciales del capı́tulo. . . . . . . . . . . . . . . . . . . . .. 18. DISEÑO DE SOFTWARE . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 20. 2.1.. Introducción. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 20. 2.2.. Modelado del problema. . . . . . . . . . . . . . . . . . . . . . . . . . .. 20. 1.2.. 1.3.. 1.4. 1.5. 2.. iv.

(8) 2.2.1. Actores. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 21. 2.2.2. Casos de uso.. . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 21. 2.3.. Sistema de Comunicación. . . . . . . . . . . . . . . . . . . . . . . . . .. 24. 2.4.. Estructura del sistema de comunicación . . . . . . . . . . . . . . . . . .. 25. 2.5.. Tareas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 27. 2.5.1. Asignación de prioridades. . . . . . . . . . . . . . . . . . . . . .. 28. Manejo de Datos. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 28. 2.6.1. Bases de Datos. . . . . . . . . . . . . . . . . . . . . . . . . . . .. 29. Navegación Inercial . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 31. 2.7.1. Sistemas de Referencia. . . . . . . . . . . . . . . . . . . . . . . .. 32. 2.7.2. Suavizado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 34. 2.7.3. Sistema de Navegación Inercial . . . . . . . . . . . . . . . . . . .. 36. 2.7.4. Modelo Dinámico para Navegación . . . . . . . . . . . . . . . . .. 37. 2.7.5. Filtro Extendido de Kalman . . . . . . . . . . . . . . . . . . . .. 39. Guiado del Vehı́culo. . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 42. 2.8.1. Planificación de Trayectoria. . . . . . . . . . . . . . . . . . . . .. 43. 2.8.2. Ley de guiado . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 44. Conclusiones parciales del capı́tulo. . . . . . . . . . . . . . . . . . . . .. 46. IMPLEMENTACIÓN EN TIEMPO REAL . . . . . . . . . . . . . . . . . . . .. 47. 3.1.. Introducción. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 47. 3.2.. Recompilación del kernel de Linux. . . . . . . . . . . . . . . . . . . . .. 47. 3.2.1. Actualización del sistema . . . . . . . . . . . . . . . . . . . . . .. 47. 3.2.2. Obtención del código fuente . . . . . . . . . . . . . . . . . . . .. 48. 3.2.3. Configuración . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 48. 3.2.4. Compilar el nuevo kernel . . . . . . . . . . . . . . . . . . . . . .. 49. 3.2.5. Instalar el nuevo kernel . . . . . . . . . . . . . . . . . . . . . . .. 50. Implementación de la aplicación. . . . . . . . . . . . . . . . . . . . . . .. 50. 3.3.1. Capa 1. Abstracción. . . . . . . . . . . . . . . . . . . . . . . . .. 50. 3.3.2. Capa 2. Función. . . . . . . . . . . . . . . . . . . . . . . . . . . .. 50. 3.3.3. Capa 3. Comunicación. . . . . . . . . . . . . . . . . . . . . . . .. 51. 3.3.4. Capa 4. Navegación . . . . . . . . . . . . . . . . . . . . . . . . .. 51. 3.3.5. Capa 5. Bases de Datos. . . . . . . . . . . . . . . . . . . . . . .. 52. 3.3.6. Capa 6. Sistema. . . . . . . . . . . . . . . . . . . . . . . . . . . .. 52. 2.6. 2.7.. 2.8.. 2.9. 3.. 3.3.. v.

(9) 3.3.7. Servicios de Linux . . . . . . . . . . . . . . . . . . . . . . . . . .. 53. Validación de los resultados. . . . . . . . . . . . . . . . . . . . . . . . .. 54. 3.4.1. Navegación. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 54. 3.4.2. Guiado. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 55. 3.4.3. Requisitos Temporales. . . . . . . . . . . . . . . . . . . . . . . .. 56. Análisis económico . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 56. CONCLUSIONES Y RECOMENDACIONES . . . . . . . . . . . . . . . . . . . . .. 58. A.. Sistema de comunicación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 63. B.. Teorema de Rotación de Euler . . . . . . . . . . . . . . . . . . . . . . . . . . .. 70. C.. Modelos de errores de las mediciones del sensor inercial . . . . . . . . . . . . .. 74. D.. Modelo del error del INS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 76. E.. Método Recursivo de Mı́nimos Cuadrados . . . . . . . . . . . . . . . . . . . .. 78. F.. Módulos que componen el sistema . . . . . . . . . . . . . . . . . . . . . . . . .. 82. 3.4.. 3.5.. vi.

(10) Índice de cuadros Cuadro. Página. 1–1. Comparación entre arquitecturas de AUVs . . . . . . . . . . . . . . . . . .. 8. 1–2. Comparación entre HRT y SRT . . . . . . . . . . . . . . . . . . . . . . . .. 15. 2–1. Esquema de Comunicación . . . . . . . . . . . . . . . . . . . . . . . . . . .. 25. 2–2. Estructura de mensajes . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 26. 2–3. Mensajes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 26. 2–4. Tabla de Estado (Tiempo Real) . . . . . . . . . . . . . . . . . . . . . . . . .. 30. 2–5. Tabla de Configuración (Tiempo Real) . . . . . . . . . . . . . . . . . . . . .. 30. 2–6. Tabla de Alarmas (Tiempo Real) . . . . . . . . . . . . . . . . . . . . . . . .. 31. 2–7. Tabla de Mediciones (Históricos) . . . . . . . . . . . . . . . . . . . . . . . .. 31. 2–8. Notación utilizada para AUVs. . . . . . . . . . . . . . . . . . . . . . . . . .. 33. 2–9. Ecuaciones del Filtro Extendido de Kalman Discreto . . . . . . . . . . . . .. 40. 3–1. Módulos de la Capa de Abstracción . . . . . . . . . . . . . . . . . . . . . .. 51. 3–2. Módulos de la Capa de Función . . . . . . . . . . . . . . . . . . . . . . . . .. 51. 3–3. Módulos de la Capa de Comunicación . . . . . . . . . . . . . . . . . . . . .. 51. 3–4. Módulos de la Capa de Navegación . . . . . . . . . . . . . . . . . . . . . . .. 52. 3–5. Módulos de la Capa de Bases de Datos . . . . . . . . . . . . . . . . . . . .. 52. 3–6. Módulos de la Capa de Sistema . . . . . . . . . . . . . . . . . . . . . . . . .. 53. 3–7. Razones de Uso . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 56. vii.

(11) Índice de figuras Figura. Página. 1–1. Ilustración del AUV Hugin. . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 7. 1–2. MARES AUV. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 9. 1–3. Estructura general del MARES. . . . . . . . . . . . . . . . . . . . . . . . . .. 9. 1–4. Cormoran AUV. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 9. 1–5. Estructura general del Cormoran. . . . . . . . . . . . . . . . . . . . . . . . .. 10. 1–6. Arquitectura de hardware para el HRC-AUV. . . . . . . . . . . . . . . . . . .. 11. 1–7. Diagrama de Flujo del software de comunicación. . . . . . . . . . . . . . . . .. 12. 1–8. Pantallas del software de supervisión. . . . . . . . . . . . . . . . . . . . . . .. 12. 1–9. Arquitectura del Filtro Extendido de Kalman. . . . . . . . . . . . . . . . . . .. 13. 1–10.Sistema de Tiempo Real. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 14. 2–1. Casos de uso generales. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 21. 2–2. Casos de Uso de la Estación Remota. . . . . . . . . . . . . . . . . . . . . .. 22. 2–3. Casos de Uso de la Estación Abordo. . . . . . . . . . . . . . . . . . . . . .. 23. 2–4. Nodos fundamentales. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 24. 2–5. Diagramas de Flujos de las Tareas Principales. . . . . . . . . . . . . . . . . . .. 28. 2–6. Sistemas de Referencias . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 32. 2–7. Planificación de Trayectoria . . . . . . . . . . . . . . . . . . . . . . . . . .. 44. 2–8. Guiado por LOS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 45. 3–1. Validación del algoritmo de Navegación. . . . . . . . . . . . . . . . . . . . . .. 54. viii.

(12) 3–2. Simulación del algoritmo de guiado. . . . . . . . . . . . . . . . . . . . . . . .. ix. 55.

(13) INTRODUCCIÓN. El medio marino es un ambiente rico en recursos naturales, aunque por lo difı́cil que resulta acceder a él, todavı́a es un terreno que permanece en su mayorı́a inexplorado. En los últimos años se ha suscitado un creciente interés por llegar a obtener estos recursos. A partir de aquı́, surge la necesidad del desarrollo de herramientas que sean capaces de adentrarse cada vez más en este medio. El desarrollo de vehı́culos sumergibles ha sido impulsado fundamentalmente por la industria petrolera, las comunicaciones intercontinentales y la industria militar; que cada vez se adentran más profundamente en los mares y océanos con el desarrollo de las plataformas de extracción de combustible, cables submarinos e investigaciones de la biodiversidad marina, entre otros. Ası́ surgen dos tendencias, primeramente están los Vehı́culos Operados Remotamente (ROV, Remotely Operated Vehicles), que resuelven la necesidad de la ausencia de tripulación pero imponen la restricción de la comunicación constante entre el vehı́culo y la estación de mando, por otra parte, se encuentran los Vehı́culos Sumergibles Autónomos (AUV, Autonomous Underwater Vehicles), los cuales gozan de gran popularidad en la actualidad, ya que por su autonomı́a, permiten cierta tolerancia en aspectos tan chocantes como la comunicación en el medio acuático y la presencia de tripulación en misiones que resultan a menudo riesgosas para la vida del hombre. Varios centros de investigación y universidades del mundo han mostrado un creciente interés en los AUVs, debido a las ventajas que ofrecen y el mercado que se genera alrededor de ellos. En Cuba su desarrollo todavı́a es escaso, sin embargo el Centro de Investigación y Desarrollo Naval (CIDNAV), se ha propuesto el reto de una implementación nacional de esta tecnologı́a debido a las posibilidades que brindan estos vehı́culos. Además, vale. 1.

(14) INTRODUCCIÓN. 2. destacar su especial utilidad en la explotación de los vastos recursos marinos que posee el archipiélago cubano. Con el objetivo de llevar a cabo el desarrollo de un sistema de supervisión y control para un prototipo desarrollado por el CIDNAV, que lleva por nombre HRC-AUV, este le presenta una propuesta de colaboración al Grupo de Automatización, Robótica y Percepción (GARP) de la Universidad Central de Las Villas(UCLV ), teniendo en cuenta la experiencia del último en materias de teledirección y modelado de vehı́culos. Ası́, se establece un proyecto de colaboración entre ambas partes con el objetivo de desarrollar un sistema autopiloto para el HRC-AUV. Con este fin se han desarrollado ya varios trabajos entre los que constan varias tesis de grado y publicaciones mostrando los resultados alcanzados. El inconveniente fundamental en la implementación de autopiloto para un AUV, recae en el sistema de navegación. Los sensores de posición absoluta convencionales como los Sistemas Globales de Navegación por Satélites (GNSS, Global Navigation Satellite Systems) (Ruiz, 2009) no tienen cobertura bajo el agua, las alternativas como los Sistemas de Posicionamiento Acústico (LBL, Long Baseline) (Cruz, 2008) resultan muy costosos y su utilización requiere el acondicionamiento previo del teatro de operaciones. Es por ello que para la aplicación en cuestión, GARP se ha dado a la tarea de implementar un Sistema de Navegación Inercial (INS, Inertial Navigation System). La implementación del sistema autopiloto en las unidades de cómputo, requiere el cumplimiento estricto del perı́odo de muestreo para el buen funcionamiento del INS, ası́ como el sistema de control en general. Esto ha constituido un problema en versiones anteriores debido a las altas latencias presentes en Sistemas Operativos (OSs, Operating Systems) como W indows c . Otro problema ha sido la implementación de los algoritmos de guiado desarrollados por el GARP, los cuales se ejecutaban en la estación de supervisión, limitando la autonomı́a del vehı́culo. A partir de aquı́ se propone un desarrollo en.

(15) INTRODUCCIÓN. 3. tiempo real que implemente algoritmos de navegación y guiado, sin perder las funcionalidades de versiones anteriores e incrementando sus prestaciones. Con este fin se plantean los siguientes objetivos: Objetivo general: Implementar y validar el software para la estación a bordo del HRC-AUV, integrando en el mismo los algoritmos desarrollados por GARP para el procesamiento y fusión de la información manejada por dicho medio. Objetivos especı́ficos: Determinar los requerimientos de la aplicación. Seleccionar el OS y lenguaje de desarrollo a emplear en función de los requerimientos determinados. Implementar módulos con algoritmos desarrollados por GARP para el filtrado y procesamiento de las señales manejadas por el HRC-AUV. Implementar módulos con algoritmos de navegación y guiado desarrollados por GARP para el HRC-AUV. Implementar la aplicación que será empleada en la estación a bordo del HRC-AUV para permitirle la capacidad de operación autónoma. Incluir como parte del trabajo de diploma, en secciones y anexos, información que le permita ser usado como manual de usuario de dicha aplicación. Con esta investigación se pretende sintetizar los conocimientos y avances alcanzados por el GARP en materia del sistema de autopiloto brindando una implementación del mismo para el HRC-AUV, que cumpla con las especificaciones y requerimientos temporales necesarios para el buen funcionamiento de la aplicación. El desarrollo de un vehı́culo nacional de este tipo permitirá al paı́s el uso de estas tecnologı́as ahorrándole grandes sumas en divisas que serı́an necesarias para su adquisición y mantenimiento en el mercado internacional..

(16) INTRODUCCIÓN. 4. Estructura y contenido del trabajo de diploma El trabajo de diploma, posterior a esta introducción, incluye tres capı́tulos, conclusiones, recomendaciones, referencias bibliográficas y anexos. Los contenidos de los capı́tulos, de forma resumida, son los siguientes: Capı́tulo 1 En el primer capı́tulo se realizará el análisis crı́tico de la literatura revisada. Primeramente se presentarán los principales conceptos que se tratan, introduciendo el tema de los vehı́culos autónomos y el desarrollo de software. Se abordará la panorámica general existente entorno al problema que motiva esta investigación. También se detallará la arquitectura general del sistema para el HRC-AUV. Capı́tulo 2 En el segundo capı́tulo se realizará el modelado y diseño de la aplicación, teniendo en cuenta algoritmos de corrección, integración sensorial, navegación y guiado. Capı́tulo 3 En el tercer capı́tulo, se abordará la implementación de la aplicación, brindando una panorámica de su estructura y funcionamiento. También se realizará una evaluación de los resultados alcanzados para su validación. Finalmente se presentarán las conclusiones y recomendaciones generales del trabajo..

(17) Capı́tulo 1 MARCO TEÓRICO. 1.1.. Introducción. El desarrollo de los vehı́culos autónomos ha tomado gran auge en los últimos años,. impulsado principalmente por la necesidad de ganar movilidad y capacidad de explotación de una fuente de recursos tan vasta e inexplorada como son los mares y océanos de nuestro planeta. Es por ello, que varias universidades y centros de investigación alrededor del mundo han venido desarrollando proyectos y publicaciones de un alto nivel cientı́fico, algunas de ellas se muestran a continuación: “Universidad Nacional del Sur” (UNS), Argentina (Jordán, 2008); “Universidad de Oporto”, Portugal (Ramos, 2008; Cruz, 2008), “Universidad BEIHANG”, China (Liang, 2008), “Universidad de Zagreb”, Croacia (Miskovic, 2008); “Universidad de Newcastle”, Australia (Pérez, 2008) y la “Universidad Noruega de Ciencia y Tecnologı́a” (NT NU) (Fossen, 1994, 2006, 2008). Por otra parte, se suman los estudios realizados por instituciones como el “Instituto de Problemas Tecnológicos Marinos de la Academia Rusa de Ciencias del Lejano Oriente” (IMT P F EB RAS) (Inzartsev, 2008) y la “Agencia de ciencia y tecnologı́a de tierra y mar” en Japón (Yoshida, 2008). Todas estas entidades han logrado mantener un desarrollo creciente de estas tecnologı́as, llevando a cabo prototipos que facilitan cada vez más la exploración y explotación de los recursos marinos en sus respectivos territorios. Los trabajos citados anteriormente, han servido de referencia para el desarrollo del autopiloto llevado a cabo por el GARP para el HRC-AUV. También, en el marco del proyecto de colaboración GARP-CIDNAV, se han presentado un conjunto de publicaciones que avalan la labor realizada en el marco del desarrollo del sistema autopiloto para el HRC-AUV. A continuación se enumeran algunas de ellas : 5.

(18) REVISIÓN BIBLIOGRÁFICA. 6. (Guerra, 2010; Cañizares, 2010; Sosa, 2010; Garcı́a, 2010; Rodriguez, 2011a,b; Martı́nez, 2010a,b) A lo largo de este capı́tulo se realizará una revisión bibliográfica acerca del desarrollo de los AUV en el mundo y las principales tendencias, ası́ como los principios y prácticas sobre el desarrollo de software con requisitos temporales estrictos, llevados al contexto de los AUVs, en especial a la arquitectura del prototipo del CIDNAV, sobre el cual se basará el desarrollo de esta tesis. 1.1.1.. Definición.. Los AUVs son vehı́culos motorizados que se trasladan en un medio acuático y realizan diferentes misiones sin llevar a bordo operadores humanos. Su capacidad de navegación autónoma le permite ejecutar tareas previamente programadas. Además pueden ser dirigidos desde estaciones remotas ubicadas para su monitoreo. Los AUVs forman parte de un gran grupo de robot submarinos conocidos como Vehı́culos Submarinos no Tripulados (UUV, Unmanned Underwater Vehicle) en los que se incluyen también los (ROV ) (Blidberg, 2001; Jaffe, 2001; Batlle, 2004). 1.1.2.. Aplicaciones.. El desarrollo de los AUVs ha brindado una herramienta de alta tecnologı́a capaz de extender la mano del hombre más allá de sus posibilidades en el medio marino. Estas ventajas son explotadas extensivamente por un diverso número de usuarios en diferentes entornos (Rodriguez, 2011a):.

(19) 7. REVISIÓN BIBLIOGRÁFICA. Aplicaciones Industriales. Aplicaciones Cientı́ficas. Aplicaciones Militares.    Supervisión de tuberı́as    Mantenimiento de plataformas petrolı́feras      Inmersión en aguas poco profundas    Monitoreo de volcanes marinos    Seguimiento de peces y grandes mamı́feros      Recolección de datos sobre corrientes oceánicas    Exploración   Detección y mapeo de campos minados. Un ejemplo claro de la aplicación de esta tecnologı́a lo constituye el AUV Hugin (Gorset, 2007),desarrollado por Kongsberg Maritime y Forsvarets Forsknings Institute (FFI) de Noruega. Está dispuesto para el mapeo de alta precisión del fondo marino, vigilancia y reconocimiento de minas. La comunicación con la superficie se realiza con señales acústicas.. Figura 1–1: Ilustración del AUV Hugin. 1.2.. Precedentes Los AUVs son sistemas que necesitan una arquitectura computacional y sensorial. a bordo capaz de satisfacer sus demandas de operación. Por lo general incluyen: sonares, cámaras, múltiples tipos de sensores, equipos de comunicación, entre otros dispositivos. Los.

(20) 8. REVISIÓN BIBLIOGRÁFICA. centros que trabajan en este tipo de proyectos emplean diversas estructuras de hardware y software para su funcionamiento. En el Cuadro 1–1 se muestran algunas. Cuadro 1–1: Comparación entre arquitecturas de AUVs AUV. Año. Odyssey II MIT USA OTTER MBARI USA ODIN II UH USA Tiram ITB Indonesia SEAWOLF I. 1993. STARFISH. 2008. MARES Cormorán. 2008 2009. 1994 1995 2003 2005. Sistema Operativo OS - 9. CPU Principal. Otros Medios de Cómputo MC68030/8M MC68HC11 SAIL network V x Works MVME 167 (68040) MVME 167 Protocolo NDDS V x Works VME MC68040 Windows 2000 Pentium III 7 microcontroladores Celeron 1.1 GHz Atmel AT90S8535 Linux PC/104 1 microcontrolador Kernel 2.6 550 MHz 1 DSP TMS320VC5502 Linux PC/104 1 microcontrolador 1 ARM SRT9 GNU-Linux PC/104 Windows XP PC/104 -. Entre las arquitecturas mostradas, destacan las dos últimas, MARES y CORMORAN. Ambas se basan en computadoras industriales y utilizan OSs convencionales: W indows c y GNU-Linux ; por lo cual guardan una estrecha relación con la arquitectura general del HRC-AUV, la cual se describe posteriormente en la Sección 1.2.3. 1.2.1.. MARES AUV. MARES (Modular Autonomous Robot for Environment Sampling) es un AUV de 1.5m de largo y 32Kg de masa (Cruz, 2008) programado para seguir trayectorias predefinidas, recolectar datos relevantes del medio ambiente en que se encuentra y sumergirse hasta un máximo de 100m. El sistema computacional a bordo se basa en una computadora de tipo PC-104. El software, desarrollado en C++, se ejecuta sobre un Kernel de Linux y está compuesto por un conjunto de procesos que se trabajan de forma independiente..

(21) 9. REVISIÓN BIBLIOGRÁFICA. (a) MARES AUV.. (b) PC-104 a bordo de MARES.. Figura 1–2: MARES AUV.. Figura 1–3: Estructura general del MARES. Para la estimación de la posición del vehı́culo, el sistema combina las mediciones de un sensor de presión, brújula digital y posicionamiento acústico LBL a través de un Filtro de Kalman (KF, Kalman Filter ). 1.2.2.. Cormorán AUV. El proyecto Cormorán (Ruiz, 2009), propone el desarrollo de una plataforma de observación oceánica de bajo costo. Su arquitectura sensorial incluye: brújula, inclinómetros, GPS, sonar, sensores: anticolisión, de temperatura, de humedad, entre otros.. (a) Cormoran AUV.. Figura 1–4: Cormoran AUV.. (b) PC-104 a bordo de Cormoran.. Su arquitectura computacional, se basa en una PC-104. El sistema de software a bordo corre sobre W indowsr XP Profesional y está desarrollado en Visual Basic..

(22) REVISIÓN BIBLIOGRÁFICA. 10. Figura 1–5: Estructura general del Cormoran. 1.2.3.. HRC-AUV. En función del desarrollo del sistema autopiloto para el HRC-AUV, el GARP ha llevado a cabo una ardua labor alcanzando resultados prometedores. Como se muestra en la figura 1–1, la tendencia en los últimos años ha venido convergiendo hacia la utilización de arquitecturas de la forma computadora-microcontrolador. Esta estructura utiliza computadoras industriales como la unidad de cómputo principal, unidas a sistemas empotrados basados en microcontroladores que desempeñan tareas especı́ficas dentro del sistema. En (Guerra, 2010), se describe la arquitectura general del HRC-AUV, a la cual se le han hecho modificaciones orientadas fundamentalmente a la actualización de los componentes, pero sin cambiar su estructura principal. La figura 1–6 muestra la arquitectura general tal y como se encuentra en la actualidad. A continuación se brinda una descripción de los componentes que la conforman: Los sensores principales utilizados son: Unidad de Movimiento Inercial (IMU, Inertial Movement Unit) MTI-G de Xsens. Sensor inteligente que contiene acelerómetros, magnetómetros y giróscopos en los tres ejes y que además calcula la actitud del vehı́culo. Esta versión del sensor, incluye un receptor GPS que brinda latitud , longitud, altitud y velocidad en los tres ejes..

(23) REVISIÓN BIBLIOGRÁFICA. 11. Figura 1–6: Arquitectura de hardware para el HRC-AUV. Cerabar T PMP 133 de Endress+Hauser. Sensor analógico de presión utilizado para determinar la profundidad del AUV. Actuador lineal eléctrico TLM30 de Tritex. Se encarga del posicionamiento de los timones. Incluye encoders (sensores digitales de posición) para medir la posición de los mismos. Sensor digital que emite una secuencia de pulsos cuyo intervalo es proporcional a las revoluciones del motor propulsor. Sensores digitales que detectan presencia de agua dentro del casco. Las unidades de cómputo utilizadas son: Computadora industrial tipo PC-104 , encargada de la adquisición de las mediciones del sensor MTI-G, además de ejecutar algoritmos de navegación, comunicación y guiado del vehı́culo. Hardware empotrado basado en Microchip DsPIC 30F4013 , encargado de la ejecución de los lazos de control de dirección y profundidad además de la adquisición.

(24) 12. REVISIÓN BIBLIOGRÁFICA. de varios sensores (profundidad, revoluciones del motor, carga de baterı́as y sensores de nivel de agua). Computadora de supervisión, encargada de correr el software supervisorio, que constituye la interfaz del sistema con el operadores humanos. En versiones anteriores, GARP desarrolló un software de comunicación para la estación a bordo, que se ejecutaba sobre W indowsr XP Profesional desarrollado en C# y encargado de establecer el vı́nculo entre los elementos del sistema y de generar una base de datos históricos para el posterior análisis de los resultados. La figura 1–7 muestra el diagrama de flujo de las tareas que lo componen.. Figura 1–7: Diagrama de Flujo del software de comunicación. También se llevó a cabo un sistema de supervisión, que consta de varias pantallas para brindar a los usuarios de la aplicación diversas opciones de interacción con el vehı́culo:. (a) Pantalla de Supervisión.. (b) Pantalla de Control.. (c) Pantalla de Trayectoria.. Figura 1–8: Pantallas del software de supervisión..

(25) 13. REVISIÓN BIBLIOGRÁFICA. Pantalla de Supervisión: Es la pantalla destinada a mostrar información visual acerca del estado del vehı́culo y de las principales alarmas. Entre los datos que brinda, destacan: posición, velocidad, pose, dirección, profundidad, y estado de las alarmas. Pantalla de Control: Permite al Operador ajustar los controladores y llevar a cabo pruebas modificando los mandos de los lazos de control. Pantalla de Trayectoria: Permite la configuración de la trayectoria deseada, muestra además la evolución del móvil para evaluar el cumplimiento de la misión. Para llevar a cabo la navegación, en (Sosa, 2010), se desarrolló un algoritmo que integra las mediciones de la IMU, y las corrige con GPS (mientras el sistema se encuentra en la superficie) y el Modelo Dinámico para Navegación del Vehı́culo (DNVM, Dynamic Navigation Vehicle Model ) a partir de un Filtro Extendido de Kalman (EKF, Extended Kalman Filter ), cuya arquitectura se muestra en la Figura 1–9.. Figura 1–9: Arquitectura del Filtro Extendido de Kalman. Donde: b fb y wib. Mediciones de la aceleración y velocidad de giro. n pnib , vib y η2. Posición y velocidad del vehı́culo. ϕ, λ y hp. Latitud, longitud y profundidad. n y δT. Mandos de los actuadores (entradas al DNVM). n vib DNV M. Velocidad resultante del DNVM.

(26) REVISIÓN BIBLIOGRÁFICA 1.2.4.. 14. Ventajas y Desventajas. La principal desventaja que presenta el software de comunicación es que no implementa los algoritmos de navegación. El guiado del vehı́culo se lleva a cabo desde el software supervisorio, lo cual limita la autonomı́a del sistema, quedando en un plano crucial la comunicación con la estación de supervisión. Además, la utilización de W indowsr XP Profesional, limita los tiempos de muestreo debido a las altas latencias del sistema operativo que provocan el incumplimiento de los plazos de entrega e imposibilitan la atención a tiempo de los eventos del sistema a frecuencias en el orden de los 100Hz. Resulta crucial entonces, resolver estos problemas sin sacrificar las prestaciones incluidas en versiones anteriores. 1.3.. Sistemas de Tiempo Real. La implementación de los algoritmos de navegación y guiado presenta requerimientos. temporales estrictos, caracterı́stica que señala la necesidad de un Sistema Computacional de Tiempo Real (RTCS, Real Time Computational System). Un RTCS es aquel cuyo desempeño no depende solo de los resultados de sus cálculos y procedimientos computacionales, sino que además depende del instante de tiempo en el que estos se producen (Kopets, 2002). Un sistema computacional en tiempo real, es siempre parte de un sistema más grande denominado entonces Sistema de Tiempo Real (RTS, Real Time System). Para su estudio, se descompone en tres subsistemas:. Figura 1–10: Sistema de Tiempo Real..

(27) 15. REVISIÓN BIBLIOGRÁFICA. Subsistema Controlado: Está conformado por el objeto de control (planta, sensores, actuadores). Subsistema Computacional: Está conformado por el sistema computacional de tiempo real. Subsistema Operador: Está conformado por el operador humano. 1.3.1.. Clasificaciones de los Sistemas de Tiempo Real. De acuerdo a sus caracterı́sticas, los RTS se pueden clasificar en (Kopets, 2002): Tiempo Real Fuerte - Tiempo Real Leve El diseño de sistemas de Tiempo Real Fuerte (HRT, Hard Real Time) es esencialmente diferente de los sistemas de Tiempo Real Leve (SRT Soft Real Time) por los requerimientos y tolerancias inherentes a ambos. A continuación se muestra un Cuadro comparativo que ilustra lo anteriormente expuesto (Kopets, 2002). Cuadro 1–2: Comparación entre HRT y SRT Caracterı́stica Tiempo Real Fuerte tiempo de respuesta preciso desempeño ante picos de carga predecible sincronismo medio ambiente. Tiempo Real Leve medio degradable computadora. Tiempo de Respuesta: Las demandas de tiempo de respuesta en los sistemas HRT, generalmente del orden de los milisegundos o menos, debe ser respetada en todo momento y requiere autonomı́a de la intervención de operadores tanto en operación normal como en situaciones crı́ticas. Los sistemas con requerimientos de SRT, por el contrario, presentan tiempos de respuesta en el orden de los segundos, y si se incumple un plazo de entrega, los resultados no son graves. Desempeño ante picos de carga: En los sistemas HRT, el escenario de los picos de carga debe estar bien definido, se debe garantizar que la arquitectura computacional del sistema permita el cumplimiento de los plazos de entrega a pesar de los picos. En los sistemas SRT, el desempeño medio del sistema es lo realmente importante, y la.

(28) REVISIÓN BIBLIOGRÁFICA. 16. degradación de las condiciones de operación a razón de un pico de carga que ocurre de forma aislada, es tolerada por razones económicas. Sincronismo: El sistema de HRT, debe mantenerse sincronizado con su medio ambiente (objeto de control y operador), ante todos los posibles escenarios. Los SRT, en cambio, pueden ejercer algún tipo de control sobre el medio (por ejemplo, modificar la velocidad de respuesta) en caso que no se pueda procesar la carga de datos requerida. Seguridad ante Fallos - Operabilidad ante Fallos En algunas aplicaciones de HRT, existen uno o más estados seguros que pueden alcanzarse en caso de la ocurrencia de un fallo. Si este es el caso, y el estado seguro se puede alcanzar en un tiempo lo suficientemente pequeño como para evitar roturas graves o grandes pérdidas materiales, el sistema se dice que es seguro ante fallos. Sin embargo, en algunas aplicaciones, no existe este tipo de estados seguros, en cambio, el sistema debe ofrecer un mı́nimo de operabilidad para evitar pérdidas graves, en este caso se dice que el sistema debe ser operable ante fallos (Kopets, 2002). Respuesta Garantizada - Mejor Esfuerzo Si al analizar un sistema partiendo de un evento cualquiera, aún en presencia de un pico de carga, se puede garantizar capacidad de respuesta sin necesidad de acudir a argumentos probabilı́sticos, se dice que el sistema brinda respuesta garantizada, si este supuesto no se puede garantizar, entonces se plantea que el sistema está diseñado de acuerdo al mejor esfuerzo (Kopets, 2002). 1.3.2.. Requerimientos de los sistemas de tiempo real. Los requerimientos de un RTS se dividen en: funcionales y temporales (Kopets, 2002). Requerimientos Funcionales: Recolección de Datos: El sistema se encarga de encuestar los sensores para obtener información acerca del estado del objeto de control, esta función incluye el acondicionamiento de las señales y la detección de alarmas..

(29) REVISIÓN BIBLIOGRÁFICA. 17. Ejecución del Control Digital: El sistema de tiempo real, debe calcular, a partir de un algoritmo de control, el mando de los actuadores. Interacción Hombre-Máquina: El sistema debe informar constantemente al operador del estado de la planta y asistirlo en el proceso de su manipulación y control. Requerimientos Temporales: Tiempo de Cómputo: Dado que los algoritmos de control son generalmente cı́clicos, el sistema debe ser capaz de devolver el resultado en un plazo de entrega menor que el perı́odo de muestreo. Mı́nima Latencia de Jitter: El retardo de jitter es un factor de incertidumbre en el lazo de control y debe ser minimizado a toda costa, ya que la mayorı́a de los sistemas de control lo consideran cero. 1.4.. Sistema Operativo Dados los requerimientos expuestos anteriormente, resulta de vital importancia la. selección de un OS para la PC-104, que se ajuste a los requerimientos temporales de la aplicación. En el Cuadro 1–1 se incluye una comparación entre los OSs utilizados atendiendo a las diferentes arquitecturas. A partir de la evolución de los sistemas empotrados hacia las computadoras industriales, se evidencia una tendencia a la utilización de sistemas operativos de propósito general como W indows c y GNU-Linux en arquitecturas similares a la presentada en la sección 1.2.3. En versiones anteriores, se utilizó W indows c XP Profesional sobre una PC-103 (Rodriguez, 2011b,a) obteniéndose buenos resultados, pero con limitaciones en cuanto a la frecuencia de muestreo debido a las altas latencias inherentes a este OS. Además, W indows c , presenta el inconveniente de que es un software propiedad de la compañı́a transnacional estadounidense Microsof tr , por lo cual Cuba no puede adquirir las licencias pertinentes debido a la polı́tica de embargo seguida por los Estados Unidos. Una opción más tentadora la constituye el GNU-Linux, el cual a diferencia de Windows surge a partir del.

(30) REVISIÓN BIBLIOGRÁFICA. 18. movimiento de software libre y está sujeto a la Licencia Pública General (GPL, General Public License). 1.4.1.. Potencialidades de Linux para el trabajo en tiempo real. En su forma básica, GNU-Linux no es un OS diseñado para el trabajo en tiempo real. Los principales aspectos que atentan contra ellos son: 1. La programación de las tareas depende de la carga del sistema. 2. Las llamadas de sistema a funciones del kernel son ininterrumpibles. 3. Utiliza un espacio de memoria virtual. 4. Desabilita el manejo de interrupciones en modo usuario para ganar en sincronización. 5. Reordena las peticiones de entrada-salida para ganar eficiencia. Además, los procesos de Linux son en sentido general pesados, por lo cual un cambio de contexto suele tomar varios cientos de microsegundos. A pesar de estos obstáculos, la tendencia al uso de OSs convencionales en aplicaciones de tiempo real se ha venido incrementando. Ası́, variantes y modificaciones al Kernel de Linux han venido impulsando sus prestaciones para este tipo de aplicaciones. A partir de la distribución 2.6.29 del Kernel de Linux, se incluye en el código fuente un parche que corrige varios de los obstáculos antes mencionados permitiendo la implementación de temporizadores con perı́odos de hasta 1ms y zonas de interrupción en las llamadas de sistema que reducen las latencias del OS. Nótese que los requerimientos de frecuencias de trabajos de 100Hz, lo cual equivale a 10ms, se cumplen perfectamente con esta variante. Es por esto que para la aplicación en cuestión se selecciona el OS GNU-Linux con la versión del Kernel 2.6.31.2 distribución Debian 5.7 lenny. 1.5.. Conclusiones parciales del capı́tulo. En este capı́tulo se ha realizado un análisis introductorio al tema de los AUVs, princi-. pales conceptos y aplicaciones, quedando evidenciada la importancia de una investigación como esta. A partir del análisis de los precedentes, llevado a cabo en la sección 1.2, se han podido determinar los principales requerimientos del software a implementar:.

(31) REVISIÓN BIBLIOGRÁFICA. 19. 1. Se deben implementar algoritmos de navegación y guiado capaces de garantizar la autonomı́a del vehı́culo. 2. El sistema debe desarrollar frecuencias de muestreo no menores de los 100Hz en la ejecución del algoritmo de navegación. 3. Se debe generar una base de datos históricos que permita evaluar el desempeño del sistema y validar los algoritmos de control y navegación desarrollados por GARP. 4. Se debe incluir un sistema de comunicación capaz de integrar eficientemente los demás componentes del sistema en general. De acuerdo a los requerimientos, el sistema debe contar con las siguientes caracterı́sticas: HRT: Aunque el sistema de control pudiera no resultar tan exigente de acuerdo a las constantes de tiempo dominantes en el sistema, los algoritmos de navegación inercial, requieren de alta precisión en pos de minimizar los errores que conducen a la desviación del vehı́culo de la trayectoria deseada. Esto se consigue garantizando la atención al sensor inercial. Seguro ante Fallos: El HRC-AUV presenta flotabilidad positiva, por lo cual sale a la superficie en ausencia de la acción del sistema de propulsión, este comportamiento garantiza la alcanzabilidad del estado seguro una vez desconectado el sistema de propulsión. Respuesta Garantizada frente a eventos de navegación y alarmas: El sistema debe garantizar una respuesta rápida frente a los eventos del sensor inercial y las alarmas de presencia de agua. Respuesta de Mejor Esfuerzo frente a eventos de supervisión y configuración: La capacidad de autonomı́a del vehı́culo, le permite prescindir de la comunicación con la estación de supervisión durante intervalos prudenciales de tiempo. También se seleccionó el OS GNU-Linux como la mejor opción de acuerdo a los requerimientos de la aplicación, señalándose la necesidad de activar el parche de baja latencia, utilizándose el Kernel 2.6.31.2..

(32) Capı́tulo 2 DISEÑO DE SOFTWARE. 2.1.. Introducción. En el presente capı́tulo, se desarrolla el modelo general del problema en cuestión. ası́ como el diseño base que se utilizará como punto de partida para la implementación del software de navegación en tiempo real para la PC-104 del vehı́culo. 2.2.. Modelado del problema. La capacidad de procesamiento de información de la mente humana, comparada con. las grandes cantidades de información presentes en los procesos del mundo real es limitada. Esto hace necesaria la implementación de una estrategia de reducción de la información en función de metas concretas para producir una representación simplificada del mundo, un modelo. Para el desarrollo apropiado del modelo del sistema, se utiliza el Lenguaje Unificado de Modelado (UML, Unified Model Language). El UML, es una de las herramientas más populares en el mundo del desarrollo de sistemas. Esta le permite a los creadores generar modelos que capturen sus ideas en formas convencionales y fáciles de comprender para comunicarlas a otras personas (Schuller, 2000). En la figura 2–1 se muestra el diagrama general de casos de uso. En el que se incluyen los principales actores que interactúan con el sistema, ası́ como los casos de uso que se generan en torno a ellos.. 20.

(33) 21. DISEÑO DE SOFTWARE. Figura 2–1: Casos de uso generales. 2.2.1.. Actores.. El sistema cuenta con dos actores fundamentales, cada uno asociado a casos de uso especı́ficos: Supervisor: Es el actor que está relacionado directamente con el caso de uso Supervisión. Mediante este caso de uso el mismo puede visualizar el conjunto de variables que definen el estado del sistema a partir de una interfaz gráfica. Operador: Este actor es el encargado de la manipulación y el control del vehı́culo y para ello cuenta con el conjunto de funcionalidades descritas por los casos de uso: Teledirección, Identificación, Ajuste de Control y Trayectoria. Mediante estos casos de uso, el Operador puede: realizar el guiado del sistema a través de la teledirección sin la intervención de los lazos de control, realizar experimentos a lazo abierto para la obtención de juegos de datos con vista a la identificación experimental del vehı́culo, cambiar y probar el ajuste de los controladores que intervienen en el manejo automático del móvil y trazar trayectorias a seguir por el vehı́culo. 2.2.2.. Casos de uso.. Las actividades a llevar a cabo por los actores dan lugar a un conjunto de casos de uso que describen los requerimientos generales del sistema y definen las funcionalidades.

(34) 22. DISEÑO DE SOFTWARE. necesarias para el cumplimiento de su propósito. A continuación se describen las particularidades de los casos de uso expuestos en el diagrama general mostrado en la figura 2–1:. (a) Caso de Uso Supervisión. (b) Caso de Uso Ajuste de Control. (c) Caso de Uso Trayectoria. Figura 2–2: Casos de Uso de la Estación Remota. Supervisión: Este caso de uso está asociado directamente al actor Supervisor. Su función es la de brindar al actor información visual acerca de las variables que describen el estado del sistema (2–2(a)): La posición del vehı́culo en un mapa bidimensional en coordenadas de latitud y longitud (o ). La pose del vehı́culo con respecto al plano horizontal (horizonte) (o ). La dirección del vehı́culo con respecto al norte magnético de la tierra (brújula) (o ). La profundidad del vehı́culo en (m). La velocidad de crucero del vehı́culo (m/s). Además se muestran señalizadores de las alarmas más importantes como son: la presencia de agua en el interior del casco, estado del motor de la propela (encendido, apagado) y temperatura. Teledirección: Este caso de uso le permite al operador dirigir directamente el vehı́culo sin la intervención de los controladores, a partir de un joystick o de un teclado a través.

(35) 23. DISEÑO DE SOFTWARE. del software de supervisión corriendo en la estación remota, esta funcionalidad es especialmente útil cuando el vehı́culo realiza maniobras de precisión que requieren de la experiencia de un operador humano. Identificación: Este servicio, brinda la posibilidad de realizar experimentos en lazo abierto a los actuadores de los lazos de dirección y profundidad, con el objetivo de obtener juegos de datos para la identificación experimental de algunos parámetros del modelo matemático del sistema. Ajuste de Control: Brinda la posibilidad de modificar las ganancias de los controladores presentes en el vehı́culo, activar y desactivar de manera independiente los lazos de control y definir los valores deseados de rumbo y profundidad para verificar el funcionamiento del sistema de control (2–2(b)). Trayectoria: Permite al operador definir una trayectoria deseada a partir de un conjunto de puntos ordenados que el vehı́culo debe seguir (2–2(c)).. (a) Caso de Uso Atención de Sensores. (b) Caso de Uso Manejo del Vehı́culo. (c) Caso de Uso Sistema de Navegación. (d) Caso de Uso Configuración. (e) Caso de Uso Guiado del Vehı́culo. Figura 2–3: Casos de Uso de la Estación Abordo..

(36) 24. DISEÑO DE SOFTWARE. Navegación: Es el caso de uso relacionado con el seguimiento del estado del vehı́culo, ası́ como de su control. Este incluye los casos de uso: Lectura de Sensores, Sistema de Navegación, Configuración de Controladores, Manejo del Vehı́culo y Guiado del Vehı́culo. Atención a Sensores: Abarca la adquisición de la información proveniente de los sensores, ası́ como su acondicionamiento y conversión a unidades de ingenierı́a (2–3(a)). Manejo del Vehı́culo: Incluye la posibilidad de cambiar de estado manual a automático y viceversa, ası́ como definir respectivamente el mando de los actuadores (en lazo abierto) o de los controladores (en lazo cerrado) (2–3(b)). Sistema de Navegación: Incluye algoritmo de estimación de posición, a partir de las mediciones de la IMU corregido por las mediciones del resto de los sensores presentes en el vehı́culo a partir del DNVM y el EKF (2–3(c)). Configuración: Permite actualizar la configuración del software de navegación y propagarla hacia el hardware de control si es necesario (2–3(d)). Guiado del Vehı́culo: El guiado se lleva a cabo a partir de un arreglo de puntos deseados manipulando el mando de los lazos de control y venciendo los puntos de consigna uno por uno (2–3(e)). 2.3.. Sistema de Comunicación. El sistema en general presenta una arquitectura distribuida, en la cual se identifican. 4 nodos fundamentales:. Figura 2–4: Nodos fundamentales..

(37) DISEÑO DE SOFTWARE. 25. 1. Nodo de Control (Hardware de Control basado en DsPic 30F-4013 ). 2. Nodo IMU (Sensor inteligente MTI-G). 3. Nodo de Navegación (PC-104 ). 4. Nodo de Supervisión (Computadora de Supervisión). Un nodo es una unidad de cómputo independiente con sus propios recursos de hardware (memoria, procesador e interfaz de comunicación) y software (programas de aplicación y sistema operativo) que realiza un conjunto de tareas bien definidas en el sistema computacional distribuido (Kopets, 2002). Estos nodos interactúan entre sı́ a través de un sistema de comunicación serie basado en el protocolo RS-232 y transmisión por radio frecuencia (RF ), como se muestra en la figura 2–4. En la figura también se muestra el flujo de información. Es evidente que el sistema se encuentra centrado en la PC-104, ya que es la encargada de almacenar, procesar y transmitir la información proveniente del resto de los nodos y sirve de vı́nculo integrador para el sistema en general. El sistema de comunicación, es el encargado de vincular y coordinar las tareas desarrolladas por los diferentes nodos en el RTS. 2.4.. Estructura del sistema de comunicación En el caso del HRC-AUV el sistema de comunicación se encuentra dividido en dos. clases fundamentales: Comunicación IMU (enlace entre PC-104 e IMU ) y Comunicación GARP (enlace entre dispositivos programados por GARP). La Comunicación GARP, a su ves se divide en dos más: Comunicación DsPic (enlace entre PC-104 y DsPic) y Comunicación Remota (enlace entre PC-104 y Computadora de Supervisión), como se muestra en el Cuadro 2–1. Cuadro 2–1: Esquema de Comunicación   Comunicación         DsPic    Comunicación     GARP    Sistema de Comunicación    Remota Comunicación         Comunicación    IMU.

(38) 26. DISEÑO DE SOFTWARE Cuadro 2–2: Estructura de mensajes Comunicación IMU. Preámbulo. Comunicación GARP. BID MID. Preámbulo. Longitud del Dato Dato Checksum. MID Longitud del Dato Dato Checksum. La información se transmite de un nodo a otro en forma de mensajes. La estructura general de estos mensajes presenta los siguientes campos: Los mensajes intercambiados se clasifican en dos grupos: mensajes de configuración y mensajes de datos. A continuación se describen los mensajes utilizados por cada clase de comunicación. Una descripción más detallada, se brinda en el Apéndice A. Cuadro 2–3: Mensajes Clasificación Configuración. Datos. Clasificación Configuración Datos. Clasificación Configuración Datos. Comunicación IMU Mensaje Dirección GoToConfig PC-104 → IMU SetConfig PC-104 → IMU SetMode PC-104 → IMU GoToMeasurement PC-104 → IMU MTData PC-104 ← IMU. Ack si si si si no. Comunicación DsPic Mensaje Dirección SetControlConfig PC-104 → DsPic SetMainConfig PC-104 → DsPic ReqData PC-104 → DsPic DsPicData PC-104 ← DsPic. Ack si si si no. Comunicación Remota Mensaje Dirección SetControlConfig PC-104 ← CPU Remota SetMainConfig PC-104 ← CPU Remota TrayectoryData PC-104 ← CPU Remota StateData PC-104 → CPU Remota. Ack si si no no.

(39) DISEÑO DE SOFTWARE. 27. Más información acerca de la Comunicación IMU, incluyendo el contenido de los mensajes y una descripción detallada de su propósito y efecto, puede encontrarse en (Xsens, 2008). 2.5.. Tareas Una tarea: es la ejecución de una función o programa secuencial. Generalmente inicia. con la lectura de la entrada y su estado interno y termina produciendo un resultado y actualizando su estado. (Kopets, 2002) En la sección 2.2 se describe el diagrama de casos de uso general del sistema, del cual se derivan sus principales requerimientos funcionales. A partir de ellos se establece el conjunto de tareas que el sistema deberá llevar a cabo: Atención a Sensores Navegación Guiado Actualización de la Base de Datos Configuración Enviar Datos al Supervisorio Si agrupamos estas tareas de acuerdo a las relaciones de dependencia existentes entre ellas, se pueden definir 4 tareas principales que pueden correr al unı́sono. Las tareas principales en el sistema son: Navegación: Incluye las funciones de atención de los sensores, navegación y actualización de la base de datos históricos. Esta tarea se sincroniza con el mensaje de datos del sensor inercial trabajando a 100Hz. Guiado: Esta tarea se encarga de correr los algoritmos de guiado del vehı́culo. Es controlada por un temporizador a 10Hz Servidor de Datos Se encarga de enviar periódicamente los datos relacionados con el estado del sistema a la estación de supervisión para ser mostrados al usuario. Esta operación se controla por un temporizador a 5Hz..

(40) 28. DISEÑO DE SOFTWARE. Configuración Se encarga de manejar los cambios de configuración definidos por la estación de supervisión y propagarlos al resto de los nodos en caso de que sea necesario. En la figura 2–5 se muestran los diagramas de flujo concebidos para estas tareas.. (a) Navegación.. (b) Guiado.. (c) Servidor de Datos.. (d) Configuración.. Figura 2–5: Diagramas de Flujos de las Tareas Principales. 2.5.1.. Asignación de prioridades.. Las prioridades se asignan según el método de Frecuencia Monótona (RMA, Rate Monotonic Analysis), el cual plantea, que a mayor frecuencia, mayor prioridad. El orden de las tareas, de acuerdo a este criterio se muestra a continuación. 1. Navegación (100Hz) 2. Guiado (10Hz) 3. Servidor de Datos (5Hz) 4. Configuración (menos de 5Hz) 2.6.. Manejo de Datos. Los datos procesados por el sistema provienen de dos fuentes fundamentales: la IMU,. y el hardware de control. La IMU transmite las aceleraciones, velocidades de giro e intensidad del campo magnético en tres direcciones. Además, realiza la estimación de la actitud del vehı́culo expresada en ángulos de Euler. La posición en latitud, longitud y altitud,.

(41) DISEÑO DE SOFTWARE. 29. ası́ como las velocidades en tres ejes, son transmitidas en el caso de estar disponibles las mediciones del receptor GPS integrado al sensor. El hardware de control se encarga de la adquisición del sensor de presión, mediante el cual se estima la profundidad, de los encoders que transmiten la posición de los actuadores, el sensor digital encargado de medir la velocidad de rotación de la propela y los sensores digitales de presencia de agua. 2.6.1.. Bases de Datos.. Para la organización y accesibilidad de los datos manejados por el sistema, se proponen dos Bases de Datos: Base de Datos de Tiempo Real (RTDB, Real Time Database) y Base de Datos Históricos (HDB, Historical Database). La primera se encarga de mantener una imagen estática del estado del sistema en tiempo real, ası́ como la información de configuración de la aplicación. Esta permite a las tareas intercambiar y mantener actualizada la información necesaria para llevar a cabo sus funciones. La segunda, se encarga de almacenar consecutivamente, muestras del estado del sistema, creando un arreglo de datos históricos del mismo. Esta permite analizar fuera de lı́nea su comportamiento, evaluar su rendimiento, detectar y diagnosticar errores ası́ como comprobar y validar nuevos algoritmos a partir de los datos almacenados. La RTDB, consta de 3 tablas: 1. Tabla de Estado (2–4) 2. Tabla de Configuración (2–5) 3. Tabla de Alarmas (2–6) La HDB, cuenta con las mismas tablas que la RTDB, además de una Tabla de Mediciones (2–7) La RTDB, utiliza estructuras de C y almacena solamente un registro en cada una de sus tablas, con el objetivo de mantener información actualizada acerca del estado, configuración y alarmas del sistema. Nótese que la RTDB constituye una sección crı́tica.

(42) DISEÑO DE SOFTWARE Cuadro 2–4: Tabla de Estado (Tiempo Real) Campo φ θ ψ λ ϕ z φ̇ θ̇ ψ̇ ẋ ẏ ż u. 30. Tipo de Dato Descripción. double Inclinación lateral del sistema. double Inclinación Frontal del sistema. double Dirección con respecto al norte magnético. double Latitud. double Longitud. double Profundidad. double Razón de cambio de φ. double Razón de cambio de θ. double Razón de cambio de ψ. double Velocidad en dirección norte. double Velocidad en dirección este. double Velocidad hacia abajo. double Velocidad crucero del vehı́culo. Cuadro 2–5: Tabla de Configuración (Tiempo Real). Campo Tipo de Dato Descripción. Man-Aut boolean Cambio de manual a automático. Y Kp double Ganancia proporcional, lazo de dirección. Y Kd double Ganancia derivativa, lazo de dirección. Y Ki double Ganancia integral, lazo de dirección. P rKp double Ganancia proporcional, lazo de profundidad. P rKi double Ganancia integral, lazo de profundidad. P Kp double Ganancia proporcional, lazo de cabeceo. P Kd double Ganancia derivativa, lazo de cabeceo. P Ki double Ganancia integral, lazo de cabeceo. Ysp double Mando al lazo de dirección. P rsp double Mando al lazo de profundidad. SegT ray boolean Activar / Desactivar seguimiento de trayectoria. OnOf f boolean Señal de Encendido / Apagado del motor de la propela. en el espacio de memoria de la aplicación, ya que se utiliza para intercambiar información entre las tareas. Para proteger la integridad de sus datos, se utiliza, un objeto de Exclusión Mutua (mutex, Mutual Exclusion) encargado de sincronizar el acceso a la misma. La Base de Datos Históricos, por otra parte, utiliza ficheros de texto e incluye un registro, en cada una de sus tablas por cada instante de muestreo en la tarea de Navegación. Esto permite validar el comportamiento del sistema, ası́ como detectar y diagnosticar errores. Nótese que la Base de Datos Históricos, brinda la posibilidad de recrear fuera de lı́nea el entorno en el que se desenvuelve el sistema, permitiendo un análisis detenido del mismo frente a los cambios en los mandos y las perturbaciones, ası́ como la puesta a prueba.

(43) DISEÑO DE SOFTWARE Cuadro 2–6: Tabla de Alarmas (Tiempo Real). 31. Campo Tipo de Dato Descripción. Man-Aut boolean Cambio de manual a automático. H2 O(popa) boolean Señal del sensor de agua en la popa. H2 O(proa) boolean Señal del sensor de agua en la proa. P ropOn boolean Confirmación del estado del motor de la propela. Cuadro 2–7: Tabla de Mediciones (Históricos) Campo fb wb EulAng P MT I − G V MT I − G Pr n δT δP. Tipo de Dato Descripción. float[3] Mediciones de los acelerómetros. float[3] Mediciones de los giróscopos. float[3] Ángulos de Euler estimados por la IMU. float[3] Latitud, Longitud y Altitud, Medidas por la MTI-G. float[3] Velocidades Medidas por la MTI-G. float Profundidad. float Velocidad de la propela en rpm. float Posición del timón de dirección. float Posición del timón de profundidad.. de nuevos algoritmos sin la necesidad de la presencia fı́sica del vehı́culo o el desarrollo de un nuevo experimento. 2.7.. Navegación Inercial Según (Sosa, 2010) un sistema de navegación inercial (INS ) consta de:. Unidad de Medición Inercial (IMU ): Conteniendo al menos tres acelerómetros y tres giróscopos, montados en una base común, manteniendo la ortogonalidad entre los ejes. Computadora de Navegación: Calcula el efecto de la gravedad y corrige los biases y drifts de los acelerómetros de la IMU. A su vez, debe integrar doblemente los valores de la aceleración para obtener la posición estimada. El inconveniente fundamental de este procedimiento es la acumulación del error. El proceso de integrar doblemente las aceleraciones acarea un error que crece con el tiempo y eventualmente supera los márgenes de tolerancia de la aplicación. Es por esto que para reducir al mı́nimo estos errores, se utiliza el algoritmo de Filtro Extendido de Kalman (EKF, Extended Kalman Filter ) (Rogers, 2003). La fuente de corrección más frecuente en la implementación del EKF para soluciones de navegación es el GPS, pero, como el mismo no se encuentra disponible bajo el agua, para la aplicación en cuestión se introduce además.

(44) 32. DISEÑO DE SOFTWARE. con este objetivo el Modelo Dinámico del Vehı́culo para Navegación (DNVM, Dynamic Navigation Vehicle Model ). De acuerdo con el diagrama descrito en 1–9, el algoritmo de EKF para INS, cuenta con 4 etapas fundamentales: Suavizado de las Mediciones, Mecanización Inercial, Evaluación del DNVM y Corrección mediante EKF. 2.7.1.. Sistemas de Referencia.. Para el desarrollo del sistema de navegación, se utilizan dos sistemas de referencia, como se muestra en la Figura 2–6.. Figura 2–6: Sistemas de Referencias A continuación, se muestra una breve descripción para ambos (Fossen, 1994; Titterton, 2004): n-frame: Despreciando los efectos de la rotación, se puede definir un sistema cartesiano ortogonal inercial a partir de un punto cualquiera sobre la superficie terrestre con los ejes nX , nY y nZ apuntando hacia el norte, este y abajo respectivamente. b-frame: Es un sistema cartesiano ortogonal ubicado en el centro de masa del vehı́culo y alineado según la actitud del mismo. En la tabla 2–8 se describe la nomenclatura utilizada para la posición, velocidad, fuerzas y momentos: A partir de la notación anterior, se pueden definir el vector de posiciones con respecto al n-frame y el de velocidades con respecto al b-frame: . .  η1  η=  con η1 = η2. . x, y, z. T. y η2 =. . φ, θ, ψ. T. (2.1).

(45) 33. DISEÑO DE SOFTWARE Cuadro 2–8: Notación utilizada para AUVs. Traslación Fuerza Avance X Desplazamiento lateral Y Arfada Z Rotación Momento Balanceo K Cabeceo M Guiñada N. . .  ν1  ν=  con υ1 = ν2. . u, v, w. Velocidad lineal Posición u x v y w z Velocidad angular Ángulo p φ q θ r ψ. T. y υ2 =. . p, q, r. T. (2.2). A partir de las transformaciones de ángulos de Euler se pueden definir las relaciones entre las magnitudes:. η̇ = J(η)ν. (2.3). donde:. . . .  cψcθ (cψsθsφ − sψcφ) (sψsφ + cψcφsθ) J (η ) 0  1 2   J(η) =   J1 (η2 ) =   sψcθ (cψcφ + sφsθsψ) (sθsψcφ − cψsφ)  0 J2 (η2 ) −sθ cθsφ cθcφ . .  1 tθsφ tθcφ     J2 (η2 ) =   0 cφ −sφ    cφ sφ 0 cθ cθ donde: c∗ = cos(∗), s∗ = sen(∗) y t∗ = tan(∗), y notando que θ 6= π2 ..      .

(46) 34. DISEÑO DE SOFTWARE 2.7.2.. Suavizado. El proceso de suavizado consta de dos etapas fundamentales (Garcı́a, 2010; Shin, 2001): Filtrado: En primer lugar las mediciones, tanto de los acelerómetros como los giróscopos, son muy ruidosas. Además, en el medio marino se encuentran contaminadas por el efecto del oleaje. Con el objetivo de eliminar estas fuentes de ruido se plantea la utilización de un filtro paso bajo de buterworth. Para el diseño del filtro se utiliza el software MAT LAB c , especı́ficamente la función “butter” del toolbox de procesamiento de señales. De acuerdo con las investigaciones previamente realizadas por GARP con respecto al oleaje en condiciones de mar fuerza 2 (Garcı́a, 2010), se seleccionó una frecuencia de corte de 6,2rad/s. Para la utilización de la función butter, es necesario normalizar la frecuencia de corte con respecto a la frecuencia de Nyquist correspondiente a la mitad de la frecuencia de muestreo del sistema. Para una frecuencia de muestreo de F m = 100Hz, la frecuencia de corte normalizada serı́a:. Wn =. Wc 6,2 = = 0,0194 Fm · π 100π. (2.4). El filtro digital resultante tiene la siguiente función de transferencia:. G(Z) =. (0,0797 + 0,3189Z −1 + 0,4784Z −2 + 0,3189Z −3 + 0,0797Z −4) × 10−5 1 − 3,8408Z −1 + 5,5348Z −2 − 3,5468Z −3 + 0,8527Z −4. (2.5). Calibración: En las ecuaciones 2.6 y 2.7 se muestran los modelos reducidos de las mediciones de los acelerómetros y los giróscopos, una explicación más detallada de los modelos se brinda en el Apéndice C.. δf b = b + wf. (2.6). δw b = d + ww. (2.7).

(47) 35. DISEÑO DE SOFTWARE. A partir de las ecuaciones 2.6 y 2.7 se puede trazar una estrategia basada en el hecho de que independientemente de la pose del sensor, en condiciones estáticas o de velocidad constante en el caso de los acelerómetros, el valor total de las mediciones de la IMU para los acelerómetros y giróscopos, serán iguales a la aceleración de la gravedad y 0 respectivamente. A partir de la afirmación anterior y dado el hecho de que ww es un componente ruidoso de media 0 se puede estimar el drift de los giróscopos como la media de sus mediciones en un experimento estático de acuerdo a la ecuación 2.8: n. d=. 1X b w n i=1 i. (2.8). donde: n. Número de muestras. w b Medición del sensor En el caso de los acelerómetros, se puede calcular la componente de la gravedad en cada uno de los ejes del sensor como:. b fgx = −sen(θ)g + bx + wf x. (2.9). b fgy = sen(φ)cos(θ)g + by + wf y. (2.10). b fgz = cos(φ)cos(θ)g + bz + wf z. (2.11). donde: fgb. Mediciones del sensor inercial de las componentes de la gravedad. g Aceleración de la gravedad A partir de aquı́, se puede definir:. y estimar b como:. b bxi = fgxi + sen(θ)g. (2.12). b byi = fgyi − sen(φ)cos(θ)g. (2.13). b bzi = fgzi − cos(φ)cos(θ)g. (2.14).

(48) 36. DISEÑO DE SOFTWARE. n. 1X b= bi n i=1 con bi = 2.7.3.. . bxi byi bzi. T. (2.15). .. Sistema de Navegación Inercial. El proceso de mecanización, es el corazón de INS. A partir de las mediciones suavizadas del sensor inercial, se integran doblemente las aceleraciones para obtener las variables de estado de navegación del vehı́culo: η1 y ν1 ; η2 y ν2 se toman directamente de los datos suavizados. De acuerdo a la metodologı́a desarrollada anteriormente por miembros del GARP y la literatura consultada (Rogers, 2003; Simon, 2006; Titterton, 2004), la dinámica del bloque INS queda definida como sigue:. η˙1 = J1 (η2 )ν1. (2.16). ν̇1 = f + g b. (2.17). La dinámica del error de estimación se obtiene perturbando las variables de las ecuaciones 2.16 y 2.17 como se muestra a continuación:. η̂1 = η1 + δη1 ν̂1 = ν1 + δν1 Jˆ1 (η2 ) = (I + E)J1 (η2 ) fˆ = f + δf donde ˆ representa los valores estimados, δ representa los errores de estimación y E es la matriz diagonal simétrica del error de aptitud (ǫ)..

(49) 37. DISEÑO DE SOFTWARE .  0 −ǫD ǫE  E= 0 −ǫN  ǫD  −ǫE ǫN 0.      . (2.18). A partir de aquı́, se hallan las ecuaciones que describen la dinámica del error del bloque INS :. δ η̇1 = J1 (η2 )δν1 + ν1n × ǫ. (2.19). δ ν̇1 = δf − g b × ǫ. (2.20). Una descripción más detallada del proceso de obtención de estas ecuaciones se describe en el Apéndice D 2.7.4.. Modelo Dinámico para Navegación. En (Fossen, 1994), se demuestra que la segunda ley de newton aplicada a un vehı́culo submarino tiene la forma:. MRB ν̇ + CRB (ν)ν + MA ν̇ + CA (ν)ν + D(ν)ν + | {z } | {z }. términos del cuerpo r ígido. términos hidrodinámicos. g(η) |{z}. =τ. (2.21). términos hidrostáticos. donde: MRB ∈ R6×6 CRB (ν) ∈ R6×6 MA ∈ R6×6 CA (ν) ∈ R6×6 D(ν) ∈ R6×6 g(η) ∈ R6×1. matriz de inercia del cuerpo rı́gido matriz de fuerzas centrı́peta y de Coriolis del cuerpo rı́gido matriz de inercia de masas añadidas matriz de fuerzas centrı́peta y de Coriolis de masas añadidas matriz de amortiguamiento vector de momentos gravitacionales y de flotabilidad. Esta ecuación puede representarse de forma compacta como sigue:.

(50) 38. DISEÑO DE SOFTWARE. Mν̇ + C(ν)ν + D(ν)ν + g(η) = τ. (2.22). M = MRB + MA. (2.23). C = CRB + CA. (2.24). notando que:. Los parámetros del modelo, para el caso del HRC-AUV ya han sido calculados en (Cañizares, 2010):. D(ν) = −diag [−181, 45 − 1219, 8 − 1219, 8 − 126, 62 − 9096, 9 − 9096, 9]T. g(η) =. . . 0, 0, 0, 890, 5cθsφ, 890, 5sθ, 0. T. 0 0 0 91 0  4345, 4   0 7929 0 −91 0 0     0 0 7929 0 0 0 M=   0 −91 0 450, 1 0 −275    91 0 0 0 36582 0   0 0 0 −275 0 36388.                .

(51) 39. DISEÑO DE SOFTWARE . 0 0  0,1946   0 318,39 0    0 0 −24, 1  τ =   0 0 0    0 0 96, 32   0 1273,56 0. .       |n|n      δ  T      δE    . . 0 0 0 91r 7929w −7929v    0 0 0 −7929w 91r 4535, 4u    0 0 0 7929v − 91p −4535, 4u − 91q 0  C(ν) =    −91r 7929w −7929v + 91p 0 −275p + 36388r −36582q    −7929w −91r 4535, 4u + 91q 275p − 36388r 0 450, 1p − 275r   7929v −4535, 4u 0 36582q −450, 1p + 275r 0 donde um = 2.7.5.. . |n|n δT δE. . es la entrada de control del sistema.. Filtro Extendido de Kalman. Las ecuaciones generales del algoritmo se muestran en el Cuadro 2–9 (Titterton, 2004; Sosa, 2010). Es sencillo notar el carácter recursivo del filtro, destacando el hecho de que en la primera etapa, los estados a “priori” (− ) se calculan a partir de los estados a “posteriori” (+ ) de la segunda en el instante anterior. Del Cuadro(2–9): (−). - x̂k. (+). y x̂k. - ẑk y zk (−). - Pk. Estado estimado a “priori” y “posteriori” respectivamente.. Medición estimada y real respectivamente. (+). y Pk. Matriz de covarianza del error estimada a “priori” y “posteriori” respec-. tivamente. - Qk y Rk - Kk. Matrices de acoplamiento del ruido de la planta y el sensor respectivamente.. Matriz de Ganancia de Kalman..                .

(52) 40. DISEÑO DE SOFTWARE Cuadro 2–9: Ecuaciones del Filtro Extendido de Kalman Discreto xk = ξk−1 (xk−1 ) + wk−1 zk = h(xk ) + vk. wk ∼ ℵ(0, Qk ) vk ∼ ℵ(0, Rk ). Modelo Dinámico no Lineal Modelo de Medición no Lineal. Etapa de predicción (−). x̂k. (−). Pk. (+). = Φk−1x̂k−1. Φk−1 =. ∂ξk−1 (x̂,k) |x=x̂k−1 ∂x. (+). = Φk−1 Pk−1ΦTk−1 + Qk−1. Predicción a “priori” (−) de xk Predicción a “priori” (−) de Pk. Etapa de corrección (−). ẑk = Hk x̂k. Hk =. (−). ∂h(x̂,k) |x=x̂k−1 ∂x. (−). Kk = Pk HTk [Hk Pk HTk + Rk ]−1 (+). x̂k. (+). Pk. (−). (−). = x̂k + Kk [zk − Hk x̂k ] (−). = [I − Kk Hk ]Pk. Linealización de la medición Ganancia de Kalman Obtención a “posteriori” (+) de xk Obtención a “posteriori” (+) de Pk. En el caso especı́fico del HRC-AUV, la información de corrección del GPS, se encuentra presente solo cuando el vehı́culo se encuentra en la superficie, por lo cual el filtro debe definirse en dos entornos diferentes: Entorno de inmersión: En el entorno de inmersión el vehı́culo se encuentra sumergido y no cuenta con la corrección del GPS. Las matrices que constituyen al filtro quedan definidas como sigue: x =. . T. δz δu δv δw δx δy Qt = diag σf2 σǫ2 T z = p − z udnvm − uins vdnvm − vins wdnvm − wins H = I4×4 04×2 2 R = σp2 σdnvm. (2.25) (2.26) (2.27) (2.28) (2.29).

(53) 41. DISEÑO DE SOFTWARE. Entorno de superficie: En el entorno de superficie, el sistema cuenta con las mediciones del GPS. Los parámetros x y Q, no presentan cambio con respecto al entorno de inmersión, el resto de los parámetros se definen: z =. H =. . p − z udnvm − uins vdnvm − vins wdnvm − wins T. xgps − xisn ygps − yins . (2.30) (2.31). I6×6 2 2 R = diag σp2 σdnvm σgps. (2.32). La dinámica del estado del filtro se obtiene a partir del modelo del error desarrollado en la Sección2.7.3 y se puede escribir de la forma:. ẋ = F x + Bu donde u =. . δf ǫ. T. y las matrices F y B se definen como:. .  01×1 J1 (η2 )(3,1−3) 01×2  F =  03×3 03×2  03×1  02×1 J1 (η2 )(1−2,1−3) 02×2   n  01×3 V(3,1−3)     B =  Γb  I3×3    n 02×3 V(1−2,1−3) donde:. (2.33).      .