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
- Authors: Vokhmintcev A.V.1, Pachganov S.A.2
-
Affiliations:
- Chelyabinsk State University
- Yugra State University
- Issue: Vol 14, No 3 (2018)
- Pages: 49-56
- Section: Articles
- URL: https://vestnikugrasu.org/byusu/article/view/10789
- DOI: https://doi.org/10.17816/byusu2018049-56
- ID: 10789
Cite item
Full Text
Abstract
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.
Full Text
Введение
В робототехнике и машинном зрении разработано множество методов построения плотных трехмерных карт на основе сканеров диапазона [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.
Рисунок 1 – Общая схема комбинированного алгоритма регистрации данных
Предложен новый метод регистрации изображений и облаков точек, основанный на комбинации данных о цвете и глубине наблюдаемой сцены. Предложенный подход SLAM содержит следующие основные шаги.
Шаг 1. Дискретизация изображений и плотных 3D облаков точек.
Шаг 2. Выделение характерных признаков в 2D изображениях (видимый или инфракрасный диапазон) на основе предложенного двумерного дескриптора.
Шаг 3. Установление соответствия между особыми точками изображений на основе метода ближайших соседей и KD-деревьев (взято стандартное решение).
Шаг 4. Сопоставление последовательных 3D кадров данных с использованием алгоритма RANSAC (взято стандартное решение) [12].
Шаг 5. Оптимизация графа положений робота, обнаружение замыкания цикла, глобальная оптимизация (взято стандартное решение).
Шаг 6. Регистрация облаков точек и изображений (взято стандартное решение).
Для сопоставления изображений используется двумерный дескриптор на основе гистограмм [13]. Гистограммы направленных градиентов (ГНГ) вычисляются в круглых областях и используются для сопоставления изображений.
Пусть – исходное облако точек и – целевое облако точек в . Предположим, что отношения между точками в облаках и такие, что для каждой точки в можно вычислить соответствующую точку в . Во многих работах итеративный алгоритм ближайших точек (ICP algorithm) [11] рассматривается как геометрическое преобразование ригидных объектов из в :
(1)
где R – матрица поворота, T – вектор переноса, i=1,...,n. Аналогично можно ввести определение масштабированного алгоритма (S-ICP) как следующее геометрическое преобразование:
(2)
где S – матрица масштаба.
Группа E(3) аффинных преобразований в измерении имеет генераторов. Это означает, что аффинное преобразование является функцией от двенадцати переменных. Рассмотрим вариационную задачу алгоритма итеративных ближайших точек (ICP) для произвольного аффинного преобразования [14]. Пусть J(A,T) можно представить как функцию следующего вида:
(3)
Тогда ICP вариационная проблема может быть определена как:
(4)
где
(5)
Можно заметить, что:
(6)
Пусть новые координаты могут быть выражены через старые координаты следующим образом:
(7)
Аналогично точки второго облака точек могут быть записаны следующим образом:
(8)
Давайте определим коэффициенты :
,(9),(10)
,(11)
,(12)
.(13)
Предположение. Элементы первой строки матрицы , которые минимизируют , могут быть вычислены следующим образом
,(14)
,(15)
.(16)
Для второй и третьей строки матрицы подобные формулы могут быть получены аналогичным образом.
В первой итерации матрица , которая может быть сгенерирована из матрицы A and вектора T, может быть инициализирована при помощи процедуры RANSAC преобразования. В последующих итерациях для каждой точки в исходном облаке точек X может быть выделена ближайшая точка в целевом облаке точек Y. Такое соответствие между точками может быть получено при использовании комбинации данных о характерных точках на изображении и данных о форме объекта, евклидово расстояние может быть быстро найдено при помощи процедуры на основе k-d дерева. Далее мы находим совместное решение регистрационной задачи, минимизируя значение ошибки выравнивания. Функция совместной ошибки может быть получена следующим образом:
,(17)
где первое слагаемое измеряет средние квадраты расстояний для виузально связанных характерных точек с нормирующим множителем, т. е. вариация метрической характеристики функции от двух переменных и второе слагаемое измеряют средние квадраты расстояний для плотных облаков точек на основе метрики “point-to-plane” (точка-плоскость) [15]. Вместо того, чтобы измерять расстояние между характерными точками в трехмерном пространстве, мы вычисляем значение среднеквадратической ошибки (MSE) в пиксельном пространстве. Функциии и обеспечивают проекции особых точек из координат в евклидовом пространстве в систему координат камеры. Двум слагаемым в выражении (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-карта: вид офиса сверху
Предложенный алгоритм на основе ГНГ использует три круглых скользящих окна радиуса r, зависящего от размера объекта (рисунок 3).
Рисунок 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 |
Рисунок 4 – Точность алгоритмов регистрации в терминах среднеквадратичной ошибки
Заключение
Подход на основе визуального SLAM, использованный для решения вариационной задачи на основе итеративного алгоритма особых точек (Iterative Close Point, ICP), находится в русле основных тенденций развития современных методов и алгоритмов одновременной навигации и составления карты в неизвестном пространстве. В предложенной постановке проблемы мы находим решение вариационной задачи на основе комбинации данных об особых точках (данные о цвете сцены) и данных в виде плотного трехмерного облака точек (данные о глубине). В рассматриваемые функционалы входят слагаемые, которые измеряют средние квадраты расстояний для визуально-связанных характерных точек с нормирующим множителем, т. е. вариация метрической характеристики функции от двух переменных, и слагаемые, измеряющие средние квадраты расстояний для плотных облаков точек на основе метрики “point-to-plane” (точка-плоскость) и различные обобщения таких функционалов. Вместо того, чтобы измерять расстояние между характерными точками в трехмерном пространстве, мы вычисляем значение среднеквадратической ошибки (MSE) в пиксельном пространстве. Полученные результаты показывают, что предложенный комбинированный метод на основе данных о глубине и цвете наблюдаемой сцены показывает лучшие по точности результаты, чем известные подходы к решению SLAM задачи на основе визуального SLAM. Также были проведены вычислительные эксперименты для эталонных баз данных, которые показали, что предложенный метод имеет хорошую сходимость и может использоваться в приложениях, работающих в реальном масштабе времени.
About the authors
Alexandr V. Vokhmintcev
Chelyabinsk State University
Author for correspondence.
Email: vav@csu.ru
Candidate of Technical Sciences
Head of Research Laboratory
Russian Federation, 129, Kashirin brothers street, Chelyabinsk, 454001Stepan A. Pachganov
Yugra State University
Email: s.pachganov@yandex.ru
Graduate student
Russian Federation, 16, Chehova street, Khanty-Mansiysk, 628012References
- 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.
- 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.
- 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.
- 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.
- 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).
- 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.
- Chen, Y. Kalman filter for robot vision [Text] : a survey / Y. Chen // IEEE Transa ctions on Industrial Electronics - 2012. - V. 59.
- 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.
- Estrada, C. Hierarchical SLAM: Real-time accurate mapping of large environments [Text] / С. Estrada, J. Neira, J. D. Tardos // Proc.IEEE Transactions On Robotics - 2005. - V. 21(4). - P. 588-596.
- 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.
- 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.
- 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.
- 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.
- 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.
- 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.
- 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.
- 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.
- 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.
- SURF: speeded up robust features [Text] / H. Bay, A. Ess, T. Tuytelaars [et al.] // Comput. Vis. Image Underst. - 2008. - V. 110. - P. 346-359.