CC BY
338
УДК 681.5 + 007
DOI: 10.18698/0236-3933-2017-3-76-94
АЛГОРИТМ НАВИГАЦИИ БЕСПИЛОТНОГО ЛЕТАТЕЛЬНОГО АППАРАТА НА ОСНОВЕ УЛУЧШЕННОГО АЛГОРИТМА ОДНОВРЕМЕННОЙ ЛОКАЛИЗАЦИИ И КАРТОГРАФИРОВАНИЯ С АДАПТИВНЫМ ЛОКАЛЬНЫМ ДИАПАЗОНОМ НАБЛЮДЕНИЯ
Кэ Кэ Гэн Н.А. Чулин
[email protected] [email protected]
МГТУ им. Н.Э. Баумана, Москва, Российская Федерация
Аннотация
Предложено улучшить алгоритм одновременной локализации и картографирования с расширенным фильтром Калмана (Extended Kalman Filter for Simultaneous Localization and Mapping, EKF-SLAM), что позволяет при достаточной точности локализации в сложной среде существенно сократить объемы необходимых вычислений путем адаптации диапазона наблюдения в режиме реального времени в трехмерных средах для беспилотных летательных аппаратов (БПЛА). Построена облачно-точечная карта окружающей среды и вычислены координаты характерных точек среды восьмиточечным нормированным алгоритмом на основе монокулярного компьютерного зрения. Улучшения достигнуты с помощью адаптивного динамического ограничения текущих размеров наблюдаемой части окружающей среды и числа наблюдаемых ориентиров для коррекции позиционирования летательных аппаратов. Результаты моделирования показывают, что предложенный метод, существенно сокращающий объемы вычислений при сохранении точности локализации, может быть применен для навигации беспилотного летательного аппарата
Ключевые слова
Беспилотный летательный аппарат, улучшенный алгоритм БКГ-8ЬЛМ, ассоциация данных, характерные точки
Поступила в редакцию 17.03.2016 © МГТУ им. Н.Э. Баумана, 2017
В настоящее время беспилотные летательные аппараты (БПЛА) используются достаточно широко и разнообразно. Однако их использование ограничено, в основном, режимами «ручного» дистанционного управления с пульта оператора и прохождения точек маршрута в простой (свободной от препятствий) среде. Актуальной является задача разработки системы управления, позволяющей осуществлять автономный полет по заданному маршруту. Один из ключевых вопросов в решении этой задачи — динамическое планирование маршрута в сложной среде, т. е. в пространстве, содержащем как заранее известные, так и неизвестные объекты, которые могут использоваться как ориентиры, но одновременно представляют собой препятствия при движении. Из возможных подходов к решению проблемы планирования маршрута в последнее десятилетие
популярен алгоритм одновременной локализации и создания карты (картографирования) с расширенным фильтром Калмана (Extended Kalman Filter for Simultaneous Localization and Mapping, EKF-SLAM), дающий возможность прогнозирования координат и скорости БПЛА в неизвестной среде с одновременной оценкой положения ориентиров и созданием полной карты.
Принципиальное решение задачи одновременной локализации и создания карты SLAM предложено достаточно давно [1], и к настоящему времени эта концепция достигла определенного уровня, достаточного для практической реализации в области робототехники при решении задач автономного движения [2-4]. Разработаны различные улучшения алгоритма SLAM [5-9], в том числе с использованием калмановской фильтрации [7-9]. В настоящее время алгоритмы SLAM с использованием расширенного фильтра Калмана (Extended Kalman Filter, EKF) имеют установившуюся аббревиатуру EKF-SLAM, которая использована и в данной статье. Существенный недостаток алгоритмов EKF-SLAM — значительный рост объема необходимых вычислений с увеличением числа наблюдаемых ориентиров. Стремление сократить объем вычислений приводит к снижению точности позиционирования и возможности получения противоречивой информации при составлении карты [10].
В настоящей статье предложено усовершенствовать алгоритм EKF-SLAM путем адаптивных ограничений зоны наблюдения (Adaptive Observation Range, AR, аббревиатура с учетом этих усовершенствований — AR-SLAM-EKF) и удаления из алгоритма расчета ориентиров, оказавшихся избыточными. Предложены алгоритмы динамического изменения размеров зоны наблюдения и определения избыточности обнаруживаемых ориентиров. Эти алгоритмы являются развитием алгоритмов, предложенных в работе [11], и позволяют решать задачу в пространстве (трехмерной среде). Приведены описание математической модели движения, используемой в алгоритме, процедура расширенной калманов-ской фильтрации для рассматриваемой задачи и предлагаемые улучшения. Усовершенствования также вносятся и в процедуру ассоциации данных в алгоритме SLAM. Задачу ассоциации данных SLAM можно представить как задачу оптимизации. Одним из широко используемых алгоритмов оптимизации является муравьиный алгоритм, обладающий свойствами положительной обратной связи и возможностью параллельного поиска, вследствие чего этот алгоритм может быть применен для решения задачи ассоциации данных SLAM. Однако традиционный муравьиный алгоритм может в процессе поиска легко попасть в локальный оптимум. Избежать локального оптимума можно добавлением в процесс обновления глобального феромона случайного возмущения. Установка ограничения феромона на маршруте позволяет расширить пространство поиска и упростить обнаружение оптимального маршрута. Результаты моделирования показывают, что эти усовершенствования могут эффективно улучшить точность позиционирования и эффективность планирования маршрута в целом. Структура усовершенствованного алгоритма приведена на рис. 1.
Среда
Ориентиры ,
Координаты ориентиров ,
Ассоциация данных
Координаты ориентиров
Квадрокоптер
Получение и обработка изображения
Реальные состояния движения
Существующие ориентиры
Полная карта
Предсказанные состояния движения
Расширенный фильтр {Салмана
Сращивание
Новые ориентиры
Изменение радиуса наблюдения
Локальная карта
Инициализация ориентиров
Рис. 1. Структура усовершенствованного алгоритма AR-EKF-SLAM
Представляемые результаты изложены в настоящей статье в следующем порядке: суть проблемы с кратким обзором подходов к ее решению и основным содержанием данной статьи в краткой форме; разработка алгоритма получения и расчета координат точек ориентиров среды; разработка расширенного фильтра Калмана (EKF) в алгоритме SLAM; ассоциация данных для алгоритма SLAM; разработка алгоритма EKF-SLAM с адаптацией диапазона наблюдений; анализ результатов эксперимента визуальной навигации во внешней среде.
Получение и расчет координат точек ориентиров среды. В настоящее время существует много алгоритмов обнаружения характерных (особых, в частности, угловых) точек изображений, например, алгоритм Harris^, алгоритм FAST, алгоритм FASTER, алгоритм Moravec^, алгоритм SUSAN и др., аналитический обзор которых приведен в работе [12]. Здесь выбран алгоритм SUSAN, предложенный С. Смитом и Дж. Бреди (S.M. Smith, J.M. Brady, 1997) [13], который является более быстрым и устойчивым в случае размытия и неравномерной яркости фона изображений [14]. Для описания корреляции между угловыми точками двух изображений использована функция нормированной взаимной корреляции NCC (Normalized Cross Correlation) [15]. После грубого установления соответствия остается возможность существования неправильных пар соответственных характерных точек. В этой работе применен алгоритм RANSAC для получения точного соответствия характерных точек и фундаментальной матрицы F, связывающей координаты одинаковых точек на двух последовательных изображениях, удовлетворяющей уравнению [16]:
(х)т • F • x = 0,
где x = [u, у,1]т, x = [u , у',1]т— пиксельные координаты характерных точек на двух изображениях.
Координаты характерных точек могут быть получены с помощью уравнений:
(1)
zti * [ui, vi ] — ■m' * [xi, xw, xw Д J , i —1,2,
m1 — K3x3 * [13x3 |03x1 ]3x4 ; m2 — K3x3 * [R3x3 |L3x1 ]3x4 •
Здесь — проекция координат точек на ось z системы координат камеры; [ui, vi,1]т — пиксельные координаты характерных точек на i-м изображении;
[ xl, xl, xl ,1J — координаты этих характерных точек в связанной системе координат, совпадающей в начале движения с системой координат камеры; K3x3 — матрица внутренних параметров камеры; [ R3x3 |L3x1 ]3x4 — матрица преобразования координат двух последовательных изображений (внешних параметров) камеры; R — матрица вращения; L — матрица перемещения.
Перед получением координат характерных точек необходима калибровка внутренних параметров камеры. Параметры камеры получены с использованием инструментария системы Matlab для калибровки камер (Camera Calibration Toolbox for Matlab, Bouquet [17]):
K =
1015,78212 0 320,03704 0 1012,65800 240,40334 0 0 1
Матрица преобразования изображений, учитывающая параметры камеры, может быть записана в виде E = КТК.
В настоящей работе использовано монокулярное компьютерное зрение, поэтому К' = К.
В соответствии с используемым алгоритмом [17] матрицы вращения (перехода) и перемещения камеры могут быть получены из выражения
[R|l I, =
UWVT UWVт UWT V UWT VT
U [0,01]T; -U [0,01]T; U [0,01]T; -U [0,01]T,
(2)
где ! = Ь/р — матрица перемещения камеры; р — масштабный коэффициент; " 0 -1 0"
матрица Ш =1 0 0;и, V — сингулярное разложение матрицы Е = иБУт, 0 0 1
алгоритм которого представлен в работах [19, 20]; матрица Б = diag(k1,к2,0), к > кг.
Так, в приведенном ниже эксперименте такое разложение для изображений, представленных на рис. 2, имеет вид
E =
1,0135 - 23,0231 - 4,8249 23,6569 1,0480 1,2916 4,8797 -1,0790 - 0,0076
U =
0,0100 0,9984 - 0,0549 - 0,9799 - 0,0011 - 0,1995 -0,1992 0,0558 0,9784
V =
-0,9976 0,0533 - 0,0442 -0,0431 - 0,9774 - 0,2068 -0,0542 - 0,2044 0,9774
D
24,2016 0 0 0 23,5806 0 0 0 0,0000
Из четырех вариантов выражения (2) правильный вариант выбирается по условию нахождения точек перед камерой, т. е. проекция координат точек на ось ^ должна быть больше нуля. Таким образом, из (1) с помощью (2) могут быть получены четыре линейных уравнения:
(М1Ш^1 - т11 )%1 + (М1ш3г - ш^)у1 + (и^3 - т1з )х1 = ш^ - ;
(У1т31 - ш21)х{ + (У1ш32 - ш2г)у1 + (У1ш33 - ш23 )х1 = ш24 - у^34 ;
(игш21 -ш2п)х1 + (игш2г -ш2и)у1 + {и2ш]ъ -ш\ъ)х1 = ш24 -^4;
(Угш31 -ш21)х1 + (Угш2г -ш^у' + (^3 -ш23)4 = ш24 -Угш24.
Система уравнений является переопределенной, поэтому для решения использован метод наименьших квадратов. В результате могут быть получены трехмерные координаты характерных точек.
Для проверки работоспособности алгоритма был проведен натурный эксперимент в помещении — коридоре (рис. 2).
Сравнение положения характерных точек на изображениях и значений их двухмерных и трехмерных координат показывает, что полученные координаты характерных точек приблизительно отражают их реальное положение. Это свидетельствует о правильности метода. Время реакции составляет 0,138629 с, что доказывает быстродействие метода.
Системы координат, которые были использованы в настоящей работе, показаны на рис. 3.
Координаты характерных точек и центра масс БПЛА можно вычислить по данным визуальной одометрии [20]:
Xf -Тв
Xb,k~ LV,k
+ tVW + Rvt RWX f
^Vk^W^w,k'
vv _ TB _1_T>B Yv
Xb,k - LV,к + RV,kXw,k '
RB ,k -П R - Z l
k-1 >
JV ,k
k-1,
где хЬ,к = [х1к, Уь,к, гь,к ]т, хЬ,к = [хь,к, Уь,к, к ]т — координаты характерных точек и центра масса БПЛА в неподвижной системе координат;
хI,к = [ Х1л > у1 л > ^,к ]т > хХ,к = [ ,к > уХ,к > К,к ]т - координаты характерных точек и центра масса БПЛА в связанной системе координат; Яу, Ьу к — матрицы вращения и перемещения системы координат БПЛА на неподвижную систему координат; , Цу — матрицы преобразования и перемещения системы координат камеры на систему координат БПЛА; Ял , Ц — матрицы вращения и перемещения системы координат первого кадра изображения на систему координат второго кадра изображения.
а
Рис. 2. Реконструкция координат характерных точек: а — выделение и соответствие точек; б — двухмерные координаты точек; в — трехмерные координаты точек
В начальный момент система координат БПЛА совпадает с неподвижной системой координат. Примем, что движение бортовой камеры с достаточной точностью совпадает с движением БПЛА, т. е. матрицу принимаем единичной, а матрицу Цу — нулевой. Применение только визуального алгоритма компьютерного зрения не позволяет точно получить масштабный коэффициент р (как правило, используют сочетание с инерциальным измерительным блоком), поэтому
XCl,k(XW,k)
Рис. 3. Системы координат для момента времени к:
ObXbYbZb — базовая неподвижная система координат; OV kkXV,kYV ,kZV,k — связанная система координат БПЛА в момент к; OWkXW,kYWkkZW,k — система координат камеры в момент к; 0ClkXClkYClkZClk — система координат камеры для первого кадра изображения в момент к; Oc2kXc2kYc2kZc2>k — система координат камеры для второго кадра изображения в момент к
ошибка матрицы Ly увеличивается со временем. Для решения этой проблемы координаты БПЛА получены с помощью алгоритма SLAM, а не визуальной одо-метрией, т. е. координаты характерных точек можно найти из уравнения:
Расширенный фильтр Калмана (EKF) в алгоритме SLAM. Для алгоритма планирования полета БПЛА целесообразно использовать модель, описывающую только кинематику траекторного движения. При этом можно принять, что на каждом шаге алгоритма скорость полета не меняется, а изменения скорости от шага к шагу ограничены располагаемыми ускорениями.
Вектор состояния модели движения камеры на БПЛА и матрицу координат ориентиров в неподвижной системе координат можно записать следующим образом:
координат; п — число наблюдаемых ориентиров.
Расширенный вектор состояния камеры на БПЛА и ориентиров карты в не-
Х{к - Xb,к + RV,кXW,к.
V, кл»,к
т
подвижной системе координат имеет вид X — [XV, X M ]т.
Согласно принципу визуальной одометрии [21], уравнения состояния движения камеры на БПЛА и ориентиров карты можно записать в виде
Xk+1 -[XV,k+b XM,k ] -
Xvb,k + ( + n ) Чк+n
Y v I TJ B Yf
Xb,k + RV ,kX w, к
где П — гауссовский белый шум с нулевым средним. Вектор наблюдения
Zk - XW,к.
Оценка вектора состояния в алгоритме одновременной локализации и картографирования (SLAM) для рассматриваемой модели, которая является нелинейной, но с «гладкими» нелинейными функциями в правых частях уравнений состояний и наблюдений, может быть получена с помощью расширенного фильтра Калмана (Extended Kaiman Filter, EKF), в котором на каждом шаге проводится линеаризация путем разложения в ряд Тейлора с отбрасыванием членов ряда выше первого порядка.
Если нелинейную модель БПЛА и ориентиров карты записать в виде
X k+i - f (X k, uk)+e k; Sk - h(Xk) + nk,
где Uk — ük — управляющие переменные; ek, nk — гауссовский белый шум с нулевым математическим ожиданием и ковариациями Q и R, то применение расширенного фильтра Калмана для алгоритма SLAM можно представить в виде приведенной ниже последовательности действий.
1. Прогноз («одношаговый») состояния и ковариационной матрицы ошибок с использованием локальной линеаризации нелинейной системы:
где Fk —
df (XX k, Uk)
dX k
Xk+i|k — [ f (Xk,Uk),Mk]т;
Pk+i|k — FkPkFkT + Gk QkGk, df (Xk,Uk)
Gk -
X k ,uk
duk
матрицы частных производ-
X k ,uk
ных первого порядка (матрица Якоби) при разложении нелинейных функций / (Хк, щ) в ряд Тейлора в окрестности оценки на к-м шаге (Хк, ик):
/ (Хк, щ) / (Х к, щ )+Э/ (Хк, щ) (Хк - Хк).
дХ к Х к щк
С учетом приведенного выше анализа, ковариационная матрица может быть записана следующим образом:
" Рп (6 х 6) Рут (6 х 3п)' Рту ( 3п х 6 ) Ртт (3п X 3п)
Pk ((3n + 6)x(3n + 6))-
Здесь Pvv — ковариационная матрица положения и ориентации робота; Р,^, Руш — кросс-ковариационная матрица робота и ориентиров; Ршш — ковариационная матрица положения ориентиров.
2. Определение ошибки прогноза и ковариационной матрицы по наблюдениям на шаге к + 1:
у к+1 = ^+1 - н (хс к+1|к); 1к+1 = Hk+1рк+1|к Щ+1 + R к+1>
где Нк+1 — матрица Якоби при разложении нелинейных функций Н (Xк) в ряд Тейлора в окрестности XX к+1|к.
3. Коррекция оценок состояния X к+1 = X к+1|к + К к+1у к+1 и ковариационной матрицы Рк+1 =(1 - К к+1Нк+1 )Рк+1|к, где К к+1 = Рк^Н^^.
4. Расширение вектора состояния и ковариационной матрицы. Ориентиры, обнаруженные датчиками аппарата на каждом шаге, включают
в себя ориентиры, уже существующие на карте, а также новые ориентиры. Существовавшие ориентиры были использованы для оценок состояния на приведенной выше последовательности действий. Новые ориентиры добавляются в вектор состояния системы через процесс инициализации. Пусть на шаге к ]-й наблюдаемый ориентир в векторе наблюдения с координатами XW,{ является новым. Его координаты в неподвижной системе координат:
Xß' = g(Xy,k,Zt )= X^ + R^xßk.
Новый вектор состояния системы после расширения
Xk
X
new,k =
X f, j
Ковариационная матрица нового вектора состояния
Pnew,k =
Pvv Pvm Vg XPvv
Pmv Pmm Vg ZPmm
(VgXPvv )т (VgzPmm )т VgXPvVgXт + VgzPmmVgZт
где VgX, Vgz — матрицы Якоби g (,k, Xf'j) для Xy,k и Zk.
Ассоциация данных для алгоритма SLAM. Во-первых, необходимо обнаружить, есть ли новые ориентиры в области Si по алгоритму ассоциации данных. В настоящее время наиболее часто используемым алгоритмом ассоциации данных является алгоритм ближайшего соседа [22, 23] ввиду его простоты и небольшого объема вычислений. В этом алгоритме для ассоциации каждого наблюдаемого ориентира проводится сравнение расстояния до существующих ориентиров с заранее заданным порогом (Gate). Если расстояние от наблюдаемого до уже существующего на карте ориентира меньше определенного порога, то наблюдаемый ориентир совмещается с существующим ориентиром. Когда на
карте есть несколько ориентиров, совместимых с наблюдаемым, в качестве ассоциированного ориентира выбирается ориентир с минимальным расстоянием.
Учитывая ошибки измерений, за расстояние от i-го наблюдаемого ориентира до j-го ориентира на локальной карте целесообразно использовать расстояние Махаланобиса [23]:
M - y т J-1y ij,
где yij - yi - yj — вектор разности координат по результатам измерений; yi — вектор измерений координат i-го наблюдаемого ориентира; y j — вектор координат j-го ориентира в локальной карте; J — ковариационная матрица ошибок измерений.
Если Mij < Gate, то полагаем i-й наблюдаемый ориентир совпадающим с j-м существующим.
Если более чем один ориентир из присутствующих на локальной карте удовлетворяет условиям Mij < Gate, то в качестве ассоциированного ориентира выбираем ориентир с минимальным нормированным расстоянием Nk -- mm(yтJ-1yj + b(IJ|)).
В соответствии с проведенным анализом, этот простой алгоритм ассоциации данных не учитывает взаимосвязь между ориентирами, поэтому вероятность неправильной ассоциации у алгоритма высока. В настоящей работе для решения указанной проблемы использован алгоритм локальной ассоциации данных SLAM на основе улучшенного муравьиного алгоритма, который был изложен в работе [24]. Этот алгоритм можно разделить на два этапа: на первом этапе определяются ориентиры в локальном пространстве совпадений и наблюдаемые ориентиры, имеющие возможности ассоциации по критерию индивидуальной совместимости (Individual Compatibility, IC) [23]; на втором этапе находят совпадающие ориентиры и координаты совпадающих наблюдаемых ориентиров на множестве состояний с помощью улучшенного муравьиного алгоритма.
Алгоритм EKF-SLAM с адаптацией диапазона наблюдений. С учетом изложенного, размерность вектора состояния системы 6 + 3n. В большой и сложной среде, одновременно с приращением числа наблюдаемых ориентиров, будет также постоянно увеличиваться размер вектора состояния системы. Вычисление ковариационной матрицы и матрицы Якоби будет резко усложняться, ошибки вычисления матриц Якоби — увеличиваться, что приведет к снижению эффективности работы алгоритма и точности позиционирования. Чтобы избежать этого, предложен алгоритм EKF-SLAM с адаптацией зоны наблюдения — AR-EKF-SLAM (Adaptive Observation Range-EKF-SLAM). Суть алгоритма состоит в использовании локальной круговой карты для текущей оценки координат аппарата и локализации зоны используемых ориентиров в глобальной системе координат, с одновременным обновлением глобальной карты. Принцип локализации зоны наблюдения показан на рис. 4.
Если не изменять радиус локальной карты, то возможны следующие проблемы:
1) в среде с редкими ориентирами число наблюдаемых ориентиров в области 51 может оказаться слишком малым, даже равным нулю, вследствие чего невозможно уточнение позиционирования, т. е. ошибка предсказания будет продолжать накапливаться;
2) в среде с множественными ориентирами число ориентиров в области 5 может оказаться слишком большим, многие из этих ориентиров будут избыточными для позиционирования робота, что приведет к увеличению размерности вектора состояния и повлияет на скорость вычислений;
3) если диапазон наблюдения является слишком большим, то достоверность наблюдения отдаленных ориентиров снижается, что влияет на точность позиционирования робота.
Для решения этих проблем используем алгоритм ЕКБ-8ЬЛМ с адаптацией области наблюдения в зависимости от состояния потока поступающих ориентиров. Если число ориентиров п в наблюдаемой области 51 меньше, чем минимально необходимое для надежной коррекции прогнозируемого вектора состояния число ориентиров пт;п (п < пт;п ), и радиус наблюдения Я меньше максимального радиуса Ятах надежного наблюдения (Я < Ятах), то предлагается увеличить радиус локальной карты. Если число ориентиров больше максимального числа ориентиров птах, позволяющее избегать чрезмерной избыточности коррекции (п > птах ), или радиус наблюдения больше максимального радиуса надежного наблюдения (Я > Ятах), то предлагается уменьшать радиус локальной карты. Когда число ориентиров птп < п < птах и Я < Ятах, радиус локальной карты остается неизменным. Соответствующая диаграмма изменения радиуса локальной карты показана на рис. 5.
Рис. 4. Круговая локальная карта
и диапазон наблюдения: 5 — круговая локальная карта; 5 — диапазон наблюдения; 52 — дополнительный диапазон; • — ориентиры
Множество изменений радиуса локальной карты Q = ^з}, где ql —
увеличение радиуса; q2 — уменьшение радиуса; q3 — сохранение радиуса. Множество дискретных событий, соответствующих множеству Q, № = = {^12, w13,^21, w23,^31, w32}, где wmn — переключение из т в п; т, пе [1,2,3].
Моделирование и анализ результатов. Для иллюстрации работоспособности предлагаемых улучшений проведен эксперимент на открытой местности (в открытой среде). Экспериментальные результаты, определенные с помощью чисто визуальной навигационной системы, сравнивались с показаниями спутниковой навигационной системы. В последовательность включены 1605 изображений (кадров), полученных при движении на расстоянии около 820 м. Движение начинается из начала координат, начальный радиус локальной карты 25 м, скорость движения 0,3 м/с, минимальное число ориентиров для надежной коррекции прогнозируемого вектора состояния пт;п = 8, максимальное число ориентиров для устранения чрезмерной избыточности коррекции птах = 50, максимальный радиус надежного наблюдения Ятах = 35 м, шаг одноразового изменения радиуса локальной карты ДЯ = 0,1 м. Характерные точки (зеленые) и точки, использованные в качестве ориентиров на кадрах 583 и 584 (красные), показаны на рис. 6, а, б, вычисленные координаты ориентиров из кадров 583 и 584 — на рис. 6, в, траектория движения камеры и координаты ориентиров в неподвижной системе координат — на рис. 6, г.
_5 -ю У'м -50 О
в г
Рис. 6. Результаты эксперимента навигации по монокулярным данным 583 (а) и 584 (б) кадров изображения, вычисленные координаты ориентиров из кадров 583 и 584 (в), траектория движения камеры и координаты ориентиров в неподвижной системе координат (г)
Для проверки работоспособности предложенного алгоритма проведено сравнение времени вычислений для обработки каждого кадра изображения с постоянным диапазоном наблюдения 25 м и с адаптивным диапазоном.
Результаты, представленные на рис. 7, показывают, что время вычисления алгоритмом с постоянным диапазоном 25 м увеличивается с ростом числа кадров изображений, а время вычисления алгоритмом с адаптивным диапазоном наблюдения остается в небольшом приемлемом диапазоне.
«1 0,20
U 0,15
3
ijj 0,10
«
Ú 0,05
я
^LJi/iiwMt
100 200 300 400 500 Число кадров изображений а
100 200 300 400 500 Число кадров изображений б
Рис. 7. Изменение времени вычисления алгоритма SLAM при постоянном радиусе наблюдения 25 м (а) и при адаптивном радиусе наблюдения (б)
Для сравнения точности позиционирования традиционным и предлагаемым алгоритмами на рис. 8 приведены изменения дисперсии ошибок определения положения БПЛА, т. е. дисперсии позиционирования БПЛА по координатам х в момент k:
SEX,k =(xk -Xk )2, где xk, xk — реальные и спрогнозированные координаты БПЛА.
200 300 400
Число кадров изображений
Рис. 8. Дисперсия позиционирования по координате x
Ошибка между реальным и спрогнозированным положением БПЛА алгоритма EKF-SLAM с постоянным диапазоном наблюдения достигает значительных величин, а для предлагаемого алгоритма с адаптивным диапазоном наблю-
дения эта ошибка мала и соизмерима с ошибкой традиционного алгоритма EKF-SLAM с большим диапазоном наблюдения, хотя и обеспечивается при гораздо меньшем времени вычислений.
Для проверки точности предложенного метода проведено сравнение траектории, рассчитанной по данным монокулярной съемки, с информацией спутниковой (GPS) навигации. Результаты показаны на рис. 9 и рис. 10.
300
X, м
-200 0
-10 _ю
в г
Рис. 9. Результаты эксперимента навигации по монокулярным данным 1604 (а) и 1605 (б) кадров изображения, вычисленные координаты ориентиров из кадров 1604 и 1605 (в), траектория движения камеры и координаты ориентиров в неподвижной системе координат (г)
у, м
50 100 150 200 250 х, м
а б
Рис. 10. Вычисленная траектория движения на плоскости Oxy (а) и истинная траектория движения (б) на спутниковой карте
Сравнение результата эксперимента, приведенного на рис. 9 и 10, показывает, что предсказанная траектория движения с приемлемой точностью совпадает с истинной траекторией движения на спутниковой карте, что подтверждает правильность алгоритма.
Заключение. Для решения проблемы высокой вычислительной сложности и низкой точности позиционирования стандартного алгоритма EKF-SLAM предложен улучшенный алгоритм EKF-SLAM с адаптивным диапазоном наблюдения (AR-EKF-SLAM). Диапазон наблюдения БПЛА и локальная карта динамически ограничивают круговой областью с изменяемым радиусом, при этом рассматривают лишь ориентиры, удовлетворяющие вводимым ограничениям. В результате размер вектора состояния системы не будет постоянно увеличиваться с приращением числа наблюдаемых ориентиров, и, как следствие, не будет возрастать время расчета при сохранении достаточной точности позиционирования. Результаты моделирования показывают, что предложенный метод может эффективно улучшить точность позиционирования, скорость вычисления и эффективность работы алгоритма одновременной локализации и картографирования при оперативном в реальном времени планировании маршрута БПЛА, особенно в сложных и протяженных средах.
ЛИТЕРАТУРА
1. Cheeseman P., Smith R., Self M. A stochastic map for uncertain spatial relationships // 4th Int. Symp. on Robotic Research. 1987. P. 467-474.
2. Biswas J., Veloso M. Depth camera based indoor mobile robot localization and navigation // IEEE Int. Conf. on Robotics and Automation. 2012. P. 1697-1702.
DOI: 10.1109/ICRA.2012.6224766 URL: http://ieeexplore.ieee.org/document/6224766
3. Tu Y., Huang Z., Zhang X., et al. The mobile robot SLAM based on depth and visual sensing in structured environment // Robot Intelligence Technology and Applications 3. 2015. P. 343-357. DOI: 10.1007/978-3-319-16841-8_32
URL: https://link.springer.com/chapter/10.1007/978-3-319-16841-8_32
4. Choi Y.W., Kim K.D., Choi J.W., Lee S.G. Laser image SLAM based on image matching for navigation of a mobile robot // Journal of the Korean Society for Precision Engineering. 2013. Vol. 30. No. 2. P. 177-184. DOI: 10.7736/KSPE.2013.30.2.177
URL: http://koreascience.or.kr/article/ArticleFullRecord.jsp?cn=JMGHBV_2013_v30n2_177
5. Fabresse F.R., Caballero F., Maza I., Ollero A. Localization and mapping for aerial manipulation based on range-only measurements and visual markers // IEEE Int. Conf. on Robotics and Automation (ICRA). 2014. P. 2100-2106. DOI: 10.1109/ICRA.2014.6907147
URL: http://ieeexplore.ieee.org/document/6907147
6. Roh H.C., Sung C.H., Kang M.T., Chung M.J. Fast SLAM using polar scan matching and particle weight based occupancy grid map for mobile robot // 8th Int. Conf. on Ubiquitous Robots and Ambient Intelligence (URAI). 2011. P. 756-757. DOI: 10.1109/URAI.2011.6146004
URL: http://ieeexplore.ieee.org/document/6146004
7. Qu L., He S., Qu Y. An SLAM algorithm based on improved UKF // 24th Chinese Control and Decision Conf. (CCDC). 2012. P. 4154-4157. DOI: 10.1109/CCDC.2012.6243112
URL: http://ieeexplore.ieee.org/document/6243112
8. Chatterjee A., Ray O., Chatterjee A., Rakshit A. Development of a real-life EKF based SLAM system for mobile robots employing vision sensing // Expert Systems with Applications. 2011. Vol. 38. No. 7. P. 8266-8274. DOI: 10.1016/j.eswa.2011.01.007
URL: http://www.sciencedirect.com/science/article/pii/S0957417411000273
9. Sola J., Vidal-Calleja T., Civera J., Montiel J.M.M. Impact of landmark parametrization on monocular EKF-SLAM with points and lines // Int. Journal of Computer Vision. 2012. Vol. 97. No. 3. P. 339-368. DOI: 10.1007/s11263-011-0492-5
URL: https://link.springer.com/article/10.1007/s11263-011-0492-5
10. Consistency of the EKF-SLAM algorithm / T. Bailey, J. Nieto, J. Guivant, M. Stevens, E. Nebot // IEEE/RSJ Int. Conf. on Intelligent Robots and Systems. 2006. P. 3562-3568.
DOI: 10.1109/IR0S.2006.281644 URL: http://ieeexplore.ieee.org/document/4058955
11. Geng Ke Ke. An improved EKF-SLAM algorithm for mobile robot // Интернаука. 2016. № 2. С. 74-78.
12. Krig S. Computer vision metrics: Survey, taxonomy, and analysis. Apress, 2014.
13. Smith S.M., Brady J.M. SUSAN — a new approach to lowlevel image processing // Int. Journal of Computer Vision. 1997. Vol. 23. No. 1. P. 45-78. DOI: 10.1023/A: 1007963824710 URL: https://link.springer.com/article/10.1023%2FA%3A1007963824710
14. Гэн Кэ Кэ, Чулин Н.А. Метод реконструкции облачно-точечной карты окружающей среды на основе монокулярного компьютерного зрения в режиме реального времени // Международный журнал экспериментального образования. 2015. № 12-3. С. 437-442. URL: https://expeducation.ru/ru/article/view?id=9163
15. Di Stefano L., Mattoccia S., Mola M. An efficient algorithm for exhaustive template matching based on normalized cross correlation // Proc. 12th Int. Conf. on Image Analysis and Processing. 2003. P. 322-327. DOI: 10.1109/ICIAP.2003.1234070
URL: http://ieeexplore.ieee.org/document/1234070
16. Armangué X., Salvi J. Overall view regarding fundamental matrix estimation // Image and Vision Computing. 2003. Vol. 21. No. 2. P. 205-220. DOI: 10.1016/S0262-8856(02)00154-3
URL: http://www.sciencedirect.com/science/article/pii/S0262885602001543
17. Camera calibration toolbox for Matlab // Vision.caltech: веб-сайт
URL: http://www.vision.caltech.edu/bouguetj/calib_doc (дата обращения: 19.10.2016).
18. Hartley R.I. Estimation of relative camera positions for uncalibrated cameras // European Conf. on Computer Vision. 1992. P. 579-587. DOI: 10.1007/3-540-55426-2_62
URL: https://link.springer.com/chapter/10.1007/3-540-55426-2_62?no-access=true
19. Hartley R., Zisserman A. Multiple view geometry in computer vision. Cambridge University Press, 2003.
20. Bunschoten R., Krose B. Visual odometry from an omnidirectional vision system // IEEE Int. Conf. on Robotics and Automation. 2003. Vol. 1. P. 577-583. DOI: 10.1109/ROBOT.2003.1241656 URL: http://ieeexplore.ieee.org/document/1241656
21. Nistér D., Naroditsky O., Bergen J. Visual odometry // Proc. 2004 IEEE Computer Society Conf. on Computer Vision and Pattern Recognition. 2004. No. 1. P. 652-659.
DOI: 10.1109/CVPR.2004.1315094 URL: http://ieeexplore.ieee.org/document/1315094
22. A solution to the simultaneous localization and map building (SLAM) problem / M.G. Dis-sanayake, P. Newman, S. Clark, H.F. Durrant-Whyte, M. Csorba // IEEE Transactions on Robotics and Automation. 2001. Vol. 17. No. 3. P. 229-241. DOI: 10.1109/70.938381
URL: http://ieeexplore.ieee.org/document/938381
23. Bailey T. Mobile robot localization and mapping in extensive outdoor environments. The University of Sydney, 2002.
24. Гэн Кэ Кэ, Чулин Н.А. Алгоритм локальной ассоциации данных SLAM на основе улучшенного муравьиного алгоритма // Наука и образование: научное издание МГТУ им. Н.Э. Баумана. 2015. С. 340-355. DOI: 10.7463/1015.0818707
URL: http://technomag.edu.ru/jour/article/view/165
Гэн Кэ Кэ — аспирант кафедры «Системы автоматического управления» МГТУ им. Н.Э. Баумана (Российская Федерация, 105005, Москва, 2-я Бауманская ул., д. 5, стр. 1).
Чулин Николай Александрович — канд. техн. наук, доцент кафедры «Системы автоматического управления» МГТУ им. Н.Э. Баумана (Российская Федерация, 105005, Москва, 2-я Бауманская ул., д. 5, стр. 1).
Просьба ссылаться на эту статью следующим образом:
Гэн Кэ Кэ, Чулин Н.А. Алгоритм навигации беспилотного летательного аппарата на основе улучшенного алгоритма одновременной локализации и картографирования с адаптивным локальным диапазоном наблюдения // Вестник МГТУ им. Н.Э. Баумана. Сер. Приборостроение. 2017. № 3. C. 76-94. DOI: 10.18698/0236-3933-2017-3-76-94
UAV NAVIGATION ALGORITHM BASED ON IMPROVED ALGORITHM OF SIMULTANEOUS LOCALIZATION AND MAPPING WITH ADAPTIVE LOCAL RANGE OF OBSERVATIONS
Ke Ke Geng [email protected]
N-А. Chulin [email protected]
Bauman Moscow State Technical University, Moscow, Russian Federation
Abstract
The paper proposes an improved algorithm for the extended Kalman filter for simultaneous localization and mapping (EKF-SLAM), allowing the essential reduction of the amount of computation required by adapting the range of observation in real time in different three-dimensional environments for unmanned aerial vehicles (UAVs). We built cloudy-point environment map and calculated coordinates of the characteristic points using 8-point normalized algorithm based on computer vision monocular. The improvement is achieved by adaptive dynamic restriction of the current dimensions of the environmental observable part and the number of observable targets for UAV positioning correction. The simulation results show that the proposed method significantly reduces the volume of calculations, while maintaining the accuracy of localization, and can be applied to the UAV navigation
Keywords
UAV, improved algorithm for EKF-SLAM, data fusion, characteristic points
REFERENCES
[1] Cheeseman P., Smith R., Self M. A stochastic map for uncertain spatial relationships. 4th Int. Symp. on Robotic Research, 1987, pp. 467-474.
[2] Biswas J., Veloso M. Depth camera based indoor mobile robot localization and navigation. IEEE Int. Conf. on Robotics and Automation, 2012, pp. 1697-1702. DOI: 10.1109/ICRA.2012.6224766 Available at: http://ieeexplore.ieee.org/document/6224766
[3] Tu Y., Huang Z., Zhang X., et al. The mobile robot SLAM based on depth and visual sensing in structured environment. Robot Intelligence Technology and Applications 3, 2015, pp. 343-357. DOI: 10.1007/978-3-319-16841-8_32 Available at: https://link.springer.com/chapter/10.1007/ 978-3-319-16841-8_32
[4] Choi Y.W., Kim K.D., Choi J.W., Lee S.G. Laser image SLAM based on image matching for navigation of a mobile robot. Journal of the Korean Society for Precision Engineering, 2013, vol. 30, no. 2, pp. 177-184. DOI: 10.7736/KSPE.2013.30.2.177 Available at: http://koreascience.or.kr/article/ ArticleFullRecord.jsp?cn=JMGHBV_2013_v30n2_177
[5] Fabresse F.R., Caballero F., Maza I., Ollero A. Localization and mapping for aerial manipulation based on range-only measurements and visual markers. IEEE Int. Conf. on Robotics and Automation (ICRA), 2014, pp. 2100-2106. DOI: 10.1109/ICRA.2014.6907147
Available at: http://ieeexplore.ieee.org/document/6907147
[6] Roh H.C., Sung C.H., Kang M.T., Chung M.J. Fast SLAM using polar scan matching and particle weight based occupancy grid map for mobile robot. 8th Int. Conf. on Ubiquitous Robots and Ambient Intelligence (URAI), 2011, pp. 756-757. DOI: 10.1109/URAI.2011.6146004
Available at: http://ieeexplore.ieee.org/document/6146004
[7] Qu L., He S., Qu Y. An SLAM algorithm based on improved UKF. 24th Chinese Control and Decision Conf. (CCDC), 2012, pp. 4154-4157. DOI: 10.1109/CCDC.2012.6243112
Available at: http://ieeexplore.ieee.org/document/6243112
[8] Chatterjee A., Ray O., Chatterjee A., Rakshit A. Development of a real-life EKF based SLAM system for mobile robots employing vision sensing. Expert Systems with Applications, 2011, vol. 38, no. 7, pp. 8266-8274. DOI: 10.1016/j.eswa.2011.01.007 Available at: http://www.sciencedirect.com/ science/article/pii/S0957417411000273
[9] Sola J., Vidal-Calleja T., Civera J., Montiel J.M.M. Impact of landmark parametrization on monocular EKF-SLAM with points and lines. Int. Journal of Computer Vision, 2012, vol. 97, no. 3, pp. 339-368. DOI: 10.1007/s11263-011-0492-5
Available at: https://link.springer.com/article/10.1007/s11263-011-0492-5
[10] Bailey T., Nieto J., Guivant J., Stevens M., Nebot E. Consistency of the EKF-SLAM algorithm. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2006, pp. 3562-3568.
DOI: 10.1109/IROS.2006.281644 Available at: http://ieeexplore.ieee.org/document/4058955
[11] Ke Ke Geng. An improved EKF-SLAM algorithm for mobile robot. Internauka, 2016, no. 2, pp. 74-78.
[12] Krig S. Computer vision metrics: Survey, taxonomy, and analysis. Apress, 2014.
[13] Smith S.M., Brady J.M. SUSAN — a new approach to lowlevel image processing. Int. Journal of Computer Vision, 1997, vol. 23, no. 1, pp. 45-78. DOI: 10.1023/A:1007963824710
Available at: https://link.springer.com/article/10.1023%2FA%3A 1007963824710
[14] Geng Ke Ke, Chulin N.A. Environment reconstruction method with cloudy-point maps based on real-time monocular computer vision. Mezhdunarodnyy zhurnal eksperimental'nogo obra-zovaniya [International Journal of Experimental Education], 2015, no. 12-3, pp. 437-442. Available at: https://expeducation.ru/ru/article/view?id=9163
[15] Di Stefano L., Mattoccia S., Mola M. An efficient algorithm for exhaustive template matching based on normalized cross correlation. Proc. 12th Int. Conf. on Image Analysis and Processing, 2003, pp. 322-327. DOI: 10.1109/ICIAP.2003.1234070 Available at: http://ieeexplore.ieee.org/ document/1234070
[16] Armangué X., Salvi J. Overall view regarding fundamental matrix estimation. Image and Vision Computing, 2003, vol. 21, no. 2, pp. 205-220. DOI: 10.1016/S0262-8856(02)00154-3 Available at: http://www.sciencedirect.com/science/article/pii/S0262885602001543
[17] Camera calibration toolbox for Matlab. vision.caltech: website
Available at: http://www.vision.caltech.edu/bouguetj/calib_doc (accessed 19.10.2016).
[18] Hartley R.I. Estimation of relative camera positions for uncalibrated cameras. European Conf. on Computer Vision, 1992, pp. 579-587. DOI: 10.1007/3-540-55426-2_62
Available at: https://link.springer.com/chapter/10.1007/3-540-55426-2_62?no-access=true
[19] Hartley R., Zisserman A. Multiple view geometry in computer vision. Cambridge University Press, 2003.
[20] Bunschoten R., Krose B. Visual odometry from an omnidirectional vision system. IEEE Int. Conf. on Robotics and Automation, 2003, vol. 1, pp. 577-583. DOI: 10.1109/ROBOT.2003.1241656 Available at: http://ieeexplore.ieee.org/document/1241656
[21] Nistér D., Naroditsky O., Bergen J. Visual odometry. Proc. 2004 IEEE Computer Society Conf. on Computer Vision and Pattern Recognition, 2004, no. 1, pp. 652-659.
DOI: 10.1109/CVPR.2004.1315094 Available at: http://ieeexplore.ieee.org/document/1315094
[22] Dissanayake M.G., Newman P., Clark S., Durrant-Whyte H.F., Csorba M. A solution to the simultaneous localization and map building (SLAM) problem. IEEE Transactions on Robotics and Automation, 2001, vol. 17, no. 3, pp. 229-241. DOI: 10.1109/70.938381
Available at: http://ieeexplore.ieee.org/document/938381
[23] Bailey T. Mobile robot localization and mapping in extensive outdoor environments. The University of Sydney, 2002.
[24] Gen Ke Ke, Chulin N.A. Algorithm of particle data association for SLAM based on improved ant algorithm. Nauka i obrazovanie: nauchnoe izdanie MGTU im. N.E. Baumana [Science and Education: Scientific Publication of BMSTU], 2015, pp. 340-355. DOI: 10.7463/1015.0818707 Available at: http://technomag.edu.ru/jour/article/view/165
Geng Ke Ke — post-graduate student of Automatic Control Systems Department, Bauman Moscow State Technical University (2-ya Baumanskaya ul. 5, str. 1, Moscow, 105005 Russian Federation).
Chulin N.A. — Cand. Sc. (Eng.), Assoc. Professor of Automatic Control Systems Department, Bauman Moscow State Technical University (2-ya Baumanskaya ul. 5, str. 1, Moscow, 105005 Russian Federation).
Please cite this article in English as:
Geng Ke Ke, Chulin N.A. UAV Navigation Algorithm Based on Improved Algorithm of Simultaneous Localization and Mapping with Adaptive Local Range of Observations. Vestn. Mosk. Gos. Tekh. Univ. im. N.E. Baumana, Priborostr. [Herald of the Bauman Moscow State Tech. Univ., Instrum. Eng.], 2017, no. 3, pp. 76-94. DOI: 10.18698/0236-3933-2017-3-76-94
