Tarea 4: Filtro de Kalman Extendido con el robot Khepera€¦ · El Filtro de Kalman Extendido...

EL7009 Robotica Movil

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 2013

1

Indice

1. Introduccion 3

2. Marco teorico 32.1. Filtro de Kalman Extendido . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1.1. Etapa predictiva . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.1.2. Etapa correctiva . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

3. Planteamiento del problema 4

4. Desarrollo del trabajo 44.1. Etapa predictiva . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

4.1.1. Modelo cinematico: Funcion f . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44.1.2. Matriz A . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44.1.3. Matriz Q . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

4.2. Etapa correctiva . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54.2.1. Modelo observacional: Funcion h . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54.2.2. Modelo analıtico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54.2.3. Matriz H . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

4.3. Matriz R . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84.4. Resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

4.4.1. Configuracion de la prueba . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84.4.2. Modelo analıtico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84.4.3. Modelo con h interpolada . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

5. Discusion y analisis de resultados 115.1. Respecto a los resultados obtenidos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115.2. Observaciones del simulador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

6. Conclusiones 12

7. Referencias 12

Facultad de Ciencias Fısicas y MatematicasUniversidad de Chile

EL7009 Robotica MovilPrimavera 2013

1. Introduccion

En el presente trabajo se muestran los resultados obtenidos en la cuarta tarea del curso de RoboticaMovil. En esta oportunidad se trabajara nuevamente con el robot diferencial Khepera, a traves del simu-lador Kiks disponible para MATLAB. En esta tarea se pide resolver el problema de auto-localizacion derobots moviles a traves de la solucion clasica: el Filtro de Kalman. Sin embargo, debido a la no-linealidaddel problema, se trabajo con una version no-lineal del filtro conocida como EKF (Extended KalmanFilter) Este informe pretende mostrar las etapas de desarrollo realizadas para su implementacion, comoası tambien mostrar los principales resultados obtenidos con ella.

2. Marco teorico

Debido a que ya se trabajo con Kiks anteriormente, no se detallaran los aspectos tecnicos relacionados aeste, ni las especificaciones tecnicas del robot Khepera. Sı se expondran los conceptos nuevos introducidoseste trabajo, como el Filtro de Kalman extendido.

2.1. Filtro de Kalman Extendido

El Filtro de Kalman Extendido corresponde a la version no-lineal del conocido Filtro de Kalman.Este considera que la prediccion del estado es una funcion del estado anterior y de la accion ejecutadaf(xk−1, uk), como ası tambien el modelo observacional es una funcion del estado actual, h(x−k ).

Las etapas ejecutadas con el filtro se muestran a continuacion, con las respectivas ecuaciones asociadas.Es importante senalar que estas ecuaciones son puestas en esta seccion de forma referencial, dado que sonparte del marco teorico del problema. Sin embargo, en la siguiente seccion seran explicadas con mayordetalle, indicando como se adaptaron para el caso en cuestion.

2.1.1. Etapa predictiva

Proyeccion del estado

x−k = f(xk−1, uk)

Proyeccion de la covarianza del error

P−k = AkPk−1A

Tk +WkQk−1W

Tk

2.1.2. Etapa correctiva

Calculo de la innovacionvk = zk − h(x−k )

Covarianza de la innovacionMk = HkP

−k H

−k

Matching de observacionesvkM

−1k vk ≤ Th

Ganancia de KalmanKk = P−

k HTk Mk

3



Correccion del estadoxk = x−k +Kkvk

Actualizacion de la covarianza del error

Pk = (I −KkHk)P−k

3. Planteamiento del problema

El problema a resolver consiste en la implementacion de un filtro de Kalman Extendido para lalocalizacion de un robot Khepera, el cual se utiliza a traves del simulador Kiks, disponible para MATLAB.

4. Desarrollo del trabajo

4.1. Etapa predictiva

4.1.1. Modelo cinematico: Funcion f

Para el modelo cinematico del robot Khepera se utilizo una expresion analıtica, dada por la cinematicadirecta de un robot diferencial. La expresion de la funcion f es las siguiente:

f(x, y, θ, vR, vL) =

xr + (vR+vL

2 ) cos(θr + vL−vR2L )

yr + (vR+vL2 ) sin(θr + vL−vR

2L )

θr + vL−vRL

Donde se ha utilizado L = 53[mm], correspondiente a la distancia entre las ruedas del robot Khep-era. Es importante identificar que en la expresion anterior las variables (x, y, θ) corresponden al estadoxk−1, mientras que las entradas (vL, vR) corresponden a la accion uk que se observaron en las ecuacionesanteriores.

