Размещено на http://www.allbest.ru/

ФЕДЕРАЛЬНОЕ ГОСУДАРСТВЕННОЕ АВТОНОМНОЕ

ОБРАЗОВАТЕЛЬНОЕ УЧРЕЖДЕНИЕ ВЫСШЕГО ОБРАЗОВАНИЯ

«НАЦИОНАЛЬНЫЙ ИССЛЕДОВАТЕЛЬСКИЙ УНИВЕРСИТЕТ

«ВЫСШАЯ ШКОЛА ЭКОНОМИКИ»

Московский институт электроники и математики им. А.Н. Тихонова

ЛОКАЛИЗАЦИЯ АВТОНОМНОГО ТРАНСПОРТНОГО СРЕДСТВА В УСЛОВИЯХ ПЛОХОГО GPS-СИГНАЛА

Выпускная квалификационная работа

по направлению

01.03.02 Инфокоммуникационные технологии и системы связи

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

«Инфокоммуникационные технологии и системы связи»

Гужва Никита Сергеевич, группа БИТ163

Москва 2020 г.

ФЕДЕРАЛЬНОЕ ГОСУДАРСТВЕННОЕ АВТОНОМНОЕ ОБРАЗОВАТЕЛЬНОЕ УЧРЕЖДЕНИЕ ВЫСШЕГО ОБРАЗОВАНИЯ

«НАЦИОНАЛЬНЫЙ ИССЛЕДОВАТЕЛЬСКИЙ УНИВЕРСИТЕТ

«ВЫСШАЯ ШКОЛА ЭКОНОМИКИ»

МОСКОВСКИЙ ИНСТИТУТ ЭЛЕКТРОНИКИ И МАТЕМАТИКИ

им. А. Н. ТИХОНОВА

ЗАДАНИЕ

на выпускную квалификационную работу бакалавра

студенту группы БИТ163 Гужва Никите Сергеевичу

1. Тема работы

Локализация автономного транспортного средства в условиях плохого GPS-сигнала

2. Требования к работе

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

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

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

4. Сроки выполнения этапов работы

Проект ВКР представляется студентом в срок до	«19» февраля 2020г.
Первый вариант ВКР представляется студентом в срок до	«30» апреля 2020г.
Итоговый вариант ВКР представляется студентом руководителю до загрузки работы в систему «Антиплагиат» в срок до	«18» мая 2020г.

Задание выдано	«11» декабря 2019 г.	______________ Ф.И. Иванов подпись руководителя
Задание принято к исполнению	«11» декабря 2019 г.	______________ Н.С. Гужва подпись студента



ФЕДЕРАЛЬНОЕ ГОСУДАРСТВЕННОЕ АВТОНОМНОЕ ОБРАЗОВАТЕЛЬНОЕ УЧРЕЖДЕНИЕ ВЫСШЕГО ОБРАЗОВАНИЯ

«НАЦИОНАЛЬНЫЙ ИССЛЕДОВАТЕЛЬСКИЙ УНИВЕРСИТЕТ

«ВЫСШАЯ ШКОЛА ЭКОНОМИКИ»

МОСКОВСКИЙ ИНСТИТУТ ЭЛЕКТРОНИКИ И МАТЕМАТИКИ

им. А. Н. ТИХОНОВА

График сдачи этапов

выпускной квалификационной работы бакалавра

студента группы БИТ163 Гужва Никиты Сергеевича

Тема работы

Локализация автономного транспортного средства в условиях плохого GPS-сигнала

Дата представления проекта ВКР	«18» февраля 2020 г.	______________ Ф.И. Иванов подпись руководителя
Дата представления первого варианта ВКР	«30» апреля 2020 г.	______________ Ф.И. Иванов подпись руководителя
Дата представления итогового варианта ВКР	«16» мая 2020 г.	______________ Ф.И. Иванов подпись руководителя



Аннотация

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

Рассматриваемым транспортным средством является городской трамвай.

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

Реализация метода, описанного в данной работе, была осуществлена на языке программирования C++ с активным использованием библиотеки OpenCV.

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



Localization of an autonomous vehicle in conditions of poor gps signal

Abstract

The goal of this work is to improve the accuracy of vehicle positioning, using a limited set of sensors and a map in conditions of poor GPS-signal.

