PARA ROBOTS ARQUITECTURA DE NAVEGACION MOVILES EN ENTORNOS EXTERIORES USANDO VISUAL ODOMETRY PARA LA LOCALIZACION

R. Gonz alez, F. Rodr guez, J.L. Guzm an Dpto. de Lenguajes y Computaci on, Universidad de Almer a, Almer a, Espa na
{rgonzalez, frrodrig, joguzman}@ual.es

C. Pradalier, R. Siegwart Autonomous Systems Lab, ETH Z urich, Suiza

cedric.pradalier@mavt.ethz.ch, rsiegwart@ethz.ch

Resumen
En este art culo se presenta una arquitectura de navegaci on jer arquica para robots m oviles usando como t ecnica de localizaci on visual odometry. El esquema de control de movimiento esta compuesto por un controlador principal, que asegura que el robot sigue la trajector a de referencia. Este controlador genera las consignas de velocidad apropiadas a dos controladores de bajo nivel. Resultados reales, en condiciones exteriores, muestran el correcto funcionamiento de la arquitectura de control dentro de un tiempo de computaci on reducido. Palabras clave: Robots M oviles Aut onomos, Visual Odometry, Control de robots, Deslizamiento.

Visual odometry constituye una t ecnica de localizaci on relativamente barata puesto que una c amara puede reemplazar a un conjunto de sensores como codicadores, IMU, GPS, etc. Adem as, la informaci on visual obtenida evita el problema del deslizamiento, t pico en soluciones tipo odometr a. Por contra, su principal limitaci on es que depende de las condiciones de iluminaci on del entorno. Generalmente, existen dos formas de implementar visual odometry. La t ecnica m as extendida es llamada optical ow [12]. Esta se basa en seguir caracter sticas distintivas entre im agenes adquiridas sucesivamente, determinando entre la imagen actual y la anterior varios vectores velocidad. Otra aproximaci on es a trav es de la t ecnica de template matching. Esta se basa en realizar un proceso de correlaci on de un punto, denido en la imagen actual, en una imagen previa, de esta forma se estima el desplazamiento ocurrido entre una imagen y otra [11], [9]. Generalmente, el tiempo de computaci on es menor que en el primer caso, y es especialmente u til en terrenos poco texturizados (asfalto, arena, etc.). Este art culo presenta una arquitectura de navegaci on jer arquica para robots m oviles en entornos exteriores usando la t ecnica de visual odometry para estimar la localizaci on del robot. El esquema de control de movimiento est a compuesto por un controlador principal, que asegura que se sigue la trayectoria de referencia, y dos controladores de bajo nivel que se encargan de controlar la velocidad de las ruedas. Esta arquitectura se ha probado en condiciones exteriores obteniendo un funcionamiento satisfactorio en un tiempo de computaci on reducido. En la siguiente secci on se presenta la arquitectura de navegaci on jer arquica completa. En la Secci on 3 se describe la t ecnica de localizaci on basada en visual odometry. La Secci on 4 esta relacionada con la estrategia de control de movimiento. Los resultados experimentales se discuten en la Secci on 5. Por u ltimo, se exponen las conclusiones y los trabajos futuros en la Secci on 6.

1.

INTRODUCCION

La aplicaci on de robots m oviles para trabajos en entornos exteriores (tareas de exploraci on, labores agr colas, etc.) constituye un campo de investigaci on muy atractivo y complejo. Los problemas m as importantes que se deben resolver, a la hora de trabajar en este tipo de entornos, est an relacionados con la determinaci on de la posici on (localizaci on) y el control de movimiento del robot. Para poder trabajar con exito en condiciones exteriores, se debe aplicar un m etodo de localizaci on apropiado, capaz de solventar problemas t picos como el deslizamiento de las ruedas [5], [1], ruidos y perturbaciones (terreno irregular, fuentes externas de ruido, etc.). Estas condiciones llevan a que soluciones t picas como la odometr a puedan resultar inapropiadas. Incluso, a veces, soluciones tipo GPS pueden no ser aplicables, por ejemplo, en exploraci on espacial o en entornos parcialmente cubiertos (proximidad a arboles, edicios, etc.). Una de las soluciones m as exitosas, hasta ahora, ha sido la t ecnica denominada visual odometry o ego-motion estimation [10], [12]. Visual odometry se dene como la estimaci on incremental de la localizaci on de un robot m ovil usando una secuencia de im agenes tomadas por una (o m as) c amara(s) a bordo del robot.

