Tarea 4: Filtro de Kalman Extendido con el robot Khepera

Transcripción

1 Tarea 4: Filtro de Kalman Extendido con el robot Khepera Matías Mattamala Aravena Prof: Javier Ruiz del Solar Auxiliar: Mauricio Correa Ayudante: Fernando Bernuy 26 de Noviembre de

2 Índice 1. Introducción 3 2. Marco teórico Filtro de Kalman Extendido Etapa predictiva Etapa correctiva Planteamiento del problema 4 4. Desarrollo del trabajo Etapa predictiva Modelo cinemático: Función f Matriz A Matriz Q Etapa correctiva Modelo observacional: Función h Modelo analítico Matriz H Matriz R Resultados Configuración de la prueba Modelo analítico Modelo con h interpolada Discusión y análisis de resultados Respecto a los resultados obtenidos Observaciones del simulador Conclusiones Referencias 12

3 1. Introducción En el presente trabajo se muestran los resultados obtenidos en la cuarta tarea del curso de Robótica Móvil. En esta oportunidad se trabajará nuevamente con el robot diferencial Khepera, a través del simulador Kiks disponible para MATLAB. En esta tarea se pide resolver el problema de auto-localización de robots móviles a través de la solución clásica: el Filtro de Kalman. Sin embargo, debido a la no-linealidad del problema, se trabajó con una versión no-lineal del filtro conocida como EKF (Extended Kalman Filter) Este informe pretende mostrar las etapas de desarrollo realizadas para su implementación, como así también mostrar los principales resultados obtenidos con ella. 2. Marco teórico Debido a que ya se trabajó con Kiks anteriormente, no se detallarán los aspectos técnicos relacionados a éste, ni las especificaciones técnicas del robot Khepera. Sí se expondrán los conceptos nuevos introducidos este trabajo, como el Filtro de Kalman extendido Filtro de Kalman Extendido El Filtro de Kalman Extendido corresponde a la versión no-lineal del conocido Filtro de Kalman. Este considera que la predicción del estado es una función del estado anterior y de la acción ejecutada f(x k 1, u k ), como así también el modelo observacional es una función del estado actual, h(x k ). Las etapas ejecutadas con el filtro se muestran a continuación, con las respectivas ecuaciones asociadas. Es importante señalar que estas ecuaciones son puestas en esta sección de forma referencial, dado que son parte del marco teórico del problema. Sin embargo, en la siguiente sección serán explicadas con mayor detalle, indicando como se adaptaron para el caso en cuestión Etapa predictiva Proyección del estado Proyección de la covarianza del error ˆ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 Etapa correctiva Cálculo de la innovación Covarianza de la innovación Matching de observaciones Ganancia de Kalman v k = z k h(ˆx k ) M k = H k P k H k v k Mk 1 v k T h K k = P k HT k M k 3

4 Corrección del estado Actualización de la covarianza del error ˆx k = ˆx k + K kv k 3. Planteamiento del problema P k = (I K k H k )P k El problema a resolver consiste en la implementación de un filtro de Kalman Extendido para la localización de un robot Khepera, el cual se utiliza a través del simulador Kiks, disponible para MATLAB. 4. Desarrollo del trabajo 4.1. Etapa predictiva Modelo cinemático: Función f Para el modelo cinemático del robot Khepera se utilizó una expresión analítica, dada por la cinemática directa de un robot diferencial. La expresión de la función f es las siguiente: f(x, y, θ, v R, v L ) = x r + ( v R+v L 2 ) cos(θ r + v L v R 2L ) y r + ( v R+v L 2 ) sin(θ r + v L v R 2L ) θ r + v L v R L Donde se ha utilizado L = 53[mm], correspondiente a la distancia entre las ruedas del robot Khepera. Es importante identificar que en la expresión anterior las variables (x, y, θ) corresponden al estado x k 1, mientras que las entradas (v L, v R ) corresponden a la acción u k que se observaron en las ecuaciones anteriores Matriz A La matriz A permite caracterizar como afecta el movimiento efectuado en la covarianza P k del estado x k. Esta se define como A(x, y, θ, v R, v L ) = J(f) (1) donde J(f) denota el jacobiano de la función f respecto a (x, y, θ). Al desarrollar el jacobiano analíticamente se obtiene: 1 + ( v R+v L 2 ) cos(θ r + v L v R 2L ) 0 0 A(v R, v L ) = 0 y r + ( v R+v L 2 ) sin(θ r + v L v R 2L ) v L v R L 4

