1

Localizaci n De Un Robot M vil De Conguraci n o o o Ackerman Usando El Filtro Extendido De Kalman

Autores Giraldo Castillo, Oscar oscar.gi.cast@gmail.com Duran Tolentino, Eyner einerdt@gmail.com Asesor: Calle Flores, Ivan Universidad Nacional de Ingeniera

AbstractLa navegaci n de un robot aut nomo es un proceso o o donde la conabilidad es fundamental y dado que nuestro aut mata esta orientado para navegar en un entorno urbano o (con interacci n con otros vehculos o personas) implica que el o m vil este seguro de su posici n en todo instante, con el n o o de cumplir su objetivo sin causar danos civiles. La localizaci n o en un ambiente no cl sico, es decir donde la incertidumbre no a es despreciable, juega un rol sumamente importante al inicio de todo algoritmo de navegaci n m s complejo. Se desea que nuestro o a sistema sea lo m s preciso posible por ello se opta por aplicar a t cnicas probabilsticas las cuales son las m s ideales para la rese a oluci n de este tipo de problemas. En este artculo se desarrollar o a la localizaci n mediante el ltro extendido de Kalman aplicado a o un robot m vil de tipo Ackerman puesto que esta conguraci n o o es la m s empleada en los vehculos motorizados y nos servir a a de base para una futura implementaci n en un vehculo real. o Index TermsRobot m vil, localizaci n, Ackerman, Rob tica o o o probabilstica, Filtro extendido de Kalman.

Las principales fuentes de incertidumbre en la navegaci n o aut noma de un robot son: o 1) Entorno: Mundos fsicos son inherentemente impre decibles. Mientras que el grado de incertidumbre en ambientes bien estructurados (como en las lneas de montaje) es peque o. Ambientes, tales como carreteras n y casas particulares son muy din micos e impredecibles. a 2) Sensores. Los sensores est n inherentemente limitados a en lo que perciben. Las limitaciones surgen de dos factores principales, el rango y la resoluci n de un sensor o est n sujetos a las leyes fsicas. Por ejemplo, las c maras a a no pueden ver a trav s de paredes, e incluso dentro del e rango de percepci n de la resoluci n espacial de las o o im genes de la c mara es limitada. a a 3) Actuadores. Los actuadores del robot consisten b sicamente en motores que son, al menos en cierta a medida, impredecibles, debido a efectos como el ruido de control, desgaste o fatiga. Algunos actuadores, tales como en los manipuladores industriales, son bastante precisos. Mientras que en otros, como en los robots m viles de bajo costo, puede ser muy inexacta. o 4) Modelos. Los modelos son inherentemente incorrectos. Los modelos son abstracciones del mundo real. Como tal, s lo modelan parcialmente los procesos fsicos funo damentales del robot y su entorno. Errores de modelamiento son una fuente de incertidumbre que ha sido en gran parte ignorado en la rob tica, a pesar del hecho de o que los modelos m s utilizados en los sistemas rob ticos a o son bastante imprecisos. 5) Algoritmos. Los robots son sistemas de tiempo real, lo que limita la cantidad de c lculos que se pueden realizar. a Muchos algoritmos son aproximaciones, con el n de lograr una respuesta oportuna a cuestas de precisi n. o En este paper se desarrollar el modelamiento del robot (II), a el modelamiento de los sensores (III), y la localizaci n para o un modelo basado en velocidades usando el ltro extendido v de Kalman (IV), es decir: u = [ ] , donde u representa a las variables de control que en el caso de la conguraci n o Ackerman son la velocidad (v) y angulo de direcci n principal o (). Cabe resaltar que existe otro m todo basado en odometra e que se basa en la medici n resultante de los encoders la cual o

