visual servoing

Evaluacin de tcnicas de control visual monocular

para la locomocin de robots humanoides NAO en

tareas de posicionamiento

Mario Alberto Zarco Lpez

Directores:

Dr. Hctor M. Becerra Fermn (CIMAT)

Dr. Juan Mauricio Angeles Cervantes (UNAM)

Centro de Investigacin en Matemticas, GuanajuatoFacultad de Ingeniera, Ciudad Universitaria

Mario Alberto Zarco Lpez (UNAM-CIMAT) Exmen Profesional Mayo 2015 1 / 30

Contenido

1 Introduccin

2 Marco terico

3 Control visual en Matlab

4 Implementacin

5 Resultados

6 Conclusiones


Contenido

1 IntroduccinMotivacin

Objetivos

Antecedentes

2 Marco terico


4 Implementacin

5 Resultados

6 Conclusiones


Motivacin

Evaluar tcnicas de control visual para colocar un robot humanoide en una

pose deseada con el mnimo error, ya que estas tcnicas proveen abundanteinformacin del entorno, tienen un bajo costo computacional y son fciles de

implementar.


Objetivos

Adaptar diferentes tcnicas de control visual monocular para la realizacin de

tareas de posicionamiento de robots humanoides NAO en un ambiente

virtual.

Modificar el prototipo del robot NAO en Webots para aadir un GPS y un

compass a su modelo.

Modificar el controlador naoqisim en Webots para adquisicin de datos.

Programar en C++ con ayuda del software de desarrollo naoqi-sdk :

1. control visual basado en posicin.

2. control visual basado en imagen

Proponer una estructura modular para la ejecucin de los controladores.

Validar el funcionamiento de los esquemas de control a travs de simulaciones

en Webots y comparar su desempeo.


Antecedentes

Visual Servoing: es el uso de informacin visual como realimentacin en un

lazo cerrado de control.

Eye-in-hand : la informacin es obtenida por una cmara montada en el robot.

Eye-to-hand : la informacin es obtenida por una cmara fija en el espacio de

trabajo.

Teach-by-showing: el valor deseado se establece dando una imagen de

referencia u objetivo, la cual debe ser memorizada por el robot.

El objetivo del control visual es regular la posicin y orientacin de la cmara,

y en consecuencia del robot, de tal forma que la imagen de la vista actual dela cmara llegue a ser la misma que la imagen objetivo a travs de minimizar

una funcin de error (funcin de tarea).


Contenido

1 Introduccin

2 Marco tericoControl visual

Control visual basado en posicin

Control visual basado en imagen


4 Implementacin

5 Resultados

6 Conclusiones


Control visualLa funcin de error a minimizar es

e(t) = s(m(t),a) s, (1)

m(t) es un conjunto de medidas de la imagen.

a es un conjunto de parmetros que representan conocimiento potencialadicional.

s es el valor deseado de las caractersticas visuales.

Matriz de interaccin: relaciona el cambio de velocidades de la cmara conel cambio en las caractersticas visuales en el espacio imagen.

s = Lsvc = Ls[v c,c]

(2)

vc son las velocidades instantneas del origen del sistema de coordenadas de la

cmara.

Si se considera vc como la entrada de control, y una disminucin exponencialdel error (e = e), se obtiene:

vc = L+

e e, (3)


Control visual basado en posicinPBVS o 3D: el error es calculado en el espacio Cartesiano de la tarea a partirde un conjunto de parmetros 3D, los cuales son estimados mediante

medidas visuales.

El vector s es un conjunto de parmetros tridimensionales estimados pormedio de las mediciones de la imagen.

s = (c

tc, u) (4)

ctc es un vector de translacin que define las coordenadas del marco de

referencia actual de la cmara con respecto al deseado.

u es la parametrizacin ngulo/eje para la rotacin.

La matriz de interaccin resulta

Le =

[R 0

0 Lu

](5)