4.1.2. Matriz A

La matriz A permite caracterizar como afecta el movimiento efectuado en la covarianza Pk del estadox−k . Esta se define como

A(x, y, θ, vR, vL) = J(f) (1)

donde J(f) denota el jacobiano de la funcion f respecto a (x, y, θ). Al desarrollar el jacobiano analıtica-mente se obtiene:

A(vR, vL) =

1 + (vR+vL

2 ) cos(θr + vL−vR2L ) 0 0

0 yr + (vR+vL2 ) sin(θr + vL−vR

2L ) 0

0 0 1 + vL−vRL

4



4.1.3. Matriz Q

La matriz Q permite caracterizar el error de odometrıa del robot. En clases se vieron dos maneras decaracterizar esta covarianza: Una empırica y otra analıtica. La empırica considera la realizacion de pruebascon un robot real, midiendo la posicion esperada y la obtenida mediante la medicion de la odometrıa,de modo de realizar estadıstica sobre los datos y caracterizar este error. La segunda forma considera laexistencia de un modelo analıtico del error de odometrıa, y se pre y post multiplica por una matriz W(WQW T ), correspondiente al Jacobiano de la funcion f pero respecto a las ordenes dadas al sistema. Estasegunda caracterizacion permite obtener un modelo dinamico del error que permite responder de formadistinta frente a distintas ordenes.

Para motivos de este trabajo, se realizo una mezcla de las dos caracterizaciones: se utilizo una matrizQ constante para estimar el error de odometrıa pero se realizo la multiplicacion anterior, de modo deconsiderar el efecto de las ordenes efectuadas.

4.2. Etapa correctiva

4.2.1. Modelo observacional: Funcion h

La funcion h corresponde al modelo observacional del robot, el cual permite estimar las observacionesque deberıa tener el sensor dado que se conoce la pose del robot estimada en la etapa predictiva x−k . Estacorrespondio a la etapa mas difıcil de determinar en el trabajo, debido a una serie de problemas que seranexplicados en las proximas secciones. Para determinar la funcion h se utilizaron tres metodos distintos,que se presentan a continuacion:

4.2.2. Modelo analıtico

El primer enfoque utilizado correspondio al diseno de un modelo analıtico de las observaciones. Paraello, se utilizo la informacion disponible en el Manual de Usuario del robot Khepera [1] y en la tesis demagıster de Theodor Nilsson [2], quien desarrollo Kiks.

El procedimiento para calcular h se resume en el siguiente algoritmo:

Algorithm 1 Algoritmo para calcular h

Require: Pose, Beacons1: Calcular la posicion del sensor ~xs respecto al sistema de referencia global.2: for cada Beaconi do3: Calcular distancia del sensor al beacon4: Calcular angulo del sensor al beacon θbs5: Transformar distancia en medicion de sensor, R (ver 1).6: Ajustar la medicion de acuerdo al angulo usando la relacion Ro + (1− cos(θbs))|500−R|7: end for8: return Medicion del beacon que se encuentra mas cerca

Este enfoque analıtico, a pesar de ser correcto teoricamente, no permitio obtener buenos resultados,ya que los sensores no quedaban bien caracterizados.

5



Figura 1: Curva del sensor del Khepera

Modelo interpolado

Dado que el modelo analıtico no realizaba una buena caracterizacion del sensor, se analizaron otrasalternativas. La primera de ellas consistio en realizar un muestro de las mediciones realizadas por el sensorpara observar el comportamiento. Para ello se implemento un script en MATLAB, sensor sampling.m,el cual permitio obtener una serie de datos del sensor 8 en una grilla. La configuracion utilizada se observaen la figura 2.

El objetivo de esta prueba era medir la pose (x, y) del robot y la medicion del sensor ss, de modo deobtener una funcion que permitiera obtener la medicion esperada del sensor dada la distancia (dx, dy) queexiste entre el sensor y el beacon:

ss = sensormodel(dx, dy) (2)

Para ello, los datos fueron interpolados mediante la herramienta cftool del toolbox de ajuste de curvasde MATLAB, con el que se obtuvo el siguiente modelo (figura 3):

Este modelo interpolado funciona de manera equivalente a una funcion, por lo que no existieronproblemas para el calculo de la funcion H, que se explica mas adelante. Este modelo permitio obteneruna aproximacion mejor que el modelo analıtico, ya que fue obtenida de una forma mas pragmatica.

Modelo con Redes Neuronales

