Internas Desarrollo de Un Sistema de Navegación - Impresion

Desarrollo de un sistema de navegación
integrada para la evaluación

de medidas inerciales
Desarrollo de un sistema de navegación
integrada para la evaluación
de medidas inerciales
Daniel Reyes
Jonatan Aponte
José Jairo Soriano Méndez
Este trabajo es el fruto de un arduo esfuerzo, el cual no hubiera sido
capaz de realizar sin el apoyo de todas las personas que han estado a mi
alrededor haciendo mi vida feliz. Quiero agradecer a todas estas
personas por estar a pesar de la distancia y las adversidades, por
apoyarme escucharme y aconsejarme. Les agradezco a Matı́as y a
Marcelo por su colaboración en el campo laboral, a mis compañeros del
colegio por enseñarme el valor de la amistad, a mis compañeros de
fútbol por los mejores años de mi vida, a mis compañeros de la
universidad, Samir, Jaime, Julio, Juan, Fernando, Carlos, Fabián por
haberme permitido hacer parte de un excelente grupo de trabajo y por
sembrar en mi esta continua curiosidad que me impulsó durante estos
seis años; asimismo, a mis compañeros de intercambio, los cuales me
motivaron constantemente a arriesgarme. Un agradecimiento especial a
mi padre por su apoyo incondicional, mis tı́os y hermanos por creer y
confiar en mı́; a Karen, Luisa, Romina y Natalia, por escucharme y
darme fuerzas en los momentos que mas lo necesité. A mi compañero
Daniel por atreverse a seguir este camino a mi lado, por su amistad
incondicional, su esfuerzo y su paciencia. Finalmente a la persona que
además de darme la vida me inculcó valores y a quien le debo todo lo
que soy, gracias madre.
Esto va dedicado a las personas que ahora me acompañan desde la
eternidad...
Jonatan
A mis padres, Julio Cesar y Blanca Ruth por su amor, entrega y
voluntad para que todo esto sea posible. A mi hermano por motivarme a
ser parte de su inspiración. A Pato por estar ahı́ siempre y ser mi
inspiración. A mi familia por su apoyo y confiar en mı́ en todo
momento. A mi compañero Jonatan por su persistencia, disciplina y
dedicación a lo largo de esta aventura, a su familia que desde la lejanı́a
se volvieron parte de la mı́a y cultivaron nuestra voluntad. A cada
persona que nos acompañó durante el proceso y nos dio su apoyo
estando fuera de casa. A los que están, a los que no, a los que estarán.
A ti, viejo amigo, tus palabras siguen en mı́: “ser más para servir
mejor”.
Daniel
Agradecimientos
Al ingeniero Carlos Dams por ser nuestro guı́a en la llegada a la Facultad

de Ingenierı́a, de la Universidad de Buenos Aires (UBA) y despertar la
curiosidad por afrontar nuevas metas.
Al ingeniero Juan Giribet, nuestro director en la UBA, por su confianza,
guı́a y enseñanzas tanto académicas como personales. Nos brindó todas
las herramientas necesarias para que esto fuese posible.
Al ingeniero Jairo Soriano por su apoyo, formación y decisión al dirigir
este trabajo como representante de nuestra alma máter.
Al ingeniero Mauricio Anigstein y sus compañeros, Pablo Gonzáles y
Carlos Cartelli por su colaboración incondicional en el laboratorio y en
la cátedra de Robótica.
Al ingeniero Ariel Lujtenberg por su hospitalidad y por brindarnos un
área de trabajo cómoda.
A la Universidad Distrital Francisco José de Caldas por brindarnos la for-
mación y bases necesarias para enfrentar nuevos retos, tanto personales
como profesionales.
A la UBA por la orientación académica y la inclusión en los espacios
necesarios para el desarrollo de este proyecto.
Daniel y Jonatan
© Universidad Distrital Francisco José de Caldas
© Centro de Investigaciones y Desarrollo Científico
©Daniel Reyes, Jonatan Aponte, José Jairo Soriano Méndez
Primera edición, abril de 2019
ISBN: 978-958-787-111-1
Dirección Sección de Publicaciones

Rubén Eliécer Carvajalino C.
Coordinación editorial
Nathalie De la Cuadra N.
Corrección de estilo
Editorial UD
Diagramación en LaTeX
Carlos Arturo Rodríguez Martínez
Diseño de cubierta
Diego Abello Rico
Imagen de cubierta
Luis Miguel Cortés Martínez
Editorial UD
Universidad Distrital Francisco José de Caldas
Carrera 24 No. 34-37
Teléfono: 3239300 ext. 6202
Correo electrónico: publicaciones@udistrital.edu.co
Desarrollo de un sistema de navegación integrada para la

evaluación de medidas inerciales / José Jairo Soriano
Méndez
[y otros]. -- Bogotá: Universidad Distrital Francisco José de
Caldas, 2019.
130 páginas; 24 cm.
ISBN 978-958-787-111-1
1. Robótica 2. Robots 3. Ingeniería eléctrica 4. Sistemas de
navegación. I. Soriano Méndez, José Jairo, autor. II. Tít.
629.892 22 ed.
A1629156
CEP-Banco de la República-Biblioteca Luis Ángel Arango
Todos los derechos reservados.

Esta obra no puede ser reproducida sin el permiso previo escrito de la
Sección de Publicaciones de la Universidad Distrital.
Hecho en Colombia
Contenido
Introducción 17
1
1 Preliminares 25
9
1.1 Métodos de representación de actitud . . . . . . . . . . . . . . . . . . 25
9
1.1.1 Notación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
9
1.1.2 Ternas de referencia . . . . . . . . . . . . . . . . . . . . . . . 26
10
1.1.3 Métodos de representación de orientación . . . . . . . . . . . . 26
10
1.1.3.1 Matrices de rotación . . . . . . . . . . . . . . . . . . 26
10
1.1.3.2 Cuaterniones . . . . . . . . . . . . . . . . . . . . . . 27
11
1.1.3.3 Ángulos de Euler . . . . . . . . . . . . . . . . . . . . 29
13
1.2 Conceptos de robótica . . . . . . . . . . . . . . . . . . . . . . . . . . 29
13
1.2.1 Cinemática directa . . . . . . . . . . . . . . . . . . . . . . . . 31
15
1.2.2 Cinemática inversa . . . . . . . . . . . . . . . . . . . . . . . . 31
15
1.2.3 Generación de trayectoria . . . . . . . . . . . . . . . . . . . . 32
16
1.2.4 Dinámica y control . . . . . . . . . . . . . . . . . . . . . . . . 32
16
1.3 Unidad de medidas inerciales: modelo de error . . . . . . . . . . . . . 32
16
1.3.1 Giróscopos mecánicos . . . . . . . . . . . . . . . . . . . . . . . 33
17
1.3.2 Giróscopos ópticos . . . . . . . . . . . . . . . . . . . . . . . . 33
17
1.3.2.1 Interferometric fiber optic gyro technology . . . . . . 33
17
1.3.2.2 Ring laser gyroscope . . . . . . . . . . . . . . . . . . 34
18
1.3.3 Sistemas micro electro-mecánicos . . . . . . . . . . . . . . . . 34
18
1.3.4 Modelo de error . . . . . . . . . . . . . . . . . . . . . . . . . . 35
19
1.4 Modelado en espacio de estados . . . . . . . . . . . . . . . . . . . . . 37
21
2 Navegación inercial 41
25
41
2.1 Sistemas de navegación inercial . . . . . . . . . . . . . . . . . . . . . 25
2.1.1 42
Ecuaciones de navegación . . . . . . . . . . . . . . . . . . . . 26
2.1.2 43
Presupuesto de error . . . . . . . . . . . . . . . . . . . . . . . 27
2.1.2.1 44
Integración del sesgo . . . . . . . . . . . . . . . . . . 28
2.1.2.2 44
Integración términos de calibración . . . . . . . . . . 28
2.1.2.3 44
Integración del ruido blanco . . . . . . . . . . . . . . 28
45
2.2 Validación de navegación inercial . . . . . . . . . . . . . . . . . . . . 29
2.2.1 45
Trayectoria VS30 . . . . . . . . . . . . . . . . . . . . . . . . . 29
2.2.1.1 46
Resultados . . . . . . . . . . . . . . . . . . . . . . . 30
2.2.2 48
Trayectoria SAC-D sobre el robot . . . . . . . . . . . . . . . . 32
2.2.2.1 50
Resultados . . . . . . . . . . . . . . . . . . . . . . . 34
51
2.3 Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3 Filtro de Kalman 53
37
53
3.1 Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
54
3.2 Formulación del problema . . . . . . . . . . . . . . . . . . . . . . . . 38
55
3.3 Filtro de Kalman extendido . . . . . . . . . . . . . . . . . . . . . . . 39
57
3.4 Estimación de actitud . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3.4.1 57
Modelo de error de actitud . . . . . . . . . . . . . . . . . . . . 41
3.4.2 59
Fusión de información . . . . . . . . . . . . . . . . . . . . . . 43
60
3.5 Modelo del sistema . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
63
3.6 Validación de la estimación en error de actitud . . . . . . . . . . . . . 47
3.6.1 63
Trayectoria VS30 . . . . . . . . . . . . . . . . . . . . . . . . . 47
3.6.2 66
3.6.2.1 66
Resultados . . . . . . . . . . . . . . . . . . . . . . . 50
69
3.7 Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
4 Navegación integrada: calibración On-Line 71

55
71
4.1 Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
73
4.2 Modelo dinámico del error en la UMI . . . . . . . . . . . . . . . . . . 57
4.2.1 73
Sesgo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
4.2.2 73
Factor de escala . . . . . . . . . . . . . . . . . . . . . . . . . . 57
4.2.3 74
Falta de ortogonalidad . . . . . . . . . . . . . . . . . . . . . . 58
4.2.4 75
Modelo en espacio de estados . . . . . . . . . . . . . . . . . . 59
75
4.3 Determinación de actitud . . . . . . . . . . . . . . . . . . . . . . . . . 59
4.3.1 76
Calibración on-line . . . . . . . . . . . . . . . . . . . . . . . . 60
4.3.2 Corrección de orientación 77
. . . . . . . . . . . . . . . . . . . . 61
79
4.4 Validación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
4.4.1 79
Trayectoria VS30 . . . . . . . . . . . . . . . . . . . . . . . . . 63
4.4.2 82
86
4.5 Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
5 Análisis de observabilidad 89
73
89
5.1 Observabilidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
5.1.1 Análisis por matriz de observabilidad . . . . . . . . . . . . . . 73
5.1.2 Grado de observabilidad . . . . . . . . . . . . . . . . . . . . . 75
5.1.3 Observabilidad de la trayectoria: SAC-D . . . . . . . . . . . . 76
5.2 Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
5.1 Observabilidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
5.1.1 89
Análisis por matriz de observabilidad . . . . . . . . . . . . . . 73
5.1.2 91
Grado de observabilidad . . . . . . . . . . . . . . . . . . . . . 75
5.1.3 Observabilidad de la trayectoria: SAC-D . . . . . . . . . . . . 76 92
96
5.2 Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
6 Implementación de la plataforma de simulación con Hardware en

el Lazo 97
81
97
6.1 Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
6.1.1 Arquitectura . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83 99
6.2 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10084
6.2.1 Unidad de medidas inerciales: ADIS16364 . . . . . . . . . . . 101
85
6.2.2 Tarjeta de adquisición de datos: LPCXpresso 1769 . . . . . . . 102
86
6.2.3 Computadora de navegación: BeagleBoard xM . . . . . . . . . 103
87
6.2.4 Robot IRB140-Controlador S4c . . . . . . . . . . . . . . . . . 105
89
6.3 Interfaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
89
6.3.1 TAD-UMI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
90
6.3.2 TAD-CN . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
90
6.3.3 CN-controlador de robot . . . . . . . . . . . . . . . . . . . . . 108
92
6.3.4 CN-hombre . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109
93
6.4 Evolución temporal del sistema . . . . . . . . . . . . . . . . . . . . . 110
94
6.5 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
95
6.5.1 Metodologı́a . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
95
6.5.2 Descripción modular . . . . . . . . . . . . . . . . . . . . . . . 112
96
6.5.2.1 Módulos adquisición de datos UMI . . . . . . . . . . 112
96
6.5.2.2 Módulos CN . . . . . . . . . . . . . . . . . . . . . . 114
98
6.5.2.3 Proceso de adquisición datos del robot . . . . . . . . 115
99
6.5.2.4 Proceso principal . . . . . . . . . . . . . . . . . . . . 115
99
6.6 Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
102
6.7 Trabajo futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
103
Referencias 121
105
Introducción
Little people discuss other

people.
Average people discuss
events.
Big people discuss ideas.
Rudolph E. Kalman
Los satélites artificiales son puestos en órbita con el fin de obtener distintas

prestaciones. En la actualidad existen satélites militares, de telecomunicaciones, de
observación climática, de observación del espacio, entre otros [1, 2, 3, 4]. Cada satéli-
te posee un sistema de navegación que permite determinar el estado cinemático
(parámetros de navegación), esto es, posición, velocidad y orientación (actitud) con
respecto a un sistema de referencia. Los sistemas de navegación están construidos
a partir de sensores inerciales, de referencia externa o la combinación de ambos [5, 6, 7,
8, 9]. La navegación inercial requiere al menos una terna de giróscopos y una de aceler
ómetros dispuestos de forma ortogonal y generalmente agrupados en un dispositivo
denominado Unidad de Medidas Inerciales (UMI), junto con una compu-tadora de
navegación (CN) encargada de resolver el algoritmo de navegación a partir de la
información de los sensores y las condiciones iniciales del vehı́culo [10, 11, 12]. Las
UMI se caracterizan porque sus sensores permiten realizar navegación con in-formaci
ón intrı́nseca del vehı́culo, mientras que los giróscopos entregan información de la
variación de la actitud (velocidad angular ω), los acelerómetros miden la fuer-za espec
ı́fica a la que este está expuesto. Lo interesante de usar esta estrategia es que las UMI
tienen una alta tasa de muestreo, lo cual es importante para vehı́culos cuya dinámica
es alta, además de su independencia frente a eventos extrıńsecos al vehı́culo. Sin
embargo, la gran desventaja de estos sistemas es que su error crece de manera no
acotada en el tiempo, debido a que la información que entregan hace referencia a
variaciones de los parámetros de navegación y requieren un proceso de integración en
la determinación de los parámetros de navegación. Esto implica que
17
si dicha medida esta afectada por algún sesgo u otro parámetro de error tı́pico en la
instrumentación, este error se irá acumulando en el tiempo como resultado del pro-
ceso de integración. Actualmente es posible encontrar sistemas inerciales de altı́sima
calidad, pero su costo es alto y la relación calidad-costo es un factor importante por
tener en cuenta.
Otra opción son los sistemas de referencia externa. Estos se basan en plataformas
móviles de radionavegación como radiofaros no direccionales (NDB1 ), ası́ como sen-
sores estelares (star trackers: usualmente telescopios con campo de visión angosto),
magnetómetros, cámaras CCD, altı́metros, GPS, radares, sonares entre otros [13].
Este tipo de navegación es dependiente del entorno del vehı́culo y presenta una tasa
de muestreo baja si se compara con la navegación inercial; más allá de esto, presen-
tan una gran ventaja porque su error en general es acotado y se puede caracterizar
a priori.
Dadas las caracterı́sticas complementarias de ambos sistemas y la constante evolu-
ción de las tecnologı́as, en especial de los microprocesadores y las UMI, la mayorı́a de
los sistemas de navegación de buena calidad y bajo costo se diseñan sobre la base de
la navegación integrada. Este enfoque pretende fusionar la información proveniente
de los distintos sensores de manera óptima, y subsanar ası́ las desventajas de ambos
tipos de sensores a cambio de un procesamiento de la información más complejo.
Por esta razón, en la actualidad la navegación integrada es materia de investigación.
La Comisión Nacional de Actividades Espaciales (CoNAE) es la entidad guber-

namental Argentina en donde se desarrollan proyectos orientados a la investigación
en materia espacial. Una de las primeras tareas de este organismo fue diseñar el
plan espacial nacional de Argentina, el cual tiene en sus objetivos principales ofrecer
información acerca del impacto generado al desarrollar tecnologı́as de tipo espacial
encaminadas a la protección del medio ambiente, la construcción de satélites livianos
para teledetección, análisis cientı́fico, comunicaciones, además de emprender accio-
nes con alto contenido tecnológico e innovador [14].
En el marco de este plan y en conjunto con instituciones universitarias, empre-

sas argentinas, entidades públicas locales y extranjeras como NASA2 , ESA3 , ASI4 ,
1
Non-Directional Beacons.
2
National Aeronautics and Space Administration.
3
European Space Agency.
4
Agenzia Spaziale Italiana.
18
Desarrollo de un sistema de navegación integrada para la evaluación de medidas inerciales
BNSC1 , CNES2 , AEB3 , la CoNAE ha desarrollado un conjunto amplio de proyectos

de investigación entre los que se destacan las misiones satelitales de la serie SAC4 ,
de los cuales han sido enviados al espacio exitosamente los satélites SAC-A, SAC-C
y SAC-D. Actualmente se encuentran en órbita el SAC-C y el SAC-D [15].
La misión SAC-D Aquarius se llevó a cabo en cooperación cientı́fica de las agen-

cias espaciales de los Estados Unidos y la Argentina. Diversos centros espaciales
participan en el proyecto, entre ellos el Jet Propulsion Laboratory (JPL) de Califor-
nia y el Centro Espacial Goddard de Washington. Su objetivo principal abarca la
obtención de mediciones de salinidad superficial de los océanos a escala global, pres-
tando particular atención a los procesos de circulación y mezcla de aguas, ası́ como
también detectar focos de alta temperatura en la superficie con el fin de obtener
mapas de riesgo de incendios y humedad del suelo para entregar alertas tempranas
de inundaciones. El SAC-D (ver figura 1) cuenta con ocho instrumentos de los cuales
interesa para el presente proyecto el Paquete de Demostración Tecnológica (TDP),
el cual es un instrumento experimental utilizado para determinación de órbita, po-
sición y velocidades angulares del satélite; además, está pensado para ser usado en
futuras misiones de la CoNAE.
Uno de los principales problemas de las misiones satelitales se presenta en la etapa de
diseño, donde hay un alto grado de incertidumbre con respecto al entorno real sobre
el cual se van a desempeñar los sistemas construidos. Con el fin de minimizar esta
incertidumbre, investigadores han desarrollado ambientes controlados de simulación
para realizar pruebas de dichos sistemas y evaluar el desempeño de las propuestas que
se están desarrollando [16, 17, 18, 19].
1
British National Space Centre.
2
Centre National dÉtudes Spatiales.
3
Agencia Espacial Brasileira.
4
Satélites de Aplicaciones Cientı́ficas.
19
Figura 1. Satélite SAC-D.
Fuente: [20].
En este trabajo se desarrolla una plataforma de simulación para navegación de
actitud con Hardware en el Lazo(HIL1 , por sus siglas en inglés). El cual puede ser
usado para evaluar Unidades de Medidas Inerciales y algoritmos de navegación. Se
desarrolla un algoritmo de estimación de actitud mediante el filtro de Kalman ex-
tendido (FKE) y se implementa en la computadora de navegación (CN). El vehı́culo
por navegar es un brazo robótico de seis grados de libertad (sistema parte del La-
boratorio de Robótica de la Facultad de Ingenierı́a de la Universidad de Buenos
Aires-FiUBA); en él, se monta fı́sicamente la carga útil del sistema de navegación
(UMI, Tarjeta de Adquisición de Datos-TAD, CN). Esto con el fin de obtener una
plataforma para validar los datos del TDP en el SAC-D, y ası́ obtener un sistema de
prueba para sensores como giróscopos y acelerómetros, tanto de fabricantes externos
como de los fabricados por CONAE.
1
Hardware in the Loop.
20
Figura 2. Metodologı́a del proyecto.

Fuente: elaboración propia.
La estrategia para el desarrollo de esta plataforma fue orientada por los objetivos
especı́ficos descritos a continuación:
• Estudiar y definir un modelo de error para el sensor inercial.
• Desarrollar un algoritmo de estimación de actitud a partir de la fusión estadı́sti-

ca de las mediciones obtenidas de los instrumentos de navegación, mediante el
filtro de Kalman extendido.
• Evaluar el desempeño de los algoritmos de estimación de actitud.
• Implementar los algoritmos de estimación desarrollados en la computadora de

Navegación (CN).
• Generar las trayectorias del robot IRB140 a partir de los datos de telemetrı́a
del satélite SAC-D.
• Desarrollar el software y hardware requerido para implementar las interfaces

entre los dispositivos del sistema.
21
• Integrar la plataforma con hardware en el lazo, a partir del robot IRB140, un
sistema de adquisición de datos (TAD)1 y la Unidad de Medidas Inerciales
(UMI).
La figura (2) presenta las fases de ejecución del proyecto, que llevaron al cumpli-
miento de cada objetivo.
Estructura del documento
Capı́tulo 1
En este capı́tulo se desarrollan algunos conceptos previos y definiciones necesa-

rias para realizar el análisis de la estimación de actitud en los capı́tulos subsecuentes.
Capı́tulo 2
En este capı́tulo se desarrolla el algoritmo de navegación inercial y se hace un

