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

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

Пусть $X=\left\{x_1,...,x_n\right\}$ – исходное облако точек и $Y=\left\{y,...,y_m\right\}$ – целевое облако точек в ${ℝ}^3$. Предположим, что отношения между точками в облаках $X$ и $Y$ такие, что для каждой точки в $x_i$ можно вычислить соответствующую точку в $y_i$. Во многих работах итеративный алгоритм ближайших точек (ICP algorithm) [11] рассматривается как геометрическое преобразование ригидных объектов из $X$ в $Y$:

$R{x}_i+T$ (1)

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

$RS{x}_i+T$ (2)

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

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

$J(A,T)={\sum }_{i=1}^n{||A{x}_i+T-y_i||}^2.$ (3)

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

$\arg \min J(A,T)$ (4)

где

$a=\left(\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right), T=\left(\begin{array}{c}{t}_1\\ t_2\\ t_3\end{array}\right), x_i=\left(\begin{array}{c}{x}_{1i}\\ x_{2i}\\ x_{3i}\end{array}\right), y_i=\left(\begin{array}{c}{y}_{1i}\\ y_{2i}\\ y_{3i}\end{array}\right)$ (5)

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

                                     

$J(A,T)={\sum }_{i=1}^n(a_{11}{x}_{1i}+a_{12}{x}_{2i}+a_{13}{x}_{3i}+t_1-y_{1i}{)}^2+(a_{21}{x}_{1i}+a_{22}{x}_{2i}+a_{23}{x}_{3i}+t_2-y_{2i}{)}^2+(a_{31}{x}_{1i}+a_{32}{x}_{2i}+a_{33}{x}_{3i}+t_3-y_{3i}{)}^2$ (6)

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

$x_{ki}=x_{ki}-\frac{1}{n}{\sum }_{j=1}^n{x}_{kj},k=1,...,3,i=1,...,n$ (7)

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

$y_{ki}=y_{ki}-\frac{1}{n}{\sum }_{j=1}^n{y}_{kj},k=1,...,3,i=1,...,n$ (8)

Давайте определим коэффициенты ${\alpha }_i, {\beta }_i, {\gamma }_i, {\phi }_i и {\psi }_i для i=1,...,n$:

${\alpha }_i=x_{2i}-\frac{x_{1i}}{{\displaystyle {\sum }_{j=1}^n{x_{1j}}^2}}\sum _{j=1}^n{x}_{2j}{x}_{1j}$ ,(9),(10)

${\beta }_i=x_{3i}-\frac{x_{1i}}{{\displaystyle {\sum }_{j=1}^n{x_{1j}}^2}}\sum _{j=1}^n{x}_{3j}{x}_{1j}$,(11)

${\gamma }_i=y_{1i}-\frac{x_{1i}}{{\displaystyle {\sum }_{j=1}^n{x_{1j}}^2}}\sum _{j=1}^n{y}_{1j}{x}_{1j}$,(12)

${\phi }_i={\beta }_i-\frac{{\alpha }_i}{{\displaystyle {\sum }_{j=1}^n{{\alpha }_j}^2}}\sum _{j=1}^n{\beta }_j{\alpha }_j$.(13)

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

$a_{11}=\frac{{\displaystyle {\sum }_{i=1}^n\left(y_{1i}-a_{12}{x}_{2i}-a_{13}{x}_{3i}\right)x_{1i}}}{{\displaystyle {\sum }_{i=1}^n{x_{1i}}^2}}$,(14)

$a_{12}=\frac{{\displaystyle {\sum }_{j=1}^n{\gamma }_j{\alpha }_j-a_{13}{\sum }_{j=1}^n{\beta }_j{\alpha }_j}}{{\displaystyle {\sum }_{j=1}^n{{\alpha }_j}^2}}$,(15)

$a_{13}=\frac{{{\displaystyle \sum }}_{k=1}^n{\phi }_k{\psi }_k}{{\displaystyle {\sum }_{k=1}^n{{\phi }_k}^2}}$.(16)

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

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

$\text{E}\left(\text{R},\text{T}\right)={\text{argmin}}_{}\left[\begin{array}{c}\alpha \frac{1}{\text{W}}\left(\frac{1}{\left|{\text{A}}_{\text{f}}\right|}\sum _{\text{i}}{\text{w}}_{\text{i}}{\left|{}_{\text{s}}^{\text{i}}-{\text{ F}}_{\text{d}}^{\text{i}}\right|}^2\right)+\\ \left(1-\alpha \right)\frac{1}{\text{W}}\left(\frac{1}{\left|{\text{A}}_{\text{d}}\right|}\sum _{\text{j}}{\text{w}}_{\text{j}}{\left|\left(\left({}_{\text{j}}-{\text{d}}_{\text{j}}\right){\text{n}}_{\text{j}}\right)\right|}^2\right)\end{array}\right]$,(17)

где первое слагаемое измеряет средние квадраты расстояний для виузально связанных характерных точек с нормирующим множителем, т. е. вариация метрической характеристики функции от двух переменных и второе слагаемое измеряют средние квадраты расстояний для плотных облаков точек на основе метрики “point-to-plane” (точка-плоскость) [15]. Вместо того, чтобы измерять расстояние между характерными точками в трехмерном пространстве, мы вычисляем значение среднеквадратической ошибки (MSE) в пиксельном пространстве. Функциии $F_s^i$ и $F_d^i$ обеспечивают проекции особых точек из координат в евклидовом пространстве в систему координат камеры. Двум слагаемым в выражении (17) сопоставлен весовой коэффициент $\alpha$, который подбирается эмпирическим образом на основе информации об условиях наблюдения за сценой. Численное решение поставленной вариационной задачи осуществляется с помощью различных итерационных методов.

Компьютерное моделирование и анализ результатов

В статье было проведено компьютерное моделирование, используя помещения Челябинского государственного университета в качестве сцены. Все объекты и детали обнаруженной сцены были получены с помощью следующих датчиков: 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, 454001

Stepan A. Pachganov

Yugra State University

Email: s.pachganov@yandex.ru

Graduate student

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

References

Supplementary files