Разработка оптимальных алгоритмов одновременной локализации и построения карты при помощи телекамеры (V-slam) с учетом данных инерциальной навигации
Получены алгоритмы и программное обеспечение на языке VisualC++, обеспечивающие в реальном времени проведение одновременной локализации и построения трехмерной карты с использованием одной телевизионной камеры. В основе разработки лежит расширенный фильтр Калмана, что обеспечивает возможность интеграции различной навигационной информации для получения оптимальной оценки координат. Одновременно модуль инерциальной навигации, разработанный ранее для скоростей более 1 м/с, адаптирован к использованию на мобильных роботах (при скоростях движения порядка 0,1-1 м/с) и разработан принцип совместного использования информации от телевизионной и инерциальной навигационных систем.
На основе выполненного обзора существующих алгоритмов было принято решение взять за основу разрабатываемого алгоритма прототип, представленный в [3].
Разработанный алгоритм предоставляет возможность осуществлять последовательную навигацию робота с одновременным построением карты местности и определением положения робота. Последовательность определяется тем, что алгоритм должен в реальном времени обрабатывать новые кадры, поступающие с телевизионной камеры – тем самым исключается применение алгоритмов «структура из движения», существенным отличием которых является применение пакетной, одновременной обработки большого количества кадров с целью глобальной оптимизации.
Для работы в реальном времени необходимо, чтобы время обработки одного кадра было ограничено некоторой константой, в противном случае порог «реального времени» рано или поздно будет преодолен. По этой причине предлагаемый алгоритм не хранит историю всех обнаружений, а представляет систему в виде текущего состояния, обновляемого со временем.
Другой принципиальной особенностью алгоритма является то, что в проводимых им измерениях отсутствует уход координат, что гарантирует ограниченную погрешность измерения координат со временем. Для достижения этого в качестве опорных точек используются стабильные признаки, для выделения которых использован алгоритм Shi,Tomasi, а хранение обеспечивается с учетом изменения вероятностных характеристик их местоположения. В макетной реализации необходимое количество одновременно сопровождаемых признаков сохраняется не ниже заданного порога за счет автоматического добавления новых признаков, когда это необходимо.
Реализованный алгоритм калибровки камеры обеспечивает определение всех параметров внешней и внутренней калибровки в полуавтоматическом режиме с использованием калибровочного изображения. Калибровка позволяет использовать априорную информацию о среде и движении робота. Принципиально, может быть предложена модификация алгоритма с самокалибровкой. При этом калибровочные параметры должны быть учтены в вероятностной модели.
Состояние системы описывается вектором состояния, состоящим из вектора состояния робота и координат окружающих объектов, дополняемым ковариационной матрицей:
Задачей ковариационной матрицы является представление неточностей измерения всех элементов вектора состояния с точностью до первого порядка. Вектор состояния и ковариационная матрица в процессе работы могут увеличиваться и уменьшаться в соответствии с количеством отслеживаемых признаков.
При движении шаг предсказания использует модель движения для определения движения робота и увеличения погрешности его положения. При получении новых координат признаков модель измерений используется для определения того, как уменьшается неточность измерения координат робота и признаков.
Определим системы координат W– глобальную, связанную с землей, иR– связанную с камерой.
Рисунок 5 Системы координат
С использованием кватернионов для описания трехмерной ориентации вектор положения может быть определен в виде:
Для моделирования движения робота используется модель постоянной скорости и постоянной угловой скорости. С учетом статистического моделирования процесса движения, это подразумевает, что неопределенные ускорения будут соответствовать модели гауссова процесса. Такая модель, предложенная Дэвисоном [3], накладывает некоторые ограничения, фактически заключающиеся в гладкости траектории и отсутствии очень больших ускорений, нетипичных для гусеничных роботов. С целью использования такой модели движения необходимо дополнить вектор состояния робота скоростями и угловыми скоростями:
Таким образом, получается вектор с 13 элементами. При этом необходимо учитывать, что для кватернионов выполняется условие нормирования:
Предположим, что на каждом временном
шаге неизвестные ускорение
и угловое ускорение
с нормальным распределением и нулевым
математическим ожиданием вызывают
скачок скорости и угловой скорости:
Предполагая, что элементы вектора nявляются независимыми, получаем обновление состояния в виде:
В фильтре Калмана новая оценка состояния должна быть дополнена увеличением неопределенности состояния системы:
Pn– матрица ковариации шума определяет, насколько гладкое движение заложено в модель движения. При малых значениях предполагается модель плавного движения с малыми ускорениями и угловыми ускорениями. При этом легко подавляется неточность в измерении координат признаков. При больших значениях вPnмодель допускает большие ускорения, однако для успешной работы алгоритма необходимо качественное сопровождение большого числа признаков на каждом шаге.
В качестве устойчивых признаков для обеспечения трассировки выбраны «патчи» – элементы изображения размером 9х9 или 15х15 пикселей. Для таких элементов показано, что они исключительно устойчивы к изменению ориентации. Каждый элемент обнаруживается однажды детектором Shi,Tomasiи сопровождается из кадра в кадр при помощи нормализованной корреляции на основе суммы квадратов разностей.
Положение точечного признака относительно камеры может быть предсказано в виде:
Положение (u, v)того же признака в изображении может быть получено с использованием модели камеры-обскуры в виде:
Далее, может быть также получена оценка достоверности данного предсказания:
Эта матрица ковариации описывает форму двумерного гауссовского распределения в координатах изображения. Используя правило «Nσ» (обычно 3σ), можно определить эллиптический регион, в котором с высокой долей вероятности находится признак в следующем кадре.
Кроме того, данная матрица является мерой информации, получаемой от измерений. Для получения максимальной пользы от следующего измерения камера может быть специально направлена на тот регион, от которого ожидается наибольший вклад. В системе с неподвижной (пассивной) камерой элементы матрицы могут использоваться для выбора наиболее информативных признаков, если их количество слишком велико. Это позволяет избежать неограниченного возрастания неопределенности в каком-либо одном направлении.
Для совместного применения инерциальной и визуальной навигации разработана схема, представленная на рисунке Рисунок 6. Такая схема обеспечивает коррекцию опорной траектории, предоставляемой БИНС, при помощи визуальной навигации.
Рисунок 6 Схема комплексирования