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

ВЕСТНИК ЮГОРСКОГО ГОСУДАРСТВЕННОГО УНИВЕРСИТЕТА

_2018 г. Выпуск 3 (50). С. 49-56_

DOI: 10.17816/byusu2018049-56

УДК 51-7

А. В. Вохминцев, С. А. Пачганов

АЛГОРИТМ ОДНОВРЕМЕННОЙ НАВИГАЦИИ И СОСТАВЛЕНИЯ КАРТЫ МОБИЛЬНЫМ РОБОТОМ НА ОСНОВЕ ИТЕРАТИВНОГО АЛГОРИТМА БЛИЖАЙШИХ ТОЧЕК И ДЕСКРИПТОРА, ВЫЧИСЛЯЕМОГО В КРУГЛОМ СКОЛЬЗЯЩЕМ ОКНЕ

Данная работа была выполнена при поддержке Научного Фонда ЮГУ № 13-01-20/25.

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

Ключевые слова: сопоставление изображений, регистрация облаков точек, локализация, методы одновременной навигации и составления карты, двумерные дескрипторы, слежение за объектами, регистрация облаков точек.

A. V. Vokhmintcev, S. A. Pachganov

THE ALGORITHM OF SIMULTANEOUS NAVIGATION AND MAPPING FOR MOBILE ROBOT BASED ON THE ITERATIVE ALGORITHM OF THE NEAREST PIXELS AND DESCRIPTOR CALCULATED IN A CIRCULAR MOVING WINDOW

New combined algorithm for simultaneous navigation and map construction is developed using visual characteristics and depth information to compare images, register 3D-point clouds, and build global sequential 3D-maps of the surrounding space. The performance and computational complexity of the proposed RGB-D SLAM algorithm are presented and discussed with reference and real data. The results can be applied in real-time tracking of objects, in non-cooperative remote observation, and semantic mapping of mobile robot navigation problems.

Key words: images comparison, point clouds registration, localization, methods for simultaneous navigation and mapping, two-dimensional descriptors, the tracking of an object._

Введение

В робототехнике и машинном зрении разработано множество методов построения плотных трехмерных карт на основе сканеров диапазона [1], стереокамер [2], монокулярных камер [3], и даже неотсортированных коллекций фотографий [4]. Задача одновременной навигации и составления карты (Simultaneous localization and mapping-SLAM) [5] связана с построением трехмерной карты неизвестного пространства мобильным роботом во время его движения, которые находят применение для решения задачи визуального SLAM. Известные алгоритмы одновременной навигации и составления карты основаны на следующих подходах: частичный фильтр SLAM (Particle Filter SLAM), также называется FAST SLAM; расширенный фильтр Калмана для SLAM (Extended Kalman Filter, EKF); на графах SLAM (Graph-Based SLAM); визуальный SLAM (Visual SLAM) [6]. EKF SLAM аналогичен алгоритму EKF [7], применяемому для решения задачи локализации. Вектор состояния в EKF SLAM имеет существенно большую размерность, чем вектор состояния в EKF при локализации. С данным обстоятельством связан основной недостаток EKF SLAM - высокая алгоритмическая сложность.

В FAST SLAM [8] задача SLAM решается на основе частичных фильтров, которые представляют распределение вероятности в виде дискретного набора частиц, занимающих пространство состояний. FAST SLAM может применяться только для ограниченного класса прикладных задач, в которых исследуются малые области окружающего пространства. Подход на основе FAST SLAM имеет следующие недостатки: низкая масштабируемость вероятностных фильтров, линейная зависимость неопределенности на больших расстояниях от начала координат карты и

как следствие невозможность сопоставления данных соответствующим элементам при высокой неопределенности. В Graph-Based SLAM [9] узлы разряженного графа соответствуют координатам робота на трехмерной карте, а связи в графе - соответствуют координатам в последовательности положений робота на карте в относительной системе координат.

Современные практические решения для задачи SLAM для карт большого размера основаны на метрико-топологическом и визуальном подходах. Большинство методов построения трехмерных карт содержат следующие этапы: пространственное выравнивание последовательности кадров данных (регистрация изображений или облаков точек), решение проблемы замыкания цикла и глобальное выравнивание всех кадров данных. При использовании лазеров, времяпролетных или инфракрасных камер большое применение находит итеративный алгоритм ближайших точек ICP и его варианты [10]. Данный алгоритм регистрации позволяет сопоставлять трехмерные геометрические модели, сводя к минимуму расстояние между точками из двух кадров на основе метода наименьших квадратов, выходными данными ICP является матрица вращения и вектор переноса. Сходимость алгоритма ICP может быть значительно улучшена [11]. Для решения проблемы замыкания цикла большинство современных подходов используют технику быстрого сопоставления изображений, основанную на бинарной «корзине слов». После того как цикл в графе был обнаружен, новое соответствие между кадрами данных может быть использовано как дополнительное ограничение в графе, описывающем пространственные отношения между кадрами. Общим подходом для оптимизации графа положений является метод слепой подстройки, который одновременно оптимизирует граф положений и карту с пространственными особыми точками.

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