The considered vehicle is a city tramway.

In the course of this work, an analysis of existing approaches and methods for localizing autonomous vehicles and robots was performed. According to the results of this research it was decided to take the particle filter and Kalman filter as the basis for the original algorithm.

The implementation of the method described in this paper was carried out in the C ++ programming language with the active usage of the OpenCV library.

The experimental results demonstrate that under different conditions with varying degrees of difficulty, the software copes with the task smoothly and shows the localization accuracy of up to 2 meters.



Оглавление

сигнал gps сенсор карта

Введение

1. Теоретическая часть

1.1 Системы спутниковой навигации

1.2 Колесная одометрия

1.3 SLAM

1.4 Feature Detection

1.5 Оптический поток

1.6 Внутренние и внешние параметры камеры

1.6.1 Внутренние параметры

1.6.2Внешние параметры

1.6.3Дисторсия

1.7 Визуальная одометрия

1.8 Фильтрация данных

1.8.1 Фильтр Калмана

1.8.2 Фильтр частиц

1.9 Сплайн

1.10 Сравнение гистограмм

2. Мировая практика

3. Практическая часть

3.1 Системы координат

3.2 Маршрут движения

3.3 Моделирование условий плохого GPS-сигнала

3.4 Синхронизация данных с сенсоров

3.5 Алгоритм локализации

3.6 Реализация

3.6.1 Предобработка изображения

3.6.2 Детектирование статических объектов

3.6.3 Составление гистограммы на основе наблюдений с камеры

3.6.4 Фильтр частиц

3.6.5 Фильтр Калмана

3.7. Результаты

3.7.1 Сравнение результатов фильтра частиц отдельно и совместно с фильтром Калмана

3.7.2Сравнение результатов при изменении количества частиц

3.7.3Сравнение результатов на разных участках пути

Заключение

Список литературы

Приложение А



Введение

Современный мир находится на грани транспортной революции. В обозримом будущем новейшие технологии заменят водителей. Множество крупнейших мировых IT-компаний и автопроизводителей, таких как BMW, Mercedes-Benz, Tesla, Google, Яндекс и др., разрабатывают концепцию автономных транспортных средств. Некоторые прототипы показывают многообещающие результаты, однако их пока нельзя назвать полностью функциональными и завершенными. Есть ряд проблем, для которых необходимо найти решение. Например, одной из самых важных задач является локализация на местности. Стандартные способы, использующие системы спутниковой навигации, такие как ГЛОНАСС и GPS, имеют погрешность около трех метров в хороших условиях и могут достигать ошибки в 50 метров при внешних помехах. За последние несколько лет было сделано множество открытий в области робототехники. Новейшие технологии и алгоритмы дают возможность найти свежие решения старых задач.

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

1. Исследовать современные методы и алгоритмы компьютерного зрения и позиционирования роботов.

2. Изучить мировую практику в области локализации автономных транспортных средств.

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



1. Теоретическая часть

1.1 Системы спутниковой навигации

Системы ГЛОНАСС и GPS являются неотъемлемой частью любой комплексной системы навигации. Обе используют схожий принцип работы и имеют точность позиционирования 3-6 метров.

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

Для уменьшения искажений и достижения лучшей точности существует система дифференциальной коррекции RTK (Real Time Kinematic или Кинематика реального времени) [1]. Принцип работы заключается в использовании двух приемников одновременно. Первый - базовый, его координаты должны быть четко определены; он передает по каналу связи (радиомодем, GSM-модем, интернет и др.) набор данных, называемых поправками. Эти поправки и сам спутниковый сигнал обрабатываются специальным ПО, и после этого на другой приемник поступают дифференциальные поправки. В результате второй приемник может использовать эти данные для уточнения местоположения до 1см.

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

Рис. 1. Визуализация данных GPS без RTK поправок (на плоскости x, y)

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

1.2 Колесная одометрия

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

, где x, y - координаты точки на плоскости, S - метр маршрута, dv - изменение скорости за время dt.

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

1.3 SLAM

Одним из самых популярных альтернативных способов локализации на местности, широко применяемого в робототехнике, является SLAM (Simultaneous localization and mapping) [2]. Основная задача заключается в определении оценки местоположения объекта и карты в определенный момент времени t (причем t - дискретная величина), исходя из наблюдений . Для решения применяется правило Байеса, так как все величины являются вероятностными. Тогда формально, задачу можно поставить следующим образом:

