Содержание к диссертации
Введение
1.1. Вводные замечания 11
1.2. Подходы к решению задачи одновременной локализации и картирования
1.2.1. Решения на основе расширенного фильтра Калмана 14
1.2.2. Решения на основе фильтра частиц 20
1.3. Детекторы и дескрипторы особых точек 27
1.3.1. Алгоритмы детектирования особых точек 27
1.3.2. Дескрипторы особых точек 34
1.4. Системы прикладного объемного телевидения 40
1.4.1. Классификация систем объемного телевидения 41
1.4.2. Математические модели камеры 43
1.4.3. Формирование карты глубины в RGB-D системе 45
1.5. Краткие выводы 49
ГЛАВА 2. Алгоритм одновременной локализации и картирования с использованием видеосигнала с компенентой глубины 51
2.1. Вводные замечания 51
2.2. Модель системы и ее характеристики
2.2.1. Объемная камера и ее оптические характеристики 52
2.2.2. Калибровка камеры 55
2.3. Детектиование и отслеживание особых точек 60
2.3.1. Сравнение алгоритмов детектирования особых точек 62
2.3.2. Дескрипторы и сопоставление особых точек 71
2.4. Реализация алгоритма одновременной локализации и картирования 76
2.4.1. Особенности реализации 77
2.4.2. Сравнение подходов FastSLAM и EKF SLAM 83
2.4.3. Фильтрация ложных соответствий 90
2.4.4. Обработка замыканий 93
2.4.5. Тестирование алгоритма 94
2.5. Краткие выводы 100
ГЛАВА 3. Модификация компонентов системы 102
3.1. Вводные замечания 102
3.2. Предобработка карты глубины
3.2.1. Причины возникновения искажений карты глубины 102
3.2.2. Предобработка с помощью медианной фильтрации 105
3.2.3. Алгоритм интерполяции карты глубины 107
3.3. Сигма-точечный фильтр Калмана в задаче наблюдения ориентиров 113
3.3.1. Сигма-точечное преобразование 115
3.3.2. Алгоритм сигма-точечного фильтра Калмана 117
3.3.3. Реализация сигма-точечного фильтра Калмана 119
3.4. Адаптивный алгоритм генерации частиц 122
3.4.1. Пороговое значение эффективного числа частиц 122
3.4.2. Адаптивное задание порога 124
3.5. Краткие выводы 128
Заключение 130
Список литературы 133
- Решения на основе расширенного фильтра Калмана
- Классификация систем объемного телевидения
- Детектиование и отслеживание особых точек
- Алгоритм интерполяции карты глубины
Решения на основе расширенного фильтра Калмана
Как можно увидеть, погрешность при этом накапливается с каждым последующим шагом. В данном случае построенной картой является двумерная карта ориентиров. Если подходить к робототехнической задаче одновременной локализации и построения карты пространства глобально, то можно обозначить три парадигмы в подходах к решению данной проблемы: - Решения на основе расширенного фильтра Калмана; - Решения на основе фильтра частиц; - Решения на основе задачи оптимизации графа. Как правило, выбор той или иной парадигмы диктуется особенностями технической реализации робота, внешними условиями, а также требованиями к производительности программного комплекса. Для решения задачи одновременной локализации и картирования на основе анализа телевизионных изображений наиболее часто используются первые два подхода. Решения на основе оптимизации графов более характерны для систем, оснашенных лазерными датчиками и функционирующих внутри помещений в небольшом пространстве [98, 88]. При этом ориентирами выступают более высокоуровневые объекты, нежели особые точки изображний. В качестве ориентиров могут использоваться плоскости (стены) и углы помещений. Число ориентиров при этом значительно меньше, чем в случае использования естественных локальных особенностей изображений. Это вызвано в первую очередь тем, что трудоемкость задачи оптимизации построенного графа зависит от размера окружающего пространства и длины пути движения камеры. Поэтому решения на основе графов не рассматриваются в даннной работе, так как наиболее характерны для систем, оснащенных лазерными датчиками не предназначенных для работы в режиме реального времени.
Алгоритмы данной группы используют вероятностный подход к решению проблемы на основе определенной формы байесовских фильтров. Главная идея этого направления состоит в получении начальной, априорной оценки положения, которая основана на модели наблюдений, накопленной динамики измерений, а также наборе методов, используемых для вычисления уточненной, апостериорной оценки после непосредственных измерений. Расширенный фильтр Калмана
Основным инструментом рассматриваемого пути решения задачи локализации является та или иная реализация широкого класса фильтров Калмана. Данные алгоритмы предназначены для рекурсивного дооценивания вектора состояния априорно известной динамической системы. Таким образом, для расчёта текущего состояния системы необходимо знать текущее измерение, а также предыдущее состояние самого фильтра. Фильтр Калмана, подобно другим рекурсивным фильтрам, реализован не в частотном, а во временном представлении, но также, в отличие от других подобных решений, фильтр Калмана оперирует не только оценками состояния, но еще и оценками плотности распределения вектора состояния, опираясь на формулу Байеса условной вероятности. Состояние фильтра находится в двух переменных: Xk – оценка вектора состояния динамической системы в момент времени k; Pk – ковариационная матрица ошибок – мера точности оценивания вектора состояния в момент времени k. Расширенный фильтр Калмана обладает схожими с простым фильтром Калмана особенностями, за тем исключением, что он может быть использован в нелинейных процессах. На данный момент это один из наиболее распространенных методов решения задачи одновременной локализации и построения карты пространства [38]. Он позволяет не только уточнять оценку положения робота на карте, но и положение всех обнаруженных ориентиров (ключевых особенностей). Обычно процесс оценки состояния системы, в контексте рассматриваемой задачи, разбивают на три этапа: 1. обновление оценки состояния системы на основе данных одометрии; 2. обновление оценки состояния системы на основе повторно обнаруженных ориентиров; 3. добавление новых ориентиров в систему. Основным недостатком решения на основе расширенного фильтра Калмана является серьезная зависимость вычислительной сложности алгоритма от количества рассматриваемых ориентиров. Это вызвано тем, что матрица ковариации фильтра є имеет размерность п х п, где п - количество ориентиров. На каждом этапе обновления матрицы є, должен быть обновлен каждый ее элемент, в связи с чем сложность алгоритма составляет 0(п2).
Таким образом, расширенный фильтр Калмана наиболее применим в ситуации, когда среда имеет малое количество устойчиво отслеживаемых ориентиров, которое в большинстве практических случаев составляет несколько сотен элементов.Q
Классификация систем объемного телевидения
Реализация системы одновременной локализации и картирования невозможна без датчиков, дающих информацию об окружающем пространстве. Как отмечено в первой главе работы, на современном этапе развития устройств цифровой обработки сигналов, основным источником данных для таких систем становятся цифровые видеокамеры. Традиционно применение устройств прикладного телевидения для этих целей является вычислительно сложным и склонным к ошибкам при изменении уровня освещенности. Однако, оптимизация алгоритмов, лежащих в основе системы, и технологии объемного телевидения позволяют говорить о визуальных датчиках, как наиболее перспективных для решения задачи одновременной локализации и картирования.
В данной главе работы рассматривается вопросы реализации алгоритма одновременной локализации и картирования, проводятся исследования характеристик подходов на основе расширенного фильтра Калмана и фильтра частиц, исследуются качественные характеристики полученного решения. В качестве основного датчика в разрабатываемой системе используется объемная цифровая камера Kinect, позволяющая наряду со стандартным RGB изображением получить карту глубины наблюдаемого пространства. Это делает возможным измерение трехмерных координат объекта, распознанного на изображении. Датчик Kinect долгое время де факто являлся стандартной цифровой камерой с компонентой глубины, которая наилучшим образом подходит для решения самого широкого спектра задач технического зрения. Низкая цена, высокая надежность и скорость измерений сделали данную камеру основным трехмерным датчиком для мобильной робототехники, трехмерной реконструкции и задач распознавания объектов.
Подходы к описанию геометрической модели камеры Kinect, которые появились в последние годы, представляют хорошую основу для понимания ее работы [57, 51]. Данная камера представляет собой составное устройство, включающее в себя проектор инфракрасного диапазона, предназначенный для проецирования текстурного паттерна на объекты наблюдаемого пространства, а также камеры видимого и инфракрасного диапазона (рисунок 2.1). Как измерительное устройство, камера позволяет получить на выходе цветное и ИК изображения, а также карту глубины наблюдаемого пространства.
Инфракрасная камера имеет разрешение 12801024 пикселя, углы обзора 5745, фокусное расстояние 6.1 мм. Основное ее назначение -регистрация отраженного инфракрасного паттерна для трехмерной реконструкции наблюдаемой сцены. При условии постоянной освещенности данная камера может быть откалибрована стандартными методами с использованием калибровочного паттерна "шахматная доска" [28]. Демонстрация процесса калибровки приведена на рисунке 2.2. Камера демонстрирует пренебрежимо малые значения радиальной и тангенциальной дисторсий.
Цветная камера имеет разрешение 12801024 пикселя, углы обзора 63х50, фокусное расстояние 2.9 мм. Снимки с камеры среднего качества и в большинстве схожих задач технического зрения применяются для оценки траектории движения камеры относительно объектов окружающего пространства. Процесс калибровки в этом случае повторяет методики, применяемые при калибровке ИК камеры.
Калибровочное изображение на цветном, ИК изображениях и карте глубины: изображение паттерна "шахматная доска" с ИК камеры с наложенной проектором текстурой (а), при освещении галогенной лампой без проектора (б), калибровочные точки, выделенные на RGB изображении (в), калибровочные точки на карте глубины (г) Главной особенностью Kinect является возможность получения изображения демонстрирующего расстояния до точек сцены - карты глубины. На выходе камеры значения глубины являются инвертированными относительно действительных. Данная зависимость приведена на рисунке 2.3. Изображение карты глубины наблюдаемой сцены вычисляется аппаратно с помощью данных с ИК камеры, проектора и методов триангуляции.
На рисунке 2.4 приведена зависимость разрешения карты глубины от расстояния до наблюдаемого объекта. Измерения проводились при отдалении камеры от плоского объекта в диапазоне 0.5 - 15м, при помощи контрольных точек, расположенных около центра изображения в пределах угла обзора не более 5. Шаг квантования q, представляющий собой расстояние между двумя соседними значениями карты глубины, изменяется относительно значения глубины z по следующему закону. #(z) = 2.73z2+0.74z-0.58, где z - расстояние в метрах. Рис. 2.4. Зависимость пространственного квантования карты глубины от расстояния до объекта съемки
Таким образом на границах диапазона: q(0.5м) = 0.65мм, q(15.7м) = 685мм. Кроме того, между изображением с ИК камеры и картой глубины существует сдвиг, вызванный самим принципом получения карты глубины с помощью триангуляции. Его величина также зависит от расстояния до объекта.
При использовании датчика Kinect для проведения измерений необходимо учитывать геометрические искажения, вносимые внутренней структурой цветной и ИК камер. Поскольку ИК камера является одновременно и приемником датчика глубины, то полученные калибровочные данные могут быть использованы также и для устранения искажений карты глубины. Калибровка направлена получение внутренних параметров камер, а именно — матрицы камеры и коэффициентов дисторсии. При калибровке камер учитываются два основных вида искажений: радиальная и тангенциальная дисторсии. Радиальная дисторсия характеризуется искажением изображения в результате неидеальной параболической формы линзы. Искажения, вызванные радиальной дисторсией, равны 0 в оптическом центре сенсора и увеличиваются к краям. Как правило, радиальная дисторсия вносит наибольший вклад в искажение изображения. Тангенциальная дисторсия характеризуется искажениями изображения, вызванными погрешностями в установки линзы параллельно плоскости изображения.
Детектиование и отслеживание особых точек
В первой главе данной работы было отмечено, что долгое время наиболее распространенным походом к решеиню задачи одновременной локализации и построяния карты пространства являлся алгоритм на основе расширенного фильтра Калмана. Основоной проблемой данного метода является его вычислительная сложность, ограничивающая возможности его применения на картах, насчитывающих значительное число ориентиров. Для решения этой проблемы в 2002 году был разработан новый подход к решению задачи одновременной локализации и картирования [50], получивший название FastSLAM. В основе данного похода лежит разделение задачи на множество равнозначных подзадач, используя независимость состояния отдельных элементов модели SLAM. В текущем параграфе работы приводится сравнение данных подходов по различным характеристикам с точки зрения их применимости в системе прикладного объемного телевидения.
Рассматриваемая область применения алгоритма одновременной локализации и картирования, а именно система прикладного объемного телевидения, диктует особые требования к разрабатываемым методам. Такие системы, как правило, автономны и не обладают значительными вычислительными ресурсами. Так, алгоритм одновременной локализации и картирования находит широкое применение в мобильной робототехнике, где компактность робота существенно ограничивает его вычислительные возможности. Поэтому, наряду с этапом детектирования и обработки особых точек на изображениях, эффективность реализации задач этапа локализации и картирования также оказывает существенное влияние на скорость работы алгоритма и его работоспособность в целом [13].
Сложность алгоритма на основе расширенного фильтра Калмана (EKF SLAM) определяется размером матриц ковариации, обрабатываемых в процессе локализации и построения карты. Размерность этих матриц в наибольшей степени определяется количеством особых точек изображений, отслеживаемых алгоритмом (рис. 2.22).
Существует ряд подходов, позволяющих уменьшить вычислительную сложность. Так, в работе [24, 36] предлагается метод оптимизации работы алгоритма за счет уменьшения числа особых точек на каждом шаге обновления состояния фильтра. Но в любом случае вычислительная сложность данного метода будет прямо пропорциональна количеству обрабатываемых ориентиров, так как их положения и матрица ковариации должны обновляться на каждом шаге работы алгоритма, что приводит к квадратичной сложности O(N2).
В подходе, примененном в алгоритме FastSLAM, удалось уйти от затратного пошагового вычисления матрицы ковариации с помощью использования фильтра частиц и разделения задач локализации и картирования (рис. 2.23). Несмотря на то, что при этом в задаче наблюдения ориентиров все еще используется набор расширенных фильтров Калмана, они значительно уступают в размерности фильтру, применяемому в подходе EKF SLAM, кроме того их размерность постоянна. Каждая частица содержит полный набор отслеживаемых ориентиров, поэтому вычислительная сложность алгоритма FastSLAM составляет O(NK), где N определяет число ориентиров, а K – число частиц. Существует ряд решений, позволяющих еще более значительно уменьшить сложность алгоритма. Так, в работе [50] предлагается оптимизация структуры данных, обрабатываемых алгоритмом, с использованием бинарных деревьев, что позволяет создавать решения на основе FastSLAM со логарифмической сложностью O (N log K).
Схема работы алгоритма одновременной локализации и картирования на основе фильтра частиц (FastSLAM) Таким образом, быстродействие обеих решений задачи одновременной локализации и картирования, как с применением расширенного фильтра Калмана, так и фильтра частиц, в значительной степени зависят от количества отслеживаемых ориентиров. Алгоритм на основе фильтра частиц обладает лучшей масштабируемостью, так как позволяет работать с большим количеством ориентиров, чем метод на основе расширенного фильтра Калмана. Однако точность решения FastSLAM определяется количеством частиц, которыми оперирует фильтр. Их число также увеличивает итоговую вычислительную сложность. Количество частиц является параметром, с помощью которого достигается нужный баланс между сложностью и быстродействием алгоритма (рис. 2.24).Q
Алгоритм интерполяции карты глубины
Фильтр частиц представляет собой моделирующий метод оценки состояния зашумленной системы, хранящий взвешенное, нормализованное множество выборки состояний, называемых частицами. В рамках рассматриваемой задачи каждая частица представляет собой предполагаемые координаты камеры в пространстве в совокупности с вектором состояния наблюдаемых пространственных ориентиров. В течение каждого цикла работы фильтра после получения вектора измерений создается выборка новых состояний, каждое из которых взвешивается согласно Марковской модели наблюдений. После чего веса нормализуются для нового множества состояний [4]. Описание принципа работы фильтра частиц приводится в первой главе диссертации.
Одним из наиболее важных этапов работы фильтра частиц, оказывающим сильное влияние на производительность и точность работы фильтра, является процесс обновления вектора частиц. Задача при этом состоит в замене частиц с меньшими весами частицами с большими весами, при котором часть значений отсеивается по определенному закону. В данной работе для этого используется метод, называемый колесом отсева (Resampling wheel), подробно рассмотренный в работах [27, 34]. Момент цикла работы фильтра, при котором необходимо обновить набор частиц определяется величиной, называемой эффективным размером выборки, которая определяется как: где TV - это общее количество частиц, а wm- нормализованный вес к–й ZZ частицы. Когда разброс весов частиц относительно среднего значения увеличивается, величина Neff уменьшается. В том случае, когда веса частиц одинаковы (т.е. w[k] =l/N), эффективный размер выборки будет равен числу частиц. В противоположном случае, когда одна единственная частица обладает ненулевым весом, эффективный размер равен единице Neff =l.
В большинстве решений необходимость обновления набора частиц определяется жестко заданным порогом v отношения величины Nef/к общему числу частиц N, при достижении которого выполняется отсев частиц с наименьшими весами и генерация новых. Величина порога, как правило, находится эмпирическим путем. В большей части работ при этом величина порога выбирается на уровне 50% [42]. Это позволяет ограничить рост погрешности работы фильтра.
Однако, фиксированное задание значения порога не учитывает эволюцию процесса изменения величины Ыф что может приводить как к чрезмерной сложности алгоритма, так и к вырождению выборки частиц. Достаточно рассмотреть пример такого случая, когда эффективный размер выборки относительно невелик, однако его значение осциллирует относительно некоторого постоянного значения, что приводит к хорошему уровню аппроксимации искомого процесса. В этом случае порог необходимо понизить для предотвращения частого обновления выборки частиц. Также, среднее количество эффективных частиц в течение определенного временного интервала характеризует качество аппроксимации процесса в течение этого интервала.
Необходимость адаптивного задания порога обновления выборки частиц в особенности характерна для разрабатываемого алгоритма одновременной локализации и картирования, где основным методом измерения движения камеры является визуальная одометрия. Точность работы алгоритма визуальной одометрии напрямую зависит от количества пространственных ориентиров, которое постоянно меняется в зависимости от характера сцены и сопутствующих условий. Даже если детектор особых точек позволяет найти достаточное количество ориентиров, не все они находятся в рабочем диапазоне карты глубины. Если крупный объект находится на слишком большом расстоянии или слишком близко к камере, перекрывая большую часть сцены, погрешность алгоритма визуальной одометрии значительно увеличивается. В этом случае целесообразно увеличить порог, задающий частоту обновления выборки частиц.
Рассмотрим изменение величины Nejf в пределах периода времени т -1, в течение которого она достигает крайних значений Nmin и Nmax, таким образом, что 1 Nmin Neff Nmax N. Определим величину порога в период времени г -1 как vx_Y. При вычислении нового значения порога vT должно учитываться то, с какой скоростью изменялась величина Neff на протяжении предыдущего периода времени, а также среднее количество эффективных частиц в течение этого периода. Пусть кт_х - число итераций фильтра в течение периода r-1, а Neff(t) - число эффективных частиц на t-ой итерации. Тогда величины и , характеризующие среднее число частиц в период т -1 и степень их разброса относительно среднего значения, могут быть определены следующим образом: