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

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.


Введение

В робототехнике и машинном зрении разработано множество методов построения плотных трехмерных карт на основе сканеров диапазона [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]. Гистограммы направленных градиентов (ГНГ) вычисляются в круглых областях и используются для сопоставления изображений.

Пусть X=x1,...,xn – исходное облако точек и Y=y,...,ym – целевое облако точек в 3. Предположим, что отношения между точками в облаках X и Y такие, что для каждой точки в xi можно вычислить соответствующую точку в yi. Во многих работах итеративный алгоритм ближайших точек (ICP algorithm) [11] рассматривается как геометрическое преобразование ригидных объектов из X в Y:

Rxi+T (1)

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

RSxi+T (2)

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

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

J(A,T)=i=1nAxi+Tyi2. (3)

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

arg min J(A,T) (4)

где

a=a11a12a13a21a22a23a31a32a33, T=t1t2t3, xi=x1ix2ix3i, yi=y1iy2iy3i (5)

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

                                     

J(A,T)=i=1n(a11x1i+a12x2i+a13x3i+t1y1i)2+(a21x1i+a22x2i+a23x3i+t2y2i)2+(a31x1i+a32x2i+a33x3i+t3y3i)2 (6)

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

xki=xki1nj=1nxkj,k=1,...,3,i=1,...,n (7)

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

yki=yki1nj=1nykj,k=1,...,3,i=1,...,n (8)

Давайте определим коэффициенты αi, βi, γi, φi и ψi для i=1,...,n:

αi=x2i x1ij=1nx1j2j=1nx2j x1j  ,(9),(10)

βi=x3i x1ij=1nx1j2j=1nx3j x1j ,(11)

γi=y1i x1ij=1nx1j2 j=1ny1jx1j,(12)

φi = βi   αij=1n  αj2 j=1n βjαj.(13)

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

a11 =i=1n y1i a12x2i a13x3i  x1ii=1nx1i2,(14)

a12=j=1n γj αja13 j=1n  βj αjj=1n  αj2,(15)

a13= k=1n  φk ψkk=1n  φk2.(16)

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

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

ER,T=argminα1W1Afiwisi Fdi2+1α1W1Adjwjjdjnj2,(17)

где первое слагаемое измеряет средние квадраты расстояний для виузально связанных характерных точек с нормирующим множителем, т. е. вариация метрической характеристики функции от двух переменных и второе слагаемое измеряют средние квадраты расстояний для плотных облаков точек на основе метрики “point-to-plane” (точка-плоскость) [15]. Вместо того, чтобы измерять расстояние между характерными точками в трехмерном пространстве, мы вычисляем значение среднеквадратической ошибки (MSE) в пиксельном пространстве. Функциии Fsi  и Fdi  обеспечивают проекции особых точек из координат в евклидовом пространстве в систему координат камеры. Двум слагаемым в выражении (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. вне плоскости сцены

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

Угол поворота

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 – Точность (в %) и вычислительная сложность (сек.) алгоритмов регистрации в зависимости от угла поворота

Алгоритм регистрации

Угол поворота (градусы)

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. Также были проведены вычислительные эксперименты для эталонных баз данных, которые показали, что предложенный метод имеет хорошую сходимость и может использоваться в приложениях, работающих в реальном масштабе времени.

Alexandr V. Vokhmintcev

Chelyabinsk State University

Author for correspondence.
Email: vav@csu.ru

Russian Federation, 129, Kashirin brothers street, Chelyabinsk, 454001

Candidate of Technical Sciences

Head of Research Laboratory

Stepan A. Pachganov

Yugra State University

Email: s.pachganov@yandex.ru

Russian Federation, 16, Chehova street, Khanty-Mansiysk, 628012

Graduate student

  • 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.

Supplementary files

There are no supplementary files to display.

Views

Abstract - 10

PDF (Russian) - 6

PlumX


Copyright (c) 2018 Vokhmintcev A.V., Pachganov S.A.

Creative Commons License
This work is licensed under a Creative Commons Attribution-ShareAlike 4.0 International License.