Комбинированный алгоритм регистрации данных

Схема предлагаемого алгоритма представлена на рисунке 1.

Входные данные: Последовательность RGB-D кадров

Извлечение особых точек Сопоставление изображений (SURF, HOGs, ORB)

Оценка положения (RANSCAC)

Сопоставление и регистрация облаков точек (1СР)

Коррекция позы и траектории движения робота(иКР)

Выходные данные: Плотная ЗО Карта помещения

Рисунок 1 - Общая схема комбинированного алгоритма регистрации данных

50

Алгоритм одновременной навигации и составления карты мобильным роботом

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

Шаг 1. Дискретизация изображений и плотных 3D облаков точек.

Шаг 2. Выделение характерных признаков в 2D изображениях (видимый или инфракрасный диапазон) на основе предложенного двумерного дескриптора.

Шаг 3. Установление соответствия между особыми точками изображений на основе метода ближайших соседей и KD-деревьев (взято стандартное решение).

Шаг 4. Сопоставление последовательных 3D кадров данных с использованием алгоритма RANSAC (взято стандартное решение) [12].

Шаг 5. Оптимизация графа положений робота, обнаружение замыкания цикла, глобальная оптимизация (взято стандартное решение).

Шаг 6. Регистрация облаков точек и изображений (взято стандартное решение).

Для сопоставления изображений используется двумерный дескриптор на основе гистограмм [13]. Гистограммы направленных градиентов (ГНГ) вычисляются в круглых областях и используются для сопоставления изображений.

Пусть } - исходное облако точек и } - целевое облако точек в

. Предположим, что отношения между точками в облаках и такие, что для каждой точки в можно вычислить соответствующую точку в . Во многих работах итеративный алгоритм ближайших точек (ICP algorithm) [11] рассматривается как геометрическое преобразование ригидных объектов из в :

R Xi + Т, (1)

где R - матрица поворота, Т - вектор переноса, i = 1,...,п. Аналогично можно ввести определение масштабированного алгоритма (S-ICP) как следующее геометрическое преобразование:

R Sxi + Т, (2)

где - матрица масштаба.

Группа Е ( 3 ) аффинных преобразований в измерении имеет 12 генераторов. Это означает, что аффинное преобразование является функцией от двенадцати переменных. Рассмотрим вариационную задачу алгоритма итеративных ближайших точек (ICP) для произвольного аффинного преобразования [14]. Пусть / (А, Т) можно представить как функцию следующего вида:

КА,Т)='% II I А Х{ + Т- у I I I2.

Тогда ICP вариационная проблема может быть определена как:

а г§т т / (А, Т),

(3)

(4)

где

А =

X; =

Уг =

(5)

/а11 0,12 а13\

а21 а22 а23 ), Т =

ча31 а32 о-зз/