estudio del impacto del error en el sensor sobre esta estrategia. Además, se hace una
se hace una implementación del algoritmo y se evalúan los resultados a partir de las
trayectorias de referencia seleccionadas para este documento (cohete VS30 y satélite
SAC-D).
Capı́tulo 3
En este capı́tulo se desarrolla el contexto del filtro de Kalman y su extensión

para resolver problemas no lineales; de igual manera, el modelo de error de actitud
para el sistema y se implementa un algoritmo de navegación integrada sobre este
modelo. La implementación requiere la definición del modelo del sensor externo, el
cual en el caso del VS30 es simulado, mientras que para la trayectoria del SAC-D
es tomado del robot IRB140 y contaminado con ruido. Esto con el fin de realizar el
análisis de desempeño del algoritmo.
Capı́tulo 4
1
Tarjeta de Adquisición de Datos, se utiliza la herramienta de desarrollo LPCXpresso 1769,
la cual es descrita en la sección de Implementación.
22
En este capı́tulo se construye el modelo de estado aumentado a partir de los

modelos de error en actitud y en UMI, cuyo objetivo es mejorar el desempeño del
sistema mediante un procedimiento que se denominará calibración on-line. Se realiza
la validación del algoritmo propuesto sobre las dos trayectorias de referencia y se
comparan los resultados.
Capı́tulo 5
En el capı́tulo 5 se desarrolla un análisis de observabilidad para el modelo de

errores planteado en el capı́tulo 3, y un análisis del grado de observabilidad a par-
tir de los resultados obtenidos en el capı́tulo 4 para la trayectoria del SACD. Esto
permite evaluar desde otra perspectiva los resultados del sistema de navegación.
Capı́tulo 6
Finalmente, en el capı́tulo 6 se describe la metodologı́a usada para implementar

la plataforma de simulación con HIL. Se describen el hardware y el software imple-
mentados. Se muestran los resultados sobre la plataforma.
23
Capı́tulo 1
Preliminares
The individual is ephemeral,

races and nations come and
pass away, but man
remains.
Nikola Tesla
1.1 Métodos de representación de actitud

1.1.1 Notación
• V i : Magnitud vectorial V expresada en terna i.
• V̇ i : Derivada respecto al tiempo de V , expresada en terna i.
• Cbi : Matriz de rotación que va desde la terna b a la i.
• qbi : Cuaternión de rotación que va desde la terna b a la i.
i
• ωib : Velocidad angular de la terna b con respecto a la terna i, medida sobre la
terna i.
• x̃: Magnitud vectorial x contaminada con error.
• x̂: Magnitud vectorial x estimada.
• C̄: Matriz calculada por la computadora de navegación (CN).
• q̆: Cuaternión de rotación entregada por el Robot.
25
1.1.2 Ternas de referencia
Una terna de referencia {i} es un sistema cartesiano de tres ejes ortogonales, de-
notados de la forma, xi , y i , z i , y cuyo origen se establece como punto de interés
para el análisis de referencia en el espacio. Se definen como sistemas dextrógiros, es
decir, siguiendo la convención de la mano derecha. Dentro de este trabajo se usan
las siguientes ternas de referencia:
• Terna inercial {i}: su origen está en reposo o se mueve sin aceleración (mo-
vimiento rectilı́neo y uniforme) y cuya orientación no cambia.
• Terna del sensor {s}: a esta se encuentran referidas las medidas entregadas
por el sensor.
• Terna del vehı́culo {b}: tı́picamente es la terna solidaria al centro de masa

del vehı́culo. En este trabajo la terna del vehı́culo coincide con la terna del
sexto eslabón del Robot.
• Terna de navegación {ν}: en esta se referencian los parámetros de nave-

gación del vehı́culo. Hace alusión a la posición del observador u observadores
externos al vehı́culo a partir de los cuales se determina la cantidad de movi-
miento y cambio de orientación del vehı́culo. En este caso corresponde a la
terna definida sobre la base del robot (ver figura 2.6).
1.1.3 Métodos de representación de orientación

La orientación entre dos ternas puede ser representada usando distintas parametriza-
ciones. A continuación, se muestran algunas de ellas con sus principales propiedades.
1.1.3.1 Matrices de rotación

La matriz de rotación es una descripción de la relación rotacional existente entre dos
sistemas de referencia. Es usada para transformar un vector referido en un sistema
de referencia a otro. Por ejemplo, para pasar del sistema {a} al sistema {b}:
xb = Cab xa (1.1)
El conjunto de las matrices de rotación en R3 tiene algunas propiedades impor-
tantes, las cuales se denotan como SO(3):
• Son matrices cuadradas Cab ∈ R3×3 .
• Existe la matriz I ∈ SO(3) donde I es la identidad.
26
• Son matrices ortogonales (Cab )T = (Cab )−1 .
• Su determinante es det[Cab ] = 1.
• El producto de dos matrices (Cbc Cab ) que pertenecen al conjunto SO(3), también
hace parte de dicho conjunto (Cac ) ∈ SO(3)
Una propiedad muy interesante que poseen las matrices de rotación es que se
puede realizar una composición de rotaciones mediante la multiplicación. Esta ope-
ración permite pasar un vector definido en terna {a} a la terna {c} sin necesidad
del paso intermedio a través de la terna {b}.
Cac = Cbc · Cab (1.2)
En el ámbito de navegación también se denominan como matrices de cosenos

directores (MCD), la evolución temporal de una MCD está determinada por la si-
guiente ecuación diferencial:
Ċba = Cba S(ωab

b
) (1.3)
b
Donde ωab es la velocidad angular de la terna {b} respecto a la terna {a} medida
b
sobre la terna {b}. Y S(ωab ) es la matriz antisimétrica definida en la ecuación (1.4),
construida a partir del vector velocidad angular.
⎡ ⎤
0 −ωz ωy
S(ω) = ⎣ ωz 0 −ωx ⎦ (1.4)
−ωy ωx 0
1.1.3.2 Cuaterniones
Los cuaterniones son elementos que extienden el sistema de los números complejos
propuesto por Hamilton [21]. El cuaternión, de forma general, es un vector de cuatro
componentes:
⎡ ⎤
q1
⎢ q2 ⎥ � �
qa = ⎢
b ⎥
⎣ q3 ⎦ · i j k 1 (1.5)
q4
Donde [i,j,k] son las bases del sistema de numeración. La forma más usual de
representar un cuaternión es separarlo en parte escalar y parte vectorial:
� � ��
V sen� φ2�
qba = (1.6)
cos φ2
27
Donde:
• V es un vector de norma 1 alrededor del cual se realiza la rotación.
• φ es el ángulo que se rota en sentido positivo alrededor del vector invariante

para hacer coincidir las ternas {a} y {b}.
El producto de dos cuaterniones unitarios es un cuaternión unitario dado por1 :

⎡ ⎤⎡ ⎤
a4 −a3 a2 a1 b1
⎢ a3 a4 −a1 a2 ⎥ ⎢ b2 ⎥
q = qa ◦ qb = ⎢
⎣−a2 a1
⎥⎢ ⎥ (1.7)
a4 a3 ⎦ ⎣ b 3 ⎦
−a1 −a2 −a3 a4 b4
Para transformar un vector del sistema2 {a} al sistema {b} es necesario:
� a�
p
pb = qba ⊗ pa = qba ◦ ◦ qb∗
a (1.8)
0
Donde:
• ⊗: Es la operación rotación.
• qb∗ b
a : Es el conjugado de qa :
� � φ ��
−V sen
� φ �2
qba = (1.9)
cos 2
Análogo a lo que sucede con la MCD, la composición de rotaciones se puede

realizar mediante el producto de cuaterniones:
qca = qcb ◦ qba (1.10)

La dinámica del cuaternión está descrita por la siguiente ecuación diferencial:
� b �
1 ω (t)
q̇bi (t) = qbi (t) ◦ ib (1.11)
2 0
Los cuaterniones unitarios proveen una representación que, comparada con los
ángulos de Euler, es más simple y evita el problema de las singularidades, ası́ como
también requiere el uso de menos parámetros que las MCD. Por otro lado, admiten
la composición de rotaciones y no necesitan funciones trigonométricas para calcular la
MCD [5]. La desventaja respecto a la representación por eje y ángulo de Euler, es que
existe un parámetro redundante donde se definen dos cuaterniones que describen la
misma orientación [22].
1
Donde (◦) denota al producto entre cuaterniones.
2
En el desarrollo de este trabajo se usará indistintamente terna de referencia o sistema de
referencia.
28
1.1.3.3 Ángulos de Euler

� �
Los ángulos de Euler ψ θ φ son las rotaciones alrededor de los ejes x “roll”,
y “pitch”, y z “yaw” respectivamente (ver figura 1.1). De modo que la matriz de
rotación es la composición de rotaciones sucesivas alrededor de los 3 ejes z, y y x,
respectivamente.
Figura 1.1. Yaw, Pitch, Roll.

Fuente: [20].
Rxyz (ψ, θ, φ) = Rx (φ)Ry (θ)Rz (ψ) (1.12)
Después de realizar las operaciones necesarias, la matriz de rotación correspon-

diente a la representación en ángulos de Euler es1 :
⎡ ⎤
cθcψ sθsφcψ − cφsψ sθcφsψ + sφsψ
Rxyz (ψ, θ, φ) = ⎣cθsψ sθsφsψ + cφcψ sθcφsψ − sφcψ ⎦ (1.13)
−sθ cθsφ cθcφ
La principal desventaja que poseen los ángulos de Euler es que para ciertos valo-
res de sus parámetros se pierde un grado de libertad. En la literatura se le denomina
gimbal lock [23]. Sin embargo, su uso es indispensable a la hora de interpretar resul-
tados pues entre las parametrizaciones mencionadas es la mas intuitiva.
1.2 Conceptos de robótica

La robótica es un área de estudio relativamente nueva que supera los lı́mites de la
ingenierı́a tradicional. La complejidad de los robots requiere conocimientos en in-
genierı́a electrónica, mecánica, industrial, ciencias de computación y matemáticas,
1
En lo que sigue la notación usada para el sin(θ) será sθ y para cos(θ) será cθ.
29
por solo nombrar las más importantes. Actualmente la gran mayorı́a de los robots
existentes se encuentran funcionando en procesos de manufactura; este tipo de ma-
nipuladores son automáticamente controlados por una computadora dedicada, lo
cual permite ser reprogramados para cumplir una amplia variedad de tareas. Los
manipuladores industriales se caracterizan por realizar procesos y garantizar ası́ las
siguientes métricas:
• Velocidad.
• Repetitividad.
• Precisión.
• Resistencia.
Los sistemas robóticos industriales poseen generalmente una cadena de eslabones

mecánicos conectados por articulaciones de tipo rotatorio o lineal, la cual muchas
veces se asemeja a un brazo; además, cuentan con un arreglo de sensores externos
e internos, como sensores de fuerza, de posicionamiento, encoders etc), una mano o
actuador final, un sistema de energı́a, una interfaz con el usuario y el controlador
del robot.
Figura 1.2. Arquitectura tı́pica de un sistema robótico [24].

30
Problema 1. En robótica una aplicación tı́pica consiste en mover la herramienta

desde una posición inicial A a través de una trayectoria T hasta la posición B dada
una velocidad constante v deseada.
Generalmente, tanto las posiciones A y B como la trayectoria T se definen dentro

de un área de trabajo alrededor de la cual el robot puede moverse libremente y se
encuentran definidas con respecto a una terna de referencia conocida por el robot
como lo es la terna del eje o la base.
1.2.1 Cinemática directa

Para realizar esta tarea el robot necesita conocer su ubicación actual (resolver el
problema de navegación) y la ubicación deseada con respecto a un sistema de re-
ferencia. El robot es capaz de sensar su estado (posición y orientación) usando los
encoder localizados en cada articulación para determinar las coordenadas articulares
θi 1 , el proceso realizado para obtener la posición y orientación dados estos ángulos
se denomina cinemática directa. La solución de este problema está relacionada con
las dimensiones del mecanismo y su diseño; el problema queda resuelto cuando se
determina la transformación que lleva desde el espacio articular hacia el espacio
cartesiano.
x̄ = F(θ̄) (1.14)
1.2.2 Cinemática inversa

En el problema formulado se tiene el conocimiento de las coordenadas en el espacio
cartesiano. El controlador del robot debe conocer las coordenadas articulares para
realizar la generación de la trayectoria, además de ejercer las acciones de control
requeridas.
θ = F−1 (x) (1.15)
Donde:
• θ es el conjunto de ángulos articulares [θ1 , θ2 , ...θi ]
• x es el vector en coordenadas cartesianas.
• F es la función de transformación.
1
El espacio de coordenadas articulares es un sistema de coordenadas en el cual la posición del
robot se define en términos de los ángulos de rotación de cada eje.
31
Daniel Reyes, Jonatan Aponte, José Jairo Soriano Méndez
La función F es no lineal y este problema puede llegar a no tener solución o tener

varias soluciones [24]; razón por la cual, el algoritmo de cinemática inversa es más
complejo que el de cinemática directa [24].
1.2.3 Generación de trayectoria

El sistema de control del Robot se encarga de usar como referencia la salida del
generador de trayectoria para indicarle al mecanismo en cada tiempo cómo debe
moverse. La generación de la trayectoria se realiza on-line y utiliza la información
provista por el algoritmo de cinemática inversa para entregarle al sistema de control
una referencia respecto a dónde debe ir.
1.2.4 Dinámica y control

El sistema de control se encarga de usar información de la dinámica del vehı́culo para
realizar las tareas de seguimiento y rechazo a perturbaciones en posición y velocidad.
Todo esto requiere que el controlador del robot solucione numéricamente la ecuación
dinámica del estado del robot on-line. Esta dinámica se usa para construir un lazo
de control tipo feed-forward, que entrega las órdenes a los actuadores para que el
robot siga la trayectoria previamente programada.
1.3 Unidad de medidas inerciales: modelo de error

Si bien una UMI es capaz de medir fuerza especı́fica y velocidad angular, la determi-
nación de actitud no necesita el uso de medidas de fuerza especı́fica. A continuación,
se presenta un análisis de las tecnologı́as de construcción de los giróscopos1 .
Un giróscopo es un sensor que detecta la velocidad de rotación inercial de un

objeto; existen tres grandes grupos de tecnologı́as de construcción de giróscopos:
• Mecánicos.
• Ópticos.
• MEMS.
1
Si se desea información sobre acelerómetros puede consultarse a [25].
32
1.3.1 Giróscopos mecánicos
Un giróscopo mecánico consta de una rueda móvil que está montada en un gimbal 1 .
Cuando se presenta rotación en alguna dirección del gimbal la conservación del
momento angular genera un torque proporcional a la velocidad angular, el cual trata
de mantener invariante la orientación de la rueda. El problema de estos sensores
es que al ser mecánicos tienden a desgastarse con el tiempo, por lo que requieren
costosos procesos de calibración y reparación.
Figura 1.3. Giróscopo mecánico.

Fuente: [26].
1.3.2 Giróscopos ópticos

1.3.2.1 Interferometric fiber optic gyro technology
Este giróscopo se basa en el efecto Sagnac para medir la velocidad angular [5]. Se
introducen a una bobina de fibra óptica dos haces de luz en direcciones opuestas.
Cuando no hay rotación ambos haces recorren el anillo en el mismo tiempo y no
se genera interferencia; sin embargo, cuando la bobina rota en alguna dirección
provoca que el haz que va en esa misma dirección recorra un camino más largo, lo
que genera un cambio de fase entre ambos haces. Dicho cambio se ve reflejado en
una modificación en la intensidad, el cual es proporcional a la velocidad angular.
1
Gimbal: es un soporte que permite la rotación de un objeto alrededor de un eje único. Tı́pi-
camente un giróscopo mecánico está compuesto por 3 gimbal
33
Figura 1.4. Giróscopo IFOG.
Fuente: [27].
1.3.2.2 Ring laser gyroscope

Por otro lado los RLG que también se basan en el efecto Sagnac generan un único
haz de luz que es dividido mediante espejos para direccionarlos. De igual manera,
cuando se presenta rotación en alguna de las dos direcciones se da un patrón de
interferencia, el cual es detectado gracias a que mediante otro espejo se extrae el haz
de luz combinado [5].
Figura 1.5. Giróscopo RLG.

Fuente: [5].
1.3.3 Sistemas micro electro-mecánicos

Estos giróscopos están construidos mediante técnicas de micro maquinado en silicio.
El principio de funcionamiento aprovecha el efecto Coriolis generado sobre una masa
en estado de vibración. Cuando se presenta presenta rotación se genera una fuerza
perpendicular a la velocidad, que es proporcional a la velocidad de rotación de la
masa [5].
34
Figura 1.6. Giróscopo MEM.

Fuente: [5].
Si bien los MEMS son de bajo costo, se caracterizan por tener menor desempeño
que los giróscopos ópticos. Actualmente se están mejorando las tecnologı́as de cons-
trucción con el fin de obtener sensores MEMS de mejor desempeño que los ópticos [5].
1.3.4 Modelo de error
La mayorı́a de los errores en la medición de los giróscopos son generados por im-
perfecciones [5, 28]. Las perturbaciones que sufre la medición se clasifican como
determinı́sticas o estocásticas (ver tabla 1.1). Aquellas que son determinı́sticas pue-
den ser estimadas y corregidas off-line mediante un proceso de calibración el cual usa
un modelo parametrizado. Todas las perturbaciones adicionales encajan dentro del
modelo estocástico, para el cual si bien no es posible determinar su valor exacto en
cada instante de tiempo, puede llegar a ser caracterizado estadı́sticamente mediante
métodos como la varianza de Allan [29]. En el estándar IEEE [30] se sugiere usar la
densidad espectral de potencia para la identificación del modelo, también existen m
étodos basados en cuadrados mı́nimos no lineales [31].
35
Parámetro Clase
Sesgo Determinı́stico
Factor de escala Determinı́stico
Falta de ortogonalidad Determinı́stico
No linealidad Determinı́stico
Inestabilidad del sesgo Estocástico
Inestabilidad del fac- Estocástico
tor de escala
Inestabilidad de falta Estocástico
de ortogonalidad
Ruido de cuantización Estocástico
Efecto de temperatura Estocástico
Ruido blanco Estocástico
Cuadro 1.1: Parámetros tı́picos en un modelo de error para giróscopos tipo MEMS.
Fuente: [28].
Un modelo de error bastante usado se resume en la ecuación 1.16:
b b
ω̃ib = (I3 + Σ(�, �))ωib + b ω + ξω (1.16)
⎡ ⎤
�x �xy �xz
Σ(�ω , �ω ) = ⎣�yx �y �yz ⎦ (1.17)
�zx �zy �z
Donde:
b
• ω̃ib : velocidad angular entregada por el giróscopo.
b
• ωib : velocidad angular real del objeto.
• � ∈ R3 : error en el factor de escala de los giróscopos.
• � ∈ R6 : error por falta de ortogonalidad.
• ξω : ruido blanco del giróscopo, el cual se supone blanco gaussiano:
E�ψω � = 0 (1.18)
� �
E ψω ψωT = Q (1.19)
36
1.4 Modelado en espacio de estados

En el contexto de la identificación y modelado de sistemas, es de interés encontrar
un modelo donde queden reflejadas explı́citamente todas las variables que intervie-
nen en la dinámica del sistema analizado. Un planteamiento que cumple con esta
condición es la representación en variables o espacio de estados. Una vez obtenido
el modelo es deseable obtener información de la dinámica sin necesidad de medir
directamente todas las variables de este, bien sea por factores de diseño o por que
no sea posible. Es decir que se hace uso de la información entregada por las medi-
ciones para inferir u observar algunas de las variables ocultas. Es usual que dichas
mediciones estén contaminadas con algún tipo de ruido, lo que genera incertidumbre
en el conocimiento de su valor real. El análisis de este tipo de problemas se encuentra
dentro del marco de la teorı́a de estimación estadı́stica de señales [32].
Un modelo en espacio de estados (SSM1 , por sus siglas en inglés) se refiere a un

tipo de modelo probabilı́stico que describe la dependencia estocástica entre la varia-
ble de estado latente2 y la medición observada; dicha medición puede ser continua o
discreta. El término espacio de estado surgió en 1960 en el ámbito de la ingenierı́a de
control, producto de la investigación de Kalman [10] (ver capı́tulo 3).
El SSM proporciona un marco general para el análisis determinista y estocástico de
los sistemas dinámicos. Este marco ha sido aplicado con éxito en la ingenierı́a, la
informática y la economı́a, para resolver una variedad de problemas. Un recurso uti-
lizado para describir los SSM es mediante modelos ocultos de Markov(HMMs) [33]. Se
modela el sistema como un proceso donde la probabilidad del siguiente estado
depende de los estados previos o en términos de señales es la representación de una
secuencia de variables aleatorias o deterministas que no son independientes entre sı́.
En este trabajo se utilizó un SSM para construir el filtro de Kalman que define un
algoritmo óptimo para la inferencia o estimación de sistemas lineales en términos
del error cuadrático medio [10].
El objetivo de la modelización en espacio de estados es calcular una estimación
óptima de los estados ocultos ante los datos medidos u observados y puede ser de-
rivado como una forma recursiva de la regla de Bayes [34, 35].
En general, la formulación de un modelo en espacio de estados se da a partir de 2

señales: x(t) que denota el estado e y(0 : t) que representa las mediciones acumuladas
en el intervalo [0, t]. El análisis propuesto mediante el teorema de Bayes define que la
1
Space State Model.
2
Es decir que se encuentra presente pero no es medible.
37
distribución de probabilidad del estado x(t) condicionada a las observaciones y(0 : t)
es:
p(x(t), y(0 : t))