I. INTRODUCCI ON A rob tica probabilstica es la mejor herramienta para o abordar problemas donde la incertidumbre juega un rol muy importante puesto que no se puede obviar f cilmente a como se suele hacer en la rob tica cl sica, esta diferencia se o a debe a que en sistemas cl sicos su area de trabajo es limitada a y restringida, mientras que en los que veremos a ac son todo a lo contrario. Existen precedentes de la aplicaci n de este tipo o de t cnicas presentando sus m s altos exponentes en DARPA e a Challenge 2005 o con en el vehculo no tripulado de GOOGLE (ver Fig. 1), en todos se pueden notar presencia de t cnicas e probabilstica y el uso de la arquitectura Ackerman.

Fig. 1.

Google driverless car

no ser implementada en esta presentaci n. Finalmente se cona o cluye presentando la simulaci n y resultados (V) obtenidos. o A continuaci n se presenta una descripci n de la nomeno o clatura que se emplear : a Estado del robot: (x, y) Del robot: L b D R (xc , yc ) Estado del sensor: r s Control: v s ==

coordenadas del m vil (CG) o orientaci n global del robot o longitud del robot distancia entre ruedas motrices di metro de las ruedas a radio de curvatura posici n del CIR del robot o distancia del robot a la marca angulo entre el robot y la marca Identicador de la referencia (tag) velocidad lineal del robot angulo de direcci n (steering angle) o

Fig. 3.

a) Conguraci n Ackerman, b) Modelo bicicleta o

II. MODELAMIENTO DEL ROBOT La Arquitectura Ackerman (ver Fig. 2) es una de las m s a usadas actualmente en los vehculos motorizados, la ventaja de este modelo se encuentra en que evita el deslizamiento de las llantas bajo ciertas condiciones. Este dise o cuenta con n tracci n trasera (dos ruedas motrices) y dos ruedas directrices o (gobernadas por una volante).

2) El vehculo se mueve sobre un plano. 3) El tratar a este problema como el de una bicicleta aproxima el modelo del carro razonablemente a bajas velocidades y angulos de direcci n moderados. o 4) Las llantas no presentan deslizamiento lateral. 5) Posee como centro instant neo de rotaci n a la intera o secci n de las rectas perpendiculares a todas las llantas o (ver Fig. 4).

Fig. 4. ruedas

El CIR es generado por la intersecci n de las perpendiculares a las o

Se obtienen las siguientes relaciones geom tricas (ver Fig. e 4 y Fig. 5):
Fig. 2. Conguraci n Ackerman o

Una simplicaci n de este modelo com nmente usado en o u seguimiento de trayectorias es el modelo de bicicleta (ver Fig. 3). Para el modelamiento consideramos las siguientes asunciones: 1) Se reemplazan las cuatro ruedas por solo dos (una delantera directriz y una trasera motriz).

cot (o ) =

R + b/2 L R b/2 L b L

(1)

cot (i ) =

(2)

cot (o ) cot (i ) =

(3)

Eliminando (xf , yf ) de la Eq. 6: 0= d(x + L cos ()) d(y + L sin ()) sin ( + ) cos ( + ) dt dt

= (x L sin () sin ( + )) (y L cos () cos ( + )) = x sin ( + ) y cos ( + ) L sin ()(sin () cos () + cos () sin ()) L cos ()(cos () cos () + sin () sin ()) = x sin ( + ) y cos ( + ) b cot (s ) = L tan (s ) = L R (4) (5) L sin () cos () sin () + L cos () sin () sin () L sin2 () cos () L cos2 () cos ()

Fig. 5.

Geometra b sica del modelo bicicleta a

= x sin ( + ) y cos ( + ) L(sin2 ()+cos2 ()) cos = x sin ( + ) y cos ( + ) L cos (9)

La restricci n no holon mica en la rueda trasera (Eq. 7) se o o satisface con x = cos () y y = sin () y cualquier m ltiplo u escalar del mismo. Esta escalar corresponde a la velocidad longitudinal v, de tal manera que: x = v cos () y = v sin ()
Fig. 6. Modelo cinem tico de la bicicleta a

(10)