Figura 1: Arquitectura de navegaci on jer arquica

2.

SINOPSIS ARQUITECTURA DE NAVEGACION

m ovil usando visual odometry. En este caso, se ha implementado usando el m etodo de template matching. La estrategia planteada se basa en tomar dos secuencias de im agenes. Una secuencia de im agenes viene de una c amara que apunta al suelo justo delante del robot m ovil, y la segunda viene de una c amara panor amica. La primera secuencia se utiliza para estimar el deslizamiento longitudinal del robot y la segunda para estimar la orientaci on. El m etodo de template matching se basa en encontrar la posici on de una subimagen o plantilla en el interior de otra imagen m as grande, llamada area de b usqueda [7]. Para ello, esa plantilla se debe mover por todo el area de b usqueda, calculando la correlaci on entre los p xeles. En este caso, la plantilla se dene como un rect angulo centrado en la imagen actual, dicha plantilla se comparar a con la imagen inmediatamente anterior. El proceso de correlaci on se basa en calcular un vector de coecientes para cada posici on de la imagen (s, v ), esto es,
h1 w1

La arquitectura de navegaci on planteada sigue una estructura t pica de cuatro capas, ver Figura 1. La capa superior est a relacionada con la generaci on de la trayectoria y las velocidades de referencia. En este caso, consideramos que dichos pasos han sido realizados previamente. La segunda capa est a relacionada con el controlador principal, el cual enviar a a la capa inferior o de control de bajo nivel las consignas de velocidad necesarias para que el robot siga la trayectoria de referencia. En este caso, se han sintonizado dos controladores tipo PI para asegurar que las ruedas alcanzan la velocidad deseada. Finalmente, se encuentra la capa de localizaci on, encargada de obtener la posici on y la orientaci on del robot m ovil. Como se ha descrito previamente, se ha utilizado la t ecnica de visual odometry para este prop osito. Una factor signicativo de esta arquitectura de navegaci on es el tiempo de computaci on. En este caso, en cada instante de muestreo se requiere: (i) tomar dos im agenes para llevar a cabo el proceso de estimaci on de la posici on, (ii) ejecutar el algoritmo de template matching usando las im agenes, (iii) estimar el deslizamiento longitudinal y (iv) ejecutar el algoritmo de control para determinar las velocidades que deben seguir las ruedas. En este sentido, una de las principales contribuciones de este trabajo es que, como se demuestra en las pruebas reales, la arquitectura de navegaci on se ejecuta con un periodo de muestreo de 0.35 [s].

R(s, v ) =
i=0 j =0

