Система визуальной навигации автономного подвижного объекта тема диссертации и автореферата по ВАК РФ 00.00.00, кандидат наук Алхатиб Маджи

• Алхатиб Маджи
• кандидат наук
• 2023

Оглавление диссертации кандидат наук Алхатиб Маджи

ВВЕДЕНИЕ

Глава 1. Обзор литературы и постановка задачи исследования

1. 1 Системы визуальной навигации

1.2 Методы классификации локализации с помощью технического зрения

1.3 Первый предложенный метод

1.4 Выводы по главе

Глава 2. Геометрия с двумя видами (математическая модель)

2. 1 Фундаментальная матрица

2.2 Существенная матрица

2.3 Проблема триангуляции

2.4 Локальные признаки изображения

2.5 Выводы по главе

Глава 3. Визуальная локализация с использованием ББМ

3. 1 Построение 3D структуры

3. 2 Этап нав игации

3.3 Эксперименты

3.4 Выводы по главе

Глава 4. Оптимизированная оценка положения камеры с использованием

итеративного алгоритма на основе визуального маркера

4. 1 Метод Гаусса-Ньютона

4.2 Расчет ошибки оценки положения

4.3 Выводы по главе

Глава 5. Автономная навигация с использованием Фильтра частиц

5. 1 Отображение

5.2 Оценка положения и ориентации мобильного робота с помощью фильтра частиц

5.3 Эксперименты

Стр.

5.4 Модифицированный фильтр частиц

5.5 Выводы по главе

Глава 6. Структурированный свет

6. 1 Калибровка камеры-проектора

6.2 3Б-реконструкция с использованием структурированного света

6.3 Эксперимент

6.4 Выводы по главе

ОБЩИЕ ВЫВОДЫ И ЗАКЛЮЧЕНИЕ

СПИСОК ЛИТЕРАТУРЫ

ПЕРЕЧЕНЬ СОКРАЩЕНИЙ И УСЛОВНЫХ ОБОЗНАЧЕНИЙ

SFM - Structure from Motion (Определение структуры по движению) SIFT - Scale-Invariant Feature Transform (Масштабно-инвариантная трансформация признаков)

BRIEF - Binary Robust Independent Elementary Features (Вращающиеся бинарные надежные независимые элементарные функции) FAST - Features from Accelerated Segment Test (Извлечение признаков с помощью ускоренной проверки областей)

ORB - Oriented FAST and Rotated BRIEF (Ориентация по FAST и поворот по BRIEF)

RANSAC- RANdom SAmple Consensus (Согласованный алгоритм случайной выборки)

PnP- Perspective-n-Point (Перспектива^-Точка) GNSS-RTK - Kinematic Real-Time Global Navigation Satellite System GNSS-RTK (Глобальная навигационная спутниковая система кинематического реального времени)

VO - Visual Odometry (Визуальная одометрия)

INS- Inertial Navigation System (Инерциальная навигационная система)

KF - Kalman Filter (Фильтр Калмана)

EKF - Extended Kalman Filter (Расширенный фильтр Калмана)

UKF - Unscented Kalman Filter (Сигма-точечный фильтр Калмана)

PF - Particle Filter (Фильтр Частиц)

RRT - Rapidly exploring random tree (Алгоритм быстрого исследования

случайных деревьев)

RRT* - Rapidly exploring random tree star

A* - Searching algorithm that searches for the shortest path between the initial and the final state.

BFS - Breadth First Search (Поиск в ширину)

SL - Structured Light (Структурированный свет)

FPP - Fringe Projection Profilometry (Профилометрия проекции бахромы) PSP - Phase Shift Profilometry (Фазовая профилометрия)

Введение диссертации (часть автореферата) на тему «Система визуальной навигации автономного подвижного объекта»

ВВЕДЕНИЕ

В данной работе рассматривается задача разработки и исследования системы визуальной навигации мобильного робота в помещении с использованием таких методов, как определение структуры из движения (SFM), итерационный алгоритм, фильтр частиц и структурированный свет.

Большинству современных мобильных систем необходимо с высокой точностью оценивать свое местоположение и направление в сложной среде, где существуют препятствия и заторы, такие как автономное вождение, внутренняя навигация, дополненная реальность и виртуальная реальность. Поэтому многие исследователи сосредоточены на определении положения и ориентации движущихся объектов. В древности люди определяли свое местонахождение по движению звезд. В середине прошлого века была создана глобальная система позиционирования GPS (Global Positioning System), точность которой составляет от 1 до 3 метров. Точность может достигать сантиметров при использовании GNSS -RTK (Kinematic Real-Time Global Navigation Satellite System, Глобальная навигационная спутниковая система кинематического реального времени), но стоимость очень высокая. Система GNSS становится неточной или ненадежной во многих случаях: в помещении, между высокими зданиями. По вышеуказанным причинам GNSS интегрируется с инерциальными датчиками. Однако точность измерений в инерци-альных датчиках со временем снижается из-за дрейфа, а использование инерциальных датчиков с малыми дрейфовыми системами для многих приложений очень дорого.

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

объемами информации при низкой цене, весе, размере и ограниченном энергопотреблении.

Актуальность работы связана с возросшей необходимостью в автономных роботизированных транспортных системах во многих сферах, прежде всего - в автоматизации и роботизации производственных процессов и складского хозяйства.

Для обеспечения надежной и полностью автономной работы необходимо решить такие задачи, как определение собственного положения, избегание столкновений, обнаружение и объезд препятствий.

Один из наиболее перспективных подходов здесь связан с использованием видеоизображения и его анализа, однако его использование сталкивается с необходимостью разработки новых, точных, надежных, быстрых и достаточно сложных алгоритмов распознавания.

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

Для достижения поставленной цели необходимо рассмотреть следующие задачи:

1. Сравнительный анализ эффективности разработанных и существующих алгоритмов;

2. Исследования визуальной навигации и ее наиболее важных приложений для оценки местоположения;

3. Изучение использования итерационных алгоритмов определения местоположения с помощью визуального маркера;

4. Изучение использования фильтра частиц для оценки начального местоположения робота в известной среде;

5. Построить 3D-объект, используя структурированный свет, сканировать объект и получать карту всех его пространственных координат. Это используется во многих областях, где необходимо точно измерить объекты.

Научная новизна проведенных исследований и полученных результатов состоит в следующем:

1. Модифицированный алгоритм визуальной навигации на основе получения структуры из движения (SFM) с итерационным уточнением положения, отличающийся от известных аналогов тем, что в качестве особых точек используются признаки FAST, ORB, и каждая трехмерная точка будет связана с соответствующим дескриптором и сохранена в структуре (трехмерная карта), а также способностью работать с небольшим числом ориентиров в поле зрения и более точным определением положения.

2. Разработка алгоритма определения первоначального положения на основе модифицированного фильтра частиц, отличающегося существенно более высокой производительностью и способностью работать в реальном времени.

3. Разработка алгоритма обнаружения препятствий и избегания столкновений с помощью структурированного света с синусоидальными узорами, отличающегося от известных подходов использованием переменных частот.

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

Практическая ценность

1. Разработка комплексной навигационной системы, позволяющей автоматически переключаться между режимами работы в зависимости от состояния среды (неопределённость положения, изменение состава ориентиров, появление препятствий).

2. Разработка программного обеспечения визуальной навигации, реализующего предложенные методы SFM, фильтра частиц и структурированного света.

3. Разработка методики отладки алгоритмов визуальной навигации с использованием библиотеки OpenCV и пакетов Visual Studio, Matlab, ROS, Gazebo.

4. Результаты диссертационного исследования были использованы компанией КАМАЗ при полунатурном моделировании, а также при разработке алгоритмов визуальной навигации автономных автомобилей.

Методы исследования. Для решения задач, поставленных в работе, используются методы компьютерного зрения, теория цифровой обработки сигналов, линейная алгебра, математический анализ, теория вероятностей, фильтрация Калмана, фильтрация частиц, итерационный алгоритм, локализация Монте-Карло и методы структурированного света для создания 3D-объекта. Экспериментальное исследование проведено методом математического моделирования в среде MATLAB версии R2018b-R2021b, Visual Studio (c++) 2019-2022, библиотека OpenCV, библиотека облаков точек.

Положения, выносимые на защиту

1. Разработка комплексной навигационной системы, позволяющей автоматически переключаться между режимами работы в зависимости от состояния среды (неопределённость положения, изменение состава ориентиров, появление препятствий).

2. Разработка программного обеспечения визуальной навигации, реализующего предложенные методы SFM, фильтра частиц и структурированного света.

3. Разработка методики отладки алгоритмов визуальной навигации с использованием библиотеки OpenCV и пакетов Visual Studio, Matlab, ROS, Gazebo.

