Задача одновременной локализации и построения карты. Fast SLAM.¶
astSlam — один из подходов решения задач SLAM (англ. Simultaneous Localization And Mapping). В основе алгоритма лежит так называемый фильтр частиц и применение Байесовской сети. В FastSLAM одна большая карта рассматривается как совокупность локальных подкарт, что позволяет убрать зависимость ориентиров друг от друга и таким образом значительно сократить время пересчета оценки состояния системы[1]. Метод был разработан в 2002 году студентами Карнеги — Меллона и Стэнфордского университетов.
Вот подробное объяснение работы FastSLAM и пример реализации на Python с использованием объектно-ориентированного программирования (ООП):
Основные компоненты FastSLAM¶
- Частицы (Particles): Набор гипотез о возможных состояниях робота (местоположении и ориентации).
- Фильтр Калмана (Kalman Filter): Используется для обновления оценки местоположения и карты для каждой частицы.
- Вес частицы (Particle Weight): Оценивает, насколько хорошо частица соответствует наблюдениям.
Основные шаги алгоритма FastSLAM¶
- Инициализация: Создание начального набора частиц с равномерным весом.
- Предсказание (Prediction): Применение модели движения к каждой частице для получения предсказанного состояния.
- Обновление (Update): Обновление состояния частиц на основе наблюдений с использованием фильтра Калмана.
- Пересэмплинг (Resampling): Выбор новых частиц на основе их весов для устранения частиц с малым весом и размножения частиц с большим весом.
Пример реализации на Python¶
1. Определение классов¶
import numpy as np
class Particle:
def __init__(self, x, y, theta, weight):
self.x = x
self.y = y
self.theta = theta
self.weight = weight
self.landmarks = {} # key: landmark id, value: [mean, covariance]
def move(self, delta_x, delta_y, delta_theta):
self.x += delta_x
self.y += delta_y
self.theta += delta_theta
def update_landmark(self, landmark_id, z, R):
if landmark_id not in self.landmarks:
self.landmarks[landmark_id] = [z, R]
else:
mean, covariance = self.landmarks[landmark_id]
# Фильтр Калмана: прогноз
predicted_mean = mean
predicted_covariance = covariance + R
# Фильтр Калмана: обновление
innovation = z - predicted_mean
innovation_covariance = predicted_covariance + R
kalman_gain = np.dot(predicted_covariance, np.linalg.inv(innovation_covariance))
new_mean = predicted_mean + np.dot(kalman_gain, innovation)
new_covariance = np.dot(np.eye(2) - kalman_gain, predicted_covariance)
self.landmarks[landmark_id] = [new_mean, new_covariance]
class FastSLAM:
def __init__(self, num_particles):
self.particles = [Particle(np.random.uniform(-1, 1),
np.random.uniform(-1, 1),
np.random.uniform(-np.pi, np.pi),
1.0/num_particles) for _ in range(num_particles)]
def predict(self, control):
for particle in self.particles:
delta_x, delta_y, delta_theta = control
particle.move(delta_x, delta_y, delta_theta)
def update(self, observations):
for particle in self.particles:
for obs in observations:
landmark_id, obs_x, obs_y = obs
z = np.array([obs_x, obs_y])
R = np.array([[0.1, 0], [0, 0.1]]) # Допущение: одинаковая ковариация для всех измерений
particle.update_landmark(landmark_id, z, R)
# Обновление веса частицы
mean, covariance = particle.landmarks[landmark_id]
innovation = z - mean
innovation_covariance = covariance + R
weight = np.exp(-0.5 * np.dot(np.dot(innovation.T, np.linalg.inv(innovation_covariance)), innovation))
particle.weight *= weight
# Нормализация весов частиц
total_weight = sum(particle.weight for particle in self.particles)
for particle in self.particles:
particle.weight /= total_weight
def resample(self):
weights = [particle.weight for particle in self.particles]
new_particles_indices = np.random.choice(len(self.particles), size=len(self.particles), p=weights)
self.particles = [self.particles[i] for i in new_particles_indices]
def get_best_particle(self):
best_particle = max(self.particles, key=lambda p: p.weight)
return best_particle
# Пример использования:
num_particles = 100
fast_slam = FastSLAM(num_particles)
# Пример команд управления (delta_x, delta_y, delta_theta)
control = (0.1, 0.0, 0.05)
fast_slam.predict(control)
# Пример наблюдений [(id_landmark, x_obs, y_obs)]
observations = [(1, 2.0, 1.0), (2, -1.0, 3.0)]
fast_slam.update(observations)
# Пересэмплинг
fast_slam.resample()
# Получение наилучшей частицы
best_particle = fast_slam.get_best_particle()
print(f"Best particle: x={best_particle.x}, y={best_particle.y}, theta={best_particle.theta}, weight={best_particle.weight}")
Объяснение примера¶
-
Класс
Particle: Представляет частицу с положением (x, y, theta) и весом. Методmoveобновляет положение частицы на основе модели движения. -
Класс
FastSLAM: Содержит набор частиц и методы для выполнения основных шагов алгоритма.__init__: Инициализация набора частиц с случайным начальным положением.predict: Применяет команду управления к каждой частице.update: Обновляет вес каждой частицы на основе наблюдений.resample: Пересэмплинг частиц на основе их весов.get_best_particle: Возвращает частицу с наибольшим весом.
Этот пример показывает базовую структуру алгоритма FastSLAM. В реальной реализации обновление состояния частиц с использованием фильтра Калмана и работа с наблюдениями будет более сложной и требует точных математических вычислений.
И пример на более сложном Python
Реализация алгоритма FastSLAM на Python с применением объектно-ориентированного подхода и принципов чистого кода (Clean Code) включает несколько ключевых компонентов: класс робота, класс карты, класс частиц и сам алгоритм FastSLAM. Также мы будем использовать декораторы для логгирования и проверки типов данных.
Основные классы и декораторы¶
- Декоратор для логгирования: Этот декоратор будет использоваться для логгирования вызовов методов и их результатов.
- Декоратор для проверки типов: Этот декоратор проверит типы данных входных параметров функций.
- Класс
Particle: Представляет одну частицу в фильтре частиц. - Класс
Map: Хранит карту окружения. - Класс
Robot: Представляет робота, который перемещается и собирает данные. - Класс
FastSLAM: Основной класс, реализующий алгоритм FastSLAM.
Декораторы¶
import logging
from typing import Callable
from functools import wraps
logging.basicConfig(level=logging.INFO)
def log_method_call(func: Callable) -> Callable:
@wraps(func)
def wrapper(*args, **kwargs):
logging.info(f"Calling {func.__name__} with args: {args}, kwargs: {kwargs}")
result = func(*args, **kwargs)
logging.info(f"{func.__name__} returned {result}")
return result
return wrapper
def type_check(func: Callable) -> Callable:
@wraps(func)
def wrapper(*args, **kwargs):
annotations = func.__annotations__
for arg_name, arg_type in annotations.items():
if arg_name in kwargs:
assert isinstance(kwargs[arg_name], arg_type), f"Argument {arg_name} is not of type {arg_type}"
else:
idx = list(func.__code__.co_varnames).index(arg_name)
assert isinstance(args[idx], arg_type), f"Argument {arg_name} is not of type {arg_type}"
return func(*args, **kwargs)
return wrapper
Классы¶
import numpy as np
class Particle:
def __init__(self, pose, weight=1.0):
self.pose = pose # [x, y, theta]
self.weight = weight
self.map = {}
@log_method_call
def update_pose(self, new_pose):
self.pose = new_pose
@log_method_call
def update_weight(self, new_weight):
self.weight = new_weight
@log_method_call
def add_landmark(self, landmark_id, position):
self.map[landmark_id] = position
class Map:
def __init__(self):
self.landmarks = {}
@log_method_call
def add_landmark(self, landmark_id, position):
self.landmarks[landmark_id] = position
@log_method_call
def get_landmark(self, landmark_id):
return self.landmarks.get(landmark_id, None)
class Robot:
def __init__(self, initial_pose):
self.pose = initial_pose # [x, y, theta]
@log_method_call
def move(self, delta_pose):
dx, dy, dtheta = delta_pose
self.pose[0] += dx
self.pose[1] += dy
self.pose[2] += dtheta
@log_method_call
def sense(self):
# This is a stub for sensing landmarks.
# In practice, this would use actual sensor data.
return [(1, [2.0, 3.0]), (2, [1.0, 1.5])]
class FastSLAM:
def __init__(self, num_particles, initial_pose):
self.particles = [Particle(initial_pose) for _ in range(num_particles)]
self.robot = Robot(initial_pose)
@log_method_call
def move_robot(self, delta_pose):
self.robot.move(delta_pose)
for particle in self.particles:
particle.update_pose(self.robot.pose)
@log_method_call
def update(self, measurements):
for particle in self.particles:
for landmark_id, position in measurements:
particle.add_landmark(landmark_id, position)
particle.update_weight(1.0) # Simplified weight update
self.resample_particles()
@log_method_call
def resample_particles(self):
weights = [particle.weight for particle in self.particles]
new_particles = np.random.choice(self.particles, len(self.particles), p=weights/np.sum(weights))
self.particles = [Particle(particle.pose) for particle in new_particles]
# Example usage
initial_pose = [0, 0, 0]
fast_slam = FastSLAM(num_particles=10, initial_pose=initial_pose)
# Simulate movement
fast_slam.move_robot([1, 0, 0.1])
# Simulate sensing
measurements = fast_slam.robot.sense()
fast_slam.update(measurements)
Объяснение¶
- Декораторы:
log_method_call: Логгирует вызовы методов и их результаты.-
type_check: Проверяет типы аргументов функций. -
Классы:
Particle: Представляет частицу с состоянием робота и картой.Map: Представляет карту с методами для добавления и получения ориентиров.Robot: Управляет движением робота и получением данных от сенсоров.-
FastSLAM: Основной класс, управляющий частицами и выполняющий алгоритм SLAM. -
Использование:
- Создаем объект
FastSLAMс начальными параметрами. - Симулируем движение робота и обновляем состояние частиц.
- Симулируем получение данных от сенсоров и обновляем карты частиц.
Этот код представляет упрощенную версию алгоритма FastSLAM, демонстрируя основные принципы его работы с использованием декораторов для логгирования и проверки типов данных.