(i, j )) (T (i, j ) T (1)

(i + s, j + v )), (I (i + s, j + v ) I

3.

USANDO LOCALIZACION VISUAL ODOMETRY

donde w, h son el ancho y la altura de la plantilla, respectivamente, T (i, j ) e I (i, j ) son los valores de los p xeles en la posici on (i, j ) de la plantilla y (i, j ) e del area de b usqueda, respectivamente, y T (i, j ) son los valores medios de la plantilla y del I area de b usqueda, respectivamente. Estos valores medios se calculan como, (i, j ) = T 1 (wh)
h1 w1

En esta secci on, se explican los pasos llevados a cabo para estimar la localizaci on de un robot

T (i , j ),
i =0 j =0

(2)

e, (i + s, j + v ) = I 1 (wh)
h1 w1

I (i + s, j + v ). (3)
i =0 j =0

Ahora, con objeto de evitar los cambios en el brillo de las im agenes, el anterior vector se normaliza. Para este n, se divide por la desviaci on est andar,
h1 w1

N (s, v ) =
i=0 j =0

(i, j )2 I (i + s, j + v )2 , (4) T

Figura 2: Estimaci on del desplazamiento longitudinal (c amara apuntando al suelo) utiliza una c amara apuntando al entorno (vista panor amica). De esta forma, la orientaci on viene dada por, (k ) =
e arctan(v (k ), fx ),

(i, j ) = T (i, j ) T (i, j ) e I (i + s, j + v ) = donde T I (i + s, j + v ) I (i + s, j + v ). Finalmente, el vector de correlaci on normalizado es, (s, v ) = R(s, v ) . R N (s, v ) (5)

(8)

esta dentro del rango N otese que el valor de R [1, 1]. Cuanto m as pr oximo est e a 1 m as similares ser an los p xeles de la plantilla y del area de b usqueda. Por este motivo, el punto de m axima correlaci on ser a, M = max(R (s, v )), R y (sM , v M ) es la posici on de dicho punto. A n de obtener el desplazamiento longitudinal del robot se utilizan las im agenes tomadas por una c amara apuntando al suelo, justo delante del robot. Como se muestra en la Figura 2, al instante de tiempo t = , el robot toma una imagen del suelo justo delante de el. Al siguiente instante t = +1, se utiliza el m etodo de template matching para correlacionar una plantilla denida en la actual imagen con la imagen previa. Como resultado se obtendr a el punto de m axima correlaci on. El desplazamiento longitudinal se obtiene como la diferencia entre dicho punto y la posici on en la imagen anterior del mismo punto. Esta diferencia en p xeles se pasa a unidades m etricas usando los par ametros de calibraci on de la c amara, esto es, x(k ) = s(k ) Z g, fx (7) (6)

donde es el desplazamiento angular en radianes, v es el desplazamiento lateral en p xeles, obtenido tras aplicar el proceso de template matching a las im agenes tomadas por esta c amara, y e fx es la distancia focal de la c amara.

Figura 3: Estimaci on de la orientaci on (c amara apuntando al entorno) La localizaci on del robot m ovil a lo largo del tiempo, suponiendo un robot con mecanismo diferencial, es, X (k + 1) = Y (k + 1) = (k + 1) = donde [X robot. Y X (k ) + x(k ) cos (k ), Y (k ) + x(k ) sin (k ), (k ) + (k ),

(9)

]T es la posici on y orientaci on del

donde k Z+ es el tiempo discreto, x es el desplazamiento longitudinal en unidades m etricas, s es el desplazamiento longitudinal en p xeles, Z g es la altura de la c amara, y fx es la distancia focal de la c amara (que apunta al suelo). Por otro lado, como se muestra en la Figura 3, el proceso para obtener la orientaci on del robot es similar al utilizado para estimar el desplazamiento longitudinal. La diferencia es que ahora se

4.

ESTRATEGIA DE CONTROL

En esta secci on se describe el controlador principal implementado en la arquitectura de navegaci on presentada en este trabajo. En este caso, se ha optado por utilizar un controlador con compensaci on del deslizamiento longitudinal basado en el conocido controlador lineal con realimentaci on presentado en [2]. En este caso, el controlador utilizado incluye t erminos adicionales relacionados con

la medida actual del deslizamiento longitudinal, a n de compensar dicho efecto1 . 4.1. Modelo del error en el seguimiento de trayectorias

do en [2] se basa en la siguiente ley de control, u1 (t) = 1 (t)ex (t), u2 (t) = 2 (t)sign(v ref (t))ey (t) 3 (t)e (t), (11) donde i son ganancias dependientes del tiempo, y sign() es la funci on signo. Reemplazando (11) en (10), el sistema en bucle cerrado (e (t) = Abc (t)e(t)) se dene como, Abc (t) =
1 (t) (t) ref (t) 0 ref (t) 0 2 (t)sign(vref (t)) 0 vref (t) 3 (t)

El problema de seguimiento de trayectorias se puede ver como el problema en el cual un robot m ovil debe seguir un robot virtual, que representa las posiciones y velocidades deseadas [2]. En este caso, se utiliza el modelo del error en el seguimiento de trayectorias [5], [4], e (t) = A(t)e(t) + Bu(t), (10)

y el polinomio caracter stico de dicho sistema es, |sI Acl |= ((t) ref (t))
0 s + 1 (t) ref (t) s 2 (t)sign(vref (t)) 0 vref (t) s + 3 (t)

donde t R es el tiempo continuo, e = [ex ey e ]T R3 es el estado actual (error) y u = [u1 u2 ]T R2 es la entrada de control. La matrices A y B se denen por, A(t) = 0 ((t) ref (t)) 0 1 0 0 0 , 0 1 ref (t) 0 0 0 vref (t) , 0

Al igual que en [2] suponiendo que la din amica del sistema es de segundo orden, y siguiendo los pasos descritos en [2], [5], los valores de las ganancias se determinan en cada instante de tiempo como,
2 2 (t)) 2 , (t) + vref 1 (t) = 2 (ref 2 (t) = |vref (t)| + ref (t)(t), 2 2 3 (t) = 2 (ref (t) + vref (t)) 2 ,
1 1

(12) (13) (14)

vml il donde = vmr ir es un par ametro que deb pende de la velocidad (vmr , vml ) y del deslizamiento longitudinal de cada rueda (ir , il ), y ref , vref son las velocidades angulares y lineales de referencia, dadas por, ref ref vr (t) + vl (t) , 2 ref v ref (t) vl (t) ref (t) = r , b

donde > 0 y > 0 son constantes sintonizadas experimentalmente. N otese que en el c alculo de la ganancia 2 aparece el par ametro , que est a relacionado con el deslizamiento longitudinal (medido en cada instante de tiempo). De esta forma, esta ganancia es actualizada online dependiendo del valor del deslizamiento y, por lo tanto, compensando su efecto. 4.3. Medida del deslizamiento online

vref (t) =

ref ref donde vr y vl son las velocidades de referencia de la rueda derecha e izquierda, respectivamente, y b la distancia entre el centro de las ruedas.

El deslizamiento se dene como la relaci on entre la velocidad real de avance de un veh culo, y la velocidad angular de sus ruedas [13]. Matem aticamente, ij (t) = 1 vm (t) 100, rj (t) (15)

4.2.

Controlador lineal con compensaci on del deslizamiento

En esta subsecci on se describe brevemente el controlador con compensaci on del deslizamiento longitudinal basado en el conocido controlador lineal con realimentaci on aparecido en [2] y que ha sido previamente publicado en [5], [4]. El controlador lineal con realimentaci on presenta1 El deslizamiento (slip ) es uno de los factores m as importantes relacionados con la movilidad de robots en entornos exteriores [1], [8]. El efecto que produce es una perdida de tracci on/velocidad. Es debido principalmente a factores relacionados con la interacci on entre las ruedas y el suelo [13].

donde ij es el deslizamiento normalizado de la rueda j , vm es la velocidad lineal de avance del veh culo, r es el radio de la rueda y j es la velocidad angular de giro de la rueda j . En la literatura existen diferentes formas para determinar este deslizamiento, ver [8] para una revisi on. En este caso, se utiliza una aproximaci on similar a la aparecida en [1]. La velocidad real de avance del veh culo se obtiene del desplazamiento longitudinal obtenido usando la informaci on visual (7), y la velocidad angular de giro viene de los codicadores acoplados a las ruedas.

5.
5.1.

RESULTADOS
Robot M ovil

A n de llevar a cabo pruebas reales de la arquitectura de navegaci on planteada, se ha utilizado el robot m ovil F itorobot, disponible en la Universidad de Almer a (ver Figura 4a) [6]. Se pueden consultar otras pruebas realizadas con el CRAB rover del ASL, ETH Z urich, en [3]. El robot m ovil F itorobot tiene una masa de 500 [Kg] y unas dimensiones de 1.5 [m] largo x 0.7 [m] ancho. El sistema de locomoci on es de tipo mecanismo diferencial con orugas. La propulsi on la genera un motor de gasolina de 20 [CV].

ha utilizado un ordenador port atil para las tareas relacionadas con el tratamiento de im agenes. La posici on del robot se calcula en este ordenador (visual odometry) y cada periodo de muestreo se env a, v a RS232, al ordenador principal. En este ordenador principal se ejecuta el controlador de seguimiento de trayectoria y los controladores de bajo nivel, que controlan las velocidades de las orugas. Adem as, se utilizan unos codicadores acoplados a los ejes de las orugas para realimentar a los controladores de bajo nivel. Not ese que el periodo de muestreo del algoritmo de visi on es inferior al del bucle de control (0.1 [s]). 5.2. Pruebas Reales

En esta subsecci on se discute una prueba real de la arquitectura de navegaci on planteada. Aunque se han realizado varias pruebas en diferentes terrenos, en este caso, se ha seleccionado un experimento realizado en suelo tipo grava, con una trayectoria de referencia tipo L. La distancia completa fue de 20 [m] (ver Figura 5). Esta prueba se llev oa cabo en condiciones de luz solar directa, con un periodo de muestreo de 0.35 [s].

(a)

(a)

(b)

Figura 4: (a) Robot m ovil Fitorobot en el lugar de las pruebas (b) Componentes hardware utilizados

(b)

Como se observa en la Figura 4a se han colocado dos c amaras en el robot m ovil para la tarea de localizaci on. Una c amara apunta al suelo justo delante del robot y otra apunta al entorno (vista panor amica). Por otro lado, en la Figura 4b se describen los elementos hardware implicados en la arquitectura de navegaci on. En este caso, se

Figura 5: (a) Sitio para pruebas reales (imagen panor amica) (b) Suelo grava (imagen tomada por la c amara apuntando al suelo) En la Figura 6a se muestran las trayectorias seguidas. En este caso, se ha utilizado un DGPS como ground-truth para validar la trayectoria

seguida y una br ujula magn etica para validar la orientaci on. En estas guras se observa como el robot sigue de manera satisfactoria la referencia tanto en posici on como en orientaci on. Se distinguen tres valores at picos en la reconstrucci on de la trayectoria seguida, este hecho se debe a false matches no detectados por el algoritmo de correlaci on.
Trajectorias 10

Error longitudinal 1.4 1.2 1 0.8 ex [m] 0.6 0.4 0.2 0 0.2 0.4

Referencia Robot DGPS (groundtruth)

10 distancia [m]

15

20

(a)
Error lateral 0.8 0.6 0.4 0.2 ey [m]

6 Y [m]

0 0.2 0.4 0.6 0.8

2 2

4 X [m]

10

12

(a)
Orientaciones 120

100

Referencia Robot

10 distancia [m]

15

20

80

Brjula (ground truth)

(b)

[deg]

60

40

Figura 7: (a) Error longitudinal (b) Error lateral en funci on de la distancia

Velocidades lineales 0.8 0.7 0 10 20 30 40 tiempo [s] 50 60 70 0.6 0.5 velocidad [m/s] 0.4 0.3 0.2 0.1 Robot Cod. izquierdo Cod. derecho

20

20

(b)

Figura 6: (a) Trayectorias (b) Orientaciones

En la Figura 7 se muestran el error longitudinal y el error lateral, esto es, la diferencia entre la posici on del robot y la posici on del robot referencia en relaci on a la distancia recorrida. Como se observa, el punto cr tico en esta trayectoria es el giro hacia la izquierda. El robot llega con un error de aproximadamente -0.4 [m] y luego corrige hasta llegar a un error cercano a cero. El valor medio del error longitudinal es 0.1 [m] que signica el 5 % de la distancia recorrida, con una desviaci on est andar de 0.23 [m]. El valor medio del error lateral es de -0.03 [m], con una desviaci on est andar de 0.18 [m]. En la Figura 8 se comprueba que la velocidad real de avance del robot es muy similar a la velocidad de los codicadores. Esto signica que el deslizamiento longitudinal en este terreno es muy reducido. Recu erdese que es un terreno tipo grava compactada. En la Figura 9, se muestran las se nales generadas por el controlador principal (velocidades de las

0 0.1

10

20

30 40 tiempo [s]

50

60

70

Figura 8: Velocidades orugas y velocidad lineal del robot

orugas). Se puede observar como los controladores de bajo nivel PI alcanzan dichas referencias satisfactoriamente.

6.

CONCLUSIONES Y TRABAJOS FUTUROS

Este trabajo muestra la aplicaci on pr actica de una arquitectura de navegaci on de un robot m ovil en entornos exteriores utilizando la t ecnica de visual odometry para la localizaci on. Adem as, debido a que el movimiento del robot esta afectado por

Velocidades oruga derecha 0.9 0.8 0.7 0.6 velocidad [m/s] 0.5 0.4 0.3 0.2 0.1 0 0.1 0 10 20 30 40 tiempo [s] 50 60 70 Codificador vr vref r

Springer, 1997. [3] R. Gonzalez, Localization of the CRAB rover using Visual Odometry, Tech. Report. ETH Z urich, Suiza; 2009. Disponible: http://www.ual.es/personal/rgonzalez/ english/publications.htm. [4] R. Gonzalez, M. Fiacchini, T. Alamo, J.L. Guzman and F. Rodriguez, Adaptive Control for a Mobile Robot under Slip Conditions using an LMI-based Approach, European Journal of Control, vol 16(2), pp. 144155 (2010). [5] R. Gonzalez, F. Rodriguez, J.L. Guzman and M. Berenguel, Localization and Control of Tracked Mobile Robots under Slip Conditions, IEEE International Conference on Mechatronics, M alaga, Spain, April 2009, pp. 1-6.
Codificador v
l

(a)
Velocidades oruga izquierda 0.7 0.6 0.5 0.4 velocidad [m/s] 0.3 0.2 0.1 0 0.1 0.2 0.3 0 10 20 30 40 tiempo [s]

vref l

50

60

70

(b)

[6] R. Gonzalez, F. Rodriguez, J. SanchezHermosilla and J.G. Donaire, Navigation Techniques for Mobile Robots in Greenhouses, Applied Engineering in Agriculture, vol. 25, 2009, pp 153-165. [7] A. Goshtasby, S.H. Gage and J.F. Bartholic, A Two-Stage Correlation Approach to Template Matching, IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 6, 1984, pp 374-378. [8] K. Iagnemma and C.C. Ward, Classicationbased wheel slip detection and detector fusion for mobile robots on outdoor terrain, Autonomous Robots, vol. 26(1), 2009, pp 33-46. [9] F. Labrosse, The Visual Compass: Performance and Limitations of an AppearanceBased Method, Journal of Field Robotics, vol. 23, 2006, pp 913-941. [10] L. Matthies, M. Maimone, A. Johnson, Y. Cheng, R. Willson, C. Villalpando, S. Goldberg and A. Huertas, Computer Vision on Mars, International Journal of Computer Vision, vol. 75, 2007, pp 62-92. [11] N. Nourani-Vatani, J. Roberts and M.V. Srinivasan, Practical Visual Odometry for Car-like Vehicles, IEEE International Conference on Robotics and Automation, Kobe, Jap on, 2009, pp. 3551 - 3557. [12] C.F. Olson, L.H. Matthies, M. Schoppers and M.W. Maimone, Rover Navigation Using Stereo Ego-motion, Robotics and Autonomous Systems, vol. 43, 2003, pp 215-229. [13] J.Y. Wong, Theory of Ground Vehicles, USA, John Wiley and Sons, 2001.

Figura 9: (a) Velocidades oruga derecha (b) Velocidades oruga izquierda

factores que dependen del terreno, como es el caso del deslizamiento longitudinal, se ha utilizado un controlador con compensaci on de dicho efecto. Para ello, las ganancias de dicho controlador se actualizan en linea dependiendo de la medida obtenida, en cada instante de tiempo, del deslizamiento longitudinal. Actualmente, se est an analizando t ecnicas de fusi on, tipo Filtro de Kalman, para combinar las lecturas de otros sensores. Adem as, se estudiaran nuevas formas de a nadir un cierto grado de reactividad a la actual arquitectura, para poder evitar obst aculos imprevistos. Agradecimientos Este trabajo ha sido nanciado con el Proyecto del Plan Nacional DPI2007-66718-C04-04 del Ministerio de Ciencia e Innovaci on y Fondos FEDER.

Referencias
[1] A. Angelova, L. Matthies, D. Helmick and P. Perona, Learning and Prediction of Slip from Visual Information, Journal of Field Robotics, vol. 24, 2007, pp 205-231. [2] C. Canudas, B. Siciliano and G. Bastin, Theory of Robot Control, The Netherlands,