В данной ВКР подразумевается, что изначально карта местности задана, поэтому задачу можно упростить:

Рис. 2. Результат SLAM-алгоритма

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

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

1.4 Feature Detection

Робот определяет точки в пространстве, за которые можно «зацепиться» и при движении пытается сопоставить их положение на предыдущем и следующем кадре. Для детекции этих точек существует множество алгоритмов [3][4]. Некоторые из них вычислительно-сложные и не подходят для решения задачи в реальном времени. Но новые разработки и модификации улучшают точность и производительность [5].

Примером может быть алгоритм определения углов Ши-Томаси.

На вход подается черно-белое изображение, далее выбирается прямоугольник (u, v). Его сдвигают на (x, y) и вычисляют сумму квадратов разностей интенсивности пикселей, между двумя прямоугольниками:

С помощью преобразований можно получить матричную запись:

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

1.5 Оптический поток

Для корреспондирования точек между кадрами обычно используется оптический поток [6][7]. Алгоритм пытается вычислить движение между двумя кадрами в моменты времени и . В основном используются дифференциальные методы, которые применяют частные производные по координатам и времени.

Берется черно-белое изображение. Каждый пиксель можно представить как воксель в точке (x, y, t) с интенсивностью I(x, y, t). Тогда можно записать уравнение перемещения между двумя кадрами:

Считая, что перемещение мало, можно использовать ряд Тейлора и предыдущее уравнение, откуда получается, что:

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

Например, широко используемый метод Лукаса-Канаде для решения задачи применяет информацию о соседних пикселях. Предположение состоит в том, что в некой локальной окрестности каждого пикселя значение оптического потока одинаково. Тогда можно для окна заданного размера с центром p записать систему уравнений, в которой решением будет являться вектор оптического потока в точке p:

Данная система решается методом наименьших квадратов:

Также используют прием взвешенного окна. Каждому пикселю в окрестности p присваивается вес в зависимости от удаленности от центра окна. Обычно в качестве весов используется нормальное распределение расстояния между каждым пикселем и p.

1.6 Внутренние и внешние параметры камеры

Внутренние и внешние параметры [8] можно получить при калибровке камеры. Это наиважнейший подготовительный этап для решения задач компьютерного зрения. Данные параметры устанавливают связь между 2D-точками изображения и 3D-точками пространства в мировых координатах . Для робототехники свойственно использовать однородные координаты. Их количество на одну больше, чем размерность пространства. Это позволяет представить даже точки, находящиеся на бесконечности.

, где - масштабный коэффициент.

1.1.1. Внутренние параметры

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

1.1.2. Внешние параметры

Внешние параметры представляют положение камеры в пространстве в виде матрицы поворота и вектора переноса . Они переводят точки из мировой системы координат в систему координат, связанную с камерой.

1.1.3. Дисторсия

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

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

1.2 Визуальная одометрия

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

Совместив алгоритм нахождения точек (feature detection) и оптический поток, можно вычислить визуальную одометрию. В классическом варианте корреспондируются точки между двумя соседними кадрами. Чтобы уменьшить влияние окклюзии объектов, в экспериментах ВКР сопоставление точек происходит через три кадра (т.е. между первым и третьим кадрами).

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

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

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

Пусть x и x' координаты точек на первом и третьем кадре соответственно, тогда существует такая матрица F размера 3х3, при которой . Она зависит только от матриц камеры P и P' в разный момент времени (на первом и третьем кадре).

Если , то

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

Поставленная задача - обратная. Имеется информация о x и x', необходимо найти изменение позы камеры за два кадра [R|t].

Для нахождения существенной матрицы чаще всего используется метод RANSAC (RANdom SAmple Consensus), устойчивый к зашумленности исходных данных. Чтобы получить решение, количество точек должно быть больше или равно 8. После этого можно декомпозировать матрицу E и вычислить матрицу поворота R и вектор трансляции t.

Полученный вектор t нормализован и поэтому его нужно домножить на скалярную величину - скорость. Так как в ВКР используется только одна камера, тяжело определить скорость движения ТС при помощи зрения, поэтому используются данные колесной одометрии.