p(x(t)|y(0 : t)) =
p(y(0 : t))
p(x(t)|y(0 : t − 1))p(y(0 : t)|x(t), y(0 : t − 1))
= (1.20)
p(y(t)|y(0 : t − 1))
p(x(t)|y(0 : t − 1))p(y(t)|x(t), y(0 : t − 1))
=
p(y(t)|y(0 : t − 1))
Donde la igualdad presentada en la ecuación 1.20 parte del supuesto de que existe
independencia lineal entre las mediciones. Ahora bien, lo que se desea conocer es la
probabilidad de la predicción del estado x(t) a partir de las mediciones y(0 : t − 1);
esto se conoce como la ecuación de Chapman-Kolmogorov:

p(x(t)|y(0 : t − 1)) = p(x(t)|x(t − 1))p(x(t − 1)|y(0 : t − 1))dx(t − 1) (1.21)
Donde la distribución de probabilidad p(x(t)|x(t − 1)) describe la ecuación de

transición de estado y p(x(t − 1)|y(0 : t − 1)) es la ecuación de medición u observa-
ción. Estas dos ecuaciones proveen las relaciones fundamentales para el desarrollo y
análisis de los modelos en espacio de estados.
El uso de esta representación es poco común, sin embargo, es pertinente cono-

cerlo. Las ecuaciones que se abstraen para ilustrar los modelos en espacio de estados
son lineales. La primera se denomina ecuación de estados y la segunda ecuación de
medición.
ẋ(t) = A(t)x(t) + v(t) (1.22)
y(t) = C(t)x(t) + w(t) (1.23)
En la ecuación de estado (1.22) se puede observar que ẋ(t) de dimensión n pre-

senta una dinámica de Markov de primer orden, debido a que solo depende de su
estado anterior en el tiempo t y se contamina por una señal de ruido v(t). A esta
señal se le conoce como ruido de estado o de proceso1 . El término A(t) es la matriz
de estados con dimensiones n × n, y permite describir la evolución del modelo de es-
pacio de estado como un sistema dinámico determinı́stico o estocástico. También se
1
La ecuación de estado en la literatura también es denominada como ecuación de proceso.
38
puede observar que el planteamiento es para un sistema variante en el tiempo, pero

que fácilmente se convierte en un sistema invariante si A(t) y C(t) son constantes
para todo t.
La ecuación de observación (1.23) entrega la medición y(t), la cual es un vector de
dimensión m y está sujeta a una transformación lineal del estado x(t). Esta ecuación
está contaminada por el ruido w(t), comúnmente conocido como señal de ruido de
medición.
Las señales de ruido v(t) y w(t) se comportan como distribuciones gaussianas de me-
dia nula y sus matrices de covarianza definidas positivas son Q y R, respectivamente.
Desde el punto de vista del modelo de estado interesa conocer la información que
este proporciona; esto es, la posibilidad de analizar el comportamiento dinámico del
sistema a lo largo del tiempo. Para ello se debe tener una expresión explı́cita del
vector de estado. Esta solución exige el cálculo de la ecuación de estado. Debido a
que la ecuación de estado es lineal, ya sea porque el sistema lo es, o porque es el
resultado de la linealización de un sistema no lineal, admite una solución analı́tica
que es fácil de obtener y proporciona información de la evolución temporal del vector
de estado del sistema. La solución puede expresarse para el intervalo T = [t0 , t] como:
t
x(t) = Φ(t0 , t)x(t0 ) + Φ(t0 , t − τ )v(τ )dτ (1.24)
t0
Donde Φ(t0 , t)1 es llamada matriz de transición de estados y permite describir la

transición del estado x(t0 ) al estado x(t). De la solución temporal de la ecuación de
estado y si A es invariante en el intervalo, se tiene que:
Φ(t0 , t) = eA(t−t0 )
En el presente trabajo se utilizará el SSM para representar el modelo utilizado

por el algoritmo de estimación. En el capı́tulo 3 se muestra cómo se representa el
modelo de error en un SSM y cómo el algoritmo de filtro de Kalman obtiene una
estimación de los estados del sistema. El algoritmo implementado requiere que el
SSM sea expresado en tiempo discreto, como se muestra en las ecuaciones (1.25) y
(1.26).
x[k + 1] = Φk+1
k x[k] + v[k] (1.25)
1
Para ver una definición más completa de la MTS ver [36].
39
y[k] = Hk x[k] + w[k] (1.26)
Donde v[k] y w[k] son secuencias de ruido que perturban el estado y la medición
en el instante tk , mientras que Φk+1
k es la matriz transición de estados en tiempo
discreto y está definida sobre el intervalo1 Ts = tk+1 − tk , la cual queda definida para
sistemas variantes en el tiempo como:
Φk+1
k = eA[k]Ts
1
Tiempo de muestreo del sistema discreto.
40
Capı́tulo 2
Navegación inercial
Madam, I have come from a

country where people are
hanged if they talk.
Leonhard Euler
2.1 Sistemas de navegación inercial

Como se mencionó, los sistemas de navegación inercial son aquellos que determi-
nan los parámetros de navegación de un vehı́culo basándose en sensores inerciales;
giróscopos y acelerómetros, los cuales entregan medidas de velocidad angular ω y
fuerza especı́fica f , respectivamente. Estos sistemas poseen una UMI, algún sistema
de adquisición y procesamiento de señales junto con una computadora encargada de
resolver las ecuaciones necesarias para determinar la posición, velocidad y orienta-
ción del vehı́culo.
Los sensores inerciales se caracterizan por entregar mediciones contaminadas con
ruido, lo que provoca la divergencia del sistema [5]. Existen dos tipos de navegación
inercial, mediante plataforma inercial y tipo strapdown (SD). La diferencia radica
en el tipo de montaje de los sensores. Los sensores SD se encuentran adheridos al
vehı́culo, con lo cual las magnitudes que se miden son las que este percibe. Mientras
tanto, los sensores de plataforma están montados en un sistema móvil que posee
un sistema de control y cuyo objetivo es mantener la plataforma invariante frente a
una terna de referencia conocida. Al tener acceso a estas señales de control se puede
medir indirectamente respecto a dicha terna conocida. Tı́picamente las plataformas
son sistemas susceptibles a errores de calibración, ya que el sistema de control la
mantiene estabilizada según la referencia entregada en el proceso de calibración.
Una desventaja de las plataformas inerciales tiene que ver con la imposibilidad para
41
sensar magnitudes en vehı́culos con dinámicas altas, ya que esto se encuentra arrai-
gado a la respuesta del sistema de control que se utiliza. En este trabajo se hace
énfasis en los sistemas SD-INS1 .
2.1.1 Ecuaciones de navegación

Las magnitudes de velocidad angular ω y fuerza especı́fica f se encuentran definidas
en la terna del sensor, la cual llamaremos {s}; sin embargo, dado que la transforma-
ción entre la terna del sensor y el vehı́culo es constante por simplicidad se supone
que la terna {b} y la terna {s} son iguales, y se deja la transformación Csb sin definir,
puesto que se encuentra sujeta a la construcción del vehı́culo. En lo que sigue se toma
como terna de navegación {ν} la terna inercial; por lo tanto, la mecanización para
este sistema corresponde a la identidad. El conjunto de las ecuaciones dinámicas que
describen el movimiento del vehı́culo dadas las ternas {b} e {i}:
Ċbi (t) = Cbi (t)S(ωib

b
(t)) (2.1)
Conociendo Cbi (t0 ) se puede solucionar la ecuación diferencial para todo t > t0 .
v̇ i (t) = Cbi (t)f b (t) + g i (t) (2.2)
ṗi = v i (2.3)
Posteriormente, dada la fuerza especifica f i , se pueden determinar posición y velo-

cidad conociendo p(t0 ) y v(t0 ) para todo t > t0 .
Si bien existe una solución de forma cerrada para este sistema de ecuaciones, en
la práctica este problema es resuelto on − line por la computadora de navegación
mediante algún método de integración numérica. Como es de esperarse la navegación
de actitud requiere solamente la solución de (2.1). Por lo tanto, dado el problema
con condiciones iniciales y(0) = y0 :
y (t) = f (y, t) (2.4)
Existen distintos tipos de métodos de integración numérica que se pueden aplicar;

uno de los más usados por su simplicidad y potencia es el de Runge-Kutta de cuarto
1
Strap-Down Inertial Navigation System.
42
orden, el cual se resume a continuación:
k1 = hf (xj , yj ) (2.5)
h k1
k2 = hf (xj + , yj + ) (2.6)
2 2
h k2
k3 = hf (xj + , yj + ) (2.7)
2 2
k4 = hf (xj + h, yj + k3 ) (2.8)
1
yj+1 = yj + (k1 + 2k2 + 2k3 + k4 ) (2.9)
6
Con el fin de obtener estabilidad en el algoritmo y para simplificar su ejecución
dentro de la computadora de navegación, es conveniente trabajar con cuaterniones
a la hora de solucionar la ecuación cinemática de orientación usando (1.11). El algo-
ritmo empleado es una forma simplificada de resolver la integración, ya que existen
otros métodos que contemplan la normalización del cuaternión [37]. Partiendo de es-
te planteamiento el algoritmo de navegación inercial se puede resumir en el algoritmo
(2.1).
Algoritmo 2.1 Navegación inercial de actitud

for tk = t0 ,t0 ≤ tk≤ tf ,tk+1 = tk + h do
b
ω (t k )
K1 = h2 qbi (tk ) ◦ ib
0

h i K1
b
ωib (tk + h2 )
K2 = 2 [qb (tk ) + 2 ] ◦
0

b h
ω (t k + )
K3 = h2 [qbi (tk ) + K22 ] ◦ ib 2
0

b
h i ωib (tk + h)
K4 = 2 [qb (tk ) + K3 ] ◦
0
qbi (tk+1 ) = qbi (tk ) + 16 (K1 + 2K2 + 2K3 + K4 )
qbi (tk+1 )
q̄bi (tk+1 ) = ||qbi (tk+1 )||
end for
2.1.2 Presupuesto de error

La navegación inercial de actitud es altamente sensible al error en las mediciones y
las condiciones iniciales. A continuación se hace un análisis cuantitativo del error en
orientación que se genera al solucionar la ecuación dinámica de orientación mediante
una velocidad angular contaminada.
43
2.1.2.1 Integración del sesgo
La integración del sesgo provoca un error angular que crece linealmente con el tiem-
po, y además es proporcional a la magnitud del sesgo (ver ecuación 2.10)
t
φ̃b = bω dτ = t · bω (2.10)
0
2.1.2.2 Integración términos de calibración

Esto hace referencia a los factores de escala, desalineamiento, y no linealidades. La
integración de estos factores genera un corrimiento adicional en la integración pro-
porcional a la velocidad angular y al tiempo durante el cual se integran. Asumiendo
b
que ωib es constante, se tiene que:
t
b b
φ̃�,σ = Σ(�, σ)ωib dτ = Σ(�, σ)ωib ·t (2.11)
0
2.1.2.3 Integración del ruido blanco

Como se menciono en el capı́tulo anterior, se supone que las mediciones se encuentran
perturbadas por ruido blanco no correlacionado, con valor esperado 0 y varianza σ 2 .
Puede verse que la integración de este tipo de ruido genera un error φ̃η con las
siguientes caracterı́sticas:

E ψ(τ )dτ t0 =0 (2.12)

V ar ψ(τ )dτ t0 = δt · t · σ 2 (2.13)
Donde:
• t = kδt: Definido en tiempo discreto t el tiempo transcurrido desde el inicio

de la integración.
Este tipo de proceso estocástico es un proceso de Gauss-Markov de primer orden

conocido como movimiento browniano o proceso de caminata aleatoria [5]. Se
puede ver que el error crece de manera proporcional a la raiz cuadrada del
tiempo. El fabricante del sensor entrega un dato llamado Angle Random Walk
(ARW).
44
2.2 Validación de navegación inercial

Esta validación se llevo a cabo con dos trayectorias, las cuales se describen a con-
tinuación junto con las caracterı́sticas de la validación realizada. Es importante re-
marcar el hecho de que tanto los algoritmos como el análisis propuesto se usan para
navegación de actitud y son independientes del vehı́culo al que se aplique. Muestra
de esto es que para la validación se usa una trayectoria de un cohete suborbital
(VS30) y una trayectoria de un satélite (SAC-D). Inicialmente se hace la validación
con los datos de una trayectoria conocida, particularmente la del cohete VS30 y
posteriormente con la segunda trayectoria, que representa una maniobria de actitud
del satélite SAC-D. Para el VS30 se hacen pruebas netamente de simulación por
software y para la segunda trayectoria se hace una simulación HIL sobre el brazo
robótico IRB140.
2.2.1 Trayectoria VS30

Esta trayectoria es el producto de un experimento realizado por CONAE1 [38] el 16
de diciembre de 2007 a las 6:15 horas en Natal, Brasil. En cooperación con la Agencia
Espacial Brasilera (AEB) se lanzó un cohete suborbital que contenı́a una carga útil
desarrollada enteramente en la Argentina. Esta carga útil estaba compuesta por:
• Una UMI compuesta por una UMI comercial Motion Pack de Systron Donner
y un giróscopo IFOG construido por el Centro de Investigaciones Ópticas de
La Plata (CIOP) y un acelerómetro construido por el Instituto Universitario
Aeronaútico.
• Dos receptores GPS desarrollados por la Universidad Nacional de La Plata

(UNLP) y la Universidad Federal de Rio Grande do Norte.
• Un arreglo de celdas solares para medir el vector solar.
• Una computadora de navegación.
La computadora de navegación se encargó de ejecutar un algoritmo de navegación

inercial y de almacenar los datos obtenidos por los sensores antes mencionados, para
realizar análisis y evaluación de los sensores posvuelo. El cohete VS30 alcanzó una
velocidad angular que superó los lı́mites de saturación de los giróscopos; por tanto,
en este trabajo se usaron datos sintéticos de una trayectoria.
1
Este trabajo se encuentra documentado en [38].
45
Figura 2.1. Velocidad angular del VS30.
2.2.1.1 Resultados
Las simulaciones realizadas pretenden evaluar y corroborar la validez del proceso de
integración llevado a cabo, ası́ como del modelo de error elegido para el giróscopo.
Los datos seleccionados para contaminar la velocidad angular se extraen de la hoja
de datos de la UMI Adis 16364 [39]; los parámetros de la simulaciones realizadas se
resumen en la tabla 2.1.
Parámetro Magnitud
Condiciones Iniciales [◦ ] ±3
Sesgos [◦ /s] ±0,2
Factor de escala [ %] 1
Falta de ortogonalidad [ %] 5
Varianza del ruido blanco [◦2 ] ±3
Frecuencia de sensor interno ω[ Hz.] 50
Cuadro 2.1: Parámetros utilizados en la simulación.
Se ejecuta el algoritmo contaminando la velocidad angular con cada parámetro

por separado, permitiendo ver el impacto de cada fuente de error en el sistema. Los
resultados de error se presentan en ángulos de Euler (yaw, pitch, roll).
46
Figura 2.2. Error por condiciones iniciales.

Figura 2.3. Error por sesgo.

Evidentemente el error más crı́tico en el proceso de integración es el sesgo; aspecto

que no es grave ya que es posible caracterizarlo y compensarlo de forma off-line. El
resto de los parámetros contribuye en menor medida a la propagación del error.
Considerando que la duración de la trayectoria es de aproximadamente 370 s, el
error en orientación alcanzado es muy alto.
47
Figura 2.4. Error por factor de escala.
Figura 2.5. Error por falta de ortogonalidad.

2.2.2 Trayectoria SAC-D sobre el robot

Se conectó mecánicamente la UMI a la sexta articulación del robot, esto aprove-
chando las facilidades que este posee para cambiar de herramienta; además, Se pro-
gramó la trayectoria de actitud del SAC-D. Para ello es necesario tomar los datos
de telemetrı́a, los cuales están en formato de cuaternión y resolver el problema in-
48
verso para el robot IRB140, y transformar ası́ la información a la representación de

ángulos articulares.
Figura 2.6. Montaje UMI en el robot.

Fuente: [40].
El programa principal que ejecuta el robot se encarga de leer el archivo con la

trayectoria y realizar los movimientos necesarios en sus distintos eslabones. Con el
fin de lograr que la trayectoria programada se lleve a cabo sin que el robot pase por
alguna singularidad o deba cambiar de configuración [24, 41] el programa se encarga
de llevar al robot a una posición inicial prefijada. En este experimento se almacenan
los datos de velocidad angular entregados por la UMI. Una consideración importante
en la realización de este experimento es el hecho de acelerar la lectura de los datos
de actitud. La duración de la trayectoria real es de aproximadamente 60 min. Como
la dinámica del satélite es lenta al tratar de seguir esta trayectoria el robot se mueve
a pasos; esto es un problema para la adquisición de datos ya que depende de la
resolución de las mediciones entregadas por la UMI. Por esto se decidió acelerar
la ejecución de la trayectoria 50 veces para generar ası́ un movimiento medible de
acuerdo con la resolución de la UMI. Esto hace que la trayectoria simulada sobre el
robot dure cerca de 80 s. La frecuencia de muestreo para la velocidad angular en
este experimento y los siguientes será de 33 Hz1 .
1
Ver capı́tulo 6.
49
Figura 2.7. Velocidad angular.
A partir de estos datos se ejecutó el algoritmo (2.1). Con el objetivo de darse una
idea de la trayectoria realizada por el robot y de los resultados obtenidos al ejecutar
el algoritmo se muestra una comparación entre los ángulos de Euler (yaw, pitch y
roll ) reales1 y los calculados dada la velocidad angular (ver figura 2.7).
2.2.2.1 Resultados
Los resultados muestran la pobre calidad del proceso de integración directa, como
era de esperarse el error es alto. Aparentemente existe un sesgo, pero este no es tan
alto porque la pendiente de crecimiento del error es pequeña. Esto tiene relación con
el hecho de que en la UMI se programó un proceso de calibración del sesgo en la
etapa previa al inicio de la trayectoria. El análisis sobre la existencia o no de sesgo
en este experimento se realiza en los siguientes capı́tulos.
1
El yaw, pitch y roll reales son extraı́dos de la trayectoria de telemetrı́a por una transformación
50
Figura 2.8. Actitud SAC-D.

Figura 2.9. Error de actitud.

2.3 Conclusiones
Tomando en consideración los resultados sobre los experimentos para las trayecto-
rias del VS30 y el SAC-D, queda evidenciado el carácter divergente de la navegación
inercial con sensores de bajo costo como es la ADIS16364. Se realizó un análisis del
51
presupuesto de error para el proceso de integración realizado, se describió un méto-
do de integración numérico para solucionar las ecuaciones dinámicas, se aplicó este
método a la solución de la ecuación dinámica de actitud y se encontró el error entre
la integración y las magnitudes reales para dos trayectorias.
Los parámetros que más afectan el desempeño del algoritmo son el sesgo, las
condiciones iniciales y el factor de escala. Sin embargo, esto queda sujeto a la tra-
yectoria, puesto que para una trayectoria con magnitudes de velocidad angular pe-
queñas (dinámicas lentas) puede llegar a ser nociva también la perturbación de ruido
blanco. En este caso, para la trayectoria del VS30, como la magnitud de velocidad
angular en el eje x (roll ) es tan alta, el efecto del ruido en la integración no es tan
apreciable, contrario a lo que sucede en la trayectoria del SAC-D donde la dinámica
es más lenta; razón por la cual, la excitación de los giróscopos es baja.
52
Capı́tulo 3
Filtro de Kalman
If I have seen further

it is by standing on the
shoulders of giants
Isaac Newton
3.1 Introducción
Mientras trabajaba en el Research Institute for Advanced Studies, Rudolph Kalman
en 1960 [10] propuso una solución al problema cuadrático lineal o problema de Wie-
ner [42]. Su solución es un método iterativo capaz de suavizar, estimar o predecir de
manera óptima el estado de un sistema lineal, siempre y cuando el sistema modelado
cumpla con algunas condiciones [43, 44].
Existen diferentes variantes al método original que han sido propuestas en muchos
casos como soluciones ad hoc a problemas especı́ficos[45, 46, 47]. Desde su publi-caci
ón ha sido usado con éxito en áreas como control, economı́a, navegación, entre otras.
Para citar un ejemplo, el filtro de Kalman hizo parte del sistema de navegación de la
misión Apollo que llevó al hombre a la Luna en 19691.
1
Rudolph Kalman mostró sus resultados a Stanley F. Schmindt en el centro de investigación
Ames de NASA, quien inmediatamente reconoció el potencial de este nuevo método y empezó a
trabajar en su implementación; posteriormente mostró sus resultados a Charles Battim, encargado
de liderar el proyecto guiado-navegación-control de la misión Apollo.
53
3.2 Formulación del problema
Considerando un sistema dinámico lineal en tiempo discreto:
xk +1 = Φkk +1 xk + Γk ξk (3.1)
yk = Hk xk + ηk (3.2)
Donde:
• φkk +1 : es la matriz transición del estado k al estado k + 1.
• La primera ecuación corresponde a la ecuación del proceso, se encuentra con-

taminada por un ruido ξ de media cero y covarianza definida:
V ar[ξk ] = Qdk . (3.3)
• La segunda ecuación corresponde a la ecuación de medición, se encuentra con-