Степень достоверности результатов исследований. Основные результаты, полученные в работе, обоснованы путем моделирования. Помимо этого, эффективность разработанных алгоритмов подтверждена путем сопоставления их характеристик с характеристиками известных из литературы алгоритмов.

Апробация работы. Основные положения и результаты диссертационной

работы докладывались на международном симпозиуме INTELS'2020, на всероссийских конференциях XLV, XLVI Академические чтения по космонавтике (Москва, 2021 г, 2022 г.); на научном семинаре кафедры "Системы автоматического управления" МГТУ им. Н.Э. Баумана (2022 г).

Публикации. По теме диссертации опубликовано 4 научные работы, 1 из них в изданиях, индексируемых Scopus,1 - публикация в журнале, входящем в Перечень ВАК Минобрнауки РФ и 2 из них - в конференциях XLV, XLVI Академические чтения по космонавтике.

Объем и структура диссертации. Диссертация состоит из списка сокращений, введения, пяти глав, заключения, рекомендаций и списка литературы. В начале каждой главы дается краткий обзор состояния соответствующих исследований. Общий объем диссертации составляет 104 страницы текста с 51 рисунком. Список цитированной литературы из 62 наименований.

Содержание работы

Работа содержит введение, шесть глав, заключение и список литературы.

Во введении к диссертации обосновывается актуальность темы исследования, показывается ее научная значимость, формулируются задачи работы, приводятся сведения о структуре и содержании работы, приводятся положения, выносимые на защиту.

В первой главе представлен обзор методов и технологий визуальной навигации, включая основные конструкции и типы.

Во второй главе представлена математическая модель восстановления трёхмерной сцены по двум проекциям с использованием эпиполярной геометрии.

В третьей главе представлен подход основанный на методе ББМ.

В четвертой главе представлен подход основанный на итеративном алгоритме для оценки местоположения робота.

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

В шестой главе представлены методы структурированного освещения для построения 3D-объекта.

В заключении сформулированы основные результаты, полученные в диссертационной работе. По результатам исследования предложены некоторые рекомендации по улучшению навигации и построению 3D карты.

Заканчивая введение, выражаю глубокую и искреннюю признательность моему научному руководителю, к.т.н. Бобков Александр Валентинович, под чутким руководством и при непосредственном участии которого выполнена данная работа.

Особенно хочу поблагодарить доктора технических наук заведующего кафедры ИУ1 Константина Авенировича Неусыпина за постоянную помощь, внимание и интерес к работе.

Я высоко ценю замечания и советы доктора технических наук Евгения Михайловича Воронова, который внимательно рассмотрел и дал свои комментарии к исследовательской работе.

ГЛАВА 1. ОБЗОР ЛИТЕРАТУРЫ И ПОСТАНОВКА ЗАДАЧИ

ИССЛЕДОВАНИЯ

1.1 Системы визуальной навигации

За последние двадцать лет опубликовано множество статей и предложено множество алгоритмов для оценки положения и ориентации в зависимости от компьютерного зрения. Этот раздел включает в себя обзор методов локализации на основе зрения и их историческую классификацию. В качестве вклада предлагается новый метод классификации.

В системах локализации на основе изображений особенности окружающей среды обычно представляются в виде карты. В последнее время нейронные сети использовались в качестве инструмента для представления карты, но этот инструмент имеет некоторые трудности, поскольку пользователи не могут легко получить к нему доступ или проверить его, когда положение истинности недоступно. В [1] для решения этой проблемы предлагается метод генеративной карты. Метод предлагает совместное использование генеративной модели с дополнительными датчиками на основе фильтра Калмана. В [2] предлагается система дополненной реальности AR(Augmented Reality) для управления объектами с использованием метода локализации в помещении. Этот метод оценивает положение и ориентацию пользователя в помещении. Это зависит от объединения точки зрения пользователя с информационным моделированием зданий (BIM) (Building Information Modeling) с использованием вычислений глубокого обучения. В [3] предложен новый метод сопоставления 2D-3D с небольшими вычислениями. Этот метод зависит от повышения производительности регистрации на основе квантования визуального словаря и поиска соответствия по приоритетам.

Изучение влияния функции потерь на производительность нейронных сетей проведено в [4] в контексте задачи локализации RGB-камеры. Наблюдается, что на захваченное изображение влияют взаимные изменения поворота и положения, и,

таким образом, создается новая модель путем добавления условия потерь к функции потерь сети регрессии положения для повышения производительности. В другом исследовании [5] была предложена система позиционирования на основе маркеров дополненной реальности в помещении с использованием информации от маркеров дополненной реальности, размещенных в окружающей среде, чтобы помочь людям с ограниченными возможностями двигаться с помощью смартфонов. Для уменьшения накопленных ошибок (Inertial Navigation System) (INS) зрительно-навигационная система реализована на одном чипе, основанном на анализе навигационного механизма в мозгу крысы [6].

Система навигации с использованием фильтра Калмана предназначена для слияния инерциального измерительного блока (Inertial Measurement Unit)(IMU) (состоящего из гироскопов, акселерометров и магнитометров) и стереозрения (СЗ) [7]. Результаты эксперимента показали, что среднеквадратическая ошибка локализации составляет менее 0,05 м. В [8] разработан метод отслеживания ориентира с использованием стереозрения путем выделения характерных точек с помощью детектора CenSurE (Center Surround Extremas for Realtime Feature Detection and Matching [9]) и дескриптора FREAK (Fast Retina Keypoint [10]). Кроме того, для удаления выбросов используется RANSAC (консенсус RANdom SAmple). Эти ориентиры отслеживаются с помощью визуальной одометрии, метода сопоставления шаблонов с использованием ZNCC (ZeroMean Normalized Cross-Correlation) (нуле-средненормализованная взаимная корреляция) и прогнозирования движения с помощью UKF (Unscented Kalman Filter )(фильтр Калмана без запаха).

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

Для обнаружения препятствий и обхода их в [12] предложен алгоритм. Этот алгоритм основан на поиске карты глубины изображения, снятого камерой Kinect, и сопоставлении ее с реальными координатами. Предложенный алгоритм встроен в raspberry pi 2, протестирован в помещении и показал надежность и эффективность.

Обзор существующих систем стереозрения для получения трехмерных изображений из реального мира представлен в [13]. В контексте данного обзора авторы представили проблемы, разработанные в программно-аппаратных средствах КА, и пути их решения. Однако в [14] предложен новый подход к локализации наземной цели с UAV (Unmanned Aerial Vehicle) с использованием движка 3D Terrain. Этот подход зависит от перемещения виртуальной камеры внутри движка и последующего сопоставления двух изображений для оценки 3D-координат. Набор требований к построению карт и самолокализации в контексте навигации по изображениям сформулирован в [15].

На основе этих требований представлен метод построения карты путем выбора наиболее подходящих изображений для навигации. Другой метод, который может использовать несколько изображений запроса, предлагается для улучшения самолокализации. Оба метода моделируются с использованием сетевого потока и решаются с помощью программ выпуклых квадратов и конусов второго порядка соответственно. В [16] предложен алгоритм отслеживания автономным подводным роботом признаков на нескольких участках траектории. Этот алгоритм использует фильтрацию «короткоживущих» признаков и метод визуальной одометрии со стереоизображениями. Для визуальной автоматической навигации и посадки на Луну предлагается гибридный алгоритм метода регистрации изображений с использованием обобщенного преобразования Хафа [17]. Этот алгоритм сравнивает наблюдаемое изображение и карту, представленную в виде набора кругов, и позволяет избежать выделения дополнительных признаков.

1.2 Методы классификации локализации с помощью технического зрения

Существует множество классификационных исследований [18] методов, которые зависят от локализации на основе изображений. Эта классификация основана на использовании датчиков технического зрения в качестве автономного датчика или его интеграции с другими датчиками (INS, GPS). В этом параграфе мы представим историческую классификацию методов позиционирования на основе зрения. [19] Включает анализ методов определения местоположения, основанных на видении движения роботов до конца 1990 -х годов, когда эти методы были разделены на две основные группы: позиционирование в помещении и позиционирование на открытом воздухе.

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

Методы позиционирования на основе карты зависят от обеспечения системы картой, состоящей из набора функций (особенностей или отличительных точек), которые, как ожидается, будут присутствовать в процессе позиционирования.

Процедура определения местоположения на основе карты выполняется в четыре основных этапа: изображение с камеры, обнаружение объектов на текущем изображении, выполнение процесса сопоставления объектов на текущем изображении с объектами на карте, и, наконец, вычисление положения в зависимости от наблюдаемых объектов и их положения на карте.

С другой стороны, методы локализации без карты включают в себя все системы, определяющие положение без какого-либо предварительного описания окружающей среды. Этот процесс позиционирования зависит от элементов (признаков), наблюдаемых в окружающей среде, и не требует создания карты.

