Post on 30-Jul-2020
Fusion de SLAM Monocularcon Sensores Inerciales para laEstimacion de la Orientacion
Aplicando Filtro Kalman
por el Ingeniero Electronico
Carlos Alberto Vianchada Estevez
Tesis sometida como requisito parcial paraobtener el grado de:
MAESTRO EN CIENCIAS EN LAESPECIALIDAD DE ELECTRONICA
en el
Instituto Nacional de Astrofısica,Optica yElectronicaAgosto 2012
Tonantzintla, Puebla
Supervisada por:
Dr. Ponciano Jorge Escamilla AmbrosioDr. Juan Manuel Ramırez Cortes
c©INAOE 2012El autor otorga al INAOE el permiso de
reproducir y distribuir copias en su totalidad o enpartes de esta tesis
i
RESUMEN
TITULO:
FUSION DE SLAM MONOCULAR CON SENSORES INERCIALES PARA LA ESTIMACION DE LA
ORIENTACION APLICANDO FILTRO KALMAN
AUTOR: CARLOS ALBERTO VIANCHADA ESTEVEZ
PALABRAS CLAVE: Sistema de Referencia de Orientacion y Rumbo, Fusion de Medidas, Marcos de
Referencia, Matrices de Rotacion, Cuaternion, Angulos de Euler, Filtro Kalman.
CONTENIDO:
La gran variedad de beneficios que ofrece el sistema SLAM basado en un sensor de vision monocular
se ve en gran parte limitada por la inexistencia de sensores adicionales que contribuyan a la estimacion
de la orientacion y trayectoria seguida por la camara. En este trabajo se adiciona un sensor inercial al
sistema SLAM con el objetivo de que la combinacion de los sensores se consolide como una fuente de
informacion cuya calidad es mejor que la provista por cualquier sensor de manera individual, del mismo
modo, se incremente la confiabilidad y disponibilidad de un sistema de medicion ante la falla de alguno de
ellos.
De manera mas especıfica, este trabajo consistio en la fusion de medidas de orientacion proporcionadas
por dos sistemas independientes uno del otro. En primera instancia se presenta una breve discusion sobre
la tecnica de mapeo y localizacion simultanea que utiliza una camara monocular como sensor del entorno.
Posteriormente se mencionan las principales caracterısticas del modulo inercial de bajo costo utilizado
para la elaboracion de un sistema de referencia de orientacion y rumbo (AHRS por sus siglas en ingles)
usando cuaterniones como herramienta de descripcion de la orientacion. La fusion de medidas fue llevada
a cabo implementando metodologıas que usan como estimador secuencial el filtro Kalman con el objetivo
de identificar la tecnica que ofreciera un menor error en la estimacion.
Los resultados evidencian una reduccion del error en la estimacion de la orientacion aunque esta varıe
dependiendo de la tecnica aplicada. Se observa que los modelos de fusion que presentan menor error son
aquellos que retroalimentan al sistema la combinacion de las medidas.
ii
ABSTRACT
TITLE:
FUSION OF MONOCULAR SLAM WITH INERTIAL SENSORS TO THE ATTITUDE ESTIMATION
APPLYING KALMAN FILTER
AUTHOR: CARLOS ALBERTO VIANCHADA ESTEVEZ
KEY WORDS: Attitude and Heading Reference System, Measurements Fusion Models, Reference Frames,
Rotation Matrix, Quaternion, Euler Angles, Kalman Filter.
CONTENTS:
The variety of benefits that SLAM based on monocular vision offers, is limited by the absence of additional
sensors that contribute to the estimation of orientation and the trajectory followed by the camera. This
dissertation adds an inertial sensor to SLAM system in order to consolidate a source information which its
quality is better than that provided by any individual sensor. In the same way, the combination increases
the reliability and availability of a measurement system before the failure of some sensor.
Specifically this work consisted in the orientation measures fusion provided by two independent systems.
In the first instance, is presented a brief discussion about the technique of simultaneous localization and
mapping which uses a monocular camera as environment sensor. Subsequently the main features of the
low cost inertial module used to the elaboration of the attitude and heading reference system (AHRS)
are mentioned, also, the use of quaternion as orientation descriptor. The measures fusion was implemented
using methodologies based in Kalman filter with the objective to identify a technique which offers minor
error in the estimation.
The results showed a error reduction in the orientation estimation although it changes depending of the
technique applied. The fusion models that have less error in the estimation are those which feedback the
measures combination.
Agradecimientos
Primero quiero agradecer a Dios por llenar mi vida de bendiciones.
A mi familia por su apoyo incondicional a todo lo que me propongo, especialmente a mi mama, mi
papa y mi hermano.
A los doctores Jorge Escamilla y Juan Manuel Ramırez por supervisar mi trabajo.
Al Instituto Nacional de Astrofısica Optica y Electronica (INAOE) por ser la institucion que me
abrio sus puertas para poder realizar mis estudios.
Al Consejo Nacional de Ciencia y Tecnologıa (CONACY T ) por birndarme el apoyo financiero para
poder culminar mis estudios de maestrıa.
Carlos Alberto Vianchada Estevez
iv
Indice general
Resumen I
Abstract II
Agradecimientos III
1. Introduccion 1
1.1. Antecedentes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2. SLAM basado en vision . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2.1. Formulacion general del problema SLAM visual . . . . . . . . . . . . . . . . . . . 5
1.2.2. Representacion de los estados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.2.3. Modelo del proceso . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.2.3.1. Modelo de posicion constante . . . . . . . . . . . . . . . . . . . . . . . . 8
1.2.3.2. Modelo de velocidad constante . . . . . . . . . . . . . . . . . . . . . . . 9
1.2.4. Modelo de la medida . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
1.2.5. Modelo de observacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.2.5.1. Inicializacion de caracterısticas . . . . . . . . . . . . . . . . . . . . . . . 13
2. Modulo inercial 15
2.1. Especificaciones del modulo inercial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.2. Microcontrolador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.3. Sensores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.3.1. Acelerometro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.3.2. Giroscopo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.3.3. Magnetometro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.4. Caracterizacion de los sensores inerciales . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
2.5. Marco de referencia del modulo inercial . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3. Sistema de referencia de actitud y rumbo (AHRS) 23
3.1. Marcos de referencia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.1.1. Marco inercial i . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
v
vi INDICE GENERAL
3.1.2. Marco de la tierra e . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.1.3. Marco de navegacion n . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.1.4. Marco del cuerpo b . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.2. Matrices de Rotacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.3. Representacion de la orientacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.3.1. Angulos de Euler . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.3.2. Cuaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.4. Computacion de la actitud . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.4.1. Actitud en terminos de cuaterniones . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.4.2. Deteccion de la inclinacion empleando acelerometros . . . . . . . . . . . . . . . . 34
3.4.3. Deteccion del rumbo empleando magnetometros . . . . . . . . . . . . . . . . . . 35
3.4.4. Algoritmo para la computacion de la actitud . . . . . . . . . . . . . . . . . . . . 37
3.5. Implementacion del algoritmo para la computacion de la actitud . . . . . . . . . . . . . 42
4. Algoritmos de fusion 45
4.1. Modelos de fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.1.1. Modelos de fusion de medidas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.1.1.1. Aumento del vector de observacion . . . . . . . . . . . . . . . . . . . . . 47
4.1.1.2. Estimacion de mınima media cuadratica . . . . . . . . . . . . . . . . . . 47
4.1.2. Modelos de fusion track to track . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
4.1.2.1. Algoritmo de fusion track to track . . . . . . . . . . . . . . . . . . . . . 48
4.1.2.2. Algoritmo de fusion modificado track to track . . . . . . . . . . . . . . 50
5. Resultados experimentales 53
5.1. Modelo de fusion de las medidas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.2. Fusion track to track . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.3. Fusion modificada track to track . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
6. Conclusiones 61
6.1. Trabajo futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
Bibliografıa 67
Referencia Grafica 69
A. Problema general de estimacion secuencial 71
A.1. Filtro Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
A.2. Filtro Kalman Extendido . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
B. Modelo de formacion de la imagen 77
B.1. Perspectiva de proyeccion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
B.2. Posicion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
Indice de figuras
1.1. Sistema SLAM basado en vision. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.2. Esquema del funcionamiento del sistema SLAM basado en vision. . . . . . . . . . . . . 5
1.3. Problema general del SLAM basado en vision. . . . . . . . . . . . . . . . . . . . . . . . 6
1.4. Modelo de posicion constante. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.5. Modelo de velocidad constante. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.6. Parametrizacion de la caracterıstica y ecuacion de medida. . . . . . . . . . . . . . . . . . 12
2.1. Modulo inercial iNEMO STEVAL-MKI062V2. . . . . . . . . . . . . . . . . . . . . . . . . 15
2.2. Medidas del giroscopo en reposo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.3. Medidas del acelerometro en reposo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.4. Medidas del magnetometro en reposo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
2.5. Sistema de referencia de iNEMO. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.6. Sentido de giro en el que las medidas del giroscopo son positivas. . . . . . . . . . . . . . 21
3.1. Sistema de referencia de actitud y rumbo (AHRS). . . . . . . . . . . . . . . . . . . . . . 23
3.2. Marco de referencia inercial i, de la tierra e y de navegacion n . . . . . . . . . . . . . . 24
3.3. Marco de navegacion n . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.4. Marco del cuerpo b. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.5. Rotacion sobre el eje Z. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.6. Rotacion sobre el eje X. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.7. Rotacion sobre el eje Y . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.8. Angulos de Euler: alabeo θ, cabeceo φ y guinada ψ. . . . . . . . . . . . . . . . . . . . . . 28
3.9. Vector antes y despues de una rotacion. . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.10. Inconvenientes para el calculo de la orientacion a partir de acelerometros y giroscopos. . 32
3.11. Deteccion del alabeo θ y cabeceo φ con el acelerometro. . . . . . . . . . . . . . . . . . . 35
3.12. Campo magnetico en coordenadas X Y Z. . . . . . . . . . . . . . . . . . . . . . . . . . . 36
3.13. Filtro Kalman extendido para la estimacion de la orientacion. . . . . . . . . . . . . . . . 37
3.14. Estimacion del bias presente en las medidas del giroscopo. . . . . . . . . . . . . . . . . . 42
3.15. Consistencia del filtro Kalman extendido para la orientacion. . . . . . . . . . . . . . . . 43
3.16. Comparacion del iNEMO con el AHRS. . . . . . . . . . . . . . . . . . . . . . . . . . . 44
vii
viii INDICE DE FIGURAS
4.1. El proceso de fusion de las medidas. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.2. Fusion track to track. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.3. Fusion modificada track to track. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
5.1. Esquema general de implementacion de la fusion de medidas. . . . . . . . . . . . . . . . 54
5.2. Resultados del modelo de fusion de medidas. . . . . . . . . . . . . . . . . . . . . . . . . 55
5.3. Resultados de la fusion track to track. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5.4. Covarianza del error para la fusion track to track ignorando P 12k|k. . . . . . . . . . . . . . 58
5.5. Resultados de la fusion modificada track to track. . . . . . . . . . . . . . . . . . . . . . . 59
B.1. Camara monocular. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
B.2. Geometrıa del modelo de la camara de agujero de alfiler. . . . . . . . . . . . . . . . . . . 78
B.3. Transformaciones entre el marco de referencia n y del cuerpo b. . . . . . . . . . . . . . . 79
Indice de tablas
2.1. Caracterısticas del acelerometro. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.2. Caracterısticas de los giroscopos. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.3. Caracterısticas del magnetometro. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
2.4. Momentos estadısticos de la medida del giroscopo . . . . . . . . . . . . . . . . . . . . . . 18
2.5. Momentos estadısticos de la medida del acelerometro . . . . . . . . . . . . . . . . . . . . 20
2.6. Momentos estadısticos de la medida del magnetometro . . . . . . . . . . . . . . . . . . . 20
3.1. Momentos estadısticos de los angulos medidos . . . . . . . . . . . . . . . . . . . . . . . . 41
3.2. Error medio cuadratico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
5.1. Momentos estadısticos de los angulos medidos (SLAM) . . . . . . . . . . . . . . . . . . 54
5.2. Error medio en el aumento del vector de observacion . . . . . . . . . . . . . . . . . . . . 56
5.3. Error medio en la estimacion de mınima media cuadratica . . . . . . . . . . . . . . . . . 56
5.4. Error medio en la fusion track to track . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.5. Error medio en la fusion track to track ignorando P 12k|k . . . . . . . . . . . . . . . . . . . 58
5.6. Error medio en la fusion modificada track to track . . . . . . . . . . . . . . . . . . . . . 58
5.7. Error medio de los modelos de fusion de medidas . . . . . . . . . . . . . . . . . . . . . . 59
5.8. Error medio de los modelos de fusion track to track . . . . . . . . . . . . . . . . . . . . . 59
ix
Capıtulo 1
INTRODUCCION
La posible creacion de mapas y localizacion en ambientes totalmente desconocidos ha sido uno de los
mas notable sucesos en la comunidad robotica en esta ultima decada. Por tal motivo, el sistema de loca-
lizacion y mapeo simultaneos SLAM1, ha sido considerado como el “santo grial” de la robotica, debido
a que esta tecnica proporcionarıa los medios necesarios para la construccion de un robot verdadera-
mente autonomo. Actualmente, en el area de la robotica tradicional el SLAM puede ser considerado
como un problema resuelto y esto ha permitido que pueda ser implementado en diferentes ambientes
interiores como exteriores, al igual que en vehıculos terrestres, aereos y acuaticos [1][2][3]. Sin embargo,
siguen existiendo problemas importantes en la realizacion de soluciones mas generales y en propor-
cionar mapas ricos en informacion.
Los algoritmos SLAM han sido elaborados usando una gran variedad de sensores como laser, sonar,
radar y encoders, entre otros; pero el uso de camaras como sensor del SLAM no habıa sido el centro
de atencion por la comunidad de vision por computadora. Esta idea cambio con el advenimiento de
camaras modernas, que actualmente son compactas, precisas, no invasivas, bien entendidas y relativa-
mente baratas. Adicionalmente, la tasa de transferencia de datos proveniente de una camara es mucho
mayor en comparacion con la de otros sensores, sin olvidar que la vision para los seres humanos como
para animales es una de las principales herramientas usadas para la navegacion. Este suceso ha per-
mitido que la localizacion y mapeo simultaneos usando una camara monocular haya recibido mucha
atencion en los ultimos anos [4][5][6].
Los sistemas SLAM basados en vision proporcionan conocimiento espacial para robots, permitiendo
la estimacion de movimentos en ambientes totalmente desconocidos. A pesar de ello, el uso de una
camara como unico sensor del entorno presenta algunos inconvenientes. Principalmente sucede cuando
el tamano del ambiente empieza a crecer, implicando un incremento en los requerimientos computa-
cionales de los algoritmos y dificultando la operacion en tiempo real. Como resultado, la consistencia y
precision en la estimacion obtenida decrece. Por otra parte, la escala de la escena no es observable. Es
decir, que el mapa generado presenta variaciones en las medidas de distancia estimadas desde la camara
hasta las caracterısticas visuales detectadas entre experimentos. En consecuencia, la gran variedad de
beneficios de localizacion y mapeo que ofrece el sistema SLAM basado en vision se ve en gran parte
limitada.
1Simultaneous Localization and Mapping
1
2 CAPITULO 1. INTRODUCCION
La fusion de sensores ha surgido como solucion a diferentes problematicas principalmente porque la
combinacion de sensores consolida una fuente de informacion cuya calidad es mejor que la provista por
cualquier sensor de manera individual. La informacion redundante proporcionada por varios sensores
reduce la incertidumbre e incrementa la exactitud, del mismo modo, se incrementa la confiabilidad y
disponibilidad de un sistema de medicion ante la falla de algun sensor. El objetivo de este trabajo es
mejorar la estimacion realizada por el sistema SLAM de la orientacion de la camara. Por consiguiente,
la idea consiste en utilizar un sensor adicional con el cual se pueda generar otra estimacion de dichos
estados y realizar una fusion de sensores con el sistema SLAM basado en vision utilizando tecnicas de
fusion de medidas basadas en el filtro Kalman. Para ello se ha optado por usar una unidad de medida
inercial IMU2 de bajo costo que dispone de sensores inerciales tales como acelerometros, giroscopos
y magnetometros que suministran informacion de la aceleracion lineal y velocidad angular detectada
en el vehıculo (en este caso la camara). Todo bajo la hipotesis de que las medidas inerciales ayudan a
mejorar la orientacion estimada de la camara y por ende, se puede obtener una mejora en el factor de
escala del mapa [7].
A diferencia del trabajo realizado por Pinies [8] que consiste en incluir las medidas inerciales en el
modelo del proceso del SLAM ; lo que se propone en este trabajo esta orientado a elaborar un sistema
de referencia de actitud y rumbo (AHRS3) que estime la orientacion de la camara, para que poste-
riormente puedan ser fusionadas con las medidas del sistema SLAM basado en vision. Por lo tanto,
la fusion es implementada usando dos sistemas independientes uno del otro. Los objetivos del trabajo
son los siguientes:
Elaborar un sistema de referencia de actitud y rumbo (AHRS) para describir la orientacion del
vehıculo usando cuaterniones.
Estudiar las tecnicas de fusion de medidas que usan como estimador secuencial el filtro Kalman.
Implementar la fusion de las medidas del AHRS con el sistema SLAM basado en vision.
1.1. Antecedentes
La combinacion de vision con sensores inerciales logra que los diferentes tipos de informacion sean
apropiados para las distintas circunstancias, lo que hace posible observar mas detalladamente el en-
torno y realizar una estimacion robusta de su propio movimiento mitigando los errores presentes en
cada sensor, en una escena 3D sin infraestructura externa. Las medidas inerciales generalmente se usan
para la elaboracion de sistemas de navegacion, pero su gran inconveniente es el offset que aparece en
las lectura de los sensores dentro del IMU . Con la inclusion de la vison este efecto puede ser reduci-
do buscando una mejora en la navegacion inercial [9], adicionalmente, ha dado pie para crear nuevas
alternativas en sistemas de navegacion como por ejemplo para peatones [10][11] y para vehıculos [12].
2Inertial Measurement Unit3Attitude Heading Reference System
1.2. SLAM BASADO EN VISION 3
Aunque se podrıa pensar que la principal aplicacion de estos sistemas serıan en robotica y sistemas
de navegacion, tambien existen aplicaciones para realidad aumentada [13] y hasta aplicaciones en la
medicina en intervenciones cistoscopicas [14].
Los estudios con sensores inerciales y vision han sido enfocados en la mejora de los sistemas de nave-
gacion, principalmente buscando una mejor estimacion de la posicion del vehıculo [15][16]. La diferencia
en dichos trabajos radica en las diferentes tecnicas o metodologıas usadas para extraer informacion a
traves del procesamiento de la imagen. De la misma forma, no solo se ha mejorado la estimacion de la
posicion del vehıculo sino que adicionalmente los procedimientos han sido extendidos para establecer
la orientacion y velocidad del mismo [17][18].
El procedimiento general que realiza la fusion de las medidas inerciales con una camara monocular
es llevado a cabo aplicando un estimador secuencial como el filtro Kalman y sus variaciones. Un sensor
es el encargado de realizar una estimacion de los estados y el otro permite realizar las correcciones a
dichas estimaciones [19][20][21]. De esta manera es posible reducir el tiempo de computacion y mejorar
el desempeno en el procesamiento de la imagen, el inconveniente es que la medidas eventualmente se
pueden alejar cuando exista perdida de informacion proveniente de la camara (oclusion, movimientos
rapidos).
Propiamente el trabajo que fusiona un sistema SLAM con camara monucular con sensores inerciales
fue elaborado por Pinies [7][8]. Aprovechando que el sistema SLAM basado en vision utiliza un filtro
Kalman extendido para la estimacion de sus estados, las medidas inerciales son incluidas en el modelo
del proceso del filtro el cual realiza la estimacion. Seguidamente, las observaciones de los estados son
extraıdas mediante el procesamiento de la imagen mientras el modelo de la medida se mantiene igual.
Sus resultados reflejan una mejora en la estimacion de la trayectoria seguida por la camara y una
reduccion en la ambuiguedad de la escala del mapa obtenida.
1.2. SLAM basado en vision
El SLAM , (Simultaneous Localization and Mapping) es una tecnica que permite construir mapas
consistentes de entornos desconocidos, mientras simultaneamente determina su ubicacion dentro del
mismo. El resultado final de la aplicacion de este tecnica, es la obtencion de mapas probabilısticos
basado en caracterısticas.
De manera general, el sistema SLAM basado en vision es capaz de detectar caracterısticas (esquinas,
figuras con formas conocidas como rectangulos, circulos, cambio de contraste y demas) en una secuen-
cia de imagenes proporcionada por la camara. Un procesamiento de la imagen hace posible estimar su
ubicacion, la trayectoria seguida por la camara y adicionalmente proporciona una medida de distancia
entre la camara y la caracterıstica detectada, permitiendo la recreacion del entorno tal como se ilustra
en la figura 1.1(b).
4 CAPITULO 1. INTRODUCCION
(a) Deteccion de caracterısticas en la imagen. (b) Mapa probabilıstico.
Figura 1.1: Sistema SLAM basado en vision: (a) Las elipses representan la region donde es buscada la
caracterıstica y la incertidumbre en su ubicacion. Las elipses verdes hacen referencia a las marcas que
han sido satisfactoriamente detectados, mientras que los rojos hacen referencia a los puntos perdidos.
(b) Incremento en la incertidumbre de la posicion de las marcas visuales detectadas a medida que se
explora nuevas areas (elipses rojas). Trayectoria de la camara (puntos verdes).
El problema del SLAM siempre ha recibido un enfoque probabilıstico desde que fue introducido,
por tal motivo, varios grupos de investigadores han estado buscando y aplicando varios metodos de
estimacion teorica para solucionar el problema de la localizacion y mapeo simultaneo. Principalmente,
los estudios realizados por Smith y Cheesman [22] y Durrant-Whyte [23] fueron claves en este proceso.
Debido a que se logro demostrar la existencia de un alto grado de correlacion entre la estimacion de
localizacion de diferentes puntos de referencia en un mapa y que estas crecerıan con las observaciones
sucesivas. Pocos anos despues, Smith en [24] presento la base de lo que es hoy en dıa el SLAM . Men-
ciono que para lograr una completa y consistente solucion para el problema de localizacion y mapeo
simultaneo, se requerirıa una union de estados compuestos por la actitud del vehıculo y la posicion
de cada punto de referencia, para ser actualizado en cada observacion. A su vez, se necesitarıa que el
estimador empleara un vector de estado tan grande como el numero de puntos de referencia que se
mantienen en el mapa.
Actualmente, la localizacion y mapeo simultaneos basado en vision es un enfoque alternativo para
la solucion del problema SLAM en lınea. Esta tecnica aplica un filtrado probabilıstico para actualizar
de forma secuencial las estimaciones de las posiciones de las caracterısticas en el mapa, y la ubicacion
actual de la camara. El SLAM basado en vision tiene diferentes fortalezas y debilidades en odometrıa
visual, siendo capaz de construir mapas consistentes y libres de derivas, pero con un numero limitado
de caracteristicas [25]. Asi mismo, el SLAM usando como nucleo el filtro Kalman extendido fue la
1.2. SLAM BASADO EN VISION 5
primera tecnica aplicada con exito en tiempo real para el seguimiento usando una camara monocular,
la cual fue desarrollada por Davison [4][5].
1.2.1. Formulacion general del problema SLAM visual
El sistema SLAM basado en vision puede ser visto como un problema general de estimacion secuen-
cial donde el objetivo es estimar la distribucion de probabilidad de un espacio de estados [26][27]. El
espacio de estados es n-dimensional y para este caso en particular esta constituido por la ubicacion 3D
de la camara y un conjunto de caracterısticas visuales. La camara tiene un vector de estados xv(k),
que describe su posicion relativa a algun marco de referencia conocido en el instante de tiempo k. A la
vez que esta se mueve a traves del ambiente, la camara toma observaciones zi(k), de la caracterıstica
i. Cada caracterıstica es representada por un vector de estados mi, el cual corresponde a la posicion
invariante en el tiempo de la i-esima marca relativa en un marco de referencia conocido.
KX
K+1X
K+2X
K+3X
1m
2m
3m
4m
5m
KZ
K+1Z
K+2Z
Robot Marca
Estimada
Real
Figura 1.2: Esquema del funcionamiento del sistema SLAM basado en vision.
La posicion de la camara y de las caracterısticas visuales en el instante de tiempo k depende de
la posicion inicial, xv(0), de la camara y la subsiguiente secuencia de medidas, z(0 : k), de todas las
caracterısticas visuales. Esto puede ser formulado como un problema general de estimacion secuencial
y representado por una funcion de densidad de probabilidad conjunta.
p(xv(k),m|z(0 : k), xv(0)) (1.1)
Donde m es el conjunto completo de posiciones de las caracterısticas. Para llevar a cabo este procedi-
miento eficentemente en tiempo real, el objetivo es encontrar una solucion recursiva de manera que el
6 CAPITULO 1. INTRODUCCION
CámaraXv(k)
Z1(k)
Z2(k)m1(k)
m2(k)
puntos 3-D(marcas)
Perspectiva deproyección
(a) Componentes de estado.
Las marcas estimadaspermanecen sin cambios
Movimiento dela cámara
La incertidumbre en la posición estimada de la
ćamara crece
(b) Prediccion de la posicion de la camara.
Marginalización de la anteriorposición de la cámara
Observación de las marcas
(c) Marginalizacion y observacion.
Condicionamiento sobrelas nuevas obsevaciones
Las incertidumbresestimadas se reducen
(d) Condicionamiento.
Figura 1.3: Problema general del SLAM basado en vision.
conjunto de observaciones z(k), del actual instante de tiempo pueda ser combinado con la distribucion
de estado en el instante de tiempo previo para encontrar la distribucion de estados actual.
p(xv(k − 1),m|z(0 : k − 1), xv(0)) (1.2)
La evolucion de la distribucion de estados es controlada por dos procesos. Tomando la posicion de
la camara del intervalo de tiempo anterior y asumiendo que las caracterısticas visuales en el mapa son
estaticas, una funcion de proceso predice la nueva distribucion del estado en el intervalo de tiempo k:
p(xv(k),m|x(k − 1)) (1.3)
Luego la distribucion de estados basada en todas las observaciones previas puede ser recursivamente
actualizada por la marginalizacion de la dependencia en el estado anterior:
1.2. SLAM BASADO EN VISION 7
p(xv(k),m|z(0 : k − 1), xv(0)) =
∫p(xv(k),m|xv(k − 1))
p(xv(k − 1),m|z(0 : k − 1), xv(0))dxv(k − 1)
(1.4)
Una funcion de medida usa la prediccion de la distribucion del estado actual para definir la distribu-
cion del conjunto de observaciones generados en el intervalo de tiempo actual:
p(z(k)|xv(k),m) (1.5)
Esta permite que la distribucion del estado pueda ser recursivamenten actualizada usando el teorema
de Bayes teniendo en cuenta el efecto de las nuevas observaciones:
p(xv(k),m|z(0 : k), xv(0)) =p(z(k)|xv(k),m)p(xv(k),m|z(0 : k − 1), xv(0))
p(z(k)|z(0 : k − 1))(1.6)
La secuencia recursiva de marginalizacion y condicionamiento de la distribucion de el estado ilustrada
en la figuras 1.3(a), 1.3(b), 1.3(c) y 1.3(d), es el proceso fundamental seguido por los filtros de estimacion
secuencial y es independiente del tipo de distribucion. Sin embargo, si el vector de estados, x, que
contiene a xv y m tiene una distribucion gaussiana, sus estadısticas pueden ser representadas por su
media, x y covarianza, P , y el filtro Kalman y sus variaciones pueden ser usadas como estimador
secuencial del problema. Una revision del filtro Kalman es presentada en el apendice A.
1.2.2. Representacion de los estados
Generalmente en implementaciones SLAM monoculares, el vector de estados del sistema x, esta com-
puesto por un conjunto de componentes individuales que representan la posicion y caracterısticas
dinamicas de la camara xv y la posicion de las marcas visuales mas relevantes del entorno mi. Cada
componente tiene asociada una serie de procesos, medidas y modelos de asociacion de datos los cuales
gobiernan el comportamiento del componente durante el ciclo de actualizacion del filtro. El vector de
estados se representa de la siguiente manera:
x =
xv
m1
m2
...
mi
(1.7)
Donde xv representa la posicion de la camara (rotacion y traslacion) y m1−i corresponde a la posicion
3D de todas las marca detectadas, en el instante de tiempo (cuadro de imagen) k.
8 CAPITULO 1. INTRODUCCION
mi =
Xi
Yi
Zi
(1.8)
La matriz de covarianza P de los estados estimados x = (xv,mi)T con media x = (xv, mi)
T tendra la
siguiente estructura:
P =
Pxvxv Pm1xv Pm2xv · · · Pmixv
Pxvm1Pm1m1
Pm2m1· · · Pmim1
Pxvm2Pm1m2
Pm2m2· · · Pmim2
......
.... . .
...
Pxvmi Pm1mi Pm2mi · · · Pmimi
(1.9)
1.2.3. Modelo del proceso
El modelo del proceso esta regido por la suposicion de como la camara se esta moviendo a traves del
entorno. La evolucion dinamica de los estados en el tiempo esta dada por una funcion de transicion de
estados no lineal.
x(k + 1) = f(x(k), w(k)) = f
(xv(k), w(k)
mi(k)
)=
(fv(xv(k), w(k))
fm(mi(k))
)(1.10)
La funcion del proceso puede ser vista como un vector de dos componentes f = (fv, fm)T . Donde
fv actua sobre los estados correspondientes a la posicion y caracterısticas dinamicas de la camara xv.
La funcion fm es una funcion identidad que actua sobre la posicion de las marcas visuales detectadas
en la imagen. Por tanto, la parte del mapa no cambia entre cuadros porque las caracterısticas visuales
son asumidas estaticas. El vector w(k) representa un ruido gaussiano no correlacionado de media cero
con covarianza Q. A continuacion se presentan dos modelos utlizados para modelar el movimiento de
la camara.
1.2.3.1. Modelo de posicion constante
El modelo de posicion constante, supone que la camara permanece estacionaria entre las actuali-
zaciones del filtro y es afectado por ruido blanco gaussiano. Los estados estimados en este modelo
corresponden a la rotacion y traslacion de la camara. La funcion del proceso asociada al modelo de
posicion constante es escrito de la siguiente manera:
xv =
[rnk+1
qnk+1
]=
[rnk + (vnk + V n)∆t
qnk q((ωbk + Ωb)∆t)
](1.11)
1.2. SLAM BASADO EN VISION 9
Donde denota la multiplicacion de cuaterniones, rn es un vector con la posicion en 3D de la camara
en el marco de referencia de navegacion n (traslacion) y qn es un cuaternion el cual describe la orien-
tacion de la camara (rotacion). El vector de velocidad lineal vn requerido para la estimacion de la
posicion es afectado por un ruido en la velocidad lineal V n. Del mismo modo, un vector de ruido en
la velocidad angular Ωb afecta la estimacion de la orientacion. Los marcos de referencia de navegacion
se ilustran en la seccion 3.1.
Debido a que el modelo de posicion constante no predice el movimiento de la camara, la covarianza
en el ruido del proceso Qv va a ser mayor para permitir posibles cambios en la posicion de la camara.
Por consiguiente, las predicciones seran menos precisas y la busqueda de la marcas en la imagenes
abracara regiones mas grandes, aumentando el tiempo de computacion. Sin embargo, este modelo es
beneficioso cuando el seguimiento es perdido [27].
Incremento en la covarianzapara cubrir el posible
movimiento de la cámara
La posición de la cámaraes predecida para
permanecer constante
Xv(k+1)
Xv(k)
-
Figura 1.4: Modelo de posicion constante.
La figura 1.4 ilustra el comportamiento del modelo de posicion constante. Donde Xv(k) corresponde
al estado de la camara anterior y X−v (k + 1) es la prediccion realizada por el modelo del proceso
teniendo en cuenta el estado de la camara anterior.
1.2.3.2. Modelo de velocidad constante
El modelo de velocidad constante es el modelo que extensamente ha sido usado en sistemas SLAM
visuales [27]. Este supone que el movimiento de la camara se realiza a velocidad angular y lineal cons-
tante. Un ruido blanco gaussiano afecta la velocidad lineal y angular de la camara para hacer frente a
los pequenos cambios en el modelo de velocidad constante. La evolucion de los estados de la camara
usando el modelo de velocidad constante es la siguiente:
xv =
rnk+1
qnk+1
vnk+1
ωbk+1
=
rnk + (vnk + V n)∆t
qnk q((ωbk + Ωb)∆t)
vnk + V n
ωbk + Ωb
(1.12)
10 CAPITULO 1. INTRODUCCION
Donde denota la multiplicacion de cuaterniones. Los estados del modelos estan compuestos por un
vector de posicion rn, un cuaternion de orientacion qn, vector de velocidad lineal vn y un vector de
velocidad angular ωb, donde n el es marco de referencia de navegacion y b el marco de referencia del
cuerpo. Los marcos de referencia se ilustran en la seccion 3.1.
El modelo de velocidad constante no significa que la camara se mueva con una velocidad constante
en todo el tiempo. El modelo implica que la velocidad de la camara es un promedio esperado para
mantenerse constante entre intervalos de tiempo, mientras que las aceleraciones no observadas son
modeladas como un ruido Gaussiano [28]. Por su parte el ruido es expresado como:
w(k) =
[V n
Ωb
]=
[an∆t
αb∆t
](1.13)
El ruido puede ser interpretado como las aceleraciones lineales an y angulares αb, las cuales producen
variaciones en la velocidad lineal y angular.
La posición de la cámara espredecida de las velocidades estimadas
La covarianza crece en pequeñaproporción para cubrir las aceleraciones
en el modelo de movimiento
Xv(k+1)-v
w
Xv(k)
Figura 1.5: Modelo de velocidad constante.
Los beneficios de usar un modelo de velocidad constante es que proporciona predicciones precisas
cuando la camara se esta moviendo sobre una trayectoria suave. Esto implica que la ubicacion de carac-
terısticas en la imagen requiera de menos busqueda para encontrar observaciones. Desafortunadamente,
el inconveniente de este modelo es que la camara continua siguiendo su trayectoria estimada en perıodos
en que el seguimiento se ha perdido, por ejemplo, cuando la camara esta ocluida. Esto significa que es
facil que el sistema sea inconsistente durante los perıodos de seguimiento inestable [27].
1.2. SLAM BASADO EN VISION 11
1.2.4. Modelo de la medida
La deteccion de nuevas caracterısticas en la imagen es quizas uno de los mas grandes problemas
en sistemas SLAM basados en vision, debido a que dependiendo del metodo utilzado este proceso
puede demandar mayores requerimientos computacionales y por ende el aumento en el tiempo de
procesamiento. En algunos trabajos la inicializacion de nuevas caracterısticas en el mapa es retrasada
(delayed) hasta que haya suficiente paralaje (cuando un objeto parece que cambia su posicion porque
la persona u objeto que lo observa ha cambiado su posicion) para estimar la profundidad de las car-
acterısticas detectadas [29]. Pero tambien existen tecnicas que no lo son, como por ejemplo el inverso
de la profundidad de las caracterısticas (metodo undelayed) [30]. Esta tecnica es capaz de lidiar de
manera uniforme, con caracterısticas cercanas y lejanas desde el primer momento en que se detectan.
Sin embargo, tanto la inicializacion con retraso o sin retraso tienen ciertos beneficios y otras desven-
tajas algunas de ellas se presentan en [31].
Las observaciones de las caracterısticas en el mapa son modeladas por un modelo de medida que
representa la proyeccion de las caracterısticas al plano de imagen de la camara:
zi(k) = hi(xv(k),mi, v) (1.14)
Donde zi es la observacion de la caracterıstica mi, hi es una funcion no lineal de medida, y v es un
ruido blanco gausiano variable representando una medida de ruido desconocida con covarianza R.
De manera general, el modelo de la camara de agujero de alfiler4 es usado para realizar una medicion
sobre la ubicacion de la caracterıstica en la imagen (ver apendice B). Este modelo es el que permite
establecer la funcion de medida hi, en donde la posicion de las caracterısticas detectadas en la imagen
es proyectada sobre el plano de la imagen usando la ecuacion B.8:
hi = Π(Cbn(mni − rn)) (1.15)
Donde mni es la posicion de las caracterısticas detectadas y la posicion de la camara rn dadas en el
marco de referencia de navegacion n. Cbn es la matriz de rotacion que transforma las medidas al marco
de referencia de la camara b. Los marcos de referencia y matrices de rotacion son presentadas en las
secciones 3.1 y 3.2 respectivamente. La funcion de medida es presentada de manera mas completa en
[27], en donde se tienen en cuenta los efectos de la distorsion en la imagen y los parametros intrınsecos
de la camara los cuales son obtenidos por calibracion.
4Pinhole camera model
12 CAPITULO 1. INTRODUCCION
1.2.5. Modelo de observacion
El inverso de la profundidad es un modelo de observacion que permite una inicializacion sin retraso
para nuevas caracterısticas. Esta formulacion es mucho mas facil para manejar y mantener el tamano
total del estado mas pequeno [25] [30]. La clave es que el inverso de la profundidad de la caracterıstica
relativa a la posicion de la camara de donde esta fue vista por primera vez, tiene casi una distribucion
gaussiana. Por lo tanto, la posicion de cada marca i puede ser determinada con ayuda de los siguientes
parametros organizados en un vector de 6 dimensiones:
yi = (xi, yi, zi, θi, φi, ρi)T
(1.16)
Donde xi, yi y zi corresponde a la posicion de la camara descrito en coordenadas de navegacion
n, donde la caracterıstica fue observada por primera vez. Los angulos θi, φi definen el azimut y la
elevacion del vector unitario de direccion u(θi, φi). Finalmente, ρi = 1/di es el inverso de la distancia
di entre la posicion de la camara y la caracterıstica (ver figura 1.6).
Xn
Yn
Zn
Xb Z
b
Yb
rn
(r , q )n n
xiyi
zi
rn-
xiyi
zi
a
hb
pi=
1 di
m( ),
ángulo deparalaje
xiyi
zi
+pi
1m( ),
Figura 1.6: Parametrizacion de la caracterıstica y ecuacion de medida.
La posicion en donde se encuentra ubicada la caracterıstica detectada puede ser obtenida utilizando
la siguiente ecuacion:
mi =
Xi
Yi
Zi
=
xi
yi
zi
+1
ρiu(θi, φi) (1.17)
1.2. SLAM BASADO EN VISION 13
u(θi, φi) = (senθicosφi − senφi cosθicosφi)T
(1.18)
La observacion de un punto mi desde la ubicacion de la camara, es expresada como un vector
hb = (hx, hy, hz) dado en el marco de referencia de la camara b:
hb = Cbc =
xi
yi
zi
+1
ρiu(θi, φi)− rn
(1.19)
La camara no observa directamente la distancia hb, pero si su proyeccion usando el modelo de la
camara de agujero de alfiler (ver apendice B).
h =
(u
v
)=
[u0 − fhx
sxhz
v0 − fhy
syhz
](1.20)
Donde, u0, v0 son el centro del plano de imagen de la camara en pixeles, f es la longitud focal y sx,
sy el tamano del pixel.
1.2.5.1. Inicializacion de caracterısticas
La ubicacion inicial para la caracterıstica observada es definida con ayuda del estado de la camara
(posicion rn y orientacion qn), la proyeccion del punto al plano de la imagen h y un valor de profundidad
inicial ρ0 tomado heurısticamente:
yi = (rn, qn, h, ρ0) = (xi, yi, zi, θi, φi, ρi)T
(1.21)
La posicion inicial de la camara de donde fue dectectada por primera vez la marca es tomada de rn.
La distancia entre la camara y la marca detectada es establecida a continuacion:
hn = Cnb hb
u
v
1
(1.22)
Los parametros u y v son las coordenadas en el plano de la imagen normalizadas. A pesar que hn
no es un vector direccional no unitario, los angulos son calulados de la siguiente manera:
(θi
φi
)=
arctan(−hny ,
√hn2
x + hn2
z
)arctan (hnx , h
nz )
(1.23)
14 CAPITULO 1. INTRODUCCION
La covarianza para xi, yi, zi θi y φi es obtenida de la covarianza en el error de la medidad en la imagen
R y la covarianza de los estados estimados P . El valor inicial de ρ0 es determinado heurısticamente para
cubrir una region del 95 % de aceptacion en el espacio de trabajo del infinito a una distancia predefinida.
Para mayor informacion sobre como es realizada la inicializacion de la caracterıstica detectada mi el
lector puede revisar las referencias [25] y [30].
Capıtulo 2
MODULO INERCIAL
El iNEMO es un modulo inercial que combina acelerometros, giroscopos y magnetometros con sen-
sores de presion y temperatura que proporcionan un sensado de moviemto lineal, angular y magnetico
en los 3 ejes [32]. Es el sensor inercial seleccionado para la realizar la fusion con el sistema SLAM con
camara monocular debido a su bajo costo y disponibilidad.
Figura 2.1: Modulo inercial iNEMO STEVAL-MKI062V2.
2.1. Especificaciones del modulo inercial
Las principales caracterısticas del modulo inercial iNEMO son las siguientes:
15
16 CAPITULO 2. MODULO INERCIAL
Dos fuentes de alimentacion: por conector USB y por conector externo.
Conector extendido para conectividad wireless.
Ranura para tarjeta MicroSD.
Conexion USB 2.0 a toda velocidad.
De manera adicional, es posible configurar el dispositivo para obtener una frecuencia de adquision
de datos de 1, 10, 25, 30 y 50 Hz.
2.2. Microcontrolador
Para procesar las medidas de los diferentes sensores, el iNEMO dispone de un microcontrolador
STM32F103RET de STMicroelectronics. Es un dispositivo de baja potencia y alto desempeno de 32
bits con nucleo ARMrCortexTM −M3. Opera a 72 MHz y dispone de una memoria flash de 512
Kbytes y SRAM de 64Kbytes, con un voltaje de operacion de 2 - 3.6 Volts.
2.3. Sensores
Los sensores requeridos para la realizacion de la aplicacion son el acelerometro, giroscopo y mag-
netometro todos con capacidad de sensado en sus 3 ejes. El iNEMO dispone un giroscopo de 2 ejes
para los movimiento de alabeo y cabeceo (roll and pitch) y un giroscopo adicional que solo mide 1
eje correspondiente al movimiento de guinada (yaw). Por su parte el acelerometro y el magnetometro
estan ubicados en el mismo dispositivo, ambos con capacidad de medida en sus 3 ejes.
2.3.1. Acelerometro
Este dispositivo tiene la capacidad de medir la aceleracion lineal. Perciben la variacion de la compo-
nente traslacional de aceleracion y la aceleracion gravitacional. El acelerometro esta disponible en el
modulo geomagnetico LSM303DLH. Las caracterısticas del acelerometro se presentan en la siguiente
tabla:
Dispositivo Fabricante Rango [g] Ejes Salida Comunicacion
LSM303DLH ST Microelectronics 2 / 4 / 8 XYZ Digital I2C
TABLA 2.1: Caracterısticas del acelerometro.
Una ventaja de los acelerometros digitales con respecto a los analogicos es la inmunidad al ruido,
aunque no dejan de ser sensores muy ruidosos. Los acelerometros generalmente requieren de una com-
pensacion para que la medida pueda ser correcta. Esto se debe a que la senal de salida presenta un
ligero offset cuando se encuentra en reposo.
2.3. SENSORES 17
El acelerometro de 3 ejes mide la aceleracion gravitacional y lineal de un vehıculo. Si la aceleracion
lineal es pequena comparada con la aceleracion gravitacional, entonces la lectura del acelerometro indica
las componentes del vector gravedad. Las senales medidas con el acelerometro pueden ser descritas
como la suma de la aceleracion lineal a, la gravedad g, un offset fbias y el ruido blanco presente en
la medida vf . Todos vectores de 3 dimensiones.
fm = a+ g + fbias + vf (2.1)
Esta aceleracion se cuantifica en las unidades del sistema internacional en metros sobre segundo al
cuadrado [m/s2], aunque algunos fabricantes prefieren dar sus especificaciones en terminos de la fuerza
G, ejemplo: miligravedades [mG] donde 1G = 9.8 m/s2.
2.3.2. Giroscopo
Como ya se menciono, el sensado de la velocidad angular es realizado a traves de dos giroscopos. El
primero el LPR430AL que proporciona las medidas de los ejes de alabeo y cabeceo y el LY 330ALH
que sensa la guinada. Sus caracterısticas se presentan a continuacion:
Dispositivo Fabricante Rango ±[dps] Ejes Salida Sensibilidad [mV/o/s]
LPR430AL ST Microelectronics 300/1200 XY Analogico 3.33/0.83
LY330ALH ST Microelectronics 300 Z Analogico 3.33
TABLA 2.2: Caracterısticas de los giroscopos.
El giroscopo mide las tres componentes de velocidad angular. Desde que los giroscopos sufren de
derivas (drift), el modelo de medida del giroscopo esta descrito por la suma de la velocidad angular
w, el offset ωbias que es lentamente variante en el tiempo y el ruido blanco gaussiano presente en la
medicion vw.
wm = w + ωbias + vw (2.2)
Las unidades de la velocidad angular medida son de grados sobre segundo [o/s]. Debido a la deriva
presente en la medicion del giroscopo, no es aconsejable realizar una simple integracion para obtener
la orientacion pues esta presenta errores a medida que transcurre el tiempo.
2.3.3. Magnetometro
El magnetometro por su parte tiene la capacidad de medir el campo magnetico de la tierra. Los
magnetometros de 3 ejes miden la componente del campo magnetico en una direccion particular. las
caracterısticas son las siguientes:
18 CAPITULO 2. MODULO INERCIAL
Dispositivo Fabricante Rango [Gauss] Ejes Salida Comunicacion
LSM303DLH ST Microelectronics 1.3-8.1 XYZ Digital I2C
TABLA 2.3: Caracterısticas del magnetometro.
La distorsion en la medida del magnetometro principalmente se debe a la cercanıa de materiales fe-
rrosos al dispositivo y campos magneticos externos (iman). Estos metales como el hierro, niquel, acero
y cobalto se encuentran en los vehıculos, aeronaves y plataformas sobre las cuales los magnetometros
son montados, por consiguiente, distorsionan o curvan el campo magnetico de la tierra provocando
errores en las medidas. Los inconvenientes causados por estos materiales pueden ser removidos si el
dispositivo es instalado de manera segura [33].
Las senales medidas por el magnetometro estan descritas por la suma del campo magnetico terrestre
en cada uno de sus ejes h, una perturbacion d y el ruido blanco presente en la medicion vm.
Hm = h+ d+ vm (2.3)
Donde las unidades de la medida del campo magnetico estan dadas en [Gauss] y miligauss [mGauss]
y estan dadas en el marco de referencia del cuerpo b.
2.4. Caracterizacion de los sensores inerciales
Para estimar el ruido que afectan las mediciones del giroscopo, el acelerometro y magnetometro, se
procedio a realizar medidas cuando el modulo inercial iNEMO se encuentra en reposo durante un
periodo de tiempo establecido. Este experimento permite determinar las caracterısticas presentes en la
medida de los sensores y comparar los resultados con los modelos de medida propuestos anteriormente.
A las medidas realizadas en cada uno de sus ejes se calculo su valor medio y su desviacion estandar.
Los resultados se presentan en las tablas 2.4, 2.5 y 2.6.
TABLA 2.4: Momentos estadısticos de la medida del giroscopo
Parametro eje x eje y eje z Unidades
m 0.0930 -3.9590 4.8400 o/seg
σ 0.3006 0.6319 0.4386 o/seg
Cabe aclarar que para tomar las medidas del acelerometro en el eje z cuando el sistema se encuentra
en reposo, fue necesario ubicar el iNEMO en posicion vertical para que la medida en el eje z no fuera
afectada por la gravedad. El bias tanto en las medidas de aceleracion y velocidad angular como se
observan en las tablas anteriores pueden tomar valores positivos y negativos. Esta toma de medidas
permite establecer de cierta manera los errores cometidos en la medicion que pueden afectar el resultado
final, por ejemplo, el calculo de la orientacion del vehıculo.
2.4. CARACTERIZACION DE LOS SENSORES INERCIALES 19
0 2 4 6 8 10 12 14 16 18 20−6
−4
−2
0
2
4
6
Medidas del Giróscopo en Reposo
Tiempo [seg]
Velo
cid
ad
An
gu
lar
[º/s
eg
]
Wx
Wy
Wz
Figura 2.2: Medidas del giroscopo en reposo.
0 2 4 6 8 10 12 14 16 18 20−40
−30
−20
−10
0
10
20
30
40
Medidas del Acelerómetro en Reposo
Tiempo [seg]
Fu
erz
a G
[m
G]
Ax
Ay
Az
Figura 2.3: Medidas del acelerometro en reposo.
20 CAPITULO 2. MODULO INERCIAL
TABLA 2.5: Momentos estadısticos de la medida del acelerometroParametro eje x eje y eje z Unidades
m 15.3690 -22.8380 16.9750 mG
σ 2.6448 2.2104 3.6952 mG
0 2 4 6 8 10 12 14 16 18 20−700
−600
−500
−400
−300
−200
−100
0
Medidas del Magnetómetro en Reposo
Tiempo [seg]
Cam
po
Mag
néti
co
[m
Gau
ss]
Hx
Hy
Hz
Figura 2.4: Medidas del magnetometro en reposo.
TABLA 2.6: Momentos estadısticos de la medida del magnetometro
Parametro eje x eje y eje z Unidades
m -37.1920 -100.5750 -625.538 mGauss
σ 3.3718 3.5446 4.0485 mGauss
2.5. Marco de referencia del modulo inercial
Cada sensor inercial presenta unos ejes coordenados sobre los cuales son llevadas a cabo las medi-
das. El modulo inercial iNEMO se ha encargado de ubicar cada sensor de manera que sus marcos
de referencia coincidan y por tanto es posible establecer un unico marco de referencia para todo el
dispositivo, tal como se muestra en la figura 2.5.
2.5. MARCO DE REFERENCIA DEL MODULO INERCIAL 21
Figura 2.5: Sistema de referencia de iNEMO.
Esta configuracion evita que las medidas de los sensores tengan que ser transformadas de un sistema
de referencia a otro, obteniendo una mayor comodidad en la lectura de datos e incurriendo en menor
error en la medicion.
Z
XY
X Y
Z
Figura 2.6: Sentido de giro en el que las medidas del giroscopo son positivas.
22 CAPITULO 2. MODULO INERCIAL
Capıtulo 3
SISTEMA DE REFERENCIA DE
ACTITUD Y RUMBO
Un sistema de referencia de actitud y rumbo (AHRS) esta dedicado a la estimacion de la orientacion
y rumbo de un vehıculo (plataforma movil) y generalmente puede ser elaborado usando sensores iner-
ciales tales como acelerometros, giroscopos y magnetometros tridimensionales. El diagrama de bloques
de un AHRS solo requiere de un IMU y de un procesamiento conocido como computacion de la
actitud, que utiliza las medidas realizadas para solucionar las ecuaciones que describen la orientacion.
Sensores inerciales
Electrónica delos instrumentos
Computaciónde la actitud
Unidad de medida inercial (IMU)
Sistema de referencia de actitud y rumbo (AHRS)
Figura 3.1: Sistema de referencia de actitud y rumbo (AHRS).
En las siguientes seciones, la computacion de la actitud es descrita con mas detalle al igual que
conceptos adicionales necesarios para su comprension.
3.1. Marcos de referencia
En esta seccion se presentan los diferentes marcos de referencia usados para describir la orientacion
de un vehıculo. Estas definiciones son importantes por varias razones, entre ellas: las mediciones prove-
nientes de los sensores inerciales y la camara se entienden dentro de diferentes marcos de referencia,
mientras que algunas especificaciones como las trayectorias, mediciones de posicion, velocidad, GPS
se determinan sobre un marco de referencia inercial o de navegacion.
23
24 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)
3.1.1. Marco inercial i
Tiene su origen en el centro de la tierra, su eje zi coincide con el eje polar de la tierra, el eje xi
apunta hacia el equinoccio vernal (Es el nodo ascendente entre el plano ecuatorial y la eclıptica [36]),
y el eje yi completa el marco ortogonal.
xe
ze
Ye
xn
nY
zn
x i
Yi
zi
Figura 3.2: Marco de referencia inercial i, de la tierra e y de navegacion n
3.1.2. Marco de la tierra e
El marco de referencia de la tierra tiene su origen en el centro de masa de la tierra. El eje xe se
encuentra a lo largo de la interseccion del plano del meridiano de Greenwich con el plano ecuatorial
de la tierra, el eje ze es paralelo al eje de rotacion de la tierra y perpendicular al plano ecuatorial, por
ultimo, el eje ye completa el marco ortogonal. El marco de referencia de la tierra e rota con respecto
al marco inercial i, a una velocidad Ω sobre el eje zi.
3.1.3. Marco de navegacion n
El marco de referencia de navegacion presenta un sistema de coordenadas fijo sobre un punto local
de la tierra. Es representado por un vector ortogonal (xn, yn, zn) donde xn apunta al Norte, yn al Este
y zn se dirige al centro de la tierra. Tambien llamado marco de referencia inercial o NED1. (ver figuras
3.2 y 3.3).
3.1.4. Marco del cuerpo b
El marco de referencia del cuerpo esta asociado al vehıculo, en este caso a la camara o al modulo
inercial. Es representado por el vector (xb, yb, zb) tal como se ilustra en la figura 3.4.
1North East Down
3.2. MATRICES DE ROTACION 25
xn
nY
zn
Norte
Este
Abajo
Figura 3.3: Marco de navegacion n
Y
Z
Xb
b
b
Figura 3.4: Marco del cuerpo b.
3.2. Matrices de Rotacion
En el espacio las matrices de rotacion representan la orientacion de un sistema de coordenadas que
gira alrededor de uno de los ejes de un sistema de referencia fijo. En la Figura 3.5, el sistema de coor-
denadas UVW gira entorno al eje Z del sistema de referencia XY Z un angulo ψ. Cada giro sobre cada
uno de los ejes del sistema de coordenadas de referencia tendra asociada una correspondiente matriz
de rotacion.
El concepto de matrices de rotacion permite hacer la transformacion de las coordenadas de un
vector de un sistema de referencia a las del otro [37]. La notacion Cxyzuvw corresponde a la matriz de
rotacion del sistema de coordenadas UVW a las coordenadas XY Z, rotado un angulo ψ sobre el eje Z.
Cxyzuvw(ψ) =
cosψ −senψ 0
senψ cosψ 0
0 0 1
(3.1)
26 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)
Z
W
X UY
V
Figura 3.5: Rotacion sobre el eje Z.
Z
W
X
U
Y
V
Figura 3.6: Rotacion sobre el eje X.
Procediendo de manera similar, cuando el sistema de coordenadas UVW gira sobre el eje X un
angulo θ, se obtiene:
Cxyzuvw(θ) =
1 0 0
0 cosθ −senθ0 senθ cosθ
(3.2)
Por ultimo cuando el sistema de coordenadas UVW gira sobre el eje Y un angulo φ se obtiene:
Cxyzuvw(φ) =
cosφ 0 senφ
0 1 0
−senφ 0 cosφ
(3.3)
La transformaciones del marco de referencia del cuerpo b al de navegacion n, esta basado en las
3 anteriores rotaciones pero de manera consecutiva. Por tanto, la matriz de rotacion del marco de
referencia del cuerpo b al de navegacion n esta dada por:
3.3. REPRESENTACION DE LA ORIENTACION 27
Z
W
X
U
Y
V
Figura 3.7: Rotacion sobre el eje Y .
Cnb (θ, φ, ψ) =
cosψ −senψ 0
senψ cosψ 0
0 0 1
cosφ 0 senφ
0 1 0
−senφ 0 cosφ
1 0 0
0 cosθ −senθ0 senθ cosθ
(3.4)
3.3. Representacion de la orientacion
Un sistema robotico queda totalmente definido en el espacio tridimensional a traves de dos especifi-
caciones: La posicion y la orientacion con respecto a un sistema de referencia. Estas descripciones se
pueden hacer siguiendo diferentes metodologıas. A continuacion se presentan dos conceptos utilizados:
Los angulos de Euler y Cuaterniones.
3.3.1. Angulos de Euler
Mediante tres angulos ψ, θ, φ denominados angulos de Euler es posible describir la orientacion
de cualquier objeto con respecto a un sistema de referencia. Es la representacion mas utilizada en
aeronautica, e indica los giros realizados sobre cada uno de los ejes del sistema fijo.
Como se observa en la Figura 3.8, cuando el sistema de coordenadas de la aeronave gira un angulo ψ
entorno al eje Z del sistema de referencia, el giro corresponde al movimiento aeronautico denominado
como guinada (yaw). Cuando gira un angulo θ entorno al eje X, el movimiento correspondiente es
llamado alabeo (roll). Por ultimo, cuando gira un angulo φ entorno a eje Y , el movimiento correspon-
diente es llamado cabeceo (pitch).
28 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)
Yb
Zb
Xb
Figura 3.8: Angulos de Euler: alabeo θ, cabeceo φ y guinada ψ.
Una representacion con angulos de Euler aunque es sencilla es indeseable, debido a que presenta
singularidades cuando dos de sus ejes de rotacion estan alineados. Sin embargo, existen dos representa-
ciones adecuadas que no presentan este tipo de inconvenientes: cuaternion y mapas exponenciales.
3.3.2. Cuaternion
El cuaternion es una representacion de cuatro parametros basada en que la transformacion de un
sistema coordenado a otro puede ser efectuada por una sencilla rotacion sobre un vector v definido en
el marco de referencia utilizado, tal como se ilustra en la figura 3.9. El cuaternion proporciona una
representacion de angulos que evita singularidades, adicionalmente, es una herramienta matematica
de gran versatilidad computacional. Un cuaternion se representa de la siguiente manera:
Z
X
Y
v (0,0,1) -X
r (-1,1,0)
br (1,1,0)
n
Figura 3.9: Vector antes y despues de una rotacion.
3.3. REPRESENTACION DE LA ORIENTACION 29
q = qs + qx· i+ qy· j + qz· k (3.5)
Consiste de una parte real escalar qs y un vector de 3 partes (qx, qy, qz). De manera vectorial:
q = [qs, qx, qy, qz]T
(3.6)
Para utilizar un cuaternion como forma de representacion de la orientacion, se asocia un angulo θ a
un vector v = (v1, v2, v3), por lo tanto, el cuaternion q se encuentra definido como:
q = Rot(θ, v) =
(cos
θ
2, v· senθ
2
)(3.7)
q = cosθ
2+ v1· sen
θ
2· i+ v2· sen
θ
2· j + v3· sen
θ
2· k
Vectorialmente:
q =
qs
qx
qy
qz
=
cos( θ2 )
v1· sen( θ2 )
v2· sen( θ2 )
v3· sen( θ2 )
(3.8)
El conjungado, norma e inverso de un cuaternion se definen como:
q = [qs,−qx,−qy,−qz]T
|q| =√q2s + q2x + q2y + q2z
q−1 =q
|q|
Para la representacion de la orientacion usando cuaterniones solo serıa necesario realizar la multipli-
cacion de los cuaternios correspondientes a cada angulo de navegacion y su vector de referencia.
q = qyaw qpitch qroll (3.9)
Donde es el operador que simboliza la multiplicacion de cuaterniones.
qyaw = Rot(ψ, [0, 0, 1])
30 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)
qpitch = Rot(φ, [0, 1, 0])
qroll = Rot(θ, [1, 0, 0])
El producto de 2 cuaterniones, q = qs + qx· i+ qy· j + qz· k y p = ps + px· i+ py· j + pz· k es obtenido
usando las reglas generales del producto de dos numero complejos.
i· i = −1 i· j = k j· i = −k
Entonces:
q p = qsps − qxpx − qypy − qzpz + (qspx + qxps + qypz − qzpy)· i
(qspy − qxpz + qyps + qzpx)· j + (qspz + qxpy − qypx + qzps)· k (3.10)
La multiplicacion entre cuaterniones puede ser vista como una multiplicacion matricial entre una
matriz compuesta por los elementos del primer cuaternion por el segundo cuaternion p:
r = q p = Q(q)· p (3.11)
rs
rx
ry
rz
=
qs −qx −qy −qzqx qs −qz qy
qy qz qs −qxqz −qy qx qs
ps
px
py
pz
Usando cuaterniones es posible que a partir de un vector definido en el marco de referencia del cuerpo
rb este puede ser expresado en el marco de navegacion rn. Este procedimiento se lleva a cabo a traves
de la siguiente ecuacion:
rn′
= q rb′ q (3.12)
Donde rb′
en la ecuacion 3.12 corresponde al cuaternion con parte escalar igual a cero y la parte
vectorial igual al vector rb. El resultado rn′
tambien correspondera a un cuaternion donde su parte
escalar es igual a cero y la parte vectorial contienen las coordenadas del vector despues de su transfor-
macion. A manera de ejemplo, estableciendo que el angulo de rotacion en la figura 3.9 es igual a 90o,
se tiene que q = Rot(90o, [0, 0, 1]), por ende:
3.3. REPRESENTACION DE LA ORIENTACION 31
[0,−1, 1, 0]T
=
[√2
2, 0, 0,
√2
2
]T [0, 1, 1, 0]
T [√
2
2, 0, 0,
−√
2
2
]T
Estableciendo que q describa la orientacion del vehıculo en cada uno de sus ejes (alabeo, cabeceo,
guinada) la ecuacion 3.12 puede ser escrita de la siguiente manera:
rn′
= C ′rb′
Donde:
C ′ =
[0 0
0 C
]rb
′=
[0
rb
]
C =
(q2s + q2x − q2y − q2z) 2(qxqy − qsqz) 2(qsqy + qxqz)
2(qsqz + qxqy) (q2s − q2x + q2y − q2z) 2(qyqz − qsqx)
2(qxqz − qsqy) 2(qyqz + qsqx) (q2s − q2x − q2y + q2z)
(3.13)
Vectorialmente es equivalente escribir:
rn = Crb (3.14)
Donde C en la ecuacion 3.14 es equivalente a la matriz de rotacion Cnb . Los angulos de euler obtenidos
a partir del cuaternion se calculan de la siguiente manera utilizando los terminos de la matriz C.
θ = arctan2
(2(qyqz + qsqx)
(q2s − q2x − q2y + q2z)
)(3.15)
φ = arcsen(−2(qxqz − qsqy)) (3.16)
ψ = arctan2
(2(qsqz + qxqy)
(q2s + q2x − q2y − q2z)
)(3.17)
Para situaciones en las que φ se aproxima a π/2 radianes, la ecuaciones para θ y ψ se hacen in-
determinadas por que tanto el numerador como el denominador de esas expresiones se hace cero
simultaneamente. Esta dificultad puede ser superada usando otros elementos de la matriz de cosenos
directores C [35]. Para φ cercano a +π/2:
ψ − θ = arctan2
(c23 − c12c13 + c22
)(3.18)
Para φ cercano a −π/2:
32 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)
ψ + θ = arctan2
(c23 + c12c13 − c22
)(3.19)
En consecuencia ψ o θ debe ser elegido arbitrariamente para satisfacer la ecuacion. Para evitar saltos
en las medidas entre calculos sucesivos se aconseja congelar en su valor actual a uno de los 2 angulos,
por ejemplo, ψ y θ se determina aplicando la ecuacion necesaria. En la siguiente iteracion se congela
θ y se calcula ψ.
3.4. Computacion de la actitud
El enfoque convencional para la determinacion de la actitud es computar la matriz de cosenos di-
rectores Cnb , relacionando el marco de referencia del cuerpo b del vehıculo al sistema de referencia
coordenado, o su cuaternion equivalente, usando un esquema de integracion numerica [25].
0 5 10 15 20 25 30 35 40 45−50
−40
−30
−20
−10
0
10
20
Integración de la medida del giróscopo
Tiempo [Seg]
Án
gu
lo m
ed
ido
[º]
(a) Integracion de la senal del giroscopo.
0 5 10 15 20 25 30 35 40 45−40
−30
−20
−10
0
10
20
Ángulo de inclinación a partir del acelerómetro
Tiempo [Seg]
Án
gu
lo m
ed
ido
[º]
(b) Orientacion a partir del acelerometro.
Figura 3.10: Inconvenientes para el calculo de la orientacion a partir de acelerometros y giroscopos.
Las medidas inerciales juegan un papel importante en la estimacion de la actitud porque proporcionan
informacion suficiente para el calculo de la orientacion del vehıculo. La velocidad angular suministrada
por el giroscopo hace pensar que es posible obtener la representacion de la orientacion integrando
las senales, pero este procedimiento presenta errores a medida que transcurre el tiempo por la deriva
(drift) presente en las medidas. Por tanto, se acumulan por accion de la integracion. Por otra parte,
a partir de las medidas de aceleracion y campo magnetico de la tierra tambien es posible obtener esta
representacion, el principal inconveniente es que es una medida muy ruidosa y de manera adicional
puede ser afectada por fuerzas externas como vibracion. Estas mediciones son acertadas cuando el
vehıculo no presenta desplazamientos, debido a que las aceleraciones lineales son detectadas por el
3.4. COMPUTACION DE LA ACTITUD 33
acelerometro influyendo en la medida (ver seccion 3.4.2). La principal causa de errores en la medicion
realizada por los magnetometros es la presencia de fuentes externas de campo magnetico y materiales
ferromagneticos.
Un analisis de la literatura refleja que los autores resolvieron el problema de la determinacion de la
orientacion usando sensores inerciales, aplicando filtrado Kalman. Por consiguiente, el filtro Kalman
ha sido ampliamente utilizado como algoritmo de fusion de sensores permitiendo corregir las diferentes
imperfecciones presentadas en las mediciones. Actualmente, nuevos enfoques han sido desarrollados
y han probado ser superiores al filtro Kalman extendido. Muchas de estos enfoques mantienen la
estructura basica del EKF , pero emplean varias modifiaciones en orden de proporcionar una mejor
convergencia o mejorar otras caracterısticas de desempeno [38]. Una breve revison del filtro Kalman
se presenta en el apendice A.
3.4.1. Actitud en terminos de cuaterniones
Si q es la actitud o el cuaternion de orientacion, entonces la tasa de cambio del cuaternion de un
cuerpo con velocidad angular esta dada por:
q =1
2Ωk· qk (3.20)
Donde Ωk representa una matrix que contiene las velocidades angulares de cada uno de los ejes en
cada instante de tiempo k:
Ωk =
0 −wx −wy −wzwx 0 wz −wywy −wz 0 wx
wz wy −wx 0
Donde wx, wy, wz son las velocidades angulares en el instante de tiempo k. La funcion que relaciona
el estado actual del cuaternion con el anterior y que es solucion a la ecuacion 3.20 es:
qk+1 =[exp(1
2
∫ tk+1
tk
Ωk· dt)]· qk (3.21)
Por lo tanto:
∫ tk+1
tk
Ωk· dt = Ωk·∆t =
0 −σx −σy −σzσx 0 σz −σyσy −σz 0 σx
σz σy −σx 0
(3.22)
34 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)
Donde ∆t = tk+1 − tk corresponde al tiempo de muestreo. Reescribiendo la ecuacion 3.21:
qk+1 = exp(Ωk∆t
2
)· qk (3.23)
Expandiendo la funcion exponencial de la ecuacion 3.23 por series de taylor alrededor de cero, el
termino exponencial puede ser escrito como [35]:
qk+1 = qk rk (3.24)
Donde rk:
rk =
ac
σx· asσy· asσz· as
(3.25)
ac = cos(σ
2) (3.26)
as =sen(σ2 )
σ(3.27)
σ =√σ2x + σ2
y + σ2z (3.28)
La ecuacion 3.24 tambien puede ser escrita de la siguiente manera [39]:
qk+1 = M(σ)· qk (3.29)
Donde M(σ):
M(σ) = cos(σ
2
)· I4X4 +
sen(σ2 )
σ·Ωk∆t (3.30)
3.4.2. Deteccion de la inclinacion empleando acelerometros
El acelerometro sirve como inclinometro para medir los angulos de alabeo y cabeceo bajo la suposicion
de que el objeto donde el AHRS esta ubicado no se esta moviendo o moviendo a velocidad constante.
Por lo tanto la gravedad es la unica fuente de aceleracion que actua sobre los sensores [40] [41]. Es
3.4. COMPUTACION DE LA ACTITUD 35
decir, que este metodo implica que las aceleraciones observadas en el marco de referencia del cuerpo
b son exclusivamente debidas a la aceleracion gravitacional. De acuerdo a la figura 3.11 las ecuaciones
que determinan los angulos de alabeo θ y cabeceo φ son:
Ay
Ax
zA
X
YZ
X
Y
Z
bb
n
b
n
n
Figura 3.11: Deteccion del alabeo θ y cabeceo φ con el acelerometro.
θ = tan−1(AyAz
)(3.31)
φ = tan−1
Ax√A2y +A2
z
(3.32)
El sensor no debe ser afectado por aceleraciones adicionales. En caso de movimiento del sensor, es
aconsejable que esta medida tenga un menor peso, que no sea asumida como verdadera o sustraer de
las medidas las componentes de aceleracion diferentes a la gravedad. En [42] se propone una manera
de determinar el angulo de guinada ψ o rumbo, pero esta no es muy acertada debido a que carece de
referencia (ejemplo: norte magnetico de la tierra).
3.4.3. Deteccion del rumbo empleando magnetometros
Para determinar el rumbo del vehıculo a partir del magnetometro, la componente vertical del campo
magnetico es ignorada y solo se usan las componentes X y Y , tal como se muestra en la figura 3.12,
por consiguiente el rumbo puede ser calculado a traves de la ecuacion 3.33.
ψ = tan−1(Hy
Hx
)(3.33)
36 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)
x
Y
z
Norte
Este
Abajo
Hx
Hy
Hz
Figura 3.12: Campo magnetico en coordenadas X Y Z.
Generalmente las brujulas estan montadas sobre una aeronave o vehıculo, el cual no siempre se
encuentra en posicion horizontal a la superficie de la tierra. Esta situacion hace que la medida sea
afectada por los angulos de inclinacion, introduciendo un error [33]. Para la solucion del inconveniente
resulta necesario conocer los angulos de alabeo θ y cabeceo φ, con el fin de transformar las medidas del
magnetometro realizadas en el marco de referencia del cuerpo b al marco de referencia de navegacion n,
procedimiento que permite garantizar que las medidas obtenidas se encuentren en un plano horizontal
a la superficie de la tierra.
Hn = Cnb (φ, θ)·Hb =
cosφ 0 −senφ
0 1 0
senφ 0 cosφ
1 0 0
0 cosθ −senθ0 senθ cosθ
Hbx
Hby
Hbz
(3.34)
La matriz de rotacion correspondiente al angulo de cabeceo φ no es la misma a la presentada en la
ecuacion 3.4, debido a que los angulos de euler medidos son positivos de acuerdo al sentido de giro en
el que las medidas del giroscopo son positivas (figura 2.6). Por consiguiente, fue necesario modificar la
matriz de rotacion del angulo de cabeceo φ de manera que se ajuste al sistema de referencia utilizado.
Hnx
Hny
Hnz
=
Hbx· cosφ− senφ· [Hb
y· senθ +Hbz · cosθ]
Hby· cosθ −Hb
z · senθHbx· senφ+ cosφ· [Hb
y· senθ +Hbz · cosθ]
Finalmente, es posible concluir que a pesar de que se requieran las componentes X y Y del campo
magnetico para calcular el rumbo, es necesario disponer de un magnetometro de 3 ejes.
ψ = tan−1
(Hby· cosθ −Hb
z · senθHbx· cosφ− senφ· [Hb
y· senθ +Hbz · cosθ]
)(3.35)
3.4. COMPUTACION DE LA ACTITUD 37
3.4.4. Algoritmo para la computacion de la actitud
El algoritmo implementado para la determinacion de la actitud utiliza como estimador secuencial el
filtro Kalman extendido EKF 2, principalmente por que las relaciones entre las variables a estimar no
son lineales. Para el modelo del proceso se ha establecido que a partir de las medidas del giroscopo se
obtenga una representacion de la orientacion. Asi mismo, las medidas realizadas por el acelerometro y
magnetometro rigen el modelo de la medida. La figura 3.13 ilustra el procedimiento realizado para el
calculo de la orientacion aplicando directamente el EKF [43].
Giróscopo
Acelerómetro yMagnetómetro
Compensación del error
Compensación del error
FiltroKalman
Extendido
Inclinómetroy compás
, ,
w w -
q
f , Hb b
f , Hb b
bias
biasb b
Figura 3.13: Filtro Kalman extendido para la estimacion de la orientacion.
La clave del procedimiento radica en la estimacion del bias presente en las medidas del giroscopo, a
partir de la diferencia existente entre el estado estimado por el modelo del proceso y el de la medida.
En consecuencia el error y el ruido presente en el estado estimado es reducido de manera considerable.
El bloque de compensacion del error en el giroscopo es realizado internamente en el modelo del proceso
en el EKF . La compensacion del error en las medidas del acelerometro y magnetometro consiste en
eliminar el offset en la aceleracion y reducir el efecto producido por campos magneticos externos y
materiales ferromagneticos [33]. El compas hace uso de los angulos de alabeo θ y cabeceo φ obtenidos
en el inclinometro para determinar el correspondiente angulo de guinada ψ del vehıculo. El vector de
estados referente a la descripcion de la orientacion es el siguiente:
xk =
biasx
biasy
biasz
qs
qx
qy
qz
=
x1
x2
x3
x4
x5
x6
x7
(3.36)
2Extended Kalman Filter
38 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)
De acuerdo a lo mencionado anteriormente, la ecucacion 3.36 presenta 3 estados adicionales corres-
pondientes al bias en la medicion del giroscopo. La orientacion es descrita empleando cuaterniones.
El modelo del proceso es representado por la ecuacion 3.23 cuya solucion describe la orientacion del
vehıculo. A diferencia del procedimiento mostrado en la seccion 3.4.1 el termino exponencial de la
ecuacion 3.23 fue aproximado a solo los dos primeros terminos de la serie de taylor alrededor de cero
obteniendo [44] [45]:
qk+1 =(I4x4 +
1
2Ωk∆t
)· qk (3.37)
Esta aproximacion fue realizada por comodidad para facilitar el calculo de la matriz jacobiana A[i,j].
Las senales de control presentadas en el modelo del proceso hacen referencia a las velocidades angulares
medidas, principalmente porque son necesarias para la prediccion del cuaternion. Teniendo en cuenta
la compensacion del error en las medidas de velocidad angular, la funcion no lineal que relaciona el
estado actual con el anterior esta dada por:
x1
x2
x3
x4
x5
x6
x7
k
=
1 0 0 0 0 0 0
0 1 0 0 0 0 0
0 0 1 0 0 0 0
0 0 0 1 − 12k(wx − x1)∆t − 1
2k(wy − x2)∆t − 1
2k(wz − x3)∆t
0 0 0 12k(wx − x1)∆t 1 1
2k(wz − x3)∆t − 1
2k(wy − x2)∆t
0 0 0 12k(wy − x2)∆t − 1
2k(wz − x3)∆t 1 1
2k(wx − x1)∆t
0 0 0 12k(wz − x3)∆t 1
2k(wy − x2)∆t − 1
2k(wx − x1)∆t 1
x1
x2
x3
x4
x5
x6
x7
k−1
Observese que se substrae el bias estimado de la respectiva medida de velocidad angular para obtener
el valor real de la medida del giroscopo. Las unidades para los estados x1, x2 y x3 es [o/seg] para los
demas estados es [rad]. k = π/180 es el factor de conversion de grados a radianes para los estados
correspondientes al cuaternion. Despues de la multiplicacion matricial se obtiene:
x1
x2
x3
x4
x5
x6
x7
k
= f(xk−1, uk−1, 0) =
x1
x2
x3
x4 − 12k(wx − x1)∆t·x5 − 1
2k(wy − x2)∆t·x6 − 1
2k(wz − x3)∆t·x7
12k(wx − x1)∆t·x4 + x5 + 1
2k(wz − x3)∆t·x6 − 1
2k(wy − x2)∆t·x7
12k(wy − x2)∆t·x4 − 1
2k(wz − x3)∆t·x5 + x6 + 1
2k(wx − x1)∆t·x7
12k(wz − x3)∆t·x4 + 1
2k(wy − x2)∆t·x5 − 1
2k(wx − x1)∆t·x6 + x7
k−1
La matriz jacobiana A[i,j] de las derivadas parciales de f con respecto a cada estado x es:
A[i,j] =∂f[i]
∂x[j](xk−1, uk−1, 0) =
3.4. COMPUTACION DE LA ACTITUD 39
1 0 0 0 0 0 0
0 1 0 0 0 0 0
0 0 1 0 0 0 012k∆t·x5
12k∆t·x6
12k∆t·x7 1 − 1
2k(wx − x1)∆t − 1
2k(wy − x2)∆t − 1
2k(wz − x3)∆t
− 12k∆t·x4
12k∆t·x7 − 1
2k∆t·x6
12k(wx − x1)∆t 1 1
2k(wz − x3)∆t − 1
2k(wy − x2)∆t
− 12k∆t·x7 − 1
2k∆t·x4
12k∆t·x5
12k(wy − x2)∆t − 1
2k(wz − x3)∆t 1 1
2k(wx − x1)∆t
12k∆t·x6 − 1
2k∆t·x5 − 1
2k∆t·x4
12k(wz − x3)∆t 1
2k(wy − x2)∆t − 1
2k(wx − x1)∆t 1
Los terminos de la matriz A[i,j] para los estados correspondientes al bias del giroscopo no son cero,
indicando que se va a tener un comportamiento dinamico, es decir, variante con el tiempo.
La matriz Q que simboliza el ruido en el proceso depende del ruido presente en las mediciones
realizadas por el giroscopo. Esta afirmacion esta basada en que el proceso esta regido por las mediciones
de velocidad angular. La tabla 2.4 presenta los momentos estadısticos del giroscopo. Por tanto, es
posible concluir que la media calculada en cada eje corresponde al bias, debido a que el sensor no debe
indicar ninguna velocidad angular cuando este se encuentra en reposo. En consecuencia, la desviacion
estandar en cada eje representa el ruido presente en los estados x1, x2 y x3. Asi mismo, se asume que
la desviacion estandar tambien representa el ruido obtenido en cada medida realizada en el giroscopo
para cada uno de sus ejes. Con base en la tabla 2.4 se obtiene los siguiente:
σ2wx
= 0,0904 [o/seg]2
σ2wy
= 0,3993 [o/seg]2
σ2wz
= 0,1924 [o/seg]2
Debido a que el calculo de los demas estados hace uso de todas las medidas de velocidad angular se
establece que:
1
σ2gyro
=1
σ2wx
+1
σ2wy
+1
σ2wz
= 18,7638 [seg/o]2 (3.38)
Entonces Q esta definida como:
Q =
σ2wx
0 0 0 0 0 0
0 σ2wy
0 0 0 0 0
0 0 σ2wz
0 0 0 0
0 0 0 σ2gyro 0 0 0
0 0 0 0 σ2gyro 0 0
0 0 0 0 0 σ2gyro 0
0 0 0 0 0 0 σ2gyro
(3.39)
40 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)
Como en el proceso para el calculo del cuaternion las unidades usadas son los radianes, entonces es
necesario realizar una conversion a [rad/seg]2 para el termino σ2gyro. La matriz Q es:
Q =
0,0904 0 0 0 0 0 0
0 0,3993 0 0 0 0 0
0 0 0,1924 0 0 0 0
0 0 0 1,6236e−5 0 0 0
0 0 0 0 1,6236e−5 0 0
0 0 0 0 0 1,6236e−5 0
0 0 0 0 0 0 1,6236e−5
La matriz W corresponde a la matriz identidad W[i,j] = I7X7.
El calculo de la orientacion con acelerometros y magnetometro no presenta deriva (drift) en sus
medidas, por consiguiente la innovacion en el filtro Kalman permite estimar el bias en la medida del
giroscopo gracias a que provienen de diferentes fuentes de informacion. El modelo de la medida es el
siguiente:
h(xk, 0) =
θ
φ
ψ
=
arctan2(2x6x7 + 2x4x5, (x
24 − x25 − x26 + x27))
arcsen(2x4x6 − 2x5x7)
arctan2(2x5x6 + 2x4x7, (x24 + x25 − x26 − x27))
(3.40)
La funcion h(xk) es la encargada de calcular la orientacion de la camara a partir de los estados
estimados, para su posterior comparacion con los datos medidos. Los angulos φ, θ, ψ (alabeo, cabeceo
y guinada) correspondientes a las medidas realizadas, son calculados con las ecuaciones 3.31, 3.32 y
3.35 con la ayuda del acelerometro y el magnetometro.
θ = tan−1(AyAz
)
φ = tan−1
Ax√A2y +A2
z
ψ = tan−1
(Hby· cosθ −Hb
z · senθHbx· cosφ− senφ· [Hb
y· senθ +Hbz · cosθ]
)
La matriz jacobiana H[i,j] de las derivadas parciales de h(xk, 0) con respecto a cada estado x es:
H[i,j] =∂h[i]
∂x[j](xk, 0)
3.4. COMPUTACION DE LA ACTITUD 41
H[i,j] =
0 0 0 2x5·c33−2x4·c32
c233+c232
2x4·c33+2x5·c32c233+c
232
2x7·c33+2x6·c32c233+c
232
2x6·c33−2x7·c32c233+c
232
0 0 0 2x6√1−c231
−2x7√1−c231
2x4√1−c231
−2x5√1−c231
0 0 0 2x7·c11−2x4·c21c211+c
221
2x5·c11−2x6·c21c211+c
221
2x5·c11+2x6·c21c211+c
221
2x4·c11+2x7·c21c211+c
221
(3.41)
Donde las variables cij son terminos de la matriz de cosenos directores de la ecuacion 3.13.
c33 = x24 − x25 − x26 + x27
c32 = 2x6x7 + 2x4x5
c31 = 2x4x6 − 2x5x7
c11 = x24 + x25 − x26 − x27c21 = 2x5x6 + 2x4x7
Los datos obtenidos en la tabla 2.5 no pueden ser usados para determinar el ruido en la medicion,
pero si proporciona la medida del offset del aceleremetro que necesariamente debe ser sustraido a la
medida de aceleracion (bloque de compensacion del error). Con el objetivo de obtener el verdadero
valor de medida e incurrir en menos error cuando se calcule el correspondiente angulo usando el in-
clinometro y el compas.
Para calcular el ruido en la medicion se realizo basicamente el mismo procedimiento para el calculo
de los momentos estadısticos de los sensores inerciales, pero en este caso se midieron los angulos cuando
el sensor se encontraba en reposo utilizando las ecuaciones de medicion 3.31, 3.32 y 3.35. Los resultados
se ilustran en la tabla 3.1.
TABLA 3.1: Momentos estadısticos de los angulos medidos
Parametro θ φ ψ Unidades
m 0.8195 -1.4735 -120.1920 o
σ 0.0640 0.0770 1.3385 o
Despues del cambio de unidades de [o] a [rad], la matriz R puede ser definida como:
R =
σ2θ 0 0
0 σ2φ 0
0 0 σ2ψ
=
1,2461e−6 0 0
0 1,8057e−6 0
0 0 5,4578e−4
(3.42)
La matriz V corresponde a la matriz identidad V[i,j] = I3X3.
42 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)
3.5. Implementacion del algoritmo para la computacion de la
actitud
El algoritmo presentado en la seccion 3.4.4 fue implementado en MATLAB usando las medidas
inerciales proporcionadas por el iNEMO. En primer lugar para comprobar que las estimaciones del
bias que afecta en el giroscopo son acertadas se realizo una prueba en reposo. El resultado esperado es
que la estimacion de dichos estados sean similares a los obtenidos experimentalmente. Los resultados
son presentados en la figura 3.14.
0 2 4 6 8 10 12 14 16 18 20−6
−5
−4
−3
−2
−1
0
1
Bias Estimado
Tiempo [seg]
Velo
cid
ad
An
gu
lar
[º/s
eg
]
biasWx
biasWy
biasWz
Figura 3.14: Estimacion del bias presente en las medidas del giroscopo.
La comparacion de los resultados obtenidos en la grafica 3.14 y tabla 2.4 refleja la convergencia del
filtro y se comprueba su correcto funcionamiento bajo las mismas condiciones. La estimacion obtenida
para el bias en el eje z biasWz es negativa por que el sentido de giro establecido para este eje como
positivo es contrario al presentado en la figura 2.6.
Para demostrar la consistencia del filtro Kalman Bar-Shalom [46] propuso los siguientes criterios:
El estado estimado xk/k no presenta bias, ejemplo: el error del estado estimado es de media cero.
La covarianza del estado estimado, Pk/k, concuerda con la covarianza del error del estado.
La innovacion zk −H · xk/k−1, es un vector de ruido blanco aleatorio, ejemplo: de media cero y
no correlacionado.
3.5. IMPLEMENTACION DEL ALGORITMO PARA LA COMPUTACION DE LAACTITUD 43
0 2 4 6 8 10 12 14 16 18 20−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Innovación para el ángulo de Alabeo
Tiempo [seg]
Err
or
[º]
(a) Innovacion para el angulo de alabeo θ.
0 2 4 6 8 10 12 14 16 18 20−1.5
−1
−0.5
0
0.5
1
Innovación para el ángulo de Cabeceo
Tiempo [seg]
Err
or
[º]
(b) Innovacion para el angulo del cabeceo φ.
0 2 4 6 8 10 12 14 16 18 20−120
−100
−80
−60
−40
−20
0
20
Innovación para el ángulo de Guiñada
Tiempo [seg]
Err
or
[º]
(c) Innovacion para el angulo de guinada ψ.
0 2 4 6 8 10 12 14 16 18 200
0.2
0.4
0.6
0.8
1
1.2x 10
−3 Traza de la Matriz de Covarianza del Error
Tiempo [seg]
Co
vari
an
za d
el E
rro
r [r
ad
2]
(d) Covarianza del error.
Figura 3.15: Consistencia del filtro Kalman extendido para la orientacion.
La innovacion para cada uno de los angulos de Euler se asemeja a un ruido blanco de media cero. Por
otra parte, la covarianza del error Pk/k presenta un comportamiento decreciente comprobando la con-
vergencia del filtro. La covarianza del error Pk/k corresponde a la suma de los errores de cada termino
del cuaternion. Para un experimento en el cual se hicieron los tres movimientos de manera consecutiva
en cada uno de sus ejes, las mediciones realizadas con el AHRS elaborado fueron comparadas con el
sistema de descripcion de la orientacion que dispone el modulo inercial iNEMO.
Los resultados reflejan un mismo comportamiento en los dos sistemas pero se observa un offset
entre las medidas atribuido a las condiciones establecidas como referencia. Para el caso de la figura
3.16(c) es mas facil detectar que los dos sistemas tienen su propia referencia y no es la misma. El error
medio cuadratico calculado asumiendo como verdadera la medida proporcionada por el iNEMO y
44 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)
0 5 10 15 20 25 30 35 40 45−40
−30
−20
−10
0
10
20
Ángulo de Alabeo
Tiempo [seg]
Án
gu
lo m
ed
ido
[º]
iNEMO
AHRS
(a) Resultados para el angulo de alabeo θ.
0 5 10 15 20 25 30 35 40 45−30
−20
−10
0
10
20
30
40
Ángulo de Cabeceo
Tiempo [seg]
Án
gu
lo m
ed
ido
[º]
iNEMO
AHRS
(b) Resultados para el angulo del cabeceo φ.
0 5 10 15 20 25 30 35 40 45−140
−120
−100
−80
−60
−40
−20
0
20
Ángulo de Guiñada
Tiempo [seg]
Án
gu
lo m
ed
ido
[º]
iNEMO
AHRS
(c) Resultados para el angulo de guinada ψ.
Figura 3.16: Comparacion del iNEMO con el AHRS.
TABLA 3.2: Error medio cuadraticoAngulo Error Unidades
θ 1.1670 o
φ 1.0858 o
ψ 3.7582 o
estableciendo la misma referencia para las dos medidas se muestra en la tabla 3.2.
Capıtulo 4
ALGORITMOS DE FUSION
La teorıa de la probabilidad fue utilizada durante mucho tiempo para hacer frente a casi todo tipo de
informacion imperfecta, porque era la unica teorıa existente. Como resultado, las tecnicas probabilısti-
cas tales como la red basada en modelos, filtrado Kalman y la simulacion Monte Carlo secuencial han
sido las tecnicas de fusion de datos mas comunes. Las tecnicas alternativas como el calculo de inter-
valos, la logica difusa y razonamiento probatorio se han propuesto para hacer frente a las limitaciones
observadas en los metodos probabilısticos tales como la complejidad, la incoherencia, la precision de los
modelos y la incertidumbre. Recientemente, algunos investigadores tambien han tratado de resolver la
fusion de datos dentro de un marco de optimizacion. Por ultimo, hay metodos hıbridos que combinan
la fusion de varios enfoques para desarrollar un algoritmo de meta-fusion [48].
El proposito de la fusion de datos es producir un modelo o representacion de un sistema a partir
de un conjunto independiente de fuentes de datos. La combinacion de la informacion de los sensores
y posterior estimacion del estado del entorno debe hacerse de forma coherente consistente, de manera
tal que la incertidumbre sea reducida. Por consiguiente, los algoritmos deben ser capaces de tratar con
multiples observaciones, multiples sensores y multiples objetivos.
4.1. Modelos de fusion
El filtro Kalman es el algoritmo recursivo mas conocido y mas ampliamente aplicado en fusion de
sensores, capaz de calcular de manera optima el estado desconocido de un sistema dinamico lineal de
observaciones ruidosas. El algoritmo utiliza un modelo predefinido del sistema para predecir el estado
en el proximo paso del tiempo. A esto se anade un componente para actualizar los errores en el modelo
con las observaciones reales del sistema. La prediccion y la actualizacion se combinan con la ganancia
de Kalman que se calcula para minimizar el error cuadratico medio de la estimacion del estado.
En el modelo de fusion que aplica filtrado Kalman la dinamica del objetivo esta gobernada por la
ecuacion 4.1. Por simplicidad se asume que la frecuencias de muestreo de los sensores son identicas.
xk+1 = Fkxk +Gkwk (4.1)
45
46 CAPITULO 4. ALGORITMOS DE FUSION
Donde xk es el vector de estado en el instante de tiempo k, y wk−1 es una variable aleatoria que
representa el ruido del proceso, blanco y con una distribucion de probabilidad normal.
E[wk] = 0; E[wkwTk ] = Qk
La diferencia radica en la ecuacion de medida, ya que al incluir multiples sensores esta puede ser
descrita de la siguiente manera:
zmk = Hmk xk + vmk , m = 1, 2, · · · , n (4.2)
Donde zmk es la medida del sensor m en el instante de tiempo k, y el ruido en las medidas vmk son de
media cero, blancos, con covarianza Rmk y mutuamente independientes.
E[vmk ] = 0; E[vmk vmkT ] = Rmk
E[v1kv2kT
] = E[v2kv1kT
] = 0
Establecido este modelo, la fusion ahora puede tener lugar al nivel del vector de estado o a nivel del
vector de medicion. La fusion del vector de estados y la fusion de medidas son los tipos de metodos
convencionales para la fusion de datos basados en el filtro Kalman [49], y la fusion de medidas presenta
un menor error de estimacion, pero un mayor costo computacional. Este procedimiento puede ser
extendido para las diferentes variaciones del filtro Kalman (ejemplo: filtro Kalman extendido).
4.1.1. Modelos de fusion de medidas
Predicción Correcciónxk|k-1^ xk|k
z-1
xk-1|k-1
Filtro Kalman
Fusión
zk
zkzk2 1
^ ^
Figura 4.1: El proceso de fusion de las medidas.
4.1. MODELOS DE FUSION 47
Hay esencialmente dos metodos para la fusion de las medidas. La primera simplemente es la mezcla de
las mediciones en un vector de observacion aumentado y el segundo combina las mediciones utilizando
estimaciones de mınima media cudratica [50]. El esquema general del proceso de fusion de las medidas
se ilustra en la figura 4.1.
4.1.1.1. Aumento del vector de observacion
En esta tecnica de fusion de medidas, los vectores de medida z1k y z2k de dos o mas sensores son
fusionados dentro de un nuevo vector de medida aumentado:
zk = [(z1k)T (z2k)T ]T (4.3)
En consecuencia Hk = [(H1k)T (H2
k)T ]T y vk = [(v1k)T (v2k)T ]T tambien son modificadas, permitiendo
establecer la nueva ecuacion de medida:
zk = Hkxk + vk (4.4)
Basados en la suposicion de que los dos sensores son estadısticamente independientes la matriz de
covarianza Rk para el ruido en la medida fusionado vk es definido como:
Rk =
(R1k 0
0 R2k
)
Por ultimo la estimacion del vector de estado xk/k puede ser determinado aplicando el filtro Kalman
convencional.
4.1.1.2. Estimacion de mınima media cuadratica
Este enfoque para la fusion de las medidas consiste en ponderar las medidas individuales de cada
sensor y luego seguir la pista de esas medidas fusionadas por un filtro Kalman para obtener una
estimacion del vector de estados [51]. Dado que el ruido en la medida es independiente para los dos
sensores, la ecuacion para la fusion de los vectores de medida z1k y z2k, en forma recursiva, para una
estimacion de mınima media cuadratica esta dada por:
zk = z1k +R1k(R1
k +R2k)−1(z2k − z1k) (4.5)
Donde Rmk es la matriz de covarianza del vector de medida del sensorm. Luego la matriz de covarianza
de las medidas fusionadas zk puede ser obtenida de la siguiente manera:
Rk = [(R1k)−1 + (R2
k)−1]−1 (4.6)
48 CAPITULO 4. ALGORITMOS DE FUSION
Estas medidas fusionadas pueden ser seguidas para obtener el vector de estados estimado usando el
filtro Kalman convencional.
Estos algoritmos son matematicamente equivalentes a el filtro Kalman con un sensor fusionado. Un
hecho interesante es que los dos metodos de fusion son funcionalmente equivalentes uno con el otro
cuando las matrices de medida son iguales H1k = H2
k y es independiente del caracterıstico ruido de
estado Qk [50][52].
4.1.2. Modelos de fusion track to track
En muchas situaciones practicas el entorno es observado por una variedad de sensores donde cada
sensor procesa sus propias mediciones y las mantiene por separado, una pregunta importante es como
decidir si dos mediciones procedentes de diferentes sensores representan el mismo objetivo. Este es
el problema de track to track association o probelma de correlacion. El problema de track to track
association y la subsecuente fusion fue discutida bajo la suposicion que los errores de estimacion
en dos archivos con observaciones correspondientes al mismo objetivo y provenientes de diferentes
sensores, son independientes. Pero en [53] se demostro que dichos errores estan correlacionados porque
el ruido del proceso del objetivo esta presente en los dos errores de estimacion cuando las estimaciones
tienen el mismo origen. El proceso de decision involucrado en la combinacion (fusion) de las mediciones
para obtener un unico objetivo que tenga menor incertidumbre que los obtenidos individualmente, es
conocido como track to track fusion problem (que tambien proporciona una compresion de datos).
4.1.2.1. Algoritmo de fusion track to track
Uno de los metodos de fusion frecuentemente usado es el llamado algoritmo de fusion track to track
que fue propuesto por Bar-Shalom et al [54] [55] y su diagrama de bloques es ilustrado en la figura
4.2. En este metodo se fusionan las estimaciones del estado x1k|k y x2k|k correspondientes al sensor 1 y
2, respectivamente, en una nueva estimacion del vector de estado. La nueva estimacion del vector de
estado xk|k se genera a partir de la siguiente ecuacion de estimacion lineal estatica:
xk|k = x1k|k + [P 1k|k − P 12
k|k] · [P 1k|k + P 2
k|k − P 12k|k − P 21
k|k]−1 · (x2k|k − x1k|k) (4.7)
Pmk|k es la matriz de covarianza para la estimacion xmk|k basada en la medida del sensor m (m = 1, 2),
y P 12k|k = (P 21
k|k)T es la matriz de covarianza cruzada entre x1k|k y x2k|k. La matriz de covarianza para
las estimaciones fusionadas se calcula utilizando:
Pk|k = P 1k|k − [P 1
k|k − P 12k|k] · [P 1
k|k + P 2k|k − P 12
k|k − P 21k|k]−1 · [P 1
k|k − P 21k|k] (4.8)
4.1. MODELOS DE FUSION 49
Predicción Correcciónxk|k-1^ xk|k
z-1
xk-1|k-1
Filtro Kalman
FUSIÓN^ ^
Predicción Correcciónxk|k-1^ xk|k
z-1
xk-1|k-1^ ^1 1 1
2 2 2
xk|k^
Figura 4.2: Fusion track to track.
La matriz de covarianza cruzada P 12k|k puede ser calculada de manera recursiva a partir de las matrices
de observacion y las ganancias del filtro Kalman aplicando la siguiente ecuacion:
P 12k|k = (I −K1
kH1k)Fk−1P
12k−1|k−1F
Tk−1(I −K2
kH2k)T + (I −K1
kH1k)Gk−1Q
12k−1G
Tk−1(I −K2
kH2k)T (4.9)
Sin embargo, hay ocasiones en donde las matrices H, K, F y Q no son conocidas (sensores indus-
triales). Para este caso una manera alternativa de calcular P 12k|k es propuesta en [56], donde la matriz de
covarianza cruzada es aproximada por el producto de Hadamard entre las dos matrices de covarianzas.
P 12k|k = ρ ·
√P 1k|k · P 2
k|k (4.10)
Donde ρ representa un coeficiente de correlacion efectivo. Este puede ser determinado numerica-
mente por una simulacion Monte Carlo para una configuracion especıfica; en [56] el valor de ρ = 0,4
fue encontrado como una buena aproximacion para situaciones tıpicas de seguimiento.
En la ausencia de dependencia P 12k|k = 0, o cuando la covarianza cruzada puede ser ignorada, el
algoritmo de fusion esta dado por las siguientes ecuaciones y se conoce como la combinacion convexa
basica:
xk|k = x1k|k + P 1k|k · [P 1
k|k + P 2k|k]−1 · (x2k|k − x1k|k) (4.11)
Pk|k = P 1k|k − P 1
k|k · [P 1k|k + P 2
k|k]−1 · P 1k|k (4.12)
50 CAPITULO 4. ALGORITMOS DE FUSION
4.1.2.2. Algoritmo de fusion modificado track to track
El modified track to track fusion (MTF ) es otro algoritmo de fusion que esta basado en la retroali-
mentacion de la estimacion del estado fusionado xk|k a la etapa de prediccion filtro Kalman en vez de
retroalimentar las estimaciones individuales x1k|k y x2k|k. La idea es que el procedimiento de prediccion
del filtro Kalman sea realizado por un unico bloque y mejorado con la inclusion de la estimacion del
estado fusionado (ver figura 4.3).
Predicción
Correcciónxk|k
z-1
FUSIÓN^
Corrección
xk|k-1^
xk|k
xk-1|k-1^
^1
2
xk|k^
Filtro Kalman
Figura 4.3: Fusion modificada track to track.
La nueva estimacion fusionada xk|k y su matriz de covarianza Pk|k estan dadas por las ecuaciones
4.7 y 4.8, respectivamente. El algoritmo de fusion modificado track to track basado en el filtro Kalman
tiene la siguiente estructura:
xk|k−1 = Fkxk−1|k−1
Pk|k−1 = FkPk−1|k−1FTk +GkQkG
Tk
Kmk = Pk|k−1(Hm
k )T [Hmk Pk|k−1(Hm
k )T +Rmk ]−1 m = 1, 2
xmk|k = xk|k−1 +Kmk [zmk −Hm
k xk|k−1] m = 1, 2
Pmk|k = [I −Kmk H
mk ]Pk|k−1 m = 1, 2
P 12k|k = (P 21
k|k)T = [I −K1kH
1k ]Pk|k−1[I −K2
kH2k ]T
Al final se obtiene la estimacion del estado fusionado necesario para el siguiente ciclo.
4.1. MODELOS DE FUSION 51
xk|k = x1k|k + [P 1k|k − P 12
k|k] · [P 1k|k + P 2
k|k − P 12k|k − P 21
k|k]−1 · (x2k|k − x1k|k)
Pk|k = P 1k|k − [P 1
k|k − P 12k|k] · [P 1
k|k + P 2k|k − P 12
k|k − P 21k|k]−1 · [P 1
k|k − P 21k|k]
El algoritmo anterior es similar en estructura al algoritmo de fusion track to track, pero con diferentes
ganancias de Kalman Kmk . Sin embargo, el costo computacional del algoritmo es identico al algoritmo
de fusion track to track.
52 CAPITULO 4. ALGORITMOS DE FUSION
Capıtulo 5
RESULTADOS EXPERIMENTALES
Para la aplicacion de los modelos de fusion de medidas se cuenta con dos sistemas independientes
uno del otro. El primero es el sistema SLAM basado en vision y el otro es el sistema de referencia
de actitud y rumbo (AHRS), los cuales entregan diferentes mediciones de los mismos estados. Una
ventaja es que los dos sistemas usan el filtro Kalman como estimador secuencial, facilitando la imple-
mentacion de las tecnicas de fusion mencionadas anteriormente.
La implementacion de los modelos de fusion fue realizada en MATLAB de manera off − line por
las siguientes razones:
Desincronizacion. Es decir, los dos sistemas no presentan la misma frecuencia de muestreo, adi-
cionalmente, la frecuencia de muestreo del sistema SLAM es variante en el tiempo (depende del
tiempo en que tarde en procesar la imagen).
No se tiene acceso al sistema SLAM . Se requiere un conocimiento apropiado del lenguaje de
programacion en el que el sistema esta elaborado para realizar cualquier tipo de modificacion.
Aunque los incovenientes anteriores son solucionables la idea de este trabajo es presentar resulta-
dos preliminares de la aplicacion de esta metodologıa. La fusion solo fue realizada para determinar la
orientacion del vehıculo la cual utilizo como base el modelo de prediccion descrito para la elaboracion
del AHRS. La prediccion y medicion de los estados a estimar es mucho mas sencilla para el AHRS
en comparacion con el sistema SLAM basado en vision. Se maneja menos informacion (en el SLAM
se tiene los estados de la camara y de las caracterısticas visuales). Por tanto, el costo computacional
es menor (menos operaciones).
El experimento consistio en acoplar el modulo inercial iNEMO a la camara monocular que sirve
como sensor del entorno del sistema SLAM de manera tal que ambos elementos originara un unico
dispositivo. Los dos sistemas fueron encendidos al mismo tiempo y se procedio a realizar los movimien-
to de cabeceo φ, alabeo θ y guinada ψ que describen la orientacion de la plataforma movil. A medida
que transcurrıa la prueba los datos propocionados por ambos sistemas (SLAM e iNEMO) fueron
almacenadas en 2 laptops distintas. La informacion adquirida del SLAM fue la estimacion de la ori-
entacion, su covarianza del error y un vector de tiempo. Por su parte, el modulo inercial suministro las
todas las medidas inerciales y su vector de tiempo.
53
54 CAPITULO 5. RESULTADOS EXPERIMENTALES
En MATLAB, a partir de las mediciones inerciales se estimaba la orientacion y su covarianza del
error muestra a muestra utilizando el modulo AHRS descrito en secciones anteriores. Aprovechando
que la frecuencia de los sensores inerciales es periodica, una manera de evadir el problema de desin-
cronizacion fue utilizar un retenedor para el SLAM , es decir, mantener la muestra el tiempo necesario
hasta que haya una actualizacion. De esa manera se garantiza que existen las dos medidas de los dos
sistemas para hacer las respectivas correcciones. Luego de obtener las dos mediciones se aplicaba los
diferentes modelos de fusion de medidas.
SLAM
iNEMO
Modelo deFusión de Medidas
AHRS
, ,
zk
zk2
1
Figura 5.1: Esquema general de implementacion de la fusion de medidas.
5.1. Modelo de fusion de las medidas
Para la estimacion de la orientacion, el modelo de prediccion es el mismo al presentado en la seccion
3.4.4, conservando la estructura de la funcion no lineal que relaciona el estado actual con el anterior
f(xk−1, uk−1, 0) y las matrices A[i,j] y Q. Al ser el mismo modelo de prediccion, se tiene que Hk =
[(HAHRSk )T (HAHRS
k )T ]T , el vector de medida y la matriz de covarianza Rk para el ruido en la medida
fusionados es establecido como:
zk = [(zAHRSk )T (zSLAMk )T ]T (5.1)
Rk =
(RAHRSk 0
0 RSLAMk
)(5.2)
Para el ruido en la medida del sistema SLAM se realizaron unas medidas del angulo cuando se
encontraba en reposo, mientras tanto el ruido en la medida para el AHRS fue el mismo. Los resultados
son ilustrados en la figura 5.2.
TABLA 5.1: Momentos estadısticos de los angulos medidos (SLAM)
Parametro θ φ ψ Unidades
m 0.2625 -0.3539 -0.0856 o
σ 0.1788 0.1247 0.0427 o
5.1. MODELO DE FUSION DE LAS MEDIDAS 55
0 5 10 15 20 25 30 35 40 45−40
−30
−20
−10
0
10
20
Ángulo de Alabeo
Tiempo [seg]
Gra
do
s [
º]
SLAM
AHRS
iNEMO
Fusión
(a) Angulo de alabeo θ.
0 5 10 15 20 25 30 35 40 45−30
−20
−10
0
10
20
30
40
Ángulo de Cabeceo
Tiempo [seg]
Gra
do
s [
º]
SLAM
AHRS
iNEMO
Fusión
(b) Angulo del cabeceo φ.
0 5 10 15 20 25 30 35 40 45−140
−120
−100
−80
−60
−40
−20
0
20
Ángulo de Guiñada
Tiempo [seg]
Gra
do
s [
º]
SLAM
AHRS
iNEMO
Fusión
(c) Angulo de guinada ψ.
0 5 10 15 20 25 30 35 40 450
0.2
0.4
0.6
0.8
1
1.2
1.4x 10
−3 Traza de la Matriz de Covarianza del Error
Tiempo [seg]
Err
or
[rad
2]
AHRS
SLAM
Fusión
(d) Covarianza del error.
Figura 5.2: Resultados del modelo de fusion de medidas.
Aunque el sistema AHRS tiene menor error en comparacion con el sistema SLAM , la fusion refleja
que este parametro no tiene ningun efecto en el resultado. Como se observa en la grafica 5.2, en
ocasiones la medida seguida es la del sistema SLAM y en otras sigue el comportamiento del AHRS.
La tendencia es a seguir la curva que ofrezca un menor error de correccion. La media del error obtenido
de la grafica 5.2(d) se presenta en la tabla 5.2.
56 CAPITULO 5. RESULTADOS EXPERIMENTALES
TABLA 5.2: Error medio en el aumento del vector de observacionAngulo Error Unidades
SLAM 3,2344e−4 rad2
AHRS 2,1029e−5 rad2
Fusion 1,3095e−5 rad2
Como era de esperarse el error cuando los datos son fusionados es reducido. El factor de reduccion
del error es aproximadamente 25 veces el error del sistema SLAM . Al aplicar la tecnica de fusion de
medidas de mınima media cuadratica los resultados son similares a los mostrados en la figura 5.2 y
la media del error cambia muy poco con respecto a la presentada en la tabla 5.2, comprobando la
similitud matematica existente entre los dos modelos cuando H1k = H2
k .
TABLA 5.3: Error medio en la estimacion de mınima media cuadraticaAngulo Error Unidades
SLAM 3,2344e−4 rad2
AHRS 2,1029e−5 rad2
Fusion 1,3213e−5 rad2
5.2. Fusion track to track
Aunque se dispone de las estimaciones de los estados xmk|k y su correspondiente matriz de covarianzas
en los dos sistemas Pmk|k, es muy difıcil obtener la informacion correspondiente a la matriz de obser-
vacion H y la matriz de la ganancia del filtro Kalman K para el sistema SLAM basado en vision.
Por lo tanto se recurrio la ecuacion 4.10 para establecer la matriz de covarianzas cruzadas P 12k|k. Se
aplicaron las ecuaciones 4.7 y 4.8 para la fusion de los estados.
En los resultados, se observa una tendencia a seguir, que es el compartamiento de la curva que
presenta menor error en la estimacion, pero esta tiene en cuenta las dos mediciones realizadas por los
sistemas.
TABLA 5.4: Error medio en la fusion track to trackAngulo Error Unidades
SLAM 3,2344e−4 rad2
AHRS 2,1029e−5 rad2
Fusion 1,9540e−5 rad2
5.2. FUSION TRACK TO TRACK 57
0 5 10 15 20 25 30 35 40 45−40
−30
−20
−10
0
10
20
Ángulo de Alabeo
Tiempo [seg]
Gra
do
s [
º]
SLAM
AHRS
iNEMO
Fusión
(a) Angulo de alabeo θ.
0 5 10 15 20 25 30 35 40 45−30
−20
−10
0
10
20
30
40
Ángulo de Cabeceo
Tiempo [seg]
Gra
do
s [
º]
SLAM
AHRS
iNEMO
Fusión
(b) Angulo del cabeceo φ.
0 5 10 15 20 25 30 35 40 45−140
−120
−100
−80
−60
−40
−20
0
20
Ángulo de Guiñada
Tiempo [seg]
Gra
do
s [
º]
SLAM
AHRS
iNEMO
Fusión
(c) Angulo de guinada ψ.
0 5 10 15 20 25 30 35 40 450
0.2
0.4
0.6
0.8
1
1.2
1.4x 10
−3 Traza de la matriz de covarianza
Tiempo [seg]
Err
or
[rad
2]
SLAM
AHRS
Fusión
(d) Covarianza del error.
Figura 5.3: Resultados de la fusion track to track.
La matriz de covarianza del AHRS es diagonal por lo que el resultado de la matriz de covarianzas
cruzada tambien lo sera. Esta consideracion implica que la matriz P 12k|k puede ser ignorada y no ten-
dra un efecto relevante a la hora de aplicar las ecuaciones 4.7 y 4.8. Los resultados obtenidos ignorando
la matriz de covarianzas cruzadas son muy similares a los presentados en la figura 5.3, pero el error
sı es reducido.
58 CAPITULO 5. RESULTADOS EXPERIMENTALES
0 5 10 15 20 25 30 35 40 450
0.2
0.4
0.6
0.8
1
1.2
1.4x 10
−3 Traza de la matriz de covarianza
Tiempo [seg]
Err
or
[ra
d2]
SLAM
AHRS
Fusión
Figura 5.4: Covarianza del error para la fusion track to track ignorando P 12k|k.
TABLA 5.5: Error medio en la fusion track to track ignorando P 12k|k
Angulo Error Unidades
SLAM 3,2344e−4 rad2
AHRS 2,1029e−5 rad2
Fusion 1,7874e−5 rad2
5.3. Fusion modificada track to track
El procedimiento es muy parecido al realizado en el modelo de fusion de medidas, solo que para este
caso se hacen dos correcciones de manera independiente con las mediciones de los 2 sistemas y luego
son fusionadas para ser realimentados. Los resultados son los siguientes:
TABLA 5.6: Error medio en la fusion modificada track to trackAngulo Error Unidades
SLAM 3,2344e−4 rad2
AHRS 2,1029e−5 rad2
Fusion 1,2697e−5 rad2
Este metodo arroja mejores resultados en comparacion con los demas esquemas de fusion gracias a la
realimentacion de los estados estimados y que la etapa de prediccion es la misma para los dos sistemas.
Esta configuracion permite que el error pueda ser calculado a partir de su valor anterior en cada
instante de tiempo k. En resumen los errorres obtenidos en cada metodo para el mismo experimento
son los siguientes:
5.3. FUSION MODIFICADA TRACK TO TRACK 59
0 5 10 15 20 25 30 35 40 45−40
−30
−20
−10
0
10
20
Ángulo de Alabeo
Tiempo [seg]
Gra
do
s [
º]
SLAM
AHRS
iNEMO
Fusión
(a) Angulo de alabeo θ.
0 5 10 15 20 25 30 35 40 45−30
−20
−10
0
10
20
30
40
Ángulo de Cabeceo
Tiempo [seg]
Gra
do
s [
º]
SLAM
AHRS
iNEMO
Fusión
(b) Angulo del cabeceo φ.
0 5 10 15 20 25 30 35 40 45−140
−120
−100
−80
−60
−40
−20
0
20
Ángulo de Guiñada
Tiempo [seg]
Gra
do
s [
º]
SLAM
AHRS
iNEMO
Fusión
(c) Angulo de guinada ψ.
0 5 10 15 20 25 30 35 40 450
0.2
0.4
0.6
0.8
1
1.2
1.4x 10
−3 Traza de la Matriz de Covarianza del Error
Tiempo [seg]
Err
or
[rad
2]
AHRS
SLAM
Fusión
(d) Covarianza del error.
Figura 5.5: Resultados de la fusion modificada track to track.
TABLA 5.7: Error medio de los modelos de fusion de medidasModelo Error Unidades
Aumento del vector de observacion 1,3095e−5 rad2
Estimacion de mınima media cuadratica 1,3213e−5 rad2
TABLA 5.8: Error medio de los modelos de fusion track to trackModelo Error Unidades
Fusion track to track 1,9540e−5 rad2
Combinacion convexa basica 1,7874e−5 rad2
Fusion modificada track to track 1,2697e−5 rad2
60 CAPITULO 5. RESULTADOS EXPERIMENTALES
Capıtulo 6
CONCLUSIONES
Aunque el SLAM es un dispositivo que ofrece muchos beneficios en localizacion y mapeo, este todavıa
conserva algunos inconvenientes los cuales no permiten sacar todo el provecho a esta herramienta. Por
consiguiente, el objetivo de este trabajo ha sido proporcionar una mejora al sistema SLAM con lo que
tiene que ver con el factor de escala del mapa. Esta problematica ha sido atacada adicionando sensores
inerciales con la intencion de que dichas mediciones contribuyan a mejorar la trayectoria seguida por la
camara y soporten la informacion obtenida a traves del procesamiento de la imagen. Una revision de la
literatura refleja que los trabajos han sido enfocados en utilizar los sensores inerciales para predecir los
estados involucrados con la dinamica de la camara, mientras la observacion de los mismos es obtenida
a traves de la informacion suministrada por la imagen obteniendo buenos resultados.
Demostrada la efectividad del uso de sensores inerciales para la mejora de la trayectoria y por ende
reduccion del escalamiento en el mapa, se ha recurrido a la fusion de sensores como una alternativa
adicional con la que se puede hacer la combinacion de los sensores. Por tanto, dos sistemas que actuan
como sensores del entorno uno independiente del otro fueron fusionados. En una primera instancia se
elaboro un sistema de referencia actitud y rumbo (AHRS) para que ofreciera una estimacion de la orien-
tacion de la camara a manera de que coincidieran con los proporcionados por el sistema SLAM . Por
ultimo, se implementaron diferentes modelos de fusion de sensores que usan como estimador secuencial
el filtro Kalman. Las conclusiones obtenidas del trabajo descrito anteriormente son las siguientes:
La escala del mapa, la posicion y orientacion obtenida a partir del sistema SLAM puede cambiar
entre observaciones. Es decir, que se pueden tener resultados diferentes para el mismo experi-
mento y depende de las caracterısticas visuales detectadas en la imagen y la perdida de las
mismas.
Se elaboro un sistema de referencia de actitud y rumbo (AHRS) a partir de sensores inerciales
y el uso del cuaterinion como herramienta para la descripcion de la orientacion sin presentar
singularidades y comparable con dispositivos comerciales.
No existen criterios para establecer con las medidas inerciales si un vehıculo se encuentra en
reposo o por el contrario en movimiento. Se requiere un sensor adicional que permita identificar
este fenomeno y poder proporcionar una estimacion de la velocidad y posicion seguida por la
camara.
61
62 CAPITULO 6. CONCLUSIONES
Es aconsejable en lo posible que el modelo del proceso y de la medida en el filtro Kalman
para la variable que se desea estimar manejen la misma referencia. Con el objetivo de obtener
resultados mas acertados y la posibilidad de eliminar los distintos inconvenientes que pueda
presentar cualquier sensor (ejemplo: offset).
La fusion de sensores no garantiza que la medida verdadera sea la estimada. Principalmente
sucede cuando los sensores a fusionar no tienen la misma referencia en sus mediciones. La esti-
macion puede tener una tendencia a seguir una de las dos medidas o por el contrario un promedio
de las dos en el peor caso.
En la fusion de sensores no siempre la tendencia es a seguir la curva que presenta menor error.
Esta siempre es estimada teniendo en cuenta las dos mediciones realizadas por los sensores aunque
se observe lo contrario.
En terminos del error obtenido, la fusion de sensores refleja un mejor resultado que el propor-
cionado por los demas sensores de manera individual sin importar el modelo de fusion utilizado.
Pero los modelos que presentan menor error son aquellos que combinan las medidas de los 2
sensores en la etapa de correccion y son retroalimentadas al sistema.
6.1. Trabajo futuro
Como trabajo futuro se propone elaborar un sistema de navegacion inercial (INS) a partir de sen-
sores inerciales diferente al modelo de integracion de la senal de aceleracion proporcionada por los
acelerometros con el fin de evitar los problemas de acumulacion del error debido a la integracion bias
presente en las mediciones de los giroscopos y acelerometros. O en caso contrario, utilizar un sensor
adicional que proporciones otras observaciones de posicion y velocidad que permita establecer los erro-
res.
Implementar en tiempo real un algoritmo que fusione el sistema de navegacion inercial (INS) con el
sistema (SLAM) basado en vision, en vez de usar un sensor como predictor y el otro como corrector.
Para ello se requiere conocer como esta elaborado el SLAM para poder realizar cualquier tipo de
modificacion. Adicionalmente, retroalimentar los resultados obtenidos en la fusion de los dos sistemas
al SLAM para evaluar las mejoras obtenidas en el factor de escala del mapa y por ende la estimacion
de la trayectoria seguida por la camara.
Explorar si con las medidas inerciales se pueden tener predicciones mas precisas de la ubicacion de
las caracterısticas en la imagen.
Implementar fusion de sensores aplicando otras metodologıas como redes neuronales y elaborar sen-
sores inteligentes apoyados con los resultados obtenidos a partir de la aplicacion del filtro Kalman.
Bibliografıa
[1] J.-H. Kim and S. Sukkarieh, “Airborne simultaneous localisation and map building,” in Robotics
and Automation, 2003. Proceedings. ICRA ’03. IEEE International Conference on, vol. 1, sept.
2003, pp. 406 – 411 vol.1.
[2] J. Kim and S. Sukkarieh, “Real-time implementation of airborne inertial-slam,” Robotics
and Autonomous Systems, vol. 55, no. 1, pp. 62 – 71, 2007. [Online]. Available:
http://www.sciencedirect.com/science/article/pii/S0921889006001485
[3] I. Mahon and S. Williams, “Slam using natural features in an underwater environment,” in Con-
trol, Automation, Robotics and Vision Conference, 2004. ICARCV 2004 8th, vol. 3, dec. 2004,
pp. 2076 – 2081 Vol. 3.
[4] A. Davison, I. Reid, N. Molton, and O. Stasse, “Monoslam: Real-time single camera slam,” Pattern
Analysis and Machine Intelligence, IEEE Transactions on, vol. 29, no. 6, pp. 1052 –1067, june
2007.
[5] A. Davison, “Real-time simultaneous localisation and mapping with a single camera,” in Computer
Vision, 2003. Proceedings. Ninth IEEE International Conference on, oct. 2003, pp. 1403 –1410
vol.2.
[6] D. Schleicher, L. Bergasa, R. Barea, E. Lopez, and M. Ocana, “Real-time simultaneous localization
and mapping using a wide-angle stereo camera and adaptive patches,” in Intelligent Robots and
Systems, 2006 IEEE/RSJ International Conference on, oct. 2006, pp. 2090 –2095.
[7] P. Pinies, T. Lupton, S. Sukkarieh, and J. Tardos, “Inertial aiding of inverse depth slam using a
monocular camera,” in Robotics and Automation, 2007 IEEE International Conference on, april
2007, pp. 2797 –2802.
[8] P. Pinies, “Slam in large environments with wearable sensors,” Ph.D. dissertation, Universidad
de Zaragoza, mar 2009.
[9] S. Roumeliotis, A. Johnson, and J. Montgomery, “Augmenting inertial navigation with image-
based motion estimation,” in Robotics and Automation, 2002. Proceedings. ICRA ’02. IEEE In-
ternational Conference on, vol. 4, 2002, pp. 4326 – 4333 vol.4.
63
64 BIBLIOGRAFIA
[10] E. Foxlin and L. Naimark, “Vis-tracker: A wearable vision-inertial self-tracker,” Virtual Reality
Conference, IEEE, vol. 0, p. 199, 2003.
[11] S.-K. Park, Y. S. Suh, and T. N. Do, “The pedestrian navigation system using vision and inertial
sensors,” in ICCAS-SICE, 2009, aug. 2009, pp. 3970 –3974.
[12] D. Randeniya, S. Sarkar, and M. Gunaratne, “Vision-imu integration using a slow-frame-rate
monocular vision system in an actual roadway setting,” Intelligent Transportation Systems, IEEE
Transactions on, vol. 11, no. 2, pp. 256 –266, june 2010.
[13] T. Oskiper, S. Samarasekera, and R. Kumar, “Tightly-coupled robust vision aided inertial naviga-
tion algorithm for augmented reality using monocular camera and imu,” in Mixed and Augmented
Reality (ISMAR), 2011 10th IEEE International Symposium on, oct. 2011, pp. 255 –256.
[14] A. Behrens, J. Grimm, S. Gross, and T. Aach, “Inertial navigation system for bladder endoscopy,”
in Engineering in Medicine and Biology Society,EMBC, 2011 Annual International Conference of
the IEEE, 30 2011-sept. 3 2011, pp. 5376 –5379.
[15] H. Rehbinder and B. Ghosh, “Pose estimation using line-based dynamic vision and inertial sen-
sors,” Automatic Control, IEEE Transactions on, vol. 48, no. 2, pp. 186 – 199, feb. 2003.
[16] Y. M. Choi, Y. S. Suh, and S. Kyeong, “Pose estimation from landmark-based vision and inertial
sensors,” in SICE-ICASE, 2006. International Joint Conference, oct. 2006, pp. 1668 –1671.
[17] A. Martinelli, “Closed-form solution for attitude and speed determination by fusing monocular
vision and inertial sensor measurements,” in Robotics and Automation (ICRA), 2011 IEEE In-
ternational Conference on, may 2011, pp. 4538 –4545.
[18] ——, “Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and
bias determination,” Robotics, IEEE Transactions on, vol. 28, no. 1, pp. 44 –60, feb. 2012.
[19] J. Hol, T. Schon, H. Luinge, P. Slycke, and F. Gustafsson, “Robust real-time tracking
by fusing measurements from inertial and vision sensors,” Journal of Real-Time Image
Processing, vol. 2, pp. 149–160, 2007, 10.1007/s11554-007-0040-2. [Online]. Available:
http://dx.doi.org/10.1007/s11554-007-0040-2
[20] G. Klein and T. Drummond, “Tightly integrated sensor fusion for robust visual tracking,” Image
and Vision Computing, vol. 22, no. 10, pp. 769 – 776, 2004, british Machine Vision Computing
2002. [Online]. Available: http://www.sciencedirect.com/science/article/pii/S0262885604000447
[21] G. Bleser and D. Stricker, “Advanced tracking through efficient image processing and
visual–inertial sensor fusion,” Computers and Graphics, vol. 33, no. 1, pp. 59 – 72, 2009. [Online].
Available: http://www.sciencedirect.com/science/article/pii/S0097849308001465
[22] R. Smith and P. Cheeseman, “On the representation and estimation of spatial uncertainty,”
Robotics Research, The international Journal of, vol. 5, no. 4, pp. 56 –68, dec 1986.
BIBLIOGRAFIA 65
[23] H. Durrant-Whyte, “Uncertain geometry in robotics,” Robotics and Automation, IEEE Journal
of, vol. 4, no. 1, pp. 23 –31, feb 1988.
[24] R. Smith, M. Self, and P. Cheeseman, “Estimating uncertain spatial relationships in robotics,” in
Robotics and Automation. Proceedings. 1987 IEEE International Conference on, vol. 4, mar 1987,
p. 850.
[25] J. Civera, A. Davison, and J. Montiel, “Inverse depth parametrization for monocular slam,”
Robotics, IEEE Transactions on, vol. 24, no. 5, pp. 932 –945, oct. 2008.
[26] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and mapping: part i,” Robotics
Automation Magazine, IEEE, vol. 13, no. 2, pp. 99 –110, june 2006.
[27] A. P. Gee, “Incorporating higher level structure in visual slam,” Ph.D. dissertation, University of
Bristol, may 2010.
[28] D. Chekhlov, “Towards robust data association in real-time visual slam,” Ph.D. dissertation,
University of Bristol, sep 2009.
[29] R. Munguia and A. Grau, “Delayed features initialization for inverse depth monocular slam,” in
European Conference on Mobile Robots ECMR 2007, sep 2007, pp. 1–6.
[30] J. Civera, A. J. Davison, and J. M. M. Montiel, “Unified inverse depth parametrization for monoc-
ular slam,” in In Proceedings of Robotics: Science and Systems, 2006.
[31] J. Sola, A. Monin, M. Devy, and T. Lemaire, “Undelayed initialization in bearing only slam,” in
Intelligent Robots and Systems, 2005. (IROS 2005). 2005 IEEE/RSJ International Conference
on, aug. 2005, pp. 2499 – 2504.
[32] STMicroelectronics, iNEMO user manual UM0937, may. 2010.
[33] M. Caruso, “Applications of magnetoresistive sensors in navigation systems,” SAE Technical Paper
970602, 1997.
[34] M. S. Grewal, L. R. Weill, and A. P. Andrews, Global Positioning System, Inertial
Navigation and Integration. John Wiley and Sons, Inc., 2002. [Online]. Available:
http://dx.doi.org/10.1002/0471200719.fmatter indsub
[35] D. Titterton and J. Weston, Strapdown Inertial Navigation Technology, 2nd ed. The American
Institute of Aeronautics and Astronautics, 2004.
[36] S. Eun-Hwan, “Accuracy improvement of low cost ins/gps for land applications,” Master’s thesis,
University of Calgary, dec 2001.
[37] A. Barrientos, Fundamentos de robotica, 2nd ed. McGraw-Hill, 2007.
[38] J. L. Crassidis, F. L. Markley, and Y. Cheng, “Nonlinear attitude filtering methods,” in Navigation,
and Control Conference and Exhibit; San Francisco, CA, USA, aug 2005, pp. 1–32.
66 BIBLIOGRAFIA
[39] R. Vepa, Biomimetic Robotics Mechanisms and Control. Cambridge University Press, 2009.
[40] Y.-C. Lai, S.-S. Jan, and F.-B. Hsiao, “Development of a low-cost attitude and heading reference
system using a three-axis rotating platform,” Sensors, vol. 10, no. 4, pp. 2472–2491, 2010.
[Online]. Available: http://www.mdpi.com/1424-8220/10/4/2472
[41] G. Schall, D. Wagner, G. Reitmayr, E. Taichmann, M. Wieser, D. Schmalstieg, and B. Hofmann-
Wellenhof, “Global pose estimation using multi-sensor fusion for outdoor augmented reality,” in
Mixed and Augmented Reality, 2009. ISMAR 2009. 8th IEEE International Symposium on, oct.
2009, pp. 153 –162.
[42] K. Tuck, Tilt Sensing Using Linear Accelerometers, Freescale Semiconductor, Application note,
jun. 2007.
[43] E. Foxlin, “Inertial head-tracker sensor fusion by a complementary separate-bias kalman filter,”
in Virtual Reality Annual International Symposium, 1996., Proceedings of the IEEE 1996, mar-3
apr 1996, pp. 185 –194, 267.
[44] M. Marmion, “Airborne attitude estimation using a kalman filter,” Master’s thesis, University
Centre of Svalbard, Norwegian University of Science and Technology, Superior National School of
Electrical Engineering, feb 2006.
[45] S. Fux, “Development of a planar low cost inertial measurement unit for uavs and mavs,” Master’s
thesis, Swiss Federal Institute of Technology Zurich, 2008.
[46] Y. Bar-Shalom, X.-R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and
Navigation: Theory, Algorithms and Software. John Wiley and Sons, Inc., 2002. [Online].
Available: http://dx.doi.org/10.1002/0471221279.fmatter indsub
[47] E. Foxlin, “Pedestrian tracking with shoe-mounted inertial sensors,” Computer Graphics and
Applications, IEEE, vol. 25, no. 6, pp. 38 –46, nov.-dec. 2005.
[48] B. Khaleghi, S. Razavi, A. Khamis, F. Karray, and M. Kamel, “Multisensor data fusion: An-
tecedents and directions,” in Signals, Circuits and Systems (SCS), 2009 3rd International Con-
ference on, nov. 2009, pp. 1 –6.
[49] J. Roecker and C. McGillem, “Comparison of two-sensor tracking methods based on state vec-
tor fusion and measurement fusion,” Aerospace and Electronic Systems, IEEE Transactions on,
vol. 24, no. 4, pp. 447 –449, jul 1988.
[50] J. Gao and C. Harris, “Some remarks on kalman filters for the multisensor fusion,”
Information Fusion, vol. 3, no. 3, pp. 191 – 201, 2002. [Online]. Available: http:
//www.sciencedirect.com/science/article/pii/S1566253502000702
[51] D. Willner, C. B. Chang, and K. P. Dunn, “Kalman filter algorithms for a multi-sensor system,” in
Decision and Control including the 15th Symposium on Adaptive Processes, 1976 IEEE Conference
on, vol. 15, dec. 1976, pp. 570 –574.
BIBLIOGRAFIA 67
[52] Q. Gan and C. Harris, “Comparison of two measurement fusion methods for kalman-filter-based
multisensor data fusion,” Aerospace and Electronic Systems, IEEE Transactions on, vol. 37, no. 1,
pp. 273 –279, jan 2001.
[53] Y. Bar-Shalom, “On the track-to-track correlation problem,” Automatic Control, IEEE Transac-
tions on, vol. 26, no. 2, pp. 571 – 572, apr 1981.
[54] Y. Bar-Shalom and L. Campo, “The effect of the common process noise on the two-sensor fused-
track covariance,” Aerospace and Electronic Systems, IEEE Transactions on, vol. AES-22, no. 6,
pp. 803 –805, nov. 1986.
[55] Y. Bar-Shalom, Tracking and data association. San Diego, CA, USA: Academic Press Profes-
sional, Inc., 1987.
[56] Y. Bar-Shalom and X. Li, Multitarget-multisensor Tracking: Principles and Techniques,
1995. Yaakov Bar-Shalom, 1995. [Online]. Available: http://books.google.com.mx/books?id=
NmXGQgAACAAJ
[57] P. S. Maybeck, Stochastic Models, Estimating, and Control. New York: Academic Press, 1979,
vol. 1.
[58] G. Welch and G. Bishop, “An introduction to the kalman filter,” 1995, updated july 2006.
68 BIBLIOGRAFIA
Referencia Grafica
Figura 1.1 Fuente: Autor.
Figura 1.2 Fuente: Autor. Adaptada de Formulation and Structure of the SLAM Problem [26].
Figura 1.3 Fuente: Autor. Adaptada de General Formulation of Visual SLAM Problem [27].
Figura 1.4 Fuente: Autor. Adaptada de Process Model [27].
Figura 1.5 Fuente: Autor. Adaptada de Process Model [27].
Figura 1.6 Fuente: Autor. Adaptada de Measurement equation [25].
Figura 2.1 Fuente: [32].
Figura 2.2 Fuente: Autor.
Figura 2.3 Fuente: Autor.
Figura 2.4 Fuente: Autor.
Figura 2.5 Fuente: [32].
Figura 2.6 Fuente: Autor.
Figura 3.1 Fuente: Autor.
Figura 3.2 Fuente: Autor. Adaptada de Reference Frames and Transformations [36].
Figura 3.3 Fuente: Autor.
Figura 3.4 Fuente: Autor. Adaptada de Reference Frames and Transformations [36].
Figura 3.5 Fuente: Autor. Adaptada de Matrices de Rotacion [37].
Figura 3.6 Fuente: Autor. Adaptada de Matrices de Rotacion [37].
Figura 3.7 Fuente: Autor. Adaptada de Matrices de Rotacion [37].
Figura 3.8 Fuente: Autor.
Figura 3.9 Fuente: Autor.
Figura 3.10 Fuente: Autor.
Figura 3.11 Fuente: Autor.
Figura 3.12 Fuente: Autor. Adaptada de Earth’s Magnetic field [33].
Figura 3.13 Fuente: Autor. Adaptada de Complimentary Kalman Filter [43].
Figura 3.14 Fuente: Autor.
Figura 3.15 Fuente: Autor.
Figura 3.16 Fuente: Autor.
Figura 4.1 Fuente: Autor. Adaptada de Measurement fusion models [50].
Figura 4.2 Fuente: Autor. Adaptada de The track-to-track fusion models [50].
69
70 BIBLIOGRAFIA
Figura 4.3 Fuente: Autor. Adaptada de The track-to-track fusion models [50].
Figura 5.1 Fuente: Autor.
Figura 5.2 Fuente: Autor.
Figura 5.3 Fuente: Autor.
Figura 5.4 Fuente: Autor.
Figura 5.5 Fuente: Autor.
Figura B.1 Fuente: [27].
Figura B.2 Fuente: Autor. Adaptada de Perspective Projection [27].
Figura B.3 Fuente: Autor. Adaptada de Pose [27].
Apendice A
PROBLEMA GENERAL DE
ESTIMACION SECUENCIAL
Considerando un sistema con un vector de estados x, n−dimensional, la idea consiste en determinar
la distribucion de probabilidad del estado en el instante k, dado un valor inicial particular para el
estado, x(0), y las secuencias de medidas y entradas de control posteriores. Lo anterior puede ser
representado como una funcion de densidad de probabilidad conjunta:
p(x(k)|z(0 : k), u(0 : k), x(0)) (A.1)
Donde z(0 : k) y u(0 : k) son el conjunto completo de observaciones y entradas de control en todos
los instantes de tiempo. Con el objetivo de realizar este procedimiento eficientemente en tiempo real, se
busca una solucion recursiva de tal manera que si tenemos una estimacion de la distribucion del estado
para el instante de tiempo anterior, k−1, podemos utilizar esta con la ultima entrada de control, u(k),
y observacion, z(k), para calcular la distribucion del estado para el instante de tiempo actual, k.
p(x(k − 1)|z(0 : k − 1), u(0 : k − 1), x(0)) (A.2)
Estableciendo que la distribucion del estado puede ser controlado por dos procesos. Entonces tomando
el estado anterior, x(k − 1), y una entrada de control conocidos, u(k), una funcion de proceso predice
la nueva distribucion del estado en el instante de tiempo k.
p(x(k)|u(k), x(k − 1)) (A.3)
Luego la distribucion del estado basado sobre todas las entradas de control y observaciones anteriores
se pueden actualizar de forma recursiva, para tomar en cuenta el efecto de las nuevas entradas de control
marginando la dependencia en el estado anterior.
p(x(k)|z(0 : k − 1), u(0 : k), x(0)) =
∫p(x(k)|u(k), x(k − 1))
p(x(k − 1)|z(0 : k − 1), u(0 : k − 1), x(0))dx(k − 1)
(A.4)
71
72 APENDICE A. PROBLEMA GENERAL DE ESTIMACION SECUENCIAL
Una funcion de medida usa la distribucion del estado actual x(k) para definir la distribucion del
conjunto de observaciones generados en el intervalo de tiempo actual.
p(z(k)|x(k)) (A.5)
Esta permite que la distribucion del estado pueda ser recursivamente actualizada usando el teorema
de Bayes teniendo en cuenta el efecto de las nuevas observaciones.
p(x(k)|z(0 : k), u(0 : k), x(0)) =p(z(k)|x(k))p(x(k)|z(0 : k − 1), u(0 : k), x(0))
p(z(k)|z(0 : k − 1), u(0 : k))(A.6)
Esta secuencia recursiva de marginalizacion y acondicionamiento de la distribucion del estado es
el proceso fundamental seguido por los filtros de estimacion secuencial y es independiente del tipo
de distribucion. Si se asume que el estado x tiene una distribucion Gaussiana, entonces se puede
representar sus estadısticas usando su media x y covarianza Pxx. Si lo anterior es posible, esto permite
el uso del filtro Kalman como un estimador secuencial para el problema.
A.1. Filtro Kalman
El filtro Kalman es un estimador optimo del primer y segundo momento estadıstico de una variable.
Optimo, en el sentido que garantiza la mınima varianza del error en la estimacion (error cuadratico
medio) dadas ciertas condiciones [57] [58]. Es un algoritmo recursivo de procesamiento de informacion
y es considerado un metodo de fusion directa de mediciones. Utiliza de manera eficiente toda la infor-
macion disponible para estimar el valor de las variables de interes aun con la presencia de mediciones
ruidosas. Principalmente usa:
Modelos dinamicos del sistema o proceso en cuestion y del o los dispositivos de medicion.
La descripcion estadıstica del ruido en el sistema, el error en la medicion y la incertidumbre en
los modelos dinamicos.
Cualquier informacion disponible sobre las condiciones iniciales de las variables de interes.
El problema general de estimar el estado de un proceso en tiempo discreto esta gobernado por la
siguiente ecuacion en diferencias estocastica.
xk = Axk−1 +Buk−1 + wk−1 (A.7)
A.1. FILTRO KALMAN 73
Donde xk es el estado prededido, xk−1 es el estado anterior, uk−1 es la entrada de control y wk−1
es una variable aleatoria que representa el ruido del proceso. Las variables A y B son las matrices de
transicion de estados y la de entrada, respectivamente. Con el modelo para las medidas se pretende
obtener el valor real de la medida o algo que sea una buena aproximacion de este.
zk = Hxk + vk (A.8)
Donde vk es una variable aleatoria que representa el ruido de la medida. H es la matriz de medidas.
El ruido presente en el proceso wk y vk se asumidos estadısticamente independientes, blancos y con
una distrubucion de propabilidad normal.
p(w) ∼ N(0, Q) (A.9)
p(v) ∼ N(0, R) (A.10)
Las ecuaciones para el Filtro Kalman pueden ser divididas en 2 grupos. Las ecuaciones de time update
y las de measurement update. Las primeras son las responsables de la proyeccion hacia adelante del
estado actual y la estimacion de las covarianzas del error, para obtener las estimaciones a priori para
el proximo paso. Las ecuaciones de measurement update son las encargadas de la retroalimentacion,
por incorporar una nueva medida dentro de la estimacion a a priori para obtener una mejor estimacion
a posteriori [58].
Las ecuaciones de time update pueden ser pensadas como las ecuaciones de prediccion, mientras
que las measurement update como las ecuaciones de correcciones. las ecuaciones de prediccion son las
siguientes:
xk = Axk−1 +Buk−1 (A.11)
Pk = APk−1AT +Q (A.12)
Las ecuaciones de correccion son:
Kk = PkHT (HPkH
T +R)−1 (A.13)
xk = xk +Kk(zk −Hxk) (A.14)
Pk = (I −KkH)Pk (A.15)
74 APENDICE A. PROBLEMA GENERAL DE ESTIMACION SECUENCIAL
A.2. Filtro Kalman Extendido
Si el proceso a ser estimado o las realciones de medida son no lineales es necesario usar el Filtro
Kalman Extendido o EKF . Este filtro realiza una linealizacion respecto a la medida actual y su
covarianza. Pare este caso el proceso es gobernado por una ecuacion de diferencias estocastica no
lineal.
xk = f(xk−1, uk−1, wk−1) (A.16)
zk = h(xk + vk) (A.17)
De manera similar las variables w y v representan el ruido en el modelo del proceso y en el de la
medida respectivamente. En la practica no se requiere conocer los valores individuales de ruido wk y vk
en cada intervalo de tiempo. Por tanto el vector de estados y de medida puede ser aproximado como:
xk = f(xk−1, uk−1, 0) (A.18)
zk = h(xk, 0) (A.19)
Donde xk es una estimacion del estado aposteriori para el intervalo de tiempo anterior. Las ecuaciones
de prediccion son las siguientes:
x−k = f(xk−1, uk−1, 0) (A.20)
P−k = AkPk−1ATk +WkQk−1W
Tk (A.21)
Donde x−k es notacion apriori y xk a posteriori. Por su parte las matrices A, W , H y V son las
matrices jacobianas de las derivadas parciales de las funciones no lineales con respecto a los estados y
al ruido que deben ser calculadas para cada tiempo k.
A[i,j] =∂f[i]
∂x[j](xk−1, uk−1, 0) (A.22)
W[i,j] =∂f[i]
∂w[j](xk−1, uk−1, 0) (A.23)
H[i,j] =∂h[i]
∂x[j](xk, 0) (A.24)
V[i,j] =∂h[i]
∂v[j](xk, 0) (A.25)
A.2. FILTRO KALMAN EXTENDIDO 75
Donde xk es el vector de estado aproximado obtenido de la ecuacion A.18. Las ecuaciones de co-
rreccion son:
Kk = P−k HTk (HkP
−k H
Tk + VkRkV
Tk )−1 (A.26)
xk = x−k +Kk(zk − h(x−k , 0)) (A.27)
Pk = (I −KkHk)P−k (A.28)
76 APENDICE A. PROBLEMA GENERAL DE ESTIMACION SECUENCIAL
Apendice B
MODELO DE FORMACION DE LA
IMAGEN
En el sistema SLAM basado en vision, el principal sensor es una camara que proporciona las medidas
de las caracterısticas visuales en las imagenes. En la mayorıa de los sistemas, el modelo de la camara
de agujero de alfiler (pinhole camera model) estandar ofrece un marco matematico que es usado para
modelar la formacion de la imagen. Este modelo es una simplificacion del real.
Figura B.1: Camara monocular.
B.1. Perspectiva de proyeccion
El modelo de la camara de agujero de alfiler representa la apertura de la camara en un solo punto
comunnmente llamado como el centro optico O de la camara. Un punto en 3 dimensiones en el marco
de referencia euclideano, es proyectado sobre el plano de imagen de la camara a lo largo de un a lınea
a traves del centro optico de la camara, tal como se muestra en la figura B.2.
La longitud focal f , es la distancia mas corta entre el centro optico O y el plano de la imagen en
donde se observa la vista proyectada del mundo en 3 dimensiones. El punto p = [px, py, f ]T es una
imagen de un punto en 3D P = [Px, Py, Pz]. El centro de la imagen Oi se encuentra intersectando el
77
78 APENDICE B. MODELO DE FORMACION DE LA IMAGEN
X
Y
Z
u
v
Oif
O
eje óptico
Punto en 3D
Punto proyectado
Rayo de proyección
Figura B.2: Geometrıa del modelo de la camara de agujero de alfiler.
plano de la imagen con el eje optico z. Por lo tanto, usando el teorema de triangulos semejantes se
obtienen las ecuaciones fundamentales de la perspectiva de proyeccion:
px = fPxPz
py = fPyPz
(B.1)
La manera como se representa una proyeccion de un punto en 3D a otro en 2D es usando la funcion
de proyeccion Π: <3 7→ <2 :
p =
[px
py
]= Π(P ) =
[f Px
Pz
fPy
Pz
](B.2)
Las coordenadas de la imagen son establecidas a partir de los ejes u y v. Donde u toma valores
positivos de izquierda a derecha y v de arriba hacia abajo y su centro u origen es el punto de interseccion
entre el plano de la imagen y el eje optico u0 = (u0, v0)T . La transformacion entre coordenadas
requiere conocer el tamano de cada pixel en unidades del sistema coordenado de la camara. El tamano
de un pixel rectangular (sx, sy) puede ser establecido como un elemento del arreglo CCD. El punto
p = (px, py, f)T en las correspondiente coordenadas de imagen (u, v) tienen las siguientes relaciones:
px = −(u− u0)· sx py = −(v − v0)· sy (B.3)
B.2. POSICION 79
u = u0 −pxsx
v = v0 −pysy
(B.4)
El signo menos en la ecuacion B.3 aparece porque los ejes u y v tienen direcciones opuestas a los ejes
X y Y del sistema de coordenadas de la camara. Por consiguiente, la ecuacion de proyeccion final de
un punto P es:
Π(P ) =
[u0 − fPx
sxPz
v0 − fPy
syPz
](B.5)
Los parametros f, (u0, v0)T , (sx, sy)T son llamados parametros intrınsecos de una camara los cuales
son obtenidos de un procedimiento de calibracion [27].
B.2. Posicion
Para proyectar un punto en 3D que representa la posicion de una marca en la imagen y adicionalmente
este se encuentre definido en el marco de referencia de navegacion n, es necesario describir la posicion
de la camara en este mismo marco. De esta manera sera posible formular la transformacion euclideana
que tome un punto 3D en el marco de navegacion n para determinar su equivalente en el marco de
referencia de la camara o del cuerpo b. Los marcos de referencia son presentados en la seccion 3.1.
u
xin
xcn
xcin
Xn
Yn
Zn
Xb
Yb
Zb
Cámara
Referencia
Plano deimagen
Figura B.3: Transformaciones entre el marco de referencia n y del cuerpo b.
80 APENDICE B. MODELO DE FORMACION DE LA IMAGEN
Tomando como referencia la figura B.3, es posible escribir que la distancia entre la camara y la marca
en la imagen xci dada en el marco de referencia del cuerpo b (equivalente al marco de referencia de la
camara) es igual a la diferencia entre la posicion la caracterıstica xi y de la camara xc, en el marco de
navegacion n, y multiplicada por la correspondiente matriz de rotacion Cbn:
xbci = Cbn(xni − xnc ) (B.6)
Ahora sabiendo que en realidad disponemos de la informacion correspondiente a la posicion de la
camara xc en el marco n y de la distancia existente entre la camara y la marca xci en el marco de
referencia del cuerpo b, se puede deducir que:
xni = Cnb xci + xnc (B.7)
Debido a que la ecuacion B.6 representa la posicion de la marca en el marco de referencia de la
camara b, es posible formular la nueva ecuacion de proyeccion:
Π(xbci) = Π(Cbn(xni − xnc )) (B.8)