La ultima alternativa evaluada de utilizar un modelo de Redes Neuronales para realizar el modelodel sensor, de modo analogo a lo realizado con interpolacion. La ventaja de este metodo, es que lared neuronal permitirıa obtener un modelo continuo y suave del sensor. Sin embargo, a pesar de quese realizo un script que permitıa entrenar la red de forma rapida (nn sensor model sampling.m) y seobtenıa una caracterizacion suave del sensor, su desempeno al ser evaulada de forma iterativa era bastantepoco eficiente, por lo que finalmente fue descartada.

6



Figura 2: Prueba realizada para la obtencion 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: Graficos ilustrativos del modelo del sensor obtenido mediante interpolacion. A la izquierda se muestra elgrafico real y a la derecha un modelo que ilustra el esperado. En el plano x-y se muestra la distancia del sensor albeacon, y en z la medicion del sensor.

4.2.3. Matriz H

Para el calculo de la matriz H, definida como el jacobiano de h respecto a las ordenes uk, existıan doscaminos: por un lado, se puede realizar la derivacion analıtica, mientras que la otra alternativa correspondea una aproximacion discreta numerica de la derivada. Por motivos practicos de la forma de implementacionde la funcion h, que ya fue vista en la seccion anterior, se escogio la alternativa numerica, que corresponde

7



a la siguiente ecuacion:

H[ij],k =∂hi

∂x−j,k=hi(x

−j,k + ∆, 0)− hi(x−j,k −∆, 0)

2∆(3)

Esta caracterizacion posee mas errores, pero permitıa obtener una aproximacion de la derivada, yaque modelos como el de interpolacion no son directamente analıticos.

4.3. Matriz R

La matriz R representa los errores observacionales que produce el sensor. Para estimarla, se desarrollo elscript estimate R.m, el cual realiza un procedimiento analogo al utilizado para caracterizar el sensor coninterpolacion de puntos. En este caso, tambien se realizo un desplazamiento del robot a traves de unagrilla, pero se realizaron mediciones con los 8 sensores, y se calcularon las estimaciones de la funcion hinterpolada determinada anteriormente. Una vez realizadas las iteraciones y obtenidas las muestras, secalculo

zerror = zobs− z (4)

Obteniendose una matriz de N × 8, con N el numero de muestras. Sobre esta matriz se aplico lacovarianza, obteniendose la matriz R de 8× 8 esperada.

4.4. Resultados

4.4.1. Configuracion de la prueba

Para la realizacion de pruebas del modelo, se realizo una adaptacion de la demo avoid.m disponibleen Kiks. Esta demo ejecuta una rutina de movimiento aleatorio del robot Khepera, de borde a borde de laarena. Esta prueba permitirıa probar una situacion aleatoria que podrıa resultar bastante representativadel robot funcionando en un entorno real, lo que a fin de cuentas termino produciendo bastantes problemasen la estimacion debido los choques.

4.4.2. Modelo analıtico

En general no se obtuvieron buenos resultados con el modelo analıtico: La prediccion realizada porla odometrıa era como se esperaba, con errores que aumentaban en el tiempo, pero las observaciones notuvieron el impacto esperado. El comportamiento del error con el modelo analıtico se observa en la figura4, mientras que una secuencia de movimiento obtenida con el simulador se muestra en la figura 5.

4.4.3. Modelo con h interpolada

Para el modelo con el Filtro de Kalman con funcion h interpolada tampoco se obtuvieron buenosresultados. Al momento de cumplir el criterio de la innovacion con el umbral Th y considerar las obser-vaciones, se producıa un salto en el estado que no se recuperaba, debido a que la correccion inducidapor 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 funcion h no realizaba buenas predicciones de lasobservaciones a pesar del enfoque con que fue disenada sugerıa lo contrario.

8



Figura 4: Error en las 3 componentes de la pose para el Filtro de Kalman con funcion h analıtica. Se observa queexisten peaks donde el error es mayor, probablemente asociados a momentos en que el robot colisionaba con lasparedes.

Figura 5: Resultados observados en la simulacion con el modelo analıtico. En cyan la localizacion por ground truthy en verde la prediccion del Filtro de Kalman. Claramente se observa que despues de varias iteraciones el error delocalizacion es enorme, y el robot tiene una prediccion completmente erronea.

9



Figura 6: Error en las 3 componentes de la pose para el Filtro de Kalman con funcion h interpolada.Se observaun error enorme debido a los problemas con el modelamiento de h. Debido al cambio de magnitud, el resto de lasvariaciones se hace despreciable.

Figura 7: Resultados observados en la simulacion con el modelo con h interpolada. Se observa que existe un erroren el modelo, ya que cuando se comienzan a percibir observaciones utiles por el criterio de innovacion, existe unsalto en la prediccion que no vuelve a recuperar.