taminada por un ruido η de media cero y covarianza definida:
V ar[ηk ] = Rdk (3.4)
• Se supone que los ruidos de proceso y de medición son descorrelacionados, con

lo cual:
E [ξi · ηjT ] = 0 (3.5)
El problema consiste en determinar de manera óptima un x̂k a partir de:
• Las observaciones: (y0 , y1 , · · · , yk ).
• El conocimiento del sistema: representado en el modelo.
• Las estadı́sticas de las perturbaciones: valor esperado y covarianza.
• Las condiciones iniciales.
Es posible plantear una función costo que sea no negativa y no decreciente [44] en
términos del error cuadrático medio de estimación:
Jk = E [(xk − x̂k )2 ] (3.6)
Usando como herramientas el principio de ortogonalidad y el teorema del

estima-dor condicional [48, 49] se puede llegar a demostrar que el algoritmo de
estimaciónóptima tiene la estructura mostrada en la tabla 3.1.
54
Si las suposiciones de gaussianidad [50] e independencia del ruido de proceso y

de medición son ciertas para el sistema que se analiza y si el modelo usado es lineal,
este algoritmo se caracteriza porque:
• Es un algoritmo de estimación no sesgado, esto es, el valor esperado del error

de estimación es cero.
• Es el estimador de mı́nima varianza, es decir, la covarianza del error es la m

ı́nima posible.
• Es asintóticamente estable [51].
En realidad encontrar un sistema que cumpla todas estas suposiciones es utópico;

sin embargo, se pueden manejar sistemas que no cumplan estas u otras caracterı́sti-
cas. Por ejemplo, en sistemas contaminados con ruidos correlacionados es posible
plantear un sistema de estado aumentado [52] y para manejar sistemas no lineales
se pueden ejecutar linealizaciones como en el Filtro de Kalman Linealizado (LFK1 ,
por sus siglas en inglés) o el FKE.
3.3 Filtro de Kalman extendido

La limitación más grande que tiene el filtro de Kalman radica en estar construido
para solucionar problemas lineales, puesto que en la realidad los sistemas estudiados
son no lineales. La estimación en sistemas no lineales requiere el uso de técnicas
complejas de análisis y diseño, ası́ como una alta capacidad de cómputo. El filtro de
Kalman extendido maneja este problema ejecutando una linealización del sistema
en cada iteración sobre el último estado estimado, lo cual lo hace mucho más simple
y computacionalmente efectivo. Esto se ha demostrado aplicándolo a una gran can-
tidad de problemas con éxito [53, 54], en especial en la implementación de sistemas
de navegación [55, 56].
1
Linearized Kalman Filter.
55
Ecuaciones para el Filtro de Kalman
Modelo en espacio de estados:
xk +1 = Φkk +1 xk + Γk ξk (3.7)
yk = Hk xk + ηk (3.8)
Condiciones iniciales conocidas:
x̂0 = E [x0 ] (3.9)

T
P0 = E [(x0 − E [x0 ])(x0 − E [x0 ]) ] (3.10)
Iteración:
Estimación a priori :
x̂k− = Φkk−1 x̂k+−1 (3.11)
Propagación de la covarianza:
T
P− k k d T
k = Φk −1 Pk−1 Φk −1 + Γk −1 Qk−1 Γk −1 (3.12)
Cálculo de la ganancia de Kalman:

d −1
Kk = P − T − T
k Hk Hk Pk Hk + Rk (3.13)
Innovación:

x̂k+ = x̂k− + Kk yk − Hk x̂k− (3.14)
Actualización de la covarianza:
−
P+
k = (I − Kk Hk ) Pk (3.15)
Cuadro 3.1: Filtro de Kalman.
56
3.4 Estimación de actitud

Un enfoque muy popular usado en la implementación del filtro de Kalman para
navegación consiste en trabajar con el error de los parámetros de navegación y no
con los parámetros propiamente dichos. Ya que el FKE trabaja con linealizaciones
de la dinámica real, es factible proponer que la linealización del modelo de error de
los parámetros de navegación tiene más similitud con el comportamiento real del
error (ver ecuación 3.161 ) en los parámetros de navegación.
x̄k = xk − x̂k (3.16)
Otra ventaja de este enfoque es que permite pasar de manera intuitiva y simple
al planteamiento de estado aumentado en el cual se contempla un modelo de error
en los sensores, lo que permite realizar una calibración on-line del sistema.
3.4.1 Modelo de error de actitud

En algunas aplicaciones como el control de actitud de un satélite es común desaco-
plar la estimación de los parámetros de navegación; esto es, estimar por un lado
posición y velocidad mientras otro sistema se encarga de la estimación de actitud.
Este desacoplamiento permite obtener mejores resultados en desempeño del esti-
mador de actitud, puesto que al estar acoplados parte del error que se tiene en la
determinación de actitud puede provenir del resto del modelo de posición y velocidad.
Para construir el modelo de error de actitud en este trabajo se usa la represen-

tación en MCD y se parte de la siguiente definición:

C̄bi = Cbi I + S(φb ) (3.17)
Donde:
• Cbi : es la transformación de la terna {b} a la {i}, real del vehı́culo.
• C̄bi : es la transformación de la terna {b} a la {i}, calculada por el sistema.
• φb : es el vector de error de orientación entre las transformaciones real y calcu-

lada por el sistema de navegación.

• I + S(φb ) : es la transformación de perturbación existente entre las transfor-
maciones real y calculada por el sistema de navegación.
1
Donde: xk es el vector de valores reales, x̂k vector de valores estimado y x̄k el vector de error.
57
Inicialmente se establece esta transformación partiendo de que φb es lo suficien-
temente pequeño para poder definir la transformación de perturbación de esa forma.
Por otro lado, se tiene la ecuación (2.1):
Ċbi (t) = Cbi (t)S(ωib

b
(t)) (3.18)
C̄˙ bi (t) = C̄bi (t)S(ω̃ib

i
(t)) (3.19)
Donde:
b
• ωib (t): es la velocidad angular real del vehı́culo.
b
• ω̃ib (t): es la velocidad que lee la computadora de navegación de los giróscopos.
Reemplazando (3.19) en (3.17):