La entrada de control es {vc = R

ctc ,c = u.

(6)



IBVS o 2D: el error es calculado a partir de un conjunto de caractersticasvisuales que estn directamente disponibles en el espacio imagen.

Las medidas de la imagen m son usualmene las coordenadas en pxeles deun conjunto de puntos en la imagen.

La funcin de tarea es:

e = W+e1 + (I6 W+W)

hs

X, (7)

es un escalar positivo.

W = [033 I33].

e1i(X) = p

i (X)(Epi(X1)) es la distancia con signo de pi(X) a la linea epipolar.

hs = (k t1

2 t1)2 es la funcin costo a minimizar bajo la restriccin e1 = 0.



La entrada de control es:

vc = W+L

+

ofrote1i(X ) 2(I6 W

+W)[2tk 0] (8)

2tk es el vector de translacin de la k-sima vista a la vista objetivo.

La matriz de interaccin resulta:

Lofrot =

[xi yi (1+ x

2i ) yi

(1+ y2i ) xiyi xi

](9)

La rotacin se mantiene funcionando hasta que

ei1(X ) < (10)

es un umbral que indica la mxima distancia de los puntos a las lneas

epipolares.


Contenido

1 Introduccin

2 Marco terico




4 Implementacin

5 Resultados

6 Conclusiones


Control visual en MatlabError promedio de pxeles:

error =1

NP

NPi=1

||queryi traini ||, (11)

NP es el nmero de puntos,

query son los puntos en la imagen actual,

train son los puntos en la imagen objetivo.

Sean R1, R2, t1 y t2 las soluciones de la descomposicin de la matrizesencial, donde Rij indica el elemento ij de la matriz y tk la componente k del

vector.

1: Si R111 > 0 && R122 > 0 && R

133 > 0 entonces

2: R = R1

3: Si no4: R = R2

5: Si t13 < 0 entonces6: t = t1

7: Si no

8: t = t2



0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1-2

-1

0

1

2

3

4

5

6

7

time (s)

con

trol i

nput

vxvyvzwxwywz

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

10

20

30

40

50

60

70

time (s)

ave

rage

erro

r

error promedio



0 100 200 300 400 500 6000

50

100

150

200

250

300

350

400

450

-2-1

01

2 -2

-1

0

1

2

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

Y

X

X X

TC

Y

Y Y

TCTT Z

Z Z

X

Z

0 0.5 1 1.5 2 2.5-1

-0.5

0

0.5

1

1.5

2

time (s)

con

trol i

nput

s

vxvyvzwxwywz

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

time (s)

no

rm e

|e|


Contenido

1 Introduccin

2 Marco terico


4 ImplementacinModificaciones al software de simulacin

Programacin del robot NAO

MdulosAlgoritmos de control visual

5 Resultados

6 Conclusiones


Webots y simulador de NAOqi

Figura : Esquema para un complemento del controlador

Saber los datos del robot instanciado

Comunicar con el simulador

Comunicar con NAOqiva el HAL

(Hardware Abstraction Layer)

Figura : Esquema del simulador


Modificaciones

1. Modificar el archivo .proto del robot NAO. Habilitar el GPS y el compass.

2. Agregar una biblioteca servidor al controlador Naoqisim. Esperar la conexin en algn puerto en modo de bloqueo (blocking mode). Ver el descriptor del fichero para saber si hay datos en modo no bloqueado

(non-blocking mode).

3. Modificar la clase Nao del controlador Naoqisim. Posibilita obtener los valores de los sensores en cada iteracin del

controlador.


Programacin del robot NAO Broker : intermediario de peticin de objetos. Proxy : objeto de determinada mdulo. Mdulo: clase dentro de una biblioteca.


Mdulos


Detalles de los mdulos

webotsModule

Biblioteca cliente.

Bind method: solicita la lectura del GPS y el compass.

opencvModule

Genera una instancia (proxy ) a:

1. ALMotion

2. ALPosture

3. ALCamera

4. ALProxy

Genera una instancia (proxy ) al mdulo webotsModule.

Bind method: ejecuta el algoritmo de control visual.


Algoritmos de control visual

Leer imagen objetivo (target).

Convertir a escala de grises.

Generar una instancia al detector de caractersticas.

Generar una instancia al descriptor de caractersticas.

Inicializar el proceso de movimiento del robot.

Tomar foto de la imagen actual de la cmara (current).

Salvar valores de GPS y compass.

Detectar caractersticas locales en current y target.

Describir caractersticas locales en current y target.

Emparejar caractersticas locales en current y target.

Filtrar los outliers

Salvar los inliers de current (query ) y de target (train).



Mientras contador < mximo nmero de iteraciones hacerMedir el error promedio de pxeles.

Si error promedio de pxeles < umbral entonces

break

Estimar la matriz fundamental.

Calcular la matriz esencial.

Calcular la orientacin y posicin relativa.

Calcular la parametrizacin ngulo/eje para la rotacin.

Calcular la entrada de control.

Transformar las velocidades al sistema de referencia del robot.

Aplicar las velocidades al robot.

Tomar foto de la imagen actual de la cmara (next)


Rastrear los puntos buenos de current a next .


Asignar next a current .

Asignar los puntos rastreados a query .

Sumar 1 al contador.

Detener el movimiento del robot.


Control visual basado en posicin por fases


Si matriz de Rotacin = matriz Identidad entonces

break







Aplicar la velocidad angular al robot.Tomar foto de la imagen actual de la cmara (next).











Si error promedio de pxeles < umbral entonces

break







Aplicar las velocidades lineales al robot.Tomar foto de la imagen actual de la cmara (next).










Mientras contador < mximo nmero de iteraciones hacerMedir el error promedio de pxeles.Si error promedio de pxeles < umbral entonces

breakSi los inliers estan en las lneas epipolares entonces

Detener el movimiento del robot (dormir el robot 1 segundo) .Inicializar el proceso de movimiento del robot ( = 1).

Estimar la matriz fundamental.Calcular la matriz esencial.Si = 1 entonces

Calcular la posicin relativa.

Calcular la entrada de control.Ajustar las velocidades al centro de masa del robot.Aplicar las velocidades al robot.Tomar foto de la imagen actual de la cmara (next).Convertir a escala de grises.Rastrear los puntos buenos de current a next.Salvar valores de GPS y compass.Asignar next a current.Asignar los puntos rastreados a query .Sumar 1 al contador.



Contenido

1 Introduccin

2 Marco terico


4 Implementacin

5 ResultadosDescripcin de las pruebas


Control visual basado en posicin por fasesControl visual basado en imagen

6 Conclusiones


Descripcin de las pruebas

Figura : Localizacin del robot NAO en las pruebas



0.6 0.4 0.2 0.0 0.2 0.4 0.6

y

1.2

1.0

0.8

0.6

0.4

0.2

0.0

x

Localizacin

0.6 0.4 0.2 0.0 0.2 0.4 0.6

y

1.2

1.0

0.8

0.6

0.4

0.2

0.0

x

Orientacin

0 200 400 600 800 1000 1200 1400

Iteracin

0

10

20

30

40

50

60

70

80

Error

Error promedio de pxeles

Error promedio

0 200 400 600 800 1000 1200 1400

Iteracin

0.000

0.005

0.010

0.015

0.020

0.025

0.030

0.035

0.040

0.045

v

x

, v

y

Velocidades lineales

Velocidad x

Velocidad y

0 200 400 600 800 1000 1200 1400

Iteracin

0.025

0.020

0.015

0.010

0.005

0.000

Velocidad angular

Vel. angular z



0.6 0.4 0.2 0.0 0.2 0.4 0.6

y

1.2

1.0

0.8

0.6

0.4

0.2

0.0

x

Localizacin

0.6 0.4 0.2 0.0 0.2 0.4 0.6

y

1.2

1.0

0.8

0.6

0.4

0.2

0.0

x

Orientacin

0 200 400 600 800 1000 1200 1400 1600

Iteracin

0

20

40

60

80

100

120

140

160

Error


Error promedio

0 200 400 600 800 1000 1200 1400 1600

Iteracin

0.02

0.01

0.00

0.01

0.02

0.03

0.04

0.05

0.06

v

x

, v

y


Velocidad x

Velocidad y

0 200 400 600 800 1000 1200 1400 1600

Iteracin

0.025

0.020

0.015

0.010

0.005

0.000

0.005

Velocidad angular

Vel. angular z

0 200 400 600 800 1000 1200 1400 1600

Iteracin

0.00

0.05

0.10

0.15

0.20

0.25

0.30

0.35

||IR1R2

||

Norma de Frobenius

||I-R1*R2'||



0.4 0.2 0.0 0.2 0.4

y

1.2

1.0

0.8

0.6

0.4

0.2

0.0

x

Localizacin

0.4 0.2 0.0 0.2 0.4

y

1.2

1.0

0.8

0.6

0.4

0.2

0.0

x

Orientacin

0 200 400 600 800 1000 1200 1400 1600 1800

Iteracin

0

20

40

60

80

100

120

140

Error


Error promedio

0 200 400 600 800 1000 1200 1400 1600 1800

Iteracin

0.015

0.010

0.005

0.000

0.005

0.010

0.015

0.020

0.025

0.030

v

x

, v

y


Velocidad x

Velocidad y

0 200 400 600 800 1000 1200 1400 1600 1800

Iteracin

0.002

0.000

0.002

0.004

0.006

0.008

0.010

0.012

Velocidad angular

Vel. angular z

0 200 400 600 800 1000 1200 1400 1600 1800

Iteracin

0.0

0.5

1.0

1.5

2.0

2.5

3.0

3.5

Norma de e

|e|


Resultados

El robot presenta una deriva hacia la izquierda.

El desempeo depende del buen emparejamiento de los puntos.

Existen problemas por debajo de los 20 pxeles del error promedio. Mala estimacin de la matriz fundamental debido a los inliers. Mala estimacin de la matriz fundamental debido a la cercana de la imagen

de referencia a la escena 3D. Mal condicionamiento de la matriz fundamental (short baseline).

La velocidad angular y la velocidad lineal no estan completamente

desacopladas.

El robot no es capaz de retroceder.

Sensible al aumento en las ganancias de control.


Contenido

1 Introduccin

2 Marco terico


4 Implementacin

5 Resultados

6 Conclusiones


Conclusiones

El control visual basado en posicin y el basado en imagen funcionan

para situar al robot NAO en una vecindad de la localizacin deseada.

No se puede asegurar que un esquema sea mejor que el otro.

Las modificaciones hechas a naoqisim y el mdulo webotsModule

podrn integrarse a otros trabajos.


Trabajo a futuro

Evaluar los esquemas de control simulados en un ambiente real.

Evaluar los esquemas de control en tareas de navegacin donde la ruta

est compuesta por varias imgenes de referencia.

Programacin de un control visual hbrido.

Proponer estrategias para realizar tareas de posicionamiento en un

tiempo deseado.


Muchas gracias por su atencin!


IntroduccinMotivacinObjetivosAntecedentes

Marco tericoControl visualControl visual basado en posicinControl visual basado en imagen

Control visual en MatlabControl visual basado en posicinControl visual basado en imagen

ImplementacinModificaciones al software de simulacinProgramacin del robot NAOMdulosAlgoritmos de control visual

ResultadosDescripcin de las pruebasControl visual basado en posicinControl visual basado en posicin por fasesControl visual basado en imagen

Conclusiones

visual servoing

Documents