5 Matriz Q La matriz Q permite caracterizar el error de odometría del robot. En clases se vieron dos maneras de caracterizar esta covarianza: Una empírica y otra analítica. La empírica considera la realización de pruebas con un robot real, midiendo la posición esperada y la obtenida mediante la medición de la odometría, de modo de realizar estadística sobre los datos y caracterizar este error. La segunda forma considera la existencia de un modelo analítico del error de odometría, y se pre y post multiplica por una matriz W (W QW T ), correspondiente al Jacobiano de la funcion f pero respecto a las ordenes dadas al sistema. Esta segunda caracterización permite obtener un modelo dinámico del error que permite responder de forma distinta frente a distintas órdenes. Para motivos de este trabajo, se realizó una mezcla de las dos caracterizaciones: se utilizó una matriz Q constante para estimar el error de odometría pero se realizó la multiplicación anterior, de modo de considerar el efecto de las ordenes efectuadas Etapa correctiva Modelo observacional: Función h La función h corresponde al modelo observacional del robot, el cual permite estimar las observaciones que debería tener el sensor dado que se conoce la pose del robot estimada en la etapa predictiva x k. Esta correspondió a la etapa más difícil de determinar en el trabajo, debido a una serie de problemas que serán explicados en las próximas secciones. Para determinar la función h se utilizaron tres métodos distintos, que se presentan a continuación: Modelo analítico El primer enfoque utilizado correspondió al diseño de un modelo analítico de las observaciones. Para ello, se utilizó la información disponible en el Manual de Usuario del robot Khepera [1] y en la tesis de magíster de Theodor Nilsson [2], quien desarrolló Kiks. El procedimiento para calcular h se resume en el siguiente algoritmo: Algorithm 1 Algoritmo para calcular h Require: Pose, Beacons 1: Calcular la posición del sensor x s respecto al sistema de referencia global. 2: for cada Beacon i do 3: Calcular distancia del sensor al beacon 4: Calcular ángulo del sensor al beacon θ bs 5: Transformar distancia en medición de sensor, R (ver 1). 6: Ajustar la medición de acuerdo al ángulo usando la relación R o + (1 cos(θ bs )) 500 R 7: end for 8: return Medición del beacon que se encuentra más cerca Este enfoque analítico, a pesar de ser correcto teóricamente, no permitió obtener buenos resultados, ya que los sensores no quedaban bien caracterizados. 5

6 Figura 1: Curva del sensor del Khepera Modelo interpolado Dado que el modelo analítico no realizaba una buena caracterización del sensor, se analizaron otras alternativas. La primera de ellas consistió en realizar un muestro de las mediciones realizadas por el sensor para observar el comportamiento. Para ello se implementó un script en MATLAB, sensor sampling.m, el cual permitió obtener una serie de datos del sensor 8 en una grilla. La configuración utilizada se observa en la figura 2. El objetivo de esta prueba era medir la pose (x, y) del robot y la medición del sensor s s, de modo de obtener una función que permitiera obtener la medición esperada del sensor dada la distancia (d x, d y ) que existe entre el sensor y el beacon: s s = sensormodel(d x, d y ) (2) Para ello, los datos fueron interpolados mediante la herramienta cftool del toolbox de ajuste de curvas de MATLAB, con el que se obtuvo el siguiente modelo (figura 3): Este modelo interpolado funciona de manera equivalente a una función, por lo que no existieron problemas para el cálculo de la función H, que se explica más adelante. Este modelo permitió obtener una aproximación mejor que el modelo analítico, ya que fue obtenida de una forma más pragmática. Modelo con Redes Neuronales La ultima alternativa evaluada de utilizar un modelo de Redes Neuronales para realizar el modelo del sensor, de modo análogo a lo realizado con interpolación. La ventaja de este método, es que la red neuronal permitiría obtener un modelo continuo y suave del sensor. Sin embargo, a pesar de que se realizó un script que permitía entrenar la red de forma rápida (nn sensor model sampling.m) y se obtenía una caracterización suave del sensor, su desempeño al ser evaulada de forma iterativa era bastante poco eficiente, por lo que finalmente fue descartada. 6