В ссылке [20] методы позиционирования были подробно изучены, где были предложены различные классификации. В этом исследовании проводится различие между двумя типами без карты и с картой, некоторые из этих методов могут применяться в помещении или на открытом воздухе. Согласно этому исследованию,

методы определения местоположения с помощью карты делятся на три группы: первая группа - это те, которые используют метрические карты, этот метод не может построить карту, он только использует ее, и система позиционирования должна быть оснащена с ним перед навигацией. Вторая группа — это те, кто строит карту. Картостроение. Этот метод исследует окружающую среду и автоматически строит карту во время онлайн-навигации или перед навигацией (в автономном режиме). Третья группа, зависящая от топологических карт, этим методом представляет среду в виде топологического графа, который состоит из группы узлов и связей. Системы позиционирования на основе карты топологии окружающей среды представляются в наглядном графическом виде [21] [22]. Другая предложенная классификация позиционирования на основе видения содержится в статье [23], где проводится различие между локальным (также называемым инкрементным) и глобальным (абсолютным). В методе глобального позиционирования транспортное средство позиционируется без каких-либо предварительных сведений о его местоположении.

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

Локальный метод предполагает известное исходное положение движущегося объекта, и текущее положение постепенно рассчитывается путем оценки движения, которое выводится из сравнения текущего изображения с предыдущими захваченными изображениями.

Методы позиционирования также делятся на основе зрения в соответствии с конфигурацией датчиков технического зрения (количество камер и способ их размещения). Большинство систем полагаются на одну монокулярную камеру, как в статье [24] [25] [26] [27] или две камеры [12] [28]. В случае одной камеры параметры движения снабжены коэффициентом масштабирования, который является

результатом преобразования 2D/3D, поскольку каждая бинарная точка на плоскости изображения является проекцией бесконечного числа точек 3D мира [27]. Чтобы преодолеть проблему масштабного фактора, используются бинокулярные камеры. Это позволяет найти 3D-координаты мировой точки с помощью метода триангуляции, основанного на знании базовой линии между двумя камерами. Однако недостаток стереокамер по сравнению с монокулярными камерами в основном связан с дополнительными затратами на программное и аппаратное обеспечение. Кроме того, в крупномасштабных средах изображения, снятые камерой, могут содержать объекты, расположенные слишком далеко. Обработка этих изображений не позволяет восстановить значения глубины, если базовая линия сте-реокамеры составляет несколько метров. Как правило, это невозможно в случае небольших платформ, таких как мини-беспилотные летательные аппараты. Тринокулярные (три камеры) конфигурации также существуют, но в основном используются для локализации подводных аппаратов, учитывая трудности, возникающие в подводных условиях [29].

Выделяют две основные тенденции в визуальной навигации: SLAM (одновременная локализация и картирование) [22] [26] [28] [30] и оптический калькулятор расстояния. Тем не менее, эта классификация игнорирует некоторые тенденции, которые можно считать важными, особенно в приложениях навигации на большие расстояния, таких как навигация летающих объектов, которые используют доступную модель Земли, такую как трехмерная модель местности. Эта классификация также объединяет методы SLAM и SFM (Структура из движения) [31], поскольку каждый из них строит модель и полагается на нее для расчета местоположения.

Между SLAM и SFM существует большая разница, поскольку построение модели с помощью SFM осуществляется до навигации, а в методе SLAM модель достигается во время навигации. Можно согласиться с классификацией в [18], так как она исключает классификацию, основанную на идее карты, поскольку в большом

проценте исследований и приложений карта используется без возможности классифицировать ее как конкретный метод. Поэтому предлагается новая классификация, которая зависит от упомянутых классификаций [18], как показано на Рис. 1.1. Кроме того, новая классификация различает две группы. Первая группа использует ЭБ-модель без построения SD-модели. Система позиционирования снабжена 3D -моделью перед навигацией (3D-модель местности из Google Earth) [14]. Вторая группа — это методы, которые исследуют окружающую среду и автоматически строят SD-модель либо во время навигации в режиме онлайн SLAM, либо перед навигацией в режиме оффлайн SFM. Методы, которые не используют какую-либо ЭБ-модель, в основном связаные визуальной одометрией. В данной работе в схему классификации был также добавлен новый метод построения карты с использованием структурированного света [32] [ЭЭ].

Рис. 1.1. Классификация методов локализации с помощью зрения

1.3 Первый предложенный метод

Как упоминалось ранее, это исследование проводится только на основе обзора и одной видеокамеры для оценки местоположения и использования метода SFM для представления трехмерной среды перед началом навигации. Причина выбора этого метода заключается в том, что характеристики 3D-модели приемлимы с точки зрения точности и скоростных характеристик при расчете положения. Эта модель строится до навигации. Общая блок-схема алгоритма реализации показана на Рис. 1.2. Схема включает в себя две отдельные фазы: фазу обучения (офлайн) и фазу навигации (в реальном времени). На этапе обучения создается 3D-модель, которая соответствующим образом сохраняется в базе данных. Первоначально с камеры собираются несколько кадров, так что эти кадры перекрываются, затем для каждого кадра извлекаются и описываются особенности. Сопоставление (2 D / 2D) применяется между элементами в каждом из кадров. Каждая точка совпадающих пар точек является проекцией одной и той же трехмерной точки, и таким образом находятся три координаты местоположения каждого объекта (точки). В конце этого этапа каждая трехмерная точка связывается с соответствующим дескриптором и сохраняется в структуре (трехмерные точки и дескрипторы). Эта структура выражает трехмерную карту сцены с ее характеристиками. Этап навигации осуществляется в режиме реального времени. Выполняется захват текущего изображения, затем извлекаются и описываются ключевые точки этого изображения. Затем выполняется операция сопоставления 2 D / 3D между элементами в 2 D-изображении запроса и элементами в структуре 3D-карты. После процесса сопоставления вычисляется положение текущего изображения. Это означает вычисление трех координат x, у, z и углов поворота по рысканью, тангажу, крену.

Первым шагом в любом методе локализации по видеоизображению является расчет внутренней матрицы камеры. Набор инструментов MATLAB используется для вычисления внутренней матрицы и матрицы искажений. Далее выполняются два этапа метода: построение 3 D-структуры и навигация.

Рис. 1.2. Блок-схема предлагаемого метода 1.4 Выводы по главе 1

В этой главе были описаны различные типы систем визуальной навигации. С учетом преимуществ и недостатков архитектур, предложенных в литературе, формируется постановка задачи исследования. В этой главе мы представили методы классификации позиционирования на основе изображения и используемых инструментов. Для их реализации была представлена собственная классификация этих методов, основанная на недавних статьях.

Была также представлена блок-схема первой предложенной системы, ко-торая реализована на основе построения карты на этапе обучения, а затем бы-ла использована на этапе навигации для определения положения на основе функций отслеживания особых точек (Tracking local featues).

Список литературы диссертационного исследования кандидат наук Алхатиб Маджи, 2023 год

т - - - -

Т - » 1 \ I , - »

у —• - — . - — -

• ✓ • » > - « • / -

% / # • ✓ 0

0

Рис. 2.6. Разделение окна на 4*4 квадрата

-В каждом поле вычислим гистограмму направления градиента, взвешенную по амплитуде градиента с шагом 45 градусов, т.е. с восемью градиентами, как показано на Рис. 2.7.

Рис. 2.7. Гистограмма направления градиента

-В итоге соберем значения гистограммы для получения вектора длиной 16*8=128, описывающего выделенную точку так, как показано на Рис. 2.8.

Рис. 2.8. Вектор дескриптора ключевой точки 2.4.3 Ускоренные надежные функции (Speeded Up Robust Features, SURF)

Метод SURF использует фильтры квадратной формы в качестве аппроксимации сглаживания по Гауссу. (Подход SIFT использует каскадные фильтры для обнаружения масштабно--инвариантных характерных точек, где разность гауссианов (DoG) вычисляется постепенно на масштабированных изображениях). Фильтрация изображения с помощью квадратного окна выполняется намного быстрее, если используется интегральное изображение:

x У

s (x, У) = YL! ( x, y)

i=0 J=0

(2.13)

Сумма исходного изображения внутри прямоугольника может быть быстро оценена с помощью интегрального изображения, требующего оценки в четырех углах прямоугольника.

Метод SURF использует детектор пятен, основанный на матрице Гессе, для поиска точек интереса. Определитель матрицы Гессе используется как мера локального изменения вокруг точки, и выбираются точки, где этот определитель максимален. В отличие от гессианско-лапласианского детектора Миколайчика и Шмида, SURF также использует определитель гессиана для выбора масштаба, как и Линдеберг. Для точки p = (x, y) на изображении I матрица Гессе H (p, о) в точке p и масштабе о равна:

H (p,&) =

