Локализация и составление карты с помощью DP-SLAM

Данная статья основана на оригинальной публикации разработчиков алгоритма DP-SLAM

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

Однако даже при наличии качественного лазерного дальномера задача составления карты нетривиальна: для этого необходимо с высокой точностью определить текущее положение робота, а для того, чтобы определить положение по показаниям дальномера, нужно составить карту. Такую задачу называют задачей одновременной локализации и составлении карты (Simultaneous Localization And Mapping, SLAM). Прямолинейные подходы к ее решению, в которых сначала производится локализация по существующей частично составленной карте, а затем на основе наиболее вероятного текущего положения достраивается недостающая ее часть, имеют тенденцию к составлению карт с ошибками. Более того, эти ошибки накапливаются с течением времени. Движение робота по замкнутому кругу при таком подходе может привести к серьезным проблемам с выравниванием в месте  замыкания цикла.

Одним из первых алгоритмов, способных решать данную задачу в один проход и без дополнительных эвристик для разрешения циклов, был FastSLAM (Montemerlo, Thrun, Koller, & Wegbreit, 2002). Этот алгоритм основывался на идее Мерфи (Murphy, 1999). Он использовал фильтр частиц Рао-Блэквелла для построения гипотез о текущем положении робота и фильтр Калмана для отслеживания положений наперед заданных меток. Данный метод решал проблему составления карты ценою введения меток. Проблема же их распознавания была достаточно сложна,  хотя существовали наработки и по этой тематике (Montemerlo & Thrun, Simultaneous localization and mapping with unknown data association using FastSLAM., 2002).

В данной статье представлен оригинальный алгоритм DP-SLAM, который, так же как и FastSLAM, использует идеи Мерфи. Он тоже использует фильтр частиц, однако он не требует каких-либо предопределенных меток. Проблема ассоциации данных в нем решена путем хранения в памяти множества промежуточных карт. Таким образом, обе части задачи: локализация и составление карты решаются одним и тем же фильтром частиц. Карты хранятся в в памяти в специальном виде, называемом «распределенные частицы» (distributed particle, DP), что позволяет управлять сотнями и тысячами вероятных карт-кандидатов и вероятных позиций одновременно.

Алгоритм DP-SLAM достаточно прост в реализации, поскольку он не использует дополнительных эвристических методов разрешения циклов. Кроме того, он обладает достаточно высокой производительностью.

Фильтры частиц как решение задачи локализации и составления карты.

Фильтр частиц представляет собой моделирующий метод оценки состояния не полностью наблюдаемой системы.

Фильтр частиц хранит взвешенное, нормализованное множество выборки состояний, S = {s1, s2, …, sm}, называемых частицами. На каждом шаге, получив измерение o (или вектор измерений), фильтр частиц:

  1. Создает выборку m новых состояний S′ = {s1′, s2′, …, sm′} из S с заменой.
  2. Совершает переход в новое состояние в Марковской модели позиции робота: P(s″|s′).  Это действие моделирует движение робота в пространстве.
  3. Взвешивает  каждое полученное состояние согласно Марковской модели наблюдений: P(o|s″).
  4. Нормализует веса для нового множества состояний.

Более подробно применении фильтров частиц можно посмотреть в (Doucet, Freitas, & Gordon, 2001) и (Thrun, 2000).

Фильтр частиц в задаче локализации

Фильтр частиц хорошо подходит для решения задачи локализации, где необходимо отслеживать позицию робота, которая является скрытой величиной. Переход между состояниями – это движение робота, а наблюдения – показания лазерного дальномера. Обе эти величины сильно зашумлены.

Изменение внутреннего состояния с течением времени производится посредством моделирования движения внутри робота. Обычно в качестве базиса модели движения  выбирается перемещение, измеренное системой одометрии. Одометрия может очень точно измерять угол поворота колес, однако определить реальные перемещения робота с ее помощью проблематично. Проскальзывания, сдвиги и неровности подстилающей поверхности могут стать причиной ошибок, которые со временем будут только накапливаться. Модели движения для различных роботов на различных типах поверхностей могут серьезно отличаться, но все они будут тем или иным образом будут учитывать систематические и случайные ошибки. Рассмотрим простейшую модель движения. Пусть согласно показаниям одометра текущее положение робота изменилось на x, y, Θ. Фильтр частиц применяет модель ошибки и для каждой созданной частицы i получает:

