Уявіть, що ви опинились у незнайомому будинку з зав’язаними очима. Вашим завданням є створення точної карти приміщення, при цьому також визначаючи своє місцезнаходження в ньому в реальному часі. Здається майже неможливим, чи не так? Саме таке завдання вирішують алгоритми SLAM (Simultaneous Localization and Mapping) — однієї з ключових технологій у робототехніці, автономних транспортних засобах, доповненій реальності та багатьох інших сферах.
У цій статті ми детально розглянемо, що таке SLAM, які існують алгоритми для його реалізації, математичну основу технології та практичні приклади впровадження. Ми також дослідимо актуальні напрямки розвитку SLAM та його застосування у реальному світі.
Що таке SLAM?
SLAM (Simultaneous Localization and Mapping) — це обчислювальна проблема, яка полягає в одночасному визначенні положення (локалізації) мобільного агента та побудові карти невідомого середовища цим агентом. Простіше кажучи, SLAM дозволяє роботу або іншому пристрою створювати карту навколишнього середовища і одночасно відстежувати своє положення на цій карті.
Основні компоненти SLAM:
- Сенсори — використовуються для сприйняття навколишнього середовища (лазерні далекоміри, камери, радари, ультразвукові датчики, тощо)
- Фронт-енд — обробка сенсорних даних та виявлення особливостей (features) чи орієнтирів (landmarks)
- Бек-енд — оптимізація та оцінка положення робота та орієнтирів
- Картографування — побудова просторового представлення середовища
Класичний парадокс курки та яйця
SLAM часто називають “парадоксом курки та яйця”, і ось чому:
- Щоб точно локалізувати себе, роботу потрібна точна карта середовища
- Щоб побудувати точну карту, роботу потрібно знати своє точне положення
Вирішення цього парадоксу — основна суть алгоритмів SLAM, які ітеративно уточнюють і карту, і положення.
Основні типи алгоритмів SLAM
Фільтр-орієнтований SLAM
Фільтр-орієнтовані підходи використовують рекурсивні фільтри для оцінки стану системи.
EKF-SLAM (Extended Kalman Filter SLAM)
Один із найперших і найвідоміших алгоритмів SLAM, що базується на розширеному фільтрі Калмана.
# Спрощена ілюстрація EKF-SLAM
def ekf_slam_update(mean, covariance, measurement, landmark_id):
# Оновлення стану за допомогою вимірювання
predicted_measurement = predict_measurement(mean, landmark_id)
innovation = measurement - predicted_measurement
# Обчислення якобіана
H = compute_jacobian(mean, landmark_id)
# Розрахунок коваріації інновації
S = H @ covariance @ H.T + measurement_noise
# Фільтр Калмана
K = covariance @ H.T @ np.linalg.inv(S) # Коефіцієнт підсилення Калмана
# Оновлення стану та коваріації
mean = mean + K @ innovation
covariance = (np.eye(len(mean)) - K @ H) @ covariance
return mean, covariance
Переваги:
- Добре вивчений та зрозумілий
- Ефективний для невеликих середовищ
- Забезпечує оцінку невизначеності
Недоліки:
- Обчислювальна складність O(n²), де n — кількість орієнтирів
- Проблеми з нелінійностями
- Не масштабується для великих карт
Particle Filter SLAM (FastSLAM)
Використовує фільтр частинок для оцінки положення робота і фільтри Калмана для оцінки орієнтирів.
Переваги:
- Краще справляється з нелінійностями
- Може моделювати мультимодальні розподіли
- Складність O(M log N), де M — кількість частинок, N — кількість орієнтирів
Недоліки:
- Проблеми виродження частинок
- Зазвичай вимагає багато частинок для хороших результатів
Graph-SLAM
Graph-SLAM представляє проблему SLAM як граф, де:
- Вершини — положення робота та орієнтири
- Ребра — обмеження між вершинами, що виникають з вимірювань або руху
# Концептуальний код для Graph-SLAM
def optimize_pose_graph(graph):
# Ініціалізація оптимізатора
optimizer = PoseGraphOptimizer()
# Додавання вершин (поз робота та орієнтирів)
for vertex in graph.vertices:
optimizer.add_vertex(vertex.id, vertex.pose)
# Додавання ребер (обмежень)
for edge in graph.edges:
optimizer.add_edge(edge.from_vertex, edge.to_vertex, edge.measurement, edge.information)
# Запуск оптимізації
optimizer.optimize(max_iterations=100)
# Отримання оптимізованих положень
optimized_poses = {}
for vertex in graph.vertices:
optimized_poses[vertex.id] = optimizer.get_vertex_pose(vertex.id)
return optimized_poses
Переваги:
- Значно краще масштабується для великих карт
- Покращена точність за рахунок глобальної оптимізації
- Можливість виправляти попередні оцінки при розпізнаванні петель (loop closure)
Недоліки:
- Більші обчислювальні вимоги для оптимізації
- Складніше реалізувати онлайн-оптимізацію
Візуальний SLAM
MonoSLAM
Використовує монокулярну камеру для SLAM.
Переваги:
- Потребує тільки одну камеру
- Відносно проста реалізація
Недоліки:
- Проблема масштабу (неможливо визначити абсолютний масштаб без додаткової інформації)
- Чутливий до умов освітлення
ORB-SLAM
Один із найуспішніших алгоритмів візуального SLAM, що використовує ORB-дескриптори.
Переваги:
- Працює з монокулярною, стерео та RGB-D камерами
- Висока точність та швидкодія
- Ефективне відстеження ключових точок
Недоліки:
- Потребує достатню кількість текстурних особливостей
- Проблеми в умовах низької освітленості
RGB-D SLAM
Використовує комбінацію RGB-камери та датчика глибини (наприклад, Microsoft Kinect, Intel RealSense).
Переваги:
- Пряме вимірювання глибини
- Можливість побудови щільних тривимірних карт
- Вирішення проблеми масштабу
Недоліки:
- Обмежений діапазон роботи датчиків глибини
- Проблеми з відбиваючими та прозорими поверхнями
LiDAR SLAM
SLAM на основі лазерних далекомірів (LiDAR), які забезпечують високоточні вимірювання відстаней.
Переваги:
- Висока точність вимірювань
- Незалежність від умов освітлення
- Добре працює у відкритих просторах
Недоліки:
- Висока вартість LiDAR-датчиків
- Складнощі з розпізнаванням текстури та кольору
- Проблеми у середовищах з мало чітко визначеними особливостями
Математична основа SLAM
З математичної точки зору, SLAM можна представити як задачу оцінки стану:
- Стан системи: $ X = (x_1, x_2, …, x_T, m_1, m_2, …, m_N) $, де $ x_t $ — положення робота в момент часу $ t $, $ m_i $ — положення орієнтиру $ i $.
- Спостереження: $ Z = (z_1, z_2, …, z_T) $, де $ z_t $ — сукупність спостережень орієнтирів в момент часу $ t $.
- Керуючий сигнал: $ U = (u_1, u_2, …, u_{T-1}) $, де $ u_t $ — керуючий сигнал між моментами $ t $ та $ t+1 $.
Байєсівське формулювання SLAM:
$$ p(X | Z, U) = p(x_1, x_2, …, x_T, m_1, m_2, …, m_N | z_1, z_2, …, z_T, u_1, u_2, …, u_{T-1}) $$
Оцінка максимальної правдоподібності:
$$ \hat{X} = \arg\max_X p(Z | X, U) p(X | U) $$
У Graph-SLAM це формулюється як проблема найменших квадратів:
$$ \hat{X} = \arg\min_X \sum_{i,j} e_{ij}(X)^T \Omega_{ij} e_{ij}(X) $$
де $ e_{ij}(X) $ — помилка між очікуваним та дійсним вимірюванням, а $ \Omega_{ij} $ — інформаційна матриця (обернена коваріація).
Практична реалізація: 2D SLAM з Python та Robot Operating System (ROS)
Давайте розглянемо простий приклад реалізації 2D SLAM з використанням gmapping в ROS. Gmapping — це алгоритм, що базується на фільтрі частинок для побудови сітки зайнятості (occupancy grid map).
Налаштування ROS з Python
#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import OccupancyGrid, Odometry
from geometry_msgs.msg import PoseWithCovarianceStamped
import numpy as np
import matplotlib.pyplot as plt
class SimpleSlam:
def __init__(self):
rospy.init_node('simple_slam_visualizer', anonymous=True)
# Підписка на карту, згенеровану gmapping
rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
# Підписка на дані лазерного сканера
rospy.Subscriber('/scan', LaserScan, self.scan_callback)
# Підписка на одометрію робота
rospy.Subscriber('/odom', Odometry, self.odom_callback)
# Зберігання даних
self.map_data = None
self.current_scan = None
self.robot_pose = None
# Створення вікна для візуалізації
plt.figure(figsize=(10, 8))
plt.ion() # Інтерактивний режим
def map_callback(self, data):
self.map_data = data
self.visualize_map()
def scan_callback(self, data):
self.current_scan = data
def odom_callback(self, data):
self.robot_pose = data.pose.pose
def visualize_map(self):
if self.map_data is None:
return
# Отримання параметрів карти
width = self.map_data.info.width
height = self.map_data.info.height
resolution = self.map_data.info.resolution # метри на піксель
# Перетворення map_data.data в 2D-масив
map_array = np.array(self.map_data.data).reshape((height, width))
# Візуалізація карти
plt.clf()
plt.imshow(map_array, cmap='gray', origin='lower')
plt.title('SLAM Map')
# Додавання положення робота, якщо доступне
if self.robot_pose is not None:
x = (self.robot_pose.position.x - self.map_data.info.origin.position.x) / resolution
y = (self.robot_pose.position.y - self.map_data.info.origin.position.y) / resolution
plt.plot(x, y, 'ro', markersize=10)
plt.draw()
plt.pause(0.01)
if __name__ == '__main__':
slam = SimpleSlam()
try:
rospy.spin()
except KeyboardInterrupt:
print("Завершення роботи")
Цей приклад демонструє базову інтеграцію з ROS для візуалізації SLAM. Для повної реалізації gmapping потрібно налаштувати параметри та запустити відповідні вузли ROS.
Запуск SLAM у ROS
Для запуску gmapping в ROS використовується наступна команда:
roslaunch gmapping slam_gmapping.launch
Потім можна запустити наш Python-скрипт для візуалізації:
python simple_slam_visualizer.py
Створення власної реалізації SLAM
Для розуміння основ SLAM, давайте розглянемо спрощену реалізацію 2D SLAM з використанням фільтра частинок:
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
class SimpleParticleFilterSLAM:
def __init__(self, map_size=(100, 100), num_particles=100):
# Ініціалізація карти (grid map)
self.map = np.zeros(map_size) # 0 = невідомо, 100 = зайнято, -1 = вільно
self.map_size = map_size
self.resolution = 0.05 # 5 см на клітинку
# Ініціалізація частинок (положення x, y та орієнтація θ)
self.num_particles = num_particles
self.particles = np.zeros((num_particles, 3))
self.weights = np.ones(num_particles) / num_particles
# Початкове положення у центрі карти
center_x, center_y = map_size[0] // 2, map_size[1] // 2
for i in range(num_particles):
self.particles[i] = [center_x, center_y, 0] # x, y, θ
def predict(self, motion_command, noise=[0.1, 0.1, 0.05]):
"""
Оновлення позицій частинок на основі команди руху та шуму.
motion_command: [dx, dy, dθ] - зміна позиції та орієнтації
noise: [σx, σy, σθ] - стандартне відхилення шуму
"""
for i in range(self.num_particles):
# Додавання шуму до команди руху
noisy_motion = motion_command + np.random.normal(0, noise, 3)
# Оновлення позиції та орієнтації частинки
self.particles[i, 0] += noisy_motion[0] * np.cos(self.particles[i, 2]) - \
noisy_motion[1] * np.sin(self.particles[i, 2])
self.particles[i, 1] += noisy_motion[0] * np.sin(self.particles[i, 2]) + \
noisy_motion[1] * np.cos(self.particles[i, 2])
self.particles[i, 2] += noisy_motion[2]
# Обмеження положень межами карти
self.particles[i, 0] = np.clip(self.particles[i, 0], 0, self.map_size[0] - 1)
self.particles[i, 1] = np.clip(self.particles[i, 1], 0, self.map_size[1] - 1)
# Нормалізація кута
self.particles[i, 2] = (self.particles[i, 2] + np.pi) % (2 * np.pi) - np.pi
def update_weights(self, sensor_readings, sensor_model_std=1.0):
"""
Оновлення вагів частинок на основі сенсорних даних.
sensor_readings: список [range, bearing] для кожного виявленого орієнтиру
"""
for i in range(self.num_particles):
particle_likelihood = 1.0
for reading in sensor_readings:
measured_range = reading[0]
measured_bearing = reading[1]
# Для кожного виміру, порівнюємо його з очікуваним
# Це спрощена модель - в реальності потрібно порівнювати з картою
expected_range = self._simulate_sensor(self.particles[i], measured_bearing)
# Обчислення ймовірності спостереження за моделлю сенсора
prob = norm.pdf(measured_range - expected_range, 0, sensor_model_std)
particle_likelihood *= prob
self.weights[i] *= particle_likelihood
# Нормалізація вагів
if np.sum(self.weights) > 0:
self.weights /= np.sum(self.weights)
def _simulate_sensor(self, particle, bearing):
"""
Симуляція сенсорного виміру для частинки.
Повертає очікувану дальність до перешкоди у заданому напрямку.
"""
# У реальному SLAM це вимагало б кастингу променя на карті
# Для простоти використовуємо фіксоване значення + шум
return 5.0 + np.random.normal(0, 0.1)
def resample(self):
"""
Ресемплінг частинок відповідно до їх вагів.
"""
indices = np.random.choice(
np.arange(self.num_particles),
size=self.num_particles,
replace=True,
p=self.weights
)
self.particles = self.particles[indices]
self.weights = np.ones(self.num_particles) / self.num_particles
def update_map(self, sensor_readings):
"""
Оновлення карти на основі сенсорних даних та оціненого положення.
"""
# Обчислення найкращого оціненого положення (зважене середнє)
best_pose = np.average(self.particles, axis=0, weights=self.weights)
# Оновлення карти для кожного сенсорного виміру
for reading in sensor_readings:
measured_range = reading[0]
measured_bearing = reading[1]
# Обчислення абсолютного кута
abs_bearing = best_pose[2] + measured_bearing
# Обчислення положення орієнтиру в координатах карти
landmark_x = best_pose[0] + measured_range * np.cos(abs_bearing)
landmark_y = best_pose[1] + measured_range * np.sin(abs_bearing)
# Конвертація в індекси карти
map_x = int(landmark_x)
map_y = int(landmark_y)
# Перевірка, чи точка знаходиться в межах карти
if 0 <= map_x < self.map_size[0] and 0 <= map_y < self.map_size[1]:
# Маркуємо клітинку як зайняту
self.map[map_x, map_y] = 100
# Маркуємо клітинки між роботом і орієнтиром як вільні (променевий кастинг)
self._ray_cast(best_pose[:2], [map_x, map_y], -1)
def _ray_cast(self, start, end, value):
"""
Реалізація алгоритму Брезенхема для променевого кастингу.
Маркує клітинки між start і end значенням value.
"""
x0, y0 = int(start[0]), int(start[1])
x1, y1 = int(end[0]), int(end[1])
dx = abs(x1 - x0)
dy = abs(y1 - y0)
sx = 1 if x0 < x1 else -1
sy = 1 if y0 < y1 else -1
err = dx - dy
while x0 != x1 or y0 != y1:
if 0 <= x0 < self.map_size[0] and 0 <= y0 < self.map_size[1]:
self.map[x0, y0] = value
e2 = 2 * err
if e2 > -dy:
err -= dy
x0 += sx
if e2 < dx:
err += dx
y0 += sy
def get_best_pose(self):
"""
Повертає найкраще оцінене положення робота.
"""
return np.average(self.particles, axis=0, weights=self.weights)
def visualize(self):
"""
Візуалізація поточного стану SLAM.
"""
plt.clf()
# Відображення карти
plt.imshow(self.map.T, cmap='gray', origin='lower')
# Відображення частинок
plt.scatter(self.particles[:, 0], self.particles[:, 1],
color='blue', alpha=0.5, s=1)
# Відображення найкращого положення
best_pose = self.get_best_pose()
plt.scatter(best_pose[0], best_pose[1], color='red', s=50)
# Відображення орієнтації найкращого положення
arrow_length = 5
plt.arrow(best_pose[0], best_pose[1],
arrow_length * np.cos(best_pose[2]),
arrow_length * np.sin(best_pose[2]),
head_width=1, head_length=1, fc='red', ec='red')
plt.title('SLAM Visualization')
plt.draw()
plt.pause(0.01)
Це дуже спрощена реалізація, але вона демонструє основні принципи фільтра частинок для SLAM.
Актуальні дослідження та майбутні напрямки
SLAM залишається активною областю досліджень з багатьма відкритими проблемами:
- Динамічні середовища — більшість алгоритмів SLAM припускають статичність світу, адаптація до рухомих об’єктів залишається складною проблемою.
- Семантичний SLAM — інтеграція семантичної інформації в карти, що дозволяє роботам розуміти не лише геометрію, але й зміст середовища.
- Стійкість до умов — покращення роботи в складних умовах з низькою освітленістю, відбиваючими поверхнями та однорідними текстурами.
- Легковагі реалізації — оптимізація алгоритмів для роботи на обмежених обчислювальних ресурсах, наприклад, дронах чи мобільних пристроях.
- Глибоке навчання в SLAM — використання нейронних мереж для покращення різних аспектів SLAM, від виявлення особливостей до замикання петель.
Застосування SLAM у реальному світі
SLAM знаходить застосування у багатьох галузях:
Робототехніка та автономні системи
- Роботи-пилососи для навігації по приміщеннях
- Промислові автономні транспортні засоби (AGV)
- Доставка останньої милі (кур’єрські роботи)
Транспорт
- Автономні автомобілі для навігації та побудови HD-карт
- Автономні дрони для обстеження територій
- Підводні автономні апарати
Доповнена та віртуальна реальність
- Просторове позиціонування в AR-додатках
- Побудова 3D-моделей приміщень для VR
Інші галузі
- Пошуково-рятувальні операції в невідомих середовищах
- Дослідження печер та підземель
- Картографування зон стихійних лих
Висновки
SLAM є однією з фундаментальних технологій, що забезпечує просторове усвідомлення для автономних систем. Від простих роботів-пилососів до складних автономних автомобілів — здатність одночасно визначати своє положення та будувати карту навколишнього середовища є ключовою для навігації в реальному світі.