7 Figura 2: Prueba realizada para la obtención de datos del sensor del Khepera. El beacon se encuentra al centro. Los puntos amarillos denotan las posiciones donde se ubicaba el robot para realizar el muestreo. Figura 3: Gráficos ilustrativos del modelo del sensor obtenido mediante interpolación. A la izquierda se muestra el grafico real y a la derecha un modelo que ilustra el esperado. En el plano x-y se muestra la distancia del sensor al beacon, y en z la medición del sensor Matriz H Para el cálculo de la matriz H, definida como el jacobiano de h respecto a las órdenes u k, existían dos caminos: por un lado, se puede realizar la derivación analítica, mientras que la otra alternativa corresponde a una aproximación discreta numérica de la derivada. Por motivos prácticos de la forma de implementación de la función h, que ya fue vista en la sección anterior, se escogió la alternativa numérica, que corresponde 7

8 a la siguiente ecuación: H [ij],k = h i x j,k = h i(x j,k +, 0) h i(x j,k, 0) 2 Esta caracterización posee más errores, pero permitía obtener una aproximación de la derivada, ya que modelos como el de interpolación no son directamente analíticos Matriz R La matriz R representa los errores observacionales que produce el sensor. Para estimarla, se desarrolló el script estimate R.m, el cual realiza un procedimiento análogo al utilizado para caracterizar el sensor con interpolación de puntos. En este caso, también se realizó un desplazamiento del robot a través de una grilla, pero se realizaron mediciones con los 8 sensores, y se calcularon las estimaciones de la función h interpolada determinada anteriormente. Una vez realizadas las iteraciones y obtenidas las muestras, se calculó z error = z o bs ẑ (4) Obteniéndose una matriz de N 8, con N el número de muestras. Sobre esta matriz se aplicó la covarianza, obteniéndose la matriz R de 8 8 esperada Resultados Configuración de la prueba Para la realización de pruebas del modelo, se realizó una adaptación de la demo avoid.m disponible en Kiks. Esta demo ejecuta una rutina de movimiento aleatorio del robot Khepera, de borde a borde de la arena. Esta prueba permitiría probar una situación aleatoria que podría resultar bastante representativa del robot funcionando en un entorno real, lo que a fin de cuentas terminó produciendo bastantes problemas en la estimación debido los choques Modelo analítico En general no se obtuvieron buenos resultados con el modelo analítico: La predicción realizada por la odometría era como se esperaba, con errores que aumentaban en el tiempo, pero las observaciones no tuvieron el impacto esperado. El comportamiento del error con el modelo analítico se observa en la figura 4, mientras que una secuencia de movimiento obtenida con el simulador se muestra en la figura Modelo con h interpolada Para el modelo con el Filtro de Kalman con función h interpolada tampoco se obtuvieron buenos resultados. Al momento de cumplir el criterio de la innovación con el umbral T h y considerar las observaciones, se producía un salto en el estado que no se recuperaba, debido a que la corrección inducida por las observaciones era del orden de 1000, afectando enormemente la pose estimada (ver figura 6 y 7). Se realizaron pruebas para analizar el algoritmo, y la función h no realizaba buenas predicciones de las observaciones a pesar del enfoque con que fue diseñada sugería lo contrario. (3) 8

9 Figura 4: Error en las 3 componentes de la pose para el Filtro de Kalman con función h analítica. Se observa que existen peaks donde el error es mayor, probablemente asociados a momentos en que el robot colisionaba con las paredes. Figura 5: Resultados observados en la simulación con el modelo analítico. En cyan la localización por ground truth y en verde la predicción del Filtro de Kalman. Claramente se observa que después de varias iteraciones el error de localización es enorme, y el robot tiene una predicción completmente errónea. 9

10 Figura 6: Error en las 3 componentes de la pose para el Filtro de Kalman con función h interpolada.se observa un error enorme debido a los problemas con el modelamiento de h. Debido al cambio de magnitud, el resto de las variaciones se hace despreciable. Figura 7: Resultados observados en la simulación con el modelo con h interpolada. Se observa que existe un error en el modelo, ya que cuando se comienzan a percibir observaciones útiles por el criterio de innovación, existe un salto en la predicción que no vuelve a recuperar. 10