Чтобы отобразить траекторию движения камеры необходимо аккумулировать значения изменения позы:

.

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

Также для улучшения точности были реализованы некоторые правки:

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

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

1.3 Фильтрация данных

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

1.3.1 Фильтр Калмана

Самым распространенным инструментом для фильтрации данных с датчиков является фильтр Калмана [9][10].

Рис. 3. Пример использования фильтра Калмана

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

Фильтрация происходит в несколько этапов:

· Предсказание значения измеряемой величины в следующий (дискретный) момент времени, используя актуальное значение, физическую модель и контрольные параметры.

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

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

· Получение данных с сенсоров с учетом ошибки.

Для определения координаты очевидным сенсором является GPS.

Здесь - это координата, измеренная сенсором; - погрешность показаний датчика (также случайная величина с нормальным распределением).

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

· Алгоритм Калмана

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

Вес называется коэффициентом Калмана. Для того, чтобы вычислить значение коэффициента, нужно минимизировать ошибку:

Тогда:

· Матричная форма

Для общности, уравнение физической модели описанное выше, можно представить в матричной форме:

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

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

, где - матрица ковариации ошибки в момент времени ; - матрица ковариации шума физического процесса.

Коэффициент (усиление) Калмана:

, где - матрица измерений, переводящая вектор состояния в вектор измерений; - матрица ковариации шума измерений.

В результате итоговая оценка состояния системы и ошибки:

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

1.3.2 Фильтр частиц

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

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

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

Также фильтр частиц справляется с нелинейностью физических процессов.

Рис. 4. Пример работы фильтра частиц.

Фильтрация происходит в три шага:

· Предсказание или экстраполяция состояния на следующем шаге

Также, как и в фильтре Калмана, на данном этапе происходит моделирование физического процесса для предсказания состояния в следующий момент времени. Например, можно взять измерения визуальной одометрии, тогда:

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

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

· Распределение весов и учет погрешности

На данном этапе как раз фильтр дает возможность учесть погрешность измерений сенсоров.

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

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

, где - погрешность измерения; - измерение реального объекта; - измерение частицы.

Тогда - это вероятность совпадения положения частицы с положением реального объекта.

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

После, веса всех частиц важно нормализовать:

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

· Отсеивание частиц

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

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

Самым простым и эффективным способом реализации этого шага является колесо отсева.

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

Затем, пока , из параметра вычитается вес -ой частицы и индекс увеличивается на один:

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

Стоит отметить, что индекс меняется циклично и всегда соответствует условию .

1.4 Сплайн

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

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

Сплайн имеет несколько параметров. Один из них - это степень сплайна , задающий максимальную степень внутренних многочленов.

Рис. 5. Сплайн и контрольные точки.

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

Получается сплайн можно представить набором из трех параметров .

1.5 Сравнение гистограмм

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

Существует несколько метрик для сравнения гистограмм:

· Correlation

, где

и - количество столбцов гистограммы.

· Intersection

·

· Chi-Square

· Bhattacharyya

Таблица 1. Метрики сравнения гистограмм

Гистограммы	Correlation	Intersection	Chi-Square	Bhattacharyya
Точное совпадение	1.0	1.0	0.0	0.0
Середина	0.7	0.5	0.67	0.55
Несовпадение	-1.0	0.0	2.0	1.0



2. Мировая практика

Автономные ТС требуют точного знания о своем местоположении и ориентации для планирования пути, контроля дорожных событий и безопасности. Различные требования описаны в данной статье [13].

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