10



5. Discusion y analisis de resultados

5.1. Respecto a los resultados obtenidos

En lo que respecta a los resultados obtenidos, como se aprecio anteriormente no se obtuvieron re-sultados esperados: El filtro de Kalman implementado nunca logro converger correctamente a la posecorrecta, a pesar de que teoricamente los conceptos e ideas propuestas eran correctas. Los errores exis-tentes se produjeron principalmente por la formulacion de la funcion h utilizada, la cual no realizaba unabuena estimacion de las observaciones en ninguno de los dos casos, por lo que tampoco se efectuaron lascorrecciones esperadas.

Respecto a la prediccion odometrica, se observo el comportamiento esperado, con una prediccionbastante cercana a la realidad en las primeras iteraciones, pero con un pequeno error que aumentabaconsiderablemente al recorrer grandes distancias. Eso, sumado al hecho de que las rotaciones influyenen la determinacion de la pose y que las correcciones no funcionaron como deberıan, finalmente siempreacabaron en una prediccion completamente erronea.

Otro fenomeno que se pudo apreciar en el trabajo, y que es posible ver en las figuras anteriores respectaal comportamiento de la odometrıa frente a choques: debido a la naturaleza del choque, la predicion escompletamente erratica, y es la principal culpable de un aumento considerable del error en los resultados.

5.2. Observaciones del simulador

Una de las observaciones principales que respectan a Kiks que fueron observadas durante su uso enesta tarea, respectan al comportamiento del robot y sus sensores. En general se observo que existendos fenomenos bastante curiosos asociados a los dos principales sensores del robot: los sensores de lasruedas, y los sensores de luminosidad. Respecto a los primeros, utilizando la funcion kGetEncoders, lacual entrega la posicion de la rueda en numero de pulsos, se observo en reiteradas ocasiones que susmediciones no se relacionan con el estado actual del robot, y actuan como si existiera un reloj interno delrobot independiente del sistema. Esto podıa apreciarse de forma facil con el siguiente codigo (disponibleen 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 codigo ejecuta una rutina simple de movimiento del robot, produciendo que se mueva en lınearecta. Se espera que el robot realice una pausa de TIME segundos en cada iteracion, antes de continuar conla siguiente. El problema ocurrıa justamente al variar TIME, ya que las mediciones de odometrıa variabande manera proporcional al tiempo que se mantuviera pausado. Esto no hace sentido si se considera quese esta utilizando la misma velocidad en todas las pruebas.

11



Respecto a los sensores de luminosidad, se aprecio que era bastante difıcil caracterizarlos, ya que cadasensor respondıa de forma distinta. Utilizando el mismo modelo observacional para todos, no se obtenıanresultados similares, y esa podrıa ser una de las causas de la falla del segundo modelo propuesto.

6. Conclusiones

El trabajo desarrollado permitio comprender las complejidades que involucra la implementacion deun algoritmo de localizacion como lo es el filtro de Kalman.

Se observo que a pesar de que en su concepto es sencillo e intuitivo, su implementacion puede resultarbastante complicada si no se realiza un correcto modelamiento de las funciones que involucra. Enparticular desarrollar un correcto modelo observacional es vital para el correcto desempeno del filtro.

A pesar de que no se lograron resultados satisfactorios, el trabajo realizado permitio comprenderuna gran cantidad de conceptos relacionados al problema de auto localizacion de robots moviles.

Se espera que la experiencia lograda en el desarrollo de este trabajo, ya sea la relacionada con losalgoritmos de localizacion, como tambien con las soluciones propuestas para solucionar el problemaque existıa en la caracterizacion del modelo, pueda ser utilizada a futuro en otros problemas. Enparticular, se espera que los errores cometidos en el desarrollo de este trabajo puedan ser consider-ados y corregidos en la siguiente tarea de auto localizacion mediante Filtros de Partıculas, de modode ahora si obtener un modelo de estimacion de estado como es esperado.

7. Referencias

[1] K-Team, Khepera User Manual, Laussane, 1999.

[2] T. Nilsson, KiKS is a Khepera Simulator - Master thesis.

[3] J. Ruiz-del-Solar, Filtro de Kalman, Apuntes del curso EL7009 Robotica Movil, 2013.

12

Tarea 4: Filtro de Kalman Extendido con el robot Khepera€¦ · El Filtro de Kalman Extendido...

Documents

Transcript of Tarea 4: Filtro de Kalman Extendido con el robot Khepera€¦ · El Filtro de Kalman Extendido...