11 5. Discusión y análisis de resultados 5.1. Respecto a los resultados obtenidos En lo que respecta a los resultados obtenidos, como se apreció anteriormente no se obtuvieron resultados esperados: El filtro de Kalman implementado nunca logró converger correctamente a la pose correcta, a pesar de que teóricamente los conceptos e ideas propuestas eran correctas. Los errores existentes se produjeron principalmente por la formulación de la función h utilizada, la cual no realizaba una buena estimación de las observaciones en ninguno de los dos casos, por lo que tampoco se efectuaron las correcciones esperadas. Respecto a la predicción odométrica, se observó el comportamiento esperado, con una predicción bastante cercana a la realidad en las primeras iteraciones, pero con un pequeño error que aumentaba considerablemente al recorrer grandes distancias. Eso, sumado al hecho de que las rotaciones influyen en la determinación de la pose y que las correcciones no funcionaron como deberían, finalmente siempre acabaron en una predicción completamente errónea. Otro fenómeno que se pudo apreciar en el trabajo, y que es posible ver en las figuras anteriores respecta al comportamiento de la odometría frente a choques: debido a la naturaleza del choque, la predición es completamente errática, y es la principal culpable de un aumento considerable del error en los resultados Observaciones del simulador Una de las observaciones principales que respectan a Kiks que fueron observadas durante su uso en esta tarea, respectan al comportamiento del robot y sus sensores. En general se observó que existen dos fenómenos bastante curiosos asociados a los dos principales sensores del robot: los sensores de las ruedas, y los sensores de luminosidad. Respecto a los primeros, utilizando la función kgetencoders, la cual entrega la posición de la rueda en número de pulsos, se observó en reiteradas ocasiones que sus mediciones no se relacionan con el estado actual del robot, y actúan como si existiera un reloj interno del robot independiente del sistema. Esto podía apreciarse de forma fácil con el siguiente código (disponible en el script velocitytest.m): for i=1:100 odo=kgetencoders(ref); kiks_ksend([ D,1,1 10],ref) pause(time); new_odo=kgetencoders(ref); other_odo=kgetencoders(ref); disp(new_odo-odo); disp(other_odo); end Este código ejecuta una rutina simple de movimiento del robot, produciendo que se mueva en línea recta. Se espera que el robot realice una pausa de TIME segundos en cada iteración, antes de continuar con la siguiente. El problema ocurría justamente al variar T IME, ya que las mediciones de odometría variaban de manera proporcional al tiempo que se mantuviera pausado. Esto no hace sentido si se considera que se está utilizando la misma velocidad en todas las pruebas. 11

12 Respecto a los sensores de luminosidad, se apreció que era bastante difícil caracterizarlos, ya que cada sensor respondía de forma distinta. Utilizando el mismo modelo observacional para todos, no se obtenían resultados similares, y esa podría ser una de las causas de la falla del segundo modelo propuesto. 6. Conclusiones El trabajo desarrollado permitió comprender las complejidades que involucra la implementación de un algoritmo de localización como lo es el filtro de Kalman. Se observó que a pesar de que en su concepto es sencillo e intuitivo, su implementación puede resultar bastante complicada si no se realiza un correcto modelamiento de las funciones que involucra. En particular desarrollar un correcto modelo observacional es vital para el correcto desempeño del filtro. A pesar de que no se lograron resultados satisfactorios, el trabajo realizado permitió comprender una gran cantidad de conceptos relacionados al problema de auto localización de robots móviles. Se espera que la experiencia lograda en el desarrollo de este trabajo, ya sea la relacionada con los algoritmos de localización, como también con las soluciones propuestas para solucionar el problema que existía en la caracterización del modelo, pueda ser utilizada a futuro en otros problemas. En particular, se espera que los errores cometidos en el desarrollo de este trabajo puedan ser considerados y corregidos en la siguiente tarea de auto localización mediante Filtros de Partículas, de modo de ahora si obtener un modelo de estimación de estado como es esperado. 7. Referencias [1] K-Team, Khepera User Manual, Laussane, [2] T. Nilsson, KiKS is a Khepera Simulator - Master thesis. [3] J. Ruiz-del-Solar, Filtro de Kalman, Apuntes del curso,