i = ax * x + bx + N(0, σx)

i = ay * y + by + N(0, σy)

Θi = aΘ * x + bΘ + N(0, σΘ)

Переменные a и b представляют собой систематические ошибки определения положения при движении. Функция N(0, σ) добавляет случайный шум с нормальным распределением вблизи точки 0 и имеющим стандартное отклонение σ. Стандартное отклонение определяется для каждого робота индивидуально экспериментальным путем и может зависеть, в том числе, от порядка величин x, y и Θ.

После моделирования необходимо взвесить все частицы согласно текущим наблюдениям робота. При решении проблемы чистой локализации, робот имеет полную карту в памяти. Позиция, описанная каждой частицей, соответствует некоторой известной точке и направлению на карте. Следовательно, можно сравнительно легко определить, какие показания должен вернуть дальномер, если робот находится в этой позиции. Обычно предполагают, что погрешность показаний дальномера имеет нормальное распределение, и следовательно, если согласно карте первое препятствие должно встретиться на расстоянии d, а показание датчика d', то ошибка δ = d' – d будет распределена нормально вокруг точки 0. Предполагая, что значения позиции робота и показания дальномера можно рассматривать как независимые случайные величины (Murphy, 1999), полный вес частицы i будет иметь вид:

где δik разница между ожидаемым и измеренным расстоянием до препятствия на проходе лазера k и частице (в данном случае положении) i.

Фильтр частиц для одновременной локализации и построения карты

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

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

Алгоритмическая же проблема заключается в том, что необходимо хранить и обрабатывать большое множество частиц, обеспечивая при этом высокую производительность. При этом нужно учитывать, что каждая частица – это не просто позиция робота, но еще и карта. Так как карты не являются «легкими» структурами, хранение сотен, а то и тысяч их представляет значительные трудности. Одним из подходов к решению этой проблемы является допущение, что неопределенность на карте может быть представлена в простой параметрической форме. Это и есть подход, примененный в алгоритме FastSLAM, для которого карта представляет собой фильтр Калмана над множеством меток, расставленных в известных точках пространства. Далее будет показано, что это алгоритмическое ограничение можно обойти путем использования эффективных структур данных.

DP-SLAM

В данном разделе будет рассмотрен алгоритм DP-SLAM, который, в целом, представляет собой простой фильтр частиц над картами и позицией робота. Его особенностью является то, что он использует технику «распределенного проецирования частиц» (Distributed Particle Mapping, DP-Mapping), что позволяет ему оперировать большим количеством карт одновременно.

Наивный SLAM

При использовании наивного подхода, каждая частица соответствует некоторой траектории в пространстве и имеет свою собственную карту. Когда одна частица порождает другую, вся карта целиком рассматривается как часть скрытого состояния и копируется в новую частицу. Предположим, что карта имеет сетку занятости размера M и фильтр работает с P частицами, тогда (без учета стоимости локализации) O(MP) операций будут тратиться только на копирование данных карты. Для количества частиц, достаточного для точной локализации и карты разумного размера, этот подход потребовал бы перемещений гигабайтов данных на каждой итерации.

Распределенное проецирование частиц

Очевидно, что наивный подход вынужден выполнять слишком много работы. Чтобы сделать обсуждение более строгим, введем понятие наследования частиц. Пусть некоторая частица на итерации i выбирается для порождения другой частицы итерации i+1. Будем называть такую частицу родительской, а порожденную частицу дочерней. Две дочерние частицы, принадлежащие одному родителю, будем называть сверстниками. С этого момента, идея наследования частиц развивается сама собой. Предположим, что дальномер сканирует область размером A << M. Рассмотрим двух сверстников, s1 и s2. Каждый сверстник будет соответствовать различным положениям робота и вносить не более A обновлений в карту, которую он наследует от своего родителя. Таким образом, s1 и s2 могут отличаться не более чем на A ячеек карты.

