Задача
Задача заключается в движении по траектории, заданной последовательностью положений, в изначально неизвестном окружении с возможными препятствиями. Для её решения необходимо уметь:
- Строить карту препятствий
- Определять положение квадрокоптера относительно траектории и препятствий
- Корректировать траекторию с учетом облета препятствий
- Рассчитывать управляющие сигналы – реализовать контроллер
Технической базой является 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
Комментарии (29)
svyatoslavalekseev
05.02.2016 13:24-14Очень опасная технология, такой дрон вполне может ориентироваться на местности по обычным спутниковым картам и летать автономно, т.е. это идеальный боевой дрон-камикадзе. Очень перспективная разработка для спецслужб, но не как не для гражданки, широкому кругу лиц нельзя иметь доступ к подобным технологиям. Уже сейчас в него можно загрузить алгоритм распознавания автомобильных номеров, привязать взрыв пакет и отправить на охоту. На втором этапе, когда подтянутся вычислительные мощности и разрешающая способность камер — загрузить алгоритм распознавания лиц. Вспомните теракты в Париже — президент Франции сидел на стадионе, когда раздались взрывы, один подобный дрон и для него все могло быть кончено. Все политики будет обречены ходить под зонтиком всю жизнь, как Майкл Джексон. Хотелось бы подчеркнуть, что когда мы говорим об обычных радиоуправляемых дронах, то их легко отключить просто включив радиоглушилку. Такой автономный дрон сможет вывести из строя только ЭМИ (если микрухи дешевые — сильный радар) или боевое оружие. В первом случае могут погибнуть люди с кардиостимуляторами и выведутся из строя все электронные устройства. И еще раз, отдельно, стоит помянуть ИГИЛ, у этих ребят работает множество инженеров и это всего-лишь вопрос времени — сделать косметический ремонт подобному программному обеспечению. Сам уже давно занимаюсь многороторными ЛА и понимаю что тут до трагедий один шаг, надеюсь законодатели поспеют.
BelBES
05.02.2016 14:23+7Тот редкий случай, когда я не поленился зайти в профиль и поставить минус там…
1) Если технологию можно использовать не только в гражданских целях — это еще не значит, что её нужно запрещать. Один нелепый закон может на долго затормозить прогресс, т.к. исследователи не смогут у себя в гараже собирать автономных дронов и делать их совершенней.
2) Никакой закон не запретит мне разрабатывать алгоритмы для дронов, используя виртуальные симуляции. А перенести результаты на реального дрона — дело техники.
3) Судя по попадающим в интернет новостям, несмотря на все законодательные запреты, вполне себе существуют подпольные оружейные мастерские. Наладить подпольное производство беспилотников тоже не сложно. Т.ч. законы как всегда ударят по гражданским пользователям, а преступникам, идущим на более тяжелое преступление, запрет на сборку квадрокоптера явно не помешает осуществить задуманое)
4) На данный момент и в некоторой недалекой перспективе дроны с временем автономного полета дай бог 10 минут и хилым процессором на борту не смогут быть полноценными беспилотниками.svyatoslavalekseev
05.02.2016 14:59-3Да я же просто не хотел чтобы людей убивали. Импульсивно, конечно, было предлагать кому-то ограничивать доступ к подобной и любой другой информации, не учтя, какие последствия это вызовет для моего аккаунта. ) А так, конечно надо ограничивать, как бы вам не нравилось. Но аккаунт жалко. Другое дело, что такое ограничение не возможно, и предлагать это было моей второй ошибкой. ) Что же, судьба видимо такая у многих ученых — сделать мир опаснее. Прогресс двигается вперед и его не остановить, технологии становятся доступнее и где-то на пол пути к сингулярности мы придем к ситуации, когда отдельный индивид ум сможет создать устройство апокалипсиса из средств создания, которые ему доступны дома. Правительство не сможет контролировать развитие технологий, значит, судьба у нас такая — существовать в условиях абсолютного, исчерпывающего контроля над человеком со стороны государства, наших мыслей, намерений. Просто судьба. Тотальный контроль. Хотелось спокойствия в мире, вот и написал. Будем жить в этом, удачи будущим поколениям! )
n1tra
05.02.2016 20:49-1Весна вроде еще не наступила, хотя хрен ее знает с этим климатом, чертов технический прогресс!
impetus
06.02.2016 02:50+2«Президент Франции» — это всего лишь должность. Людей на ней Франция меняет как перчатки уже пару столетий.
BelBES
05.02.2016 14:27+2Практика показывает, что монокулярные SLAM алгоритмы работают мягко говоря не очень, особенно на outdoor сценах, когда и освещение сложное и пространства большие. Навигация по одной камере работает адекватно разве что тогда, когда коптер летит высоко в небе и вся сцена является существенно планарной, а вот при полетах низко над землей возникают пробелмы с качеством построения 3D модели сцены… и это не говоря о том, что большое число подвижных объектов в сцене — это смерть для монокулярных методов.
При этом написать работающую одометрию на основе стереокамеры в разы проще.niklyaev
05.02.2016 15:10+1Вы правы, в разы — это мягко говоря.
Стереопара на основе двух китайских поделок выдает после получасовой отстройки приемлемую карту глубины. А вот для монокулярного мэппинга камера нужна много лучше (дороже), да и результат будет скорее всего хуже (особенно потому, что не даст определить масштаб сцены). Думаю, монокулярную картографию надо применять только в случаях жестких технических ограничений на использование одной камеры (наш случай), или небольших требований к качеству получаемых карт, или в помещениях.BelBES
05.02.2016 15:22+1У стерекамер есть еще такое свойство, что от вибраций они раскалибровываются быстро :-) Т.ч. пара вебок на синей изоленте — это плохой выбор, в конечном счете все равно приходится брать пару хороших индустриальных камер и жестко их монтировать на какой-то крепкой платформе.
Ну и разница между работающим алгоритмом и продуктовым решением велика… там и кроме проблем раскалибровки есть масса подводных каменй.
Кстати, ранние марсоходы использовали монокулярный слам, но там сцены как раз планарные и без двигающихся объектов, и движения марсохода достаточно медленные… пожалуй это сейчас единственный кейс, где такие методы реально могут быть применимы)
svyatoslavalekseev
05.02.2016 15:24+3Дополнительное физическое устройство нецелесообразно практически всегда, особенно в дронах. Математика гораздо дешевле, особенно на стадии серийного производства (при наличии на рынке относительно недорогих процессоров, которые могут с этим справиться, такие уже и сейчас есть).
ToSHiC
06.02.2016 01:05+1А если кормить монокулярный SLAM данными с ИНС, где будут уже абсолютные цифры перемещения, ему сильно лучше станет? Типа как у человека вестибулярный аппарат и зрение работают в паре при ходьбе.
niklyaev
06.02.2016 11:29Очень неплохая идея, мы к ней пришли не сразу :) Но тут есть проблема с двойным интегрированием: не получится рассчитывать перемещение за короткий период только по ИНС, а на длинном периоде накапливается большая ошибка интегрирования. При ходьбе есть моменты, когда скорость ноги гарантированно равна нулю, и алгоритмы этим пользуются, чтобы скомпенсировать ошибку интегрирования ускорения.
ToSHiC
06.02.2016 12:49Но по-идее можно использовать первый интеграл (скорость) для восстановления масштаба из картинки. Ускорение интергировать, положение ключевых точек (это я наверняка неправильно назвал, но суть, надеюсь, понятна) из SLAM дифференцировать, результаты приравнять. Тогда по исходной SLAM можно и положение в пространстве находить для коррекции положения в ИНС.
niklyaev
06.02.2016 15:09Проблема в том, что будет накапливаться ошибка интегрирования — значение скорости будет «уплывать» на достаточно длительном отрезке времени. Теоретически можно попробовать считать масштаб из отношения ускорений, но у меня получилось очень большое расхождение даже с использованием сглаживания и идентификации кривых на большом временном окне.
Parilo
06.02.2016 01:32+1У вас в итоге получилось завести монокулярный slam? Можете для примера выложить картинки ваших результатов? 3д модель местности, траекторию?
niklyaev
06.02.2016 11:24+1Получилось более-менее даже облетать препятствия. К сожалению, сейчас нахожусь далеко от тех данных, как доберусь — выложу картинки.
Parilo
06.02.2016 14:08вы использовали код из их гитхаба, я про lsd-slam? Пришлось ли что-то допиливать в нем?
niklyaev
06.02.2016 15:14+1Мы немного изменяли код, чтобы извлечь ковариацию позиционирования и подставлять внешнее значение позы при потере трекинга, но в целом алгоритм не трогали, он работает из коробки как надо. Возможно, Вам придётся что-то изменять, если Вы будете использовать разрешение, сильно отличное от 640х480 — некоторые параметры захардкожены.
niklyaev
08.02.2016 10:55+2Пришлось сделать новую запись :)
Гифки с изображениями нашего офиса: в виде облака точек (с путем следования камеры) и кусочек видео с камеры (естественно, качество не соответствует настоящему видеопотоку, только для представления о форме комнаты)
ToSHiC
08.02.2016 12:13Зелёные линии — это траектории камеры, рассчитанные алгоритмом?
niklyaev
08.02.2016 12:22Не совсем: алгоритм не выдает путь движения камеры как таковой, только граф ключевых кадров. В узлах графа находятся собственно ключевые кадры, а ребрами соединены те кадры, которые трекер смог связать друг с другом при помощи механизма замыкания циклов (loop closure). Цвет линии показывает, насколько велика вероятность такой связи (зеленые — большая вероятность, красные — низкая).
ToSHiC
08.02.2016 12:28Т.е., с некоторыми огрехами, ещё и бесплатно местоположение в пространстве получаем, если граф непрерывный. Например, чтобы в конкурсе от Крока вернуться через дверь :) Очень круто!
niklyaev
08.02.2016 12:46Собственно, SLAM и есть Simultaneous Localization And Mapping — локализация и картография одновременно. То есть, такие алгоритмы всегда выдают и карту (в каком-то виде), и местоположение. Текущее положение камеры на первой гифке отмечено красной пирамидкой.
varagian
Как-то хочется больше технических деталей.
Одно время игрался с Parrot (правда их мини-дронами) и видел, что у них есть собственное SDK, нельзя ли его использовать, чтобы упростить разработку? Может быть там уже есть какие-то встроенные вариации SLAM? Желание самому за это сесть есть, но немного смущает порог вхождения.
niklyaev
Насколько я знаю, SDK как для AR.Drone, так и для Bebop (весьма интересный относительно новый дрон от Parrot) предоставляют только доступ к контроллеру, навигационным данным с ИНС, видеопотокам и конфигурации дрона. Самое продвинутое, на что способен SDK — распознавать маркеры определенной формы и цвета.
Какие технические детали Вас могли бы заинтересовать? Работа именно SLAM с одной камерой, настройка всех компонент системы, платформа?
varagian
Было бы интересно увидеть подробный tutorial, как все настроить и начать работать со SLAM (такой ~«how to get started»). Общая идея понятна и она клевая, но боюсь, чтобы сделать самому придется перелопатить некислое количество материала, а если всё собрано в одном месте, то, мне кажется, будет отлично.
CapitanBlood
в целом неплохая идея, но это конечно в рамки одной статьи не влезет, но, если это будет интересно можно подумать и над таким материалом.