Навигация квадрокоптера с использованием монокулярного зрения
Сейчас для многих компьютерное зрение не является тайной за семью замками. Однако новые алгоритмы и подходы не перестают впечатлять. Одним из таких направлений является монокулярное зрение, в особенности SLAM. О том, как мы решали задачу навигации квадрокоптера, оснащенного единственной камерой, и пойдет речь в этой статье.
Задача
Задача заключается в движении по траектории, заданной последовательностью положений, в изначально неизвестном окружении с возможными препятствиями. Для её решения необходимо уметь:
- Строить карту препятствий
- Определять положение квадрокоптера относительно траектории и препятствий
- Корректировать траекторию с учетом облета препятствий
- Рассчитывать управляющие сигналы — реализовать контроллер
Технической базой является Parrot AR.Drone. AR.Drone снабжен следующими интересующими нас устройствами:
- Фронтальная камера: 640×360, 30 fps, диагональный угол обзора 92 градуса
- Нижняя камера: используется встроенным автопилотом для компенсации ветра и дрифта вообще
- Ультразвуковой датчик высоты: работает в пределах 0.25 — 3 м
- ИНС (акселерометр + гироскоп + магнетометр) + барометр: все датчики интегрированы в единую систему при помощи (видимо) sensor fusion
Кроме того, на основе показаний ИНС и нижней камеры формируется единая одометрия.
Итак, для построения карты окружения при помощи штатных средств AR.Drone мы можем использовать по большому счету только фронтальную камеру. Это непосредственно приводит нас к задаче монокулярного зрения, а именно к монокулярному SLAM.
Large Scale Direct SLAM
Можно смело сказать, что SLAM при помощи единственной камеры — писк современных технологий. Такие алгоритмы, появившиеся в последние несколько лет, можно пересчитать по пальцам руки неосторожного фрезеровщика — это ORB SLAM, LSD (Large Scale Direct) SLAM (и его предшественник SVO (Semi-direct Visual Odometry)), PTAM (Parallel Tracking And Mapping). Еще меньше алгоритмов, строящих более-менее плотные (semi-dense) карты окружения. Из наиболее продвинутых алгоритмов такие карты выдает лишь LSD SLAM:
В двух словах, LSD SLAM работает следующим образом. Параллельно работают три процедуры: трекинг, построение карты и оптимизация карты. Компонент трекинга оценивает положение каждого нового кадра относительно текущего ключевого кадра. Компонент построения карты обрабатывает кадры с известным положением, либо производя очистку карты кадра хитрым способом, либо создавая новый ключевой кадр. Компонент оптимизации карты занимается поиском циклов в графе ключевых кадров и устранением эффекта плавающего масштаба. Более подробно ознакомиться с алгоритмом можно в статье разработчиков.
Для стабильной и эффективной работы алгоритма (эти требования применимы к любому алгоритму монокулярного SLAM) необходимо следующее:
- Максимально более точная калибровка камеры и последующая ректификация изображения. Точность калибровки и ректификации, а также используемой модели искажений напрямую влияет на качество получаемых карт.
- Широкий угол обзора камеры. Для более-менее надежной работы нужны камеры с FOV более 80–90 градусов.
- Достаточное количество кадров секунду. При FOV в 90 градусов количество кадров в секунду не должно быть меньше 30 (лучше — больше).
- Движения камеры не должны содержать повороты без переноса. Такое движение ломает алгоритм.
Пункты 2 и 3 связаны друг с другом простым соображением: для расчета перемещения между двумя соседними кадрами изображения на этих кадрах должны перекрываться в достаточной степени. Соответственно, чем быстрее перемещается камера, тем больше должны быть угол обзора или частота кадров, чтобы связь между кадрами не потерялась.
При соблюдении этих требований можно получить карты весьма неплохого качества, в чем можно убедиться, посмотрев видео от создателей LSD SLAM:
Впечатляет, не правда ли? Однако даже если Вы достигли такого качества карт, Вас ждет еще одна неприятность: ни один алгоритм монокулярного SLAM принципиально не может оценить абсолютного масштаба полученных карт и, следовательно, локализации. Поэтому необходимо прибегнуть к некоторым хитростям и найти внешний источник данных либо помогающий определить размер объектов карты, либо оценивающий абсолютные значения перемещений камеры. Первый способ ограничен только Вашей фантазией: можно поместить объект известного размера в поле зрения камеры и затем сравнивать его с масштабами похожих частей карты, можно проводить инициализацию алгоритма в заранее известной обстановке, и так далее. Второй способ довольно легко применить, используя, например, данные альтиметра, что мы и проделали.
Для оценки масштаба мы использовали данные о перемещениях по вертикальной оси, полученных из двух источников: от алгоритма LSD SLAM и альтиметра AR.Drone. Отношение этих значений и есть масштаб карты и локализации монокулярной системы. Для устранения случайных возмущений полученное значение масштаба мы отфильтровали фильтром низких частот.
Обход препятствий и корректировка траектории
LSD SLAM хранит карту окружения в виде графа ключевых кадров с привязанными к ним частичными картами глубины. Объединяя все узлы графа, получаем карту известной части окружения в виде облака точек. Однако это еще не карта препятствий! Чтобы получить плотную (dense) карту препятствий, мы воспользовались библиотекой Octomap, строящей карту препятствий в виде октодерева на основе облака точек.
Для проверки столкновений и корректировки траектории мы использовали стек библиотек FCL (Flexible Collision Library) + OMPL (Open Motion Planning Library). После обновления карты запускается проверка столкновения траектории с препятствиями, в случае обнаружения столкновений сегмент траектории пересчитывается планировщиком (мы использовали BIT*, но здесь могут быть варианты).
Контроллер
Контроллер оказался в итоге довольно простым, на основе ПИД-регулятора. Этого оказалось достаточно для следования по траектории. Единственное, что пришлось добавить — ограничение скорости поворота камеры для сохранения стабильности SLAM.
Платформа и общая схема решения
В качестве платформы всего решения мы использовали ROS. Платформа предлагает всю необходимую инфраструктуру для быстрой разработки параллельно работающих компонент (узлов в терминологии ROS), коммуникаций между ними, мониторинга, динамической настройки, отличный симулятор Gazebo и многое другое, облегчающее разработку серьезных робототехнических решений. Хотя стабильность отдельных компонент системы все же оставляет желать лучшего, и использовать её в продакшне ответственного проекта пока не стоит.
Общая схема решения получилась примерно такой:
Выводы
Бочка меда:
- монокулярный SLAM вполне можно пытаться использовать;
- ROS — очень удобная платформа как минимум для разработки и тестирования, реализация крутых робототехнических проектов становится проще и проще.
Ложка дегтя:
- заставить работать монокулярный SLAM — дело весьма и весьма хитрое, особенно это касается задачи калибровки камеры.
Ссылки:
Страница LSD SLAM на сайте разработчиков: vision.in.tum.de/research/vslam/lsdslam
Open Motion Planning Library: ompl.kavrakilab.org
Flexible Collision Library: github.com/flexible-collision-library/fcl
Octomap: octomap.github.io
