CC BY
99
Механика и математическое моделирование
Вестник ДВО РАН. 2016. № 4
УДК 519.674
В.А. БОБКОВ, А.П. КУДРЯШОВ, С В. МЕЛЬМАН, М.А. МОРОЗОВ
Восстановление траектории движения робота и реконструкция среды по изображениям
Описан подход к решению задач визуальной навигации автономного подводного робота (АПР) и SD-реконструкции подводной обстановки, основанный на визуальной одометрии с использованием стереоизображений. Представлены возможности программного моделирующего комплекса для исследования методов управления движением АПР.
Ключевые слова: компьютерное зрение, последовательности стереоизображений, визуальная одометрия, eD-реконструкция, имитационное моделирование.
Recovering of vehicle movement pattern and environment reconstruction from images. V.A. BOBKOV, A.P. KUDRYASHOV, C.V. MELMAN, M.A. MOROZOV (Institute of Automation and Control Processes, FEB RAS, Vladivostok).
This article describes an approach to solving problems of visual navigation of autonomous underwater vehicle (AUV) and the 3D reconstruction of the underwater environment, based on visual odometry using stereo images. Possibilities of software simulation complex for the study of AUV motion control methods are also presented.
Key words: computer vision, stereo image sequences, visual odometry, 3D-reconstruction, simulation.
Введение
Проблема восстановления структуры среды и движения по изображениям является в компьютерном зрении одной из основных проблем, активно решаемой отечественными и зарубежными исследователями в последние 2-3 десятилетия. В робототехнике эта проблема известна под аббревиатурой SLAM (simultaneous localization and mapping). В литературе используется также термин visual SLAM, когда речь идет о решении задачи восстановления траектории робота и реконструкции среды методами компьютерного зрения. Несмотря на достигнутый прогресс в этой области, проблема остается актуальной, поскольку требования со стороны прикладных задач в достигаемой точности и производительности методов/алгоритмов постоянно растут. Подводная робототехника применяется сегодня для решения широкого круга прикладных задач, включая поисковые и спасательные работы, океанологические и экологические измерения, инспекцию подводных коммуникаций, поиск источников экологического загрязнения акваторий и др.
*БОБКОВ Валерий Александрович - доктор технических наук, заведующий лабораторией, КУДРЯШОВ Алексей Павлович - кандидат технических наук, младший научный сотрудник, МЕЛЬМАН Сергей Владимирович -кандидат технических наук, старший инженер-программист, МОРОЗОВ Михаил Александрович - ведущий инженер-программист (Институт автоматики и процессов управления, ДВО РАН, Владивосток). *Е-таП: bobkov@iacp.dvo.ru
Работа выполнена при финансовой поддержке РФФИ (проекты № 15-07-00341, № 16-07-00350) и программы «Дальний Восток» (проект 15-1-4-025).
Важной при этом является задача точной локализации и навигации автономных подводных роботов (АПР) в априори неизвестной сложной подводной обстановке. Разрабатываемые в последнее время визуальные методы навигации служат хорошим дополнением и, в отдельных случаях, альтернативой традиционно используемым гидроакустическим средствам навигации. В частности, представляет интерес подход, известный как визуальная одометрия с использованием стереоизображений [6, 8, 11].
Другой аспект проблемы - реконструкция пространственных объектов среды по изображениям - также имеет большое практическое значение. Для решения этой задачи предлагаются различные методы; одним из хорошо зарекомендовавших себя считается воксельный подход, впервые предложенный в [7] и успешно используемый для реконструкции подводной обстановки в [9]. Однако его недостатком является значительная вычислительная трудоемкость, затрудняющая его применение в интерактивном режиме.
При разработке методов навигации, управления автономными роботами, а также методов построения 3D-моделей объектов окружающей обстановки важным инструментом автоматизации научных исследований являются системы имитационного моделирования, которые позволяют существенно повысить эффективность исследований и сократить материальные и временные затраты на этапе, предваряющем натурные испытания. Такого рода компьютерные системы, именуемые симуляторами, тренажерами и моделирующими комплексами, позволяют создавать виртуальную среду, в которой имитируется не только обстановка, но и работа сенсоров и функциональных систем автономных роботов. Обзор работ в этом направлении можно найти в [10].
В настоящей статье затрагивается обозначенный круг вопросов в рамках предлагаемого подхода к решению задач визуальной навигации АПР и 3D-реконструкции подводной среды с применением программного моделирующего комплекса.
Базовый метод визуальной навигации
Разработанный авторами ранее базовый метод визуальной навигации (без учета самопересечений траектории) [1, 6], следуя классической схеме реализации визуальной одометрии, содержит следующие шаги:
1. Выделение общего множества особенностей на четырех изображениях двух стереопар. Сопоставление точечных особенностей выполняется на 2 стереопарах изображений (1-2 и 3-4), соответствующих двум последовательным позициям автономного подводного аппарата (АПА) на траектории. Для сопоставления особенностей используется детектор SURF (библиотека OpenCV) или трекер KLT. Для каждой пары изображений выполняется сопоставление слева направо и справа налево (cross-checking). Для исключения ложных сопоставлений применяется эпиполярный фильтр для той и другой пары. Полученное в итоге множество особенностей, сопоставленных для всех 4 изображений (рис. 1), позволяет далее построить два сопоставленных облака 3D-точек, отвечающих двум стереопарам (двум позициям АПА).
2. Генерация и фильтрация двух облаков 3D-точек.
3. Вычисление с помощью алгоритма ICP (iterative closest points) локального геометрического преобразования H, связывающего локальные системы координат (СК) двух соседних позиций. Поиск этого преобразования основывается на имеющемся взаимно-однозначном сопоставлении двух облаков (множеств) 3D-точек, наблюдаемых стереокамерой в позициях 1 и 2 соответственно. Первое облако C'(x, y, z) задано в СК 1-й стереопары, второе C2(x, y, z) - в СК 2-й стереопары. Задача нахождения матрицы H формулируется как оптимизационная задача и решается применением процедуры библиотеки общего пользования MatLab. В качестве параметров оптимизации используются 3 координаты вектора переноса и 4 координаты кватерниона, определяющего вращение в H. Ограничение задается условием: норма кватерниона = 1.
Рис. 1. Построение единого множества особенностей на 4 изображениях двух стереопар
Целевая функция Е = ^ |\с\ - с\* н||, где k - номер точки в облаке.
Здесь {о\} - множество точек в первом облаке и {с2к} - множество точек во втором облаке. Тогда с учетом вычисленной матрицы Н новое положение АНПА вычисляется через предыдущее как р2 = р1 * Н.
4. Вычисление параметров (6DOF) текущей позиции АПА в мировой СК путем объединения последовательности локальных преобразований предшествующих позиций.
Адаптивный метод
При реализации визуального метода обычно используется схема расчета траектории с фиксированным шагом выбора обсчитываемых позиций АПР. Однако она не оптимальна, поскольку не учитывает характера движения АПР в условиях конкретной сцены. В частности, не гарантируется достаточное количество сопоставленных особенностей; не учитывается форма траектории и динамика движения АПР; число накапливаемых в буфере памяти кадров съемки при реальном движении аппарата не будет соответствовать фиксированному шагу. Как следствие, это препятствует обеспечению режима реального времени в случае вычислительной трудоемкости используемых алгоритмов. Предлагаемая адаптивная методика [3] учитывает геометрию сцены и требования по реализуемой алгоритмической эффективности (высота траектории над морским дном, скорость движения АПР, степень перекрытия соседних видов камеры). Согласно методу визуальной одометрии вычисление текущей позиции АПР основывается на результатах обработки всех предшествующих позиций. Поэтому для соответствия режиму реального времени необходима скорость алгоритмических вычислений, достаточная для того, чтобы время компьютерной обработки поступающих визуальных данных не превышало времени движения АПР по траектории на текущий момент. Адаптивность реализуется тремя совместно работающими механизмами: а) выбором размера очередного шага в зависимости от степени перекрытия зон видимости стереокамеры для двух соседних позиций; б) уменьшением шага, если число сопоставленных особенностей меньше заданного порога;
в) отсечением тех частей изображения, которые не относятся к перекрытию зон видимости. Оно выполняется непосредственно перед генерацией и сопоставлением особенностей в каждом из 4 исходных изображений.
Первый механизм позволяет задавать максимальный размер очередного шага, гарантирующий заданную величину общей (для двух позиций) зоны видимости, что направлено на минимизацию числа обсчитываемых позиций и, соответственно, на уменьшение времени счета. Уменьшение или увеличение размера общей области видимости позволяет увеличивать или, соответственно, уменьшать максимальный шаг. Зависимость между величиной перемещения камеры и величиной общей зоны видимости определяется геометрией, показанной на рис. 2.
Рис. 2. Вычисление адаптивного шага в зависимости от зоны общей видимости
Второй механизм - уменьшение шага - направлен на поддержание числа особенностей на уровне не ниже заданного порога, что необходимо для обеспечения высокой точности вычисления матрицы локального преобразования (определяет перемещение АПА из предыдущей позиции в текущую). Если количество сопоставленных особенностей меньше заданного порога, то делается повторное сопоставление после деления шага пополам (в экспериментах порог варьировал от 30 до 100 точечных особенностей). Процесс итеративный, пока не выполнится условие или размер шага не достигнет 1 кадра. Этот же алгоритм обеспечивает и последующее постепенное восстановление размера шага до максимального расчетного путем его удвоения.
Механизм отсечения части изображения, не относящейся к общей зоне видимости, дает два преимущества. Во-первых, сокращение времени обработки на этапе сопоставления особенностей за счет меньших размеров обрабатываемых изображений. Во-вторых, эта процедура служит также и фильтром, поскольку естественным образом исключает возможные ошибочные сопоставления особенностей на неперекрывающихся областях, что в конечном счете приводит к повышению точности вычислений локальных преобразований. Последующие эксперименты подтвердили это предположение.
Локальное маневрирование
Предлагаемый метод визуальной навигации применительно к ситуации локального маневрирования АПА (когда движение осуществляется в ограниченной области подводной среды с возможными самопересечениями траектории) основывается на описанном
выше и реализованном авторами базовом методе визуальной одометрии. Цель предлагаемого метода - воспрепятствовать накоплению ошибки навигации АПА при длительном локальном маневрировании за счет обработки ситуаций повторного посещения аппаратом одних и тех же мест и тем самым повысить точность навигации в целом. Входной информацией служит видеопоток, фиксируемый при движении АПА стереокамерой (направленной вниз).
В работе метода выделяются два этапа, которые соответствуют двум этапам движения АПА по траектории. На первом формируется сеть опорных систем координат (ОСК), которая при последующем движении АПА, т.е. на втором этапе, используется для повышения точности вычисления траектории АПА за счет возможных привязок к ОСК (рис. 3). В качестве ОСК рассматриваются локальные системы координат АПА/камеры в конкретных позициях траектории. Все ОСК связаны с мировой системой координат (МСК) через цепочку матриц геометрических преобразований, которые: а) порождаются непосредственно при работе базового метода (число матриц равно числу шагов) или б) берутся из уже существующей ОСК в случае успешной координатной к ней привязки (тогда число матриц = числу матриц в ОСК + 1). Параметры траектории вычисляются в МСК, которая фиксируется в начальный момент времени. Поскольку накопление ошибки напрямую зависит от длины цепочки преобразований, то степень «точности» конкретной ОСК можно характеризовать степенью ее «близости» к МСК. То есть можно присвоить каждой ОСК коэффициент степени близости, равный длине цепочки преобразований, ведущей к МСК (если связь напрямую, то k = 1). Чем короче цепочка преобразований, тем меньшую ошибку при вычислении 6DOF она порождает. Этот коэффициент можно использовать для оптимального выбора ОСК на этапе навигации АПА в случае, когда АПА «видит» более одной ОСК.
На рис. 4 показан пример виртуальной сцены для тестирования метода. Для этой сцены накопленная ошибка в конце траектории при работе данного метода навигации уменьшается в сравнении с базовым методом с 13 до 1,6 см.
Рис. 4. Траектория движения АПР для тестирования визуального метода навигации в условиях локального маневрирования
/Я
//1
/II 1
7\
( 1
1 I 1
I I 1
/ I 1
I I I
к! \1
Рис. 3. Привязка текущей позиции АПР к ОСК
3D-реконструкция
В каждой позиции АПА на траектории можно построить 3D-модель (локальную реконструкцию) части сцены, наблюдаемой из этой точки. Для этого используется 3D-облако точек, сгенерированное методом визуальной навигации, или карта глубин, полученная «дальномером». Моделью служит 2.5D-триангуляционная сетка, построенная с помощью 2D-триангуляции множества 2D-точек на виде и их 3D-координат. Полная модель создается объединением локальных реконструкций. Для этого их необходимо поместить в одну систему координат и сформировать единую модель, исключив избыточность, возникающую из-за перекрытия смежных видов. Первая задача решается, как было описано выше, методом визуальной навигации - для каждой позиции вычисляется преобразование в МСК. Для решения задачи объединения локальных реконструкций был применен воксельный подход, реализация которого описана в [7]. В настоящей работе используется модифицированная версия этой реализации, обеспечивающая более высокие производительность и качество.
Описание метода. Пусть имеется п триангуляционных сеток, построенных вышеуказанным способом. В воксельном пространстве сцены задается непрерывная неявная функция D(V.), представленная значениями в узлах воксельной решетки. Функция конструируется как взвешенная сумма получаемых для п видов расстояний й^У.), й2(У.),... йп(У.) от точки V. до ближайшей поверхности. Расстояние й берется на луче, направленном из центра проекций вида. в точку V , и является величиной со знаком (положительной для точек, находящихся перед поверхностью, и отрицательной для точек за поверхностью).
В качестве весовой функции W(V.) берется скалярное произведение вектора нормали к поверхности и вектора направления на точку наблюдения. Такой выбор исходит из предположения, что при корреляционном подходе к определению расстояния до поверхности степень неопределенности для наклонных поверхностей выше, чем для поверхностей, наблюдаемых под прямым углом.
Для вычисления D и W используются следующие инкрементальные правила:
о.+1(У.) = №.(у.)Б.(У.) + ^.+1(У)а.+1(У)] / №.(У) + ^.+1(У)], ^.+1(У) = + ^т^да
где I - номер воксела, а . - номер вида.
Весовая функция W(V.) - константа для всех вокселов до видимой поверхности (для данного вида), а для вокселов за поверхностью линейно убывает до нуля в пределах е-окрестности. Такой выбор области определения весовой функции направлен на предотвращение возникновения ложных поверхностей.
В результате последовательной обработки всех видов каждому вокселу присваивается значение интегральной функции расстояния D(V.) (величина со знаком) и значение интегральной весовой функции W(V .). Тогда построение изоповерхности D(V.) = 0 и есть решение нашей задачи. Преимуществом сведения исходной задачи к такой постановке является то, что построение изоповерхности скалярного поля, заданного на воксельной решетке, можно выполнить с помощью известного алгоритма марширующих кубиков.
Поскольку наиболее ресурсоемкой частью описанного метода является определение расстояний от вокселов до ближайшей поверхности, для ускорения вычислений был реализован алгоритм с предварительной индексацией всех треугольников сетки каждого вида.
Это позволило избежать полного перебора по всем треугольникам каждого вида и существенно уменьшить время работы алгоритма.
На последнем этапе, после формирования скалярного поля с помощью алгоритма марширующих кубиков, выполняется построение изоповерхности D(V.) = 0, которое и является решением задачи построения общей триангуляционной сетки по множеству видов.
Рис. 5. Пример 3D-реконструкции объекта по данным дальномера воксельным методом
Применение аппаратного ускорения (графические процессоры) с помощью средств CUDA-технологии дало дополнительное ускорение работы алгоритма до 30 раз в сравнении с обычной версией (только центральный процессор) [4]. На рис. 5 показаны результаты 3D-реконструкции объекта по изображениям.
Моделирующий комплекс
Имитационный программный моделирующий комплекс (МК) предназначен для проведения компьютерных экспериментов, направленных как на моделирование рабочих миссий (океанологические и экологические измерения, инспекция подводных коммуникаций, поисковые и спасательные работы и др.) автономных подводных роботов, так и на решение задач разработки, исследования и отладки алгоритмов и методов, используемых в программном интеллекте робота. В конечном счете компьютерное имитационное моделирование позволяет сократить организационные, финансовые и временные затраты на проведение натурных испытаний.
В основу МК заложен модульный принцип, согласно которому все функциональные блоки были реализованы в качестве отдельных программных динамических модулей (бортовые сенсоры, расчет движения, управление и т.д.). Необходимые динамические модули при моделировании подключаются к МК автоматически. Такой подход позволяет наращивать алгоритмическую базу без изменений основного приложения программного комплекса. В текущей версии МК [2, 5] реализованы возможности распределенно-параллельных вычислений, что существенно расширяет круг решаемых вычислительно сложных задач.
С помощью МК моделируются подводная обстановка (рис. 6), в которой перемещается виртуальный АПР, а также сенсоры (рис. 7), система управления АПР и другие его функциональные системы, обеспечивающие движение по планируемой траектории и выполнение рабочей миссии. Комплекс предоставляет наряду с возможностями моделирования и управления миссиями также развитые инструменты визуализации результатов моделирования в наглядной форме, позволяя выбирать любой момент миссии для анализа и оценки эффективности заложенных решений. Отличительной особенностью комплекса является возможность сопряжения с ним программной среды реального АПР, что позволяет использовать комплекс и для испытаний систем управления реальных АПР. Таким образом, МК обеспечивает решение следующих основных задач: • отладка миссии аппарата;
• исследование методов управления АПР и отладка системы управления путем «погружения» в среду имитационного комплекса;
• поддержка режима виртуальной реальности и генерирование данных бортовой регистрирующей аппаратуры.
К другим функциональным возможностям МК относятся:
• использование МК в качестве тренажерного комплекса для операторов АПР;
• тестирование работоспособности оборудования и бортового программного обеспечения подводного аппарата при его подключении к виртуальной среде моделирующего комплекса;
• исследование методов распознавания объектов (трубопроводы, месторождения полезных ископаемых, затонувшие объекты) с использованием штатных гидроакустических средств навигации;
• исследование методов визуальной навигации АПР;
• моделирование нештатных ситуаций, происходящих как на борту АПР, так и вне его.
Применительно к проведению компьютерных экспериментов для исследования рассматриваемого метода визуальной навигации, средствами МК: а) генерируются виртуальные сцены подводной обстановки разной сложности с нанесением на рельеф дна реальной
а б и г
Рис. 7. Моделирование работы гидролокатора. Общий вид сцены (а), интенсивность отраженного сигнала (б), буфер глубины (в), шкала дальности для буфера глубины (г)
Рис. 8. Кадр анимации виртуальной сцены, сгенерированной для тестирования визуального метода навигации
текстуры; б) осуществляется движение виртуального АПР по заданной траектории (с использованием реальной динамики АПР) с одновременным выполнением стереосъемки «морского дна» виртуальной камерой (с частотой съемки, разрешением снимков и другими параметрами, характеризующими реальную стереокамеру) (рис. 8); в) полученная последовательность стереоизображений передается системе визуальной навигации в качестве входной информации; г) обеспечиваются необходимые для оценки эффективности метода данные, включая геометрию сцены и матрицы геометрических преобразований, связывающие локальные системы координат АПР с единой внешней системой координат.
Заключение
В работе представлена технология решения задач визуальной навигации и 3D-реконструкции применительно к АПР, основанная на применении методов/алгоритмов компьютерного зрения, обработки изображений, компьютерной графики и визуальной одометрии. Реализованные в МК возможности имитационного моделирования позволили провести исследования и тестирование ряда модификаций предлагаемых методов с целью повышения их эффективности и получения сравнительных оценок.
В последующих работах будут продолжены исследования по повышению эффективности предложенных методов с тестированием на реальных данных во взаимодействии с Институтом проблем морских технологий ДВО РАН.
ЛИТЕРАТУРА
1. Бобков В.А., Машенцев В.Ю. Визуальная навигация подводного аппарата для целей локального маневрирования // Подвод. исслед. и робототехника. 2013. № 2 (16). С. 33-37.
2. Бобков В.А., Морозов М.А., Багницкий А.В., Инзарцев А.В., Павин А.М., Щербатюк А.Ф. Имитационный моделирующий комплекс для обследовательского автономного подводного робота // Науч. визуализация: электрон. журн. 2013. Т. 5, № 4. С. 47-70. - http://sv-journal.org/
3. Бобков В.А., Машенцев В.Ю. Навигация подводного робота по стереоизображениям // Мехатроника, автоматизация, управление. 2016. Т. 17, № 2. С. 101-109.
4. Кудряшов А.П., Черкашин А.С. Построение единой триангуляционной поверхности по набору видов с зашивкой дыр // Информатика и системы упр. 2015. № 1. С. 36-40.
5. Мельман С.В., Бобков В.А., Инзарцев А.В., Павин А.М., Черкашин А.С. Программный моделирующий комплекс для автономных подводных аппаратов на базе многопроцессорной архитектуры // Подвод. исслед. и робототехника. 2015. № 1 (19). С. 23-32.
6. Bobkov V. A., Ron'shin Yu.I., Kudryashov A.P., Mashentsev V.Yu. 3D SLAM from stereoimages // Program. and Comp. Software. 2014. Vol. 40, N 4. P. 159-165.
7. Curless B., Levoy M. A volumetric method for building complex models from range images // ACM SIGGRAPH: Computer graphics and interactive techniques. N. Y.: ACM, 1996. P. 303-312.
8. Hogue A., German A., Jenkin M. Underwater environment reconstruction using stereo and inertial data // 2007 IEEE Intern. Conf. on Systems, Man and Cybernetics, 7-10 Oct. 2007. P. 2372-2377. - http://ieeexplore.ieee.org/xpl/ mostRecentIssue.jsp?reload=true&punumber=4413560 (дата обращения: 12.05.2016).
9. Johnson-Roberson M., Pizarro O., Williams S.B., Mahon I. Generation and visualization of large-scale three-dimensional reconstructions from underwater robotic surveys // J. Field Robotics. 2010. Vol. 27, iss. 1. P. 21-51. (Spec. iss.: Three-Dimensional Mapping. Pt 3).
10. Parodi O., Lapierre L., Jouvencel B. Hardware-in-the-loop simulators for multi-vehicles scenarios: survey on existing solutions and proposal of a new architecture // Proc. of the IEEE/RSJ Intern. Conf. on Intelligent Robots and Systems. IROS 2009. St Louis, USA, 2009. P. 34-52.
11. Salvi J., Petilot Y., Battle E. Visual SLAM for 3D Large-scale seabed acquisition employing underwater vehicles // IEEE Intern. Conf. on Intelligent Robots and Systems. IROS 2008, Sept., 22-26. Nice, France, 2008. P. 1011-1016.