Aplicando esto a la restricci n en la rueda delantera (Eq. 9) o se obtiene una soluci n para o x sin ( + ) y cos ( + ) = L cos () = v cos ()(sin () cos () + cos () sin ()) L cos () v sin ()(cos () cos () + sin () sin ()) L cos () = v(cos2 () + sin2 ()) sin () L cos () (11)

Consideramos que la velocidad constante, las ecuaciones de restricci n no holon mica para las ruedas delanteras y traseras o o son (ver Fig. 6): xf sin ( + ) yf cos ( + ) = 0 x sin () y cos () = 0 (6) (7)

(xf , yf ) es la coordenada global de la rueda directriz principal (imaginaria), pudiendo ser expresadas como: xf = x + L cos () yf = y + L sin () (8)

v tan () = L Resumiendo el estado del robot queda denido por:

v cos ()

(12)

x y = v sin ()
v tan () L

Discretizando el sistema mediante euler: xt + T v cos () xt+1 yt+1 = yt + T v sin () t+1 t + T v tan () L Modelando el movimiento real:

(13)

Fig. 8.

Medida real del sensor

1 v2 +2 2 3 v2 +4 2

(14) 2 2 rt i (mj,x x) + (mj,y y) i = a tan 2(mj,y y, mj,x x) t si t sj 2 r 2 + 2 s (17)

Entonces el modelo real del robot esta dado por: x + T v cos () x y = y + T v sin () T v tan () t + L III. MODELAMIENTO DE LOS SENSORES

(15)

Empleamos un mapa de caracterstica basado en landmarks o marcas(referencias) las cuales ser n sensadas mediante sena sores del tipo range bearing. A continuaci n se presenta el o caso ideal (ver Fig. 7):

IV. FILTRO EXTENDIDO DE KALMAN Usar la informaci n de sus sensores para localizar al robot o en su propio entorno es el problema m s fundamental que a presenta un robot m vil con capacidades aut nomas. Puesto o o que la cinem tica del robot y la medida de los sensores son a sistemas no lineales (Eq. 15 y Eq. 17), aproximamos dicho modelos de tal modo que usemos las ecuaciones de los casos ideales mas ruido blanco. El ltro extendido de Kalman es una generalizaci n del lo tro de Kalman para sistemas no lineales y una implementaci n o del ltro de Bayes en el caso de sistemas gaussianos lineales, cuya condici n es que se conozca el belief inicial asumi ndose o e que este es del tipo gaussiano. El n cleo central de la este m todo de localizaci n es u e o la estimaci n del estado usando toda la data disponible. La o creencia del robot (belief) respecto al estado x esta dada por la distribuci n de la Eq. 18, debiendo ser conocido el belief o inicial apriori. belxt = p(xt |z1:t , u1:t ) (18)

Fig. 7.

Medida ideal del sensor

i 2 2 rt (mj,x x) + (mj,y y) i = a tan 2(mj,y y, mj,x x) t si t sj Caso real (ver Fig. 8):

(16)

Mientras el robot no sea retroalimentado por la medici n o de sus sensores, su belief ira disminuyendo, es decir la incertidumbre de su estado aumentar ya que su localizaci n se basa a o solo en el desarrollo de su modelo (predicci n). Si el robot o llega a sensar alguna referencia (landmark) este actualizar su a localizaci n corrigiendo de este modo su estado (ver Fig. 9). o Se presenta a continuaci n el algoritmo que desarrolla el o Filtro extendido de Kalman: Entradas: (t1 , t1 , ut , zt )

Fig. 9. Mientras el robot no sea retroalimentado por la medici n de sus o sensores su belief ira disminuyendo, es decir la incertidumbre de su estado aumentar ya que su localizaci n se basar solo en el desarrollo de su modelo a o a (predicci n). Si el robot llega a sensar alguna referencia este actualizar su o a localizaci n haciendo su debida correcci n. o o

x xt xt = yt = y t xt1 + T v cos () g(ut , xt1 ) = yt1 + T v sin () t1 + T v tan () L t = N (0, R) Hallando la covarianza Rt :