Маркус А. Брубейкер и соавт. [14] предлагают доступное решение для самостоятельной локализации с использованием только визуальной одометрии и дорожных карт. Статья содержит интересные идеи и потрясающие результаты. Они используют преимущества проекта OpenStreetMap (OSM), карты с открытым исходным кодом, управляемого сообществом. Дороги делятся на сегменты улиц двух типов: дуговые и линейные, создавая граф карты, где сегменты соответствуют ребрам, а точки пересечения соответствуют вершинам. Чтобы локализовать транспортное средство на карте, необходимо иметь хотя бы некоторую информацию об окружающей среде. Авторы предлагают использовать одну или две камеры и применить визуальную одометрию, чтобы получить траекторию движения транспортного средства. Поскольку эти данные являются чрезмерно шумными из-за их природы, предлагается использовать вероятностную модель вместо прямого сопоставления формы. Следующим шагом является определение модели пространства состояний, которая будет содержать всю необходимую информацию, такую ??как расстояние, угол, улица в предыдущий и текущий момент времени. Наблюдения с камер включают линейное и угловое смещение от кадра к кадру, представленное в виде матриц перемещения и поворота соответственно. Основной алгоритм представляет собой модифицированную версию фильтра частиц, который вычисляет вероятность нахождения в некоторой точке карты с учетом траектории в качестве наблюдения. Предполагается, что модель перехода между состояниями является гауссово-линейной, что означает использование смеси гауссианов и функции плотности вероятности для обновления весов частиц и устранения недоступных сегментов улиц. Результатом является самолокализация до 3 метров за 10-15 секунд на карте 2000 км (с использованием наборов данных KITTI).

Рис. 6. Результат локализации по карте

Эрик Стэнборг и соавт. [15] рассказывают о способе визуальной локализации при помощи семантической сегментации. Различные дескрипторы, вроде SIFT, плохо справляются с сильными изменениями окружающей среды. Появилась потребность придумать способ более устойчивый к подобным изменениям. Смысл подхода в том, чтобы сопоставить заранее составленную карту и изображение с камеры. Самое интересное - это вид карты. Вместо обычного 2D представления объектов, они используют точечную 3D карту местности. Каждому вокселю присваивается свой семантический лэйбл. Затем при движении семантическая сегментация кадра с камеры накладывается на карту. Тем самым авторы определяют позу ТС (6 степеней свободы). Точность координат в среднем до 1 метра.

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

В своей диссертации Роберт Шпангенберг [17] предлагает способ локализации при помощи ориентиров. В частности он рассказывает, что в сценариях передвижения по городу можно использовать “столбо-подобные” ориентиры, такие как дорожные знаки, опоры, светофоры, деревья и т.д.. Столбы обнаруживаются при помощи обработки контуров, полученных из кадра и карты диспаратности. Фильтр частиц оценивает вероятное положение в пространстве и фильтр Калмана связывает данные с разных сенсоров и дает окончательный результат.

Рис. 7. Процесс вычисления совпадений столбов

3. Практическая часть

В данной ВКР основная задача - это локализовать транспортное средство на карте при плохом сигнале GPS, опираясь на данные других сенсоров и компьютерные алгоритмы.

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

На крыше установлен GPS-датчик Emlid, с точно известным расположением. Спереди закреплена цветная камера с углом обзора около шестидесяти градусов. Дополнительно в трамвай встроен датчик колесной одометрии. Тестовые данные с показаниями датчиков собираются компьютером и форматируются для проведения «оффлайн» (удаленных) экспериментов.

3.1 Системы координат

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

1. GM (Global Metrics) - Глобальные координаты с заданным началом отсчет. Эти же координаты используются для сплайна маршрута. В них рассчитывается итоговое положение трамвая.

2. VR (Vehicle Related) - Система координат, связанная с транспортным средством. За начало отсчета берется точка на земле под краем переднего бампера. Ось Y направлена вперед.

3. CR (Camera Related) - Система координат, связанная с камерой. Внешняя поза камеры задана относительно VR-центра, поэтому все расчеты на изображении будут переводиться из CR в VR.

3.2 Маршрут движения

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

1. Происходит фильтрация выбросов.

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

3. Вычисляется подходящий сплайн.

4. Параметры результирующего сплайна записываются в текстовый файл.

3.3 Моделирование условий плохого GPS-сигнала

Для экспериментов необходимо знать точное положение транспортного средства (ground-truth) для оценки результатов работы алгоритма локализации. К GPS-датчику, установленному на трамвай, подключены RTK-поправки. Для моделирования условий плохо сигнала GPS, к полученным данным добавляется шум, заданный нормальным распределением с . Так как 99% значений случайной величины, распределенной по нормальному закону, попадают в промежуток , итоговая ошибка получается около 15-ти метров по и .



3.4 Синхронизация данных с сенсоров

Данные с сенсоров приходят с разными интервалами по времени. Это важно учитывать и синхронизировать данные.

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