b
C̄bi (I − S(φb ) = C̄bi I − S ωib (3.20)
Derivando y separando algunos términos:

C̄˙ bi I − S(φb ) − C̄bi S(φ̇b ) = C̄bi S(ωib
i
) − C̄bi S(φb )S(ωib
i
) (3.21)
Reemplazando 3.18 en el primer término del lado izquierdo, reordenando y agru-

pando se obtiene:
C̄bi S(δωib
b
) + C̄bi [S(φb )S(ωib
b b
) − S(ωib )S(φb )] = C̄bi S(φ̇b ) (3.22)
El término de perturbación es la diferencia entre la velocidad angular real del

vehı́culo y la usada por el sistema:
b i i
δωib = ωib − ω̃ib (3.23)
Continuando con (3.22) si se resuelve el segundo termino de la izquierda y se

desprecian los productos de segundo orden resultan:
δωφb 0 (3.24)
C̄bi S(δωib
b
) − C̄bi S(S(ωib
i
)φb ) C̄bi S(φ̇b ) (3.25)
58
Al multiplicar por C̄biT y aplicar S −1 a ambos lados de la igualdad1 se puede

obtener un modelo lineal aproximado que representa el error de actitud:
φ̇b −S(ωib
i
)φb + δωib
i
(3.26)
La ecuación (3.26) define el comportamiento del sistema sobre el cual se aplica el

filtro de Kalman.
3.4.2 Fusión de información

Como se viene mencionando, el filtro de Kalman fusiona la información que proviene
de distintos sensores, cada uno de estos posee un modelo que lo caracteriza y en
donde se contempla la incertidumbre de la medición.
Los sensores tı́picos que usa un sistema de navegación de actitud son seguidores
de estrellas (Star Tracker ), cámaras CCD, sensores de horizonte, de sol, de tierra,
giróscopos y magnetómetros [57, 58]. Sin importar qué proceso lleva a cabo para
efectuar la medición, el sensor adicional de actitud debe entregar una transformación
de la terna del cuerpo {b} a la terna {i} con una frecuencia que puede o no ser
constante; es lógico concluir que cuanto más frecuente sea la medida externa menos
crecerá el error dentro de esos intervalos en los cuales la estimación se realiza a partir
del modelo (3.26).
En este sentido, es posible definir el siguiente modelo de error para el sensor adicional:
i

C̆bR = Cbi I + S(φbR ) (3.27)
Donde:
i
• C̆bR : es la transformación que entrega el sensor adicional.
• φbR : es el error de actitud del sensor adicional.
A partir de esta definición se puede determinar la relación que existe entre el

error de actitud del giróscopo y el sensor adicional, la cual es muy importante en la
realización del filtro de Kalman.
La transformación real puede escribirse en términos del error del giróscopo o del
error del sensor externo, lo que permite considerar la siguiente igualdad:
i

C̆bR I − S(φbR ) = C̄bi I − S(φb ) (3.28)
1
S −1 : Es la inversa de S y S −1 = −S
59
Separando los términos de interés:
i
� �� T
C̆bR C̄biT = I − S(φb ) I − S(φbR ) (3.29)
Si se multiplican los términos del lado derecho y se reordena se tiene:
i
� �
C̆bR C̄biT = I + S(φbR ) − S(φb ) − S(φb )S(φbR ) (3.30)
Nuevamente suponiendo que φb y φbR son suficientemente pequeños se puede eliminar

el término de segundo orden y la expresión se simplifica:
� �
i
C̆bR C̄biT − I S φbR − φb (3.31)
Es muy importante tener en cuenta que las simplificaciones que se hacen en (3.25)
y (3.30) son válidas siempre y cuando los errores de actitud se mantengan acotados
[59]. Esta suposición juega un papel importante en el desempeño de la estimación. En
este planteamiento se tiene el conocimiento de C̆biR gracias al sensor externo y C̄bi
puesto que es la salida del navegador inercial, con lo cual la relación entre los errores
de actitud está dada por:
� �
φbR − φb S −1 C̆bRi
C̄biT − I (3.32)
Donde la operación S −1 se refiere a la extracción de las componentes vectoriales

de la matriz definida en la ecuación (1.4).
3.5 Modelo del sistema

A partir de los conceptos expuestos hasta aquı́, se construye un modelo dinámico
lineal para el sistema de navegación de actitud desarrollado en este trabajo. Se parte
de un modelo que no contempla los parámetros del modelo de error propuesto en la
sección 1.3.4, con el fin de evaluar el impacto de estos parámetros en el sistema de
navegación. Las ecuaciones de proceso (3.1) y de medición (3.2) quedan descritas
en la siguiente definición:
⎡ ⎤ ⎡ ⎤ ⎡ b⎤ ⎡ ⎤ ⎡ ⎤
φ˙bx 0 −ω̃z ω̃y φx 1 0 0 ξφx
⎢ ˙b ⎥ ⎣
φ
⎣ y⎦ = ω̃z 0 −ω̃x ⎦ · ⎣φby ⎦ + ⎣0 1 0⎦ · ⎣ξφy ⎦ (3.33)
φ˙bz −ω̃y ω̃x 0 φbz 0 0 1 ξφz
⎡ ⎤ ⎡ ⎤ ⎡ b ⎤ ⎡ ⎤ ⎡ ⎤
yx 1 0 0 φxR 1 0 0 ηφx
⎣ yy ⎦ = ⎣ 0 1 0⎦ · ⎣φbyR ⎦ + ⎣0 1 0⎦ · ⎣ηφy ⎦ (3.34)
yz 0 0 1 φbzR 0 0 1 ηφ z
Donde:
60
• ξφ : es el ruido de proceso.
• ηφ : es el ruido de medición.
El algoritmo del filtro de Kalman discreto utilizado para la implementación sobre

la computadora de navegación (el tiempo por iteración del algoritmo se define como
Ts = (tk+1 − tk ), se presenta en la ecuación (3.35).
T

φk+1 = eA[k]Ts φk + eA[k]Ts ξk Ts (3.35)
k=0
yk = Hφk + ηk (3.36)
Partiendo de este planteamiento y del filtro de Kalman descrito en (3.1), se

plantea el algoritmo (3.1) para la estimación del error de actitud. El desarrollo del
filtro de Kalman depende directamente de la debida caracterización en términos
estadı́sticos de los procesos envueltos, esta caracterización se ve reflejada en las
matrices de covarianza Q, R y la condición inicial de la matriz de covarianza del
error de estimación P0. La definición de las condiciones iniciales influye en el tiempo
de convergencia del filtro [60]. La elección de una P con valores grandes implica que
se tiene poco conocimiento de las condiciones iniciales del estado x; en el caso
contrario se le da más credibilidad a las condiciones iniciales.
En este algoritmo se reconocen dos fases; por un lado, dada la información cono-
cida en el instante [k − 1] se predice el estado en el instante [k]. Mientras no exista
medida del sensor de ayuda1 la salida del sistema está determinada por el modelo.
La siguiente fase es de corrección o actualización en la cual se utiliza la medida
disponible del sensor de ayuda para comparar y corregir el estado que se predice a
partir del modelo.
1
En el desarrollo de este trabajo los términos sensor externo y sensor de ayuda se usan indis-
tintamente.
61
Algoritmo 3.1 Estimación de error de actitud con FKE
P = P0
Q = Q0
q̄ i (0) = q˘i (0)
b b
x̂0 = φ0
for tk = t0 ,t0 ≤ tk≤ tf ,tk+1 = tk + h do
b
ω̃ (tk )
K1 = h2 q̄bi (tk ) ◦ ib
0

h i K1
b
ω̃ib (tk + h2 )
K2 = 2 [q̄b (tk ) + 2 ] ◦
0

h i K2 ω̃ib (tk + h2 )
b
K3 = 2 [q̄b (tk ) + 2 ] ◦
0

b
h i ω̃ ib k + h)
(t
K4 = 2 [q̄b (tk ) + K3 ] ◦
0
q̄bi (tk+1 ) = q̄bi (tk ) + 16 (K1 + 2K2 + 2K3 + K4 )
q i (t )
q̄bi (tk+1 ) = ||qbi (tk+1
b k+1 )||
C̄bi (tk+1 ) = fq2c (q̄bi (tk+1 ))
i
A = −S(ω̃ib (tk+1 ))
k+1 Ah
Φk = e
x̂−k+1 = Φk x̂k
k+1 +
k+1 + k+1 T
P− k+1 = Φk Pk Φk + HQHT h
if Sensor externo diponible then
q̆ i (t )
q̆bi (tk+1 ) = ||q̆bi (tk+1
b k+1 )||
C̆bi (tk+1 ) = fc2q C̄bi (tk+1 )
T
ϕk+1 = S−1 [C̆bi (tk+1 ) C̄bi (tk+1 ) − I]
Kk+1 = P− T +
k+1 H [HPk H + R]
−1
ιk+1 = Kk+1 ϕk+1

−
x̂+
k+1 = x̂k+1 + ιk+1
−
P+k+1 = (I − Kk+1 H)Pk+1
else
−
x̂+
k+1 = x̂k+1
Pk+1 = P−
+
k+1
end if
end for
62
Figura 3.1. Fases del filtro de Kalman.

3.6 Validación de la estimación en error de actitud

La validación de la estimación del error de actitud se llevó a cabo con las mismas
trayectorias mencionadas en el capitulo 2. La particularidad en estas simulaciones
es la existencia de la medida provista por un sensor externo.

Dado el algoritmo 3.1, se implementa el algoritmo de Kalman para la estimación de
error de actitud, la trayectoria usada para realizar las pruebas es la misma definida
anteriormente, con la particularidad de que se debe emular el comportamiento de un
sensor externo en el sistema. Ya que se tiene conocimiento de la trayectoria sintética
sin contaminación, es posible determinar la actitud real del sistema. A partir de estos
datos no contaminados se genera la velocidad angular contaminada y el cuaternión
de transformación del sensor externo contaminado. En la tabla 3.2 se resumen los
parámetros usados para realizar esta simulación.
El ruido de contaminación en ambos procesos es gaussiano con media nula y
varianza conocida, como queda establecido en la tabla 3.2.
Para evaluar el desempeño del sistema se verifica el comportamiento de las cotas de
error 2σ; estas son extraı́das de la matriz de covarianza del error P. Además, se realiza
una validación estadı́stica del algoritmo usado, para esto se procesa la información
con ruido de contaminación diferente N veces. A partir de estas N realizaciones se
puede extraer para cada instante la media y el desvı́o del error en estimación dadas
las N realizaciones.
En la figura 3.2 se observan los resultados de la simulación para el error de esti-
mación y sus respectivas cotas de 2σ.
63
◦
Desvı́o del sensor externo [ ] ±2
Condiciones Iniciales[◦ ] ±3
Sesgos Velocidad [◦ /s] ±0,1
Falta de ortogonalidad[ %] 5
Desvı́o en ω [◦ ] ±3
Frecuencia de sensor externo [Hz] 2.5
Frecuencia de sensor interno ω [Hz] 50
Figura 3.2. Error de estimación y sus cotas de 2σ para 200 pruebas.

El hecho de que el error caiga dentro de sus cotas estimadas es buen indicio de
que el algoritmo esta funcionando bien; por otro lado, se observa que la magnitud del
error no sobrepasa de 0,5◦ . Esto comparado con el desvı́o real incorporado de ±2◦ y
±3◦ muestra que al utilizar el filtro de Kalman la incertidumbre que se obtiene sobre
la información es menor que la del mejor sensor. Como se mencionó, el funcionamien-
to del estimador se corrobora verificando el comportamiento de las innovaciones. En
este caso se evaluó la autocorrelación eliminando el sesgo en la contaminación de
la velocidad angular, lo cual muestra que cuando existe un sesgo en la medición, la
autocorrelación de la innovación no se parece mucho a la función delta de Dirac o,
lo que es equivalente, que la secuencia no es exactamente un proceso blanco. En [60]
64
se muestra que si la innovación no es un proceso blanco de media nula, el problema

puede deberse bien a una falla del modelo o a un error en las estadı́sticas supuestas
para el modelo. El análisis cualitativo de los gráficos de autocorrelación entrega una
idea de qué tan adecuado es el modelo planteado y permite evaluar indirectamente
el comportamiento del estimador. Otro enfoque para el análisis del desempeño del
algoritmo es la evaluación de la observabilidad del modelo (ver capı́tulo 5).
Figura 3.3. Autocorrelación de la innovación sin sesgo.

Figura 3.4. Autocorrelación de la innovación con sesgo.

65
Con el objetivo de obtener los datos para esta simulación, se debe nuevamente co-
nectar fı́sicamente la UMI a la sexta articulación del robot y se programa para que
ejecute la trayectoria del SAC-D; en este caso el sensor externo será emulado por el
robot.
Figura 3.5. Cuaternión medido por el robot.

Se debe ejecutar un servicio RAP1 desde la CN para realizar la adquisición de

datos de cuaternión provistos por el robot. Si bien ABB no provee un dato exacto
de la incertidumbre que se tiene en esta medición, empı́ricamente se estima que es
de ±1◦ . En la figura 3.5 se muestra el cuaternión obtenido en este proceso.
Se decide contaminar el cuaternión del robot con ruido blanco, lo que permite evaluar
la importancia del sensor externo y su impacto en el desempeño del sistema. El desvı́o
del sensor externo se estableció en 5◦ .
3.6.2.1 Resultados
Los datos se almacenaron y fueron procesados usando el algoritmo previamente
descrito; en la figura 3.6 se comparan el valor de la estimación y el real para el error
en actitud de la trayectoria. A simple vista se observa que el desempeño comparado
con el navegador inercial es mucho mejor; en la figura 3.7 se puede observar la
evolución en el error de estimación y sus respectivas cotas de 2σ.
1
RAP: Robot Application Protocol.
66
Figura 3.6. Comparación error real y estimada.

Figura 3.7. Estimación del error en actitud y cotas de 2σ.

67
Figura 3.8. Autocorrelación de la innovación.
Figura 3.9. Comparación error real (rojo), error estimado (azul).

68
Similar a lo que ocurre con la trayectoria del VS30 se observa que existe alguna
correlación en la innovación. Podrı́a llegar a suponerse que esta correlación observada
en la innovación tiene que ver con un sesgo u otro parámetro por compensar en las
mediciones. El perfil de error real comparado con el estimado puede observarse en
la figura (3.9), aquı́ se observa que el error en actitud está acotado entre 20◦ , valor
obtenido mediante la navegación inercial.
3.7 Conclusiones
Teniendo en cuenta el desempeño del navegador inercial se propuso el algoritmo de
Kalman para estimar los parámetros de navegación del sistema, en particular el error
de actitud. Se construyó un modelo linealizado de error de actitud para implementar
el algoritmo de estimación, un modelo de error para el sensor externo y se describió el
filtro de Kalman para estimación de error actitud.
Error VS30 SAC-D

◦
Valor final [ ]
ψ -1,9409 1,7363
θ -2,0095 0,2297
φ -0,0873 1,6678
Máximo [◦ ]
ψ 2,0155 3,6498
θ 2,4019 3,1193
φ 0,5406 2,6489
Cuadro 3.3: Resultados de error en estimación.
A partir de este filtro y de los datos de las dos trayectorias de referencia se reali-
zaron unas pruebas. Los resultados para la trayectoria del VS30 se obtienen a partir
de un análisis estadı́stico; se corrió el algoritmo 200 veces con ruidos de contamina-
ción diferentes. Se muestra que el error de estimación esta dentro de las cotas de 2σ
y que la incertidumbre que se obtiene después de ejecutar el algoritmo es menor que
la del mejor sensor. Un resumen de los resultados mas importantes se puede ver en la
tabla 3.3. Se comprobó el funcionamiento del FKE calculando la autocorrelación de
las innovaciones y se comparó el resultado de dicha autocorrelación cuando la velo-
cidad angular contiene sesgo y cuando no lo tiene. Se realizó un proceso similar para
los datos reales obtenidos de la UMI y el robot, el error de estimación se encuentra
dentro de las cotas estimadas por el algoritmo. La autocorrelación de la innovación
69
da indicios de que puede haber un sesgo en las mediciones. Los resultados obtenidos
muestran que el modelo lineal conseguido es válido y ahora puede ser extrapolado a
la construcción del sistema de navegación integrada.
70
Capı́tulo 4
Navegación integrada:
calibración On-Line
In questions of science the

authority of a thousand is
not worth the humble
reasoning of a single
individual.
Galileo Galilei
4.1 Introducción
Como se observa en las secciones anteriores, la navegación inercial presenta una alta
dependencia de la calidad del sensor inercial; se puede ver que el error en la medición
del giróscopo o un error en las condiciones iniciales del proceso de integración genera
la divergencia del sistema.
Con el fin de solucionar este problema es necesario encontrar la manera de reducir
al máximo el error en las mediciones del giróscopo y en las condiciones iniciales. El
problema de las condiciones iniciales se resuelve mediante el proceso de alineación1 ,
realizado en la fase previa al arranque del vehı́culo. Los errores determinı́sticos en
los sensores inerciales pueden ser reducidos mediante su calibración; sin embargo,
mitigar el impacto del error aleatorio que contiene la medida requiere una estra-
tegia más elaborada. A pesar de esto, los sensores inerciales son fundamentales en
cualquier sistema de navegación moderno puesto que en condiciones normales son
1
La alineación consiste en determinar con la menor incertidumbre posible las condiciones ini-
ciales del sistema que en términos generales son posición y orientación.
71
invulnerables1 a fenómenos externos al vehı́culo diferente a lo que sucede con sis-
temas satelitales o cámaras CCD, los cuales son dependientes de distintos factores.
Además de esto son una fuente de información que se encuentra disponible a una
alta frecuencia comparados con otro tipo de sensores.
Existe un esquema más completo que además de corregir la magnitud de los paráme-
tros de navegación mediante las medidas de sensores externos, estima los errores de
la UMI y los corrige on-line, de esta manera forma una arquitectura realimentada
como se muestra en las figuras 4.1 y 4.2. La necesidad de este tipo de arquitectura
surge porque, a pesar de compensar (off-line) los parámetros determinı́sticos del
sensor, generalmente queda un remanente que depende por ejemplo de parámetros
externos como la temperatura o las variaciones en la fuente de alimentación. La
construcción de esta arquitectura requiere modelar la dinámica de los errores de la
UMI y construir un vector de estado aumentado; dicho modelo dinámico será usado
por el estimador-observador (FKE). La salida del filtro de Kalman se divide en dos:
por un lado, permite la corrección de los parámetros de navegación y por otro se
realimenta al sistema de navegación inercial para corregir las magnitudes de la UMI.
Figura 4.1. Modelo de navegación integrada.

1
Existen condiciones extremas como campos electromagnéticos fuertes que pueden afectar la
medición de los sensores inerciales.
72
4.2 Modelo dinámico del error en la UMI

Dado el modelo paramétrico establecido en la sección 1, se pretende estimar los
parámetros b, �, �, las cuales se suponen constantes aleatorias. Se ha mostrado que
si estos parámetros se modelan como procesos de Markov de primer orden, el filtro de
Kalman tardará menos en realizar la estimación [59, 61]. A continuación se desarrolla
en detalle el modelo de cada parámetro.
4.2.1 Sesgo
El bias o sesgo en las medidas del giróscopo es muy común, su dinámica está deter-
minada por:
ḃ = 0 (4.1)
b(0) = b0 (4.2)
Donde:
• b ∈ R3 : es el sesgo existente entre la magnitud real de velocidad angular y el

valor entregado por el giróscopo.
Modelándolo como proceso de Markov se tiene:
ḃ = ξb (4.3)
b(0) = b0 (4.4)
Donde:
• ξb ∈ R3 : es un ruido blanco:
E�ξb � = b0 (4.5)

E ξbT ξb = Qb << I (4.6)
4.2.2 Factor de escala

El error en factor de escala es proporcional a la velocidad angular, su modelo dinámi-
co con forma de proceso de Markov es:
73
�˙ = ξ� (4.7)
�(0) = b0 (4.8)
Donde:
• ξ� ∈ R3 : es un ruido blanco:
E�ξ� � = �0 (4.9)
� �
E ξ�T ξ� = Q� << I (4.10)
• Se define la matriz de covarianza Q� = Υ(�):

⎡ ⎤
�x 0 0
Υ(�) = ⎣ 0 �y 0 ⎦ (4.11)
0 0 �z
4.2.3 Falta de ortogonalidad

Por último, para el error por falta de ortogonalidad su modelo dinámico en forma
de proceso de Markov es:
�˙ = ξ� (4.12)
�(0) = �0 (4.13)
Donde:
• ξ� ∈ R6 : es un ruido blanco:
E�ξ� � = �0 (4.14)
� �
E ξ�T ξ� = Q� << I (4.15)
• Definiendo Γ̄: ⎡ ⎤
0 �xy �xz
Γ̄ = ⎣�yx 0 �yz ⎦ (4.16)
�zx �zy 0
• Se define Γ: ⎡ ⎤
�xy �xz 0 0 0 0
Γ(�) = ⎣ 0 0 �yx �yz 0 0⎦ (4.17)
0 0 0 0 �zx �zy
74
Con lo cual el modelo compacto del error del giróscopo está dado por:
b b
ω̃ib = (I3 + Υ + Γ̄))ωib + bω + ηω (4.18)
Una propiedad útil de las funciones Υ, Γ y Γ̄ para construir el modelo en espacio

de estados es:
b b
Υ(ωib )� = Υ(�)ωib (4.19)
b b
Γ(ωib )� = Γ̄(�)ωib (4.20)
4.2.4 Modelo en espacio de estados

El error en los giróscopos queda completamente definido por el modelo de la ecua-
ción 4.18 y las definiciones de sus dinámicas; por otro lado, se habı́a determinado
un modelo de error para la actitud del sistema, el cual depende también del error
en las mediciones de los giróscopos. Por tanto, el modelo del estimador completo
está dado por:
• La ecuación de proceso:
⎡ ⎤ ⎡ b b b
⎤⎡ ⎤ ⎡ ⎤⎡ ⎤
φ̇ −S(ωib ) I Υ(ωib ) Γ(ωib ) φ I3x3 03x3 03x3 03x6 ηφ
⎢ḃ⎥ ⎢ 03x3 0 0 0 ⎥ ⎢b⎥ ⎢03x3 I3x3 03x3 03x6 ⎥ ⎢ ηb ⎥
⎢ ⎥=⎢ 3x3 3x3 3x6 ⎥ ⎢ ⎥+⎢ ⎥⎢ ⎥
⎣ �˙ ⎦ ⎣ 03x3 03x3 03x3 03x6 ⎦ ⎣ � ⎦ ⎣03x3 03x3 I3x3 03x6 ⎦ ⎣ η� ⎦
�˙ 06x3 06x3 06x3 06x6 � 06x3 06x3 06x3 I6x6 η�
(4.21)
• La ecuación de medición:
⎡ ⎤
φ
� � � � ⎢b⎥ � ��
φ = I3x3 03x3 03x3 03x6 ⎢ ⎥
⎣ � ⎦ + I3x3 ηr (4.22)
�
4.3 Determinación de actitud

Como se mostró en el capı́tulo 3 la navegación integrada (ver modelo en figura
4.2) comprende dos fases: en la primera se realiza la estimación del error a partir
del algoritmo de Kalman para sistemas lineales y en la segunda se realimentan los
estados estimados. Este proceso se realiza para cada iteración suponiendo que el
estado estimado es correcto cada vez que se realiza una corrección, el error del
sistema se hace cero. Ası́ pues, es necesario llevar a cero el vector de estado del filtro
de Kalman después de la corrección en cada iteración.
75
Figura 4.2. Navegación integrada.
4.3.1 Calibración on-line

La corrección en velocidad angular o calibración on-line se lleva a cabo de acuerdo
con el modelo de error planteado. En cada iteración se tiene acceso a una estimación
de los parámetros modelados (bω , �ω , �ω ) y dada la estimación realizada y la velocidad
angular obtenida de la UMI la velocidad angular corregida ω̂ que es entrada del
algoritmo inercial en la k-ésima iteración es:
ω̂k = (I + Σ(˘�k , �˘k ))−1 ω̃k − b̆k (4.23)
Ya que el vector de estados se lleva a cero en cada corrección, los estados de sesgo,
factor de escala y falta de ortogonalidad deben ser actualizados teniendo en cuenta
que persisten en la medición sin importar la corrección en el paso anterior; esto
implica que la corrección de la ecuación (4.23) se realiza de la siguiente forma:
b̆k = b̆k−1 + b̂k (4.24)

�˘k = �˘k−1 + �ˆk (4.25)
�˘k = �˘k−1 + �ˆk (4.26)
(4.27)
Donde:
• b̆k , �˘k , �˘k : son los parámetros de corrección para la k-ésima iteración.
• b̂k , �ˆk , �ˆk : son las estimaciones del filtro de Kalman para la k-ésima iteración.
76
Este proceso de calibración permite eliminar parte de la contaminación que sufre
la velocidad angular, lo que —a priori— permitirı́a mejorar el desempeño del sistema
de navegación.
4.3.2 Corrección de orientación
El modelo planteado estima el error en los parámetros de navegación, en este caso
estima el error de actitud. A partir de dicha estimación es posible corregir la salida del
navegador inercial para cada iteración, de manera que la transformación corregida
está dada por:

Ĉbi (tk ) = C̄bi (tk ) I − S(φ̂k ) (4.28)
Donde:
• Ĉbi (tk ): es la transformación corregida dada la estimación del error φ̂k .
• C̄bi (tk ): es la transformación calculada dada la velocidad angular corregida.
En resumen, existen dos lazos de realimentación que atacan los puntos débiles del
sistema de navegación inercial aprovechando la información disponible de un sensor
externo. El algoritmo de navegación integrada de actitud se muestra en el algoritmo
4.1.
77
Algoritmo 4.1 FKE: estimación con estado aumentado
1: INICIALIZACIÓN
2: P = P0
3: Q = Q0
4: q̄bi (0) = q˘bi (0)
� �T
5: x̂0 = φ0 b0 �0 �0
6: for tk = t0 ,t0 ≤ tk ≤ tf ,tk+1 = tk + h do
� �T � �T
7: b̂k �ˆk �ˆk = xˆ2 +k xˆ3 +
k xˆ4 +
k
8: b̆k = b̆k−1 + b̂k
9: �˘k = �˘k−1 + �ˆk
10: �˘k = �˘k−1 + �ˆk
11: ω̂(tk+1 ) = (I + Σ(˘ �k+1 , �˘k+1 ))−1 ω̃(tk+1 ) − b̆k+1
� �T
+
12: x̂k = 03x1 03x1 03x1 06x1
� �
13: q̄bi (tk ) = fq2c Ĉbi (tk )
� �
b (t )
ω̂ib k
14: K1 = h q̄ i (t ) ◦
2 b k 0
� �
b h
15: K2 = h [q̄ i (t ) + K1 ] ◦ ω̂ib (tk + 2 )
2 b k 2 0
� �
b h
16: K3 = h [q̄ i (t ) + K2 ] ◦ ω̂ib (tk + 2 )
2 b k 2 0
� �
b
17: K4 = h [q̄ i (t ) + K ] ◦ ω̂ib (tk + h)
2 b k 3
0
18: q̄bi (tk+1 ) = q̄bi (tk ) + 61 (K1 + 2K2 + 2K3 + K4 )
qbi (tk+1 )
19: q̄bi (tk+1 ) = ||qbi (tk+1 )||
� �
20: C̄bi (tk+1 ) = fq2c q̄bi (tk+1 )
⎡ b b ) b )
⎤
−S(ω̂ib ) I Υ(ω̂ib Γ(ω̂ib
⎢ 0 03x3 03x3 03x6 ⎥
⎢ 3x3 ⎥
21: A=⎢ ⎥
⎣ 03x3 03x3 03x3 03x6 ⎦
06x3 06x3 06x3 06x6
22: Φk+1
k = eAh
23: x̂−
k+1 = Φk+1
k x̂+
k
T
24: P−k+1 = Φk
k+1 + k+1
P k Φk + HQHT h
25: if Sensor externo diponible then
q̆bi (tk+1 )
26: q̆bi (tk+1 ) = ||q̆bi (tk+1 )||
� �
27: C̆bi (tk+1 ) = fc2q C̄bi (tk+1 )
T
28: ϕk+1 = S−1 [C̆bi (tk+1 ) C̄bi (tk+1 ) − I]
29: Kk+1 = P− T
k+1 H [HPk H + R]
+ −1
30: ιk+1 = Kk+1 ϕk+1

−
31: x̂+
k+1 = x̂k+1 + ιk+1
32: P+k+1 = (I − Kk+1 H)P− k+1
33: else
+ −
34: x̂k+1 = x̂k+1
−
35: P+k+1 = Pk+1
36: end if
37: φ̂k+1 = xˆ2 +k+1 � �
38: Ĉbi (tk+1 ) = C̄bi (tk+1 ) I − S(φ̂k )
39: end for
78
4.4 Validación
A partir del algoritmo 4.1 y usando la trayectoria de referencia para el vehı́culo

VS30 mostrada en la sección 2 se implementó el sistema de navegación integrada
descrito a lo largo de esta sección. Los parámetros tenidos en cuenta para realizar
esta simulación se listan en la tabla 4.1.
◦
Varianza del sensor externo [ ] ±2
Condiciones iniciales [◦ ] ±3
Sesgos en ω [◦ /s] ±0,5
Falta de ortogonalidad [ %] 0.5
Varianza de ω [◦ ] ±3
Frecuencia de sensor externo [Hz] 2.5
Frecuencia de sensor interno ω [Hz] 50
En la figura 4.3 se observa el error de estimación y sus respectivas cotas de 2σ. Si

bien existen intervalos en los cuales el error supera sus cotas de error, la magnitud
máxima de la estimación en actitud no supera los 0, 2◦ . Este resultado es muy bueno
considerando la incertidumbre que se tiene en la velocidad angular y en la medida
del robot.
Por otro lado, se observa en la figura 4.4 que la innovación no es un proceso

blanco como se espera, razón por la cual su autocorrelación difiere de función delta
de Dirac. Esto implica que el algoritmo no está funcionando del todo bien, lo cual
puede deberse a que para ciertos intervalos de la trayectoria algunos parámetros son
no observables (ver capı́tulo 5); al corregir con respecto a estimaciones erróneas se
incorpora una fuente de error adicional al sistema. Sin embargo, comparando con los
resultados de la navegación inercial y de la estimación de actitud el error es mucho
menor, se encuentra acotado y presenta una rápida convergencia.
79
Figura 4.3. Error de estimación y cotas de 2σ.
Figura 4.4. Innovación (Izquierda) y su autocorrelación (Derecha).

Este problema de funcionamiento del filtro se ve reflejado en la estimación de

los parámetros de calibración y el sesgo al final de la trayectoria es estimado con un
6 % de error y tarda bastante tiempo en converger. Mientras que el factor de escala
y la falta de ortogonalidad no se acercan a los valores verdaderos de contaminación,
de hecho como puede verse en las figuras 4.6 y 4.7 el factor de escala y la falta de
ortogonalidad estimada varı́an sin converger.
80
Figura 4.5. Sesgo estimado.

Figura 4.6. Factor de escala estimado.

81
Figura 4.7. Falta de ortogonalidad estimada.
Para este test se usan los datos obtenidos en el mismo experimento del capı́tulo 3.
En las figuras 4.8 y 4.9 se observa el comportamiento del error de estimación, el
cual se encuentra muy por debajo de sus cotas estimadas. En este procedimiento la
innovación resulta ser muy parecida a un proceso blanco y a su autocorrelación (ver
figura 5.10).
Comparado con la navegación inercial el desempeño del navegador integrado es

muy superior, el máximo error de estimación para Yaw, Pitch y Roll no supera el 1◦ .
La calibración on-line mejora sustancialmente el desempeño del sistema en términos
de error. En las figuras 4.11, 4.12 y 4.13 se muestra la estimación de los parámetros.
A partir de estos parámetros estimados se realiza la calibración on-line de velocidad
angular y puede concluirse que efectivamente la velocidad angular de los giróscopos
presenta un sesgo en los ejes y y z. El sesgo más grande se presenta en la dirección
z donde alcanza a ser de 0,5◦ .
82
Figura 4.8. Error de estimación.

Figura 4.9. Error de estimación y cotas de 2σ.

83
Figura 4.10. Autocorrelación de la innovación.
Figura 4.11. Sesgo estimado.

84
Figura 4.12. Factores de escala estimados.

Figura 4.13. Falta de ortogonalidad estimada.

Un resumen de los resultados más importantes en la validación de la navegación

integrada se presenta en la tabla 4.2.
85
Error VS30 SAC-D
◦
Media [ ]
ψ -0,0186 0,0083
θ 0,0218 -0,0488
φ -0,0167 0,0304
Desvio [◦ ]
ψ 0,2208 0,4938
θ 0,2168 0,4663
φ 0,2250 0,4504
Máximo [◦ ]
ψ 0,247 0,7755
θ 0,2028 1,1042
φ 0,2125 0,6294
Cuadro 4.2: Resultados de error en estimación.
Como puede observarse en la tabla 4.2, la media del error es casi cero para
ambas trayectorias. El desvı́o es muy pequeño comparado con el de los sensores y
la evolución del error se encuentra acotada. Estos resultados en comparación con el
esquema de estimación de actitud sin calibración on-line presentados en la tabla 3.3
son mejores, puesto que las cotas de error no superan los 0,5◦ y los valores máximos,
los 1,1◦ . De esta manera, los beneficios de la navegación integrada con estimación
de parámetros de error de los sensores es demostrada.
4.5 Conclusiones
Se construyó el modelo para el filtro de Kalman en el cual se realiza un proceso de
calibración on-line, sumado a la corrección de la actitud que se realizó en el capı́tulo
3. Las simulaciones realizadas sobre las trayectorias del VS30 y del SAC-D muestran
la superioridad del algoritmo frente a la solución de navegación inercial. Las mag-
nitudes de error máximas se reducen en un 70 % para el peor caso en la trayectoria
del SAC-D. Si bien el sistema no es lineal, el comportamiento de la solución para
el modelo elegido muestra que es una excelente aproximación del comportamiento
real del error en el sistema. Se puede concluir que el proceso ruidoso que contamina
al giróscopo efectivamente es cercano a una secuencia de ruido blanco. Otra carac-
terı́stica que se observa es la rápida convergencia del sistema.
86
Esta solución es robusta con respecto a la inestabilidad de los parámetros de

error en la velocidad angular, lo cual es ideal para sistemas que deben funcionar por
largos periodos con un grado de confiabilidad bueno aprovechando los beneficios de
la calibración on-line. La estrategia de navegación integrada a partir de la estima-
ción realizada por el filtro de Kalman extendido garantiza un mejor desempeño de la
navegación, aunque no se puede demostrar que el desempeño obtenido es el óptimo,
los resultados conseguidos son mejores que los que se tendrı́an con cada sensor por
separado y a un tiempo de muestreo adecuado para realizar maniobras de control.
Los resultados evaluados sobre la trayectoria del SAC-D son de gran importancia,
puesto que estos datos son adquiridos a partir de la plataforma robótica y evaluados
off-line. Si bien el sistema no esta funcionando sobre el vehı́culo, esta simulación
muestra que la implementación del algoritmo sobre la plataforma corriendo on-line
puede llegar a tener un desempeño adecuado.
87
Capı́tulo 5
Análisis de observabilidad
At any rate, I am convinced

that
He does not play dice.
Albert Einstein
5.1 Observabilidad
La observabilidad es un criterio que se aplica sobre los modelos en espacio de estados
y pretende determinar si la información que entregan las mediciones es suficiente
para reconstruir el vector de estados en un tiempo determinado. Este es un criterio
de análisis de suma importancia en problemas de estimación, ya que si en algún
instante ciertas variables de estado no son observables, quiere decir que estas no
tendrán una correcta estimación. En este trabajo se analiza la observabilidad del
sistema mediante dos métodos:
1. Análisis por matriz de observabilidad [62].
2. Medición del grado de observabilidad según la evolución de la covarianza de

cada variable de estado en el intervalo por analizar [63, 64].
5.1.1 Análisis por matriz de observabilidad

Se define la ecuación de medición (5.1) para el instante tk+1 a partir del vector de
estados x[k], de la matriz de transición de estados Φk+1
k y la matriz de medición H
que se asume invariante en el tiempo.
y[k + 1] = Hk x[k + 1] = Hk Φk+1

k x[k] (5.1)
89
Se construye la matriz de observabilidad M :
⎡ ⎤ ⎡ ⎤
y0 H
⎢ y1 ⎥ ⎢ HΦ0 1 ⎥
⎢ ⎥ ⎢ ⎥
⎢ y2 ⎥ ⎢ HΦ 2 1
Φ ⎥
⎢
ȳ[0, k] = ⎢ ⎥ ⎢ 1 0 ⎥ x[0] = M x[0]
... ⎥=⎢ ... ⎥ (5.2)
⎢ ⎥ ⎢ ⎥
⎣yk−1 ⎦ ⎣ HΦk−1 2 1
k−2 ...Φ1 Φ0
⎦
k k−1 2 1
yk HΦk−1 Φk−2 ...Φ1 Φ0
Se dice que el sistema es observable si la matriz M es de rango completo. Si el

sistema es observable en el intervalo de muestras de 0 a k, las siguientes ecuaciones
son consistentes:
(M T )ȳ[0, k] = (M T M )x(0) (5.3)
x(0) = (M T M )−1 (M T ) ∗ ȳ[0, k] (5.4)
G = M−1 M (5.5)
La matriz (M T M ) de dimensiones n × n es conocida como el Gramiano (5.5) y al

igual que M , el Gramiano debe ser de rango completo (es decir, debe ser una matriz
no singular); si esto se cumple entonces el sistema es completamente observable. En
sistemas variantes e invariantes en el tiempo no solo basta con saber si es de rango
completo, sino además qué tan cerca está de ser singular i.e. términos de orden
menor o igual 10−10 que al ser tomados como 0 hacen que la matriz sea singular. Se
debe escoger un criterio para determinar la influencia de los parámetros estimados
sobre las mediciones y para esto es pertinente analizar la energı́a de la señal de salida
ȳ[0, k], esta se puede calcular mediante la norma euclı́dea del vector salida, de la
siguiente forma:
||ȳ[0, k]||2 = (ȳ[0, k])T (ȳ[0, k]) = x(t0 )T (M T M )x(t0 ) (5.6)
Para analizar la observabilidad de cada variable de estado se hallan los auto-

valores del Gramiano, los cuales dicen qué tan observable es cada variable sobre
el intervalo para el cual se analiza el sistema. Para encontrar los autovalores del
Gramiano se puede hacer una descomposición espectral1 a partir de la expresión
G = U ∗ Σ ∗ UT
Mientras la matriz Σ es una matriz diagonal con los autovalores correspondientes,

la matriz ortogonal U está compuesta por los n autovectores columna ui , los cuales
1
Esto es posible ya que G es una matriz real y simétrica.
90
representan las bases ortonormales que indican las direcciones en las cuales xi es
más observable. Cuando x(t0 ) = ui se tiene que ||ȳ[0, k]||2 representa al autovalor
de observabilidad σi .
||ȳ[0, k]||2 = (ȳ[0, k])T (ȳ[0, k]) = uTi ∗ (M T M )ui = σi (5.7)
En la ecuación (5.7) se puede ver que los autovalores corresponden a medidas

de energı́a del vector de mediciones y que el autovector correspondiente indica la
dirección o valores del vector de estados que generan dicha medida. Los mayores
autovalores del Gramiano indican las direcciones más observables, ya que aportan
una mayor influencia sobre las mediciones realizadas.
5.1.2 Grado de observabilidad

Ham y Brown [62] propusieron la teorı́a de análisis del grado de observabilidad.
Partiendo del error de estimación del filtro de Kalman que se define como x̃ =
x − x̂, donde x es el vector de estados y x̃ es el error de estimación. Definiendo una
combinación lineal de elementos para X̃ : w = v1 x̃1 + ... + un x̃n , la varianza de w se
calcula como: n n

σw = vi vj pij = v T P v (5.8)
i j
Donde P es la matriz de covarianza del error del filtro de Kalman. El mayor valor
para σw corresponde a la menor observabilidad obtenida de la combinación lineal w.
El objetivo del análisis por grado de observabilidad es encontrar los valores máxi-
mos de σw para determinar cuán pobre puede ser el grado de observabilidad para
dicha combinación. Estos valores pueden ser determinados mediante el método de
multiplicadores de Lagrange [63, 64] a partir de la siguiente expresión simplificada:
(P − λI)v = 0 (5.9)
Donde λ corresponde a los autovalores de P y v a los autovectores de P . Consi-

derando que σw = v T P v y que v T v = 1, se tiene:
λi = viT P vi = σwi (5.10)
El mayor autovalor de P corresponde a la varianza de la combinación lineal de es-

tados que tienen una menor observabilidad y recı́procamente la mejor observabilidad
será aquella con el menor autovalor.
En el filtro de Kalman la matriz P refleja el error entre los estados reales y
los estados estimados, lo cual implica que la observabilidad del sistema puede ser
analizada a través de la matriz de covarianza del error P .
91
Para dicho análisis se propone aplicar dos etapas de transformación a la matriz
P, la primera de normalización y la segunda para acotar, ya que los autovalores al
representar distintas variables poseen dimensiones y cotas distintas. Por lo tanto, se
busca la uniformidad en la magnitud de los autovalores, esto permite comparar el
grado de observabilidad entre los distintos estados. La normalización de la matriz
de covarianza del error P se hace utilizando como referencia las covarianzas de error
iniciales en el filtro, y se define de la siguiente forma:
P1 (k) = (P (0)−0,5 ) ∗ P (k) ∗ (P (0)−0,5 ) (5.11)
Esta normalización busca que los valores de P sean adimensionales. Luego de

normalizar los valores, estos pueden ser acotados por un valor conveniente. En esta
matriz la suma de sus autovalores corresponde a la traza de dicha matriz. Entonces,
n
esta puede ser multiplicada por un factor traza(P1 (i))
, donde n es el rango de P. Esto
implicará que los autovalores de P estén entre 0 y n.
n
P2 (k) = ∗ P 1(k) (5.12)
traza(P 1(k))
Finalmente, se puede decir que los autovalores de la matriz P2 corresponden al
grado de observabilidad de la combinación lineal de los estados, la cual es descrita
por los autovectores encontrados. El menor autovalor indica la mejor observabilidad
para la combinación lineal de algunos estados; en dicha combinación los estados con
mayor ponderación son los más observables y los que más aportan a la disminución
de la varianza del error.
5.1.3 Observabilidad de la trayectoria: SAC-D

Según la matriz de observabilidad construida para esta trayectoria, se puede encon-
trar el Gramiano G para el sistema definido por (3.1) y (3.2). Si G es una matriz de
rango completo entonces el sistema es completamente observable, es decir, se parte
del supuesto de que el algoritmo de filtro de Kalman puede ser capaz de estimar cada
uno de los estados ocultos o no medibles de manera directa, ya que estos estados
son observables para el sistema.
El análisis por matriz de observabilidad para el intervalo T no es detallado en
el sentido de la evolución temporal del sistema, ya que pueden existir instantes
cuando algunos parámetros del sistema no sean observables y otros sı́. La matriz
de observabilidad se construye mediante la acumulación de la observabilidad de
los parámetros en el intervalo dado, es decir, que solo basta que el parámetro sea
observable en uno o algunos instantes del intervalo, para que lo conseguido se vea
92
reflejado en la matriz G y se obtenga como resultado que el parámetro es observable.

De acuerdo con esto el aporte en la estimación de los parámetros no observables
para los instantes medidos en vez de hacer una corrección del error, estos incorporan
uno nuevo. Por tanto, el análisis mediante matriz de observabilidad, cuyo resultado
define si el sistema es o no observable, no garantiza una noción de la estimabilidad
del sistema durante cada instante del intervalo definido.
Con el fin de darle robustez e incrementar el detalle de este análisis de observa-
bilidad, se lleva a cabo el análisis por grado de observabilidad, el cual define que en
cada iteración el filtro de Kalman se obtiene una estimación de la matriz de cova-
rianza del error (P(tk )). Esta matriz entrega información relevante sobre el error y
por ende sobre el desempeño de la estimación que se obtiene con el filtro de Kalman.
A continuación se presentan los resultados obtenidos al aplicar ambos métodos

de análisis. Inicialmente se construyó el Gramiano para el intervalo completo en
el que se da la trayectoria T = [0s; 87, 54s]. Esto es necesario aclararlo ya que
para problemas en los que la trayectoria se pueda separar en intervalos donde el
vehı́culo se enfrente a distintas condiciones (i.e propulsión, caı́da libre), se pueden
construir varios Gramianos, uno por cada intervalo y ası́ analizar con más detalle el
comportamiento del sistema. Al ser una trayectoria pura de orientación y bajo las
mismas condiciones durante todo el intervalo, esto no es necesario. Los autovalores
encontrados se describen en la tabla 5.1.
σi σi σi
σ1 2126563,63 σ6 461,079 σ11 40,0970
σ2 1416025,20 σ7 195,208 σ12 37,3006
σ3 1279263,71 σ8 186,559 σ13 0,09115
σ4 753,05 σ9 168,691 σ14 0,08979
σ5 704,70 σ10 46,2263 σ15 0,08522
Cuadro 5.1: Autovalores del Gramiano.
En la figura 5.1 se presenta la asignación de cada estado del modelo a su auto-

valor respectivo; como resultado se obtiene que los estados menos observables hacen
referencia a los parámetros de falta de ortogonalidad (particularmente a �yz ,�zx y
�zy ), cada magnitud obtenida tiene una relación con los resultados de las estima-
ciones de cada estado, presentadas en las figuras 4.11, 4.12 y 4.13. En esta última
se puede ver que el orden de magnitud de los parámetros estimados es muy bajo
comparado con el resto, lo cual justifica y se relaciona directamente con la baja
93
observabilidad de estos estados. Finalmente se puede inferir que el principal aporte
en la estimabilidad del sistema está dado por los estados de error de actitud, lo cual
implica que la actitud del simulador es muy observable en esta trayectoria, seguido
de los sesgos en cada eje de los giróscopos, los cuales son estimados correctamente.
Mientras en lo que concierne a los factores de escala y de falta de ortogonalidad su
baja magnitud estimada refleja poca observabilidad en el sistema planteado sobre
la trayectoria recorrida.
Figura 5.1. Vector de estados y autovalores del Gramiano.

Por último, se analiza el grado de observabilidad del sistema, con el fin de ver la

incidencia de cada estado estimado a lo largo de la trayectoria. Como se describió,
la matriz de covarianza de error entrega información valiosa de forma periódica. En
la figura 5.2 se presenta la evolución temporal de los autovalores de la matriz de
covarianza de error normalizada y acotada (P2 ).
Entonces los menores autovalores indican que sus autovectores representan la
dirección del vector de estados con mayor grado de observabilidad. De la figura 5.2
se obtiene que los primeros 6 autovalores los cuales hacen referencia a la covarianza
del error en actitud y de los sesgos estimados, poseen un comportamiento convergente
sobre el intervalo de esta trayectoria; a diferencia de los otros estados que poseen
una evolución divergente particularmente para los mismos estados que presentaron
la más baja observabilidad en el análisis por matriz de observabilidad, lo que permite
corroborar que para esta trayectoria la actitud y los sesgos de los giróscopos son los
estados de mayor observabilidad y por tanto de mayor estimabilidad con menor error.
94
Figura 5.2. Evolución de los autovalores de la matriz de covarianza de error.

La figura 5.3 muestra el promedio de los autovectores a lo largo de la trayectoria;

se exponen los autovectores correspondientes a los autovalores de mayor grado de
observabilidad, lo que permite determinar las direcciones que más influyen en el
cálculo del grado de observabilidad.
Figura 5.3. Autovectores promedio de la matriz de covarianza de error.

En los autovectores que referencian los estados de los 3 ejes en los que se estima
la orientación del simulador, la figura 5.3 confirma que son los 3 primeros estados los
que mayor aporte hacen en la combinación lineal que construye a dichos autovalores.
95
De igual manera para los sesgos, se observa el alto grado de observabilidad de los
estados de 4 al 6 que contribuyen en la estimabilidad de estos mismos.
5.2 Conclusiones
Al comparar ambos métodos de análisis, por matriz de observabilidad y grado de
observabilidad, se concluyó que el segundo entrega una noción detallada de los re-
sultados de estimación obtenidos por el algoritmo de navegación integrada. Mientras
que en el primero se podrı́a concluir que los únicos estados no observables son los
estados x13,14,15 correspondientes a 3 de los factores de falta de ortogonalidad. En
el segundo método, al estar directamente relacionado con el comportamiento del
estimador, se muestra que los estados que representan los 3 factores de escala y los
6 de falta de ortogonalidad tienen un bajo grado de observabilidad, pero posible-
mente en algún momento fueron instantáneamente observables y por esto la matriz
de observabilidad entrega una ponderación intermedia que presume una noción de
ser estados observables. Finalmente, una caracterı́stica interesante del análisis por
grado de observabilidad (particularmente los autovalores) es que se puede analizar
en un instante o iteración especı́fico de la trayectoria y luego ser comparada con los
autovalores de la siguiente iteración y ası́ poder observar cuál fue el mejor y peor
estado estimado entre la iteración tk y la tk+1 .
Un enfoque interesante que se puede aplicar consiste en usar el grado de observabili-
dad medido en cada iteración del algoritmo para decidir qué estados son observables
y si pueden ser realimentados, y construir ası́ un enfoque adaptivo según el grado
de observabilidad. Si el grado de observabilidad para un estado es mayor a cierto
umbral definido (lo cual implica una mala observabilidad), se puede detener la re-
alimentación de dicho estado. Este tipo de realimentación controlada puede llegar a
mejorar el rendimiento del sistema.
96
Capı́tulo 6
Implementación de la plataforma
de simulación con Hardware en
el Lazo
Too much humility is a

defect
C.R
6.1 Introducción
La simulación con Hardware en el Lazo (HIL) es un concepto muy efectivo para
verificar y probar sistemas embebidos complejos antes de ser implementados en el
proceso real [65, 66]. El esquema de simulación tı́pico utiliza un conjunto de software
junto con un modelo de realidad, lo cual permite llevar al sistema simulado a zonas
de operación que son de interés. Con el crecimiento en la complejidad de los sistemas,
el proceso de simulación resulta de utilidad y permite la reducción de costos y riesgos
en la etapa de desarrollo de un proyecto [67, 68].
97
Figura 6.1. modelo simulación por software.
Si bien esta simulación permite extraer información sobre el desempeño del sis-
tema, está sujeta a la precisión en el modelado de este. El enfoque HIL reemplaza
bloques del lazo de software por hardware, lo que reduce la complejidad del modelo.
Esto también genera un ambiente de simulación más parecido al real lo que permite:
• Extraer mas información del proceso.
• Detectar fallas del sistema.
• Verificar la confiabilidad de los componentes de hardware.
• Caracterizar eventos y restricciones no modeladas.
Una caracterı́stica de la simulación HIL es que se presenta una interacción

dinámica muy compleja entre bloques de software y de hardware, lo que requiere
prestar mucha atención a la definición de las distintas interfaces para la comunica-
ción entre bloques. La correcta implementación y selección de las interfaces permiten
garantizar la causalidad del proceso simulado. La arquitectura tı́pica de la simulación
HIL puede verse en la figura 6.2.
El modelo HIL ha sido ampliamente usado en la industria aeroespacial, lo que

ha permitido realizar simulaciones de sistemas de servicio en órbita (OOS, por sus
siglas en inglés) [66, 67, 68], de misiones no tripuladas [69], de subsistemas satelitales
[70], entre otras.
98
Figura 6.2. Modelo HIL.

6.1.1 Arquitectura
Una plataforma HIL generalmente está compuesta por elementos de software y hard-
ware que intercambian señales. Con el fin de que el sistema evolucione en el tiempo
de manera coherente (causalidad) se requiere una computadora que administre el
tiempo. Esta computadora central intercambia información con los bloques de la pla-
taforma y debe poseer una interfaz hombre-máquina (HMI, por sus siglas en inglés)
que le permita enviar información al usuario. En este trabajo la computadora prin-
cipal es una BeagleBoard xM [71, 72], la cual maneja casi la totalidad del software
dentro de la plataforma.
Esta computadora es la encargada de ejecutar el algoritmo de navegación integrada
de actitud y para ello requiere las mediciones de la velocidad angular. La tarjeta de
adquisición de datos LPCXpresso se encarga de servir como interfaz entre la unidad
de medidas inerciales y la computadora de navegación. Por otro lado, la compu-
tadora de navegación interactúa con el S4 controlador del robot IRB140 [41, 73]
recibiendo información de actitud y tiempo. Esta información es usada como fuente
extra en el algoritmo de navegación. En el controlador S4 del robot se almacena y
programa una trayectoria, que es producto de los datos obtenidos por telemetrı́a del
satélite SAC-D Aquarius que se encuentra en órbita actualmente. Esta trayectoria
de actitud se ve reflejada en el movimiento de la sexta articulación del robot sobre
la cual se encuentra conectada fı́sicamente la UMI.
99
La interacción entre los componentes de Hardware del sistema puede verse con más
claridad en la figura 6.3.
Figura 6.3. Arquitectura del sistema.

6.2 Hardware
A continuación se describen los elementos de hardware utilizados en el presente
trabajo para la construcción de la plataforma. En la figura 6.4 se presenta una
captura de la plataforma desarrollada.
100
Figura 6.4. Plataforma de simulación.

6.2.1 Unidad de medidas inerciales: ADIS16364
Una UMI consiste en un empaquetado de sensores combinados en un dispositivo

único con un canal para la salida de datos; la ADIS16364 es una unidad de medidas
inerciales de Analog Devices con seis grados de libertad, está basada en sensores
MEMS1 , posee un acelerómetro de tres ejes sobre los cuales pueden medir fuerzas
especı́ficas de hasta ±5g y un giróscopo digital de tres ejes con caracterı́sticas de
rango que pueden ser configuradas para ±75◦/s, ±150◦/s y ±300◦/s [39].
Las especificaciones para los giróscopos y sus errores de sesgo y ruido pueden
observarse en la tabla 6.1. Esta UMI posee un sistema de sensado autónomo que,
tan pronto como se conecte a una fuente de alimentación, empieza a almacenar
medidas en sus registros a una frecuencia configurable mediante el registro SMPL -
PRD [39]. Adicionalmente posee un filtro digital interno, el cual permite reducir el
ancho de banda del ruido (NBW)2 . Según el valor de la frecuencia de muestreo este
filtro permite acotar el valor RMS del ruido de los giróscopos. En la tabla 6.1 se
resumen las principales caracterı́sticas de la UMI utilizada.
1
Micro ElectroMechanical Sensors.
2
Noise Band Width. Por defecto está configurado para 330Hz
101
Parámetro Condiciones de prueba valor tı́pico Unidad
◦
Rango dinámico ±300 /s
◦
Sensibilidad 0,05 /s]/LSB 1
◦
Error de Bias ±σ ±3 /s
Inicial
◦
√
Angle Random 1σ 2 / hr
Walk [5, 50]
◦
Ruido de salida Con un rango dinámico de 0.8 /s rms
±300◦ /s
◦
Sensibilidad al V cc = 4,75V a 5,25V ±0,3 /s
voltaje
Corriente de En modo normal.2 49 mA
consumo
Cuadro 6.1: Especificaciones de los giróscopos.
6.2.2 Tarjeta de adquisición de datos: LPCXpresso 1769
La LPCXpresso3 1769 es una plataforma de experimentación y desarrollo, es la en-

cargada de crear una interfaz entre la unidad de medidas inerciales y la computadora
de navegación.
Figura 6.5. LPCXpresso 1769.

Fuente: [74].
3
http://lpcxpresso.code-red-tech.com/LPCXpresso/
102
Parámetro Valor Unidad

min typ max
Consumo @120 MHz 50 mA
Tensión de alimentación 2.4 3.3 3.6 V
◦
Temperatura de operación -40 85 C
Memoria flash 512 kB
Memoria RAM 32 kB
Puertos ADC 8
Resolución ADC 12 bits
Muestreo ADC 200 kHz
Velocidad UART1 115.2 6250 kbps
Bus SPI
Bus SSP
Bus I 2 C
Bus I 2 S
Tamaño 140x35 mm
Peso 50 g
RTC clock interrupt drift 1 s/dı́a
Cuadro 6.2: Especificaciones del LPCXpresso 1769.
Esta tarjeta posee un microcontrolador de 32 bits LPC1769 con núcleo Cortex-

M3. En la tabla 6.2 se presentan algunas de las principales especificaciones del
dispositivo. El uso de esta tarjeta pretende que la computadora de navegación se
encuentre dedicada a la ejecución de algoritmos de navegación. Otras de las razones
por las cuales se elige esta tarjeta son:
• Costo.
• Bajo consumo.
• Potencia y velocidad de cálculo.
6.2.3 Computadora de navegación: BeagleBoard xM

BeagleBoard [71] es una tarjeta basada en la familia de procesadores DM3730 de Te-
xas Instruments que incluyen un procesador de arquitectura ARM A8 con un clock
de hasta 1GHz y un DSP TPS65950, que se encarga de tareas de administración
de energı́a y codificación, y decodificación de señales de audio. La tarjeta es capaz
103
de desarrollar funcionalidades de laptop gracias a su gran capacidad de desempeño,
las interfaces y periféricos disponibles. La capacidad de cómputo de esta platafor-
ma está diseñada para proveer procesamiento de video, imagen y gráficos, soporta
sistemas operativos como LINUX, WINDOWS CE, ANDROID y QNX. Esto la con-
vierte en un sistema embebido muy potente, de bajo consumo y costo, puesto que es
diseñada como una plataforma de experimentación y desarrollo [72].
Figura 6.6. BeagleBoard xM.

Fuente: [75].
Parámetro Valor Unidad

min typ max
Consumo 350 mA
Tensión de alimentación 4.8 5.0 5.2 V
◦
Temperatura de operación -40 85 C
Memoria NAND 256 MB
Memoria RAM 256 KB
Memoria SD 8 GB
Canales UART 2
Velocidad UART1 115.2 250 kbps
Bus USB
Bus I 2 C
Tamaño 79x77 mm
Peso 50 g
Cuadro 6.3: Especificaciones de la BeagleBoard.
104
6.2.4 Robot IRB140-Controlador S4c

El laboratorio de robótica de la FiUBA cuenta con sistema robótico industrial com-
puesto por un manipulador (6DOF) IRB 140 de ABB y su controlador el S4c. Este
es un controlador programable que posee la electrónica necesaria para interactuar
con el mecanismo, además de interfaces de comunicación Ethernet, RS485; entradas
y salidas digitales o analógicas; un procesador central y un conjunto de sensores de
posición angular. El mecanismo puede moverse con una precisión y repetitividad
excelentes a velocidades (sobre el TCP:Tool Center Point) de hasta 2,5 m/s.
Figura 6.7. Robot-controlador.

6.3 Interfaces
Uno de los problemas más complejos en la construcción de plataformas HIL se pre-
senta en la interacción entre bloques de software y hardware [65]. Dada la arquitec-
tura propuesta en 6.3, a continuación se muestran las interfaces de comunicación
requeridas en el desarrollo del sistema.
105
6.3.1 TAD-UMI
La adquisición de datos se realiza mediante la TAD a través de una interfaz SPI
(Serial Peripheral Interface). La UMI almacena en un conjunto de registros de 16
bit las magnitudes sensadas; de manera independiente la TAD está programada
para trabajar como maestro y escribir o leer los registros del sensor. El diagrama de
conexión tı́pico se muestra en la figura 6.8. La lectura de una medición requiere 2
secuencias de 16 bits; en la primera se envı́a el comando de lectura que consiste en
una trama donde se define en un bit si se lee o escribe (bit R/W=0), concatenado
con la dirección del registro objetivo por la lı́nea DIN. En la segunda secuencia de
16 bits se transmite el contenido del registro en la lı́nea DOUT. Esta interfaz puede
operar en modo full-duplex.
Figura 6.8. Interfaz SPI.

6.3.2 TAD-CN
Una vez adquiridas las mediciones por parte de la TAD, estas deben ser enviadas a
la CN para ejecutar el algoritmo de navegación integrada. Los datos son enviados
mediante una interfaz de trasmisión serial RS-232 a 115200 baud/s. Se envı́a una
trama de 8 bytes, como se muestra en la figura 6.9. Los bytes trasmitidos equivalen a
la parte alta y baja de las velocidades angulares obtenidas, más los bytes equivalentes
a un registro de identificación único propio de la UMI, esto con el fin de tener un
106
dato con el cual se pueda garantizar la fiabilidad de las mediciones trasmitidas. Los
datos son concatenados por la computadora de navegación, que posteriormente se
encarga de acondicionar los bytes concatenados. El proceso de acondicionamiento se
lleva a cabo debido a que los datos la UMI los entrega en notación complemento a
2 y con una resolución de 0,05◦ /s = 1bit.
Figura 6.9. Interfaz TAD-CN.

Figura 6.10. Esquemático: interfaz TAD-CN.

Finalmente la computadora de navegación recibe los datos cada 30 ms, estos son
concatenados y escalizados para ser procesados por parte del algoritmo de navega-
107
ción. En las figuras 6.10 y 6.11 se presenta el esquema y el PCB desarrollado como
interfaz entre la TAD y la CN. El sistema de alimentación de energı́a consta de un
regulador de voltaje LDO (LM 2575) en configuración BUCK, el cual garantiza una
regulación de menos del 1 % a un valor de 5V , por lo que la TAD y la UMI pueden
ser energizadas con una fuente externa de 7,5V hasta 48V , con consumo tı́pico de
0.6 vatios entre la TAD y la UMI. El acondicionamiento de interfaz UART a interfaz
RS-232 se hace por medio del integrado MAX3232, debido a que los niveles lógicos
que maneja la TAD son de 3,3V .
Figura 6.11. PCB: interfaz TAD-CN.

6.3.3 CN-controlador de robot

El controlador S4C plus de ABB permite la operabilidad remota a través de un
conjunto de protocolos especı́ficos. En la figura 6.12 se muestra la pila de protocolos
que utiliza un proceso de operabilidad remota.
A nivel de enlace se utiliza Ethernet 802.3 a través de la placa de red del S4c,
en la capa de transporte se usa un protocolo TCP/IP [76]; para la llamada de
procedimientos de manera remota se dispone de ONC RPC [76, 77]; a nivel de
aplicación ABB posee un protocolo privativo que permite al usuario monitorear y
ejercer control sobre el robot.
Algunos de los servicios mas importantes que provee RAP son:
• Ejecutar programas.
• Leer y escribir variables.
• Manipular archivos del controlador.
108
Figura 6.12. Pila de protocolos para comunicación con robots ABB.

La implementación de esta interfaz requiere el desarrollo de la biblioteca RAP.

Las especificaciones de dicha biblioteca se encuentran disponibles en el manual de
referencia RAP [78, 79, 80].
En el laboratorio de robótica de la FiUBA se ha desarrollado un conjunto de librerı́as
que permite la ejecución de los servicios RAP desde plataformas Windows [73]. En
este trabajo se hace uso de dichas librerı́as, las cuales fueron compiladas para correr
en la computadora de navegación.
6.3.4 CN-hombre
En la computadora de navegación se instaló un sistema operativo Linux, de distri-
bución Debian para sistemas embebidos de arquitectura ARM. El sistema completo
entrega al usuario un conjunto de resultados relacionados con el sistema de navega-
ción. Todos estos resultados son almacenados en el sistema de archivos.
• Trayectoria de velocidad angular.
• Trayectoria del sensor externo (robot).
• Estimación de actitud.
• Propagación de la covarianza del error.
109
6.4 Evolución temporal del sistema
La evolución temporal de este sistema está directamente ligada a la adquisición
de datos, de tal manera que se tiene acceso a dos fuentes de sincronización. La
TAD alerta a la CN cada 30 ms la existencia de una nueva medición de velocidad
angular. Para que el algoritmo en el tiempo k entregue el cuaternión correspondiente
a este tiempo k debe conocer las velocidades angulares ωk−1 y ωk−2 . En este sentido,
el proceso principal siempre está a la espera de una nueva velocidad angular y
dependiendo de qué orden de llegada tenga, se encarga de almacenarla o de calcular
el cuaternión. Cada vez que se obtiene un cuaternión se genera una alarma del
proceso del Robot. Sin importar esto, en cada iteración existe un condicional sobre
el estado de esta alarma; en caso de que exista dicha alarma se procede a calcular
la estimación a partir del filtro de Kalman, como puede verse en la figura 6.13.
Figura 6.13. Evolución temporal del algoritmo.

Concurrentemente el controlador del robot se encarga de dirigir su trayectoria,

la cual tiene impacto sobre las mediciones entregadas por la UMI.
110
6.5 Software
6.5.1 Metodologı́a
En el proceso de desarrollo de este trabajo se siguió una metodologı́a iterativa en
la cual el desarrollo de software se lleva a cabo de manera incremental, en cada
iteración del proceso se agrega algo de complejidad al sistema, lo cual facilitó el tra-
bajo cooperativo en la etapa de desarrollo [81], se ha mostrado que esta metodologı́a
de trabajo permite ahorrar tiempo comparada con las metodologı́as lineales tı́picas.
Debido a que el proceso de depuración se realiza iterativamente con la implementa-
ción incremental se evita la acumulación de problemas lo cual flexibiliza el diseño de
la solución al problema [82]. En la figura 6.14 se puede ver la descripción de la evoluci
ón de la metodologı́a aplicada; por un lado, se centra en el planteamiento del
problema y análisis de requerimientos y, por el otro, se va expandiendo en tareas
que contienen las iteraciones pasadas. Por lo tanto, la iteración final abarcó el cum-
plimiento de todos los requerimientos propuestos para cada etapa del proyecto.
Figura 6.14. Diagrama de fases del proyecto.

111
Las iteraciones que se construyeron son las siguientes:
• Iteración 1: análisis del problema y definición de requerimientos.
• Iteración 2: desarrollo del navegador inercial.
• Iteración 3: desarrollo del navegador integrado.
• Iteración 4: depuración e implementación final del simulador HIL.
Al final de cada iteración se hizo la integración de un nuevo conjunto de interfa-

ces con nuevos actores externos y la revisión de las anteriores. En cada proceso de
iteración se llevó a cabo un análisis de los requerimientos del sistema, el diseño de la
arquitectura, la implementación previa y un procedimiento de verificación funcional.
6.5.2 Descripción modular

La descripción modular se caracteriza por permitir el desarrollo de software en con-
junto. Este enfoque se basa en la idea de dividir un problema grande en muchos
problemas (módulos), los cuales interactúan según los requerimientos definidos. Usar
una descripción modular permite también flexibilizar el proceso de diseño y debug.
Además de ser una estrategia que permite la reutilización del código.
En el desarrollo de este trabajo se usa una descripción modular con el fin de que

el software generado sea legible y reusable. Dada la interacción entre los distintos
elementos del sistema en la figura 6.15 se muestra el diagrama modular.
6.5.2.1 Módulos adquisición de datos UMI

La adquisición de datos a través de la TAD se realiza utilizando una interfaz de
comunicación SPI. Para llevar a cabo dicha tarea se crea la librerı́a ADIS16364.c
en la cual se definieron las funciones necesarias para el funcionamiento de los módulos
que interactúan dentro de la TAD. La descripción de estos módulos se define a
continuación:
• Comunicación A-SPI: este módulo describe la comunicación entre la TAD y

la UMI. Dentro de la librerı́a desarrollada se implementaron funciones como
SPI Init() donde se definen los parámetros necesarios para la comunica-
ción con la UMI, como los puertos, la frecuencia del reloj SPI y si la TAD es-
tarı́a en modo maestro o esclavo. Se escriben las funciones IMU Write(Uint8
112
Address Register,Uint8 data) y uint16 IMU Read(uint8 Addres

Register) por medio de las cuales se tiene acceso a los datos en los registros
de la UMI. La descripción detallada de las direcciones de cada registro y el
dato que este almacena se encuentran en el manual de la IMU en [39].
• Comunicación B-UART: este módulo permite la comunicación entre la TAD y

la CN, en la librerı́a desarrollada se crean funciones para configurar el puerto
(UART3 Init()) y se ejecuta la escritura sobre el puerto con una función ya
definida por las librerı́as propias de la TAD (UART SendByte(LPC UART3,
uint8 byte)).
• Tiempo: para garantizar la trasmisión periódica y sincronizada de los datos se

utilizó el submódulo RIT1 de la TAD, el cual permite generar interrupciones
sincronizadas cada cierto tiempo, que para la aplicación en particular fue de
30 ms. Las funciones descritas para esto son la inicialización del submódu-
lo (RIT Init(LPC RIT)), la configuración del timer del submódulo (RIT -
TimerConfig(LPC RIT,TIME INTERVAL)) y la habilitación de la inte-
rrupción en el vector de interrupciones de la TAD (NVIC EnableIRQ(RIT -
IRQn)).
• Main: este módulo representa el hilo principal del programa, donde sencilla-
mente se llevan tareas de inicialización y configuración de los módulos ante-
riormente descritos. También se lleva a cabo una función de inicialización de
la UMI (adis Init()). Luego de esto el sistema queda a la espera de la acti-
vación de la interrupción temporal para llevar a cabo las tareas de adquisición
y envı́o de datos desde la UMI hacia la CN. En dicha interrupción se solicitan
las mediciones a la UMI por medio de la función de acceso uint16 IMU -
Read(Uint8 Address Register), se toman los 8 bytes solicitados y se
procede a enviarlos por el módulo UART hacia la computadora de navegación.
1
Repetitive Interrupt Timer.
113
Figura 6.15. Descripción modular.
6.5.2.2 Módulos CN
La computadora de navegación se encarga de ejecutar el algoritmo 4.1 y de recibir

la información de los sensores externos (robot) y la UMI. Posteriormente debe pro-
cesarla y enviar resultados de las tareas realizadas. La prioridad del sistema debe ser
ejecutar el algoritmo de estimación a partir de las medidas obtenidas cumpliendo
con los intervalos disponibles para dicha tarea. Estos intervalos dependen del tiem-
po de muestreo establecido para la adquisición de una nueva medida de velocidad
angular. Teniendo en cuenta que el sistema operativo es multitarea, se propone se-
parar la adquisición de información del robot en un proceso diferente al que ejecute
el algoritmo de estimación y corrección.
114
6.5.2.3 Proceso de adquisición datos del robot

La adquisición de datos del robot se realiza a través de un cliente RAP. Este proceso
se encarga de solicitar al controlador del robot la medida de orientación según un
tiempo de muestreo que en principió se eligió de 300mS y que puede llegar a ser
configurado. Para hacer esto posible se implementó una interrupción de tiempo. Una
vez se ejecuta la interrupción, el cliente RAP solicita la variable de interés al robot
(cuaternión), la cual es almacenada. Posteriormente el proceso envı́a una alarma
para informar la adquisición de la nueva medida. Esta secuencia se repite hasta que
el proceso principal informa la finalización de la trayectoria.
Las funciones usadas para la comunicación mediante el protocolo RAP son obtenidas
de la librerı́a generada en el Laboratorio de Robótica de la FiUBA [73].
6.5.2.4 Proceso principal

El proceso principal en la CN se encuentra divido en los siguientes módulos:
• Comunicación RAP: se utilizan las siguientes funciones para la comunica-

ción con el controlador del robot:
– rap initialize: inicializa la librerı́a.

– trapvar read name: permite leer el valor de una variable definida den-
tro del controlador del robot.
• Comunicación Serial: en este modulo se definen los parámetros de configu-

ración del puerto RS232 disponible en la BeagleBoard a través de la función
init serial com(), se define también la lectura del puerto configurado
reading(). La función medir() se encarga de leer el puerto, concatenar
y escalar debidamente el arreglo entregado por la TAD y almacenarla en un
arreglo de tipo float que será usado posteriormente por el algoritmo de nave-
gación.
• Comunicación entre procesos: los procesos se comunican a través de señales

enviadas mediante llamados al sistema. Cada vez que el proceso del robot
informa de una nueva medida el proceso principal ejecuta el filtro de Kalman.
• Inercial: es uno de los módulos de procesamiento, que accede a las variables

almacenadas por el módulo de comunicación, se encarga de ejecutar los pasos
de la integración siguiendo el método de Runge-Kutta. Con el fin de manejar
estructuras matriciales y vectoriales se utiliza la librerı́a GSL-GNU Scientific
115
Library, la cual soporta un amplio rango de rutinas matemáticas. Las funciones
que se definen dentro de este modulo son:
– calk14(): permite calcular los coeficientes K1 y K4 del método de

Runge-kutta definidos en (Ecu. 2.5).
– calk23(): permite calcular los coeficientes K2 y K3 del método de

Runge-Kutta.
– RKquat(): permite calcular la integración de Runge-kutta a partir de

los coeficientes anteriormente calculados por las funciones calk23() y
calk14().
• Kalman: Este modulo obtiene la información de la UMI y el controlador del

robot a través de los módulos de comunicación. Además, ejecuta las operacio-
nes del algoritmo de Kalman:
– init kalman(): inicializar las matrices de covarianza Q, R, P.
– PropagEst(): usar la aproximación de la matriz transición de estados

para pasar del estado xk−1 al
– PropCovarianza(): usa la ecuación 3.12 para propagar la covarianza

del error P.
– Calc Cins(): determina la entrada al filtro de Kalman a partir de la

transformación del robot y la calculada por el sistema inercial.
– calc k(): calcula la ganancia del filtro de Kalman.
– calc innovacion(): calcula la innovación en el estado.
– Calc innov P(): actualiza P usando la ganancia de Kalman previa-

mente calculada.
• Corrección: este modulo interactúa directamente con los módulos Inercial y

Kalman. Para ello se usan las funciones:
– acumm(): es la función que se encarga de acumular y asignar los estados

estimados b, � y �.
– feedback bef(): esta función realiza la operación de corrección en

velocidad angular dados el estado acumulado obtenido de la función
acumm().
116
Otras funciones importantes en el desarrollo del algoritmo son las transforma-

ciones entre representación (q2m(), m2q()), la ortogonalización ortogon -
q()) y la multiplicación de cuaterniones (MulQ()).
• Main: el main es el módulo encargado de concatenar la información que mane-

