Implementaci on de retro-alimentaci on visual para un ...mjgarciav/naoVisualLocalization.pdf ·...
73
Implementaci´ on de retro-alimentaci´ on visual para un robot humanoide en la ejecuci´on de trayectorias Mauricio Josafat Garc´ ıaV´azquez Mayo 2011
Transcript of Implementaci on de retro-alimentaci on visual para un ...mjgarciav/naoVisualLocalization.pdf ·...
Implementacion de retro-alimentacion visualpara un robot humanoide en la ejecucion de
trayectorias
Mauricio Josafat Garcıa Vazquez
Mayo 2011
A Dios, a mi madre Ma. Guadalupe y a mi padre Ramon. A mi abuelo Luısy a mi abuela Romualda. A mi familia, amigos y profesores.
Indice general
1. Introduccion 4
2. Breve introduccion a la robotica, vision computacional y tra-bajos existentes 62.1. Localizacion y SLAM . . . . . . . . . . . . . . . . . . . . . . . 62.2. Vision computacional, formacion de la imagen . . . . . . . . . 72.3. Trabajos existentes . . . . . . . . . . . . . . . . . . . . . . . . 12
3. Deteccion, reconocimiento y rastreo de marcas visuales 163.1. Estimacion de los puntos de fuga . . . . . . . . . . . . . . . . 173.2. Deteccion de cuadrilateros . . . . . . . . . . . . . . . . . . . . 243.3. Rastreo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4. Localizacion 344.1. Filtro de partıculas (Localizacion Monte-Carlo) . . . . . . . . 35
4.1.1. Modelo de movimiento . . . . . . . . . . . . . . . . . . 394.1.2. Modelo de observacion . . . . . . . . . . . . . . . . . . 41
5. Validacion experimental 48
6. Conclusiones y trabajos futuros 59
A. Como hacer modulos y librerıas para ejecutarse a bordo delNAO 61A.1. Terminos importantes . . . . . . . . . . . . . . . . . . . . . . 61A.2. Arquitectura del NAO . . . . . . . . . . . . . . . . . . . . . . 62
A.2.1. Broker . . . . . . . . . . . . . . . . . . . . . . . . . . . 62A.2.2. Proxy . . . . . . . . . . . . . . . . . . . . . . . . . . . 64A.2.3. Modulos locales o remotos . . . . . . . . . . . . . . . . 64
2
A.3. Generador de modulos . . . . . . . . . . . . . . . . . . . . . . 65A.4. Compilacion cruzada . . . . . . . . . . . . . . . . . . . . . . . 66A.5. Compilar una librerıa externa . . . . . . . . . . . . . . . . . . 68
3
Capıtulo 1
Introduccion
La robotica es una ciencia que ha tenido mucho exito en los ultimosanos. Ejemplos hay muchos: brazos roboticos industriales, robots de explora-cion, y recientemente robots humanoides capaces de realizar tareas comple-jas. Podrıa decirse que la robotica es una ciencia consolidada ya que muchosproductos derivados de los estudios en robotica estan actualmente en el mer-cado al alcance de todos. Sin embargo, aun hay mucho por hacer. Sensores,actuadores y un sistema de procesamiento es lo que todos los robots tienenen comun, entonces podrıamos ver a los robots como una maquina capaz depercibir estımulos del mundo, procesarlos y realizar alguna accion por mediode sus actuadores. Este es el paradigma que por anos rigio la investigacionen robotica.
Una cosa muy importante es que necesitamos de los robots. Tal vez muchosno lo vean como una prioridad pero lo cierto es que los hechos lo demuestran.¿Como serıa si se arriesgara la “vida”de robots en vez de la de personas? Locierto es que hay muchas tareas que los seres humanos simplemente no po-demos hacer, y otras que aunque podamos, serıa grandioso si una maquinalo hiciera por nosotros. Entonces, debemos mudar estos robots a ambientescomunes, ambientes donde los seres humanos nos desarrollamos: corredores,habitaciones, oficinas, calles, etc. Mas aun, para que nosotros podamos “con-vivir” con ellos, tenemos que aceptarlos. Serıa mas facil si estas maquinasse parecen lo mas posible a nosotros, con un torso, extremidades, cabeza.Incluso los detalles importan, como si se puede mover con naturalidad o sipuede imitar comportamientos humanos.
4
Fue en la decada de loa ochenta cuando se introdujeron los primeros mode-los de robots humanoides. Hoy mecanicamente y electronicamente los robotshumanoides estan casi listos para asistirnos en nuestras tareas cotidianas,sin embargo eso no es todo. Introducir la idea de “inteligencia” no es cosasencilla. Simples tareas cognitivas que los seres humanos hacen con extremanaturalidad a los investigadores de robotica les puede tomar varios anos. Masaun, el concepto de autonomıa de decision y de accion implica mas dificul-tades, pues se requiere que el robot este desconectado del mundo y que todoel procesamiento se haga a bordo del robot.
Esta tesis va enfocada a esto, a desarrollar la inteligencia artificial en robotshumanoides. Nuestro principal objetivo es que un robot humanoide se localiceen un ambiente interior. Para completar esta tarea debe utilizar una marcavisual, la cual conoce su ubicacion en un marco de referencia y en base a estael robot podra localizarse en este marco.
La presente tesis se puede dividir en dos partes a grandes rasgos: la partede vision y la parte de localizacion. En el capıtulo 2 se hace una pequenarecopilacion de los trabajos que hay acerca del tema, para descubrir lo queya esta hecho, lo que se puede mejorar y sobre todo donde se puede innovar.En el capıtulo 3 se propone un sistema de deteccion de marcas visuales. Cabemencionar que la vision es uno de los campos de percepcion robotica con maspotencial. Tambien en este capıtulo se propone un algoritmo para el rastreode la marca visual durante la ejecucion de las trayectorias. En el capıtulo4, un sistema de localizacion se implemento. Algo que se tomo mucho encuenta en el desarrollo de los algoritmos y su codificacion fue la robustez deestos, pues queremos que funcionen en ambientes lo mas reales y comunesposibles. Por ultimo en el capıtulo 5 realizamos algunos experimentos paraevaluar el desempeno de nuestros algoritmos y realizamos una discusion de losresultados, ası como plantear trabajos futuros. Ademas se agrega un apendicecon un corto manual de como programar modulos para ejecutarse a bordodel robot humanoide NAO de Aldebaran Robotics.
5
Capıtulo 2
Breve introduccion a larobotica, vision computacionaly trabajos existentes
2.1. Localizacion y SLAM
El problema de localizacion de robots moviles es la determinacion de lapose del robot relativa a algun mapa del entorno. Es uno de los problemasperceptuales mas importantes en robotica. Esto es porque casi todas las ta-reas que realizan los robots requieren conocimiento de su propia posicion yla posicion de los objetos del entorno.
La localizacion tambien puede ser vista como el proceso de establecer unacorrespondencia entre un sistema de coordenadas global y un sistema decoordenadas local del robot. Al conocer esta transformacion el robot puedeubicar objetos en su propio sistema de coordenadas, lo cual es necesario parala navegacion. Entonces el problema de localizacion se resume a encontrar losparametros de esta transformacion. En el caso plano (generalmente usado)es suficiente conocer la posicion (x, y) y su orientacion θ.
Generalmente la pose del robot no puede ser medida directamente. Tampo-co es suficiente a veces mediciones de un unico sensor, pues se puede necesitarmas informacion para desambiguar lugares, como se muestra en la Figura 2.1.
6
Entonces la pose del robot debe ser calculada mediante la integracion de da-tos de sensores a traves del tiempo.
La localizacion asume que se conoce el mapa de forma mas o menos precisa.Este mapa puede ser descrito de distintas maneras. Una forma comun dedescribir los mapas es mediante el uso de marcas visuales (landmarks). Eneste caso el robot debe conocer esta marca visual para que la pueda encontraren el entorno (mediante metodos de vision computacional por ejemplo); ytambien debe conocer la posicion de esta en el mundo.
La tarea de localizacion se hace bajo la premisa de que se conoce el mapadel entorno. Esto es valido en algunas aplicaciones en robotica pero, en lamayorıa de casos, no se cuenta con un mapa o no con uno suficientementepreciso para realizar la tarea. Si el robot fuera capaz de construir un mapadel entorno y localizarse al mismo tiempo, reducirıa mucho los problemas enla navegacion de robots, Figura 2.2. Ademas de que el robot podrıa adaptarsea cambios en el entorno sin necesidad de proveerle de un nuevo mapa.
La localizacion y el mapeo simultaneos (SLAM por sus siglas en ingles) esuna de los problemas clave en el desarrollo de robots completamente autono-mos. Como ya se menciono, esta tesis va a enfocada a la localizacion del robothumanoide NAO. Sin embargo, se deja abierto el camino para extenderlo aun verdadero SLAM. Sobre todo en la parte de vision computacional, ya queno se necesita conocer el landmark, de tal forma que puede ser cualquierelemento sobresaliente del entorno. Sin embargo, si es necesario conocer suposicion en el mundo. Lo que de alguna manera se toma como un mapa.
2.2. Vision computacional, formacion de la
imagen
La mayorıa de los dispositivos fotograficos modernos se basan en el unmodelo sencillo: el modelo de hoyo de aguja el cual se ilustra en la Figura2.3.
En este modelo, la imagen es formada por rayos de luz procedentes de laescena e incidiendo en la caja. El hoyo de aguja tambien se denomina centro
7
(a) (b)
(c) (d)
(e)
Figura 2.1: Localizacion de un robot en un entorno ambiguo, las manchas negrasindican las posibles posiciones del robot. En (a) se observa que el robot puede estaren casi cualquier lado. Conforme va haciendo mediciones comienza a desambiguarsu posicion en el mapa (b). Al transcurrir el tiempo, quedan pocos lugares posiblesdonde se puede encontrar (c) y (d). Finalmente el robot resuelve el problema delocalizacion (e). (Imagen de [18]).
optico del sistema. La geometrıa proyectiva es la herramienta que se usa paraestudiar las relaciones entre la escena y la imagen formada.
En la figura 2.4 se define lo siguiente:
El eje normal al plano Π’ y que pasa por O se le denomina eje optico.
8
Figura 2.2: Mapa hecho por un robot movil usando un laser. (Imagen de [18]).
Figura 2.3: El modelo de hoyo de aguja. (Imagen de [5]).
El punto donde se intersecta el eje optico con el plano Π’ es el puntoprincipal.
La distancia focal: z′ = f ′
Se puede mostrar con simple geometrıa que la relacion entre los puntos enel plano de la imagen x′ y y′ y los puntos en la escena x, y, z es la siguiente:
x′ = f ′x
z
y′ = f ′y
z.
(2.1)
Las coordenadas (u, v) en la imagen son coordenadas sobre el sensor fısicopor ejemplo un CCD. Para pasar de cordenadas (x′, y′) (en unidades de lon-
9
Figura 2.4: Relaciones en proyeccion perspectiva (Imagen de [5]).
gitud, por ejemplo milimetros) a las coordenadas en pixeles hay varias cosasque se tiene que tomar en cuenta.
Una ecuacion mas general que relaciona las coordenadas de los puntos enel mundo expresadas en el marco de la camara (x, y, z) y las coordenadas enla imagen en pixeles (u, v) es:
u = αux
z+ u0
v = αvy
z+ v0.
(2.2)
Donde αu, αv representan la distancia del centro optico al plano de laimagen expresada en numero de pixeles. Por otro lado u0, v0 representan lascoordenadas del punto principal del sensor en coordenadas del sensor (origenesquina superior izquierda).
A partir de aquı se define la matriz de parametros intrınsecos 3 x 3
K =
αu 0 u00 αv v00 0 1
, los parametros αu, αv, u0, v0 se tienen que encontrar
mediante algun metodo de calibracion de la camara.
10
Por lo que, uv1
∝ K
xyz
, (2.3)
donde (x, y, z)T son las coordenadas 3D de los puntos de la escena en elmarco de la camara.
La proyeccion perspectiva tiene efectos importantes que hay que tener encuenta. Por ejemplo en la proyeccion perspectiva las distancias no se conser-van: las distancias en la imagen formada dependen de la distancia del centrooptico a la escena, ver Figura 2.5.
Figura 2.5: Los objetos mas lejanos al centro optico aparentan estar mas pequenosque los mas cercanos. (Imagen de [5]).
Hay otro conocido efecto de la proyeccion perspectiva. La figura 2.6 mues-tra como dos lıneas paralelas en la escena se proyectan como lıneas conver-gentes a un punto de fuga. Tambien se observa que las lıneas paralelas alplano de proyeccion (por ejemplo L) no convergen a un punto dentro de laimagen, sino que convergen a un punto en el infinito.
En la geometrıa perspectiva tambien se observa el hecho de que el conjuntode puntos de fuga correspondiendo a lıneas paralelas perteneciendo a unmismo plano forman una lınea, por ejemplo la lınea del horizonte.
11
Figura 2.6: Interseccion de las lıneas paralelas de la escena en puntos de fuga enla imagen. (Imagen de [5]).
2.3. Trabajos existentes
El problema de localizacion en robotica ha sido constantemente abordadopor la comunidad cientıfica. Ademas hay en la literatura numerosos artıculosacerca de los subproblemas y de la forma en como se plantean en la presentetesis. La localizacion es una tarea inherente de la navegacion de robots, in-cluidos los humanoides. Se supone que al hacer solamente la planificacion demovimientos y ejecucion de trayectorias como en [7], en general los robotsno seran capaces de realizar dicha tarea tal cual se le asigno; esto debido adistintas razones como el deslizamiento de las extremidades del robot conel suelo. Hacer la tarea de ejecucion de trayectorias en lazo cerrado es lasolucion y para esto se requiere la localizacion.
Existen una gran cantidad de maneras de realizar la tarea de localizacionpara los robot humanoides. Y cada una de estas conlleva otra buena cantidadde variantes. Generalmente se usan metodos probabilısticos como localizacionMonte Carlo (MCL) o filtros de Kalman con todas sus variantes (ver [18]). Loimportante de estos metodos es que integran la informacion de los sensoresinternos del robot (por ejemplo odometrıa, giroscopios, acelerometros etc.)con informacion de sensores exteroceptivos (camaras, laser, sonares etc.).
En [20], usan la informacion de la odometrıa del robot humanoide NAOy la integran con su sensor laser mediante un filtro. Otros han usado lavision como principal metodo de observacion para la localizacion de robots
12
humanoides [17, 11]. La vision es una poderosa herramienta para realizaresta tarea, incluso en ambientes tan inhospitos como el suelo marciano [12].Trabajos acerca del uso de la vision para la localizacion hay muchos, conbuenos resultados en robots de ruedas por ejemplo, sin embargo el uso de lavision para la localizacion de robots humanoides es una tarea mas complicadaque en el caso de robots de ruedas..
En vision, el calculo de los puntos de fuga en la imagen para la extrac-cion de informacion es frecuentemente usado, especialmente para localizacionpura o mapeo y localizacion simultaneos (SLAM) [4]; tambien es usado enla deteccion de estructuras rectangulares [13]. En [9], proponen un eficientemetodo para calcular los puntos de fuga. Mencionan un metodo completodesde como obtener los segmentos en la imagen, calcular los puntos de fugae incluso poder calibrar parcialmente la camara con la ayuda de los puntosde fuga calculados.
Lo que nos parecio interesante de este artıculo fue la parte donde mencionael algortimo Expectation-Maximization (EM) para agrupar los segmentos enlas orientaciones principales, y ası en las intersecciones de estos segmentosencontraremos los puntos de fuga. Con esto se logra agrupar a los segmen-tos en las tres orientaciones principales, dejando a un lado los segmentosencontrados que no pertenecen a estas. El objetivo del EM es maximizar laprobabilidad de que un segmento pertenezca a un punto de fuga dado (expec-tation), una vez agrupados se recalculan los puntos de fuga (maximization)y se itera hasta un criterio de paro.
Una cosa muy importante que mencionan es que este algortimo es muysensible a la inicializacion, por lo que tambien proponen un metodo parainicializarlo: dado un conjunto de segmentos detectados se forma un histo-grama y se buscan sus modas. Como en la inicializacion se puede encontrarmas modas que el numero de puntos de fuga, esto tiene que ser consideradodentro del algoritmo EM.
Para la formacion de estructuras rectangulares hay varios trabajos, unosbasados mas en criterios geometricos [6], pero en especial se nos hizo in-teresante el problema de deteccion de rectangulos como un problema deetiquetado [13]. En este ultimo, formulan el problema como un problema
13
de etiquetado en el Campo Aleatorio de Markov (MRF). Para abordar ladeteccion de manera efectiva, formulan el problema como una solucion a labusqueda de la Probabilidad Maxima A Posteriori (MAP) del MRF definidoen los segmentos pertenecientes a los puntos de fuga. Para esto, se necesitaya haber encontrado los puntos de fuga y haber agrupado los segmentos.Posteriormente se construye un grafo representando el MRF. El MAP es cal-culado dejando una etiqueta unica por segmento y, por ultimo, se forman losrectangulos dependiendo de sus etiquetas. Para formar el grafo, se estable-cen pares de segmentos conectados por una Triangulacion de Delaunay conrestricciones (CDT).
Para el reconocimiento de las estructuras rectangulares despues de su de-teccion, hay varios trabajos tambien. Una propuesta simple es usar la corre-lacion entre las imagenes de las dos estructuras rectangulares detectadas [6].Otra forma de hacer el reconocimiento serıa con la extraccion de puntos deinteres, e intentar emparejar estos con otra estructura rectangular, tambienmencionado en [6]. El fin de estas propuestas es caracterizar las estructu-ras rectangulares (Landmarks) por atributos invariantes de tal manera quepuedan ser reconocidos desde cualquier punto de vista. En cualquiera de lasformas mencionadas anteriormente, es necesario representar el Landmark co-mo un ıcono de tamano fijo usando la homografıa entre un cualquier puntode vista y uno invariante.
El trabajo de deteccion de Landmarks puede requerir mucho tiempo deprocesamiento, por lo que combinar ciclos de rastreo y deteccion serıa unabuena opcion. El rastreo es un viejo conocido en vision, los trabajos que nosinteresan son los que se basan en rastreo de segmentos como [15, 3]. En am-bos casos el rastreo de los segmentos se hace buscando cambios fuertes enla intensidad de color en imagen. El rastreo de una estructura rectangularpuede proponerse como el rastreo de los segmentos que la forman. Una vezmas, hay varios trabajos sobre este tema. A nosotros nos interesa un meto-do rapido de rastreo, pues pensamos correr nuestros algoritmos a bordo delrobot humanoide con un poder de procesamiento limitado. En [19] usan unapropuesta similar al rastreador RAPiD, que se basa la busqueda del maxi-mo gradiente en la direccion perpendicular al segmento a rastrear, teniendobuenos resultados en cuanto a tiempo de procesamiento.
14
En cuanto a la segunda parte de esta tesis, la parte de localizacion, en[20] usan tal cual el MCL. Ellos lo plantean como un ciclo con los siguientespasos: Prediccion, Correccion y Re-muestreo. En la prediccion al conjuntode partıculas se les aplica el modelo de movimiento. Para la correccion secalculan los pesos de cada partıcula en funcion del modelo de observacion. Porultimo en el re-muestreo, ellos seleccionan un nuevo conjunto de partıculasaleatoriamente en funcion de los pesos del conjunto anterior, poniendolespesos uniformes. En cuanto al modelo de movimiento, los autores usan laodometrıa del robot para “predecir” la posicion de este, anadiendo un ruidoGaussiano. Para modelo de observacion, integran las mediciones del sensorlaser en la cabeza del robot con lo que dicen los sensores internos de laposicion del laser con respecto a la posicion del robot.
De manera similar en [17], usan un MCL para la localizacion, pero en esteultimo el modelo de observacion se basa en vision estereo. Ellos implementanel MCL de manera similar a [20], mencionando los mismos tres pasos: re-muestreo, prediccion y medicion. Cabe senalar que en el ultimo mencionanexplıcitamente que el re-muestreo se hace en cada iteracion, lo que tal vez noes necesario. Una forma eficiente de hacer el re-muestreo se plantea en [2].
Debemos mencionar tambien algunos trabajos en donde se plantean a re-solver problemas derivados de la localizacion de robots humanoides. En [10]los autores se enfocan en los modelos de los sensores. Mencionan que parael caso del robot humanoide NAO, la camara no puede ser perfectamenteensamblada, ası que la medicion que dicen los sensores internos de donde seencuentra esta con respecto al robot es erronea. Otra cosa que mencionan esque el angulo del robot con respecto a la vertical tiene mucho error por lainherente forma de caminar de los humanoides. En ambos casos se afectarıade manera significativa al un modelo de observacion basado en vision, puesla perspectiva con la que se toman la imagenes desde la camara variarıa enla medida que estos errores sean significativos.
Por ultimo en [14], proponen una solucion novedosa de como tratar elproblema del blur por movimiento, muy presente en pequenos robots huma-noides como el NAO. Ellos modelan el blur por su intensidad y direccion.Algo interesante acerca de esta propuesta es que la direccion del blur pormovimiento no permanece constante en toda la imagen.
15
Capıtulo 3
Deteccion, reconocimiento yrastreo de marcas visuales
Ejecutaremos un ciclo de deteccion/rastreo (ver Figura 3.1) para obtenerinformacion del entorno y ası proveer mediciones a nuestro sistema de locali-zacion. Para la deteccion de marcas visuales vamos a utilizar el principio deque, la mayorıa de los ambientes hechos por el hombre tienen muchas res-tricciones que facilitan la tarea de vision. En esta tesis nos vamos a enfocara ambientes interiores como pasillos y corredores.
En los ambientes interiores vemos ciertas regularidades que podemos ex-plotar. Generalmente las construcciones humanas son ortogonales y alinea-das con los ejes principales (X, Y, Z). Incluso las cosas que colocamos dentronuestras construcciones como pinturas, cuadros, ventanas, puertas, etc. estanalineadas de la misma manera. Esto conlleva a que al tomar una fotografıade un ambiente interior la mayorıa de las lıneas que se puedan extraer de laimagen seran lıneas convergentes a los tres principales puntos de fuga.
Nuestra premisa es que la marca visual con la cual nuestro robot se lo-calizara, tambien esta alineada a los ejes principales, por lo que sus lıneasconvergeran a los puntos de fuga. Usaremos este principio para detectar estamarca visual. Esta marca visual podra ser cualquier objeto rectangular quesobresalga en la escena (como posters, pinturas, cuadros).
16
Figura 3.1: Al inicio no se tiene nada detectado, se llama a la deteccion hastaque se detecte el landmark. Una vez detectado el ladmark se rastrea hasta que sepierda. Una vez que se pierda, se inicia el ciclo una vez mas.
3.1. Estimacion de los puntos de fuga
Extraccion de segmentos. Primero extraemos los segmentos de la ima-gen para ası poder estimar los puntos de fuga. Para la extraccion de segmentosen la escena se uso un algoritmo basado en
1. el calculo de una imagen de gradiente,
2. la construccion de una imagen de cadenas de contornos conectados yfinalmente,
3. una aproximacion poligonal de esas cadenas.
17
En este paso se extraen todos los segmentos li de la imagen; para poste-riormente agruparlos en sus respectivos puntos de fuga, ver figura 3.2.
(a) (b)
Figura 3.2: (a) Calculo del gradiente representando los bordes en la imagen. (b)Extraccion de segmentos de la imagen.
Matriz de proyeccion de la camara. Vamos a definir tres marcos dereferencia, Ow, Or y Oc. Ow es el marco de referencia del mundo y esta cen-trado en la esquina inferior derecha de nuestro landmak. Or es el marco dereferencia del robot, y esta definido como una proyeccion del cuerpo del robotal piso, que queda entre los pies. Por ultimo, Oc es el marco de referencia dela camara en la cabeza del robot, y es donde la ecuacion 2.3 es aplicable.
Vamos a llamar Tw→r la transformacion 3D que mapea puntos del marcode referencia del mundo Ow centrado en el landmark al marco de referenciadel robot Or . Y Tr→c es la transformacion del marco de referencia del robotOr al marco de referencia de la camara Oc, ver Figura 3.3.
Sabemos que Tr→c esta determinada por la posicion que ocupa la camaraen el marco de referencia del robot. Esto nos lo puede dar la medicion deodometrıa del robot, integrando la informacion de angulos de las uniones. YTw→r esta directamente determinado por la posicion del robot en el mundo
X =
xyθ
(3.1)
18
Tr→c
P2
P3
Or
Ow
P4
P1
Tw→r(X)
Oc
Figura 3.3: Marco de referencia del mundo centrado en el landmark Ow. Tw→r yTr→c son las tranformaciones que se necesitan para pasar de Ow a Or y de Or aOc.
definida como se muestra en la Figura 3.4.
Entonces podemos definir la matriz de proyeccion Π(X) en la camara comouna funcion que mapea puntos 3D en la imagen. Toma como argumento laposicion del robot X
Π(X) = K[Rw→c(X) tw→c(X)] (3.2)
conRw→c(X) = Rr→cRw→r(X) (3.3)
ytw→c(X) = Rr→ctw→r(X) + tr→c (3.4)
donde
Rr→c =
cos(θr→c) sin(θr→c) 0−sin(θr→c) cos(θr→c) 0
0 0 1
cos(φr→c) 0 −sin(φr→c)0 1 0
sin(φr→c) 0 cos(φr→c)
1 0 0
0 cos(ψr→c) sin(ψr→c)0 −sin(ψr→c) cos(ψr→c)
(3.5)
19
Z
Or
Ow
Z
Y
X
y
x
θ
Y
X
Figura 3.4: Or definida en el marco de referencia Ow. El eje X de Or siempre esla direccion hacia donde apunta el robot.
tr→c =
xr→cyr→czr→c
(3.6)
[xr→c, yr→c, zr→c, ψr→c, φr→c, θr→c] es un vector que se recupera de la me-dicion de la posicion de la camara en la cabeza del robot Oc con respectoal marco de referencia del robot Or. Esta medicion se hace integrando in-formacion de varios sensores del robot, principalmente de los sensores de losmotores.
Por otro lado
Rw→r(X) =
cos(θ) sin(θ) 0−sin(θ) cos(θ) 0
0 0 1
(3.7)
tw→r(X) =
xy0
(3.8)
20
X es el vector de estado del robot, cuyos parametros definen completa-mente la transformacion Tw→r.
Para proyectar puntos hacia p = (u, v)T en la imagen, se multiplica lamatriz de proyeccion por la izquierda al punto 3D en el mundo P = (x, y, z)T ,todo esto en coordenadas homogeneas
uv1
∝ Π(X)
xyz1
. (3.9)
Punto de fuga vertical El robot solo puede desplazarse en el plano (X, Y )y girar sobre el eje Z del sistema de referencia del mundo Ow, como se de-finio anteriormente. En consecuencia el eje Z del sistema de referencia delrobot Or siempre estara alineado con el eje Z del sistema de referencia delmundo Ow, ver Figura 3.4. Del razonamiento anterior se puede inferir que elpunto de fuga vertical pv no depende del estado X. Debido a cuestiones dela dinamica al caminar del robot humanoide, puede haber pequenas desvia-ciones del eje Z del marco de referencia Or y el eje Z del marco de referenciaOw; pero estas seran consideradas posteriormente.
El punto de fuga vertical pv es estimado entonces como la tercer columnade la matriz de proyeccion Π, ya que
[pv1
]∝ Π
0010
. (3.10)
lo cual significa que proyectamos un punto en el infinito del eje Z demundo Ow.
Procedemos a agrupar los segmentos pertenecientes al punto de fuga ver-tical pv. Si el segmento li satisface d(pv, li) < ε para algun umbral ε, se diceque este segmento esta asociado al punto de fuga.
21
Lınea del horizonte No podemos hacer el mismo procedimiento para es-timar los otros dos puntos de fuga ph,1 y ph,2 porque los ejes (X, Y ) delmundo no necesariamente estan alineados con los ejes (X, Y ) del robot, de-bido a que este gira sobre su eje Z. Sin embargo contamos con informaciona priori. Como los dos restantes puntos de fuga corresponden a las lıneas delplano (X, Y ) proyectadas en la imagen, estos deben estar sobre la lınea delhorizonte lh.
Dos puntos son suficientes para definir completamente a una lınea. Paraencontrar esta lınea podemos utilizar el mismo procedimiento utilizado en laestimacion del punto de fuga vertical. Las proyecciones de los ejes (X, Y ) noson necesariamente los puntos de fuga horizontales ph,1 y ph,2, pero tambiendeben pertenecer a la lınea de horizonte lh, por lo que
lh = Π
1000
×Π
0100
. (3.11)
Discretizamos angulos αi a lo largo de la lınea del horizonte lh como semuestra en la Figura 3.5. Se calculan las intersecciones de lo segmentos lique no se asignaron a pv con la lınea de horizonte lh
lh
li
αi
Figura 3.5: Calculo del histograma de orientaciones discretizando la lınea de ho-rizonte y calculando la interseccion de los segmentos con esta.
Con el correspondiente valor de α de esta interseccion se construye unhistograma hα como una sumatoria de gaussianas (ver Figura 3.6(a)):
22
hα =n∑i=1
1√2πσ2
α
e− (hα(k)−αi)
2
2σ2α (3.12)
Por ultimo, para aislar los maximos locales, se calcula C sobre el histogra-ma, como lo ilustra la Figura 3.6(b)
C(k) = hα(k)− 1
m
k+m2+1∑
i=k−m2
hα(i) (3.13)
de esta manera se obtienen entre uno y cuatro posibles puntos de fuga.
(a) (b)
Figura 3.6: (a) Histograma de orientaciones de 60 bins representando orientacionesde 0 a 180 grados. (b) Calculo de la curvatura para extraer dos maximos.
Debemos utilizar mas informacion para obtener el par correcto de puntosde fuga sobre la lınea del horizonte (ph,1,ph,2). Sabemos que estos deben serortogonales. El par de puntos de fuga (ph,1,ph,2) es encontrado entre todoslos posibles pares (ph,j,ph,k) detectados en C, tales que el coseno del anguloγ entre sus correspondientes rayos reproyectados en el mundo 3D sea mınimo,donde el coseno puede ser calculado como
cos(γ) =pTh,j(K
−TK−1)ph,k√pTh,j(K
−TK−1)ph,j√
pTh,k(K−TK−1)ph,k
. (3.14)
23
Por ultimo se agrupan los segmentos restantes en su respectivo punto defuga (ph,1,ph,2), de la misma manera como se hizo con el punto de fugavertical pv. Para pertenecer a ph,1 o a ph,2 se debe satisfacer d(ph,1, li) < ε yd(ph,2, li) < ε respectivamente, para algun umbral ε, Figura 3.7.
(a) (b)
Figura 3.7: (a) Lınea del horizonte calculada a partir de los datos sensoriales delrobot. (b) Clasificacion de los segmentos en sus respectivos puntos de fuga, paraeste caso pv = (428.609,−4680.78) y ph,1 = (−118.149, 127.499).
3.2. Deteccion de cuadrilateros
Como ya hemos mencionado nosotros, como en [13], formulamos la detec-cion de rectangulos como una busqueda de la solucion de la Maxima Proba-bilidad a Posteriori (MAP) del Campo Aleatorio de Markov (MRF) definidoen los segmentos pertenecientes a sus respectivos puntos de fuga. Se formulael MAP del MRF a traves del problema de etiquetado MAX-SUM.
Primero definimos G = (T , E) el cual denota un grafo que consiste en unconjunto discreto de T vertices, y un conjunto E de pares de esos vertices.A cada vertice t ∈ T se le asigna una etiqueta xt ∈ X donde X es un con-junto discreto de nodos. Entonces los terminos gt(xt) y gtt′(xt, xt′) expresancantidades dado el nodo xt en el vertice t y dados los pares de nodos xt, xt′en los vertices t y t′ respectivamente, ver Figura 3.8.
24
li∈ph,1
li∈pv
t
t′
li∈ph,1
li∈pv
Figura 3.8: Cada nodo del grafo es un segmento perteneciente a un punto de fugay las aristas son conexiones de estos segmentos.
Un etiquetado MAX-SUM es un mapeo que asigna una unica etiqueta xta cada arista t ∈ T , el cual maximiza la siguiente funcion
x∗ = argmaxx∈X
[∑t
gt(xt) +∑t,t′
gtt′(xt, xt′)
]. (3.15)
Encontrar la solucion optima de la ecuacion 3.15 no esta garantizado, yaque es un problema NP-completo, aunque muchas veces si es posible encon-trarla o una solucion muy cercana a la optima.
Construccion del grafo Se construyen dos grafos, uno con la union delos conjuntos de segmentos li asociados a los puntos de fuga pv y ph,1 yotro con los asociados a pv y ph,2. Esto con el fin de encontrar las estructurasrectangulares asociadas a los planos de las paredes en los ambientes interiores.
El conjunto de pares de vertices E es construıdo por todos los pares desegmentos conectados por el siguiente algoritmo:
1. se hace una triangulacion de Delaunay con todos los puntos inicialesy finales de los segmentos li para establecer la vecindad espacial entresegmentos y,
2. se eliminan las conexiones que se intersecan con los segmentos li, ası co-mo las conexiones repetidas en un mismo segmento.
25
Etiquetas Se definen |X | = 7 etiquetas. Las etiquetas basicas {1, 2, 3, 4}corresponden a las cuatro unicas aristas de un rectangulo respecto a la po-sicion de los dos puntos de fuga; las etiquetas {5, 6} corresponden a aristascompartidas por dos rectangulos; y {7} es la etiqueta de un segmento queno forma parte de un rectangulo. Los segmentos con etiquetas {5, 6} se con-sideran como segmentos que tienen dos etiquetas al mismo tiempo {5} tiene{3, 4} y {6} {1, 2}, ver Figura 3.9.
Pv
7
4
4
3 1
4 6
13
5
2
2
2
P(h,1),(h,2)
(a)
1
1
4
3
3
2
256
44
2
(b)
Figura 3.9: (a) Las etiquetas representan la posicion de los segmentos en unretangulo. (b) Grafo correspondiente al conjunto de segmentos de la izquierda.
Definicion de funciones La funcion gt(xt) de la ecuacion 3.15 cuantificala posibilidad de que a un segmento li se le asigne una etiqueta xt ∈ X . Porejemplo los segmentos que pertenecen al punto de fuga pv les correspondenlas etiquetas {1, 2, 6, 7} y al ph,1 {3, 4, 5, 7},
gt(x) =
{[0 0 a a a b c]T segmento t− pv
[a a 0 0 b a c]T segmento t− ph,1,ph,2(3.16)
Las constantes a, b y c expresan costos. Por ejemplo a es el costo de queun segmento perteneciente a pv tambien pertenezca a ph,1. Como se quiereevitar esto, a << 0, que significa un costo alto. La constante b es el costo deque un segmento tenga dos etiquetas al mismo tiempo {5, 6}, a < b ≤ 0. Porultimo c es el costo para una segmento que no forme rectangulos.
26
La funcion gtt′(xt, x′t) de la ecuacion 3.15 entre dos pares de segmentos
{t, t′} expresa el costo de todas las posibles combinaciones de etiquetas deese par. De acuerdo a su pertenencia a puntos de fuga puede ocurrir que
1. pertenezcan al mismo punto de fuga. En este caso se estima una lınealf que se ajuste a los segmentos y al punto de fuga. Los segmentospueden o no ser uno continuacion del otro. Queremos que los segmentosque sean uno continuacion del otro se queden en el grafo. Entoncescalculamos una distancia de esta lınea a los segmentos y mediante unumbral ε controlamos los que se quedan en el grafo y los que se eliminan,Figura 3.10(a)
d(lf , lt) + d(lf , l′t) + d(lf ,pv,(h,1),(h,2)) < ε (3.17)
2. pertenezcan a dos distintos puntos de fuga. En este caso calculamosel punto de interseccion de los segmentos p = lt × l′t y calculamos ladistancia de este punto a los segmentos Figura 3.10(b), de la mismamanera, mediante un umbral γ controlamos si se quedan en el grafo ono
d(p, lt) + d(p, lt′) < γ (3.18)
lf
pv,(h,1),(h,2)
li
lj
(a)
l′t
p
lt
(b)
Figura 3.10: Restricciones en la construccion del grafo: (a) que un segmento seala continuacion de otro, y (b) que dos segmentos formen una esquina.
En la Figura 3.11 se observa en la imagen el conjunto de conexiones de lossegmentos incluyendo las restricciones antes mencionadas.
27
Figura 3.11: Conjunto de conexiones (azul) de los segmentos de la imagen (rojo)encontradas con la Triangulacion de Delaunay con restricciones.
Para resolver la ecuacion 3.15, utilizamos el codigo disponible publicamentede MAX-SUM solver 1. Como resultado tenemos asignada una etiqueta paracada segmento, Figura 3.12.
Figura 3.12: Etiquetas asignadas a los segmentos extraıdos de la imagen despuesde resolver el problema MAX-SUM.
Analisis de los rectangulos Para obtener finalmente los rectangulos, elgrafo se analiza. Para todos los segmentos del grafo se buscan todos losposibles caminos respetando la conectividad del grafo. Esto nos lleva a una
1http://cmp.felk.cvut.cz/cmp/software/maxsum/code
28
maquina de estados, Figura 3.13, donde los estados posibles corresponden ala etiqueta del segmento.
4
1 2
3
Figura 3.13: Si un segmento es continuacion de otro con la misma etiqueta co-rresponde al mismo estado. Si se forma una esquina hay un cambio en la maquinade estados.
Por ultimo se calculan las propiedades caracterısticas de los rectangulosdetectados, Figura 3.14. Estas propiedades son:
1. Los cuatro puntos que lo forman {p1,p2,p3,p4}.
2. Los cuatro segmentos de los que esta compuesto {l1, l2, l3, l4}.
3. El valor del cambio de gradiente en las aristas. Este es calculado ha-ciendo un barrido ortogonal a las aristas de afuera hacia adentro. Setoman muestras fuera del rectangulo y muestras dentro, se promediany se calcula su diferencia.
4. La region de interes rectangular (ROI) de donde se encuentra en laimagen.
5. El baricentro. Este nos servira para poder mantener en el centro de laimagen el rectangulo.
6. Los puntos de interes calculados mediante el algoritmo FAST [16] y suscorrespondientes descriptores calculados por el algoritmo BRIEF [1].
29
Figura 3.14: Landmark detectado en el entorno y la extraccion de sus puntos deinteres (rojo). Las cruces amarillas representan la interseccion de los segmentosresueltos en la maquina de estados.
Se eligıo el algoritmo FAST debido a que es muy rapido y eficiente. Imple-menta un sencillo filtro antes de considerar los potenciales puntos de interes.Si estos pasan el filtro, se procede a analizarlos mas exahustivamente para serconsiderados puntos de interes, de lo contrario son descartados reduciendosignificativamente el numero de puntos a ser analizados. Como consecuenciael algoritmo FAST tiene un considerablemente mejor desempeno en cuantoa tiempo de computo que otros algoritmos como el SIFT o SURF.
La extraccion de descriptores mediante el algoritmo BRIEF tambien esmuy eficiente computacionalmente hablando. Este algoritmo caracteriza lospuntos de interes un con un vector de bits, por lo que tambien es eficiente enmemoria. El vector de bits corresponde al conjunto de signos de las diferenciasentre puntos elegidos aleatoriamente alrededor del punto de interes. La ideacentral de este algoritmo es que con un gran numero de estas diferencias sepuede describir bien el punto, no importando como son elegidos los puntospara calcularlas. Esto implica que entre mas diferencias se tengan alrededordel punto de interes, mas preciso deberıa ser. Para nuestros objetivos en elrastreo es suficiente con 256. Para la implementacion en C++ utilizamos lalibrerıa publica de CVLAB2.
2http://cvlab.epfl.ch/research/detect/brief/
30
El uso del algoritmo FAST en la deteccion de puntos de interes y delBRIEF en la extraccion de los descriptores nos reduce el tiempo de manerasignificativa. Hicimos alguna pruebas iniciales con el algoritmo SURF y eluso de FAST y BRIEF toma diez veces menos tiempo de computo con muybuenos resultados de extraccion y emparejamiento de puntos de interes.
3.3. Rastreo
Una vez detectados los rectangulos, se implemento un sistema de rastreoen las subsecuentes observaciones. El proceso de rastreo es mucho mas rapidoque la deteccion, lo que nos permite correr nuestro sistema en tiempo real.Aunque para que el rastreo funcione de forma optima se tiene que tener apriori una buena estimacion de la posicion del landmark en la imagen. Elalgoritmo de rastreo es basicamente:
1. Extraccion de los puntos de interes (FAST) en una region de interesdefinida alrededor de la posicion del landmark en el frame anterior, ysus correspondientes descriptores (BRIEF). Emparejamiento de estospuntos de interes extraıdos con los extraıdos anteriormente del land-mark.
2. Con estos puntos de interes emparejados se calcula una traslacion dellandmark de un frame a otro.
3. Por ultimo, a partir del rectangulo traslado, se hace un rastreo ortogo-nal a sus aristas.
Extraccion y emparejamiento de los puntos de interes. Se defineuna region de interes rectangular (ROI) alrededor del rectangulo en el frameanterior. En esta region se extraen puntos de interes de la misma maneraque se hizo en el rectangulo detectado. Para emparejarlo con los puntos deinteres de un frame a otro se utilizan los descriptores tipo BRIEF y se haceuna comparacion a fuerza bruta. Debido a que son vectores binarios se utilizala distancia de Hamming para compararlos. Si esta distancia es menor a unumbral ε se dice que son el mismo punto.
31
Prediccion. Suponemos que el landmark en t esta mas o menos en la mis-ma posicion en la imagen que en el tiempo t−1 mas una pequena traslacion;esta es una aproximacion muy poco precisa debido a que ademas de trasla-cion puede haber rotacion, escalamiento, cambio de perspectiva, etc.; peromediante nuestras observaciones dedujimos que una traslacion nos da unabuena idea de donde buscar en la imagen, ademas de que solo necesitamosdos ecuaciones (tenemos que determinar tx y ty). Un punto emparejado essuficiente; si se logran emparejar mas puntos, una regresion robusta es apli-cada.
Rastreo de aristas. La ROI definida anteriormente forma una subimagen.A esta subimagen se le hace una transformacion de Sobel para encontrarel gradiente tanto en la direccion u como en la direccion v. Se calcula lamagnitud del gradiente en cada pixel de la subimagen. A partir del rectangulotrasladado en la prediccion, se hace una busqueda ortogonal a sus aristas.Se buscan los puntos de maximo gradiente en un rango, si estos puntos a suvez corresponden en signo a las aristas del rectangulo a rastrear, se dice queencontramos un punto perteneciente a esta arista. Con el conjunto de puntospertenecientes a una arista, se hace una vez mas una regresion robusta paraestimar la ecuacion de la lınea a la que esta arista pertenece.
Para finalmente obtener los puntos del nuevo rectangulo puede uno simple-mente calcular la interseccion de estas lıneas recien rastreadas. Si se requieremas informacion, tambien implementamos el calculo de la homografıa de unframe a otro, y los puntos del nuevo rectangulo son calculados a partir deesta homografıa con las ecuaciones
p′ ∝ Hp (3.19)
l′ ∝ H−T l (3.20)
que nos ayudan a tranformar un punto y una lınea respectivamente. Estahomografıa se calcula con los puntos emparejados y los puntos rastreadospertenecientes a aristas del rectangulo. Cada punto emparejado da una ecua-cion como la 3.19 y cada punto rastreado una como la 3.20, dando un sistemalineal en los elementos de H. Se hace una estimacion robusta mediante el algo-ritmo RANSAC y posteriormente una regresion para tener un calculo precisode esta homografıa. En la Figura 3.15 se muestran unos ciclos de rastreo.
32
(a) (b)
(c) (d)
(e) (f)
Figura 3.15: Algunos ciclos de rastreo durante la ejecucion de trayectorias. Lascruces verdes representan los puntos de interes emparejados entre frames. Las lıneascyan representan la traslacion del landmark en la imagen estimada en la prediccion.Las lıneas azules son las lıneas rastreadas a partir de la imagen de gradiente. Porultimo las cruces amarillas son las esquinas del landmark transformadas por laestimacion de la homografıa de un frame a otro.
33
Capıtulo 4
Localizacion
La localizacion consiste en hacer que el robot tenga nocion del espaciodonde se encuentra. Para esto debe tener una representacion de su entornoconstruida a partir de datos perceptuales. Esto se logra integrando la infor-macion proveniente de sus sensores, de sus controles y del mapa si es que loconoce.
El proceso de localizacion incluye intrınsecamente interaccion con el en-torno. El robot tiene influencia en el entorno a partir de sus actuadores(acciones de control); y puede obtener informacion del entorno a traves desus sensores (mediciones).
Mediciones. La percepcion es el proceso por el cual el robot usa sussensores para obtener informacion del entorno. Sensores comunmenteusados son camaras, lasers, ultrasonido, etc.
Control. Las acciones de control cambian el estado del mundo, esto selogra aplicando fuerzas en el entorno mediante sus actuadores. Ejemplode acciones control son el movimiento o manipulacion de objetos.
Una fuente de medicion de esas acciones son los odometros. Los odometrosson sensores que miden el desplazamiento del robot. Ası que estos traeninformacion acerca del cambio del estado del robot. Aunque los odometros sonsensores, nosotros los manejaremos como controles, debido a que la principalinformacion que proporcionan es el cambio de estado del robot.
34
Al hablar de todas estas cantidades es difıcil considerarlas como determi-nistas, porque los sensores pueden dar lecturas ruidosas, puede haber oclu-siones. Las acciones de control difıcilmente son hechas perfectamente por losactuadores. Debido a esto, es necesario tomar en cuenta todas estas incerti-dumbres. La solucion es considerar las cantidades involucradas como variablesaleatorias y considerar sus distribuciones de probabilidad.
4.1. Filtro de partıculas (Localizacion Monte-
Carlo)
En robotica probabilıstica la evolucion del estado (por controles) y lasobservaciones (mediciones) se rigen por leyes de probabilidad. En general,estado Xt es generado estocasticamente. Entonces se debe especificar la dis-tribucion de probabilidad a partir de la cual el estado Xt es generado. Elestado Xt debe estar condicionado a todos los estado pasados, mediciones ycontroles. Ası que la evolucion del estado esta caracterizado por una distri-bucion de probabilidad de la forma
p(Xt | X0:t−1,U1:t,Z1:t−1). (4.1)
Debido a que el estado es completo, este trae consigo una recopilacion delo que ha sucedido en el pasado. De la ecuacion anterior el estado Xt soloesta condicionado a Ut si se conoce Xt−1, esto es
p(Xt | X0:t−1,U1:t,Z1:t−1) = p(Xt | Xt−1,Ut). (4.2)
De la misma manera podemos modelar el proceso por el cual las medicionesse generan. Otra vez, si Xt es completo, tenemos
p(Zt | X0:t,Z1:t−1,U1:t) = p(Zt | Xt). (4.3)
En otras palabras, el conocimiento de otras variables como medicionespasadas, controles o estados pasados es irrelevante si Xt es completo.
35
Entonces tenemos dos expresiones que nos representan probabilidades con-dicionales: p(Xt | Xt−1,Ut) y p(Zt | Xt). La probabilidad p(Xt | Xt−1,Ut)es la probabilidad de transicion del estado. El hecho de que p(Xt | Xt−1,Ut)es una distribucion de probabilidad nos dice que es el entorno en el que sedesenvuelve el robot es estocastisco y no detereminista.
El termino p(Zt | Xt) es la probabilidad de la medicion. Este termino nosrepresenta la ley de probabilidad a partir de la cual las mediciones Z songeneradas a partir del estado X.
En el MCL se usa un conjunto de partıculas para describir la distribucionde probabilidad posterior p(X1:t | U1:t,Z1:t), dada una secuencia de controleshechos por el robot U1:t = U1,U2, ...,Ut y una secuencia de mediciones Z1:t.
El estado del robot Xt es lo que queremos estimar. Esto es el conjunto decantidades que nos definen su posicion y que generalmente no son directa-mente observables. En nuestro caso Xt tiene tres grados de libertad (posiciony orientacion en el plano), esto es Xt = (x, y, θ)T . El estado por definicioncambia con el tiempo.
Lo que medimos a traves de los sensores es lo que corresponde al vectorde observaciones Zt. Intrınsecamente el proceso de observacion tiene ruido,para el cual debemos tener un modelo.
Suponiendo que se cumple la propiedad de Markov, es decir que las tran-siciones entre los estados solo pueden producirse entre estados consecutivos(ver Figura 4.1), podemos usar la formulacion recursiva de Bayes de la pos-terior
p(Xt | U1:t,Z1:t) ∝ p(Zt | Xt)
∫Xt−1
p(Xt | Ut,Xt−1)p(Xt−1 | U1:t−1,Z1:t−1)dXt−1
(4.4)Analizando los terminos de la ecuacion 4.4 podemos deducir
p(Xt | U1:t,Z1:t) es la posterior del estado del robot dados los controlesy las observaciones,
36
Figura 4.1: El estado Xt solo depende del estado anterior Xt−1 y de los controlesy observaciones en t
p(Zt | Xt) es lo que llamaremos el modelo de observacion, es decir, laverosimilitud de lo “observado”(mediciones) Zt con nuestros sensoresdado el estado actual Xt,
p(Xt | Ut,Xt−1) es el modelo de transicion (o modelo de movimiento),lo que significa la transicion del estado anterior del robot Xt−1 al estadoactual Xt dado el ultimo control Ut,
recursion p(Xt−1 | U1:t−1,Z1:t−1).
MCL representa la posterior como un conjunto de muestras ponderadas(X
(n)t , w
(n)t ), generadas en cada tiempo a partir de una distribucion q.
Basicamente el algoritmo MCL funciona de la siguiente manera:
1. Inicia con muestras {(X(i)0 ,
1N
)}1≤i≤N usando alguna distribucion inicial(uniforme si no se sabe nada a priori).
Luego, Itera:
2. a partir del conjunto anterior de partıculas, genera un nuevo conjuntoaplicando el modelo de movimiento a cada partıcula, esto es, para todo
37
n 1
X(n)t ∼ p(Xt | X(n)
t−1,Ut), (4.5)
3. actualizar los pesos de las partıculas mediante el modelo de observacion
w(n)t ∝ p(Zt | X(n)
t )w(n)t−1, (4.6)
4. normalizar los pesos de las partıculas w(n)t = 1∑
n p(Zt|X(n)t )w
(n)t−1
w(n)t , cal-
cular el numero efectivo de partıculas, es decir en numero de partıcu-las que participan de forma significativa en la estimacion del estado,aproximadamente 1∑
n[wt]2 ; si este numero es inferior a un umbral, el
conjunto de partıculas se considera degenerado. En este caso se haceun re-muestreo: genera un conjunto nuevo de partıculas a partir de lasanteriores con una probabilidad w
(n)t y con peso 1
N, Figura 4.2.
Figura 4.2: Metodo de re-muestreo: el tamano de cada casilla de la tira amarillarepresenta la verosimilitud de las partıculas actuales y las flechas de abajo (pei-ne) representan las nuevas partıculas. Con este metodo aseguramos conservar ydarle mayor importancia a partıculas con alta verosimilitud, pero no descartamospartıculas con baja verosimilitud.
La ventaja de usar filtro de partıculas (Monte-Carlo Localization) es quepodemos manejar casi cualquier tipo de distribucion de la posterior. Noesta limitado a un subconjunto de distribuciones parametricas como el EKF(Extended Kalman Filter). Es comun en problemas de localizacion y SLAMtener distribuciones multimodales (Figura 4.3), por lo que puede resolver elproblema de localizacion global.
1Hay esquemas mas generales de proponer la distribucion de muestreo, por ejemplo
incluir las observaciones, esto es q ∼ p(Xt | X(n)t−1,U1:t,Z1:t).
38
(a) (b)
(c)
Figura 4.3: Problema de localizacion complejo. El robot no podrıa saber en queparte de los pasillos en forma de cuadrado se encuentra a no ser que observa lasalida. Este tipo de distribuciones son muy bien representadas por el filtro departıculas. (Imagen de [18]).
La precision es funcion del numero de partıculas N . Si incrementamos ennumero de partıculas, la precision aumenta, pero la complejidad tambienaumenta, por lo que podemos estar limitados por nuestro hardware.
Nosotros implementamos el MCL como un ciclo de prediccion, correccionre-muestreo mas etapas auxiliares: ejecutar la trayectoria, detectar/rastrearlandmark y guardar los datos, como se ilustra en la Fugura 4.4.
4.1.1. Modelo de movimiento
El robot humanoide NAO nos facilita la informacion de odometrıa. Estolo logra integrando toda la informacion de sus sensores internos (posicionde los motores en las articulaciones, acelerometros, giroscopios) y dandonosuna aproximacion de su posicion respecto a un marco de referencia que noconocemos.
En general la informacion de la odometrıa es poco fiable para representarla posicion del robot en el mundo. Si ejecutamos una trayectoria, la tra-
39
Figura 4.4: Ciclo principal de nuestro sistema de deteccion. Se observa queesta compuesto por las etapas del filtrado (prediccion, correccion y re-muestreo)mas las etapas auxiliares.
yectoria y por tanto la posicion final que nos exprese la odometrıa no va aser la trayectoria real ni la posicion final real del robot. Pero, a pequenosdesplazamientos (dx, dy, dθ)Todometry, la odometrıa nos puede dar una buenaaproximacion de lo que se ha movido el robot dx
dydθ
odometry =
xyθ
odometryt
−
xyθ
odometryt−1
. (4.7)
40
Para aplicar el modelo de movimiento, primero debemos expresar la infor-macion de desplazamiento que nos dice la odometrıa del marco de referenciadel robot (que no conocemos), a nuestro marco de referencia. Esto es
dxdydθ
=
cos(θodometryt−1 ) −sin(θodometryt−1 ) 0
sin(θodometryt−1 ) cos(θodometryt−1 ) 00 0 1
dxdydθ
odometry . (4.8)
Como en [11], anadimos un ruido proporcional a la magnitud del despla-zamiento, debido a que en nuestras mediciones, vimos que el error en laodometrıa es proporcional al desplazamiento dx
dydθ
=
dxdydθ
+
N (0, |dx| ∗ σ2x)
N (0, |dy| ∗ σ2y)
N (0, |dθ| ∗ σ2θ)
. (4.9)
Y por ultimo se aplican estos desplazamientos a todas las partıculas, paratodo n
xyθ
(n)
t
=
xyθ
(n)
t−1
+
cos(θ(n)t−1) −sin(θ
(n)t−1) 0
sin(θ(n)t−1) cos(θ
(n)t−1) 0
0 0 1
dxdydθ
. (4.10)
La determinacion de σ2x, σ
2y y σ2
θ se hace mediante ensayo y error, de-bido a que es difıcil determinar el ruido de una forma teorica, ya que haymuchas fuentes distintas de ruido, como son errores en los mismos sensores,deslizamiento del robot, etc.
4.1.2. Modelo de observacion
Podemos definir varios modelos de observacion basandonos en la matrizde proyeccion Π(X). Representamos los cuatro puntos 3D que definen allandmark como Pw
1 , Pw2 , Pw
3 , Pw4 . Asumimos que estos puntos estan escritos
en coordenadas homogeneas, esto es, Pw1 = (Xw
1 , Yw1 , Z
w1 , 1)T .
41
Modelo de los cuatro puntos del landmark La idea mas intuitiva serıaevaluar los errores de re-proyeccion de estos puntos 3D en la imagen (Figura4.5). Sabemos que nuestro sistema de vision nos da como salida los cuatropuntos en la imagen en donde se localiza el landmark. Definimos entoncesnuestro vector de observacion como Zt = (pT1 ,p
T2 ,p
T3 ,p
T4 )T . Entonces la re-
proyeccion de los puntos 3D serıa
p2
Pw2
Pw3
Pw4
Pw1
p1
p4 p3
Figura 4.5: En este modelo unicamente se evalua los errores de re-proyeccion dellandmark en la imagen con el landmark detectado.
Zt =
([ΠPw1 ]
u
[ΠPw1 ]w
,[ΠPw1 ]
v
[ΠPw1 ]w
,[ΠPw2 ]
u
[ΠPw2 ]w
,[ΠPw2 ]
v
[ΠPw2 ]w
,[ΠPw3 ]
u
[ΠPw3 ]w
,[ΠPw3 ]
v
[ΠPw3 ]w
,[ΠPw4 ]
u
[ΠPw4 ]w
,[ΠPw5 ]
v
[ΠPw4 ]w
)T.
(4.11)
Definimos Zt = h1(Xt,m), se debe notar que h1 es una funcion altamenteno lineal del estado Xt. Recordemos que Π(X) depende de todos los parame-tros de la proyeccion como son las transformaciones Tr→c y Tw→r.
La parte central de los modelos probabilısticos es la inclusion de un rui-do. Este nos permite incluir en la estimacion fenomenos aleatorios complejosque simplemente son muy complejos para modelarlos. Debido a las alta no-linealidad de h1, nos parecerıa muy difıcil modelar un ruido sin acudir a unalinealizacion. Para modelar el ruido de la observacion, nosotros proponemos
42
incluir todos los parametros de la matriz de proyeccion que pudieran provo-car ruido en un vector m = [φr→c, αu, u0, αv, v0, [P
w1 ]T , [Pw
2 ]T , [Pw3 ]T , [Pw
4 ]T ].Ası entonces podemos escribir el error de re-proyeccion como un ruido gaus-siano aditivo
p(Zt | Xt) = N (Zt; h1(Xt,m),Σh). (4.12)
El problema se reduce a la estimacion de la matriz de covarianza Σh. Unafuente de ruido ademas de la matriz de proyeccion es el algoritmo de visionpor si mismo. Entonces podemos escribir la matriz de covarianza como
Σh = Σh,m + Σh,i, (4.13)
con una componente Σh,i debida a la deteccion de los puntos del land-mark, ; la otra componente Σh,m integra las fuentes de error de la proyeccionincluıdos en m.
Para evitar la linealizacion de Σh,i proponemos usar la Unscented Trans-form [8] para estimar la covarianza del ruido Σh,m. Intuitivamente lo que sehace en la Unscented transform es variar los parametros que podrıan provo-car ruido y evaluar el efecto de estas variaciones en el error de proyeccion.Entonces la Unscented Transform se aplica de la siguiente manera:
Se estima un valor promedio para los parametros en m y se hace laproyeccion con estos valores,
se definen un conjunto de puntos sigma m(k) correspondientes a losvalores extremos de los parametros en un rango definido,
se hace la proyeccion pero ahora con los puntos sigma, uno a la vez porcada valor extremo, mientras los demas parametros permanecen en suvalor promedio,
se calcula el error de la proyeccion con los valores medios con las pro-yecciones de los puntos sigma.
La matriz de covarianza de proyeccion es estimada por
43
h =∑k
wmk h(X,m(k)), (4.14)
Σh,m =∑k
wck(h1(Xt,m(k))− h)(h1(Xt,m
(k))− h)T . (4.15)
Modelo de la forma del landmark. El modelo de observacion defini-do anteriormente no es necesariamente el mejor, aunque incluye los cuatropuntos que describen al landmark. El modelo de los cuatro puntos no puedediscriminar entre posiciones altamente similares. Entonces necesitamos unmodelo con el cual se pueda obtener mas informacion de los mismos cuatropuntos. Podemos considerar el siguiente modelo de observacion
Zt =
(p4,
1
2(‖p2 − p1‖+ ‖p3 − p4‖),
1
2(‖p4 − p1‖+ ‖p3 − p2‖), β
),
(4.16)
donde β = arctanp2,u − p1,up2,v − p1,v
y ahora Zt = h2(Xt,m). El vector Zt integra
mucha de la informacion de la observacion en menos parametros: el centro dellandmark (primer elemento), el tamano del landmark en la imagen (segundoy tercer elementos) y el angulo del segmento superior, ver figura 4.6. Se debeapreciar que con el primer elemento del vector de observacion podemos dis-criminar entre posiciones donde se observa o no el landmark. Con el segundoelemento se da una idea de la profundidad del landmark en el mundo con sutamano en la imagen. El tercer elemento nos da informacion de la perspectivacon la que se observa el landmark al igual que el cuarto elemento.
Con este vector de observacion ya podemos tener una mejor idea de laperspectiva y la profundidad desde donde se observa el landmak. De la mismamanera que en el primer modelo de observacion, definimos una distribucionsobre este vector
p(Zt | Xt) = N (Zt; h2(Xt,m),Σh′), (4.17)
y la matriz de covarianza
Σh′ = Σh′,m + Σh′,i, (4.18)
44
d2
d1
β
p4
Figura 4.6: En este modelo intentamos obtener informacion del landmark que nospermita discriminar posiciones y nos de una idea de como se esta observando ellandmark.
donde Σh′,m es estimada con la Unscented Trasnform de la misma maneracomo se explico anteriormente.
Modelo de la forma del landmark y orientacion visual. Los modelosde observacion anteriores extraen informacion unicamente de los cuatro puntoque conforman al landmark pero podemos tener aun mas informacion enla escena. Como vimos anteriormente, tenemos una escena estructurada. Yahemos calculado los tres puntos de fuga correspondientes a las tres principalesdirecciones ortogonales (X, Y, Z). Tambien contamos con la ecuacion quedefine la lınea del horizonte correspondiente a las proyecciones de puntos enel infinito en el plano Z = 0.
Con la informacion que nos proporciona la escena podemos estimar direc-tamente la orientacion del robot en el entorno, θ. Recordemos que a partirde la ecuacion 3.14 podemos calcular el angulo entre dos rayos proyectadosen la imagen. El angulo θ es ciertamente el angulo entre el robot y la pareddel corredor. Entonces podemos estimar este angulo con el rayo del punto defuga correspondiente a la direccion X (punto de fuga del corredor, que debeestar sobre la lınea del horizonte) proyectado en la imagen, y la proyecciondel centro optico de la imagen en la lınea del horizonte Figura 4.7,
y se estima como
45
θ
(u0, v0)
Figura 4.7: El angulo θ es estimado con la ayuda de la distancia que hay entreel punto de fuga del corredor ph,1 y la proyeccion del centro optico de la camarap(u0,v0).
θ = θr→c − acos(pT(u0,v0)(K
−TK−1)ph,1√pT(u0,v0)(K
−TK−1)p(u0,v0)
√pTh,1(K
−TK−1)ph,1). (4.19)
Como se explico antes, la estimacion de los puntos de fuga se hace en ladeteccion de los rectangulos. Si se esta entonces rastreando el landmark elpunto de fuga del corredor es estimado con la interseccion de las dos lıneashorizontales del landmark con la lınea del horizonte.
Entonces el vector de observacion queda
Zt =(p4,
12(‖p2 − p1‖+ ‖p3 − p4‖), 12(‖p4 − p1‖+ ‖p3 − p2‖), θt
). (4.20)
Esta vez con Zt = h3(Xt,m), y su correspondiente distribucion de pro-babilidad
p(Zt | Xt) = N (Zt; h3(Xt,m),Σh′′). (4.21)
46
En este caso la matriz de covarianza Σh′′ es estimada con la UnscentedTransform tambien, variando en rango los parametros que pudieran provocarerrores.
Con este modelo de observacion contamos con mucha mas informacion paradiscriminar poses del robot en el mundo. En este las partıculas que tenganla orientacion adecuada tendran mas probabilidad de permanecer, ademasde que al incluir las dimensiones del landmark en la imagen ya tenıamosinformacion de la distancia del robot al landmark.
47
Capıtulo 5
Validacion experimental
Procedemos a presentar los resultados de nuestros experimentos realizadosen el robot humanoide NAO, fabricado por Aldebaran Robotics. El NAOcuenta con 25 grados de libertad, dos camaras CCD (aunque por razonestecnicas solo se puede usar una a la vez), un conjunto de sensores comogiroscopios y acelerometros; ası como sensores de posicion de los motores.El poder de procesamiento del NAO es limitado ya que cuenta con un CPUAMD Geode x86 a 500 MHz. Cuenta con el sistema operativo EmbeddedLinux (32 bit x86 ELF) y es programable en distintos lenguajes como C,C++, URBI, Python.
El NAO cuenta con una API integrada llamada naoqi. Esta plataforma nosfacilita el acceso a la informacion de los sensores, a la camara y al sistema demovimiento. El NAO viene con un controlador de locomocion integrado quele permite caminar y en general controlar de una forma limitada los motores.
Para evaluar nuestro sistema de localizacion disenamos experimentos conlas siguientes caracterısticas: el entorno del robot consiste en un corredor li-bre de obstaculos. En el corredor hay puertas y ventanas y sobre una paredse coloca el landmark. Se implementaron dos modulos: uno para el sistemade vision (deteccion y rastreo del landmark) y otro para el sistema de locali-zacion. Estos fueron programados en C++, para correr a bordo del robot yse lanzan de manera remota. El robot es completamente autonomo y guardalos resultados de la localizacion.
48
Para la implementacion utilizamos OpenCV1 que es una librerıa eficientede procesamiento de imagenes. Comenzamos con pruebas para el sistema devision con imagenes de 640 × 480. Nos dimos cuenta que el procesamientoera muy lento a esa resolucion por lo que decidimos utilizar 320 × 240 conun buen desempeno.
Lo mas costoso computacionalmente hablando del sistema de vision es ladeteccion de segmentos en la imagen. Este toma un promedio de 150 mspor frame. Por otro lado el rastreo toma en promedio 50 ms si se calculala homografıa y menos de 5 ms si solo se rastrea. El sistema MCL tomaaproximadamente 200 ms con 500 partıculas. Recordemos que el MCL es decomplejidad lineal en el numero de partıculas por lo que con menos partıculasmejorara notablemente el tiempo de computo. En promedio se pudo correrel algoritmo con las caracterısticas anteriores a 4 Hz. Al contrario de lo quese pudiera pensar, esto es razonable debido a la capacidad de procesamientolimitada del NAO; sin embargo hay trabajo por hacer en este rubro.
Durante los experimentos el robot inicia desde la misma posicion y se leconsigna la misma trayectoria. Durante la ejecucion de la trayectoria detectael landmark e intenta rastrearlo con la cabeza. En cuanto a la localizacion ladistribucion inicial del estado es una distribucion uniforme, ya que se suponeque no se sabe nada a priori.
Comenzaremos presentando los resultados de la localizacion global de losdistintos modelos de observacion: en la Figura 5.1 se observa la localizacionglobal del modelo que incluye solo los cuatro puntos, en la Figura 5.2 el dela forma del landmark y la Figura 5.3 el de la orientacion visual. En estaparte solo se presentan las posiciones de las partıculas en el plano, dejandoel analisis de los resultados de la orientacion tratarse por separado.
En los resultados de la localizacion global podemos observar que en elmodelo de observacion de los cuatro puntos de landmark la ambiguedad delas dos posiciones simetricas se resuelve pronto. No es ası en el modelo de lasdos distancias y el angulo del segmento superior. En el modelo que incluyeel angulo del segmento superior la ambiguedad permanece casi hasta el final,
1http://opencv.willowgarage.com/wiki/
49
de hecho hubo experimentos donde habıa casi la misma probabilidad de estaren uno o en otro e incluso, a veces el resultado es la posicion simetrica.
Lo anterior no sucede en el modelo de las dos distancias y la orientacionvisual. Desde las primeras observaciones el robot resuelve completamente elproblema de ambiguedad en posiciones simetricas ya que el vector de obser-vacion incluye el angulo θ del vector de estado.
Otra observacion importante es que en el ultimo modelo de observacion lamayor parte de la dispersion se concentra a lo largo de la lınea que apuntahacia el landmark. Esto era de suponerse ya que el robot tiene mucho menosincertidumbre de su orientacion que de su posicion.
En la Figura 5.4 observamos la evolucion de las tres componentes del esta-do. A partir de estos resultados podemos concluir que el modelo de los cuatropuntos del landmark y el que incluye la orientacion visual son los mejores.Observamos que la componente y del estado es la que mejor se estima y laque menos ruido tiene, Figura (b). En (c) se observa lo que ya se menciono,el modelo de la orientacion visual desde el inicio converge a la orientacionreal.
Ahora en la figura 5.5 presentamos los resultados de la estimacion de latrayectoria seguida por el robot. Esta trayectoria es estimada a partir delpromedio de todas las partıculas; aunque como veremos mas adelante estonos es del todo lo mejor. Se puede apreciar que la trayectoria es muy ruidosa,Figura (b). Esto es en parte por lo ruidosa que es la odometrıa del robot,debido a la forma de caminar de los humanoides, Figura (a). Un pequeno filtropodrıa mejorar la estimacion de la odometrıa y por tanto de la trayectoria.
Presentamos unicamente los resultados de la estimacion de la trayectoriade los modelos de los cuatro puntos del landmark y el que incluye orientacionvisual. En el caso del otro modelo no se puede estimar una trayectoria yaque si no se llega a la monomodalidad no tiene sentido calcular un promedio.En la Figura (b) podemos apreciar una diferencia basica en los dos mejoresmodelos. El modelo que incluye orientacion visual llega mas rapido a la mo-nomodalidad, por lo tanto se le puede seguir la trayectoria desde la primerasiteraciones del filtro.
50
En la figura 5.6 se muestra la evolucion de la covarianza. Observamos loque se esperaba, en los tres modelos la covarianza va disminuyendo conformese hacen observaciones. Se debe poner mas atencion a la forma de la graficay no tanto a los valores ya que estos depende mucho de parametros a calibrar
Una forma mejor de evaluar nuestros modelos de observacion propuestoses la evaluacion de la entropıa. Esta es una medida que nos da informacionacerca del desorden del sistema. Como sabemos el promedio no tiene mu-cho sentido para distribuciones multimodales y la covarianza puede darnosmediciones enganosas. Si tenemos una alta covariaza puede ser que tenga-mos modas muy separadas, pero estas modas muy bien delimitadas, esto noesta necesariamente mal. En la Figura 5.7 observamos que la entropıa deltercer modelo es la que desciende mas significativamente.
Por ultimo vale la pena anexar una comparacion entre los tiempos decomputo del filtro con los tres modelos de observacion. Esta informacion esimportante cuando se tienen recursos limitados (como es el caso de NAO)y podrıa influir en el momento de elegir un modelo u otro. En general seobserva que el tiempo de computo es casi el mismo, alrededor de 180 ms poriteracion.
51
(a) (b)
(c) (d)
(e) (f)
Figura 5.1: Modelo de los cuatro puntos. (a) se inicial con una distribucion uni-forme. (b) A la primera observacion las partıculas se acomodan en grupos. En (c)se observa como casi se resuelve el problema de localizacion global, aunque aunhay algunas partıculas en la posicion simetrica. En (d) y (e) se sigue la trayectoriareduciendo la covarianza con las observaciones. Por ultimo en (f) el robot terminade ejecutar la trayectoria con el punto final real dentro de la elipse de covarianzaa un sigma. 52
(a) (b)
(c) (d)
(e) (f)
Figura 5.2: (a) al igual que en el modelo anterior, en las primeras observacioneslas partıculas se acomodan en pequenos grupos. En (b) las partıculas se agrupanen tres modas donde hay alta probabilidad que el robot se encuentre ahı. En (c),(d) y (e) se observa que no hay forma de distinguir entre posiciones simetricas,porlo que hay alta probabilidad de estar en una o en otra. Al finalizar la trayectoria(f) el robot no pudo llegar a una unica moda, sigue habiendo probabilidad de estaren una o en otra. 53
(a) (b)
(c) (d)
(e) (f)
Figura 5.3: (a) al igual que en los modelos anteriores las partıculas se juntan enpequenos grupos, pero estos grupos ahora ya estan restringidos a una subregion endonde el angulo θ es el que es estimado con el algoritmo de vision. En (b), (c), (d)se observa que la distribucion llega mas rapido a la monomodalidad permitiendoseseguir la trayectoria desde las primeras observaciones. En (e) y (f) vemos que lacovarianza va disminuyendo y que se acerca a la posicion final real.
54
(a) (b)
(c)
Figura 5.4: Evolucion del estado X a lo largo de las iteraciones del filtro delMCL. El modelo de los cuatro puntos del landmark (rojo) y el de la orientacionvisual (azul) son los que presentan mejor desempeno en la estimacion del estado.El modelo que incluye el angulo del segmento superior (verde) es muy ruidoso.
(a) (b)
Figura 5.5: En (a) observamos que la odometrıa tiene mucho ruido en la com-ponente y debido al balanceo al caminar de los humanoides. En (b) observamosla estimacion de la trayectoria del humanoide con el modelo de los cuatro puntos(rojo) y el modelo de la orientacion visual (azul).
55
Figura 5.6: Evolucion de la covarianza. Como en las figuras anteriores, el rojocorresponde al modelo de los cuatro puntos, el verde al de las distancias mas elangulo del segmento superior izquierdo y el azul al que incluye orientacion visual.
56
(a) (b)
(c)
Figura 5.7: Evolucion de la entropıa, los colores son como en la figura anterior. Sepresentan los modelos en distintas graficas ya que al ser modelos de observaciondistintos, el modelo del ruido en la observacion es distinto. Entonces el valor de laverosimilitud do lo observado (el termino p(Zt|Xt)) es distinto en magnitud.
57
Figura 5.8: Tiempo de computo para los distintos modelos de observacion, loscolores son como en las figuras anteriores. Los descensos que se ven son cuandono hay observacion, en este caso el filtro tarda unos 60 ms por iteracion. Por estoultimo se debe tomar mas en cuenta los momentos donde si hay observacion.
58
Capıtulo 6
Conclusiones y trabajos futuros
En esta tesis se implemento un sistema de localizacion visual para el robothumanoide NAO. Vimos que tener nocion de la pose actual del robot es unade las tareas mas importantes en navegacion de robots. La estimacion de lapose no es una tarea trivial y requiere la integracion de muchos sensores enenfoques probabilısticos como se hizo aquı.
Las herramientas matematicas necesarias para realizar la tarea de loca-lizacion ya estan bastante desarrolladas (filtros bayesianos). Sin embargo laforma de integrar la informacion de todos los sensores es un problema abierto.En esta tesis propusimos varios modelos para integrar la informacion prove-niente de los sensores internos del NAO mas la que proviene de la camara.Obviamente un sistema de vision artificial tuvo que ser implementado paraextraer informacion de alto nivel de simples imagenes.
La forma de abordar el problema de localizacion en esta tesis fue la locali-zacion Monte-Carlo. En este enfoque, hay dos partes donde se debe integrarla informacion de los sensores (interoceptivos y exteroceptivos): la predicciony la correccion.
En nuestra propuesta unicamente utilizamos la informacion de los sensoresinternos para el modelo de movimiento (prediccion). Sin embargo, se puedeintegrar informacion de la observacion actual en la prediccion. En otras pa-labras, generar un conjunto nuevo de partıculas a partir de la anteriores ytambien a partir de alguna distribucion definida a partir de la observacion.
59
Esto servirıa para resolver el problema del secuestro (kidnapping). Este pro-blema consiste en que el conjunto actual de partıculas no sirve para estimarla posicion actual del robot, es decir, el robot esta perdido. En cuanto a losmodelos de observacion, hay distintas variantes de los modelos propuestosque podrıan tener un mejor desempeno.
En nuestros experimentos observamos que el desempeno computacional denuestros algoritmos es un factor clave. Sabemos que la complejidad que elfiltro de partıculas es lineal en el numero de partıculas. Entonces poder con-trolar este numero con base en algun modelo serıa una mejora significativa1.
En cuanto a los trabajos futuros, se podrıa tomar como base el sistemade localizacion implementado en esta tesis para extenderlo a un sistema delocalizacion activa. La localizacion activa consiste en tomar decisiones sobrelos controles de tal manera que se pueda realizar la tarea ordenada y a la vezmantener posiciones que favorezcan la localizacion. En otras palabras llevara los controles del robot a un nivel cognitivo mas alto: la toma de decisiones2.
Se podrıa extender el nuestro sistema de localizacion a uno de mapeo ylocalizacion simultaneos (SLAM). Como ya mencionamos, el sistema de visionfue desarrollado en ese enfoque, por lo que con algunos cambios se podrıaadaptar al SLAM. En cuanto al filtrado, adaptar nuestro sistema al SLAMno es trivial, incluso podrıa utilizarse otra herramienta diferente como lo esel Filtro de Kalman Extendido (EKF-SLAM).
En general, el objetivo de la tesis fue alcanzado. Los resultados son bastanteaceptables en cuanto a la localizacion. A parte de que se evaluaron distintosmodelos para mejorar la estimacion de la pose del robot.
1En la literatura ya hay varias propuestas de como hacerlo.2Una vez mas, en la literatura ya hay varias propuestas en enfoques probabilısticos
como lo son los POMDPs
60
Apendice A
Como hacer modulos y librerıaspara ejecutarse a bordo delNAO
A.1. Terminos importantes
Modulo: es una unidad de codigo, implementada como clase C++, quecontiene codigo que sirve a realizar alguna funcionalidad. Como esta clasetiene que tener ciertos metodos necesariamente implementados, existe un ge-nerador de modulos que genera automaticamente un esqueleto de un modulo.
Librerıa: esta compuesta por trozos de codigo que contienen alguna fun-cionalidad pre-compilada. Las librerıas proporcionan servicios a programasindependientes (ejecutables), esto permite que el codigo se pueda construirde manera modular. Los ejecutables y las librerıas hacen referencia entre sia traves de un enlace (link).
Ejecutable: es un archivo binario que es interpretado por la computado-ra. Contiene instrucciones en lenguaje de maquina y se puede enlazar conlibrerıas para anadir funcionalidades.
Naoqi: es el nombre del programa-servidor que corre en el robot y al cualun programa-cliente se puede conectar para recuperar datos, o hacer unapedida.
61
A.2. Arquitectura del NAO
La arquitectura de Naoqi se puede observar en la figura A.1. Es una arqui-tectura modular, con las varias funcionalidades encapsuladas en diferentesunidades de codigo.
Figura A.1: Arquitectura de naoqi. (Imagen extraıda de la documentacion denaoqi.)
A.2.1. Broker
Un broker es un ejecutable y un servidor que puede atender comandosremotos en un IP (el de la maquina en que esta corriendo) y un puerto, esdecir por red. En practica, para implementar una funcionalidad en el robot,tienes que pasar por un broker. Dos casos son posibles:
o el generador de modulos generara un ejecutable y este se conectara alrobot (en este caso, este ejecutable es un broker secundario),
o generara una librerıa para cargarla en el broker principal, es decir elprograma Naoqi, que es el que provee las funcionalidades basicas en elrobot (adquisicion de datos por sensores, actuacion. . . ).
62
En este ultimo caso, se tiene que agregar en autoload.ini (del robot) elnombre de tu librerıa. El generador de modulos tambien maneja los enlacesy los caminos a las librerıas.
Hay que entender bien las implicaciones de cada una de esas dos eleccio-nes en cuanto a tu modulo: en el primer caso, tu modulo esta encapsulado enun ejecutable diferente del ejecutable que corre las operaciones crıticas en elrobot. Eso significa que en caso de que haya un gran error en tu codigo, tuejecutable podra fallar sin que los sistemas de control crıticos del robot fallentambien. En el segundo caso, tu codigo compilado esta usado por el mismısi-mo ejecutable Naoqi : en caso de problemas graves (error de segmentacion),el robot puede caer. Sin embargo, en el segundo caso, ya que tu codigo esta enel mismo ejecutable que los modulos-core del robot, puedes acceder muchomas rapido a datos del robot (por ejemplo, la imagen de la camara), ya quela memoria es compartida entre modulos del mismo ejecutable.
En resumen, para tu modulo, tienes siempre dos opciones:
correrlo en un broker separado (modo “remote”): seguro pero unpoco menos eficiente;
correrlo en el broker principal (modo “local”): arriesgado pero muyeficiente.
En la figura A.2 se ilustra un simple ejemplo de un modulo creado porel usuario, llamado “myModule”. myModule corre como un broker separado(modo “remote”) llamado “myBroker”que se comunica con el broker principalsobre 127.0.0.1:9559.
Observacion: Un punto importante que observar es que, en este caso par-ticular, “Main Broker” y sus modulos estan corriendo a bordo del robot,igualmente a “myBroker”(ya que la IP especificada corresponde a la maquinahuesped). Pero, hubieramos podido muy bien correr el “myBroker” en unamaquina remota, especificando a este programa una IP correspondiendo ala direccion del robot en la red. El desempeno es en este caso limitado por laspropiedades de la red, pero permite por ejemplo correr procesamientos talvez mas pesados, en tu maquina local mas poderosa que la CPU del robot.
63
Figura A.2: Broker. (Imagen extraıda de la documentacion de Naoqi).
A.2.2. Proxy
Debido a que la arquitectura software del NAO es modular, un modulono necesariamente “conoce” el codigo de otro modulo. En particular, nuncaincluimos (#include) una cabecera de otro modulo. En vez de esto, creamosun objeto especial, enteramente dedicado a la comunicacion con otros modu-los, el proxy. Es lo mismo cuando el modulo se ejecuta de manera local o demanera remota. Veremos ejemplos de ellos al momento de implementar unprimer modulo.
A.2.3. Modulos locales o remotos
Los modulos, como es mencionado arriba, son clases que estan compiladascomo librerıas. Para especificar que esta librerıa tiene que estar cargada en elsistema, se la tiene que mencionar explicitamente en el archivo de configura-
64
cion autoload.ini, con el cual se instanciara automaticamente el modulo de laclase principal. El generador de modulos crea una clase que sera comprendidapor el broker, ya sea el broker principal, o un broker creado por el usuario sies separado/remoto.
Como hay dos tipos de modulos, hay tambien dos maneras de compilar unmodulo:
Como un ejecutable (modulo remoto): Cada paquete de modulo creaun ejecutable independiente en “/path/to/aldebaran-sdk/bin/”.
Como una librerıa (modulo local): Cada paquete de modulo crea unalibrerıa dinamica guardada en “/path/to/aldebaran-sdk/lib/naoqi/”.
Observacion: En el secundo caso, para hacer que el broker principal carguetus modulos al iniciar, el archivo autoload.ini tiene que tener el nombre detu proyecto.
Observacion: Puedes escoger si tu modulo va a ser remoto o local configu-rando la variable de CMake llamada MY_MODULE_IS_REMOTE usando ccmake.
A.3. Generador de modulos
El generador de modulos es una aplicacion incluida en el sdk, y escritaen python. Para ejecutar el generador de modulos tenemos que hacer lo si-guiente (en Linux, pero se harıa de manera equivalente en otros OS):
$∼cd /path/to/aldebaran-cpp-sdk/modules/src/$∼python module generator.py
En Project Name ponemos el nombre del proyecto, este va a ser elnombre de la librerıa que va a contener tus modulos, una sola librerıa puedecontener varios modulos.
En Modules Names ponemos el nombre de los modulos que queremosgenerar, separados por un espacio en blanco.
El Author es opcional.Damos click en generar y nos aparece lo siguiente:
65
Figura A.3: Generador de modulos
Your project has been generated in /path/to/generator/
El generador de modulos nos crea un directorio donde se encuentra todoel codigo generado para nuestro proyecto. En la raız hay dos archivos .cmake,que solo vamos a modificar en caso de que queramos cambiar los parametrosde compilacion. En src/ nos genera un archivo .cpp y un .h por cada moduloque hayamos querido generar, ademas de un archivo .cpp con el nombre delproyecto. En ese ultimo esta la funcion main del ejecutable generado, si esel caso. En cada archivo .h se define una clase heredada de ALModule, consu constructor por default. A partir de este esqueleto, nosotros ya podemosseguir el codigo y definir las variables miembro y los metodos que va a tenernuestro modulo.
Para poder usar funciones de nuestro modulo mediante un proxy (usar-las en otro modulo o directamente), tenemos que “enlazarlas” mediante lamacro BIND METHOD. Para que esta macro funcione correctamente tene-mos que llamar antes las funciones functionName, en caso que la funciontenga parametros la funcion addParam por cada uno y por ultimo la funcionsetReturn en caso de que tenga valor de retorno (ver ejemplo).
Por ultimo en el directorio tests/ genera un archivo .py (un script python)para lanzar metodos de nuestros modulos. Esto tambien lo podemos hacerde manera analoga con un programa en C++.
A.4. Compilacion cruzada
Para poder correr tu codigo a bordo del NAO y no en tu maquina, tienesque hacer una compilacion cruzada. Esto por el momento solo es posible en
66
linux. La compilacion cruzada es necesaria para asegurar que se compila conlas banderas correctas de optimizacion y ligas a las correctas librerıas.
Para hacer una compilacion cruzada simplemente hacemos el proceso si-guiente:
$∼cd /path/to/sources
$∼mkdir buildcross
$∼cd buildcross
$∼cmake -DCMAKE TOOLCHAIN FILE=/path/to/ctc/toolchain-geode.cmake ..
$∼make
Antes de hacer la compilacion cruzada tenemos que definir lo siguiente:
SDK: El SDK es un directorio comun para todos nuestros proyectos.Por ejemplo, las librerıas estan siempre en lib/, los archivos de cabeceraen include/, los datos en share/ etc.
El archivo toolchain.cmake: Para poder usar codigo desde lib/cmake,tenemos que usar el archivo toolchain.cmake. Esta es la forma de cma-ke para hacer compilacion cruzada. Para especificar un archivo tool-chain.cmake a usar, se tiene que pasar la siguiente bandera en la lıneade comandos:
cmake -DCMAKE TOOLCHAIN FILE==path/to/sdk/toolchain.cmake
Ası es como esta definido el archivo toolchain.cmake:
#DO NOT EDIT
#DO NOT CHANGE LOCATION, THIS FILE ONLY WORKS IN THE ROOT
#OF A FULL SDK
cmake_minimum_required(VERSION 2.6.4)
get_filename_component(_ROOT_DIR
${CMAKE_CURRENT_LIST_FILE} PATH)
set(TOOLCHAIN_DIR ’${_ROOT_DIR}/lib/’ CACHE STRING ’’ FORCE)
set(SDK_DIRS ’${_ROOT_DIR}’)
set(SDK_ARCH linux)
include(’${TOOLCHAIN_DIR}/cmake/Toolchain-generic.cmake’)
67
# This is a bit unusual: result of the compilation will always
# be put in this directory
set(SDK_DIR
’${_ROOT_DIR}’
)
Las principales variables son:
• TOOLCHAIN DIR: Para encontrar el codigo en lib/cmake.
• SDK ARCH: Para especificar si estas usando linux, windows, com-pilacion cruzada, los valores aceptados son: linux,macosx,win-vc8,win-vc9, nao-geode. Para nuestro caso en particular debe ser nao-geode.
• SDK DIRS: Una lista de posibles directorios SDK donde se bus-caran archivos de cabecera y librerıas creadas por el usuario.
• SDK DIR: (opcional) Es donde se pondra el resultado de la com-pilacion. El default es build/sdk.
A.5. Compilar una librerıa externa
Para compilar una librerıa externa hacemos nuestra librerıa tal como elgenerador de modulos lo hace:
$∼mkdir librerıa$∼cd librerıa$∼mkdir src$∼nano bootstrap.cmake$∼nano CMakeLists.txt
En src/ colocamos los archivos fuente (.h y .cpp) de manera similar a co-mo el generador de modulos lo hace. El archivo bootstrap.cmake podemoscopiarlo tal cual de cualquier modulo generado. El archivo CMakeLists.txtlo adaptamos del ejemplo siguiente1:
###
# This is a dummy package project.
1Del foro de academics.aldebaran-robotics.com
68
# Always put this: this won’t work if version
# is less than 2.6.4
cmake_minimum_required(VERSION 2.6.4 FATAL_ERROR)
# Name of your project: (use for the .snl filename among other things)
project(packaging)
# Always include() this: this performs a few checks, and makes
# sure everything will work.
# The bootstrap.cmake file should be at the top of your project.
# it’s always the same.
include(’${CMAKE_CURRENT_SOURCE_DIR}/bootstrap.cmake’)
# Create a library:
# first arg: the name of a CMake _target_.
# following args: list of sources, or keywords such as SHARED, STATIC ...
create_lib(world ’src/world.cpp’)
# Make the lib available for other projects:
# first arg: the name of the CMake target
# second arg: the name of the ’staged’ library
stage_lib(world WORLD)
# to use in an other project:
# use_lib(my_target WORLD)
# Create a binary using world library:
create_bin(hello ’src/main.cpp’)
use_lib(hello WORLD)
# Install rules for headers:
# WORLD is the name of the ’staged’ library, so that
# that you can also use the lib for compilation in
# an other project _after_ installation.
install_header(WORLD ’src/world.h’ SUBFOLDER ’world’)
# This is how you can install data files
install_data(hello ’data/hello.dat’)
69
Posteriormente hacemos el procedimiento ya mencionado para compilar.Para poder usar esta librerıa, no basta con poner use lib en el archivo CMa-keLists.txt de nuestros modulos donde la queremos usar; tenemos que incluirel directorio sdk creado dentro de buildcross/ en nuestra librerıa en el archivotoolchain.cmake de la siguiente manera: en la parte
#it’s possible to add more sdk here
set(SDK_DIRS "${OE_CROSS_DIR}/staging/geode-linux/usr/")
debemos agregar:
set(SDK_DIRS "/path/to/the/library/buildcross/sdk")
Esto lo hacemos por cada librerıa que queramos cargar en nuestros modulos,por ultimo compilamos la librerıa.
70
Bibliografıa
[1] Michael Calonder and et al. Brief: Binary robust independent elementaryfeatures. CVLab, EPFL, Lausanne, Switzerland.
[2] Arnaud Doucet, Simon Godsill, and Christophe Andrieu. On sequentialmonte carlo sampling methods for bayesian filtering.
[3] T. Drummond and R. Cipolla. Real-time tracking of complex structureswith on-line camera calibration.
[4] Alex Flint and et al. Growing semantically meaningful models for visualslam. IEEE, 2010.
[5] D. A. Forsyth and J. Ponce. Computer Vision: A Modern Approach.2002.
[6] Jean-Bernard Hayet and et al. Visual landmarks detection and recon-gnition for mobile robot navigation. In Proc. Intl. Conf. on ComputerVision and Pattern Recognition (CVPR’03), 2003.
[7] Jean-Bernard Hayet and et al. Motion planning for a vigilant huma-noid robot. Accepted for oral presentation in IEEE-RAS Int. Conf. onHumanoids (Humanoids’09), 2009.
[8] S. J. Julier and J. K. Uhlmann. A new extension of the kalman fil-ter to nonlinear systems. In Proc. of AeroSense: The 11th SPIE In-ternational Symposium on Aerospace/Defense Sensing, Simulation andControls, 1997.
[9] Jana Kosecka and et al. Efficient computation of vanishing points. IEEEInternational Conference on Robotics and Automation., May 2002.
71
[10] Tim Laue and et al. Efficient and reliable sensor models for humanoidsoccer robot self-localization. Proceedings of the 4th Workshop on Hu-manoid Soccer Robots. A workshop of the 2009 IEEE-RAS Intl. Conf.On Humanoid Robots (Humanoids 2009), 2009.
[11] Somchaya Liemhetcharat, Brian Coltin, and Manuela Veloso. Vision-based cognition of a humanoid robot in standard platform robot soccer.Proceedings of the 5th Workshop on Humanoid Soccer Robots (Huma-noids 2010), 2010.
[12] Mark Maimone, Yang Cheng, and Larry Matthies. Two years of visualodometry en the mars exploration rovers.
[13] Branislav Micusik and et al. Detection and matching of rectilinear struc-tures.
[14] Alberto Pretto and et al. A visual odometry framework robust to motionblur.
[15] Gerhard Reitmayr and Tom W. Drummond. Going out: Robust model-based tracking for outdoor augmented reality.
[16] Edward Rosten and Tom W. Drummond. Machine learning for high-speed corner detection. Department of Engineering, Cambridge Univer-sity, UK.
[17] Simon Thompson and Satoshi Kagami. Humanoid robot localisationusing stereo vision. Proceedings of 2005 5th IEEE-RAS InternationalConference on Humanoid Robots, 2005.
[18] S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. MIT Press,2005.
[19] Teresa Vidal-Calleja and et al. A fast visual line segment tracker.
[20] Kai M. Wurm, Armin Hornung, and Maren Bennewitz. Humanoid ro-bot localization in complex indoor environments. Intelligent Robots andSystems (IROS), 2010 IEEE/RSJ.
72