3.5 Алгоритм локализации

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

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

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

Блок-схема логики работы алгоритма представлена в приложении А.

3.6 Реализация

3.6.1 Предобработка изображения

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

Рис. 8. Оригинальное изображение

Рис. 9. Ректифицированное изображение

Как видно из рисунков 8 и 9, ректифицированное изображение восстановило реальное положение объектов. Теперь для устранения появившихся черных областей, изображение обрезается с каждой стороны на одинаковое количество пикселей.

3.6.2 Детектирование статических объектов

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

Рис. 10. Семантическая сегментация

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

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

3.6.3 Составление гистограммы на основе наблюдений с камеры

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

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

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

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

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

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

Полученный вектор находится в системе координат, связанной с камерой. Вторым шагом нужно найти аналогичный вектор для VR-центра. Для этого достаточно вычесть из промежуточного результата вектор переноса камеры:

Теперь остается посчитать сам угол в радианах:

Угол считается относительно оси Y и точно принадлежит промежутку , ведь угол обзора камеры примерно 60 градусов.

Все вычисленные углы записываются в массив, из которого затем собирается гистограмма с помощью функции библиотеки OpenCV cv::calcHist(). По оси X - углы в радианах, а по оси Y - количество пикселей. Так как интересует не конкретное количество пикселей, а форма самой гистограммы, то следует ее нормализовать от 0 до 1 с помощью функции cv::normalize().

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

Рис. 11. Гистограмма углов до столбов

3.6.4 Фильтр частиц

· Инициализация

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

Тогда алгоритм для добавления гипотез выглядит так:

1. Точка с GPS-датчика проецируется на сплайн и вычисляется метр маршрута.

2. К полученному значению прибавляется шум в виде равномерно-распределенной случайной величины:

, где - произвольное отклонение.

3. Результат преобразуется обратно в GM-координаты и к ним прибавляется шум, для небольшого отклонения от заданного маршрута. Сплайн не идеально повторяет реальный путь, поэтому истинное положение трамвая может быть в точке близкой к заданному маршруту.

, где - произвольное отклонение.

4. Матрица поворота вычисляется из направления пути около полученной точки.

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

6. Вес точки изначально равен , где - количество частиц в фильтре.

7. Аналогично для остальных частиц.

· Обновление новыми данными с GPS-сенсора

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

· Предсказание следующего положения

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

Так как время между кадрами мало, то и изменение положения также мало, ведь скорость трамвая не больше 70-80 км/ч. Тогда можно сказать, что в этот короткий промежуток времени трамвай в VR-координатах движется по прямой, т.е. вдоль оси Y. К тому же трамвайные пути в основном состоят из прямых участков. Следовательно, это изменение считается так:

Очевидно, что если в VR-координатах продвинуться вперед по оси Y на величину , то получится следующее предполагаемое положение трамвая. Теперь это нужно перевести в GM-координаты и обновить положение частицы. Для этого нужно составить трехмерную точку нового положения в VR:

Чтобы перевести точку из VR в GM нужно сделать следующее преобразование:

, где - вектор переноса, полученный из текущего положения частицы;

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

Новая матрица поворота вычисляется из направления пути около предсказанного положения частицы.

· Обновление весов

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

Камера имеет определенный угол обзора и ограниченную видимость. Как указывалось ранее, ограничение по дальности видимых объектов - 75 метров. Значит для каждой частицы нужно выбирать столбы в «зоне видимости». Для этого строится треугольник, внутри которого ограничена видимость частицы. Одна вершина совпадает с положением самой частицы и угол в ней равен углу обзора камеры, т.е. 60 градусов. Высота треугольника равна максимальной дальности.

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

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

Аналогично с получением углов для самого трамвая, следует вычислить арктангенс:

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

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

1. Расстояние до столба :

2. Максимальное расстояние до столба , равное ограничению по дальности видимости.

3. Ширина столбов и дорожных знаков . Эти величины должны отражать отношение размеров данных объектов, но не являются реальными значениями.

, где - реальные значения ширины дорожного знака и столба соответственно.

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

Тогда для каждого статического объекта можно рассчитать так:

Так как в карте столбы и дорожные знаки отмечены как разные объекты, выбирается соответственно.