jan los módulos anteriormente explicados. En el main se ejecuta el condicional
que determina, según la llegada de alarma del sensor externo, si debe esti-
mar usando FKE o si debe continuar estimando con el modelo y la matriz
transición de estados. Se encarga de llamar e inicializar los módulos abrir y
cerrar los archivos de almacenamiento de datos.
Finalmente, se presenta el algoritmo implementado en la figura 6.16. En este

algoritmo se resume el Main del proceso principal. Las entradas al sistema son la
velocidad angular y el cuaternión externo, mientras la salida es la actitud en repre-
sentación de cuaternión o MCD calculada por el algoritmo.
117
Figura 6.16. Algoritmo del proceso principal.
6.6 Conclusiones
La implementación del sistema de navegación integrada de actitud utilizando HIL
permite analizar la UMI en un entorno similar al real. Generalmente cuando se rea-
lizan simulaciones de sistemas de navegación se parte de la suposición de que las
118
caracterı́sticas incluidas en la hoja de datos son verdaderas y se usan estos datos

para simular el comportamiento de la UMI. Otra forma de obtener estos parámetros
es caracterizar la UMI mediante la toma de muestras sobre excitaciones conocidas.
Es importante considerar el hecho de usar trayectorias sintéticas. La simulación es-
tuvo sujeta al modelo planteado. En el caso del lazo HIL, las condiciones de realidad
del experimento permitieron evaluar otros eventos del sistema que generalmente no
se modelan por simplicidad o no se contemplan como perturbaciones generadas por
fallas en el vehı́culo o interrupciones y obstáculos en su trayectoria. En el capı́tulo
5 se desarrollaron simulaciones con datos reales obtenidos integrando la UMI al ro-
bot. La información conseguida en estos experimentos ya muestra en gran medida
el comportamiento del algoritmo desarrollado. Los resultados logrados en el experi-
mento off-line evidencian un desempeño muy similar al observado en la simulación
HIL on-line.
6.7 Trabajo futuro

