UNIVERSIDAD NACIONAL MAYOR DE SAN MARCOS
UNIVERSIDAD DEL PERÚ, DECANA DE AMÉRICA
FACULTAD DE INGENIERÍA DE SISTEMAS E INFORMÁTICA
ESCUELA PROFESIONAL DE INGENIERÍA DE SOFTWARE
Localización por Monte Carlo
CURSO: Automatización y Control de Software
SECCIÓN: N° 1
ESTUDIANTES :
❖ Huarhua Piñas Edson Sebastian
❖ Rodas Panaifo, Jordan André
❖ Quiroz Ardiles Sergio Daniel
❖ Ascue Orosco, Carlos
DOCENTE:
Túpac Valdivia, Jesus Yván
Lima, 2024 - I
ÍNDICE
1. Introducción........................................................................................................................................4
1.1. Principios Básicos..................................................................................... 4
2. Etapas del algoritmo.......................................................................................................................... 5
2.1 Ambiente y mapa............................................................................................10
2.2 Uso del Sonar.................................................................................................12
3. Objetivos........................................................................................................................................... 13
3.1 Verosimilitud del sonar como actualización de medición............................... 13
A. Contexto General....................................................................................... 13
B. Uso del Sonar............................................................................................ 13
C. Implementación en Lua en CoppeliaSim................................................... 14
D. Actualización de las Partículas Después de la Medición...........................15
3.2 Normalización y Remuestreo......................................................................... 17
A. Normalización de pesos de partículas....................................................... 17
B. Remuestreo de partículas.......................................................................... 18
3.3 Navegacíon por hitos mientras se hace localizacion......................................20
3.3.1 Mecanismo de Navegación por Hitos Preprogramados........................ 20
3.3.2 Resampleo y Estimación de la Localización......................................... 21
3.3.3 Movimiento y Seguimiento de Ruta....................................................... 24
4. Conclusiones..................................................................................................................24
Conclusiones sobre la Navegación por Hitos mientras se realiza Localización
Monte Carlo (MCL)......................................................................................... 25
1. Introducción
La localización de Monte Carlo (MCL) es un algoritmo probabilístico
utilizado en robótica para estimar la posición y orientación (pose) de un robot
móvil en un entorno previamente conocido. Este algoritmo es parte de la
familia de filtros de partículas y se usa ampliamente por su capacidad para
manejar incertidumbres en las mediciones y el movimiento del robot.
.1. Principios Básicos
El algoritmo MCL se fundamenta en un grupo de partículas (hipótesis)
que simbolizan las potenciales localizaciones del robot. Cada partícula posee
un estado, el cual abarca la ubicación (x, y) y la dirección θ, además de un
peso que señala la posibilidad de que dicha partícula represente
adecuadamente la posición real del robot. El algoritmo sigue un ciclo que
consta de cuatro fases clave:
● Predicción del Movimiento (Prediction):
En esta etapa, las partículas se mueven de acuerdo al modelo de
movimiento del robot y los datos de odometría, incluyendo ruido para
representar la incertidumbre intrínseca en el movimiento.
● Actualización de las Mediciones (Update):
Los pesos de las partículas se modifican basándose en las
mediciones realizadas por un sensor, como un sonar. Las partículas
cuyas proyecciones se ajustan con mayor precisión a las mediciones
efectivas adquieren pesos más elevados.
● Normalización (Normalization):
Una vez que se han ajustado los pesos de las partículas, se
normalizan para que la suma total de estos sea uno, lo que produce una
distribución de probabilidad válida.
● Remuestreo (Resampling):
Por último, se elige un nuevo grupo de partículas basándose en
los pesos normalizados. Las partículas de mayor peso son más
propensas a ser seleccionadas, concentrándose de esta manera las
partículas en las zonas más probables del espacio de estados.
2. Etapas del algoritmo
● Predicción del movimiento a partir de Odometría
En este paso, la predicción del movimiento se realiza actualizando las
posiciones de las partículas en función de datos odométricos. Esta fase se
basa en la información de movimiento del robot para proyectar la nueva
posición de las partículas según el modelo de movimiento.
En este paso, la predicción del movimiento se realiza actualizando las
posiciones de las partículas en función de datos odométricos. Esta fase se
basa en la información de movimiento del robot para proyectar la nueva
posición de las partículas según el modelo de movimiento.
● Actualizaciones de las mediciones basadas en el sensor (sonar)
En el algoritmo de localización de Monte Carlo (MCL), la
actualización de las mediciones requiere ajustar el peso de las partículas en
función de la probabilidad de que el sensor observe la medición. El proceso
implica comparar las lecturas del sensor con las expectativas para cada
partícula, dando más peso a aquellas lecturas que predicen con mayor
precisión las observaciones reales. Primero, es necesario definir la función de
probabilidad. Por simplicidad, se supone que el sensor mide la distancia entre
el robot y la pared más cercana.
Segundo, integramos la función update en el ciclo completo del algoritmo
MCL.
● Normalización
La normalización del peso es un proceso indispensable en el Algoritmo de
Posicionamiento de Monte Carlo (MCL). Después de ajustar los pesos de
las partículas según las mediciones del sensor, se deben normalizar para que
la suma de todos los pesos sea igual a 1. Este paso garantiza que las
ponderaciones formen la distribución de probabilidad correcta.
Luego use el lenguaje Lua para implementar la normalización del peso.
Integramos la función normalize_weights en el ciclo completo del MCL.
● Remuestreo (de las partículas)
El remuestreo es la etapa final del algoritmo de localización de Monte
Carlo (MCL). En este paso, se crea un nuevo conjunto de partículas utilizando
los pesos previamente normalizados. Es más probable que se seleccionen
partículas con pesos más altos, lo que concentra las partículas en las regiones
más probables del espacio de estados.
Una forma eficaz de lograr el remuestreo es un algoritmo de "rueda de
remuestreo" que ayuda a seleccionar partículas según su peso.
Integración con la función resample en el ciclo completo del MCL.
2.1 Ambiente y mapa
El entorno en el que opera el robot es un espacio cerrado, cuya
configuración se define en un archivo de escenario especial llamado
Practice5.ttt.
● Configuración de Paredes:La escena contiene un script que lee
automáticamente la configuración del muro desde la interfaz gráfica de
usuario (GUI) y almacena las coordenadas finales en una matriz
llamada muro. Esta matriz es el núcleo del funcionamiento del
algoritmo MCL, ya que las paredes definen el entorno en el que el
robot se mueve y toma medidas.
● Puntos de Referencia (Waypoints): La escena también tiene una matriz
llamada waypoints que contiene las coordenadas de una serie de
waypoints que el robot debe seguir. Estos puntos se distribuyen a lo
largo del recorrido que debe recorrer el robot durante el ejercicio, y en
cada punto se realiza un algoritmo de posicionamiento Monte Carlo.
2.2 Uso del Sonar
El sonar (navegación y alcance sónicos) se utiliza principalmente en la
función calcallLikelihood y su trabajo es calcular la probabilidad.
Mide la probabilidad de que una hipótesis determinada (en este caso, la
posición y orientación del robot) sea correcta en función de las
observaciones (en este contexto, las lecturas del sonar). Una opción
puede estar representada por la siguiente función:
donde:
z es la distancia medida por el sonar.
m es la distancia esperada calculada.
σ es la desviación estándar del sensor.
3. Objetivos
3.1 Verosimilitud del sonar como actualización de medición
En esta sección, se explicará el proceso de cálculo de la verosimilitud
del sonar para la actualización de mediciones dentro del contexto de un filtro
de partículas. Este enfoque es fundamental para mantener la precisión en la
localización del robot, utilizando datos de sensores para ajustar las
estimaciones de las partículas.
A. Contexto General
En un filtro de partículas, se mantiene una serie de hipótesis
(partículas) sobre la posible posición y orientación del robot. Cada partícula
tiene un peso asociado que representa la probabilidad de que esa hipótesis sea
correcta. A medida que el robot se desplaza y toma nuevas mediciones, se
actualizan estos pesos para reflejar qué tan bien concuerda cada partícula con
las mediciones actuales.
B. Uso del Sonar
El sensor de sonar mide distancias a obstáculos, lo que resulta útil para
ajustar las posiciones estimadas de las partículas. La verosimilitud de una
partícula se calcula comparando la distancia medida por el sonar con la
distancia esperada a un obstáculo (o pared) según la hipótesis que representa la
partícula.
C. Implementación en Lua en CoppeliaSim
El script en Lua implementa un filtro de partículas que utiliza un sonar
para actualizar la verosimilitud de las partículas. A continuación, se
presentarán los detalles clave del script, específicamente en la función
calculateLikelihood(), que está relacionada con esta actualización.
Variables y Parámetros:
● x, y, theta: Coordenadas y orientación de la partícula.
● z: Distancia medida por el sonar.
● walls: Lista de paredes en el entorno, donde cada pared está definida
por sus coordenadas de inicio (Ax, Ay) y fin (Bx, By).
● sensorVariance: Varianza del sensor, que se utiliza para calcular la
verosimilitud.
Cálculo de la Distancia Esperada m:
● Para cada pared, se determina la distancia desde la partícula hasta la
pared utilizando una fórmula que se basa en la geometría de la línea.
● La distancia distanceToWall se actualiza únicamente si es la más corta
y positiva, lo que indica que la pared está ubicada frente al sensor.
Cálculo de la Verosimilitud:
● Se emplea una función gaussiana para calcular la verosimilitud,
tomando en cuenta la diferencia entre la distancia medida (z) y la
distancia esperada (m).
● En los casos en que no se detecte una pared (m = infinito), se asume
una verosimilitud de 1.0, aunque este supuesto podría revisarse según
el contexto.
D. Actualización de las Partículas Después de la Medición
A continuación, se detalla el proceso de actualización de las partículas
tras recibir una nueva medición de distancia. Este procedimiento es esencial
para ajustar la distribución de las partículas y mejorar la precisión en la
estimación de la posición del robot.
Cálculo de la Verosimilitud:
● Para cada partícula, se invoca a la función calculateLikelihood() para
obtener la verosimilitud de la medición actual.
● Los pesos de las partículas se actualizan multiplicando el peso anterior
por la verosimilitud calculada.
Normalización y Remuestreo:
● Se procede a normalizar los pesos de las partículas para que su suma
sea igual a 1.
● Se realiza el remuestreo de las partículas, lo que permite enfocar la
distribución en las áreas más probables.
● Se actualiza la visualización de las partículas en CoppeliaSim.
3.2 Normalización y Remuestreo
A. Normalización de pesos de partículas
La función normaliseParticleWeights() se encarga de normalizar los
pesos de las partículas. Primero, calcula la suma de todos los pesos actuales y,
posteriormente, divide cada peso por esa suma. Este proceso garantiza que la
suma de los pesos normalizados sea igual a 1.
B. Remuestreo de partículas
La función resampleParticles() lleva a cabo el remuestreo de
partículas empleando el método de "rueda de ruleta sesgada" (biased roulette
wheel). En primer lugar, se calcula un array de pesos acumulados a partir de
los pesos normalizados. Luego, para cada nueva partícula, se genera un
número aleatorio entre 0 y 1, lo que permite seleccionar una partícula existente
para copiar sus valores (x, y, theta) en función del rango de pesos acumulados
donde se encuentra el número aleatorio. Al finalizar el remuestreo, todos los
pesos de las partículas se reinician a 1 / numberOfParticles.
Estas funciones son utilizadas dentro de
updateParticlesAfterMeasurement(), la cual actualiza los pesos de las
partículas en función de una nueva medición de distancia, normaliza dichos
pesos y luego lleva a cabo el remuestreo.
En lo que respecta a la modificación de la máquina de estados para permitir
que el robot avance en los pasos siguientes, el código ya implementa una
máquina de estados secuencial mediante el uso de la variable stepCounter y
el array stepList. Esta máquina de estados regula el comportamiento del robot,
alternando entre la lectura del siguiente punto de referencia, girar, detenerse,
avanzar y repetir el ciclo. Puedes revisar la sección de la función
sysCall_actuation() donde se implementa esta lógica de máquina de estados.
3.3 Navegacíon por hitos mientras se hace localizacion
3.3.1 Mecanismo de Navegación por Hitos Preprogramados
El robot se desplaza a través de una secuencia de puntos de referencia para
llevar a cabo su navegación. Este mecanismo está implementado en la función
sysCall_actuation y se inicializa en la lista stepList y waypoints dentro de la
función sysCall_init.
La función sysCall_actuation gestiona la lógica necesaria para desplazarse de
un punto de referencia a otro, así como para realizar los movimientos de giro y
avance hacia el siguiente objetivo.
3.3.2 Resampleo y Estimación de la Localización
Tras cada medición, se lleva a cabo una actualización y un re-muestreo
de partículas para optimizar la estimación de la localización. Este
proceso se implementa a través de las funciones
updateParticlesAfterMeasurement, normaliseParticleWeights y
resampleParticles.
updateParticlesAfterMeasurement:
normaliseParticleWeights:
resampleParticles:
3.3.3 Movimiento y Seguimiento de Ruta
El movimiento del robot se gestiona a través de la función sysCall_actuation.
Según el paso actual (stepType), el robot puede girar hacia el siguiente punto
de referencia (turn), avanzar hacia él (forward) o realizar mediciones
(measurement).
4. Conclusiones
● El sonar es crucial en el algoritmo de Localización por Monte Carlo (MCL), ya que
suministra las mediciones necesarias para ajustar los pesos de las partículas. Estas
actualizaciones permiten al robot afinar su estimación de posición utilizando las
lecturas del entorno, lo que mejora notablemente la precisión de la localización.
● En relación con la verosimilitud del sonar como método para actualizar las
mediciones, se evidencia cómo este sensor mejora las estimaciones de la posición del
robot a través de un filtro de partículas. El aspecto fundamental de este proceso
consiste en comparar las mediciones del sonar con las distancias anticipadas según las
hipótesis que representan las partículas, ajustando los pesos de manera acorde. Esto
permite que el robot ajuste de forma continua su estimación de posición mientras se
desplaza por su entorno.
● El movimiento del robot, ya sea en desplazamientos rectos o en rotaciones, es
fundamental para prever el desplazamiento de las partículas. Las funciones de
actualización que se ejecutan después de cada movimiento
(updateParticlesAfterStraightLineMotion y updateParticlesAfterPureRotation )
incluyen ruido gaussiano para simular la incertidumbre de manera realista. Este
enfoque es crucial para mantener una distribución precisa y coherente de las partículas
dentro del algoritmo de Localización por Monte Carlo (MCL).
Conclusiones sobre la Navegación por Hitos mientras se realiza Localización Monte
Carlo (MCL)
Eficacia de la Navegación por Hitos:
● La navegación a través de hitos ofrece un marco organizado que guía al robot
a través de una serie predefinida de puntos de referencia (waypoints), lo que
facilita su desplazamiento tanto en entornos familiares como en aquellos
desconocidos. Al utilizar waypoints, se puede descomponer la tarea de
navegación en etapas más manejables, lo que mejora la capacidad del robot
para alcanzar sus objetivos de forma efectiva.
Integración de MCL con Navegación por Hitos:
● La localización por Monte Carlo (MCL) se combina de manera efectiva con la
navegación por hitos, lo que permite al robot mantener una estimación precisa
de su posición mientras se mueve entre los puntos de referencia. En cada fase
de la navegación (lectura del waypoint, giro, avance y medición), el algoritmo
MCL actualiza la estimación de la posición del robot, corrigiendo de manera
continua las desviaciones acumuladas en los datos de odometría.