Теперь из полученного массива составляется гистограмма. Функция cv::calcHist() возвращает гистограмму в виде матрицы размером , где N - заданное количество столбцов. Параметры совпадают с параметрами для составления гистограммы из наблюдений с камеры.

Так как исходная гистограмма наблюдений напоминает нормальное распределение Гаусса, нужно искусственно сделать похожей гистограмму для частицы. Для этого применяется фильтр Гаусса, реализуемый с помощью функции cv::GaussianBlur(). Он размывает гистограмму так, что все столбцы, отражающие углы до объектов, превращаются в форму колокола, т.е. нормальное распределение с равным углу.

Отклонение должно отражать дальность до столба, поэтому:

, где - произвольно заданное отклонение.

Из экспериментов значение было выбрано 0.1. Для фильтрации гистограммы нужно масштабировать :

, где - количество столбцов; - верхняя и нижняя границы заданного диапазона соответственно.

И заключительный шаг - это нормализация гистограммы в пределах от 0 до 1.

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

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

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

· Отсев гипотез

В данной ВКР для отсева гипотез не используется стандартный алгоритм «колесо отсева». Вместе этого был придуман собственный алгоритм:

1. Частицы сортируются по весу.

2. Удаляется произвольно заданный процент худших гипотез.

3. Для каждой оставшейся частицы определяется какое количество новых частиц будет создано в ее окрестности в зависимости от ее веса:

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

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

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

· Получение результирующего положения

Итоговые координаты предполагаемого местоположения считаются как взвешенное среднее облака частиц:

, где - координаты -ой частицы.

Итоговая дисперсия облака частиц:

3.6.5 Фильтр Калмана

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

Состояние включает в себя только одну величину - метр маршрута.

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

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

· Прогнозирование

Функция экстраполяции состояния совпадает с функцией прогнозирования в фильтре частиц:

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

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

Для прогнозирования матрицы ковариации ошибки формула не меняется:

, где - матрица ковариации ошибки контролирующих параметров.

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

· Обновление

Функция перехода из состояния в измерение является нелинейной, так как она использует сплайн:

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

Тогда матрица будет рассчитываться так:

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

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

3.7 Результаты

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

3.7.1 Сравнение результатов фильтра частиц отдельно и совместно с фильтром Калмана

Для сравнения результатов выбрана простая метрика - отклонение точки, получившейся на выходе алгоритма от эталонного положения трамвая (ground truth).

В результате нескольких тестов получились следующие графики:

Рис. 12. Сравнение фильтров

По оси X - временные отсчеты, по оси Y - отклонение в сантиметрах.

Для нескольких экспериментов графики оказались очень похожими. Из рисунка 12 видно, что использование фильтра Калмана (EKF) для дополнительной обработки существенно улучшает результат. Отклонение меньше 2 метров.

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



3.7.2 Сравнение результатов при изменении количества частиц

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

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

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

Таблица 2. Влияние количества частиц в фильтре

N	t, мс	, м
100	25	1.26	3.82
500	35	0.71	0.1
1000	65	0.77	0.05
2000	95	0.72	0.09
5000	210	0.74	0.05
10000	620	0.76	0.09

Для каждого значения N, эксперимент проводился несколько раз на одном участке пути.

Из результатов эксперимента, описанных в таблице 2, можно увидеть, что с увеличением количества частиц, значительно возрастает время выполнения алгоритма. Но от 500 частиц отклонение сильно не меняется. Тогда можно сказать, что оптимальное количество частиц для данного фильтра - 1000, так как время выполнения приемлемое и низкое значение дисперсии говорит о большей гладкости фильтрации.



3.7.3 Сравнение результатов на разных участках пути

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

Таблица 3. Результаты локализации на разных участках

n	, м
1	0.77	0.06
2	1.02	0.24
3	1.92	2.11
4	1.00	0.74
5	1.25	0.27
6	1.15	0.51

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

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

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

4. Сложность участка заключается в наличии моста, под которым проезжает трамвай. Так как столбов нет в зоне видимости, фильтр частиц не может дать корректную оценку. Но среднее отклонение не большое, так как после проезда моста, фильтру удается локализоваться. Большая дисперсия снова говорит о наличии «скачков».

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

...