(21)

Predicci n: o t = g(ut , t1 ) t = Gt t1 GT + Rt t

Mt =

2 1 vt + 2 2 t 0

0 2 3 vt + 4 2 t

(22) (23)

Vt =

g(ut , t1 ) ut 0 0 T v sec2 L

T cos Vt = T sin T L tan Entonces:

(24)

Correcci n: o Kt =
T T t Ht (Ht t Ht

Rt = Vt Mt VtT + Qt )
1

(25)

Hallando el jacobiano, producto de la linealizaci n por o series de Taylor: Gt = 1 Gt = 0 0 g(ut , t1 ) xt1 0 1 0 T v sin T v cos 1 (26)

t = t + Kt (zt h(t )) t = (I Kt Ht )t Retorna: t , t Procedemos a calcular las variables usadas en el algoritmo iterativo presentado anteriormente. Cinem tica del robot: a xt = g(ut , xt1 ) + t Medida de los sensores: zt = h(xt ) + t (20) (19)

Correcci n: De la ec. (17) o

rt i zt = i t si t

(27)

2 2 (mj,x x) + (mj,y y) h(xt ) = a tan 2(mj,y y, mj,x x) sj 2 r 2 t = 2 s Su matriz de covarianza ser : a 2 r 0 2 Qt = 0 0 0 Hallando su respectivo jacobiano:

(28)

Donde: t : Vector (nx1) de ruido gaussiano de media cero, y matriz de covarianza Rt t : Vector (kx1) de ruido gaussiano de media cero, y matriz de covarianza Qt Predicci n: o De la ecuaci n 13 o

(29)

0 0 2 s

(30)

Ht =

h(t , m) xt

q = (mj,x ut,x )2 + (mj,y ut,y )2 Ht =

mj,x t,x u q mj,y t,y u q mj,y t,y u q m u j,x q t,x

(31)

1 0

V. SIMULACI ON Y RESULTADOS A. Simulaci n del dise o del robot o n Dado los controles comandados no son los mismos de los que llegan a los motores debidos a los ruidos veremos el caso ideal.

Fig. 11.

El carro negro es el carro sin ruido, el carro rojo es el carro real

par metros de ruido que se multiplican con la direcci n (phi) a o al cuadrado. Para otro caso en el cual los par metros de ruido a son (ver Fig. 12 y Fig. 13): 1 = 1e 1 3 = 1e 1 2 = 1e 5 4 = 1e 5 (33)

Fig. 10. Movimiento del robot sin ruido con pose inicial en (x, y, ) = (0, 0, pi/4)

La Fig. 10 muestra un sistema sin ruido es decir donde la velocidad que llega a los motores son las velocidades que se designaron (velocidad comandada) y que el control sobre las ruedas superiores tambi n son las mismas comparemos el e caso en el que el robot ser muy aproximada al caso real. a En la Fig. 11 el carro negro es el carro sin ruido y el carro rojo es el carro real de acuerdo a la Eq. 14. Presenta los siguientes par metros de ruido: a Par metros de error en la velocidad: a 1 = 1e 3 2 = 1e 2

Par metros de error en la direcci n: a o 3 = 1e 2 4 = 1e 1 (32)

Fig. 12. Con mayor ruido en v que en

Donde el 1 , 3 son los par metros de ruido que se mula tiplican con velocidad lineal al cuadrado y 2 , 4 son los

Para estos par metros donde se puede simular alto ruido a en la velocidad se observa que el robot se desva mucho con

Fig. 13.

Con mayor ruido en v que en

Fig. 14.

Con mayor ruido en que en v

respecto al movimiento ideal veamos que sucede para el caso contrario. 1 = 1e 5 3 = 1e 5 2 = 1e 1 4 = 1e 1 (34)

