Localización de un Robot Móvil con el Filtro Extendido de Kalman Iñaki Navarro Oiza 1 de julio de
1. Introducción El objetivo de este trabajo es ver el comportamiento del Filtro Extendido de Kalman (EKF) para la localización de un robot móvil simulado en el ordenador. El robot dispone para su localización de un laser que le proporciona el ángulo con el que ve una serie de balizas. El número de balizas utilizado en todos los casos es tres, que es suficiente para conocer la posición y el ángulo del robot. La ecuación de la medida del ángulo con el que el robot ve cada baliza, obtenida geometricamente y contrastada en [1], es la siguiente: z i = arctan ( ) tiy y θ v i (1) t ix x donde (t ix, t iy ) son las coordenadas de la baliza i, (x, y, θ) la posición del robot, y v i el ruido de medida que se supone de distribución normal. El movimiento del robot viene dado con dos variables: d, incremento lineal de la posición; y β, incremento angular de la posicìón, que pueden verse en la Figura 1. β(k) d(k) k k-1 Figura 1: Movimiento del Robot Las ecuaciones odométricas que modelan este movimiento, obtenidas de [] son las siguientes: x k = x k 1 + ( d k 1 + w dk 1 ) cos(θ k 1 + β k 1 / + w βk 1 /) () y k = y k 1 + ( d k 1 + w dk 1 ) sin(θ k 1 + β k 1 / + w βk 1 /) () θ k = θ k 1 + β k 1 + w βk 1 () 1
donde w d y w β representan los errores en la medida del incremento lineal y angular de posición, considerados de distribución mormal, y los subindices k y k 1 se refieren a los instantes discretos de tiempo entre los que se produce el incremento de posición. El resto de variables que aparecen coinciden con las nombradas anteriormente.. El Filtro Extendido de Kalman (EKF) El EKF [] sirve para estimar la posisción del robot a partir de las ecuaciónes de la odometría y de las medidas de los sensores. Mediante una etapa de predicción en la que se tiene en cuenta unicamente la odometría, y una etapa de corrección en la que se utilizan también las medidas de los sensores y las distribuciones de los errores, se haya la nueva posición. La ecuación 1 que modela las medidas, repetida para las tres balizas será denominada a partir de ahora de la siguiente manera: Z k = h(x k, v k ) () donde Z k son las medidas de los ángulos a las balizas en el instante k, h es la función de la ecuación 1, X k la posición del robot en el instante k y v k el error en las medidas para el instante k. De la misma manera se define a partir de las ecuaciones, y se obtiene la ecuación del proceso: X k = f(x k 1, u k, w k 1 ) () u k es el vector del proceso, es decir, d y β, y w es el vector de ruido de éste (w d y w β ). Antes de enumerar las ecuaciones que forman el filtro se van a definir una serie de variables y matrices que se utilizan en éste. X k y Z k son los vectores reales del estado y medidas del robot en el instante k respectivamene. ˆX k y Ẑ k son los vectores a priori, en la etapa de predicción sin haber medido, del estado y medidas del robot en el instante k respectivamene. ˆX k es el vector a posteriori del estado del robot en el instante k.
A k es la matriz jacobiana de f respecto de X A k[i,j] = f [i] X [j] ( ˆX k 1, u k, ), que en el caso estudiado toma la siguiente forma: A k = 1 d k sin(ˆθ k 1 + β k /) 1 d k cos(ˆθ k 1 + β k /) 1. W k es la matriz jacobiana de f respecto de w (ruido del proceso) W k[i,j] = f [i] w [j] ( ˆX k 1, u k, ), que en el caso estudiado toma la siguiente forma: cos(ˆθ k 1 + β k /) 1 d k sin(ˆθ k 1 + β k /) W k = 1 sin(ˆθ k 1 + β k /) d k cos(ˆθ k 1 + β k /). 1 H k es la matriz jacobiana de h respecto de X H k[i,j] = h [i] X [j] ( ˆX k, ), que en el caso estudiado toma la siguiente forma: H k = t 1y ŷ k (t 1x ˆx k ) +(t 1y ŷ k ) t 1x ˆx k (t 1x ˆx k ) +(t 1y ŷ k ) 1 t y ŷ k (t x ˆx k ) +(t y ŷ k ) t x ˆx k (t x ˆx k ) +(t y ŷ k ) 1 t y ŷ k (t x ˆx k ) +(t y ŷ k ) t x ˆx k (t x ˆx k ) +(t y ŷ k ) 1. V k es la matriz jacobiana de h respecto de v (ruido de la medida) V k[i,j] = h [i] v [j] ( ˆX k, ), que en el caso estudiado es la matriz identidad: 1 V k = 1. 1
Las equaciones del EKF utilizadadas para cada paso temporal se muestran a continuación, comenzando con las de la etapa de Predicción: ˆX k = f( ˆX k 1, u k, ) () P k = A kp k 1 A T k + W k Q k 1 W T k (8) donde Pk es una estimación a priori de la covarianza del error, y Q k 1 es la matriz de covarianzas de ruido del proceso. Las ecuaciones de la etapa de Corrección son las siguientes: K k = Pk HT k (H k Pk HT k + V k R k Vk T ) 1 (9) ˆX k = ˆX k + K k(zk h( ˆX k, )) (1) P k = (I K k H k )Pk (11) donde K k se denomina ganancia, P k es una estimación a posteriori de la covarianza del error, R k es la matriz de covarianzas de ruido de la medida, e I es la matriz identidad. La ganacia K k tomará valores mayores cuando deba dar más peso a la información de las balizas y menos a la odometría, esto es cuando la información de las balizas tenga menos incertidumbre. Esto se verá con ejemplos en la siguiente sección.. Implementación y Resultados Se ha realizado un programa en Matlab que permite simular diferentes movimientos del robot y sus medidas, a la vez que realiza una estimación de la posición mediante el EKF. Se ha considerado que el movimiento del robot es ideal, es decir la trayectoria que se le manda realizar es la que realmente hace. A la hora de realizar las medidas de la odometría y de ángulo a las balizas se ha introducido un error de distribución normal con varianzas: Ri(rad ), para los ángulos a las balizas; Qd(m ), para el incremento lineal de posición; y Qb(rad ), para el incremento angular de posición. Dichas varianzas han sido variadas para comprobar como afectan al comportamiento del filtro. Cuando la trayectoria era puramente lineal Qb se ha considerado, y cuando era puramente angular se ha hecho Qd =. Se han realizado dos tipos de movimiento: el primero circular, con un radio de dos metros; y el segundo rectangular con lados de tres y dos metros. Se ha considerado que el robot se mueve a una velocidad lineal de. m/s y que la posición se muestrea cada. s., siendo por tanto el intervalo avanzado de 1 cm. por cada paso. La velocidad angular en los giros de 9 o de la
trayectoria rectangular es de π/ rad/s, lo que da un incremeto en cada paso de π/. La velocidad angular en el caso de la trayectoria circular es aquela que permite realizar la circunferencia para el radio dado y la velocidad lineal dada. Las balizas han sido colocadas en las posiciones (, 8), (11, ) y (1, 1), coordenadas expesadas en metros. El punto inicial de movimiento del robot es el (, ) y la orientación π/ radianes para las trayectorias rectangulares y radianes para las circulares. La posición inicial de referencia se le ha dado al filtro con un error de ditribución normal de varianza.1 m para la x y la y, y.1 rad para la orientación. A continuación se presentan los resultados hayados para los dos tipos de trayectorias y diferentes varianzas..1. Trayectoria Rectangular 1 Las varianzas de los errores de la odometría y del ángulo a las balizas utilizados aquí son aquellos suficientemente pequeños para que la trayectoria estimada sea muy similar a la real. Los siguientes apartados, con varianzas diferentes las de éste, serán comparados. La matriz K k tiene como elementos la media en valor absoluto de cada elemento de la matriz K k para todos los instantes k. Qd =,1(1 %) Qb =,1(1 %) R1 = R = R =,1 K k =,8,81,1,9,18,,99,819,9 Las siguientes Figuras,,, muestran los movimientos real y estimado del robot en el plano XY, en X, en Y y en θ... Trayectoria Rectangular En este caso se ha disminuido 1 veces el valor de las varianzas Qd y Qb observandose una disminución de los elementos de la matriz K k. Esto parece lógico ya que se posee una odometría mejor y por tanto la correción deberá ser menor. Aunque no se muestran las gráficas la trayectoria estimada
9 8 Plano. R1=.1, R=.1, R=.1, Qd=.1, Qb=.18 Balizas Y (m) 1 8 1 1 X (m) Figura : Movimiento del robot en el plano. X. R1=.1, R=.1, R=.1, Qd=.1, Qb=.18. X (m)... 8 1 1 1 Figura : Movimiento del robot en el eje x
Y. R1=.1, R=.1, R=.1, Qd=.1, Qb=.18.. Y (m).. 1. 8 1 1 1 Figura : Movimiento del robot en el eje y 8 θ. R1=.1, R=.1, R=.1, Qd=.1, Qb=.18 θ (rad) 1 8 1 1 1 Figura : Movimiento del robot en θ
es algo mejor, cosa de esperar puesto que las medidas de la odometría son mejores. Qd =,1(,1 %) Qb =,1(,1 %) R1 = R = R =,1 K k =,11,888,9,1,81,11,,9,.. Trayectoria Rectangular Ahora se ha disminuido 1 veces el valor de las varianzas R1, R y R respecto al caso Rectangular 1, incrementandose los elementos de la matriz K k, de tal forma que al ser la medida de los sensores de balizas comparativamente mejor que la odometría el factor de corrección es mayor. Al igual que en el caso del Rectángulo la trayectoria estimada es algo mejor que en el primer caso, debido esta vez a la menor incertidumbre en las medidas de los ángulos a las balizas. Qd =,1(1 %) Qb =,1(1 %) R1 = R = R =,1 K k = 1,1 1,1 1, 1,1 1,11,9,81,91,89.. Trayectoria Rectangular En este caso se ha aumentado 1 veces cada una de las varianzas con respecto al primer caso, observandose un deterioro considerable de la estimación de la posición. Además se observa que los valores de la matriz K k son similares a los del primer caso. Esto es lógico puesto que se han aumentado todas las varianzas por igual. Qd =,1(1 %) 8
Qb =,1(1 %) R1 = R = R =,1 K k =,,8,9,,88,,,81,9 Las siguientes Figuras,, 8, 9 muestran los movimientos real y estimado del robot en el plano XY, en X, en Y y en θ, en los que se puede ver que la estimación es bastante peor que para el caso Rectangular 1. 9 8 Plano. R1=.1, R=.1, R=.1, Qd=.1, Qb=.18 Balizas Y (m) 1 8 1 1 X (m) Figura : Movimiento del robot en el plano.. Trayectorias Circulares 1,, y Para estas cuatro trayectorias circulares ocurre lo mismo que para las rectangulares repecto a los valores de K k y de la calidad de la estimación de la trayectoria. Al disminuir los valores de Qd y Qb disminuyen los valores de los elementos de K k, y al disminuir los R1, R y R aumentan los valores de los elementos de K k. Además para ambos casos mejora la estimación de la posición. Los valores utilizados para cada trayectoria y la matriz K k obtenida se muestran a continuación: 9
. X. R1=.1, R=.1, R=.1, Qd=.1, Qb=.18. X (m)... 8 1 1 1 Figura : Movimiento del robot en el eje x Y. R1=.1, R=.1, R=.1, Qd=.1, Qb=.18.. Y (m).. 1. 8 1 1 1 Figura 8: Movimiento del robot en el eje y 1
8 θ. R1=.1, R=.1, R=.1, Qd=.1, Qb=.18 θ (rad) 1 8 1 1 1 Figura 9: Movimiento del robot en θ 1. Circular 1 Qd =,1(1 %) Qb =,1( %) R1 = R = R =,1 K k =. Circular,9,9,1981,,11,8,8,, Qd =,1(,1 %) Qb =,1(, %) R1 = R = R =,1 K k =. Circular Qd =,1(1 %) Qb =,1( %),91,,9,,,1,,8,8 11
R1 = R = R =,1 K k =. Circular Qd =,1(1 %) Qb =,1( %) 1,18 1,1 1,,989 1, 1,81,89,8,8 R1 = R = R =,1 K k =,9,,19,19,,91,9,, A continuación se muestran en las Figuras 1, 11, 1, 1 los movimientos del robot para el caso Circular. Se puede ver como al ser las varianzas bajas, especialmente las Ri, la estimación de la trayectoria es bastante buena. 9 8 Plano. R1=1e, R=1e, R=1e, Qd=.1, Qb=.99 Balizas Y (m) 1 8 1 1 X (m) Figura 1: Movimiento del robot en el plano.. Trayectoria Circular En este último ejemplo en lugar de elegir como valor inicial del EKF una posición cercana a la posición inicial real se elige una lejana. La posición 1
. X. R1=1e, R=1e, R=1e, Qd=.1, Qb=.99.. X (m)... 8 1 1 1 Figura 11: Movimiento del robot en el eje x. Y. R1=1e, R=1e, R=1e, Qd=.1, Qb=.99. Y (m)... 8 1 1 1 Figura 1: Movimiento del robot en el eje y 1
θ. R1=1e, R=1e, R=1e, Qd=.1, Qb=.99 θ (rad) 1 8 1 1 1 Figura 1: Movimiento del robot en θ inicial real es (,, ) y la de inicialización del filtro (8, 8, π). Los valores de las varianzas utilizadas se muestran a continuación: Qd =,1(1 %) Qb =,1( %) R1 = R = R =,1 En las siguientes Figuras 1, 1, 1, 1 se ve como las estimación de la posición termina convergiendo a pesar de que los puntos iniciales están muy alejados.. Conclusiones Se ha desarrollado un simulador para el movimiento de un robot móvil y su localización mediante el Filtro Extendido de Kalman. Se ha visto que el filtro se comporta como se esperaba, dando una mejor estimación cuanto menor es el error de los sensores que miden los ángulos a las balizas y de la odometría. También se ha observado que al disminuir el error odométrico los 1
9 8 Plano. R1=.1, R=.1, R=.1, Qd=.1, Qb=.99 Balizas Y (m) 1 8 1 1 X (m) Figura 1: Movimiento del robot en el plano X. R1=.1, R=.1, R=.1, Qd=.1, Qb=.99 X (m) 1 1 1 1 Figura 1: Movimiento del robot en el eje x 1
1 Y. R1=.1, R=.1, R=.1, Qd=.1, Qb=.99 1 8 Y (m) 1 1 Figura 1: Movimiento del robot en el eje y 1 θ. R1=.1, R=.1, R=.1, Qd=.1, Qb=.99 1 1 θ (rad) 8 1 1 Figura 1: Movimiento del robot en θ 1
valores de la matriz K k disminuyen puesto que debe hacerse menos corrección. Lo contrario ocurre cuando lo que disminuye es el error de medida del ángulo a las balizas. Todo esto coincide con lo esperado. Por último se ha comprobado que aún inicializando el EKF con una posición alejada de la inicial real, éste acaba convergiendo. Referencias [1] H. Hu and D. Gu, Landmark-based Navigation of Mobile Robots in Manufacturing, IEEE International Conference on Emerging Technologies & Factory Automation, UPC, Barcelona Spain, 18-1 October 1999. [] F. Matía, Sensors for Autonomous Robots, Apuntes del Curso de doctorado Robots Inteligentes. [] G. Welch and G. Bishop, An Introduction to the Kalman Filter, Dept. of C.S., University of North Carolina at Chapel Hill, Febrero 1. URL: http://www.cs.unc.edu/ welch/kalman/ 1