calibracion de robots de locomoci on...
TRANSCRIPT
CALIBRACION DE ROBOTS DE LOCOMOCION
DIFERENCIAL POR EL METODO DE MINIMOS
CUADRADOS EMPLEANDO LA TECNICA DE
SUAVIZADO Y CARTOGRAFIA INCREMENTAL
TESIS
Que para obtener el grado de
MAESTRO EN CIENCIAS EN INGENIERIA ELECTRICA
presenta
Shadai Rafael Lopez Garcıa
Leonardo Romero Munoz
Director de Tesis
Universidad Michoacana de San Nicolas de Hidalgo
Agosto 2011
Contenido
Contenido . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . v
Lista de Sımbolos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xi
1. Introduccion 1
1.1. Odometrıa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.1.1. Reduccion de los Errores No Sistematicos por Medio de las NavegacionInercial y Fusion Sensorial . . . . . . . . . . . . . . . . . . . . . . . . 5
1.1.2. Reduccion de Errores Sistematicos por Medio de la Calibracion deOdometrica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.2. Planteamiento del Problema . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.3. Antecedentes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.4. Objetivos de la Tesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.4.1. Objetivo general . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.4.2. Objetivos particulares . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.5. Descripcion de Capıtulos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2. Modelo Odometrico y Calibracion 13
2.1. Robots de Locomocion Diferencial . . . . . . . . . . . . . . . . . . . . . . . 13
2.1.1. Modelo Cinematico de Robots de Locomocion Diferencial . . . . . . 14
2.2. Proceso Odometrico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
2.3. Calibracion Odometrica Basada en el Metodo de Mınimos Cuadrados porAntonelli . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.4. Calibracion Odometrica Basada en el Metodo de Mınimos Cuadrados porPapadopolous . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3. Experimentacion y Resultados Obtenidos 27
3.1. Constante de Conversion Angular . . . . . . . . . . . . . . . . . . . . . . . . 28
3.2. Estimacion de Matriz de Parametros . . . . . . . . . . . . . . . . . . . . . . 29
3.3. Comparacion de Parametros Odometricos . . . . . . . . . . . . . . . . . . . 30
4. Conclusiones 33
4.1. Conclusiones Generales . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
4.2. Trabajos Futuros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
v
vi Contenido
A. Suavizado y Cartografıa 37A.1. Notacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37A.2. Funcion de Movimiento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37A.3. Modelo de Movimiento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
A.3.1. Funcion de Observacion . . . . . . . . . . . . . . . . . . . . . . . . . 39A.3.2. Modelo de Observacion . . . . . . . . . . . . . . . . . . . . . . . . . 41
A.4. SAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42A.5. SAM como Problema de Maxima Verosimilitud . . . . . . . . . . . . . . . . 42A.6. SAM Como Problema de Mınimos Cuadrados . . . . . . . . . . . . . . . . . 44A.7. Suavizado y Cartografıa Incremental . . . . . . . . . . . . . . . . . . . . . . 47A.8. Asociacion de Datos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
A.8.1. Asociacion de Datos por Maxima Verosimilitud . . . . . . . . . . . . 49A.8.2. Estimacion de Covarianza . . . . . . . . . . . . . . . . . . . . . . . . 50
A.9. Resultados Obtenidos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
B. Mınimos Cuadrados 53B.1. Mınimos Cuadrados en una Dimension . . . . . . . . . . . . . . . . . . . . . 54B.2. Mınimos Cuadrados en Multiples Dimensiones . . . . . . . . . . . . . . . . . 55B.3. Pseudoinversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57B.4. Mınimos Cuadrados Mediante Factorizacion QR . . . . . . . . . . . . . . . 58
B.4.1. Rotaciones de Givens . . . . . . . . . . . . . . . . . . . . . . . . . . 58
Referencias 61
Lista de Figuras
1.1. Aplicaciones de la robotica Movil . . . . . . . . . . . . . . . . . . . . . . . . 11.2. Areas de estudio en la Robotica Movil [Stachniss09] . . . . . . . . . . . . . 31.3. Problema escencial de SLAM [Durrant-Whyte06]. . . . . . . . . . . . . . . . 6
2.1. Geometrıa de la cinematica en los RMLD [Dudek00] . . . . . . . . . . . . . 152.2. Componentes vectoriales de la velocidad del robot [Antonelli05] . . . . . . . 162.3. Acciones de control de los Robots de Locomocion Diferencial . . . . . . . . 172.4. Descomposicion del vector v . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.1. Robot desarrollado en DEPFIE. . . . . . . . . . . . . . . . . . . . . . . . . 273.2. Estimacion de constante angular por cuenta . . . . . . . . . . . . . . . . . . 283.3. Trayectoria bidireccional . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.1. Convergencia de la tecnica de Suavizado y Cartografıa con el modelo sincalibrar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.2. Convergencia de la tecnica de Suavizado y Cartografıa con el modelo calibrado 35
A.1. Sistema global y local de coordenadas [Siegwart04] . . . . . . . . . . . . . . 38A.2. Modelo Odometrico. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39A.3. Modelo de medicion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41A.4. Red Bayesiana de SLAM con un movimiento, resultado de una primera it-
eracion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43A.5. Red Bayesiana de una trayectoria . . . . . . . . . . . . . . . . . . . . . . . . 44A.6. Mapa generado mediante la tecnica de Suavizado . . . . . . . . . . . . . . . 52
B.1. Proyeccion de b sobre a . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54B.2. Proyeccion de b sobre el espacio de columnas de A . . . . . . . . . . . . . . 56
vii
Lista de Sımbolos
rd Radio de la llanta derecha.
ri Radio de la llanta izquierda.
D Radio de la base del Robot.
ϕ Posicion angular.
R Radio de curvatura.
CIR Centro instantaneo de rotacion.
[v, ω]T Vector de velocidad del robot en donde v es la velocidad detraslacion y ω la velocidad angular del robot.
ωd Velocidad angular de la llanta derecha.
ωi Velocidad angular de la llanta izquierda.
x = [x, y, θ]T Vector de posicion del robot en el plano en donde x, y sonlas coordenadas del plano y θ es la orientacion del robot.
C Matriz de parametros del modelo odometrico.
xi i-esima posicion del robot.
ui i-esima accion de control del robot.
lj j-esima marca.
zk k-esima medicion del robot.
X Trayectoria del robot.
U Conjunto de acciones de control del robot.
L Mapa del entorno del robot.
Z Conjunto de mediciones del robot.
[r, φ]T Vector de parametros de la lınea en forma polar en donde res el radio y φ es el angulo del radio.
T Perıodo de muestreo.
α Constante de conversion angular.
N Numero de cuentas de encoder.
xi
Capıtulo 1
Introduccion
La Robotica Movil desde hace algunos anos se ha visto motivada en desarrollar
sistemas capaces de desplazarse en forma autonoma. Con esta habilidad los robots podrian
ser implemenados en tareas complejas dentro de ambientes dinamicos. Como por ejemplo
en el transporte, el entretenimiento, la manufactura, la asistencia a personas con capaci-
dades disminuidas, etc. De hecho actualmente existen robots con cierto grado de autonomıa
implementados en aplicaciones reales (aunque en ambientes controlados).
(a) Robot TPR-
Robina de Toyota
(b) Robot Minerva (c) Vehıculo autonomo de la Universi-
dad de Stanford.
Figura 1.1: Aplicaciones de la robotica Movil
1
2 Capıtulo 1: Introduccion
Como los robots TPR-Robina de Toyota y Minerva [Thrun99] disenados para guiar
a visitantes en ferias y exposiciones (figuras 1.1(a) y 1.1(b)). Tenemos tambien al vehıculo
autonomo de la Universidad de Stanford (figura 1.1(c)) que puede navegar tan solo con la
informacion de los sensores que lleva a bordo [Petrovskaya08].
El salto tecnologico entre un robot que simplemente realiza tareas repetitivas a un robot que
pueda navegar libremente en su entorno no es trivial. Ya que el robot debera comprender
el entorno en donde se encuntra para poder planear de forma inteligente sus movimientos
[Dudek00]. En [Borenstein96] se afirma que la comprension del entorno se puede resumir me-
diante 3 preguntas clave ¿Donde estoy? ¿Donde voy? ¿Como llegar a ese sitio? La robotica
movil aborda cada una de estas preguntas en tres areas de investigacion que son “La local-
izacion”, ”La Cartografıa” y ”La Planeacion de Movimientos”.
Conforme se ha ido desarrollando la robotica movil han surgido problemas que han estable-
cido la interdependencia entre las areas mencionadas (ver el diagrama de la figura 1.2).
Como “El Mapeo y Cartografıa Simultanea ”, SLAM por sus siglas en ingles (Simoultane-
ous Localization and Mapping) que consiste en construir un mapa del entorno al mismo
tiempo que el robot se localiza en el mapa. O el problema de la “Exploracion” que bus-
ca encontrar trayectorias optimas dentro de ambientes desconocidos. Tambien tenemos el
problema de la “Localizacion Activa” donde se busca guiar al robot hacia una posicion y
mejorar el estimado de la localizacion al mismo tiempo. Tenemos por ultimo la integracion
de todos estos enfoques en lo que se ha denominado como el problema de “Planeacion, Lo-
calizacion y Mapeo Simoultaneo” SPLAM por sus siglas en ingles (Simoultaneous Planning
Localization and Mapping). El cual es un problema en donde el robot va planeando sus
trayectorias mientras construye el mapa de su entorno y se localiza en el. Siendo este el
ultimo eslabon de una serie de problemas que buscan brindar a los robot la capacidad de
la navegacion autonoma.
1.1. Odometrıa 3
SLAM
SPLAM
Localizacion ActivaExploracion
Planeacion de Movimientos
Mapeo Localizacion
Figura 1.2: Areas de estudio en la Robotica Movil [Stachniss09]
1.1. Odometrıa
En la robotica movil la odometrıa1 es un problema que se encuentra dentro de los
topicos estudiados por la Localizacion. Se define como la tecnica que busca estimar la local-
izacion del robot a partir de las mediciones de sus sensores propioseptivos 2 [Rodrıguez-Losada04].
Fundamentalmente lo que propone la odometrıa es obtener un estimado de la posicion del
robot al sumar los pequenos incrementos de posicion que van siendo sensados.
Existen dos tipos de sensores utilizados para la odometrıa: sensores inerciales y sen-
sores electromecanicos. Los sensores inerciales son transductores que convierten las fuerzas
inerciales a las que esta sujeto el robot en senales electricas como giroscipios y acelerometros
[Borenstein97]. Cuando se emplean este tipo de sensores se dice que la odometrıa se ob-
tiene mediante un “Sistemas de Navegacion Inercial” INS por sus siglas en ingles (Inertial
1Tambien conocido como problema de Dead Reckoning.2Sensores que permiten obtener observaciones relacionadas con las variables internas del estado del robot,
es decir su velocidad, aceleracion, rotacion, etc
4 Capıtulo 1: Introduccion
Navigation Systems). En donde la funcion de los giroscopio es brindar informacion de la
orientacion del robot. Por su parte los acelerometros nos permiten estimar la localizacion
y la velocidad del robot. en forma casi directa. Ya que no es necesario conocer el modelo
cinematico del robot para obtener las variables odometricas de interes. Basta con integrar
las mediciones en intervalos definidos. Cabe mencionar que estos dispositivos no son muy
populares por ser susceptibles a las vibraciones.
En cambio los sensores electromecanicos convierten el desplazamiento angular en
pulsos electricos que posteriormente son interpretados como velocidad o posicion. Estos
sensores se acoplan a los ejes de las llantas en el robot. Existen varios tipos de sensores
electromecanicos de los que destaca el encoder rotatorio incremental que puede ser optico
o mecanico. Estos sensores estan formado por un disco y un transductor. El disco tiene
un numero determinado de muescas de algun tipo que al coinsidir con el transductor se
genera un pulso electrico. Sı se conoce el numero de pasos de encoder a los que equivale una
vuelta de la llanta (factor de conversion) es posible estimar la velocidad angular. Cuando se
utilizan estos sensores electromecanicos es necesario conocer el modelo cinematico del robot
para poder estimar la localizacion (este proceso se explica en la seccion 2.2).
Cualquier sensor que se utilice nos va brindar mediciones con errores. La acumu-
lacion de estos errores es la principal limitacion de la odometrıa como metodo de localizacion.
En [Jared10] a estos errores se les clasifica en 2 grupos, los cuales son:
• Errores sistematicos : son aquellos errores que se acumulan a traves del tiempo y son
causados por las imperfecciones en el robot. Como radios desiguales en las llantas,
llantas desalineadas, asimetrıa de la base, resolucion limitada del encoder, etc. Por lo
tanto las causas que originan estos errores pueden determinarse mediante pruebas de
laboratorio (calibracion).
• Errores no sistematicos: son aquellos errores causados por perturbaciones aleatorias.
Como irregularidades en el entorno, objetos extranos, deslizamiento de las llantas, etc.
Las causas de estos errores no se pueden determinar facilmente.
1.1. Odometrıa 5
1.1.1. Reduccion de los Errores No Sistematicos por Medio de las Nave-
gacion Inercial y Fusion Sensorial
Algunas perturbaciones como el deslizamiento de las llantas pueden provocar un
inusual efecto rotatorio. Estos fenomenos son son considerados como errores no sistematicos
que afectan la orientacion del robot. Los giroscipio en cambio pueden medir estas rotaciones,
surgiendo numerosas investigaciones en la robotica para incluir el giroscopio en las tecnicas
de localizacion [Jared10]. Cabe senalar que los giroscopios no brindan informacion acerca
de la tralacion del robot, por lo que es usual que su uso implique emplear tecnicas de fusion
de datos. Como en las tecnica propuestas en en [Azizi03], [Luo08], [Yizhong04], [Han99]
y [Pornsarayouth09] donde por medio del Filtro de Kalman se fusionan los datos de la
odometricos del encoder con los del giroscopio para mejorar la localizacion.
1.1.2. Reduccion de Errores Sistematicos por Medio de la Calibracion de
Odometrica
El proceso de calibracion odometrica consiste en identificar el conjunto de paramet-
ros cinematicos que permiten reconstruir la trayectoria del robot a partir de las mediciones
que obtiene de los encoders en sus llantas [Antonelli05]. Al identificar dichos parametros se
tiene un modelo mas cercano al real, del que se obtendran estimaciones con mayor precision.
Es por esta razon que se dice que la calibracion nos permite reducir los errores sistematicos
relacionados a la simetrıa del robot.
Para obtener los parametros del modelo es necesario tener un conjunto de medi-
ciones experimentales acerca de la forma en como se desplaza el robot. Por lo que un aspecto
muy importante en la calibracion es la manera en que se obtienen dichas mediciones. Si se
elige un metodo en el que las mediciones tienen un alto grado de incertidumbre. Es prob-
able que la calibracion no tenga efecto sobre los errores sistematicos. Por Esta razon en la
presente tesis se empleara una tecnica de SLAM completo (ver seccion 1.1.2).
6 Capıtulo 1: Introduccion
Localizacion y Cartografıa Simultanea
El SLAM se define como un problema en la robotica movil que consiste en construir
el mapa de un entorno desconocido, mientras que al mismo tiempo se obtiene su posicion
en ese entorno. Para ello el robot utilizara las acciones de control que gobiernan el vehıculo
y los datos proporcionados por sensores [Durrant-Whyte06].
lj
lj+1
lj+2
lj+3
lj+4
lj+5
lj+6ui
ui+1
ui+2
zk
zk+1
zk+2
zk+3 zk+4
zk+5
zk+6
zk+7
zk+8
marca
robot
Figura 1.3: Problema escencial de SLAM [Durrant-Whyte06].
En la figura 1.3 se muestra la idea principal en las tecnicas de SLAM. Se observa
como el robot explora su entorno ejecutando acciones de control (ui). Al ejecutarse una ac-
cion de control en un estado xi−1 el robot se traslada hasta un estado xi y asi sucesivamente
hasta completar una trayectoria. Mientras el robot se desplaza va tomando mediciones (zk)
de las marcas (lj) en el entorno. Estas marcas son objetos en el entorno que son representa-
dos como estructuras geometricas facilmente reconocibles. Con la informacion que se tiene
de la localizacion en el estado xi y con la medicion zi de la marca se puede estimar la ubi-
cacion de la marca. El conjunto de marcas es por lo tanto el mapa del entorno. Un aspecto
importante de la localizacion en el SLAM es que las marcas se usan tambien para retroali-
mentar la estimacion de la localizacion. De manera que se pueden obtener estimaciones de
1.2. Planteamiento del Problema 7
localizacion muy precisas.
En [Thrun05] se clasifican las tecnicas de SLAM en dos grupos: SLAM en lınea
y SLAM completo. Las tecnicas de SLAM en lınea se caracterizan por solo tomar en
cuenta la informacion del estado anterior para obtener el estimado del estado actual. Sus
tecnicas mas representativas son: el “Filtro Extendido de Kalman” [Chatila85], [Ayache88],
[Crowley89],[Larsen99], [Bailey02], el “Filtro de Particulas” [Thrun04], [Montemerlo03], y
el ”Filtro de Informacion” [Zhan10], [Cadena09].
En cambio el SLAM completo no solo estima el estado actual sino que cada vez
que se agrega un nuevo estado se vuelve a estimar toda la trayectoria y todo el mapa, es por
ello que se considera que estas tecnicas son de gran escala. Algunas de las tecnicas de SLAM
completo son: el “SLAM basado en Grafos” [Frese05],[Olson06],[Grisetti], “SLAM basado
en Mınimos Cuadrados” [Shoudong10], “Suavisado y Cartografıa” [Dellaert06], [Kaess08b].
1.2. Planteamiento del Problema
Desde hace algun tiempo se ha considerado que por si sola la odometrıa no puede
solucionar el problema de localizacion. Esto sucito una serie de investigaciones que origi-
naron algunas tecnicas como el SLAM que nos permiten estimar la localizacion en forma
muy precisa. La mencionar que el SLAM integra a la odometrica en un como un valor inicial
de la localizacion que se mejora con evidencia del entorno. Por lo que se puede considerar
que la odometrıa es una parte importante en el SLAM. Otra area de la robotica movil
donde la odometrıa tiene una presencia importante es en la planeacion de movimientos. Ya
que a partir de modelos odometricos podemos determinar por cuanto tiempo puede viajar
un robot a una velocidad determinada antes de colicionar contra algun objeto. Algunas
publicaciones al respecto son [Papadopolous07], [Popa08], [Irie03], [Luo08].
Es evidente que la calidad de la informacion odometrica afecta directamente los
resultados de las metodologıas en donde se requiere. Es por ello que la calibracion debe ser
el primer paso para tener un sistema exitoso de navegacion [Yun08]. Esta idea es respalda-
da por publicaciones como [Papadopolous07] en donde se demuestra que cuando se tienen
debidamente calibrados los parametros odometricos del robot. Se pueden planear trayecto-
8 Capıtulo 1: Introduccion
rias mas largas, de tal forma que se puede eficientar el desempeno del robot. O en [Lee08]
donde se propone una tecnica de calibracion odometrica dirigida a mejorar un sistema de
asistencia para el estacionamiento de automoviles.
Si tomamos en cuenta que el SLAM es un paso hacia la tecnologıa de sistemas de
navegacion autonomos. Es muy probable que los robots moviles van a tener en un futuro im-
plementada alguna tecnica de SLAM. Siendo la calibracion una medida bastante aconsejable
lo que se propone en la tesis es implementar un algoritmo de calibracion en combinacion con
una tecnica de SLAM para obtener los parametros odometricos. Particularmente se propone
utilizar la tecnica propuesta por Kaess denominada “Suavizado y Cartografıa Incremental”.
En [Kaess08a] Kaess asegura que el iSAM tiene las siguientes ventajas:
1. Utiliza toda la informacion recabada a lo largo de la trayectoria en cada iteracion.
2. A medida que evoluciona el algoritmo el punto inicial cambia. Esto es una ventaja
muy importante con respecto a los filtros bayesianos que solamente tienen un punto
inicial.
3. En complejidad compite con el filtro de Kalman.
1.3. Antecedentes
Una de las primeras publicaciones determinadas a estudiar los errores de odometrıa
fue [Wang88] en donde se utiliza un encoder optico para determinar la posicion del robot.
Se definia una trayectoria y con los errores de localizacion se estimaba matriz de covarianza.
Algunos anos mas tarde Boristeing y Feng en [Borenstein94] establecen que gran parte de
los errores de odometria son causados por la incertidumbre de la longitud efectiva de la
base del robot y por la diferencia de longitud de los radios en las llantas. Por lo que propo-
nen el primer metodo de calibracion (algunas actualizaciones del metodo se publicanron en
[Borenstein95]). Lo llamaron “Prueba de Marca de Referencia de la Universidad de Michi-
gan” o UMBmark por sus siglas en ingles (University of Michigan Benchmark test). Consiste
en programar al robot para que recorra una trayectoria cuadrada en sentido horario y en
sentido antihorario. Posteriormente se calcula el error entre la trayectoria real y la trayec-
1.3. Antecedentes 9
toria estimada. Con este error se infiere la magnitud de los parametros odometricos reales.
Otra tecnica similar es la que propone Ivanjko et. al. en [Ivanjko07] donde se argumenta que
en algunas ocaciones el metodo UMBmark puede ser impractico. Debido a que la trayec-
toria del metodo UMBmark puede tener dimensiones considerablemente grandes. Ası que
proponen una trayectoria mas corta. A este tipo de metodos se les conoce como metodos
geometricos. Una de las desventajas de estos metodos es que son vulnerables a errores no
sistematicos. Aunque la desventaja mas notable es que no se sabe a ciencia cierta cual de
todas las trayectorias posibles es mas la adecuada en cada caso.
Tenemos tambien los metodos multisensoriales. Estos metodos por lo general uti-
lizan sistemas NIS (ver seccion 1.1.1) y el filtro extendido de kalman que se usa en dos
formas distintas. Una es la estimacion de los parametros odometricos como se explica en
[Das10] . La otra es mediante la fusion sensorial como en donde se utilizan giroscopios, GPS
y encoders opticos para estimar la localizacion aunque no ahondaremos en este tema ya que
fue tratado en la seccion 1.1.1.
Por ultimo vamos a mencionar las tenicas de calibracion que requieren de una
fuente externa que brinde mediciones de la localizacion del robot despues de cada movimien-
to. Estas tecnicas utilizan el metodo de mınimos cuadrados para obtener los parametros
odometricos. Una de las primeras tecnicas de este tipo fue la propuesta por Antonelli et al.
en [Antonelli05] en donde el modelo odometrico se linealiza para despues tomar un conjun-
to de mediciones y formar un sistema lineal sobredeterminado que se resuelve mediante la
pseudoinversa. El metodo para obtener las mediciones no es una cuestion trivial por ejemplo
en se sugiere un sistema de localizacion basado en triangulacion de camaras. Otra tecnica
de este tipo es la Papadopolous y Misailis en [Papadopolous07] donde tambien linealizan el
modelo odometrico, aunque su enfoque es diferente al de antonelli. Tampoco se menciona
cual metodo de medicion puede ser adecuado. Ambos metodos se explican en el capıtulo 2.
10 Capıtulo 1: Introduccion
1.4. Objetivos de la Tesis
A continuacion se establecen los objetivos de la tesis.
1.4.1. Objetivo general
Actualmente en el Posgrado de Ingenieria Electrica de la Universidad Michoacana
se cuenta con un robot exprerimental para el desarrollo de la investigacion cientifica. El
objetivo principal de esta tesis consiste en conocer de forma experimental las caracterısticas
que definen el desplazamiento de este robot. Este robot asi como cualquier otro esta sujeto
a perturbaciones que generan errores en la localizacion. Sı conocemos las causas que origi-
nan los errores sistematicos podremos mitigar los errores de localizacion. Esto entro otras
cosas nos permitira mejorar la convergencia de las tecnicas de SLAM. Tambien nos permi-
tira conocer las restricciones cinematicas para implementar algoritmos orientados hacia la
evacion de obstaculos.
1.4.2. Objetivos particulares
• Implementar la tecnica de Suavizado y cartografıa.
• Implementar dos metodos de calibracion, el metodo propuesto por Antonelli y el meto-
do propuesto por Papadopolous. Determinar de forma experimental los parametros
odometricos con ambos metodos.
• Comparar ambos metodos para determinar cual tiene un mejor rendimiento.
1.5. Descripcion de Capıtulos
En el capıtulo 2, se describe el modelo odometrico del robot, el proceso de recon-
struccion de una trayectoria a partir los metodos de calibracion y los metodos de calibracion
de los parametros odometricos. En el capıtutlo 3, se desrcriben los experimentos realizados
y se incluyen los resultados de estos experimentos. El capıtulo 4, esta dedicado a las con-
clusiones resultado de la experimentacion y a los trabajos futuros que pudieran derivarse.
El apendice A explica la tecnica de Suavizado y Cartografıa. Finalmente en el capıtulo
1.5. Descripcion de Capıtulos 11
B, se hace una descripcion del metodo de mınimos cuadrados haciendo enfasis en algunos
principios del metodo que se utilizan a lo largo de la tesis.
Capıtulo 2
Modelo Odometrico y Calibracion
El modelo odometico es la parte central de este capıtulo, ya que es mediante este
modelo que podemos definir el movimiento del robot. El resto del capıtulo se organiza como
sigue: en la seccion 2.1 se definen y se describen las caracterısticas Robots de Locomocion
Diferencial. Posteriormente se deduce el modelo cinematico en la seccion 2.1.1. La seccion
2.2 se decribe el proceso odometrico que consiste en la reconstruccion de la trayectoria del
robot a partir de las mediciones obtenidad de los encoders en el robot. En la seccion 2.3
describimos el metodo de calibracion de Antonelli y finalmente en 2.4 se decribe el metodo
propuesto por Papadopolous que son los dos metodos implementados en la tesis.
2.1. Robots de Locomocion Diferencial
Los Robots Moviles de Locomocion Diferencial (RMLD) estan conformados por
un cuerpo rıgido y 2 llantas fijas montadas sobre un eje en comun, cada una con su propio
motor [Dudek00]. Tıpicamente cuentan tambien con al menos una rueda de castor para man-
tener el balance. Los RMLD estan dentro de la clasificacion de robots con restricciones no
holonomicas, es decir que puede rotar sobre si mismo, moverse hacia adelante o hacia atras
pero no puede moverse lateralmente sin cambiar la orientacion. Las principales ventajas de
estos robots son la sencilles de su diseno y la facilidad de su construccion. En contraste su
desventaja mas notable es la dificultad de mantener trayectorias totalmente rectas. Esto se
debe a la sensibilidad a las pequenas variaciones en el terreno y a la sensibilidad del cambio
13
14 Capıtulo 2: Modelo Odometrico y Calibracion
de direccion por la diferencia de velocidad entre sus llantas [Dudek00].
2.1.1. Modelo Cinematico de Robots de Locomocion Diferencial
Se considera que cualquier trayectoria descrita por un RMLD puede representarse
como una curva. Es decir que movimiento del robot se modela como un movimieto circular
(ver figura 2.1). El movimieto circular tiene dos componentes, la componente de velocidad
angular o velocidad de rotacion (a la que nos referiremos por la letra ω).
ω =dϕ
dt(2.1)
Donde ϕ(t) es la posicion angular en radianes. La segunga componente que describe
el movimiento circular es la velociadad de traslacion (que denotaremos co la letra v)
v = ω R (2.2)
La letra R representa al radio de la circunferencia que se le denomina como radio
de curvatura (ver figura 2.1). El termino Centro Instantaneo de Rotacion (CIR ver figura
2.1) se refiere al punto localizado al centro de la circunferencia que describe la trayectoria,
es de donde parte el radio de curvatura. Para representar la velocidad del robot se define el
vector [v, ω]T .
Las ruedas del robot describen el mismo movimiento circular. Solo que cada rueda
tendra un radio de curvatura diferente, el radio de curvatura de la llanta derecha se define
por
Rd = R+D
2(2.3)
De forma similar para la llanta izquierda Ri tenemos
Ri = R− D
2(2.4)
En ambas ecuaciones D es el diametro de la base del robot (ver figura 2.1 y 2.2).
Ası que la velocidad de traslacion de la llanta derecha es
2.1. Robots de Locomocion Diferencial 15
R
CIR
vi
ω
vf
D
2
D
Figura 2.1: Geometrıa de la cinematica en los RMLD [Dudek00]
vd = ω
(R+
D
2
)(2.5)
Del mismo modo la velocidad de traslacion de la llanta izquierda es
vi = ω
(R− D
2
)(2.6)
De la diferencia entre las velocidades vd y vi se obtiene la siguiente ecuacion
vd − vi = ω
(R+
D
2
)− ω
(R− D
2
)= ω D
(2.7)
Con la ecuacion 2.7 se puede definir a la velocidad angular como
ω =vd − vi
D(2.8)
Igualando las ecuaciones 2.5 y 2.6 se establece la siguiente expresion
vd
R+ D2
=vi
R− D2
(2.9)
16 Capıtulo 2: Modelo Odometrico y Calibracion
xi
yi
x
y
v
D
θω
Figura 2.2: Componentes vectoriales de la velocidad del robot [Antonelli05]
Con la ecuacion 2.9 podemos definir el radio de curvatura R en la siguiente forma
R =D(vd + vi)
2(vd − vi)(2.10)
Combinando las ecuaciones 2.2, 2.8 y 2.10 se define la velocidad de traslacion v
como
v =
(vd − vi
D
)D(vi + vd)
2(vd − vi)
=(vi + vd)
2(2.11)
Con las ecuaciones 2.8 y 2.11 podemos definir al vector [v, ω]T como
v
ω
=
12
12
1D
1D
vd
vi
(2.12)
Otra forma de calcular la velociadad de traslacion de las llantas es con el producto
de la velocidad angular de cada llanta por la magnitud de su radio. De manera que podemos
reescribir a la ecuacion 2.12 como se muestra a continuacion
2.1. Robots de Locomocion Diferencial 17
v
ω
=
rd2
ri2
rdD
−riD
ωd
ωi
(2.13)
Aquı ωd y ωi son la velocidad angular de la llanta derecha e izquierda respec-
tivamente. El modelo cinematico de la ecuacion 2.13 nos permite determinar el tipo de
movimiento que realizara robot controlando la velocidad angular de las llantas. Ası que
definimos las siguientes acciones de control:
ωd
ωi
(a) Desplazamiento lineal
−ωd1
−ωd
ωi
ωi
(b) Rotacion
Figura 2.3: Acciones de control de los Robots de Locomocion Diferencial
• El desplazamiento en lınea Recta: en este caso las llantas deben de mantener la mis-
ma velocidad y direccion ωd = ωi (ver la figura 2.3(a)). Al evaluar estos valores en
la ecuacion 2.13 tenemos que la velocidad de rotacion (ω) resulta ser igual a cero.
Dejando a la componente de traslacion (v) como la unica componente de velocidad
sobre el robot. Como en ningun momento se produce un cambio de direccion en el
desplazamiento del robot sigue una lınea recta.
• La rotacion del robot sobre si mismo: en este caso las llantas del robot deben girar
con la misma velocidad pero en sentido opuesto ωd = −ωi (ver la figura 2.3(b)). Al
sustituir estos valores en la ecuacion 2.13 la velocidad de traslacion (v) se anula. Por
18 Capıtulo 2: Modelo Odometrico y Calibracion
lo que la velocidad de rotacion (ω) es la unica componete de velocidad que actua sobre
el robot, haciendo que rote sobre si mismo.
2.2. Proceso Odometrico
La Localizacion del robot esta definida por el vector x = [x, y, θ] donde x, y son las
coordenadas en el plano y θ es la orientacion del robot. Ası que es necesario transformar el
vector [v, ω]t en x[x, y, θ] mediante la descomposicion de v en dos componentes ortogonales
(ver figura 2.4), como se muestra en la ecuacion
x
y
θ
=
v cos(θ)
v sen(θ)
ω
(2.14)
vy
x
Figura 2.4: Descomposicion del vector v
A esta ultima ecuacion en la literatura se le conoce como ecuacion cinematicas. Esta
ecuacion cinematicas puede transformarse en distancia recorrida mediante las siguientes
integrales definidas
xi+1 =xi +
∫ T
0v(t)cos(θ(t))dt
yi+1 =yi +
∫ T
0v(t)sin(θ(t))dt
θi+1 =θi +
∫ T
0ω(t)dt
(2.15)
2.3. Calibracion Odometrica Basada en el Metodo de Mınimos Cuadrados por Antonelli19
Aqui T equivale al perıodo de muestreo. Particularmente si se resuelven las in-
tegrales numericamente por el metodo de la regla del punto medio tenemos la siguiente
ecuacion
xk+1
yk+1
θk+1
=
xk + Tvk cos(θk + Tωk/2)
yk + Tvk sin(θk + Tωk/2)
θk + Tωk
(2.16)
En donde k representa la k-esima muestra. Por lo que la posicion final del robot
despues de K muestras es calculada mediante la sumatoria de los pequenos incrementos en
la posicion, en donde para x tenemos
xK = x0 +T
2
K−1∑k=0
(rdωd,k + riωi,k) cos(θk + Tωk/2) (2.17)
Para y
yK = y0 +T
2
K−1∑k=0
(rdωd,k + riωi,k) sin(θk + Tωk/2) (2.18)
Y finalmente para θ
θK = θ0 +T
D
K−1∑k=0
(rdωd,k − riωi,k) (2.19)
Note que se sustituyo v y ω por la eqivalencia de la ecuacion 2.13.
2.3. Calibracion Odometrica Basada en el Metodo de Mıni-
mos Cuadrados por Antonelli
Antonelli hace especial enfasis en la matriz de parametros de la ecuacion 2.13 que
se muestra a continuacion
C =
rd2
ri2
rdD
−riD
(2.20)
20 Capıtulo 2: Modelo Odometrico y Calibracion
En esta matriz tenemos los parametros de odometrıa rd, ri y D. Donde es evidente
que la relacion que guardan entre ellos no es lineal, por lo que en Antonelli sugiere calcular
todos los elementos ci,j en lugar de tan solo los 3 parametros odometricos. De esta forma
la no linealidad del problema se linealiza.
Para definir los residuales relacionados con la estimacion de los elementos c2,1 y
c2,2 en 2.19 se despeja la condicion inicial θ0 en la siguiente forma
θK − θ0 = Tc2,1
K−1∑k=0
ωd,k + Tc2,2
K−1∑k=0
ωi,k (2.21)
Para simplificar la notacion se define la matriz Φθ ∈ R1×2 como
φθ = T[ ∑K−1
k=0 ωd,k∑K−1
k=0 ωi,k
](2.22)
Con φθ se rescribe a 2.22 en una forma mas compacta como se muestra acontin-
uacion
θK − θ0 = φθ
c2,1
c2,2
(2.23)
Si el robot realiza una trayectoria con P mediciones de localizacion se forma el
vector ∆Φ ∈ RP como se muestra a continuacion
∆Φ =
θK,0 − θ0,0...
θK,p − θ0,p...
θK,P − θ0,P
(2.24)
Tambien se forma un conjunto de vectores φθ,p correspondiente a cada medicion
que forman a la matriz Φθ ∈ RP×2 en la siguiente forma
2.3. Calibracion Odometrica Basada en el Metodo de Mınimos Cuadrados por Antonelli21
Φθ =
φθ,0
...
φθ,p
...
φθ,P
(2.25)
Entonces se puede establecer un sistema lineal con incognitas c2,1 y c2,2 como se
muestra a continuacion
Φθ
c2,1
c2,2
= ∆Φ (2.26)
Este sistema puede solucionarse por la pseudoinversa (ver seccionB.3) como se
muestra acontinuacion
c2,1
c2,2
= (ΦTθ Φθ)
−1ΦTθ ∆Φ (2.27)
De forma similar se define la matriz φxy ∈ R2×2 a partir de las ecuaciones 2.17 y
2.18 como se muestra a continuacion
φxy = T
∑K−1n=0 ωd,k cos(θk + Tωk/2)
∑K−1k=0 ωi,k cos(θk + Tωk/2)∑K−1
n=0 ωd,k sin(θk + Tωk/2)∑K−1
k=0 ωi,k sin(θk + Tωk/2)
(2.28)
Asi que el residual con incognitas c1,1 y c1,2 se define por
xK − x0
yK − y0
= Φx,y
c1,1
c1,2
(2.29)
Despues de realizar una trayectoria con P desplasamientos se forma el siguiente
vector ∆xy ∈ R2P de mediciones
22 Capıtulo 2: Modelo Odometrico y Calibracion
∆xy =
xK,0 − x0,0
yK,0 − y0,0...
xK,p − x0,p
yK,p − y0,p...
xK,P − x0,P
yK,P − y0,P
(2.30)
Asociado al vector de medicion se forma la matriz Φxy ∈ R(2P )×2 como se muestra
a continuacion
Φxy =
φxy,0
...
φxy,p
...
φxy,P
(2.31)
Para tener el sistema con los terminos c2,1 y c2,2 como incornitas siguiente
Φxy
c1,1
c1,2
= ∆xy (2.32)
Sistema que se puede resolver mediante la pseudoinversa (ver seccion B.3)
c1,1
c1,2
= (ΦTxyΦxy)
−1ΦTxy∆xy (2.33)
Note que se deben estimarse c2,1 y c2,2 para poder evaluar θk , ωk en Φxy.
2.4. Calibracion Odometrica Basada en el Metodo de Mınimos Cuadrados porPapadopolous 23
2.4. Calibracion Odometrica Basada en el Metodo de Mıni-
mos Cuadrados por Papadopolous
Para la estrategıa propuesta por Papadopolous vamos retomar la ecuacion cin-
ematica y la vamos a escribir en la siguiente forma
x
y
θ
=
(ωdrd + ωiri
2
)cos(θ)(
ωdrd + ωiri2
)sen(θ)
ωdrd − ωiriD
(2.34)
Si conocemos las velocidades ωd y ωi vamos a establecer a la ecuacion 2.34 como
una funcion x = f(R) en donde R = [rd, ri, D]T . Es decir que vamos a estimar la velociadad
del robot en funcion de los parametros odometricos. Sı linealizamos a la funcion f(R)
mediante la expansion de la serie de Tylor de primer orden obtenemos
f(R) ≈ f(R0) +∇f(R0)δR (2.35)
En donde δR se define como
δR =
rd − rd0
ri − ri0
D −D0
(2.36)
Aqui rd0, ri0 y D0 son los valores del punto inicial que en este caso son los valores
nominales de los parametros odometricos del robot. Al tomar una medicion de la velociadad
del robot (x) podemos establecer la siguiente equivalencia
x− f(R0) = ∇f(R0)δR (2.37)
Aquı vamos a definir δx = x − f(R0) que en forma de vector escribiremos como
δx = [δx, δy, δθ]T . Ası que a la ecuacion 2.37 se puede reescribir en forma matricial
24 Capıtulo 2: Modelo Odometrico y Calibracion
δx
δy
δθ
=
∂(vcosθ)
∂rd
∂(vcosθ)
∂ri
∂(vcosθ)
∂D∂(vsinθ)
∂rd
∂(vsinθ)
∂ri
∂(vsinθ)
∂D∂(θ)
∂rd
∂(θ)
∂ri
∂(θ)
∂D
δrd
δri
δD
(2.38)
En donde definiremos a la matriz A como
A =
∂(vcosθ)
∂rd
∂(vcosθ)
∂ri
∂(vcosθ)
∂D∂(vsinθ)
∂rd
∂(vsinθ)
∂ri
∂(vsinθ)
∂D∂(θ)
∂rd
∂(θ)
∂ri
∂(θ)
∂D
(2.39)
Si desarrollamos las derivadas en A obtenemos
A =
ωd
2cosθ − vsenθ
ωdrdD
ωi
2cosθ + vsenθ
ωiriD
vsenθθ
Dωd
2senθ + vcosθ
ωdrdD
ωl
2senθ − vsenθ
ωiriD
−vcosθθ
Dωd
D−ωl
D− ω
D
(2.40)
Al integrar la matriz A podemos establecer el sistema δx = AδR como
δx
δy
δθ
= A
δrd
δri
δD
(2.41)
Aplicando la integral numerica de la regla del punto medio entonces los elementos
de A se calculan en la siguiente forma
2.4. Calibracion Odometrica Basada en el Metodo de Mınimos Cuadrados porPapadopolous 25
a1,1 =
K∑k=1
Tωd
2cos
(θk +
Tω
2
)− (Tv)sin
(θk +
Tω
2
)ωdrdD
(2.42)
a1,2 =K∑k=1
Tωi
2cos
(θk +
Tω
2
)+ (Tv)sin
(θk +
Tω
2
)ωdrdD
(2.43)
a1,3 =
K∑k=1
(Tv)sin
(θk +
Tω
2
)θk +
Tω2
D(2.44)
a2,1 =
K∑k=1
Tωd
2sin
(θk +
Tω
2
)+ (Tv)cos
(θk +
Tω
2
)ωdrdD
(2.45)
a2,2 =
K∑k=1
Tωi
2sin
(θk +
Tω
2
)− (Tv)cos
(θk +
Tω
2
)ωdrdD
(2.46)
a2,3 =K∑k=1
−(Tv)cos
(θk +
Tω
2
)θk +
Tω2
D(2.47)
a3,1 =
K∑k=1
Tωd (2.48)
a3,2 =
K∑k=1
−Tωi (2.49)
a3,3 =K∑k=1
−Tω
D(2.50)
Para eliminar el ruido y mejorar la estimacion de los parametros en el sistema se
propone que el robot realice una trayectoria de P desplazamientos para formar el siguiente
sistema sobredeterminado
δx1
δx2...
δxp...
δxP
=
A1
A2
...
Ap
...
AP
δrd
δri
δD
(2.51)
Aqui vamos a definir la matrices Ac ∈ R3P×3 donde Ac = [A1, A2, . . . , AP ] y el
vector Bc ∈ R3P donde Bc = [δx1, δx2, . . . , δxP ]. Ası que podemos obtener los parametros
26 Capıtulo 2: Modelo Odometrico y Calibracion
odometricos por la pseudoinversa (ver seccion B.3) como se muestra a continuacion:
δR = (ATc Ac)
−1ATc Bc (2.52)
Al determinar δR los parametros odometricos se estiman en la siguiente forma
rd = rd0 + δrd (2.53)
ri = ri0 + δri (2.54)
D = D0 + δD (2.55)
Capıtulo 3
Experimentacion y Resultados
Obtenidos
Para el caso de estudio se emplea el robot desarrollado en la Division de Estudios de
Posgrado de la Facultad de Ingenierıa Electrica de la Universidad Michoacana [Concha07],
ver figura 3.1.
Figura 3.1: Robot desarrollado en DEPFIE.
Este robot es del tipo de locomocion diferencial, cuenta con un motor de corriente
27
28 Capıtulo 3: Experimentacion y Resultados Obtenidos
directa sin escobillas Pittman 4441S010 en cada llanta. Cada uno de los cuales incluye un
encoder optico Hewlett-Packard con tres canales y 500 cuentas por revolucion.
3.1. Constante de Conversion Angular
Para poder convertir el numero de pulsos de encoder en una cantidad de desplaza-
miento angular. Es necesario encontrar a cuanto quivale en desplazamiento angular cada
pulso. Esta equivalencia la llamaremos constante de conversion angular y la denotaremos
por la letra α. Para encontrar su valor realizamos el siguiente experimento. Colocamos una
marca distintiva en la llanta en un punto A. Despues se hace girar la llanta sin tocar el
piso. Al terminar la rotacion la marca termina en un punto B (ver figura 3.2). Midiendo la
longitud de los segmentos a, b y c con la ley de cosenos establecemos la ecuacion 3.1 para
calcular el desplazamiento angular de la rueda que denotamos por la letra ϕ (ver figura 3.2)
ϕ = arc cos
(a2 + b2 − c2
2ab
)(3.1)
bC
B
a c
ϕ
A
Figura 3.2: Estimacion de constante angular por cuenta
Si N representa el numero de cuentas que el encoder registras durante la rotacion,
la propocion a la que equivale α es la siguiente
α =ϕ
N(3.2)
Al repetir el experimento con distintos valores deN , obtenemos una serie de valores
que al promediarse nos resulta
3.2. Estimacion de Matriz de Parametros 29
α = 8.8036541× 10−3 rad
3.2. Estimacion de Matriz de Parametros
Las dimensiones nominales1 de los parametros de odometrıa son: 400mm de base
(L) y 150mm de dıametro en cada llanta (rd, ri). Entonces la matriz de parametros en lo
que denominaremos como modelo nominal es el siguiente
C =
37.5 37.5
0.189873417 −0.189873417
(3.3)
Para obtener la matriz de parametros del modelo real es necesario optar por una
trayectoria . En [Antonelli05] no se especifica ninguna, sin embargo se recomienda una
trayectoria bidireccional debido a que anulan los errores no sistematicos. De manera que se
elige una trayectoria cuadrada bidireccional (ver figura 3.2), que consiste de 4 movimientos
con un avance de 1 metro y 3 giros de 90o en sentido antihorario, como se muestra en la
figura 3.3(a). Despues se realizan los mismos movimientos pero en sentido horario, como se
muestra en la figura 3.3(b).
Ası que se obtiene un conjunto de mediciones de localizacion con la trayectoria
mencionada. Con estas mediciones aplicamos el algoritmo de Antonelli et al. De manera
que obtenemos la siguiente matriz de parametros
C1 =
38.816867 38.909966
0.190729 −0.190893
(3.4)
En el algoritmo propuesto por Papadopolous y Misailidis obtenemos los siguientes
parametros odometricos
1Dimensiones que deberıa tener el robot idealmente
30 Capıtulo 3: Experimentacion y Resultados Obtenidos
rd = 78.613142
ri = 78.841121
L = 412.814892
Por lo que la matriz de parametros es
C2 =
39.30657 39.42056
0.1891129 ?0.190984195
(3.5)
Note que en la matriz C1 como en la matriz C2 tenemos que la rueda izquierda es
ligeramnete mas grande.
−90o
1.0m
−90o −90o
−90o
(a) Trayectoria en direccion horaria.
90o
90o
90o
90o
1.0m
(b) Trayectoria en direccion antiho-
raria.
Figura 3.3: Trayectoria bidireccional
3.3. Comparacion de Parametros Odometricos
La prediccion odometrica es el calculo del numero de pasos de encoder necesarios
para que el robot avance una cierta distancia determinada. Bajo la hipotesis de que de la
calidad del modelo odometrico depende la calidad de la estimacion. Compararemos cada
3.3. Comparacion de Parametros Odometricos 31
tecnicas de calibracion estimando el error en cada prediccion. Tambien vamos ha realizar la
reconstruccion odometrica con las mediciones de velocidad registradas por los encoders.
El error en el movimiento de traslacion del robot lo calcularemos mediante la
siguiente ecuacion:
Errord =√
(xreal − xestimada)2 + (yreal − yestimada)2 (3.6)
En donde se refiere al promedio. Los resultados obtenidos se muestran en las
siguientes tablas
Modelo odometrico nominal
Prediccion N. P. Encoder Errord Error por Reconstruccion
1000mm 1514 30.21321mm 28.55128mm
5000mm 7572 182.22743mm 162.1030mm
10000mm 15145 357.32210mm 333.22520mm
Modelo odometrico con parametros obtenidos con el Metodo de Antonelli
Prediccion N. P. Encoder Errord Error por Reconstruccion
1000mm 1461 5.9622mm 6.8610mm
5000mm 7310 55.7900mm 36.79256mm
10000mm 14621 80.5652mm 70.62541mm
Modelo odometrico con parametros obtenidos con el metodo de Papadopolous
Prediccion N. P. Encoder Errord Error por reconstruccion
1000mm 1442 13.89364mm 11.23589mm
5000mm 7214 60.44655mm 48.65881mm
10000mm 14428 114.93251mm 100.92331
En estas tablas N.P. Encoder se refiere al numero de pasos de encoder. Para los
errores de rotacion el error es simplemente la diferencia absoluta
Errorθ =√
(θreal − θestimada)2 (3.7)
Los resultados obtenidos son los siguientes
32 Capıtulo 3: Experimentacion y Resultados Obtenidos
Modelo Odometrico Nominal
Prediccion N. P. Encoder Errorθ360o 1903 6.73616o
720o 3806 13.34244o
1080o 5709 19.95177o
Modelo odometrico con parametros obtenidos con el metodo de Antonelli
Prediccion N. P. Encoder Errorθ360o 1870 0.43243o
720o 3740 2.14871o
1080 5610 3.29819o
Modelo odometrico con parametros obtenidos con el metodo de Papadopolous
Prediccion N. P. Encoder Errorθ360o 1874 0.69819o
720o 3748 3.30244o
1080o 5617 4.41888o
Capıtulo 4
Conclusiones
4.1. Conclusiones Generales
Es evidente que el metodo de calibracion que nos brinda los mejores parametros
odometricos es el propueto por de Antonelli et al. Al parecer el metodo de papadopolus
acarrea mas errores numericos al emplear un mayor numero de integrales numericas. Otra
fuente de errores numericos es linealizacion del modelo cinematico empleando la expancion
de la serie de Taylor de primer orden. Mientras que Antonelli no modifica en forma tan dra-
matica las propiedades del modelo cinematico para linealizarlo. Otro aspecto en el metodo
de Papadopolous es que se puede afectar la convergencia cuando se eligen los puntos iniciales
estan alejados de los valores reales.
En lo que respecta a las causas que originan errores odometricos. Tenemos que
basados en la experimentacion encontramos que la diferencia en el tamano de los radios
de las llantas origina una desviacion en la trayectoria del robot. Esta desviacion se va
acumulando con el paso del tiempo. Otra causa que puede ser determinada a partir del
modelo es la variacion en la velocidad entre las llantas. Pero en este caso pudimos evidenciar
que estas perturbaciones no tienen efectos considerables sobre la direccion del robot ya que
cuenta con un buen algoritmo de control.
Cabe mencionar que se ha encontrado que cuando las llantas de castor giran la
friccion con el suelo crea una componente de fuerza que provoca una ligera desviacion en
33
34 Capıtulo 4: Conclusiones
la direccion del robot. En [Papadopolous07] se menciona que este efecto es sumamente
difıcil de modelar, ası que se propone utilizar una omnirueda que es capaz de brindar al
robot un comportamiento mas estable. Este asi como muchos otros fenomenos prodrıan ser
considerados. Pero a costa de complicar el modelo y con resultados inciertos. Ası que lo mas
conveniente es modelar estos fenomenos como variables aleatorias que deben ser anadidas al
modelo cinematico. El asunto interesante es encontrar en forma de encontrar los parametros
y la distribucion adecuada para estas variables.
Podemos ver en las figuras 4.1 y 4.2 como es que la calibracion afecta en forma pos-
itiva a las metodologıas que utilizan la informacion odometrica. Ambas figuras se tomaron
en la misma iteracion, con lo mismos datos y con la misma tecnica de SLAM que en este
caso es el iSAM. La diferencia esta en que en la figura 4.1 tenemos la construccion del mapa
usando el modelo odometrico sin calibrar.
Figura 4.1: Convergencia de la tecnica de Suavizado y Cartografıa con el modelo sin calibrar
Mientras que en 4.2 se emplea el modelo calibrado. Esto se debe a que al tener
valores iniciales mas precisos el algoritmo converge mas rapido.
4.2. Trabajos Futuros
1. El comportamiento caotico en el movimiento del robot puede ser abordado en for-
ma probabilista. Empleando una metodologıa como la propuesta por Albores en
[Albores09]. Que permite modelar la covarianza del movimiento del robot.
4.2. Trabajos Futuros 35
Figura 4.2: Convergencia de la tecnica de Suavizado y Cartografıa con el modelo calibrado
2. En el metodo iSAM emplea una forma de resolver el problema de mınimos cuadrados
en forma incremental. Es posible usar este mismo metodo para resolver la calibracion
propuesta tanto por Antonelli como la de papadopolous en forma activa.
3. La desventaja mas notable de todos los metodos de SLAM es que cuando el mapa
crece hasta tener una dimension considerable. La complejidad del algoritmo no permite
resolver en un tiempo razonable. Por lo que una forma de resolver este problema es
segmentando el mapa en un conjunto de mapas mas pequenos. En [Ni07] se propone
una tecnica de suavizado en este sentido. Aunque solo esta orientado a mostrar como
es que se puede construir el sistema de ecuaciones y en lo que se refiere al criterio de
segmentacion no es nada claro. Como trabajo futuro se podrıa proponer un criterio
basado en la convergencia del mapa.
Apendice A
Suavizado y Cartografıa
A.1. Notacion
La localizacion del robot en el entrono es denotada por el vector xi. La trayectoria o
conjunto de posiciones del robot a traves del tiempo es denotado porX = {x0, . . . , xi, . . . , xM}.
Las marcas en el mapa son denotadas por el vector lj por lo que el mapa es el conjunto
de marcas L = {l0, . . . , lj , . . . , lN}. Las acciones de control son denotadas por ui, asi que el
conjunto U = {u1, u2, . . . , uM} representa el historial de acciones de control. Las mediciones
de las marcas se denotan por zk. Asi que el conjunto de mediciones es K = {k1, k2, . . . , kK}
A.2. Funcion de Movimiento
Esta funcion describe el desplazamieno de xi−1 a xi por efecto de la accion de
control ui. Su notacion formal es
xi = f(xi−1, ui) (A.1)
En [Siegwart04] se dice que esta funcion se puede considerar como una trasforma-
cion de coordenadas. Es decir que como la localizacion del robot y las acciones se encuentran
reciden en sistemas diferentes ( global y local respectivamente). Es necesario trasladar el
desplazamiento de la accion de control ui desde el sistema de referencia local hacia el global.
Esto se realiza con la matriz de rotacion R como se muestra a continuacion
37
38 Apendice A: Suavizado y Cartografıa
xlocal
θ
ylocal
ygobal
xglobal
yi
xiO
O’
Figura A.1: Sistema global y local de coordenadas [Siegwart04]
Rui =
cos(θ) −sen(θ) 0
sen(θ) cos(θ) 0
0 0 1
uxi
uyi
uθi
(A.2)
Por lo tanto se puede estimar xi con la siguiente ecuacion
xi = xi−1 +Rui (A.3)
De manera que xi = f(xi−1, ui) se puede definir como se muestra a continuacion
xi
yi
θi
=
xi−1 + uxi cos(θi−1)− uyi sin(θi−1)
yi−1 + uxi sin(θi−1) + uyi cos(θi−1)
θi−1 + uθi
(A.4)
A.3. Modelo de Movimiento
El movimiento desde el punto de vista probabilista decribe en terminos de una fun-
cion de probabilidad condicional la certidumbre de la posicion xi al llevarse acabo la accion
de control ui. A esta funcion de probabilidad se le conoce como el modelo de movimiento y
esta definido por la siguiente ecuacion
p(xt | xi−1, ui) (A.5)
A.3. Modelo de Movimiento 39
θi
uθi
ui
yi−1
yi
xixi−1
x
y
Figura A.2: Modelo Odometrico.
En este enfoque se considera que el movimiento del robot se encuentra descrito
por un proceso gaussiano definido por la siguiente ecuacion
xi = f(xi−1, ui) + wi (A.6)
Donde f(xi−1, ui) es la funcion de movimiento y wi es una variable aleatoria que
representa ruido gaussiano. Es modelada como una distribucion de probabilidad normal
wi = N (0,Λi). Con la ecuacion A.6 podemos rescribir la probabilidad condicional del modelo
de movimiento en forma de una distribucion de probabilidad Normal en
p(xi | xi−1, ui) ∝ exp− 1
2
{[f(xi−1, ui)− xi]
t Λ−1i [(xi−1, ui)− xi]
}(A.7)
A.3.1. Funcion de Observacion
La funcion de observacion es una parte muy importante en el proceso de obser-
vacion. Describe de forma geometrica como se lleva acabo la medicion de una marca lj desde
xi. Dicho modelo es definido por la siguiente ecuacion
zk = h(xik , ljk) (A.8)
40 Apendice A: Suavizado y Cartografıa
En esta funcion las marcas son modeladas como lıneas (ver figura A.3). Existen
varias formas de representar matematicamente a la lınea, en este caso se usa la forma polar
ri = yi cos(φ) + xi sen(φ) (A.9)
Ası que para definir el modelo, en la figura A.3 se observa que como el radio ri y
rj son palalelos entonces
φi = φj − θi (A.10)
En la misma figura observamos que
rj = rd + ri (A.11)
En donde el vector ~rd es la proyeccion ortogonal de ~d sobre ~r. Por lo tanto ~rd esta
definida por la siguiente ecuacion
~rd =
(~rj · ~d~rj · ~rj
)~rj (A.12)
Teniendo en cuenta que ~d = [xi, yi]t y que ~rj = [rcos(φj), rsen(φj)]
t, se resuelve el
escalar de la proyeccion
(~rj · ~d~rj · ~rj
)=
cos(φj)xi + sen(φj)yirj
(A.13)
Si a este escalar lo denotamos como c, resolvemos la norma cuadrada de la proyec-
cion
‖ crj ‖2 = c2r2j [cos2(φj) + sen2(φj)]
= c2r2j
(A.14)
Como rd =‖ crj ‖, de las ecuaciones A.12 y A.14 tenemos que
rd =√c2r2
=cos(φj)xi + sen(φj)yi
rj(rj)
A.3. Modelo de Movimiento 41
x
y
d
rdrj
ri
ri
φj
φi
θi
lj
φi
Figura A.3: Modelo de medicion.
Por lo que rd se puede definir como
rd = cos(φj)xi + sen(φj)yi (A.15)
Teniendo en cuenta que ri = rj − rd, con la ecuacion anterior y la ecuacion A.10
podemos establecer al vector lj por la siguiente ecuacion
ri
φi
=
rj − cos(φj)xi − sen(φj)yi
φj − θi
(A.16)
A.3.2. Modelo de Observacion
Desde un punto de vista probabilista se establece el modelo de observacion que
nos permite representar matematicamente la certidumbre de las mediciones de las marcas.
Formalmente el modelo de observacion es definido por la siguiente funcion de probabilidad
condicional
p(zk | xik , lj,k) (A.17)
42 Apendice A: Suavizado y Cartografıa
Aqui lj,k es la j-esima marca vista en la k-esima observacion. Es importante men-
cionar que se ha supuesto que se conoce la asociacion de las mediciones. Es decir que se
sabe a que mediciones corresponden las marcas. Este enfoque asume que la observacion
se describe mediante un proceso gaussiano. Matematicamente se define por la siguiente
ecuacion
zk = h(xik , ljk) + vk (A.18)
Donde h(xik , ljk) es la funcion de observacion y vk es una variable aleatoria que
representa ruido gaussiano representado por la distibucion de probabilidad normal N (0,Γi).
Asi que el modelo de medicion se puede rescribir en la siguiente distribucion de probabilidad
gaussiana
p(zk | xij , ljk) ∝ exp− 1
2
{[h(xik , ljk)− zk]
t Γ−1k [h(xik , ljk)− zk]
}(A.19)
A.4. SAM
La tecnica de Suavizado y Cartografıa (SAM por sus siglas en ingles) fue prop-
uesta por Dellaert y Kaess en [Dellaert06]. En esta metodologıa se propone utilizar toda
la informacion que se va recabando a lo largo de toda la trayectoria. Por lo que en cada
iteracion se busca mejorar el estimado de toda la trayectoria y de todo el mapa. La idea
fundamental es aplicar el principio de maxima verosimilitud al sistema que de lugar a la
funcion de probabilidad p(X,L,U, Z) producto de la red bayesiana mostrada en la figura
A.5. Que es una representacion grafica del problema de SLAM.
A.5. SAM como Problema de Maxima Verosimilitud
Suponga que se tiene la red bayesiana de la figura A.4 resultado de una primera
iteracion. La probabilidad conjunta de este pequeno sistema, se obtiene mediante el teorema
del producto de bayes en la siguiente ecuacion
A.5. SAM como Problema de Maxima Verosimilitud 43
u1
x0 x1
l2
z1 z2
l1
Figura A.4: Red Bayesiana de SLAM con un movimiento, resultado de una primera iteracion.
p({x0, x1}, {l0},{u1}, {z1, z2}) =
p(x0)p(x1 | x0, u1)p(z1 | x1,1, l1,1)p(z2 | x1,2, l2,1)p(l1,1)p(l2,1)p(u1)(A.20)
Como las probabilidades de observar alguna de las marcas del mapa y realizar
cualquier acccion de control son totalmente aleatorias, la ecuacion A.20 se puede reescribir
como
p({x0, x1}, {l0}, {u1}, {z1, z2}) ∝ p(x0)p(x1 | x0, u1)p(z1 | x1,1, l1,1)p(z2 | x1,2, l2,2) (A.21)
Si se sigue iterando hasta tenerM posiciones y acciones de control,K observaciones
y N marcas, tendremos que la probabilidad conjunta se encuentra definido por
p(X,L,U,Z) ∝ p(x0)
M∏i=1
p(xi | xi−1, ui)
K∏k=1
p(zk | xik , ljk) (A.22)
Para estimar la trayectoria y el mapa se aplica el metodo de maxima verosimilitud,
definido por la siguiente ecuacion
X∗, L∗ = argmaxX,L
p(X,L,U, Z) (A.23)
44 Apendice A: Suavizado y Cartografıa
Si sustituimos los modelos de medicion y movimiento por su forma de distribucion
gaussiana en la ecuacion anterior obtenemos
p(X,L,U, Z) ∝ exp− 1
2
{M∑i=1
‖ (f(xi−1, ui)− xi) ‖2Λi+
K∑k=1
‖ (f(xik , ljk)− zk) ‖2Γk
}(A.24)
Aqui ‖ e ‖2Σ denota la distancia de mahalanobis. Note que como p(x0) es de
distribucion uniforme se puede despresiar su contribucion. Si se aplica el logaritmo negativo
a la ecuacion anterior obtenemos
X∗, L∗ = argminX,L
− log p(X,L,U, Z)
= argminX,L
{M∑i=1
‖ (f(xi−1, ui)− xi) ‖2Λi+
K∑k=1
‖ (f(xik , ljk)− zk) ‖2Γk
} (A.25)
z2
l1 l2
u2u1 uM
x0 x1 x2 xm
z1 z3 z4 zk
l3 ln
Figura A.5: Red Bayesiana de una trayectoria
A.6. SAM Como Problema de Mınimos Cuadrados
Para poder resolver el problema de SAM por medio del metodo de mınimos cuadra-
dos es necesario linealizar los terminos en la ecuacion A.25. Para ello se utiliza la expancion
A.6. SAM Como Problema de Mınimos Cuadrados 45
de la serie de Taylor de primer orden. De manera que si se linealiza el error de movimiento
se obtiene lo siguiente
f(xi−1, ui)− xi
≈{f(x0i−1, ui) + F i−1
i δxi−1
}−{xi0 + δxi
}={F i−1i δxi−1 − δxi
}−{x0i − f(x0i−1, ui)
}={F i−1i δxi−1 − δxi
}− ai
(A.26)
Donde el error entre la prediccion y la odometrıa esta denotado por ai = x0i −
f(x0i−1, ui) y F i−1i es el jacobiano de la funcion fi(·) en el punto de linealizacion x0i−1
definido como
F i−1i =
∂f(xi−1, ui)
∂xi−1
∣∣∣∣x0i−1
(A.27)
De manera similar se linealiza el error en la medicion en la siguiente forma
h(xik , ljk)− zk
≈{h(x0ik , l
0ik) +H ik
i δxik + J jkk δljk
}−{z0k + δzi
}={H ik
i δxik + J jkk δljk − δzi
}−{z0k − h(x0ik , l
0ik)}
={H ik
i δxik + J jkk δljk − δzi
}− ck
(A.28)
Donde ck = z0k − h(x0ik , l0ik) es el error de la medicion, H ik
i y J jkk son el jacobiano
de h(·) con respecto a xik y ljk respectivamente evaluadas en el punto (x0ik , l0jk)
H iki =
∂h(xik , ljk)
∂xik
∣∣∣∣x0ik,l0jk
(A.29)
J jkk =
∂h(xik , ljk)
∂ljk
∣∣∣∣x0ik,l0jk
(A.30)
La ecuacion A.25 puede reescribirse con los terminos linealizados en la siguiente
ecuacion
46 Apendice A: Suavizado y Cartografıa
δΘ∗ = argminδΘ
{M∑i=1
‖ (F i−1i δxi−1 +Gi
iδxi − ai) ‖2 +K∑k=1
‖ (H iki δxi,k + J j,k
k δljk − ck) ‖2}
(A.31)
Aqui la matriz Gii = Idx×dx se incluye para facilitar la notacion. Con un cambio
de variables se puede sustituir la distancia de mahalanobis por la distancia euclideana al
cuadrado, como se muestra a continuacion
‖ e ‖2Σ = eT Σ−1 e
= eT (Σ−1/2Σ−T/2) e
= (Σ−T/2 e)T (Σ−T/2 e)
=∥∥∥Σ−T/2e
∥∥∥2(A.32)
Asi que una vez linealizados los errores y con el cambio de la variable de la ecuacion
A.32 es posible construir un sistemas del tipo
Θ∗ = argminΘ
‖ AΘ− b ‖2 (A.33)
Donde los parametros δxi y δlj se agrupan en el vector Θ ∈ Rn. La matrizA ∈ Rm×n
es una matriz rectangular formada por jacobianos. Por lo que normalmente es muy grande
y dispersa. En tanto que los vectores ai y ck forman al vector b ∈ Rn. La estructura tıpica
de este tipo de sistemas se muestra en la siguiente ejemplo
∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥
Λ−T/21 G1
1
Λ−T/22 F 1
2 Λ−T/22 G2
2
Λ−T/23 F 2
3 Λ−T/23 G3
3
Γ−T/21 H11
1 Γ−T/21 J11
1
Γ−T/22 H22
2 Γ−T/22 J12
1
Γ−T/23 H23
2 Γ−T/23 J13
1
Γ−T/24 H24
2 Γ−T/24 J24
2
δx1
δx2
δx3
δl1
δl2
−
Λ−T/21 a1
Λ−T/22 a2
Λ−T/21 a3
Γ−T/21 c1
Γ−T/22 c2
Γ−T/23 c3
Γ−T/24 c4
∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥∥
2
(A.34)
A.7. Suavizado y Cartografıa Incremental 47
En este ejemplo suponemos que el robot realizo tres movimientos, observo 2 marcas
y tomo cuatro mediciones.
A.7. Suavizado y Cartografıa Incremental
La tecnica de Suavizado y Cartografıa Incremental (iSAM por sus siglas en ingles)
es propuesta por Kaess y Dellaert en [Kaess08b]. Esta tecnica emplea el metodo de fac-
torizacion QR con actualizacion explicado en [Watkins02]. En donde se aprovecha la forma
rectangular de la matriz A en el sistema mostrado en la ecuacion A.33 que al factorizarse
se obtiene
A = Q
R
0
(A.35)
Aqui R ∈ Rn×n es una matriz triangular superior y la matriz Q ∈ Rm×m es una
matriz ortogonal. Si se sustituye A.35 en A.33 se tiene la siguiente expresion
‖AΘ− b‖2 =
∥∥∥∥∥∥Q R
0
Θ− b
∥∥∥∥∥∥2
(A.36)
Debido a que Q es una matriz ortogonal, tenemos la propiedad ‖ QTx ‖2=
xTQQTx = ‖ x ‖2. De manera que podemos premultiplicar en ambos terminos de la ecuacion
anterior por QT sin por ello alterarla. Obtieniendose la siguiente expresion
‖AΘ− b‖2 =
∥∥∥∥∥∥QTQ
R
0
Θ−QT b
∥∥∥∥∥∥2
=
∥∥∥∥∥∥ R
0
Θ−QT b
∥∥∥∥∥∥2
(A.37)
Si rescribimos al vector QT b en dos vectores, d ∈ Rn y e ∈ Rm−n en QT b = [d, e]T
para sustituir en la ecuacion anterior obtenemos
48 Apendice A: Suavizado y Cartografıa
‖AΘ− b‖2 =
∥∥∥∥∥∥ R
0
Θ−
d
e
∥∥∥∥∥∥2
= ‖RΘ− d‖2 + ‖e‖2
El vector e se considera como un residuo, ası que el sistema queda determinado
por la siguiente ecuacion
RΘ = d (A.38)
Dado que R es una matriz triangular superior, el sistema se puede solucionar por
medio de la sustitucion hacia atras ahorrando mucho esfuezo de computo. Posteriormente
viene un proceso que se llama actualizacion. Que es cuando se agrega una nueva restriccion
al sistema. Donde se tiene la siguiente equivalencia
QT 0
0 I
A
w
=
R
w
(A.39)
Aqui w representa la nueva restriccion. Simultaneamente se agrega un vector γw
con los elementos correspondientes a la restriccion w en el vector de observaciones. De
manera que podemos escribir la siguiente ecuacion
R
w
[ Θ]=
d
γw
(A.40)
Note como [R w]T puede ser interpretado como una matriz A y [d γw]T como un
vector b. Salvo que en este caso el proceso de factorizacion esta casi concluido. Ası que
despues de muy pocas operaciones obtenemos una matriz triangurar superior. Para resolver
de nuevo mediante una sustitucion hacia atras.
A.8. Asociacion de Datos
La asociacion de datos dentro del esquema de SLAM se refiere al proceso en el
cual se infere a cual de las marcas del mapa corresponde una medicion. O si la medicion
A.8. Asociacion de Datos 49
correspode a una nueva marca que debe anadirse al mapa.
A.8.1. Asociacion de Datos por Maxima Verosimilitud
A esta tecnica de asociacion tambien se le conoce como la prueba de compatibilidad
individual. Donde para poder inferir si una medicion corresponde a una marca determinada,
es necesario conocer la probabilidad de que la medicion zk sea causada por la marca j.
Probabilidad definida por la siguiente ecuacion
p(zk, jk = j | Z) (A.41)
En esta ecuacion zk indica que la medicion realizada no tiene aun una asociacion.
Esta probabilidad esta directamente relacionada con la probabilidad que se tiene de encon-
trarse en una posicion determinada. No es evidente esta relacion en la ecuacion ?? ya que
se ha marginalizado la variable x. Como se muestra a continuacion
p(zk, jk = j | Z) =
∫xp(zk, jk = j | x,Z)p(x) (A.42)
Si cada una de estas probabilidades se expresa por una funcion de probabilidad
gaussiana, tendremos la siguiente ecuacion
p(zk, jk = j | Z) =
∫x
1√2πdet(Γ)
e‖hikj(x)−zk‖2Γ1√
2πdet(Σ)e‖x−x‖2Σ (A.43)
Al resolver la integral tenemos
p(zk, jk = j | Z) =1√
2πdet(Cikj)e‖hikj(x)−zk‖2Cikj (A.44)
Donde Cikj es la matriz de covarianza definida por
Cikj =∂hik,j∂x
∣∣∣∣x
Σ∂hik,j∂x
∣∣∣∣x
+ Γ (A.45)
Asi que temenos un conjunto de observaciones Zi = {zk, . . . , zK} tomadas desde
la posicion xi. Y cada hipotesis de asociacion entre una observacion zk y una marca lj se
puede cuantificar por medio de la distancia de mahalanobis, en la siguiente forma
50 Apendice A: Suavizado y Cartografıa
Mk,j = [h(xi, lj)− zk]T C−1
ikj[h(xi, lj)− zk] (A.46)
Posteriormente para determinar cual de las hipotesis es la mas verosımil se aplica
la prueba Chi-cuadrada. Esta prueba consiste en aceptar las asociaciones que cumplan con
la siguiente condicion
Mk,j < χ2d,α (A.47)
Aqui χ2d,α es un umbral de desicion basado en la distribucion Chi-cuadrada. Este
umbral esta definido por 2 parametros, la dimension del vector residual denotada por el
subındice d y el nivel de confiabilidad indicado por α. Por ejemplo el umbral para un vector
de dimension 3 y con un nivel de confiabilidad del 95% es χ23,0.95 = 7.8147. Estos valores
umbrales pueden ser obtenidos mediante tablas o por medio de la funcion gamma incompleta
por el metodo de busqueda de raices propuesto en [Press92].
A.8.2. Estimacion de Covarianza
Para poder establecer las asociacion de datos es necesario recuperar la matriz Σ
de covarianzas marginales de la posicion xi y la marca lj
Σ =
Σii ΣTij
Σij Σjj
(A.48)
Esta matriz puede ser obtenida de la matriz de covarianza completa. Definida por
la inversa del cuadrado de la matriz triangular superior de la factorizacion de cholesky
(RTR)−1. Sin embargo estimar de esta forma la covarianza es demasiado costoso. Por lo
que en [Kaess08b] se sugiere un metodo para obtener Σ en tiempo lineal. En este metodo
se define la matriz X con ultimo bloque de la matriz de covarianza completa, es decir
X =
0(n−dx)×dx
Σiidx×dx
(A.49)
En donde X es de dimension Rn×dx, tambien se define la matriz B con la misma
dimension como
A.8. Asociacion de Datos 51
B =
0(n−dx)×dx
Idx×dx
(A.50)
Asi que se puede establecer la siguiente ecuacion
RTRX = B (A.51)
Si se realiza el producto RX y se denota por la letra Y tenemos la siguiente
equivalencia
Y = R−TB (A.52)
Por lo que se puede decir que Y = [0, . . . , R−1ii ] donde Y ∈ Rn×dx y que RX = Y .
De manera que se puede establecer el siguiente sistema
ri−2,i−2 ri−2,i−1 ri−3,i
0 ri−2,i−1 ri−3,i
0 0 ri,i
σi−2,i−2 σi−2,i−1 σi−3,i
σi−2,i−2 σi−2,i−1 σi−3,i
σi−2,i−2 σi−2,i−1 σi,i
=
1
ri−2,i−20 0
∗ 1ri−1,i−1
0
∗ ∗ 1ri,i
(A.53)
De la ecuacion anterior se tiene que 1ri,i
= [0 0 ri,i][σi−2,i−2 σi−2,i−1 σi,i]T . Al
desarrollar el producto tenemos
σi,i ri,i =1
ri,i(A.54)
Por lo tanto se puede obtener σi,i en la siguiente forma
σi,i =1
r2i,i(A.55)
Tambien tenemos la siguiente expresion 0 = [0 ri−2,i−1 ri−3,i][σi−2,i−2 σi−2,i−1 σi,i]T ,
resolviendo el producto tenemos
0 = ri−2,i−1 σi−2,i−1 + ri−3,i σi,i (A.56)
De aquı podemos calcular σi−2,i−1 despejando en la siguiente forma
52 Apendice A: Suavizado y Cartografıa
σi−2,i−1 =
(−1
ri−2,i−1
)(ri−3,iσi,i) (A.57)
Asi sucesivamente se puede ir calculando cada termino de la matriz Σii. Es decir
que este proceso es una forma eficiente de calcular (RTiiRii)
−1.
Finalmente en [Eustice05] se agumenta que como Σjj no crece con el tiempo, se
puede emplear su valor inicial en forma conservativa. De manera que se puede obtener una
aproximacion de la covarianza mediante la siguiente ecuacion
Σ = [Jx Jl]
Σii 0
0 Γ
Jx
Jl
(A.58)
En donde Jx es el jacobiano del modelo de observacion con respecto a x y Jl con
respecto a l y Γ es el error de medicion.
A.9. Resultados Obtenidos
Se genera el mapa de un pasillo que tiene forma circular, se muestra lo robusto del
metodo al cerrar un bucle. Es decir que con esta tecnica de suavizado es posible hacer que
el robot reconosca marcas que ha dejado de ver despues de una trayectoria circular.
Figura A.6: Mapa generado mediante la tecnica de Suavizado
Apendice B
Mınimos Cuadrados
En diversas areas de la ciencia existen problemas en donde se tiene un conjunto
de observaciones que se desean ajustar a funciones que describen la relacion entre una vari-
able dependiente y la variable independiente. A este problema se le conoce como mınimos
cuadrados y es considerado como un problema de optimizacion. Consiste en minimizar la
suma del cuadrado de los errores entre la funcion mencionada anteriormente y los datos
observados. Algunas aplicaciones a este problema son la estimacion de parametros de mod-
elos de sistemas, estimacion de parametros en funciones de distribucion de probabilidad y
la estimacion de trayectorias de cuerpos en movimiento.
Para realizar un buen ajuste es necesario incluir un numero considerable de obser-
vaciones, de manera que los sistemas a resolver estan sobrecondicionado. Es decir que estos
sistemas se caracterizan por tener un mayor numero de restricciones que de variables. En
esta tesis nos enfocaremos a los sistemas lineales del tipo Ax = b. Asi que cuando estos sis-
temas son precondicionados no se puede grantizar que el vector b se encuentre en el espacio
espacio de columnas de A R(A), es decir que el sistema puede ser inconsistente y por lo
tanto la eliminacion gaussiana puede fallar. Este es un problema cuya solucion mas obvia
seria tomar una parte del sistema e ignorar el resto. Pero esto dificilmete se puede justi-
ficar, ya que la solucion obtenida tendrıa errores considerables con algunas restricciones.
Una propuesta mas inteligente es encontrar el valor de x que minimize los errores entre los
elementos del vector b con cada restriccion de Ax.
53
54 Apendice B: Mınimos Cuadrados
B.1. Mınimos Cuadrados en una Dimension
Para llegar a la solucion general del problema tomamos el caso particular mas
sencillo. Que es cuando se tiene una sola dimension como se muestra a continuacion
a1x = b1
a2x = b2
a3x = b3...
amx = bm
(B.1)
El error en se calcula mediante el cuadrado de la distancia euclidiana entre ax y
b. Por lo que la funcion a minimizar es la siguiente
E2 =‖ ax− b ‖2= (a1x− b1)2 + (a2x− b2)
2 + . . .+ (amx− bm)2 (B.2)
Funcion que de forma compacta se puede expresar en forma de sumatorıa como
E2 =
m∑i=1
(aix− bi)2 = 0 (B.3)
b− ax
b
axa
θ
Figura B.1: Proyeccion de b sobre a
Siguiendo el procedimiento habitual de minizacion derivamos e igualamos a cero
dE2
dx= 2ai
m∑i=1
(aix− bi)2 = 0 (B.4)
B.2. Mınimos Cuadrados en Multiples Dimensiones 55
Reescriendo esta ecuacion en una forma vectorial llegamos a la siguiente expresion
aT (b− ax) = [a1, a2, . . . , am]
(a1x− b1)
(a2x− b2)...
(amx− bm)
=
0
0...
0
(B.5)
Es importante notar que este sistema puede expresarse como el producto punto
entre el vector a y el vector ax − b y que ademas este producto debe ser igual a cero. Lo
que geometricamente implica que para optimizar el sistema tenemos que encontrar un valor
para x tal que el vector ax− b sea perpendicular al vector a (ver figura). Si a continuacion
Despejamos aT b llegamos a la siguiente equivalencia
aTax = aT b (B.6)
Como aTa es un escalar entonces x se determina mediante
x =aT b
aTa(B.7)
B.2. Mınimos Cuadrados en Multiples Dimensiones
En el caso general para varias dimensiones tenemos a la matriz A ∈ Rm×n donde
m > n, el vector x ∈ Rn y el vector b ∈ Rm. Como el sistema puede ser inconsistente
entonces se va a considerar que el vector b esta fuera del espacio de columnas de A (en
otras palabras esto quiere decir que no existe una combinacion lineal de A que nos de como
resultado el vector b). Por lo que el error se define como la distancia entre el punto Ax y el
vector b (ver figura).
E =‖ b−Ax ‖ (B.8)
Para minimizar el error el vector b − Ax debe ser perpendicular a el espacio de
columnas de A (ver figura). Esto implica que cada columna de A debe ser perpendicular
con el vector b−Ax como se indica a continuacion
56 Apendice B: Mınimos Cuadrados
b− Ax
Espacio de Columnas de A
b
Ax
Figura B.2: Proyeccion de b sobre el espacio de columnas de A
aT1
aT2
aT3...
aTm
[b−Ax
]= 0 (B.9)
Es decir que el vector b−Ax debe estar dentro del espacio nulo de AT . Una forma
compacta de expresar la ecuacion anterior es la siguiente
AT (b−Ax) = 0 (B.10)
Despejando el vector x
x = (ATA)−1AT b (B.11)
Obteniendo de esta manera la solucion del problema de mınimos cuadrados.
B.3. Pseudoinversa 57
B.3. Pseudoinversa
La ecuacion B.11 nos lleva a un concepto mencionado en la seccion ??, conocido
como la pseudoinversa. La pseudoinversa de A denotada por A+, es por definicion es una
matriz que debe cumplir con el siguiente criterio
I = A+A (B.12)
Sı las columnas de A ∈ Rm×n son linealmente independientes entonces A+ tiene
la siguiente equivalencia
A+ = (ATA)−1AT (B.13)
Asi que la ecuacion B.11 puede ser reescrita como
x = A+b (B.14)
Por eso se dice que el problema de mınimos cuadrados se resuelve mediante la
pseudoinversa. Tambien tenemos otra equivalencia de la matriz pseudoinversa asociada a
la descomposicion QR en donde A = QR, sustituyendo esta igualdad en la ecuacion B.13
tenemos
A+ = (RTQTQR)−1RTQT (B.15)
Como Q es ortogonal entonces QTQ = I entonces
A+ = (RTR)−1RTQT (B.16)
Desarrollando la ecuacion anterior
A+ = R−1R−TRTQT (B.17)
Para obtenerse la definicion alternativa de A+ en
A+ = R−1QT (B.18)
58 Apendice B: Mınimos Cuadrados
B.4. Mınimos Cuadrados Mediante Factorizacion QR
Una forma para resolver el problema de mınimos cuadrados es mediante la factor-
izacion QR, retomando las ecuaciones B.11 y B.18 en la siguiente forma
x = R−1QT b (B.19)
Si consideramos que R es una matriz triangular superiorno es necesario invertirla
para obtener x, ya que la ecuacion
Rx = QT b (B.20)
Puede ser resuelta mediante una sustitucion hacia atras.
B.4.1. Rotaciones de Givens
La factorizacion QR de una matrix de una matriz A se puede realizar por medio
de rotaciones de Givens. En donde se forma una matriz qT de rotacion, como se muestra a
continuacion
qT =
1 0 0 0 0 . . . 0...
. . . . . . 0
0 . . . qi,i . . . qi,k . . . 0
0...
. . . . . . 0
0 . . . qk,i . . . qk,k . . . 0
0...
. . . . . . 0
0 0 0 . . .. . . 1
(B.21)
En donde los elementos qTi,i = cos(φ), qTi,k = sen(φ), qTk,i = −sen(φ), y qTk,k =
cos(φ). En cada rotacion se anula un elementos ai,k donde i > k. O lo que es lo mismo se
anula cada elemento por debajo de la dıagonal (como en la eliminacion gaussiana). Este
procedimiento se repite hasta obtener una matriz triangular superior que en este caso serıa
la matriz R, es decir
B.4. Mınimos Cuadrados Mediante Factorizacion QR 59
R = qT1 . . . qTm A (B.22)
Lo que es lo mismo
R = QTA (B.23)
Teniendo en cuenta que cada matriz de rotacion qT es ortogonal se puede obtener
la matriz Q con el siguiente producto de matrices
Q = q1q2 . . . qm (B.24)
Un aspecto importante es como obtener la dupla (cos(φ), sen(φ)) con los valores
adecuados para anular un elemento ai,k. La propuesta de Given es la siguiente
(cos(φ), sen(φ)) =
(1, 0) si ai,k = 0 −akk
ai,k
√1+
(ak,kai,k
)2, 1√
1+
(ak,kai,k
)2
si | akk |<| aik |
1√1+
(ai,kak,k
)2,
−ai,k√ak,k+
(ai,kak,k
)2
si | akk |>| aik |
(B.25)
Por otro lado uns forma eficiente de estimar a la matriz R es cuando se tiene en
cuenta que las matrices q solo alteran 2 filas de la matriz A1, es decir
ai = cos(φ)ai − sen(φ)ak
ak = sin(φ)ai + cos(φ)ak
En donde ai y ak son las filas i y k de la matriz A respectivamente. De forma
similar el efecto de premultiplicar las matrices q sobre el vector b es el siguiente
1Como es el caso de las operaciones elementales de matrices mediante el producto de matrices
60 Apendice B: Mınimos Cuadrados
qT b =
b1...
cos(φ)bi − sen(φ)bk...
sin(φ)bi + cos(φ)bk...
bn
(B.26)
Referencias
[Albores09] Albores, C., Josep, M., Tur, M., y Gordillo, J. L. Estate your position.
IEEE Robotics and Automation Magazine, 16(2):82–90, Junio 2009.
[Antonelli05] Antonelli, G., Chiaverini, S., y Giusepe, F. A calibration metod
for odometry of mobile robots based on the lest-square technique:
Theory and experimental validation. IEEE Transactions on robotics,
21(5):994–1004, Octubre 2005.
[Ayache88] Ayache, N. y Faugeras, O. Building, registrating, and fusing noisy
visual maps. J. Robotics Research, 7(6):45–65, 1988.
[Azizi03] Azizi, F. y Houshangi, N. Sensor integration for mobile robot position
determination. Systems, Man and Cybernetics, 2:1136–1140, october
2003.
[Bailey02] Bailey, T. Mobile Robot Localization and Mapping and Mapping in
Extensive Outdoor Environments. Tesis Doctoral, The University of
Sydney, August 2002.
[Borenstein94] Borenstein, J. y Feng, L. Umbmark - a method for measuring, com-
paring, and correcting odometry errors in mobile robots. Inf. tec.,
University of Michigan, December 1994.
[Borenstein95] Borenstein, J. y Feng, L. Correction of systematic odometry errors in
mobile robots. Proc. International Conference on Intelligent Robots
and Systems, pags. 569–574, 1995.
61
62 Referencias
[Borenstein96] Borenstein, J., Everett, H., y Feng, L. Navigating Mobile Robots:
Systems and Techniques. Wellesley, 1996.
[Borenstein97] Borenstein, J., Everett, H., Feng, L., y Wehe, D. Mobile robot
positioning: Sensors and techniques. Journal of Robotic Systems,
14(4):231–249, 1997.
[Cadena09] Cadena, C. y Neira, J. Slam in o(log n) with the combined kalman -
information filter. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent
Robots and Systems (IROS), pag. 2069, Octubre 2009.
[Chatila85] Chatila, R. y Laumond, J. Position referencing and consis- tent world
modeling for mobile robots. In Proc. IEEE Int. Conf. Robotics and
Automation, pags. 138–143, 1985.
[Concha07] Concha, S. A. Diseno y Construccion de un Robot Movil. Proyecto
Fin de Carrera, Universidad Michoacana de San Nicolas de Hidalgo,
Mayo 2007.
[Crowley89] Crowley, J. World modeling and position estimation for a mo- bile
robot using ultra-sonic ranging. In Proc. IEEE Int. Conf. Robotics
and Automation, pags. 674–681, 1989.
[Das10] Das, C. Calibration of kinematic odometric parameters for differen-
tial drive mobile robots: An overview. Inf. tec., University of Illinois
in Aerospace Engineering, 2010.
[Dellaert06] Dellaert, F. y Kaess, M. Square root sam: Simultaneous localization
and mapping via square root information smoothing. International
Journal of Robotics Reasearch, 25, 2006.
[Dudek00] Dudek, G. y Jenkin, M. Computational Principles of Mobile Robotics.
Cambridge University Press, 40 West 20th Street, New York, N.Y.,
2000. ISBN 0521 56021 7 hardback.
Referencias 63
[Durrant-Whyte06] Durrant-Whyte, H. F. y Bailey, T. Simultaneous localisation and
mapping (slam): Part i the essential algorithms. IEEE Robotics and
Automation Magazine, 2, June 2006.
[Eustice05] Eustice, R., Singh, H., y Hole, W. Visually navigating the rms titanic
with slam information filters. in Proceedings of Robotics: Science and
Systems, pags. 57–64, 2005.
[Frese05] Frese, U., Larsson, P., y Duckett, T. A multilevel relaxation algo-
rithm for simultaneous localisation and mapping. IEEE Transactions
on Robotics, pags. 196–207, 2005.
[Grisetti] Grisetti, G., Stachniss, C., Grzonka, S., y Burgard, W. A tree
parametrization for efficiently computing maximum likelihood maps
using gradient decent. In Proc. of Robotics: Science and Systems
(RSS), 2007.
[Han99] Han, W. y Ching, T. G. Fuzzy logic kalman filter estimation for 2-
wheel steerable vehicles. Intelligent Robots and Systems, 1999. IROS
’99. Proceedings. 1999 IEEE/RSJ International Conference on, 1:88,
Octubre 1999.
[Irie03] Irie, M., Nagatani, K., y Gofuku, A. Path evaluation for a mobile
robot based on a risk of collision. In Proc. IEEE Int. Conf. Robotics
and Automation (ICRA 2003), 1:485, Julio 2003.
[Ivanjko07] Ivanjko, E., Komsic, I., y Petrovic, I. Simple off-line odometry cali-
bration of differential drive mobile robots. Workshop on Robotics in
Alpe-Adria-Danube Region - RAAD 2007, (16), June 2007.
[Jared10] Jared, A. F. Localization and navigation techniques for indoor
wheeled mobile robots: A literature review (2010). Inf. tec., Poly-
technic Institute of New York University, 2010.
64 Referencias
[Kaess08a] Kaess, M. Icremental Smoothing and Mapping. Tesis Doctoral, Geor-
gia Institute of Technology, Diciembre 2008.
[Kaess08b] Kaess, M., Ranganathan, A., y Dellaert, F. isam: Incremental
smoothing and mapping. IEEE Trans. on Robotics, 24(6):1365–1378,
Diciembre 2008.
[Larsen99] Larsen, T., Hansen, K., Andersen, N., y Ole, R. Design of kalman
filters for mobile robots; evaluation of the kinematic and odometric
approach. Control Applications, 1999. Proceedings of the 1999 IEEE
International Conference on, 2:1021, Agosto 1999.
[Lee08] Lee, K. y Chung, W. Calibration of kinematic parameters of a car-
like mobile robot to improve odometry accuracy. Robotics and Au-
tomation, 2008. ICRA 2008. IEEE International Conference on, pag.
2546, Mayo 2008.
[Luo08] Luo, R. y Jian-Xian Li Chen, C.-T. Indoor localization using line
based map for autonomous mobile robot. Advanced robotics and Its
Social Impacts, 2008. ARSO 2008. IEEE Workshop on, 1:1, Agosto
2008.
[Montemerlo03] Montemerlo, M. FastSLAM: A Factored Solution to the Simultaneous
Localization and Mapping Problem With Unknown Data Association.
Tesis Doctoral, The Robotics Institute Carnegie Mellon University,
Julio 2003.
[Ni07] Ni, K., Steedly, D., y Dellaert, F. Tectonic sam: Exact; out-of-core;
submap-based slam. IEEE International Conference on Robotics and
Automation (ICRA), Julio 2007.
[Olson06] Olson, E., Leonard, J., y Teller, S. Fast iterative optimization of pose
graph whit poor initial estimates. In Proc. IEEE Int. Conf. Robotics
and Automation, pags. 2262–2269, 2006.
Referencias 65
[Papadopolous07] Papadopolous, E. y Misaildis, M. On diferential drive robot odom-
etry with aplication to path planning. Proceedings of the European
Control Conference (ECC ’07), pags. 5492–5499, Julio 2007.
[Petrovskaya08] Petrovskaya, A. y Thrun, S. Model based vehicle tracking for au-
tonomous driving in urban environments. Proceedings of Robotics:
Science and Systems IV, june 2008.
[Popa08] Popa, A., Popa, M., y Silea, I. Mobile robot navigation with obstacle
avoidance capability. Power Electronics and Motion Control Confer-
ence, 2008. EPE-PEMC 2008. 13th, pag. 1225, Septiembre 2008.
[Pornsarayouth09] Pornsarayouth, S. y Wongsaisuwan, M. Sensor fusion of delay and
non-delay signal using kalman filter with moving covariance. Robotics
and Biomimetics, 2008. ROBIO 2008. IEEE International Confer-
ence on, pag. 2045, Febrero 2009.
[Press92] Press, W. H., Flannery, B. P., Teoukolsky, S. A., y Vetterling, W. T.
Numerical Recipes: The Art of Scientific Computing. Cambridge
University Press, 2a edon., 1992.
[Rodrıguez-Losada04] Rodrıguez-Losada, D. SLAM Geometrico en Tiempo Real para
Robots Moviles en Interiores Basado En EKF. Tesis Doctoral, Uni-
versidad Politecnica de Madrid, 2004.
[Shoudong10] Shoudong, H., Yingwu, L., Frese, U., y Dissanayake, G. How far
is slam from a linear least squares problem? Intelligent Robots and
Systems (IROS), 2010 IEEE/RSJ International Conference on, pags.
3011–3016, Diciembre 2010.
[Siegwart04] Siegwart, R. y Nourbakhsh, I. Introduction to Autonomous Mobile
Robots. The MIT Press, 2004.
[Stachniss09] Stachniss, C. Robotic Mapping and Exploration. Springer Publishing
Company, Incorporated, 2009.
66 Referencias
[Thrun99] Thrun, S., Bennewitz, M., Burgard, W., Cremers, A., Dellaert, F.,
Hahnel, D., Rosenberg, C., Roy, N., Schulte, J., y Schulz, D. Minerva:
A second-generation museum tour-guide robot. In Proc. IEEE Int.
Conf. Robotics and Automation (ICRA)), 1999.
[Thrun04] Thrun, S., Montemerlo, M., Koller, D., Wegbreit, B., Nieto, J., y
Nebot, E. Fastslam: An efficient solution to the simultaneous local-
ization and mapping problem with unknown data association. Jour-
nal of Machine Learning Research, 2004.
[Thrun05] Thrun, S., Burgard, W., y Fox, D. Probabilistic Robotics (Intelligent
Robotics and Autonomous Agents). MIT Press, September 2005.
[Wang88] Wang, C. M. Localization estimation and uncetainty analisys for
mobile robots. In Proc. IEEE Int. Conf. Robotics and Automation,
pags. 1230–1235, 1988.
[Watkins02] Watkins, D. Fundamentals of Matrix Computations. Wiley Inter-
science, 2a edon., 2002. ISBN 0471213942.
[Yizhong04] Yizhong, L., Yumei, H., y Enxiu, S. Application of data fusion al-
gorithm based on kalman filter in mobile robot position measuring
system. Intelligent Control and Automation, 2004. WCICA 2004.
Fifth World Congress on, 6:4956, Junio 2004.
[Yun08] Yun, Y., Park, B., y Kyun Chung, W. Odometry calibration us-
ing home positioning function for mobile robot. Robotics and Au-
tomation, 2008. ICRA 2008. IEEE International Conference on, pag.
2116, Mayo 2008.
[Zhan10] Zhan, W. y Dissanayake, G. Efficient monocular slam using sparse
information filters. Information and Automation for Sustainability
(ICIAFs), pag. 311, Diciembre 2010.