Simultaneous Localization And Mapping
Одновременная Локализация и Картография
🤔 Чтобы построить КАРТУ, нужно знать, ГДЕ я...
🤔 Чтобы узнать, ГДЕ я, нужна КАРТА...
SLAM решает обе задачи одновременно!
Dead Reckoning — «слепое» счисление пути по энкодерам:
Старт После 100 тиков Думаю, что здесь:
● ─────────────────────────────► ●
N тиков × (2πR / тиков_на_оборот)
Дифференциальный привод:
$$ \Delta s_L = \frac{2\pi R \cdot \Delta ticks_L}{N} $$$$ \Delta s_R = \frac{2\pi R \cdot \Delta ticks_R}{N} $$$$ \Delta s = \frac{\Delta s_L + \Delta s_R}{2} $$$$ \Delta\theta = \frac{\Delta s_R - \Delta s_L}{L} $$где $L$ — расстояние между колёсами.
void updateOdometry() {
float delta_s = (delta_left + delta_right) / 2.0;
float delta_theta = (delta_right - delta_left) / wheelbase;
// Обновляем позу
theta += delta_theta;
x += delta_s * cos(theta);
y += delta_s * sin(theta);
}
Реальный путь: ●───────────────────────────●
│
Одометрия: ●─────────────────────────● │ ← ошибка!
↗ │
дрейф │
Через 10 минут: ошибка может быть МЕТРЫ!
Причины:
Разбиваем пространство на ячейки:
┌───┬───┬───┬───┬───┬───┬───┬───┐
│ │ │ ▓ │ ▓ │ ▓ │ │ │ │
├───┼───┼───┼───┼───┼───┼───┼───┤
│ │ │ │ │ ▓ │ │ │ │
├───┼───┼───┼───┼───┼───┼───┼───┤
│ │ │ │ │ ▓ │ │ │ │ ▓ = стена
├───┼───┼───┼───┼───┼───┼───┼───┤
│ │ ● │ │ │ │ │ │ │ ● = робот
├───┼───┼───┼───┼───┼───┼───┼───┤
│ │ │ │ │ │ ▓ │ ▓ │ ▓ │
└───┴───┴───┴───┴───┴───┴───┴───┘
Каждая ячейка хранит вероятность занятости:
| Значение | Интерпретация |
|---|---|
| 0.0 | Точно свободно |
| 0.5 | Неизвестно |
| 1.0 | Точно занято |
Для удобства используют логарифм шансов:
$$ l = \log\frac{p}{1-p} $$Обновление проще — просто сложение:
$$ l_{new} = l_{old} + l_{sensor} $$$$ p = \frac{1}{1 + e^{-l}} $$void updateMap(float robot_x, float robot_y, float robot_theta,
float* ranges, int num_rays) {
for (int i = 0; i < num_rays; i++) {
float angle = robot_theta + angleOfRay(i);
float range = ranges[i];
// Ray casting: помечаем свободные ячейки
for (float r = 0; r < range; r += resolution) {
int cell_x = (robot_x + r * cos(angle)) / resolution;
int cell_y = (robot_y + r * sin(angle)) / resolution;
map[cell_x][cell_y] -= 0.1; // log-odds: свободно
}
// Конечная точка: занято
int hit_x = (robot_x + range * cos(angle)) / resolution;
int hit_y = (robot_y + range * sin(angle)) / resolution;
map[hit_x][hit_y] += 0.5; // log-odds: занято
}
}
Вместо одной позиции — облако частиц (гипотез):
┌─────────────────────────────┐
│ · · ▓▓▓▓▓▓ │
│ · · · ▓ │
│ · ●· · │ ● = истинная позиция
│ · · · │ · = частицы
│ ▓▓▓ │
│ ▓▓▓ │
└─────────────────────────────┘
Каждая частица = (x, y, θ) + вес
1. Prediction (Предсказание):
Двигаем все частицы по одометрии + случайный шум
2. Update (Взвешивание):
Сравниваем ожидаемые показания лидара с реальными
Частицы, которые "видят" похоже — получают больший вес
3. Resample (Перевыборка):
Копируем хорошие частицы, удаляем плохие
struct Particle {
float x, y, theta;
float weight;
};
void particleFilter(Particle* particles, int N,
float delta_x, float delta_theta,
float* sensor_data) {
// 1. Prediction
for (int i = 0; i < N; i++) {
particles[i].x += delta_x * cos(particles[i].theta) + noise();
particles[i].y += delta_x * sin(particles[i].theta) + noise();
particles[i].theta += delta_theta + noise();
}
// 2. Update weights
float sum_weights = 0;
for (int i = 0; i < N; i++) {
float* expected = simulateSensor(particles[i], map);
particles[i].weight = computeLikelihood(expected, sensor_data);
sum_weights += particles[i].weight;
}
// Normalize
for (int i = 0; i < N; i++) {
particles[i].weight /= sum_weights;
}
// 3. Resample (низковариантный)
lowVarianceResample(particles, N);
}
Начало: После движения: После измерения:
· · · · · · · · · ·
· · · · · · · · · · ·
· · · · · ───► · · · · ───► ··● ·
· · · · · · · · · ··
· · · · · · · · ·
(равномерно) (разброс) (схождение!)
| Метод | Описание | Применение |
|---|---|---|
| EKF-SLAM | Расширенный Калман для позы + ориентиров | Маленькие карты |
| Particle SLAM | FastSLAM: частицы + EKF для ориентиров | Средние карты |
| Graph SLAM | Граф поз + оптимизация | Большие карты |
| Visual SLAM | Камеры вместо лидаров | Дроны, AR |
┌─────┐ ┌─────┐ ┌─────┐
│Поза1│──────►│Поза2│──────►│Поза3│
└──┬──┘ └──┬──┘ └──┬──┘
│ │ │
▼ ▼ ▼
┌─────┐ ┌─────┐ ┌─────┐
│Ориен│ │Ориен│ │Ориен│
│ тир │ │ тир │ │ тир │
└─────┘ └─────┘ └─────┘
└──────────────┘
Loop Closure!
(узнали знакомое место)
Loop Closure — ключ к точным картам!
# Python + ROS 2
from nav2_simple_commander import BasicNavigator
from slam_toolbox import SlamToolbox
# Запуск SLAM
ros2 launch slam_toolbox online_async_launch.py
# Сохранение карты
ros2 run nav2_map_server map_saver_cli -f my_map
┌─────────────────────────────────────────────────────┐
│ │
│ ████████████████████ │
│ █ █ │
│ █ Комната 1 █████████████ │
│ █ ░░░░░░░░░░░░█ │
│ █ ● ░ Коридор █ │
│ ████████░░░░░░░░░░░░░░░░░░░░░█ │
│ ░░░░░░░░░░░░░░░░░░░░░░█ │
│ █ █ │
│ █ Комната 2 █ │
│ █ █ │
│ ███████████████████████ │
│ │
└─────────────────────────────────────────────────────┘
░ = свободно, █ = стены, ● = робот
Реализуй простой Particle Filter:
📊 Визуализируй схождение частиц!
Профессиональный фреймворк, который используют в реальных роботах!