rL„(p,&) Ly(p,&f

VLxy (P^) Lyy (A^O

(2.14)

где Lxx(p, а) и т. д. — свертка второй производной гауссиана с изображением I(x,y) в точке p. Блочный фильтр размером 9*9 представляет собой аппроксимацию гауссовы с а = 1,2 и представляет самый низкий уровень (наивысшее пространственное разрешение) для карт отклика пятна.

2.4.4 Binary Robust Independent Elementary Features (BRIEF)

Дескриптор основан на использовании серии двоичных чисел, поскольку он сравнивает плотность 128 пар пикселей в окне, окружающем выделенную точку. Если яркость первого пикселя больше, яркости второго, то помещаем в серию 1, иначе 0. Пары пикселей выбираются случайным образом, дескриптор BRIEF чувствителен к шуму, поэтому перед вычислением радиуса дескриптора мы применяем фильтр среднего значения.

Медианный фильтр обычно применяется к окну 9*9 и окну дескриптора 48*48 вокруг выделенной точки. Дескриптор является переменным при вращении, но он быстр в процессе сопоставления, поскольку использует расстояние Хэмминга.

Мы определяем тест т на наборе p размера S * S как:

h if p(x) < p(y) ^ * y) = iofp(x),p(y) ^

где p(x) — интенсивность пикселя в сглаженной версии p при x = (u, v) Т. Выбор набора из nd (x, y) пар местоположений однозначно определяет набор бинарных тестов.

Мы принимаем наш дескриптор BRIEF за n-мерную битовую строку:

fnd (p) = X 2';x', У) (2.16)

2.4.5 Ориентация по FAST и поворот по BRIEF (ORB)

Ориентация по FAST и поворот по BRIEF (ORB)F (Oriented FAST and rotated BRIEF, ORB), неизменный двоичный дескриптор с чередованием, что является улучшением как алгоритма BRIEF, так и алгоритма FAST.

Между BRIEF и ORB есть два отличия. ORB использует метод расчета направления характерной точки так, чтобы дескриптор оставался неизменным при вращении, используя статический момент первого порядка и нулевой момент. Рассчитывать центр тяжести из следующего соотношения:

= X xyxPyqi (x, У) (2.17)

pq

Направление характерной точки (направление луча, соединяющего центр окна и центр тяжести) можно вычислить из следующего соотношения:

С = (^ ^ (2.18)

moo moo

Пара образцов выбирается оптимальным образом, тогда как в методе BRIEF они выбираются случайным образом.

Маркировка пар образцов: есть две характеристики пар образцов:

• Некоррелированные: то есть каждая новая пара дает новую информацию для дескриптора[35].

• Большая разница между парами точек.

Чтобы отметить пары образцов, нужно выполнить следующие действия. 1. Найти все пары в окне, окружающем выделенную точку.

2. Располагим тестые точки на относительно расстояния 0,5 от среднего и размещим их в пучке Т.

3. Поместим первый тест в результирующий луч R и удалим его из луча T

4. Сравним следующий тест на Т-луче балке со всеми тестами на R- луче, и если результат сравнения больше определенного предела, то добавить тест в луч R.

5. Повторяем предыдущий шаг до тех пор, пока длина луча R не станет равной 255.

2.5 Выводы по главе 2

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

Мы выбрали детектор FAST и дескриптор ORB в нашем приложении, чтобы достичь условий сложности и скорости выполнения на среднем уровне между остальными алгоритмами.

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

ГЛАВА 3. ВИЗУАЛЬНАЯ ЛОКАЛИЗАЦИЯ С ИСПОЛЬЗОВАНИЕМ SFM

Метод определения структуры из движения SFM основан на использование изображений одной камеры для оценки местоположения. Метод ББМ используется для представления трехмерной среды перед началом навигации. Причина выбора этого метода заключается в том, что характеристики 3D-модели подходят с точки зрения точностных и скоростных характеристик при расчете положения. Эта модель строится перед навигацией. Алгоритм включает в себя две отдельные фазы: фазу обучения (в автономном режиме) и фазу навигации (в реальном времени). На этапе обучения строится 3Б-модель и соответствующим образом сохраняется в базе данных. Изначально с камеры собирается несколько кадров так, чтобы эти кадры перекрывались, затем для каждого кадра извлекаются и описываются признаки. Сопоставление (2D/2D) применяется между объектами в каждом из кадров. Каждая точка совпадающей пары точек является проекцией на одну и ту же точку 3D, таким образом заключаются три координаты местоположения каждого объекта (точки). В конце этого этапа каждой 3D -точке сопоставляется соответствующий дескриптор и сохраняется в структуре (3D -точки и дескрипторы). Эта структура выражает трехмерную карту сцены с ее особенностями. Этап навигации выполняется в режиме реального времени. Первоначально заватывается изображение с видеокамеры. Затем извлекаются и описываются ключевые точки этого изображения. Затем выполняется операция сопоставления 2D/3D между объектами в запросе 2D -изображения и объектами в структуре 3D-карты. После процесса сопоставления вычисляется позиция текущего изображения. Это означает вычисление трех координат х, у, z и углов поворота: рыскание, тангаж, крен.

3.1 Построение 3D структуры

Концепция определения структуры объекта по его движению (SFM) известна в области компьютерного зрения как определение относительного положения ка-

меры и трехмерных структур из набора изображений камеры. В стереовидении матрица движения между камерами известна, но в SFM она должна быть рассчитана. Первым шагом в процессе SFM является поиск матриц движения между камерами. Все матрицы движения для обучающих изображений сохраняются в файле, чтобы использовать их для проверки положения оценки на этапе навигации. Находя матрицу движения камеры, мы имеем в виду матрицу поворота и матрицу перевода из одного места в другое. После нахождения соответствующих точек (с помощью детектора FAST [34] и дескриптора ORB [35]) в каждом из двух изображений вычисляется фундаментальная матрица, а затем выполняется нахождение существенной матрицы с использованием внутренней матрицы камеры, захватывающей изображения [36]. После этого применяется разложение по сингулярным числам (Singular Value Decomposition, SVD) существенной матрицы, затем матрица внешней камеры может быть вычислена с использованием метода Хартли и Циссермана [37].

0 4 ty 1

E = [t]XR, [t]x = tz 0 4 , E

,-ty tx 0 У

[ 0 -1 0л [k 0 0 1

W = 1 0 0 , D = 0 0

V 0 0 1 У V 0 0 k у

, E = SVD( E) = UDVT,

(3.1)

R = UWVT, t = U (0 0 1)T

(3.2)

Как видно из двух изображений, обнаружена одна матрица камеры, поэтому другая матрица камеры считается канонической (без вращения и без перевода) и записывается как:

г\ 0 0 0Л

Р = (R t) =

0 10 0 0 0 10

(3.3)

Вышеприведенные уравнения объясняют, как построить сцену из двух изображений. Наивно получать параметры камеры и больше 3D -точек, применяя тот же предыдущий процесс. Есть несколько способов исправить тройные точки сцены из нескольких изображений. Один из этих методов известен как Perspective N-Point (PNP), который используется для нахождения нового местоположения камеры на основе вычисленных ранее трехмерных точек и проецирования на 2D-изображениях, а также метод RANSAC для удаления точек выбросов. Функция resolpnp применяется из библиотеки OpenCV.

Одна из наиболее важных частей метода SFM известна как Bundle Adjustment (BA), когда все данные собираются для создания согласованной модели с оптимальным расположением трехмерных точек и местоположением камеры, минимизируя ошибку проекции. Применяется фильтр для удаления точек вне сферы вокруг 3D-карты, и получаемые результаты улучшаются. Конечный результат метода SFM в нашем случае: структура данных трехмерных точек с дескриптором, соответствующим каждой точке (размерность дескриптора ORB равна 32). Это означает, что каждая трехмерная точка связана с вектором из 32 ячеек, описывающих ключевую точку. В реализации SFM библиотека Point Cloud Library (PCLv1.81) используется для построения и визуализации сцены. Разработан программный проект [26] для построения трехмерной карты. Блок-схема этапа обучения показана на Рис. 3.1.

Рис. 3.1. Блок-схема этапа обучения 3.2 Этап навигации

Оценка положения и ориентации на этапе навигации основана на сопоставлении 2D / 3D между наблюдаемым 2Б-изображением запроса и 3D-картой, построенной в процессе SFM. Для определения местоположения предпринимаются следующие шаги:

• Обнаружить и построить описание признаков входного изображения с помощью детектора FAST и дескриптора ORB.

• Сопоставить дескрипторы входного изображения с трехмерной картой, которая хранится в древовидной структуре, для ускорения поиска.

• Отфильтровать двумерные точки из входного изображения так, чтобы точки были распределены по всему изображению, поскольку иногда точки с ближайшим

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

• Применить метод Perspective N Point (PNP) с фильтром RANSAC, чтобы найти матрицу проекции между текущим изображением и 3D-картой.

• Извлечь матрицы вращения и матрицы переноса из матрицы проекции в соответствии уравнениями:

P3x4 = K ( R3x3 t3x1 ) (3.4)

• Вычислить местоположение входного изображения, на которое ссылается первое захваченное изображение (первое изображение считается центром координат, чтобы облегчить построение карты с местоположением камеры).

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

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

К =

ft Л

cx

СУ

V t cx У

= - RTt

(3.5)

• Определить углы (рыскание, тангаж, крен)

Чтобы определить углы камеры с использованием углов Эйлера [38], матрица вращения раздевается на три матрицы, каждая матрица выражает вращение вокруг одной из координатных осей, как в следующем уравнении:

R = Rx (0) Ry в) R в) =

1 о о

о

c

0 - ^ c

Со

о -s„

о 1 о

У V S2

о

С

2 yv

R =

С2 С1

S1S2C3

V C1S2

C1S3

S1S3

C2 S3 S1S2 S3 + C1C3

C1S2 S3

S1C2

C1C2 У

m,

00

m,

01

m,

C1 s1 01

-s1 C1 0

0 0 1У

Л

02

m

m

V m20

10 '"11 m

m

12

21

m

(3.6)

22 У

s

1

2

• Определить вращение камеры вокруг боковой оси (тангаж) по уравнению:

вх = arctan 2(т 2, ш22) = arctan 2( яхс2, схс2) (3.7)

• Определить вращение камеры вокруг вертикальной оси (рыскание):

= arctan 2(-т02, с2 ) = arctan 2(я, с2) (3.8)

• Определить вращение камеры вокруг продольной оси (крен):

03 = arctan 2(т01, т00) = arctan 2(с2я3, с2с3) (3.9)

Блок-схема этапа навигации показана на Рис. 3.2.

Рис. 3.2. Блок-схема этапа навигации

3.3 Эксперименты

Алгоритмы были реализованы на языке C ++ с использованием среды Visual Studio 2017 и QT5.14 для построения графического интерфейса на ноутбуке под управлением Windows 10 с процессором Core i7 2,2 ГГц. Метод апробирован на изображениях в помещении в нашей лаборатории (5м х 6м). Сцена в помещении представлена облаком точек из изображений. Размеры изображений 752x500. Набор изображений используется для формирования облака точек, которые снимаются по оси x на расстоянии 30 см между изображением и следующим изображением. Алгоритм построения карты применяется к предыдущему набору изображений, и в результате получается облако трехмерных точек с положениями камеры, соответствующими каждому из обучающих изображений, представленных в виде пирамид, как показано на Рис. 3.3.

Рис. 3.3. 3Б-карта и расположение обучающих изображений

После этого облако точек (состоящее из 8 296 3D -точек) подвергается 3D-фильтрации для удаления точек-выбросов, находящихся за пределами сферы. Центр этой сферы является центром облака, а ее диаметр выбран так, чтобы исключить выбросы. После применения фильтра количество точек становится равным 8215. В процессе формирования облака точек оценивается местоположение камеры (по принципу координат считается первое изображение). Как упоминалось ранее, при создании облака точек расположение обучающих изображений сохраняется

при построении 3D-карты. На этапе навигации расположение обучающих изображений не совпадает с местоположением на этапе обучения. Эта ошибка называется ошибкой обучения и указывает на качество обучения. Ошибка обучения — это ошибка между измеренным значением (с использованием платформы (механического инструмента)) и оценочным значением при применении сопоставления 2Б/3Б.

Случайным образом множество изображений на оси х с различными углами в пределах ± 10° измеряются платформой и алгоритмом. Полученный результат выглядит следующим образом как показано в Таблице 1.

• Ошибка в оценке значения координаты х менее 2 см.

• Ошибка в оценке значения координаты у составляет менее 4 см.

• Погрешность значений рысканья, тангажа, крена менее 2 градусов.

Таблица 1

Результаты обучения изображений и сопоставления ошибок 2 Б-3Б

Измерено х (см) Оценка соответствия 2Б-3Б х (см) Ошибка (см) Измеренный Рыскание (градусы) Оценка согласования 2Б-3Б рыскания (градусы) Ошибка (градусы)

0 0 0 0 -0.01 0.01

30 29.33 0.7 0 0.1 0.1

60 60.14 0.1 0 0.33 0.33

90 89.81 0.8 0 0.48 0.48

120 119.64 0.4 0 0.63 0.63

150 150.19 0.20. 0 0.8 0.8

180 180.20 0.2 0 0.97 0.97

3.4 Выводы по главе 3

Ограничением этого исследования является то, что оптимизация получения изображений с использованием и перекрытия между ними по-прежнему дости-

гается методом проб и ошибок. Это нетривиальное исследование, потому что разностное получение изображений ведет себя по -разному в разных средах. Было бы целесообразно дальнейшее изучение этого вопроса.

В следующей главе будет представлен новый метод определения положения и ориентации камеры с использованием визуальных маркеров.

ГЛАВА 4. ОПТИМИЗИРОВАННАЯ ОЦЕНКА ПОЛОЖЕНИЯ КАМЕРЫ С ИСПОЛЬЗОВАНИЕМ ИТЕРАТИВНОГО АЛГОРИТМА НА ОСНОВЕ

ВИЗУАЛЬНОГО МАРКЕРА

Существует несколько методов оценки положения, основанных на обработке изображения, снятого одной или несколькими камерами, например обнаружение признаков, перспективная проекция (PNP) и ортогональная проекция (копланарная позиция). Методы, основанные на обнаружении признаков, позволяют только оценить ориентацию. Эти методы, такие как перспективные проекции, чувствительны к шуму в изображении. Это приводит к росту ошибок в оценке местоположения и ориентации. В то же время существующие проекционные методы позволяют оценивать положение приближенно и итеративно. Задачу PnP можно разделить на линейные алгоритмы и нелинейные итерационные алгоритмы [25]. Линейные алгоритмы зависят от прямого линейного преобразования (DLT), EPnP (Enhanced) [39]. Недостатками алгоритма DLT являются низкая точность и наличие точек-выбросов. Алгоритмы DLS (прямые наименьшие квадраты) отличаются относительно высокой точностью, но низкой вычислительной эффективностью. EPnP — это компромисс между скоростью и точностью. К нелинейным алгоритмам относятся LM (метод Левенберга-Марквардта), POSIT (положение по ортографическим проекциям и масштабирование с итерациями) [40].

Для решения задачи PNP было создано несколько алгоритмов. Количество возможных решений варьируется в зависимости от количества точек n с учетом относительного расположения, например, если они расположены на прямой или в одной плоскости.

Решений много в случае n = 3 и до четырех решений [41] в случае, если точки не расположены на одной прямой (неколлинеарно).

В случае n = 4 теоретически существует только одно решение, если бы точки не находились в одной плоскости [42]. Но в том случае, если они попадают в одну плоскость, то всегда есть два решения.

В этой работе будет рассмотрен итерационный алгоритм, основанный на минимизации квадратичной ошибки методом Ньютона для оценки положения и ориентации на основе изображения, снятого только одной камерой. Будем искать положение, которое минимизирует квадрат ошибки предсказанного местоположения признаков изображения до измеренного местоположения. Для вычисления ошибки оценки положения были сгенерированы синтетические изображения, как в статье [40]. Рассматривается двумерный объект (квадрат), содержащий множество различных точек (не менее четырех точек). Погрешность алгоритма рассчитывается для многих относительных расстояний между камерой и объектом (2,5,10,20) и трех уровней шума (уровень 0, уровень 1 и уровень 2). При уровне шума изображения 0 шум отсутствует. При уровне шума изображения 1 действительные числа, вычисленные для координат перспективных проекций, округляются до целочисленных положений пикселей. При уровне шума 2 эти правильные положения нарушаются вертикальными и горизонтальными искажениями в -1,1 пикселя.

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

Цель состоит в том, чтобы восстановить (оценить) относительное положение камеры (три степени свободы) и вращение (еще три степени свободы) относительно жесткого объекта в системе координат.

Второй подход основан на итеративном алгоритме для оценки положения и ориентации камеры.

4.1 Метод Гаусса-Ньютона

Метод Гаусса-Ньютона - это алгоритм решения нелинейных задач наименьших квадратов. Преимущества метода GN заключается в том, опирается только на

градиент ошибки и не требует вычисления матрицы вторых производных (гессиана). (}1}) является хорошим приближением гессиана, тогда сходимость квадратичная [43].

4.1.1 Определение проблемы

Дано:

1 .Имеется объект (3D модель);

2.Известна геометрия модели (3D точки от объекта);

3.Найдены соответствующие особенности на изображениях (2D) с помощью детектора этой функции.

-Требуетя найти:

