Download pdf - Задача 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 робот строит карту пошагово и определяет своеместоположение на этой карте по мере ее увеличения и уточнения.


Основы SLAMЭлементы для SLAM

Большинство алгоритмов SLAM создают карту естественныхэлементов сцены.Лазер/сонар: линейные сегменты, 3D планы, углы и т.д.Машинное зрение: характерные точки элементов,текстурированные поверхности.Элементы должны быть различными (распознаваемые с разныхточек обзора) - сопоставление данных.


Основы SLAMРаспространение неопределенности

SLAM похож на проблему курицы и яйца, но мы можем достичьпрогресса, предположив, что робот - единственное, что движется.Основное предположение: мир статичен.Мы вероятностно оценим (как оценивали состояние робота как вЛМК) элементы карты. В SLAM мы сохраняем и обновляемсовместное распределение состояний и робота, и карты.По мере обнаружения роботом новых элементов, размерностьзадачи совместной оценки увеличивается.


Основы SLAM

Робот начинает движение (нулевая неопределенность)Первое измерение элемента A (неопределенность обусловленамоделью ошибки датчика)


Основы SLAM

По мере движения робота, неопределенность растет вследствииошибки, вносимой одометрией


Основы SLAM

Первые наблюдения роботом B и C. Неопределенность явлетсярезультатом комбинации погрешности измерений инеопределенности местоположения робота.Карта становится коррелированной с оценкой положения робота.Аналогично, если робот обновляет оценку своего местоположения,основанную на наблюдении неточно известного элемента на карте,то результирующая оценка положения робота становитсякоррелированной с оценкой положения элемента карты.


Основы SLAM

Робот возвращается в начало.Неопределенность увеличивается вследствии ошибки, вносимойодометрией


Основы SLAM

Робот производит повторное измерение A.В этом случае наблюдение называется Обнаружение замыканияцикла.Неопределенность сократилась.


Основы SLAM

Робот производит повторное измерение B.Неопределенность C также сократилась.Карта обновляется и неопределенность других наблюдаемыхэлементов и всех предыдущих положений робота такжеуменьшилась.


Основы SLAMТри парадигмы SLAM

Большинство алгоритмов SLAM основывается на трех различныхподходах:

Расширенный фильтр Калмана для SLAM (Extended Kalman Filter,EKF)Частичный фильтр SLAM (Particle Filter SLAM). Такженазывается FAST SLAM.Основанный на графах SLAM (Graph-Based SLAM).


Основы SLAMEKF 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


Основы SLAMEKF SLAM

Если робот производит перемещение и делает измерения, векторсостояния и ковариационная матрица обновляются, используястандартные уравнения для расширенного фильтра Калмана.


Основы SLAMEKF SLAM: Фаза предсказания

Во время фазы предсказания, положение робота обновляется,применением формулы обновления положения c использованиемодометрии⎡⎣ xt

yt𝜃t

⎤⎦ =

⎡⎣ xt−1yt−1𝜃t−1

⎤⎦+

⎡⎣ ΔSr+ΔSl2 cos (𝜃t−1 +

ΔSr−ΔSl2b )

ΔSr+ΔSl2 sin (𝜃t−1 +

ΔSr−ΔSl2b )

ΔSr−ΔSlb

⎤⎦Положение элементов карты, напротив, останется неизменным


Основы SLAMEKF SLAM: Фаза предсказания

Следовательно, мы можем записать модель предсказания EKFSLAM как⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

xtyt𝜃t��0r0· · ·��n−1rn−1

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦=

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

xtyt𝜃t𝛼0r0· · ·𝛼n−1rn−1

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦+

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

ΔSr+ΔSl2 cos (𝜃t−1 +

ΔSr−ΔSl2b )

ΔSr+ΔSl2 sin (𝜃t−1 +

ΔSr−ΔSl2b )

ΔSr−ΔSlb00· · ·00

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦Pt = FyPt−1F

Ty + FuQtF

Tu


Основы SLAMEKF SLAM: Фаза оценки

Модель наблюдения та же самая, что и при EKF в задачелокализации

zi =

[��i

ri

]= h(x)

Оценка происходит в том же порядке, что и при обычном EKF:

yt = yt + Kt(z − h(x))

Pt = Pt − KtGINKTt

гдеGIN = HPHT + R

Kt = PH(GIN)−1


Основы SLAMEKF SLAM: Обсуждение

В самом начале, когда робот производит первые измерения,заполняется ковариационная матрица. Делается предположение,что эти элементы не коррелированы. Это означает, чтонедиагональные элементы нулевые.

P0 =

⎡⎢⎢⎢⎢⎢⎢⎣

Px 0 0 · · · 0 00 Pm0 0 . . . 0 00 0 Pm1 · · · 0 0· · · · · · · · · · · · · · · · · ·0 0 0 · · · Pmn−2 00 0 0 0 · · · Pmn−1

⎤⎥⎥⎥⎥⎥⎥⎦



Тем не менее, когда робот начинает движение и производит новыеизмерения, местоположение робота и элементов начинаюткоррелировать.

Pt = FyPt−1Fty + FuQtF

Tu

Соответственно, ковариационная матрица перестает бытьразреженной

P0 =

⎡⎢⎢⎢⎢⎢⎢⎣

Px Pxm0 Pxm1 · · · Pxmn−2 Pxmn−1

Pxm0 Pm0 Pm0m1 . . . Pm0mn−2 Pm0mn−1

Pxm1 Pm0m1 Pm1 · · · Pm1mn−2 Pm1mn−1

· · · · · · · · · · · · · · · · · ·Pxmn−2 Pm0mn−2 Pm1mn−2 · · · Pmn−2 Pm1mn−1

