Задача одновременной локализации и построения карты (SLAM)

Задача одновременной локализации
и построения карты (SLAM)
Робошкола-2014
Андрей Антонов
robotosha.ru
10 октября 2014 г.
План
1
Основы SLAM
2
RGB-D SLAM
3
Робот
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
2 / 45
Основы SLAM
Понятие SLAM
Simultaneous Localization and Mapping (SLAM)
Один из самых больших успехов в вероятностной робототехнике
Объект с сенсорами движется через заранее неизвестную
статическую окружающую среду, строя карту и вычисляя его
собственное движение.
В каких случаях нам нужен SLAM?
Когда робот должен быть по-настоящему автономным (без
вмешательства человека)
Когда ничего не известно об окружающей среде
Когда мы не можем установить маяки (или даже использовать
GPS - внутри помещения или под водой)
И когда роботу действительно нужно знать, где он находится.
В SLAM робот строит карту пошагово и определяет свое
местоположение на этой карте по мере ее увеличения и уточнения.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
3 / 45
Основы SLAM
Элементы для SLAM
Большинство алгоритмов SLAM создают карту естественных
элементов сцены.
Лазер/сонар: линейные сегменты, 3D планы, углы и т.д.
Машинное зрение: характерные точки элементов,
текстурированные поверхности.
Элементы должны быть различными (распознаваемые с разных
точек обзора) - сопоставление данных.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
4 / 45
Основы SLAM
Распространение неопределенности
SLAM похож на проблему курицы и яйца, но мы можем достичь
прогресса, предположив, что робот - единственное, что движется.
Основное предположение: мир статичен.
Мы вероятностно оценим (как оценивали состояние робота как в
ЛМК) элементы карты. В SLAM мы сохраняем и обновляем
совместное распределение состояний и робота, и карты.
По мере обнаружения роботом новых элементов, размерность
задачи совместной оценки увеличивается.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
5 / 45
Основы SLAM
Робот начинает движение (нулевая неопределенность)
Первое измерение элемента A (неопределенность обусловлена
моделью ошибки датчика)
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
6 / 45
Основы SLAM
По мере движения робота, неопределенность растет вследствии
ошибки, вносимой одометрией
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
7 / 45
Основы SLAM
Первые наблюдения роботом B и C. Неопределенность явлется
результатом комбинации погрешности измерений и
неопределенности местоположения робота.
Карта становится коррелированной с оценкой положения робота.
Аналогично, если робот обновляет оценку своего местоположения,
основанную на наблюдении неточно известного элемента на карте,
то результирующая оценка положения робота становится
коррелированной с оценкой положения элемента карты.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
8 / 45
Основы SLAM
Робот возвращается в начало.
Неопределенность увеличивается вследствии ошибки, вносимой
одометрией
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
9 / 45
Основы SLAM
Робот производит повторное измерение A.
В этом случае наблюдение называется Обнаружение замыкания
цикла.
Неопределенность сократилась.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
10 / 45
Основы SLAM
Робот производит повторное измерение B.
Неопределенность C также сократилась.
Карта обновляется и неопределенность других наблюдаемых
элементов и всех предыдущих положений робота также
уменьшилась.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
11 / 45
Основы SLAM
Три парадигмы SLAM
Большинство алгоритмов SLAM основывается на трех различных
подходах:
Расширенный фильтр Калмана для SLAM (Extended Kalman Filter,
EKF)
Частичный фильтр SLAM (Particle Filter SLAM). Также
называется FAST SLAM.
Основанный на графах SLAM (Graph-Based SLAM).
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
12 / 45
Основы SLAM
EKF SLAM
EKF SLAM аналогичен алгоритму EKF, применяемому при
решении задачи локализации. Отличие в том, что он использует
расширенный вектор состояния yt , который включает и
положение робота xt и положение всех элементов карты mi
yt = [xt , m1, . . . , mn−1 ]T
Если мы обнаруживаем 2D линейные ориентиры, размерность yt
была бы 3 + 2n. 3 переменные для представления местоположения
робота и 2n переменных для n линейных ориентиров, имеющих
вектор компонент (𝛼i , ri )
yt = [xt , yt , 𝜃t , 𝛼0 , r0 , . . . , 𝛼n−1 , rn−1 ]T
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
13 / 45
Основы SLAM
EKF SLAM
Если робот производит перемещение и делает измерения, вектор
состояния и ковариационная матрица обновляются, используя
стандартные уравнения для расширенного фильтра Калмана.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
14 / 45
Основы SLAM
EKF SLAM: Фаза предсказания
Во время фазы предсказания, положение робота обновляется,
применением формулы обновления положения c использованием
одометрии
⎤ ⎡
⎤ ⎡
x̂t
xt−1
⎣ ŷt ⎦ = ⎣ yt−1 ⎦ + ⎣
𝜃t−1
𝜃ˆt
⎡
ΔSr +ΔSl
2
ΔSr +ΔSl
2
−ΔSl ⎤
cos (𝜃t−1 + ΔSr2b
)
ΔSr −ΔSl
sin (𝜃t−1 +
) ⎦
2b
ΔSr −ΔSl
b
Положение элементов карты, напротив, останется неизменным
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
15 / 45
Основы SLAM
EKF SLAM: Фаза предсказания
Следовательно, мы можем записать модель предсказания EKF
SLAM как
⎤
⎤ ⎡ ΔSr +ΔSl
⎤ ⎡
⎡
−ΔSl
cos (𝜃t−1 + ΔSr2b
)
x̂t
xt
2
ΔSr +ΔSl
−ΔSl
⎢ ŷt ⎥ ⎢ yt ⎥ ⎢
sin (𝜃t−1 + ΔSr2b
) ⎥
⎥
2
⎥ ⎢
⎥ ⎢
⎢
⎢
⎥
ΔS
−ΔS
r
l
⎢ 𝜃ˆt ⎥ ⎢ 𝜃t ⎥ ⎢
⎥
⎥ ⎢
⎥ ⎢
⎢
b
⎥
⎥ ⎢ 𝛼0 ⎥ ⎢
⎢ 𝛼
ˆ
⎥
0
0
⎥+⎢
⎢
⎥=⎢
⎥
⎢ rˆ0 ⎥ ⎢ r0 ⎥ ⎢
⎥
0
⎥ ⎢
⎢
⎥ ⎢
⎥
⎢ ··· ⎥ ⎢ ··· ⎥ ⎢
⎥
···
⎥ ⎢
⎢
⎥ ⎢
⎥
⎦
⎣
⎦
⎣ 𝛼
⎣
⎦
𝛼n−1
ˆ n−1
0
rn−1
rˆn−1
0
P̂t = Fy Pt−1 FyT + Fu Qt FuT
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
16 / 45
Основы SLAM
EKF SLAM: Фаза оценки
Модель наблюдения та же самая, что и при EKF в задаче
локализации
[︂
]︂
𝛼
ˆi
ẑi =
= h(x)
rˆi
Оценка происходит в том же порядке, что и при обычном EKF:
yt = ŷt + Kt (z − h(x))
Pt = P̂t − Kt GIN KtT
где
GIN = HPH T + R
Kt = PH(GIN )−1
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
17 / 45
Основы SLAM
EKF SLAM: Обсуждение
В самом начале, когда робот производит первые измерения,
заполняется ковариационная матрица. Делается предположение,
что эти элементы не коррелированы. Это означает, что
недиагональные элементы нулевые.
⎤
⎡
Px
0
0 ···
0
0
⎢ 0 Pm0
0 ...
0
0 ⎥
⎥
⎢
⎢ 0
0 Pm1 · · ·
0
0 ⎥
⎥
⎢
P0 = ⎢
⎥
·
·
·
·
·
·
·
·
·
·
·
·
·
·
·
·
·
·
⎥
⎢
⎣ 0
0
0 · · · Pmn−2
0 ⎦
0
0
0
0
···
Pmn−1
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
18 / 45
Основы SLAM
EKF SLAM: Обсуждение
Тем не менее, когда робот начинает движение и производит новые
измерения, местоположение робота и элементов начинают
коррелировать.
P̂t = Fy Pt−1 Fyt + Fu Qt FuT
Соответственно, ковариационная матрица
разреженной
⎡
Px
Pxm0
Pxm1
···
⎢ Pxm0
P
...
P
m0
m0 m1
⎢
⎢ Pxm1
Pm0 m1
Pm1
···
P0 = ⎢
⎢ ···
·
·
·
·
·
·
···
⎢
⎣ Pxm
P
P
·
··
m0 mn−2
m1 mn−2
n−2
Pxmn−1 Pm0 mn−1 Pm1 mn−1 · · ·
Андрей Антонов (robotosha.ru)
Задача SLAM
перестает быть
Pxmn−2
Pm0 mn−2
Pm1 mn−2
···
Pmn−2
Pmn−2 mn−1
Pxmn−1
Pm0 mn−1
Pm1 mn−1
···
Pm1 mn−1
Pmn−1
10 октября 2014 г.
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
19 / 45
Основы SLAM
EKF SLAM: Обсуждение
Существование этой корреляции можно объяснить, если
вспомнить, что неопределенность элементов на карте зависит от
неопределенности, связанной с положением робота. Но она также
зависит от неопределенности других элементов, которые были
использованы для обновления положения робота. Это означает,
что, когда наблюдается новый элемент, то это позволяет
скорректировать не только оценку положения робота, но и
скорректировать положение других элементов. Чем больше
наблюдений сделано, тем большй рост корреляции между
элементами, тем лучше решение SLAM.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
20 / 45
Основы SLAM
ETH-ASL: Основанный на элементах SLAM
Угловые элементы выделяются из линий
Углы соединяются в схему
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
21 / 45
Основы SLAM
Недостатки EKF SLAM
Очевидно, что вектор состояния в EKF SLAM гораздо больше,
чем вектор состояния в EKF при локализации, где обновлялось
только положение робота. Это делает EKF SLAM очень
затратным с точки зрения вычислений.
Поскольку в его формулировке, карты в EKF SLAM, как
предполагается, основаны на элементах (т.е. точках, линиях,
плоскостях). Про появлении новых элементо в наблюдении, они
добавляются в вектор состояния. Таким образом, шум
ковариационной матрицы растет квадратично, с размером
(3 + 2n) × (3 + 2n). Размер карты, как правило, ограничивается до
величины в 1000 элементов.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
22 / 45
Основы SLAM
Particle Filter SLAM: FastSLAM
Подход FastSLAM:
Задача SLAM решается с использованием частичных фильтров.
Частичные фильтры являются математическими моделями,
которые представляют распределение вероятности в виде
дискретного набора частиц, которые занимают пространство
состояний.
Распределение вероятностей (эллипс) в виде набора частиц
(красные точки)
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
23 / 45
Основы SLAM
Particle Filter: обновление
На шаге обновления генерируется новое частичное распределение для
данной модели движения и примененного управления.
Для каждой частицы:
Сравнивается прогнозируемое значение измерения с реальным
измерением
Частицам, для которых прогнозируемые значения соответствуют
измеренным, даются большие веса
Фильтрация повторной выборки:
Повторная выборка частиц, основываясь на весах
Фильтрация повторной выборки
Назначение каждой частицы веса, в зависимости от того,
насколько хорошо его оценка состояния согласуется с
измерениями, и случаным образом берутся частицы из
предыдущего распределения, основываясь на весах. Тем самым
создается новое распределение.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
24 / 45
Основы SLAM
Particle Filter: обновление
Апостериорная вероятность определяется использованием
частичной фильтрации Rao Blackwellized, использующей
ориентиры. Каждая оценка ориентира представляется 2x2
расширенным фильтром Калмана. Каждая частица является
«независимой» (во время разложения на множители) от других и
использует оценку местоположений M ориентиров.
Множество частиц
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
25 / 45
Основы SLAM
В чистом виде SLAM, с использованем метрик ограничен малыми
областями в связи с:
Плохой вычислительной масштабируемостью вероятностных
фильтров.
Рост неопределенности на больших расстояниях от начала
координат карты делает представление неопределенности
неточным.
Сопоставление данных сосответствующим элементам очень
сложно при высокой неопределенности.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
26 / 45
Основы SLAM
Основанный на графах SLAM
Основанный на графах SLAM появился из понимания того, что
задача SLAM может быть представлена в виде разреженного
графа и связей между узлами.
Узлы графа являются местоположениями робота и элементами
карты.
Связи являются относительными позициями между
последовательностью местоположений робота (даются входом
одометрии u) и относительным местоположением между
положением робота и элементами карты, наблюдаемым из этих
местоположений.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
27 / 45
Основы SLAM
Основанный на графах SLAM
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
28 / 45
Основы SLAM
Основанный на графах SLAM
Связи не должны рассматриваться как жесткие связи. Связи подвижные.
Основанный на графах SLAM, представляет местоположение
робота и карту окружающей среды в виде узлов эластичной сетки.
Решение SLAM может быть найдено вычислением состояния с
минимальной энергией этой сетки.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
29 / 45
Основы SLAM
Основанный на графах SLAM
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
30 / 45
Основы SLAM
Основанный на графах SLAM
Существует значительное преимущество, основанных на графах
методов SLAM над EKF SLAM. Как мы видели, в EKF SLAM
количество вычислений и памяти, которые требуются для
обновления и сохранения ковариационной матрицы, растет
квадратично с числом элементов. И наоборот, в основанном на
графах SLAM, время обновления графа постоянно и требуемая
память находится в линейной зависимости от количества
элементов.
Однако, окончательная оптимизация графа может требовать
больших вычислительных затрат, если робот прошел достаточно
длинный путь.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
31 / 45
Основы SLAM
Крупномасштабные локализация и построение карты
Современные практические решения для построения
крупномасштабных карт следуют метрико-топологическому подходу,
который апроксимирует полностью метрический SLAM. Это требует
следующих элементов:
Локальное построение карты с использованием метрик для
оценки траектории и создания локальных карт.
Распознавание мест для реализации «замыкания цикла» или
повторной локализации, если робот потерялся.
Сопоставление данных сосответствующим элементам очень
сложно при высокой неопределенности.
Оптимизация/упрощение карты для оптимизации, когда циклы
завершены.
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
32 / 45
Основы SLAM
Крупномасштабные локализация и построение карты
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
33 / 45
RGB-D SLAM
Сенсор Kinect
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
34 / 45
RGB-D SLAM
Сенсор Kinect
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
35 / 45
RGB-D SLAM
Сенсор Kinect
RGB-камера. 640x480 пикселей, цвет 8 бит.
Датчик удаленности: ИК-проектор + ИК-камера (CMOS, 640x480
пикселей, 11 бит, 2048 уровней чувствительности)
Микрофоны для акустической локализации и подавления шума
Микрофон
Акселерометр
Сервопривод
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
36 / 45
RGB-D SLAM
Структурированный свет
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
37 / 45
RGB-D SLAM
Структурированный свет
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
38 / 45
RGB-D SLAM
Структурированный свет
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
39 / 45
RGB-D SLAM
Библиотеки
OpenCV
http://opencv.org
OpenNI
http://openni.ru
OpenSLAM
http://openslam.org
Roboting Operating System (ROS)
http://www.ros.org
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
40 / 45
RGB-D SLAM
ROS
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
41 / 45
RGB-D SLAM
ROS
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
42 / 45
Робот
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
43 / 45
http://robotosha.ru
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
44 / 45
Благодарю за внимание!
Андрей Антонов (robotosha.ru)
Задача SLAM
10 октября 2014 г.
45 / 45