Услышав проблему в таком виде, большинство программистов предложат сохранять только ‘diff’ы между картами, т.е. список изменений, которые частица вносит в карту своего родителя. Хотя такой подход и решил бы проблему эффективного обновления карты, он создал бы серьезные вычислительные сложности для локализации: трассировка линии на карте текущей частицы потребовала бы обхода всей родословной и просмотра каждого списка изменений, встречающегося на пути. Сложность такой операции была бы линейной по отношению к количеству итераций фильтра, т.е. ко времени работы алгоритма. Следовательно, наша задача – найти такую структуру данных, которая реализовывала бы как эффективные обновления, так и быстрое выполнение запросов системы локализации с вычислительной сложностью, не зависящей от числа итераций, совершенных фильтром. Решение данной задачи получило название «распределенное проецирование частиц» (DP-Mapping). Оно реализовано посредством двух структур: дерева наследования и самой карты.

Управление деревом наследования частиц

Идея реализации дерева наследования частиц весьма проста. Корнем дерева является начальная частица, из которой происходят все остальные. Каждая частица содержит указатель на своего родителя, имеет уникальный идентификатор (ID) и хранит список ячеек карты, которые она обновила.

Подробности о том, каким образом мы будем использовать дерево наследования для локализации, будут рассмотрены последующих разделах. В этой главе, мы сконцентрируем свое внимание на том, каким образом происходит управление этим деревом и, в частности, почему дерево имеет ограниченный размер вне зависимости от количества итераций.

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

Однако такое дерево все еще будет иметь больше информации, чем нам необходимо. Если некоторая частица является единственным наследником своего родителя, мы вполне можем ее удалить, объединив при этом содержимое ее карты и данные о положении с соответствующими данными родителя. Полученная частица будет вносить в карту точно такой же вклад, как и две исходные частицы. Применяя этот алгоритм ко всему дереву, мы получим минимальное дерево наследования, которое обладает следующими признаками:

Утверждение
Независимо от количества итераций фильтра частиц, минимальное дерево наследования для P частиц

  1. имеет точно P листьев
  2. имеет показатель ветвления не менее 2
  3. имеет глубину не более P

Реализация структуры для карты

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

В DP-SLAM реализован иной подход. В нем не карты ассоциируются с частицами, а частицы с картами. DP-mapping хранит только одну копию сетки занятости (карты). В отличие от классических подходов, каждая ячейка этой сетки содержит сбалансированное дерево. Каждый узел такого дерева проиндексирован идентификатором частицы, которая вносит изменение в ячейку занятости.

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

Теперь мы можем описать объединение предка с единственным потомком более подробно: вначале множество измененных потомком ячеек объединяется с множеством его родителя. Затем для каждой ячейки, измененной потомком, хранящийся в сбалансированном дереве идентификатор меняется на родительский (если родитель и потомок – оба изменили одну и ту же ячейку, то узел родителя приобретает изменения потомка, и последний удаляется). После этого частица-потомок удаляется из дерева наследования, и родительские внуки становятся его прямыми наследниками. Стоит заметить, что это обеспечивает ограниченность сверху количества элементов дерева каждой ячейки, оно составляет O(P).

Вычислительная сложность

Преимуществом наивного подхода хранения полной карты для каждой частицы была его простота: если не учитывать цену копирования блоков карты, то поиск на карте и ее обновление могли бы производиться за постоянное время. Без учета копирования распределенное проецирование частиц несколько проигрывает в эффективности. Однако можно показать, что в реальности DP карты асимптотически превосходят наивный подход.

Поиск на DP-карте выполняет сравнения между предками текущей частицы и узлами деревьев в ячейках карты. Пусть D – глубина дерева наследования, а следовательно, и максимально возможная длина родословной частицы. Строго говоря, дерево наследования не обязательно должно быть сбалансированным, поэтому D может равняться O(P). Однако на практике они обычно хорошо сбалансированы, и поэтому можно принять, что D O(lgP). Следовательно, один поиск на карте можно выполнить за D запросов к дереву ячейки. Поскольку само это дерево может иметь не более P узлов, то каждый запрос займет O(lgP) времени. Таким образом, полное время обращения к одной ячейке карты будет O(DlgP).

