α

N ×N N2

Sb

Sw

|Sb||Sw|

a)

Swedish 90° Swedish 45°

Swedish 45°

b) c)

CIR

noicceridedaerroC

noicceridrotoM

noiccartrotoM

noiccartaerroC

2

motor1 = speed1− turn; motor2 = speed1 + turn

μ

μ

general

Interruptor

carga-uso

Conmutador

Jack

Conector

MD23

Controladora

Fusible

+

−

Baterıa

d = 2

hc

hp − hc

d=

(θv2

)

hp θv

2/vθ

ch

ph

d

2

2

USB

USB

C2I

C2I

Asus Xtion Live Pro

MacBook (2008)

MD23

3)×SRF08 (2)×EMG30 (

Arduino Duemilanove

de 6 pines

Conector JST

2

2

Anunciar mensajes a publicar y suscribirse a mensajes necesarios

Inicializar MD23 y SRF08

Comprobar recepciónde mensajes

¿Timeoutconsigna

velocidad?

Consignas de velocidad = 0

¿Tiempo esperasónar completo?

Sí

No

Leer distancias,publicarlas y

disparar sónar

Sí

¿Tiempo espera control velocidad

completo?

Sí

No

Leer encoders,publicar lectura encoders

estimar velocidad,calcular actuación y

enviar actuación a MD23

Sí

Leer voltaje de bateríay publicarlo

Espera X ms

No

v =Δticks

Δt· π ·Dticks

Δticks Δt

Δt

ticks

D

3 · 10−2

O

i

ds(i)

iparent

∀ i /∈ O → ds(i) = ∞

i∗

i∗

i∗

ds(i)

ds(i) = ds(i∗) + di∗(i)

ds(i∗)

i∗

di∗(i) i∗

i

ds(i) = min[dolds (i), ds(i∗) + di∗(i)]

dolds (i)

i∗

s g s−1−2−g

s g

1

2

5

6

1

2

2

2

5

s g

1

2

5

6

1

2

2

2

4

8

s g

1

2

5

6

1

2

2

2

4

5

O O O

1(ds(1) = 2, iparent = s) 2(ds(2) = 4, iparent = 1 g(ds(g) = 5, iparent = 2)

2(ds(2) = 5, iparent = s) g(ds(g) = 8, iparent = 1)

v, ω

vc ωc

v ω

maxωmaxω−cω

cv

maxvv

ω

acimanid

Ventana

admisibles

Velocidades

no admisibles

Velocidades

admisibles

alcanzables y

Velocidades

O

O = a · heading(v, ω) + b · velocity(v, ω) + c · dist(v, ω)

heading

velocity

dist

a b c

vx ωz

vl vr

vx ω

vl vr

vl vr vl �= vr

ω

CIRω

lvxv

rv

R

2l/2l/

vl = ω (R− l

2)

vr = ω (R +l

2)

R

l

ω =vr − vl

l

R =l

2· vl + vrvr − vl

vx

vx = ω ·R =vl + vr

2

vl vr

ω vx

vr = vx +ω · l2

vl = vx − ω · l2

vr vl

x y θ

vl vr

CIR = [CIRx, CIRy] = [x−R (θ), y +R (θ)]

θ′

δt

θ′ = θ + δθ = θ + ω δt

x′ y′ θ′

x

y θ

′θ

R 2l/

CIRω δt

)x, y(

)′, y′x(

2l/

δt δθ = ω δt

⎛⎜⎜⎜⎝x′

y′

θ′

⎞⎟⎟⎟⎠ =

⎛⎜⎜⎜⎝

(ω δt) − (ω δt) 0

(ω δt) (ω δt) 0

0 0 1

⎞⎟⎟⎟⎠

⎛⎜⎜⎜⎝x− CIRx

y − CIRy

θ

⎞⎟⎟⎟⎠+

⎛⎜⎜⎜⎝CIRx

CIRy

ω δt

⎞⎟⎟⎟⎠

x′ = x+R [ (θ) ( (ω δt)− 1) + (θ) (ω δt)]

y′ = y +R [ (θ) (1− (ω δt)) + (θ) (ω δt)]

θ′ = θ + ω δt

vl = dl/δt vr =

dr/δt dl dr δt

x′ y′ θ′

dl dr x y θ

x′ = x+l

2

dl + drdr − dl

[(θ)

( (dr − dl

l

)− 1

)+

+ (θ)

(dr − dl

l

)]

y′ = y +l

2

dl + drdr − dl

[(θ)

(1−

(dr − dl

l

))+

+ (θ)

(dr − dl

l

)]

θ′ = θ +dr − dl

l

dr dl ω ≈ 0

Ω = dr−dll

Ω

Ω→0x′ =

Ω→0

(x+

dl + dr2

[(θ)

(Ω)− 1

Ω+ (θ)

(Ω)

Ω

])′=

′=

Ω→0

(x+

dl + dr2

[− (θ) (Ω) + (θ) (Ω)]

)=

= x+dl + dr

2(θ)

Ω→0y′ =

Ω→0

(y +

dl + dr2

[(θ)

1− (Ω)

Ω+ (θ)

(Ω)

Ω

])′=

′=

Ω→0

(y +

dl + dr2

[ (θ) (Ω) + (θ) (Ω)]

)=

= y +dl + dr

2(θ)

dl dr

dl dr vl vr

dl = ΔtickslπD

ticksdr = Δticksr

πD

ticks

vl =dlδt

vr =drδt

Δticksr Δticksl

δt

δt

ticks

D

dl �= dr dl = dr

vx ω

x y θ

x y (θ/2) + (θ/2) k

i i

α = αmin + i ·Δα

αmin i = 0 Δα

d

α = 0

w × dmin w dmin

vx

vx(d) =

⎧⎪⎪⎪⎨⎪⎪⎪⎩

0 ∀ d < dmin

vmin + (vmax − vmin)d−dmin

dvmax−dmin∀ d ∈ [dmin, dvmax ]

vmax ∀ d > dvmax

d α = 0

minα(0) =α

αΔ·i+minα) =i(α= 0α

αΔ

dmin

dvmax

vmin

vmax

w

= 0α

mind

α = 0

d(i)

w × dmin α = αmin + i · Δα

i

d(i)

w

2w/

αΔ·i+minα) =i(α

mind

i = 0

imax α = αmin + imax · Δα

ω

ω(α) = αωmax

αωmax

αωmax

ωmax

vx = 0

ω(α) =

[ωmin, α

ωmax

αωmax

]

ωmin

d

d

ω

d = d = ω = ±0.7

d

vx = 0 ω = 0

d =

ω

vx = 0 ω = ω

d

ω =

Recibir lectura de los sensores

¿Colisión frontalinminente?

Avance en línea recta

Consignas de velocidad

nulas

¿Colisión lateralinminente?

Velocidad angular=

velocidad corrección

Giro sobre símismo

¿Estancamientoinminente?

Sí

Sí

Vel angular =vel recuperación,

Vel lineal = 0

No

No

No

Sí

Parada de seguridad

Giro de recuperación

Giro de corrección

Publicar consignas velocidad linear

y angular

xy

d∗

d∗i = di · α(θi)

di i

θi i

α αmax θ = 0 αmin

θ = ±π

α(θ) =

⎧⎨⎩αmax − θ (αmax − αmin) ∀ θ ∈ [0, π]

αmax + θ (αmax − αmin) ∀ θ ∈ [−π, 0)

αmax = 1.5 αmin = 1.5

θ = 0

θ = π

θ

α

minα

maxα

ππ− 0

α

d∗

i

XY/

POi

/ = (x, y, z) di =√

x2 + y2

POi

/

i

di i

θi

θi

z/

P goal/ =

(1.9, 0, 0) qgoal/ = k

Inicio

Elegir usuarioobjetivo

Generar y enviarmeta de navegación

¿Metaalcanzada?

Buscar usuarioen posición

objetivo

¿Usuario objetivoencontrado?

Éxito=FALSE

Éxito=TRUE

Devolver "Éxito"

No

No

Sí

Sí

Recibirmensajes

¿Petición deservicio?

No

Iniciar detecciónde gestos

¿tiempo de esperaagotado?

Actualizardetección

¿Gestodetectado?

No

¿Realizado porusuario objetivo?

Sí

Sí

No

NoEnvíar respuestaservicio"TRUE"

Enviar respuestaservicio"FALSE"

Sí

Sí

Inicializar nodo

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

Subscribirse al temade estado

Cargar imágenesinterfaz

Mostrar imagenespera

Comprobarmensajes

estado

¿Cambio deestado?

Mostrar imagenasociada a

estado

Espera

No

Sí

Inicializar

Anunciar y suscribirsea los temas necesarios

Ejecutar algoritmowandering y publicar estado

Ejecutar algoritmo detección de personas y publicar estado

¿Alguna personadetectada?

Ejecutar maniobra de aproximación a usuario

y publicar estado

¿Aproximación completadasatisfactoriamente?

Ejecutar detecciónde gestos y publicar

estado

Publicar estado

Sí

No

No

Sí

15×15

e

2× e

e

3× e

e

e

e

e

→

−−

− − − −

−

− − −

− − −

− −

i j k i2 = j2 = k2 =

i j k = −1

�u = (ux, uy, uz) θ

q

q = (θ/2) + (θ/2) ux i+ (θ/2) uy j + (θ/2) uz k