Можно заметить, что:

] (А, 0 = 1( а 11Х11 + а 12X2г + а 13Х3г + ^ - У1) + + ( а21Х1 г + а22X2г + а23Х3г + Ь2 - У2г)2 + +( аз 1X1 г + а32Х2г + а33X3г + Ьз - Уз¿)2. (6)

Пусть новые координаты Хк1 могут быть выражены через старые координаты Хк1 следующим образом:

_ '7= 1 Хк1 ,к = 1 • • •, ^, I = 1 • ■ ■, (7)

х

ki

X

ki

Аналогично точки второго облака точек могут быть записаны следующим образом:

уы = Ук - ;уп=1 ущ ,к = 1.....з,/ = 1.....п. (8)

Давайте определим коэффициенты аь // 7., <р и для / = 1,..., п:

^ = х2 I - уп „ 2У/= 1 х2/ Х1/ , (9)

/ ^ = Хз Уи^2у"= ЗУ X 1- , (10)

7 = у 1 УЙ? УП= 1у 1х * (11)

* ' = / - УЙ7 У= 1 /Д (12)

=1 7 1 ^

Предположение. Элементы первой строки матрицы Л , которые минимизируют /, могут

^ = X - 1 X/«/• (13)

.1 перв<

быть вычислены следующим образом

У.= 1( i - а1 2 «2 i - а1 3 «3 i ) «1 i ^ ..

ai 1 = -У^Т^-' (14)

у i=1 «1 i у.= 1 X; - а 1 з у.= 1 а/

а!2=-V. „,2 ; ■ (15)

ут У/=1

у.=1 ^

а1 3 = уп 2 • (16)

Lfc=1 ^fc

Для второй и третьей строки матрицы Л подобные формулы могут быть получены аналогичным образом.

у—,

В первой итерации матрица М, которая может быть сгенерирована из матрицы A and вектора T, может быть инициализирована при помощи процедуры RANSAC преобразования. В последующих итерациях для каждой точки в исходном облаке точек X может быть выделена ближайшая точка в целевом облаке точек Y. Такое соответствие между точками может быть получено при использовании комбинации данных о характерных точках на изображении и данных о форме объекта, евклидово расстояние может быть быстро найдено при помощи процедуры на основе k-d дерева. Далее мы находим совместное решение регистрационной задачи, минимизируя значение ошибки выравнивания. Функция совместной ошибки может быть получена следующим образом:

Е( R,T) = а г gm i п й

^(¿LwilM F'- F'|2) + (1-а)^у^((М Si-dOn)!2)

(17)

где первое слагаемое измеряет средние квадраты расстояний для виузально связанных характерных точек с нормирующим множителем, т. е. вариация метрической характеристики функции от двух переменных и второе слагаемое измеряют средние квадраты расстояний для плотных облаков точек на основе метрики "рот1;-1;о-р1апе" (точка-плоскость) [15]. Вместо того, чтобы измерять расстояние между характерными точками в трехмерном пространстве, мы вычисляем значение среднеквадратической ошибки (МБЕ) в пиксельном пространстве. Функции и ^ обеспечивают проекции особых точек из координат в евклидовом пространстве в систему координат камеры. Двум слагаемым в выражении (17) сопоставлен весовой коэффициент а, который подбирается эмпирическим образом на основе информации об условиях наблюдения за сценой. Численное решение поставленной вариационной задачи осуществляется с помощью различных итерационных методов.

Алгоритм одновременной навигации и составления карты мобильным роботом Компьютерное моделирование и анализ результатов

В статье было проведено компьютерное моделирование, используя помещения Челябинского государственного университета в качестве сцены. Все объекты и детали обнаруженной сцены были получены с помощью следующих датчиков: Kinect 360 XBox 2.0 и двух камер видимого диапазона Beward B2720. Мы оценивали точность предложенного метода для навигации мобильного робота в серии экспериментов. Первый эксперимент: камера видимого диапазона и камера Kinect 2.0 переносились человеком в направлении движения; второй эксперимент: камера Kinect 2.0 была установлена на мобильной роботизированной платформе (Odyssey 6 Robotics); эксперименты проводились в контролируемых и неконтролируемых условиях, было установлено, что при отсутствии различных шумов и хорошем освещении предложенный комбинированный алгоритм регистрации облаков точек и изображений показывает похожие по точности (MSE-среднеквадратичное отклонение) результаты с классическим Visual SLAM, но при этом предложенный метод имеет лучшую сходимость; в неконтролируемых условиях предложенный метод показывает лучшие по точности результаты, чем известные подходы (Visual SLAM, EKF-SLAM, Graph-SLAM). Платформа для тестирования была взята с сайта openslam.org. 23 RGB-D ключевых кадра было захвачено. Изображения и облака точек были уменьшены до разрешения 320*240 и 0.1 вокселя соответственно и сохранены. На рисунке 2 показана 2D-карта помещения с обнаруженными большими циклами [16]. Циклы содержат 906 ключевых фреймов и составляют свыше 84 м в длину. Для того чтобы оценить согласованность этих карт, мы наложили 3D-карты на 2D-макеты, созданные различными способами. Для наглядности большинство точек пола и потолка были удалены из 3D-карт.

Рисунок 2 - 2D-карта: вид офиса сверху

Предложенный алгоритм на основе ГНГ использует три круглых скользящих окна радиуса г, зависящего от размера объекта (рисунок 3).

W2

Рисунок 3 - Круговая структура дисков, определенных внутри области целевого объекта в помещении

Алгоритм ГНГ был протестирован в различных условиях: повороты изображений в плоскости сцены (таблица 1), вне плоскости сцены (таблица 2) и небольшие изменения масштаба (таблица 3). Результаты экспериментов показали, что предложенный алгоритм на основе ГНГ превосходит известные алгоритмы при повороте изображения в плоскости изображения сцены, дает подобные с алгоритмом «преобразование, инвариантное к масштабу» результаты при повороте изображения вне плоскости изображения сцены и обладает скоростью обработки, близкой к алгоритму «робастные ускоренные признаки».

Таблица 1 - Точность сопоставления (в %) различных алгоритмов vs.

в плоскости сцены

Алгоритм сопоставления Угол поворота

45° 90° 135° 180° 225° 270°

Scale Invariant Feature Transform 100 98 99 98 97 95

Speeded-Up Robust Features 74 69 74 69 74 69

Oriented FAST and Rotated BRIEF 87 85 83 86 85 87

Предложенный алгоритм 100 98 96 98 97 95

Таблица 2 - Точность сопоставления (в %) различных алгоритмов vs. вне плоскости сцены

Алгоритм сопоставления Угол поворота

5° 10° 15° 20° 25° 30°

Scale Invariant Feature Transform 98 91 78 64 58 47

Speeded-Up Robust Features 82 77 64 55 38 29

Oriented FAST and Rotated BRIEF 96 84 77 61 58 54

Предложенный алгоритм 83 78 74 72 76 72

Таблица 3 - Точность сопоставления (в %) различных алгоритмов vs. при небольшом изменении масштаба

Алгоритм сопоставления Масштаб

0.8X 0.9X 1.0 X 1.1X 1.2 X

Scale Invariant Feature Transform 92 95 100 98 91

Speeded-Up Robust Features 79 90 99 97 92

Oriented FAST and Rotated BRIEF 78 79 90 83 89

Предложенный алгоритм 84 94 100 99 91

Сравнение выбранных алгоритмов по времени обработки показало, что время обработки алгоритмов HOG и ORB [17] составляет около 1,7 сек., а время обработки алгоритмов SIFT х [18] и SURF [19] - 9,82 сек. и 0,95 сек. соответственно. Точность предложенного комбинированного алгоритма представлена в табл. 4 в сравнении с известными алгоритмами. Каждая запись имеет два значения: первое значение показывает точность сопоставления (%), второе значение показывает вычислительную сложность в сек. Рисунок 5 показывает зависимость точности протестированных алгоритмов от числа итераций в терминах среднеквадратичной ошибки. Из графика мы видим, что комбинированный алгоритм имеет лучшую точность.

Таблица 4 - Точность (в %) и вычислительная сложность (сек.) алгоритмов регистрации в зависимости от угла поворота

Алгоритм регистрации Угол поворота (градусы)

5° 10° 15° 20° 25°

Fast ICP 91/0,06 80/0,1 73/0,11 66/0,13 56/0,15

ICP (point-to-plane) 97/1,56 95/1,86 91/2,34 86/3,8 81/4,17

Комбинированный алгоритм 98/1,45 99/1,78 98/2,4 96/2,95 91/3,2

Алгоритм одновременной навигации и составления карты мобильным роботом

MSE in each iteration

5 r-tr-1-1-

5 10 15 20 25

iteration time

Рисунок 4 - Точность алгоритмов регистрации в терминах среднеквадратичной ошибки

Заключение

Подход на основе визуального SLAM, использованный для решения вариационной задачи на основе итеративного алгоритма особых точек (Iterative Close Point, ICP), находится в русле основных тенденций развития современных методов и алгоритмов одновременной навигации и составления карты в неизвестном пространстве. В предложенной постановке проблемы мы находим решение вариационной задачи на основе комбинации данных об особых точках (данные о цвете сцены) и данных в виде плотного трехмерного облака точек (данные о глубине). В рассматриваемые функционалы входят слагаемые, которые измеряют средние квадраты расстояний для визуально-связанных характерных точек с нормирующим множителем, т. е. вариация метрической характеристики функции от двух переменных, и слагаемые, измеряющие средние квадраты расстояний для плотных облаков точек на основе метрики "point-to-plane" (точка-плоскость) и различные обобщения таких функционалов. Вместо того, чтобы измерять расстояние между характерными точками в трехмерном пространстве, мы вычисляем значение среднеквадратической ошибки (MSE) в пиксельном пространстве. Полученные результаты показывают, что предложенный комбинированный метод на основе данных о глубине и цвете наблюдаемой сцены показывает лучшие по точности результаты, чем известные подходы к решению SLAM задачи на основе визуального SLAM. Также были проведены вычислительные эксперименты для эталонных баз данных, которые показали, что предложенный метод имеет хорошую сходимость и может использоваться в приложениях, работающих в реальном масштабе времени.

Литература

1. Endres, F. An evaluation of the RGB-D SLAM system [Text] / F. Endres, J. Hess, N. Engelhard [et al.] // Proceedings of the IEEE International Conference on Robotics and Automation (Saint Paul, May 14-18, 2012). - Saint Paul, 2012. - P. 1691-1696.

2. Outdoor mapping and navigation using stereo vision [Text] / K. Konolige, M. Agrawal, R. C. Bolles [et al.] // Proceedings of the International Symposium on Experimental Robotics (Rio de Janeiro, July 6-10, 2006). - Rio de Janeiro, 2006. - P. 179-190.

3. MonoSLAM Real-Time Single Camera SLAM [Text] / A. J. Davison, I. D. Reid, N. D. Molton [et al.] // IEEE Trans. Pattern Anal. Mach. Intell. - 2007. - V. 7.

À. В. Вохминцеe, С. À. na^aHoe

4. Hertzberg, C. Experiences in building a visual slam system from open source components [Text] / C. Hertzberg, R. Wagner, O. Birbach // Proceedings of the IEEE International Conference on Robotics and Automation (Shanghai, May 9-13, 2011). - Shanghai, 2011. - P. 2644-2651.

5. Detailed real-time Urban 3D reconstruction from video [Text] / M. Pollefeys, D. Nister, J-M. Frahm [et al.] // Int. J. Comput. Vis. - 2008. - V. 78(2).

6. Fioraio, N. Realtime visual and point cloud SLAM [Text] / N. Fioraio, K. Konolige // Proceedings of the RSS Workshop on RGB-D: Advanced Reasoning with Depth Cameras at Robotics (Los Angeles, June 27 - July 1, 2011). - Los Angeles, 2011.

7. Chen, Y. Kalman filter for robot vision [Text] : a survey / Y. Chen // IEEE Transa ctions on Industrial Electronics - 2012. - V. 59.

8. FastSLAM: A factored solution to the simultaneous localization and mapping problem [Text] / M. Montemerlo, S. Thrun, D. Koller [et al.] // Proceedings of the AAAI National Conference on Artificial Intelligence (Edmonton, July 28 - August 1, 2002). - Edmonton, 2002. - P. 593-598.

9. Estrada, C. Hierarchical SLAM: Real-time accurate mapping of large environments [Text] / C. Estrada, J. Neira, J. D. Tardos // Proc.IEEE Transactions On Robotics - 2005. -V. 21(4). - P. 588-596.

10. Besl, P. A method for registration of 3-D shapes [Text] / P. Besl, N. McKay // IEEE Trans. Pattern Anal. Mach. Intell. - 1992. - V. 14(2). - P. 239-256.

11. Chen, Y. Object modeling by registration of multiple range images [Text] / Y. Chen, G. Medioni // Proceedings of the IEEE International Conference on Robotics and Automation (Nice, May 12-14, 1992). - Nice, 1992. - P. 145-155.

12. Fischler, M. Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography [Text] / M. Fischler, R. Bolles // Graphics and Image Processing - 1981. - V. 1. - P. 381-395.

13. Face recognition based on matching algorithm with recursive calculation of local oriented gradient histogram [Text] / A. V. Vokhmintcev, I. V. Sochenkov, V. V. Kuznetsov // Doklady Akademii Nauk. - 2016. - Vol. 466. - № 3. - pp. 261-266.

14. A fusion algorithm for building three-dimensional maps [Text] A. Vokhmintcev [et al.]. // Proceedings. SPIE's Annual Meeting : Applications of Digital Image Processing XXXVIII. - San Diego, 2015. - vol. 8452. - p. 9599-81..

15. Henry, P. RGB-D mapping: Using depth cameras for dense 3D modeling of indoor environments [Text] / P. Henry, M. Krainin, E. Herbst // Proceedings of the International Symposium on Experimental Robotics (Marrakech and Essaouira, June 15-18, 2014). - Marrakech : Essaouira, 2014 - P. 477-491.

16. Josef, S. Efficient visual search of videos cast as text retrieval [Text] / S. Josef // IEEE Trans. Pattern Anal. Mach. Intell - 2009. - V. 31(4). - P. 591-605.

17. ORB: an efficient alternative to SIFT or SURF [Text] / E. Rublee, V. Rabaud, K. Konolige [et al.] // IEEE International Conference Computer Vision (Barcelona, November 6-13, 2011). -Barcelona, 2011.

18. Lowe, D. G. Object recognition from local scale invariant features [Text] / D. G. Lowe // Proceedings of the 7th International conference on Computer Vision (Kerkyra, September 20-27, 1999). - Kerkyra, 1999. - V. 2. - P. 1150-1157.

19. SURF: speeded up robust features [Text] / H. Bay, A. Ess, T. Tuytelaars [et al.] // Comput. Vis. Image Underst. - 2008. - V. 110. - P. 346-359.