Положение и ориентация объекта относительно камеры.

-Предположения:

1 .Объект (3D модель) является жестким и имеет 6 степеней свободы;

2.Заранее известна калибровочная матрица камеры.

Найдем положение, которая минимизирует квадратичную ошибку предска-занного местоположения функций изображения относительно измеренного место -положения.

Пусть у = Г (х)

Здесь х вектор неизвестного параметра положения 6DOF (3 поворота, 3 перемещения).

Г функция, которая возвращает предсказанные точки у изображения из заданной позы х.

у0 вектор реально наблюдаемых точек изображения (количество точек изображения К).

г ~ л

X =

чл 02

V ^

У =

У1

У2

V УN.

(4.1)

Необходимо найти х, дающее минимум функции ошибки:

Е = / (х) - У0Г

Алгоритм:

Задать начальное положение x = xo. Вычислить у = f (х). Вычислить ошибку dy = у -У0. Вычислить якобиан f: J = df / dx, dy = Jdx dx = J-1 dy

функция проецирования трехмерных точек на изображение Вход:

-3D точки X = [X, Y, 7, 1] с использованием однородных координат. -х вектор 3D модели к камере (положение камеры). -К калибровочной матрицы камеры. Выход:

-р = [х, у] точки проецирования на изображения (2D точки).

(4.2)

Определим P3d набор 3D точек (количество точек N

X1 х2 • " ХМ

- У*

•■ 7 N

1 1 • •• 1

P = (xi У\ У 2 ■■■ % Ун)' (4-4)

Вычисление якобиана численно:

fx)^ f (x + ещ) -f (x)

dx; Sx '

J =

f

dxj

К К К

dxt dx2 дхм

К df2 df2

dx1 dx2

f f2

ft

N

dxx fix2

fix

M J

(4.6)

В нашем случае M=6 (3 поворота и 3 перевода). Выбирается e=0.00001;

столбец 1: J(:,1) = ( f(x+ dxi,P3d,K) - y )/e; : dxi= (e,0,0,0,0,0) столбец 2: J(:,2) = ( f(x+ dx2, P3d,K) - y )/e; : dx2= (0, e,0,0,0,0) столбец 3: J(:,3) = ( f(x+ dx3, P3d,K) - y )/e; : dx3= (0,0, e,0,0,0) столбец 4: J(:,4) = ( f(x+ dx4, P3d,K) - y )/e; : dx4= (0,0,0, e,0,0) столбец 5: J(:,5) = ( f(x+ dxs, P3d,K) - y )/e; : dxs= (0,0,0,0, e,0) столбец 6: J(:,6) = ( f(x+ dx6, P3d,K) - y )/e; : dx6= (0,0,0,0,0, e). Шаги алгоритма:

1. Задать начальное положение x^x0.

2. Вычислять y=f(x).

3. Вычислять ошибку dy=y-y0.

4. Вычислять Jacobean of f: J=df/dx,

5. Вычислять dy=Jdx.

6. Исключить dx, используя псевдоинверсию of J, (JT*J)-1, dx=(JT*J)-1JTdy.

7. Установить x^x+dx

Повторять шаги 2-7 много раз, пока изменение х не станет очень маленьким, например (10А-7).

Алгоритм применяется к случайным четырем точкам с семью итерациями. Проекция последних четырех обновлений точек показана на Рис. 4.1.

Рис. 4.1. Обновлять проекцию 3Б-точек ((4 точки)) на 2Б-изображение каждый

шаг

Алгоритм применяется к случайным десяти точкам с семью итерациями. Проекция последних четырех обновлений точек показана на Рис. 4.2.

100 200 300 400

100 200 300 400 500 600

100 200 300 400 500 600

Рис. 4.2. Обновлять проекцию 3Б-точек (10 точек) на 2Б-изображение каждый

шаг

4.2 Расчет ошибки оценки положения

Алгоритм протестирован при трех уровнях шума и четырех значениях масштаба.

• Ошибка в оценке положения при уровне шума = 0.

Угол возвышения изменяется с 10 до 90 градусов с шагом 10 градусов, а азимутальные углы изменяются с 0 до 350 градусов с шагом 10 градусов. На Рис. 4.3 показаны угол места и азимутальный угол. Средняя ошибка вычисляется при 36 азимутальных углах каждый раз для девяти углов места и четырех относительных расстояний (Масштаб = D / Размер) (2,5,10,20), как показано в [40]. Эти ошибки рассчитываются для четырех точек (вершин квадрата). Мы заметили, что погрешность увеличивается с увеличением относительного расстояния между объектом и камерой и максимальна, когда угол возвышения равен = 90 градусов. Среднее значение ошибки меньше 0,4, ее среднее квадратическое отклонение (ско) меньше 2,5, как показано на Рис. 4.4.

Object

Рис. 4.3. Угол возвышения и азимутальный угол для камеры

Рис. 4.4. Средняя ошибка местоположения камеры и ее СКО для угла возвышения, уровень шума = 0

• Ошибка в оценке положения при уровне шума = 1.

Мы заметили, что ошибка немного увеличивается с увеличением относительного расстояния между объектом и камерой. Среднее значение меньше 0,6, а СКО меньше 3, как показано на Рис. 4.5.

Рис. 4.5. Средняя ошибка местоположения камеры и ее СКО для угла возвышения, уровень шума = 1

• Ошибка в оценке положения при уровне шума = 2.

Мы заметили, что погрешность в значительной степени возрастает, поскольку уровень шума существенно не меняется с относительным расстоянием. Среднее значение меньше 1,7, а СКО меньше 6, как показано на Рис. 4.6.

Рис. 4.6. Средняя ошибка местоположения камеры и ее СКО камеры для угла возвышения, уровень шума = 2

Из предыдущего статистического процесса видно, что для относительных расстояний (2, 5, 10, 20) и углов места ошибка в оценке угла составляет менее 0,3 градуса при уровне шума = 1.

Ошибка в оценке угла увеличивается, а среднеквадратическое отклонение увеличивается с увеличением угла места. Максимальное значение в области надира (90 градусов) и увеличивается, когда на изображении нет шума. Такие изменения сложно обнаружить, так как они не вызывают существенного изменения контура тела в плоскости изображения, что приводит к росту погрешности оценки. Из предыдущего статистического процесса очевидно, что ошибка в оценке угла и его среднеквадратическое отклонение возрастают с увеличением относительного расстояния между камерой и телом. Оценка смещения менее чувствительна к шуму и изменениям относительного расстояния, чем оценка угла, поскольку относительная ошибка не превышает 3 процентов даже на самом высоком относительном расстоянии и самом высоком уровне шума. Ошибка обычно увеличивается по мере приближения к области надира по той же причине, что и при оценке угла.

4.3 Выводы по главе 4

В данной главе, представлен алгоритм Гаусса-Ньютона, который уточняет движение камеры. Из вышеизложенного можно сделать вывод, что возможно получить хороший результат в оценке положения камеры при применении алгоритма

Гауэсса Ньютена. Для этого должны быть распределены как можно больше к плоскости изображения. Мы реализовали алгоритм численно в MATLAB и протестировали его, генерируя изображения с различной высотой, азимутальными углами и изменяя относительное расстояние между объектом и камерой с тремя уровнями шума. Ошибка увеличивается с увеличением уровня шума и относительного расстояния. Для будущей работы мы предлагаем объединить визуальный маркер с окружающей средой для улучшения карты здания с использованием метода SFM или SLAM.

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

ГЛАВА 5. АВТОНОМНАЯ НАВИГАЦИЯ С ИСПОЛЬЗОВАНИЕМ

ФИЛЬТРА ЧАСТИЦ

Изучению мобильных автономных транспортных средств уделяется большое внимание в научных исследованиях, и автономная навигация является одним из наиболее важных элементов в этой области [44] [45].

В данной главе будут рассмотрены основные элементы автономной навигации и наиболее важные алгоритмы, используемые в этой области.

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

Автономность транспортного средства — это способность принимать решения и действовать самостоятельно [46].

Существуют разные уровни автономности, от транспортного средства, которым управляет человек из удаленного места с некоторыми простыми алгоритмами на борту, не позволяющими ему упасть со скалы, до полностью автономного транспортного средства без вмешательства человеком в управление.

Достижение полной автономности транспортного средства делится на два разных подхода:

1. Эвристический подход:

• Определенны практические правила или поведение.

• Оптимальность траектории не гарантируется.

• Не требует полной информации об окружающей среде.

2. Оптимальный подход:

• Требует большой информации об окружающей среде.

• Планирование траектории достигается за счет оптимизации.

Также можно использовать оба подхода для достижения глобальной цели. Основной сложностью автономной навигации является неизвестная окружающая среда.

Если окружающая среда постоянно меняется, модель окружающей среды