En este caso donde el mayor ruido est en la direcci n a o 2 , 4 Notamos r pidamente que el robot est m s cerca del caso a a a ideal en comparaci n con el caso anterior, es decir cuando el o ruido es mayor en la velocidad el robot se hace m s ruidoso a que en el caso cuando el ruido sea en la direcci n (ver Fig. o 14 y Fig. 15). Por lo tanto para dise ar un robot m vil se n o tendr que tener en cuenta los ruidos en la velocidad m s que a a en la direcci n. o

B. Simulaci n de la medida de los sensores o Para que el robot conozca su medio es necesario contar con sensores que presenten menos ruido veremos que sucede en el caso real. En la Fig. 16 se puede ver que para todos los casos medidos tanto la distancia como el angulo de la marca respecto al robot es la misma. No ocurriendo as en la mayora de sensores dado a los distintas fuentes de ruido. Simulamos dichos ruidos en la Fig. 17. Para las simulaciones consideraremos los siguientes par metros de ruido en los sensores: a

Fig. 15.

Con mayor ruido en que en v

s = 0.05 C. Simulaci n de la localizaci n del robot o o Consideremos el rango de alcance de los sensores nulo (rmax = 0m), y una control de trayectoria preseteado por la trayectoria azul, de este modo distinguiremos el comportamiento de real de nuestro robot sin correcci n alguna (ver o Fig. 18). Para par metros de ruido mnimos es decir para un robot a no y un sensor de un metro (1m) de rango la covarianza

r = 0.1 = 3/180

Fig. 16.

Medida de los sensores sin ruido

Fig. 18. El robot no realiza correcci n alguna vi ndose representado el o e movimiento real por la trayectoria roja, mientras que la linea azul representa la media ideal, la elipse que representa a la covarianza esta totalmente dispersa debido a la gran incertidumbre en el belief del robot

Fig. 19. Fig. 17. Medida de los sensores con ruido

Robot con poco ruido en v y

aumenta lentamente pero si el robot no hace ninguna medida la varianza aumentara desviando el robot del camino ideal( robot sin ruido) como se observa en la Fig. 19. 1 = 1e 4 3 = 1e 4 2 = 1e 4 4 = 1e 4 (35)

(rmax = 2m) el robot se corrige a pesar de aumentar los ruidos considerablemente (ver Fig. 20). 1 = 1e 1 3 = 1e 1 2 = 1e 1 4 = 1e 1 (36)

Para los siguientes par metros de ruido (ver Fig. 21): a 1 = 1e 3 3 = 1e 3 2 = 1e 2 4 = 1e 3 (37)

Un robot altamente ruidoso conlleva que la covarianza aumente r pidamente indicando a su vez que el robot puede a perderse con mucha facilidad pero con un sensor de rango

R EFERENCES
[1] Sebastian THRUN, Wolfram BURGARD, Dieter FOX Probabilistic Robotics. EARLY DRAFT & 1999-2000 [2] Roland SIEGWART, Illah R. NOURBAKHSH Introduction to Autonomous Mobile Robots. Massachusetts Institute of Technology & 2004 [3] Jarrod M. SNIDER Automatic Steering Methods for Autonomous Automobile Path Tracking. Robotics Institute & Carnegie Mellon University & Pittsburgh, Pennsylvania & 2009 [4] Wikipedia, the free encyclopedia. Ackermann steering geometry. 24Oct-2012.

Fig. 20. rmax = 2m con ruidos altos. La trayectoria azul es el del robot ruidoso se puede observa los exabruptos y lo entrecortes del robot en su trayectoria esto debido a los par metros de ruidos seteados. a

Fig. 21. Robot con par metros medianamente ruidosos, la trayectoria del a robot real (roja) se aproximada a la trayectoria de su media (azul)

VI. CONCLUSIONES

El modelo de una bicicleta se adapta muy bien al la conguraci n Ackerman para velocidades bajas. o Mientras mayor sea la varianza de los sensores y el rango de estos, el robot es mas propenso a perderse. Si el robot se perdiese, al ubicar cualquier marca este recobrara su posici n. o La incertidumbre en la velocidad es mucho m s crtica a que en la direcci n. o