calibracion de robots de locomoci on...

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

A todas aquellas personas que hicieron posible esta tesis.

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 Tablas

ix

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


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).


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-


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.


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)


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


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)


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


∆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


δ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


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


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


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


720o 3740 2.14871o

1080 5610 3.29819o

Modelo odometrico con parametros obtenidos con el metodo de Papadopolous


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)


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)


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)


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


δΘ∗ = 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


‖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


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


σ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


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)


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


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 mobile

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.

calibracion de robots de locomoci on...

Documents