должна постоянно обновляться, что вызывает сложности в распознавание препятствий.

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

Последний шаг - действовать в соответствии с проложенным маршрутом, чтобы двигатели и исполнительные механизмы следовали по пути.

Мы чувствуем окружающую среду, понимаем, где мы находимся по отношению к определенным ориентирам. Обновляем модель окружающей среды новым набором информации, обновляем маршрут в соответствии с установленной целью, чтобы избежать новых препятствий, контролируем объект, чтобы он следовал по пути и так далее. Блок-схема автономной навигации представлена на Рис. 5.1.

Рис. 5.1. Блок-схема автономной навигации 5.1 Отображение

Отображение — это процедура моделирования окружающей среды и передачи информации роботу о том, где находятся препятствия в окружающей среде, а также положение препятствий и объектов в окружающей среде. Есть множество способов создания карт: • Непрерывная карта.

•Вероятностная карта. Наиболее распространенные из этих методов основаны на распределении Байеса, а другие методы включают фильтр Калмана и локализацию Монте-Карло.

• Дискретная карта: разделяет среду на мелкие ячейки или сетки.

Вначале необходимо определить график видимости. Граф видимости должен представлять собой С-пространство без разделения всего пространства на ячейки или узлы, далее создаются узлы только на краях и углах препятствий, а затем полученные узлы соединяются с другими видимыми узлами с помощью ребер. После применяется алгоритм поиска графа для нахождения кратчайшего пути.

5.1.1 АЛГОРИТМЫ АВТОНОМНОЙ НАВИГАЦИИ

Алгоритм — это набор инструкций по решению задач (двигаться к цели и избегать препятствия). Мы можем разделить алгоритмы по их свойствам:

• детерминированный или случайный: детерминированный алгоритм: тип алгоритмов, которые имеют одинаковый выход для одного и того же входа. Если роботу предоставить такую же среду, он даст такое же планирование пути. Случайный алгоритм, может иметь разные выходные данные для одного и того же входа.

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

• Точность или эвристика: точный алгоритм всегда дает наилучшее возможное решение, тогда как эвристический алгоритм пытается дать наилучшее решение, но нет никаких гарантий, что это лучшее решение.

• Полный перебор: проверяет все возможные решения, а затем выбирает лучшее.

• Жадный. Всегда выбирает лучшее локальное решение, а не лучшее глобальное.

• Динамическое программирование. Разделяет проблему на подзадачи и старается найти лучшее решение для каждой подзадачи.

Производительность алгоритмов зависит от многих параметров:

• Полнота: всегда можно найти решение или нет.

• Оптимальность: всегда находит лучшее решение или нет.

• Время: время выполнения алгоритма.

• Сложность пространства: объем памяти компьютера, необходимый для поиска решения.

• Поиск в ширину

Метод поиска в ширину (Breadth-first search,BFS) - это фундаментальный алгоритм поиска, используемый для исследования узлов и ребер графа. Он работает с временной сложностью (v + e) (количество ребер + количество вершин) и часто используется в качестве строительного блока в других алгоритмах. Преимущества: позволяет выполнить поиск кратчайшего пути на невзвешенном графе [47].

Шаги:

1. Назначить все узлы как непосещаемые.

2. Составить очередь Q.

3. Добавить начальный узел к Q (обнаружен).

4. Добавить соседние узлы к Q.

5. Назначить расстояние и родительский узел соседним обнаруженным узлам.

6. Удалить из очереди самый посещаемый узел.

7. Сделать следующий соседний узел посещаемым и найти его соседей.

8. Повторить шаги с 3 до тех пор, пока не останется не обнаруженных узлов.

• Алгоритм Дейкстры

Алгоритм Дейкстры основан на принципе релаксации (principle of relaxation), в котором более точные значения постепенно заменяют приближение к правильному расстоянию, пока не будет достигнуто кратчайшее расстояние. Алгоритм Дейкстры в отличие от BFS может применяться на взвешенном графе. Недостатки: высокая вычислительная сложность [48].

Шаги:

1. Присвоить бесконечное значение всем узлам и ноль начальному узлу.

2. Ввести список посещенных узлов.

3. В текущем узле вычислить стоимость (расстояние) от начала до каждого его соседнего узла. Если новая стоимость меньше предыдущей, заменить ее новым значением.

4. Сделать текущий узел посещенным.

5. Выбрать соседний узел с минимальной стоимостью и сделать его текущим.

6. Повторить действия с шага 3, пока не будет достигнут целевой узел.

7. От целевого узла отслеживать родительские узлы в обратном направлении, пока не будет достигнут начальный узел. Это будет кратчайший путь.

• Алгоритм А*

А * - это алгоритм информированного поиска или поиск лучшего первого, что означает, что он сформулирован в терминах взвешенных графов: начиная с определенного начального узла графа, он стремится найти путь к заданному целевому узлу, имеющему наименьший стоимость (наименьшее пройденное расстояние, наименьшее время и т. д.). Алгоритм Дейкстры исследует все узлы, тогда как А * не исследует все узлы [49]. Принцип работы в основном такой же, как у алгоритма Дейкстры, за исключением того, что есть дополнительная эвристика, как далеко мы должны идти. Для выполнения алгоритма определим:

О-стоимость - это расстояние от текущего узла до начального.

Н-стоимость - это расстояние от текущего узла до целевого узла.

Б-стоимость = О-стоимость + Н-стоимость.

Шаги:

1. Начать со стартового узла, сделать его текущим и поместите в закрытый список (посещенные узлы).

2. Развернуть соседние узлы до текущего узла и поместить их в открытый список (смежные узлы и еще посещенные).

3. Вычислить Б-стоимость соседних узлов и отметьте их родительские узлы.

4. Если новая G-стоимость меньше предыдущей G-стоимости узла, заменить ее новым значением G-стоимости.

5. Выбрать соседний узел с минимальной F-стоимостью, сделать его текущим и поместить в закрытый список.

6. Повторить действия с шага 3, пока не будет достигнут целевой узел.

7. От целевого узла отслеживать родительские узлы в обратном направлении, пока не будет достигнут начальный узлел. Это будет кратчайший путь.

У предыдущих алгоритмов есть недостатки, один из которых требует больших вычислительных ресурсов, когда граф слишком велик. Количество ячеек сетки увеличивается экспоненциально с увеличением размера области поиска.

Есть типы алгоритмов, которые решают эту проблему, но иногда не дают наилучшего решения, они лучше, чем детерминированные алгоритмы, позволяют найти путь быстрее.

Одним из таких методов планирования пути являются алгоритмы, основанные на выборке. Алгоритмы на основе выборки ищут путь без столкновений от начальной точки до целевой точки. Выборка может быть детерминированной или случайной. Если мы увеличим количество выборок, это увеличит разрешение, но увеличит сложность, поэтому необходимо пойти на компромисс. Алгоритмы на основе выборки позволяют избежать проблемы локальных минимумов и довольно быстро решают многие проблемы. Они не могут определить, что пути не существует, но у них есть вероятность отказа, которая уменьшается до нуля по мере того, как тратится больше времени. Далее рассмотрены некоторые из этих алгоритмов.

• Алгоритм быстрого изучения случайного дерева

Алгоритм быстрого изучения случайного дерева (Rapidly Exploring Random Tree, RRT) строит дерево с корнем в начальном узле. RRT предназначены для эффективного исследования путей в многомерном пространстве. Случайное дерево (Random Tree, RT ) случайным образом выбирает узел из дерева и добавляет ребро в случайном направлении, но RRT сначала выбирает целевую точку, а затем пыта-

ется добавить ребро от ближайшего узла в дереве к целевой точке. Алгоритм поиска свободного от столкновения пути от начальной позиции до позиции цели [50] выгядит следующим образом.

Шаги:

1. Инициализировать дерево, задав начальный узел.

2. Определить максимальный размер выборки.

3. Выбрать точку в пространстве состояний.

4. Найти ближайший к точке отбора пробы узла.

5. Создать новый узел на пороговом расстоянии от ближайшей точки.

6. Использовать планировщик локального пути, чтобы соединить ближайший узел с новым узлом.

7. Если движение происходит без столкновений, добавить новую точку в дерево, образуя ребро с ближайшим узлом.

8. Повторять шаги 3-7, пока новый узел не станет целевым узлом.

9. От целевого узла отслеживать родительские узлы в обратном направлении, пока не будет достигнут начальный узел. Это будет искомый путь.

• Алгоритм «быстро исследуемая случайная древовидная звезда»

Алгоритм «быстро исследуемая случайная древовидная звезда» (ИКТ*) является развитием алгоритма ИЯТ. Он перестраивая дерево для формирования кратчайших путей. В пределах определенного радиуса ищется узел, где может быть кратчайшее расстояние. Повторное подключение означает, что когда мы выбираем новый узел, то также видим соседние узлы, и если находим кратчайший путь, то меняем родительский элемент этого нового узла на узел, имеющий кратчайший путь [51].