Для локализации каждая частица должна произвести O(A) обращений к карте. На каждой итерации создаются P новых частиц, которые обновляют свои карты, что требует O(AP) операций. Следовательно, итоговая сложность алгоритма O(ADPlgP).

В завершение необходимо рассмотреть еще две детали: обновление карты и содержание дерева наследования.

В ячейках карты мы храним информацию в виде сбалансированных деревьев, то есть вставка и удаление значения потребуют O(lgP) на операцию. Каждая частица производит O(A) операций добавления. Очевидно, добавленные элементы в будущем будут удалены только один раз. То есть процедура добавления элементов на карту может быть завершена за O(AlgP) на частицу или за O(APlgP) на итерацию. Удаления бездетных частиц будет производиться постепенно и составит амортизированные O(APlgP).

Осталось только показать, что цена поддержания дерева наследования в минимальном виде будет приемлемой. В частности, надо показать, что цена удаления бездетных частиц не превысит O(ADPlgP). Это не очевидно с первого взгляда, поскольку суммарно удаляемые из ветви частицы могут вносить изменения во всю карту. Покажем, что амортизированная стоимость этих изменений составит O(ADPlgP). Сначала рассмотрим стоимость объединения списков изменений карт дочернего и родительского узлов. Если потомок изменил n ячеек, то необходимо произвести O(nlgP) операций (n запросов к сбалансированному дереву по родительскому ключу), чтобы найти дублирующиеся элементы множеств. Затем необходимо обновить идентификатор для всех ячеек карты потомка. Это производится путем удаления элемента с идентификатором потомка и добавлением элемента с идентификатором родителя. Цена этой операции также O(nlgP). Учтем, что каждая ячейка карты хранит в своем дереве не более D элементов, которые можно объединить в один, поскольку D – это высота дерева. На каждой итерации P частиц создают по A элементов карты, следовательно, сложность объединения будет не более O(ADPlgP).

Обобщим сказанное выше:

Утверждение
Пусть DP-SLAM работает с дальномером, сканирующим A ячеек карты за один проход, количество частиц, генерируемое на каждой итерации равно P, а глубина дерева наследования составляет D. Тогда для работы алгоритма потребуется:

  • O(ADPlgP) операций для локализации. P частиц проверяют значение A ячеек карты; каждая проверка занимает O(DlgP).
  • O(APlgP) операций на вставку новой информации в дерево. P частиц добавляют информацию в A ячеек карты, каждое добавление стоит O(lgP).
  • Управление деревом наследования имеет амортизированную стоимость O(ADPlgP). На каждой итерации проводится не более ADP объединений частиц, стоимостью O(lgP) каждое.

Сравнение сложности

Анализ показывает, что амортизированная сложность DP-SLAM составляет O(ADPlgP), что может достигать O(AP2lgP) в случае несбалансированности дерева наследования. В наивном подходе каждая карта была представлена явным образом, таким образом, проверка значения ячейки карты могла проводиться за постоянное время. Стадия локализации, в таком случае, занимала бы всего лишь O(AP) операций. Без учета обновления дерева наследования, обновление карты также могло бы быть проведено за O(AP) операций. Однако большая часть времени тратилась бы на копирования данных карты каждой частицы, которое потребовало бы O(MP) операций, где M – это размер карты. Поскольку обычно M >> A, это и стало бы определяющим фактором сложности наивного алгоритма.

Следовательно, DP-SLAM получает преимущество при условии, что ADlgP < M. Данное условие выполняется в большинстве практических случаев, поскольку размер карты M растет квадратично линейным размерам окружающей среды, и следовательно очень велик.

Примеры карт

Карта факультета Computer Science, Duke University, полученная разработчиками алгоритма:

http://www.cs.duke.edu/~parr/dpslam/D-Wing.html

Карта второго этажа Роботоцентра МГТУ им. Баумана, полученная командой robot-develop.org

Leave a Reply

You must be logged in to post a comment.