Este proyecto fue concebido para simular trayectorias de cualquier vehı́culo que
requiera maniobras de actitud y la utilización de distintas unidades de medidas
inerciales. Se propuso una plataforma flexible para realizar pruebas y desarrollos, la
cual puede ser complementada con las siguientes tareas:
• Desarrollo de librerı́as e interfaces para unidades de medidas inerciales de la

misma u otras tecnologı́as como las descritas en la sección 1.3, es decir, como
arreglos de giróscopos IFOG ó RLG.
• Proponer e implementar nuevas estrategias de sincronización con el fin de

obtener una disminución y estandarización en el tiempo de adquisición de
datos, teniendo en cuenta que este varı́a según las caracterı́sticas y la interfaz
de comunicación de la UMI que se evalúa.
• Implementación de variantes del filtro de Kalman para sistemas no lineales co-

mo el Kalman-Unscented, sobre la computadora de navegación (CN), teniendo
en cuenta que en la literatura actualmente este entrega mejores resultados en
comparación con el FKE para ciertas aplicaciones. Esto, partiendo de que la
CN tiene una entrada constante de las mediciones de los sensores inerciales y
el sensor externo por parte del robot.
119
Referencias Referencias
of u. s. military satellite communication

[1] D. Martin,
systems,”
“A history
Cross-
of u. s. military satellite communication systems,” Cross-
–13, 2001. 1 link, vol. 3, no. 1, pp. 6–13, 2001. 1
ossow, “The international[2]satellite

R. Schiffer
cloudand
climatology
W. Rossow, “The international satellite cloud climatology
project of the world climate research
project(isccp)-
programme,”
the first
Ame-project of the world climate research programme,” Ame-
ciety, Bulletin, vol. 64, pp. 779–784,
rican Meteorological
1983. 1 Society, Bulletin, vol. 64, pp. 779–784, 1983. 1
munication systems. The Institution

[3] B. Evans,
of Engineering
Satellite communication
and systems. The Institution of Engineering and
Technology, 1999. 1
n operational estimation of[4]regional

M. Tasumi,
evapotranspiration
Progress in operational estimation of regional evapotranspiration
2003. 1 using satellite imagery. 2003. 1
tosde Navegación Integrada.[5]AADECA,

M. España,
2010.
Fundamentosde
1, 12, 17, Navegación Integrada. AADECA, 2010. 1, 12, 17,
18, 19, 25, 28, 86
M. Psiaki, “Three-axis attitude

[6] determination
F. Martel, P. Pal,
via and
kalman
M. Psiaki, “Three-axis attitude determination via kalman
ter data,” Journal of Guidance,
filtering
Contolofand
magnetometer
Dynamics, data,” Journal of Guidance, Contol and Dynamics,
514, 1989. 1 vol. 13, no. 3, pp. 506–514, 1989. 1
ed kalman filter for quaternion-based