5.2 Оценка положения и ориентации мобильного робота с помощью

фильтра частиц

Фильтр частиц похож на фильтр Калмана, это метод оценки состояния динамической системы, который принимает и обновляет текущее положение на основе

информации о движении команд управления и наблюдения. Преимущество фильтра Калмана в том, что он может работать с нелинейной функцией и негауссовским шумом. Фильтр частиц использует частицы или образцы, которые являются гипотезами для системы, находящейся в одном единственном состоянии. Например, если у нас есть тысяча частиц, это означает, что у нас есть тысяча предположений о вероятных местоположениях, в которых будет находится система (местоположение робота). Алгоритм работы фильтра частиц состоит из двух шагов, первый шаг -это шаг прогнозирования, который зависит от информации о командном движении, такой как скорость, а второй шаг - это поправка, которая учитывает наблюдение, насколько совпадает между местом наблюдения и реальным местоположением. Далее мы задается больший вес частице, которая имеет большее значение согласования. Затем выполнится повторная выборка частиц, чтобы ограничить количество частиц и исключить частицы с низким весом.

Задачу определения собственного положения можно сформулировать следующим образом. Имеется робот, блуждающий внутри здания. Робот имеет карту здания, поэтому он знает общую схему стен и мебели, но изначально не знает, где он находится на карте. Он может быть, где угодно, поэтому роботу необходимо использовать датчики и модели движения, чтобы оценить свое положение на карте. Например, у робота есть сканирующий лазерный дальномер (лидар), возвращающий расстояние до объектов в поле зрения. Датчик возвращает облако точек, но эти измерения зашумлены. Возможно сопоставить облако точек с картой окружающей среды, чтобы определить, где находится робот. В дополнение к измерениям с шумом, робот также имеет взвешенный точный расчет своего положения с использованием данных одометрии.

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

Есть два возможных способа определения местоположения: во-первых, имеются зашумленные данные лидара, которые определяют некоторые особенности окружающей среды и которые можно сравнить с картой, а во-вторых, имеются за-шумленные данные одометрии, которые оценивают пройденное роботом расстояние.

Чтобы понять, как фильтр частиц решает проблему местоположения, рассмотрим следующую ситуацию. Будем считать, что робот использует лидар, чтобы определить свое местоположение. Вначале местоположение робота на карте полностью неизвестно (Рис. 5.1). Можно сказать, что робот может находиться в любой точке открытого пространства с равной вероятностью. Представим, что у нас есть двоичная сетка занятости, как показано на Рис. 5.2.

Рис. 5.2. Карта бинарной сетки

Будем измерять сцену с помощью лидара и в результате робот узнает, есть ли перед ним препятствие или нет, а также расстояние до препятствия. Однако он не может определить точное местоположение, потому что стен много, и робот может стоять перед любой из них. Но он может знать, что это, вероятно, не угол и не дверь, и, вероятно, не середина комнаты, потому что стена находится близко. Датчики имеют шум, поэтому можно говорить лишь с наибольшей уверенностью, что робот находится в одном из этих мест. Нельзя с полной уверенностью сказать, что мы находимся не в центре комнаты, потому что возможно, что робот смотрит на временное плоское препятствие, которого не было на карте. Такая вероятность мала, но она всё же есть. Следовательно, после первого измерения можно сказать, что распределение вероятностей местонахождения робота будет выглядеть как показано на Рис. 5.3.

Рис. 5.3. Распределение вероятностей местонахождения робота

Робот все еще движется, чтобы увидеть другую деталь, например угол. Мо-жено начать сужать возможные местоположения робота, удаляя те из них, которые не удовлетворяют всем предыдущим измерениям. Итак, если робот повернет налево и увидит угол, а затем продолжит поворот и увидит другой угол, а затем увидит дверь, то сможет использовать свою память, чтобы определить, свое положение на карте. Основываясь на том, что робот видел, и на оценках того, как он двигался в окружающей среде, в каком направлении повернул, появляется возможность определить путь, который был выбран, и положение, в котором робот находится сейчас.

Попробуем представить вышесказанное в виде формализованного алгоритма. Будем считать, что робот может запоминать несколько последовательных измерений таким образом, чтобы это позволило ему со временем уточнять свое местоположение.

Вначале у робота есть карта комнаты и ничего больше. Таким образом, существует равная вероятность того, что робот находится в любом месте карты и смотрит в любом направлении. Можно аппроксимировать распределение вероятностей, генерируя равномерное количество дискретных положений рандомизации, например 30 различных положений, как показано на Рис. 5.4. Каждое из положений, или частиц, сохраняется как возможная оценка состояния.

Рис. 5.4. Равномерное распределение положения и направления робота

Чем больше частиц, тем выше точность, но для расчета требуется больше времени, поэтому существует компромисс между точностью и скоростью. Фильтр может пройти через все частицы одну за другой и определить с помощью карты, какой лидарный датчик вернул бы, похожие данные если бы положение было реальным. Затем он может сравнить две сцены и определить, насколько вероятно, что эта частица находится в реальном состоянии системы. Чем ближе совпадают сцены, тем выше вероятность, как показано на Рис. 4.5.

Рис. 5.5. Оценка вероятности по данным лидара

Далее будем увеличивать число частиц с высокой вероятностью и снижать число частицы с меньшей вероятностью. Построим повторную выборку частиц в соответствии с этим новым распределением вероятностей. Теперь большее количество частиц будет находиться в более вероятных положениях, и меньшее количество - в других. Эта случайная дискретная повторная выборка распределения вероятностей. Поэтому данный тип локализации относят к методам Монте-Карло.

Робот знает, как он перемещается между измерениями лидара, и можно применить это оценочное движение к каждой отдельной частице в фильтре. По сути, можно предсказать, куда будет смещаться каждое из возможных положений, если они будут двигаться таким же относительным образом, как и робот. Однако существует неопределенность в предполагаемом движении (расчет мертвых точек - это зашумленный процесс). Поэтому к движению частиц также необходимо добавить шум. Выполняется тот же шаг, что и раньше; и происходит измерение сцены с помощью лидара. Далее необходимо выяснить, какая из новых частиц с наибольшей вероятностью произведет подобное измерение. Если робот находит угол, это уникальная особенность комнаты, но он все равно не сужает облость поиска полностью. Если робот находит другой угол, то снова наиболее вероятные частицы увеличиваются, а менее вероятные - прореживаются. Затем распределение вероятностей изменяется. Теперь есть два основных возможных места, где мог бы находиться робот. Еще раз, когда робот находит дверь, распределение вероятностей изменится на одно место (большинство частиц сходятся в одном месте) с высокой вероятностью, где они могут быть на карте.

В зависимости от количества частиц, можно использовать более разумный метод, когда распределение становится более узким, принимая много частиц, замедляя фильтр, не добавляя большого значения, поэтому существует идея адаптивной локализации Монте-Карло, которая пересчитывает количество частиц после каждого поколения.

5.3 Эксперименты

Алгоритмы были реализованы на MATLAB с использованием функции локализации Монте-Карло (MCL) на TurtleBot в смоделированной среде Gazebo. Алгоритм требует известной карты, и задача состоит в том, чтобы оценить позу (положение и ориентацию) робота на карте на основе движения робота и лазерного датчика. Алгоритм начинается с первоначального представления о распределении вероятностей позы робота, которое представлено частицами, распределенными в

соответствии с этим представлением. Эти частицы распространяются в соответствии с движением робота каждый раз, когда изменяется положение робота. После получения новых показаний датчика каждая частица будет оценивать свою точность, проверяя, насколько вероятно получение таких показаний датчика в ее текущем положении. Затем алгоритм перераспределяет (повторно дискретизирует) частицы, чтобы компенсировать частицы, которые являются более точными. Далее продолжается повторение этих шагов движения, измерения и повторной выборки, и все частицы должны сойтись в один кластер рядом с истинным положением робота, если локализация прошла успешно [52]. Для каждой частицы мы ориентируем развертку лидара (показано красным) относительно позы частицы, а затем вычисляем показатель корреляции. Оценка корреляции S рассчитывается по следующей формуле:

где А - карта, В - данные сканера, А, в - средние значения пикселей карты и и сканера (препятствия имеют значение один, а свободное пространство -ноль); т, п, х, у - координаты пикселей.

Уравнение (5.1) для рассчета £ по сути подсчитывает количество совпадающих пикселей карты и сканирования, которые перекрывают друг друга. Процесс повторяется для каждой частицы, чтобы найти оценку корреляции для каждой из них.

Глобальная локализация означает, что робот может быть изначально расположен в любом месте на карте, как показано на Рис. 5.6.

^^ ^^ (Атп А)(Втп В)

(5.1)