Задача одновременной локализации и построения карты (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