Pxmn−1 Pm0mn−1 Pm1mn−1 · · · Pmn−2mn−1 Pmn−1

⎤⎥⎥⎥⎥⎥⎥⎦



Существование этой корреляции можно объяснить, есливспомнить, что неопределенность элементов на карте зависит отнеопределенности, связанной с положением робота. Но она такжезависит от неопределенности других элементов, которые былииспользованы для обновления положения робота. Это означает,что, когда наблюдается новый элемент, то это позволяетскорректировать не только оценку положения робота, но искорректировать положение других элементов. Чем большенаблюдений сделано, тем большй рост корреляции междуэлементами, тем лучше решение SLAM.


Основы SLAMETH-ASL: Основанный на элементах SLAM

Угловые элементы выделяются из линийУглы соединяются в схему


Основы SLAMНедостатки EKF SLAM

Очевидно, что вектор состояния в EKF SLAM гораздо больше,чем вектор состояния в EKF при локализации, где обновлялосьтолько положение робота. Это делает EKF SLAM оченьзатратным с точки зрения вычислений.Поскольку в его формулировке, карты в EKF SLAM, какпредполагается, основаны на элементах (т.е. точках, линиях,плоскостях). Про появлении новых элементо в наблюдении, онидобавляются в вектор состояния. Таким образом, шумковариационной матрицы растет квадратично, с размером(3+ 2n)× (3+ 2n). Размер карты, как правило, ограничивается довеличины в 1000 элементов.


Основы SLAMParticle Filter SLAM: FastSLAM

Подход FastSLAM:Задача SLAM решается с использованием частичных фильтров.Частичные фильтры являются математическими моделями,которые представляют распределение вероятности в видедискретного набора частиц, которые занимают пространствосостояний.

Распределение вероятностей (эллипс) в виде набора частиц(красные точки)


Основы SLAMParticle Filter: обновление

На шаге обновления генерируется новое частичное распределение дляданной модели движения и примененного управления.

Для каждой частицы:Сравнивается прогнозируемое значение измерения с реальнымизмерениемЧастицам, для которых прогнозируемые значения соответствуютизмеренным, даются большие веса

Фильтрация повторной выборки:

Повторная выборка частиц, основываясь на весахФильтрация повторной выборки

Назначение каждой частицы веса, в зависимости от того,насколько хорошо его оценка состояния согласуется сизмерениями, и случаным образом берутся частицы изпредыдущего распределения, основываясь на весах. Тем самымсоздается новое распределение.


Основы SLAMParticle Filter: обновление

Апостериорная вероятность определяется использованиемчастичной фильтрации Rao Blackwellized, использующейориентиры. Каждая оценка ориентира представляется 2x2расширенным фильтром Калмана. Каждая частица является«независимой» (во время разложения на множители) от других ииспользует оценку местоположений M ориентиров.

Множество частиц


Основы SLAM

В чистом виде SLAM, с использованем метрик ограничен малымиобластями в связи с:

Плохой вычислительной масштабируемостью вероятностныхфильтров.Рост неопределенности на больших расстояниях от началакоординат карты делает представление неопределенностинеточным.Сопоставление данных сосответствующим элементам оченьсложно при высокой неопределенности.


Основы SLAMОснованный на графах SLAM

Основанный на графах SLAM появился из понимания того, чтозадача SLAM может быть представлена в виде разреженногографа и связей между узлами.Узлы графа являются местоположениями робота и элементамикарты.Связи являются относительными позициями междупоследовательностью местоположений робота (даются входомодометрии u) и относительным местоположением междуположением робота и элементами карты, наблюдаемым из этихместоположений.





Связи не должны рассматриваться как жесткие связи. Связи -подвижные.Основанный на графах SLAM, представляет местоположениеробота и карту окружающей среды в виде узлов эластичной сетки.Решение SLAM может быть найдено вычислением состояния сминимальной энергией этой сетки.





Существует значительное преимущество, основанных на графахметодов SLAM над EKF SLAM. Как мы видели, в EKF SLAMколичество вычислений и памяти, которые требуются дляобновления и сохранения ковариационной матрицы, растетквадратично с числом элементов. И наоборот, в основанном награфах SLAM, время обновления графа постоянно и требуемаяпамять находится в линейной зависимости от количестваэлементов.Однако, окончательная оптимизация графа может требоватьбольших вычислительных затрат, если робот прошел достаточнодлинный путь.


Основы SLAMКрупномасштабные локализация и построение карты

Современные практические решения для построениякрупномасштабных карт следуют метрико-топологическому подходу,который апроксимирует полностью метрический SLAM. Это требуетследующих элементов:

Локальное построение карты с использованием метрик дляоценки траектории и создания локальных карт.Распознавание мест для реализации «замыкания цикла» илиповторной локализации, если робот потерялся.Сопоставление данных сосответствующим элементам оченьсложно при высокой неопределенности.Оптимизация/упрощение карты для оптимизации, когда циклызавершены.


Основы SLAMКрупномасштабные локализация и построение карты


RGB-D SLAMСенсор Kinect





RGB-камера. 640x480 пикселей, цвет 8 бит.Датчик удаленности: ИК-проектор + ИК-камера (CMOS, 640x480пикселей, 11 бит, 2048 уровней чувствительности)Микрофоны для акустической локализации и подавления шумаМикрофонАкселерометрСервопривод


RGB-D SLAMСтруктурированный свет






RGB-D SLAMБиблиотеки

OpenCVhttp://opencv.orgOpenNIhttp://openni.ruOpenSLAMhttp://openslam.orgRoboting Operating System (ROS)http://www.ros.org


RGB-D SLAMROS


RGB-D SLAMROS


Робот


http://robotosha.ru


Благодарю за внимание!