[7] J. Marins,
attitude
“An extended
estima- kalman filter for quaternion-based attitude estima-
Document, 2000. 1 tion,” tech. rep., DTIC Document, 2000. 1
ansman, and J. J. Dyest, “Single

[8] R. P.
antenna
Kornfeld,
gps R.
based
J. Hansman,
at- and J. J. Dyest, “Single antenna gps based at-
in In Proceedings of the Institute
titude
of Navigation
determination,”
National
in In Proceedings of the Institute of Navigation National
-NTM, Long Beach CA, pp. 345–354,
Technical
1998.
Meeting
1 ION-NTM, Long Beach CA, pp. 345–354, 1998. 1
Waldmann, “Attitude and angular

[9] D. dos
rateSantos
estimation
and J.from
Waldmann, “Attitude and angular rate estimation from
f magnetometer and sun sensorvector
for a measurements
low-cost satellite,”
of magnetometer and sun sensor for a low-cost satellite,”
2009. 1
121
[10] R. E. Kalman et al., “A new approach to linear filtering and prediction pro-
blems,” Journal of basic Engineering, vol. 82, no. 1, pp. 35–45, 1960. 1, 21,
37
[11] R. G. Brown, P. Y. Hwang, et al., Introduction to random signals and applied

Kalman filtering, vol. 1. John Wiley & Sons New York, 1992. 1
[12] M. S. Grewal and A. P. Andrews, Kalman filtering: theory and practice using
MATLAB. Wiley-IEEE press, 2011. 1
[13] P. D. Groves, Principles of GNSS, inertial, and multisensor integrated naviga-

tion systems. 2008. 2
[14] A. Sen, Y. Kim, D. Caruso, G. Lagerloef, R. Colomb, S. Yueh, and D. Le Vine,

“Aquarius/sac-d mission overview,” 2006. 2
[15] “Comisión nacional de actividades espaciales @ONLINE, argentina,” 3
[16] T. Boge, T. Wimmer, O. Ma, and T. Tzschichholz, “Epos–using robotics for

rvd simulation of on-orbit servicing missions,” 2010. 3
[17] “Plan espacial nacional, “argentina en el espacio”,” tech. rep., CoNAE, 2008. 3
[18] D. Li, R. Landry, and P. Lavoie, “Validation and performance evaluation of two
different inertial navigation system design approaches,” in International Global
Navigation Satellite Systems Society Symposium, 2007. 3
[19] S. Godha, “Performance evaluation of low cost mems-based imu integrated with
gps for land vehicle navigation application,” Department of Geomatics Enginee-
ring, p. 12, 2006. 3
[20] CoNAE, “Misión: Sac-d / aquarius,” June 2011. 4, 13
[21] W. Hamilton, “On a new species of imaginary quantities connected with a

theory of quaternions,” in Proceedings of the Royal Irish Academy, vol. 2,
pp. 424–434, 1844. 11
[22] C. Reboulet and S. Leguay, “The interest of redundancy for the design of a sphe-
rical parallel manipulator,” in Recent Advances in Robot Kinematics, pp. 369–
378, Springer, 1996. 12
[23] D. Hoag, “Apollo guidance and navigation: Considerations of apollo imu gimbal
lock,” Apollo Lunar Surface J, 1963. 13
122
[24] M. W. Spong and M. Vidyasagar, Robot dynamics and control. John Wiley &
Sons, 2008. 14, 16, 33
[25] “Ieee standard specification format guide and test procedure for linear, single-
axis, nongyroscopic accelerometers corrigendum 1: Changes to annex k and
annex l,” IEEE Std 1293-1998/Cor 1-2008 (Corrigendum to IEEE Std 1293-
1998), pp. C1 –3, 25 2008. 16
[26] Wikipedia, “Gyroscope,” 2006. 17
[27] Neubrex Technologies, “Fiber Optic Gyroscope development, FOG principle.”

18
[28] M. Grewal and A. Andrews, “How good is your gyro [ask the experts],” Control
Systems, IEEE, vol. 30, no. 1, pp. 12–86, 2010. 19, 20
[29] H. Hou, Modeling inertial sensors errors using Allan variance. University of
Calgary, Department of Geomatics Engineering, 2004. 19
[30] “Ieee standard specification format guide and test procedure for single-axis laser
gyros,” IEEE Std 647-2006 (Revision of IEEE Std 647-1995), 2006. 19
[31] M. Park, Error analysis and stochastic modeling of MEMS based inertial sensors
for land vehicle navigation applications. University of Calgary, Department of
Geomatics Engineering, 2004. 19
[32] J. Durbin and S. J. Koopman, Time series analysis by state space methods,
vol. 38. Oxford University Press, 2012. 21
[33] L. R. Rabiner, “A tutorial on hidden markov models and selected applications

in speech recognition,” Proceedings of the IEEE, vol. 77, no. 2, pp. 257–286,
1989. 21
[34] E. N. Brown, L. M. Frank, D. Tang, M. C. Quirk, and M. A. Wilson, “A sta-

tistical paradigm for neural spike train decoding applied to position prediction
from ensemble firing patterns of rat hippocampal place cells,” The Journal of
Neuroscience, vol. 18, no. 18, pp. 7411–7425, 1998. 21
[35] Z. Chen, R. Barbieri, and E. Brown, “State-space modeling of neural spike

train and behavioral data,” Statistical Signal Processing for Neuroscience and
Neurotechnology. Burlington: Elsevier Science, pp. 175–218, 2010. 21
123
[36] C.-T. Chen, Analog and digital control system design: transfer-function, state-
space, and algebraic methods. Oxford University Press, Inc., 1995. 23
[37] P. G. Savage, “Strapdown inertial navigation integration algorithm design part

1: Attitude algorithms,” Journal of guidance, control, and dynamics, vol. 21,
no. 1, pp. 19–28, 1998. 27
[38] G. J. y. E. M. Carrizo J., Análisis de los datos del vuelo del VS30 y desarrollo
de algoritmos de navegación integrada. Universidad de Buenos Aires, 2013. 29
[39] A. Devices, “Adis16364 datasheet,” 2010. 30, 85, 97
[40] GrabCAD Community, “ABB IRB 140 — 3D CAD Model Library,” 2012. 33
[41] ABB, “Product manual irb 1400 m97a, on-line manual,” 33, 83
[42] E. L. Wiener and R. E. Curry, “Flight-deck automation: Promises and pro-

blems,” Ergonomics, vol. 23, no. 10, pp. 995–1011, 1980. 37
[43] G. Welch and G. Bishop, “An introduction to the kalman filter,” 1995. 37
[44] S. S. Haykin et al., Kalman filtering and neural networks. Wiley Online Library,
2001. 37, 38
[45] R. Dhaouadi, N. Mohan, and L. Norum, “Design and implementation of an

extended kalman filter for the state estimation of a permanent magnet synchro-
nous motor,” Power Electronics, IEEE Transactions on, vol. 6, no. 3, pp. 491–
497, 1991. 37
[46] E. A. Wan and R. Van Der Merwe, “The unscented kalman filter for nonlinear
estimation,” in Adaptive Systems for Signal Processing, Communications, and
Control Symposium 2000. AS-SPCC. The IEEE 2000, pp. 153–158, IEEE, 2000.
37
[47] J. Sasiadek, Q. Wang, and M. Zeremba, “Fuzzy adaptive kalman filtering for
ins/gps data fusion,” in Intelligent Control, 2000. Proceedings of the 2000 IEEE
International Symposium on, pp. 181–186, IEEE, 2000. 37
[48] E. R. Dougherty, Random processes for image and signal processing. SPIE
Optical Engineering Press, 1999. 38
[49] G. S. Kimeldorf and G. Wahba, “A correspondence between bayesian estimation

on stochastic processes and smoothing by splines,” The Annals of Mathematical
Statistics, vol. 41, no. 2, pp. 495–502, 1970. 38
124
[50] J. Farrell and M. Barth, The global positioning system and inertial navigation,
vol. 61. McGraw-Hill New York, NY, USA:, 1999. 39, 86
[51] M. S. Grewal, L. R. Weill, and A. P. Andrews, “Introduction,” Global Positio-

ning Systems, Inertial Navigation, and Integration, pp. 1–8, 1994. 39
[52] R. Garcia, J. Puig, P. Ridao, and X. Cufi, “Augmented state kalman filtering
for auv navigation,” in Robotics and Automation, 2002. Proceedings. ICRA’02.
IEEE International Conference on, vol. 4, pp. 4010–4015, IEEE, 2002. 39
[53] R. H. Reichle, D. B. McLaughlin, and D. Entekhabi, “Hydrologic data assimila-

tion with the ensemble kalman filter,” Monthly Weather Review, vol. 130, no. 1,
pp. 103–114, 2002. 39
[54] R. F. Engle and M. W. Watson, “The kalman filter: applications to forecas-

ting and rational-expectations models,” in Fifth World Conference. Advances
in Econometrics, Edited by: Bewley, TF, vol. 1, pp. 245–83, 1985. 39
[55] G. Evensen, “The ensemble kalman filter: Theoretical formulation and practical
implementation,” Ocean dynamics, vol. 53, no. 4, pp. 343–367, 2003. 39
[56] S. Cooper and H. Durrant-Whyte, “A kalman filter model for gps navigation of
land vehicles,” in Intelligent Robots and Systems’94.Ádvanced Robotic Systems
and the Real World’, IROS’94. Proceedings of the IEEE/RSJ/GI International
Conference on, vol. 1, pp. 157–163, IEEE, 1994. 39
[57] A. S. Zabiyakin, V. O. Prasolov, A. I. Baklanov, A. V. El’tsov, and O. V.

Shalnev, “Sun sensor for orientation and navigation systems of the spacecraft,”
in Photonics for Transportation, pp. 106–111, International Society for Optics
and Photonics, 1999. 43
[58] R. M. Rogers, Applied Mathematics in Itegrated Navigation Systems. Aiaa,

2003. 43
[59] D. Vela, “Estimación de actitud en aplicaciones aeroespaciales.” Tesis para op-

tar al tı́tulo de Ing. Electrónico en la Universidad de Buenos Aires, 2010. 44,
57
[60] G. Castillo, “Navegación integrada ins-gps: Aplicación a un sar aerotransporta-

do.” Tesis para optar al tı́tulo de Ing. Electrónico en la Universidad de Buenos
Aires, 2012. 45, 48
125
[61] J. Giribet, “Algoritmos de navegación integrada aplicados a vehı́culos espacia-
les.” Tesis para optar al tı́tulo de Ing. Electrónico en la Universidad de Buenos
Aires, 2003. 57
[62] F. Ham and R. Brown, “Observability, eigenvalues, and kalman filtering,” Ae-
rospace and Electronic Systems, IEEE Transactions on, vol. AES-19, no. 2,
pp. 269–273, 1983. 73, 75
[63] S. Han and J. Wang, “Monitoring degree of observability in gps/ins integration,”

in Int. Symp. on GPS/GNSS, Yokohama, Japan, pp. 25–28, 2008. 73, 75
[64] S. Hong, H.-H. Chun, S.-H. Kwon, and M. H. Lee, “Observability measures
and their application to gps/ins,” Vehicular Technology, IEEE Transactions
on, vol. 57, no. 1, pp. 97–106, 2008. 73, 75
[65] R. Krenn and B. Schäfer, “Limitations of hardware-in-the-loop simulations

of space robotics dynamics using industrial robots,” EUROPEAN SPACE
AGENCY-PUBLICATIONS-ESA SP, vol. 440, pp. 681–686, 1999. 81, 89
[66] O. Ma, M. Zebenay, and T. Boge, “Control of industrial robots for hardware-
in-the-loop simulation of satellite docking,” in SPIE Defense, Security, and
Sensing, pp. 80440G–80440G, International Society for Optics and Photonics,
2011. 81, 82
[67] O. Ma, A. Flores-Abad, and T. Boge, “Use of industrial robots for hardware-
in-the-loop simulation of satellite rendezvous and docking,” Acta Astronautica,
vol. 81, no. 1, pp. 335–347, 2012. 81, 82
[68] T. Boge and O. Ma, “Using advanced industrial robotics for spacecraft rendez-
vous and docking simulation,” in Robotics and Automation (ICRA), 2011 IEEE
International Conference on, pp. 1–4, IEEE, 2011. 81, 82
[69] J. Yang and H. Li, “Uav hardware-in-loop simulation system based on

right-angle robot,” in Intelligent Human-Machine Systems and Cybernetics
(IHMSC), 2012 4th International Conference on, vol. 1, pp. 58–61, IEEE, 2012.
82
[70] M. Zebenay, R. Lampariello, T. Boge, and R. Krenn, “Control strategy of

hardware-in-the-loop simulator epos 2.0 for autonomous docking verification,”
PAMM, vol. 11, no. 1, pp. 843–844, 2011. 82
[71] T. Instruments, “Beagleboard system reference manual rev c4,” 2009. 83, 87
126
[72] G. Coley, “Beagleboard-xm system reference manual,” Revision A2. Beagle-

Board. org, 2010. 83, 88
[73] R. M. Ilardi Pablo, DÁscanio Mariano, RAPLIB: Sistema de Comunicación

con el Robot de IRB 140 M200 de ABB. Facultad de Ingenierı́a, Universidad
de Buenos Aires. 83, 93, 99
[74] Mouser Electronics, “Original LPCXpresso Boards,” tech. rep., 2016. 86
[75] IBM, “Booting Linux on the BeagleBoard-xM,” 2010. 88
[76] A. Blomdell, G. Bolmsjo, T. Brogardh, P. Cederberg, M. Isaksson, R. Johans-

son, M. Haage, K. Nilsson, M. Olsson, T. Olsson, et al., “Extending an industrial
robot controller: implementation and applications of a fast open sensor interfa-
ce,” Robotics & Automation Magazine, IEEE, vol. 12, no. 3, pp. 85–94, 2005.
92
[77] R. Lundqvist and T. Söreling, New Interface for Rapid Feedback Control on
ABB-robots. PhD thesis, Linköping, 2005. 92
[78] A. F. Automation, ABB RAP Protocol Specification 1.05. 93
[79] A. F. Automation, ABB RAP Service Specification 1.05. 93
[80] A. F. Automation, ABB Ethernet Services 3.0. 93
[81] M. Xie, M. Shen, G. Rong, and D. Shao, “Empirical studies of embedded softwa-
re development using agile methods: a systematic review,” in Proceedings of the
2nd international workshop on Evidential assessment of software technologies,
pp. 21–26, ACM, 2012. 95
[82] B. Boehm, J. A. Lane, S. Koolmanojwong, and R. Turner, “Architected agile

solutions for software-reliant systems,” in Agile Software Development, pp. 165–
184, Springer, 2010. 95
127
Autores
Daniel Reyes
Ingeniero Electrónico de la Universidad Distrital Francisco José de Caldas. Duran-
te sus estudios de pregrado y mediante trabajo cooperativo con la Universidad de
Buenos Aires realizó su trabajo de grado, el cual fue laureado. Se desempeñó como
ingeniero de aplicaciones en Huawei Colombia durante un año y medio. Desde 2015
empezó a trabajar en Claro Colombia como ingeniero de proyectos y arquitecto de
red para servicios de comunicaciones móviles y fijas. En la actualidad se desempeña
como ingeniero de desarrollo de productos convergentes.
Jonatan Aponte
Ingeniero Electrónico de la Universidad Distrital Francisco José de Caldas. Durante
sus estudios de pregrado, su trabajo de grado fue laureado. Se desempeñó como in-
geniero de aplicaciones y Software en Huawei Colombia durante 3 años. Magíster en
Ingeniería Electrónica y Tecnologías de la Información de la Universidad de Ciencias
Aplicadas de Darmstadt, Alemania. Actualmente se desempeña como Consultor en
la empresa Detecon GmbH en Alemania.
José Jairo Soriano Méndez

Ingeniero electrónico y magíster en Ingeniería Industrial de la Universidad Distrital
Francisco José de Caldas, Ingeniero Químico de la Universidad Nacional de Co-
lombia. Trabajó en Proquinal S. A. de 1986 a 1997 como jefe de instrumentación y
control, montajes y mantenimiento eléctrico, y participó en la instalación y puesta
en marcha de diferentes líneas de producción, subestaciones, proyectos de genera-
ción eléctrica y automatización de procesos. Actualmente es profesor titular de la
Universidad Distrital. Ha escrito más de cincuenta artículos para revistas indexadas
y eventos internacionales; sus áreas de estudio e investigación son: sistemas difusos,
álgebra abstracta aplicada, sistemas dinámicos y sistemas complejos.
Este libro se
terminó de imprimir
en abril de 2019
en la Editorial UD
Bogotá, Colombia

Internas Desarrollo de Un Sistema de Navegación - Impresion

Caricato da

Informazioni sul documento

Titolo originale

Copyright

Formati disponibili

Condividi questo documento

Condividi o incorpora il documento

Opzioni di condivisione

Hai trovato utile questo documento?

Questo contenuto è inappropriato?

Copyright:

Formati disponibili

Internas Desarrollo de Un Sistema de Navegación - Impresion

Caricato da

Copyright:

Formati disponibili

Desarrollo de un sistema de navegación

integrada para la evaluación

Al ingeniero Carlos Dams por ser nuestro guı́a en la llegada a la Facultad

Dirección Sección de Publicaciones

Desarrollo de un sistema de navegación integrada para la

CEP-Banco de la República-Biblioteca Luis Ángel Arango

Todos los derechos reservados.

4 Navegación integrada: calibración On-Line 71

6 Implementación de la plataforma de simulación con Hardware en

Little people discuss other

Los satélites artiﬁciales son puestos en órbita con el ﬁn de obtener distintas

La Comisión Nacional de Actividades Espaciales (CoNAE) es la entidad guber-

En el marco de este plan y en conjunto con instituciones universitarias, empre-

BNSC1 , CNES2 , AEB3 , la CoNAE ha desarrollado un conjunto amplio de proyectos

La misión SAC-D Aquarius se llevó a cabo en cooperación cientı́ﬁca de las agen-

En este trabajo se desarrolla una plataforma de simulación para navegación de

usado para evaluar Unidades de Medidas Inerciales y algoritmos de navegación. Se

desarrolla un algoritmo de estimación de actitud mediante el ﬁltro de Kalman ex-

tendido (FKE) y se implementa en la computadora de navegación (CN). El vehı́culo

boratorio de Robótica de la Facultad de Ingenierı́a de la Universidad de Buenos

Aires-FiUBA); en él, se monta fı́sicamente la carga útil del sistema de navegación

(UMI, Tarjeta de Adquisición de Datos-TAD, CN). Esto con el ﬁn de obtener una

prueba para sensores como giróscopos y acelerómetros, tanto de fabricantes externos

como de los fabricados por CONAE.

Figura 2. Metodologı́a del proyecto.

• Estudiar y deﬁnir un modelo de error para el sensor inercial.

• Desarrollar un algoritmo de estimación de actitud a partir de la fusión estadı́sti-

• Evaluar el desempeño de los algoritmos de estimación de actitud.

• Implementar los algoritmos de estimación desarrollados en la computadora de

• Desarrollar el software y hardware requerido para implementar las interfaces

Estructura del documento

En este capı́tulo se desarrollan algunos conceptos previos y deﬁniciones necesa-

En este capı́tulo se desarrolla el algoritmo de navegación inercial y se hace un

En este capı́tulo se desarrolla el contexto del ﬁltro de Kalman y su extensión

En este capı́tulo se construye el modelo de estado aumentado a partir de los

En el capı́tulo 5 se desarrolla un análisis de observabilidad para el modelo de

Finalmente, en el capı́tulo 6 se describe la metodologı́a usada para implementar

The individual is ephemeral,

1.1 Métodos de representación de actitud

• V̇ i : Derivada respecto al tiempo de V , expresada en terna i.

• Cbi : Matriz de rotación que va desde la terna b a la i.

• qbi : Cuaternión de rotación que va desde la terna b a la i.

• x̃: Magnitud vectorial x contaminada con error.

• x̂: Magnitud vectorial x estimada.

• C̄: Matriz calculada por la computadora de navegación (CN).

• q̆: Cuaternión de rotación entregada por el Robot.

• Terna del vehı́culo {b}: tı́picamente es la terna solidaria al centro de masa

• Terna de navegación {ν}: en esta se referencian los parámetros de nave-

1.1.3 Métodos de representación de orientación

1.1.3.1 Matrices de rotación

• Son matrices cuadradas Cab ∈ R3×3 .

• Existe la matriz I ∈ SO(3) donde I es la identidad.

• Son matrices ortogonales (Cab )T = (Cab )−1 .

Cac = Cbc · Cab (1.2)

En el ámbito de navegación también se denominan como matrices de cosenos

Ċba = Cba S(ωab

• V es un vector de norma 1 alrededor del cual se realiza la rotación.

• φ es el ángulo que se rota en sentido positivo alrededor del vector invariante

El producto de dos cuaterniones unitarios es un cuaternión unitario dado por1 :

Análogo a lo que sucede con la MCD, la composición de rotaciones se puede

qca = qcb ◦ qba (1.10)

1.1.3.3 Ángulos de Euler

Figura 1.1. Yaw, Pitch, Roll.

y (t) = f (y, t) (2.4)