Сделал новую версию автопилота для квадроцикла:

VNC поглючивает на ноуте но вроде-бы сейчас лучше не так артефачит

Основные фишки автопилота:

1) Визуально инерциальная навигационная система OpenVins





2) Построение воксельной карты



3) Планировщик траектории безопасной от препяствий



4) Режим следуй за мной по GPS (в разработке)

5) Есть ручное управление с клавиатуры на ноуте

Архитектура програмных модулей:



Аппартаная архитектура автопилота:



Идею украл у SMPRobotics Зеленоградская компания [smprobotics.ru]:







С такой погодой не возможно испытывать ничего, видел в Грузии проводят дрон рейсинг в заброшеных предприятиях где хотя-бы от ветра и снега есть защита а что можно в Ставрополе придумать или всю зиму ждать погоду? Было пару деньков в феврале +13 но этого мало((

sergo
Кончились автомоделисты в Ставрополе

Теперь когда летать запретили это наверно едиственный выход.

Как можно заметить с момента создания самолёта технологический прогресс автопилота продвинулся вперёд. Если-бы не война он мог быть ещё лучше чую я, что после выборов в этом году вообще всё загнётся.



Такое вот технологическое творчество.

Проблемы работы различных фреимворков для навигации (визуальной навигации):

Все тесты проводились на автомобиле (седан) в реальном времени на мини ПК
CPU: mobile Intel i7 12th
RAM: 16Gb DDR4 Dual-Chanel 3200MHz
GPU: mobile 3050Ti 4Gb GDDR616Gb/s PCI4 x8 (with CUDA 10.1)

ORB-SLAM3:





Визуальный SLAM, показал себя относительно не плохим фремворком с малым расходом производительности процессора, разряжённой картой ключевых точек (точек от детектора), что сокращает нагрузку и обьём памяти ОЗУ, компактный размер дистрибутива, работает без ROS, использует минимум 250 точек для расчёта и выбирает лучшие и 1500, но..

Плохая работа детектора, не устойчив к размытию изображения от движения ORB [youtube.com] (для что приводит к сбоям сопастовления [youtube.com] изображений и не устойчивости к резким перемещениям камеры) пример хорошей усточивости точек новой версии ORB OpenCV 4 [youtube.com] пример [youtube.com] ещё лучшей устойчивости детектор контуров, однако это уже не точки а 2х мерные дискрипторы, линии дают значительно больше данных
Нужна ручная инициализация IMU с перемещением камеры по определённой траектории вокруг условного центра, для калибровки. Что на квадроцикле не возможно, поднять его и поперемещать так.
Не устойчивая работа без IMU по чисто визуальному отслеживанию RGB_D, с использованием IMU режим называется RGB_D inertial.
Не работает в планарных (плоских) сценах например в поле где нет 3х мерных ориентиров или дроги где вокруг пусто (вокруг это 9м поле зрения камеры)
типо домов кустов деревьев и т.д.

VINS RGB-D:



Визуально-инерциальная навигация, не устойчиво работает на улице в принципе, возможно из-за детектора какой тут я не знаю((.
Основная задумка VINS RGB-D обьеденить режим MONO способный треангулировать дальние точки детектора за счёт использавания кадров снятых на камеру ранее (по мере движения) и подбора стереопары среди них. Таким образом стереобаза может-быть не ограничена, что в теории должно дать ему возможность по дальности сравнятся с твердотельными направленными (90 градусов угол обзора) лидарами т.е. в место стандартных 9м у RGB_D достигать фантастических 350м, если всё хорошо отслеживается и камера накалибрована. Для ближних точек в расчёте используется RGB-D (стерео камера) с фиксированной базой, что должно дать устойчивость к резким перемещениям как и в режиме RGB_D inertial. Использует много точек.
На практике устойчивость оказалась хуже чем в режиме RGB_D inertial ORB-SLAM3 и также требуется инициализация (перемещения камеры в пространстве) в ручную. Однако в помещении работает не плохо.
Не работает в планарных (плоских) сценах например в поле где нет 3х мерных ориентиров
типо домов кустов деревьев и т.д.

RTAB-MAP







Визуальный SLAM, работает лучше всех перечисленных в этом списке систем визуальной навигации.

В отличии от всех остальных систем RTAB-MAP слабосвязанная система где IMU используется лишь для нахождения вращения камеры а акселерометр только для нахождения вертикали и не используется для интеригрирования в него скоростей визуального оценщика пологаясь исключительно на визуальный отценщик, что позволяет ему не боятся сильных вибраций. Есть очень много настроек. Есть RGB_D режим который работает. Не нужна начальна иницализация камеры в ручную. Использует минимум 500 точек для расчёта движения из 1500 - 6000 кандидатов на изображении.

Не работает в планарных (плоских) сценах например в поле где нет 3х мерных ориентиров
типо домов кустов деревьев и т.д.
Использует не разряженное как в предыдущих фреимворках а плотное (ICP) облако точек на карте, что излишне нагружает видеокарту и процеесор.
Использует ресурсы камеры (карту глубины и показания IMU) внутри себя а не запрашивает их у используемого им приложения как все остальные и не делится ими (только готовыми облоками точек и готовым кватернионом абсолютной ориентации, которые медленно обновляются). Т.е. другие приложения в этот момент камеру использовать не могут а только получать то что отдаёт RTAB-MAP.
Сохраняет плотную карту на SSD что требует много памяти (эти и минус и плюс одноврменно но навигации это не помогает, точно).
Если размер всех остальных фреимворков навигации не превышает 300Мб и они стостоят из 2-3 библиотек, здесь их входит в состав больше 20 на общий обьём при сборке из исходников 40Гб !
Из-за большого размера и сложности (этот фрейиворк включает всебя и почти все пресуствующе тут фреимворки навигации для сравнения и тестов, для этого и создан RTAB-MAP как автономное приложение) его сложно интегрировать с любым приложением и извлечь данные особенно без ROS. Проще ну относительно, интегрировать своё в этот комбаин.

REBiVO



Визуально-инерциальная навигация основанная на датчике контуров а не ключевых точек, усточива к резким вращения и перемещениям камеры, углу обзора и смене освещения от солнца, но не много хуже к масштабу изображения.

На работу в планарных сценах не тестировал но 90% что не работает. Нужна ручная инициализация.
Имеет крайне малую производительность в режиме моно, 2 кадра в секунду, соотвественно в режиме стерео где масштаб сцены будет определятся “одним кадром, точнее парой” по 2м снимкам будет максимум 1 кадр в секунду, что не позволяет её использовать в реальном времени, железо пока недотягивает.


OpenVins





Визуально-инерциальная навигация основана на фильтре Калмона с множеством ограниченных состояний MSCKF используящая детектор Kanade–Lucas–Tomasi (KLT) или ORB. Это доволно новый и прогрессивный подход. Имеет инициализацию за счёт сбрасывания ошибки IMU в момент остановки дрона ZVUPT, а также есть пример динамической инициализации во время движения! Что очень удобно если произошёл сбой перезагрузить визуально-инерциальную навигацию без остановки квадроцикла. Коротковременно устойчив к резким перемещениям и вращениям, работает с большой скоростью 200Гц за счёт IMU при относительно не большом расходе вычислительных ресурсов строит разряжённую карту в ОЗУ для навигации, довольно компатный фреимвокр 150Мб +\-, работающий без ROS. Использует всего 50 точек в вычислениях для нахождения перемещения и 200 точек находит на изображении с камеры, избыток в 4 раза для отбора лучших 50 шт. кондидатов точек. Что в общем-то и хорошо и плохо хорошо что всего 50 ш.т. достаточно. Это экономит ресурсы процессора что хорошо и плохо т.к. чем их меньше тем сильнее влияют ошибки распределения точек на картинке с камеры в идеале все точки должны быть на 3х мерных обьектах равномерно по всему изображению не задевая землю или задевая её минимально и это речь именно про эти 50 точек а не оставшихся 150 отсеяных. Но как можно заметить это работает на машине и распеределение тут никак на качество не влияет.
По сути в этих тестах его основной недостаток это то что он не работает в планарных (плоских) сценах например в поле где нет 3х мерных ориентиров типо домов кустов деревьев и т.д. и в сборке без ROS нет своего визуализатора как Pangoline у ORB-SLAM3 Но т.к. у всех кандидатов визуальная навигация в планарных сценах тоже не работает а она необходима на это придётся закрыть глаза, визуализатор-же отображает лишь эти красные точки они не так и важны по сути тем более картинки с камеры с наложенными точками он отправляет этого достаточно.

Как итог OpenVins был выбран мной как что-то среднее из всех по характерстикам не сильно расходующий ресурсы мини ПК одноврменно довольно устойчивый в руках и на машине. Но несмотря на тщательную подготовку по выбору кандидата интеграция завершилась провалом! Фреимворк отказался работать на квадроцикле:



Это было неожиданно, OpenVins был интегрирован в автономное приложение QT OpenGL проведена не малая работа. Я провёл ряд дополнительных тестов в саду где есть много ориентиров типо кустов деревьев и по мягкой земле нет тряски (на форуме по ROS проф. роботехники Романов заявлял, что вибрации на колёсах у квадроцикла сильнее чем у коптера с 4 ремя винтами и больше влияют на ошибку но это не сыграло большой роли для меня) Судя по всему причиной стало распределение фитч на изображении т.к. большенство фреимворков не работают в планарных сценах из-за малого количества точек участвующего в расчёте перемещения камеры (из обнаруженных 200 на изображении по каким-то не понятным параметрам выбиралось 50 лучших для вектора состояния фильтра калмона где их всего 50 ш.т.) если больше половины из них оказывались на той части изображения что находится у земли (асфальта который занимает пол экрана при такой установке камеры, ниже чем на машине под лобовым стеклом) это приводило к эффекту планарной сцены, учитывая что 50 ш.т. на изображении из 200 точек помежет поместится аж в 4х частях изображения полностью. Я также пробовал жертвуя быстродейсвием увеличить вектор состояния до 100 ш.т. но это не решило вопрос и не дало какого-то заметного улучшения в прочем с ним я сделал всего 2 теста. Тогда я решил просто снять камеру и поднять её на высоту 1,5м как это было под лобовым стеклом на машине и пройтись тут ещё и всё заработало, но легче от этого конечно не стало. Таким образом едиственным работающим фреимвороком на квадроцикле остался RTAB-MAP его тест на тележке с проводом максимально приближен к работе на квадроцикле где камера установлена низко, однако это не отменяет технических причин с сложной интеграцией.

Без фреимворка визуальной навигации пропадает не только источник местоположения, что можно получить от колёс или оптического потока а такая важная вещь как визуальный курс, курс не подверженный магнитным помехам, дрейфа от интегирования данных, если не двигатся визуальный курс не расходится а в движении расхождение происходит значительно медлнее дешёвых IMU либо совсем отсуствует. Ну и в целом это важный источник данных о скорости и положении дрона не зависящий от энкодеров колёс или текстуры подстилающей поверхности земли как датчики оптического потока например [youtube.com] (т.к. использует в расчёте только текстуры 3х мерных ориентиров, домов кустов, столбов и т.д.)

Какое-то время я думал забросить это дело т.к. всё стало слишком сложно решить без гиганских трудозатрат или вложений под половину мульта на лидар. Но как оказалось сегодня есть 3д лидар за сумму 75тыс. рублей и это очень много но это едиственный шанс. Я решил деньги накопленные на апгрейд компа 10 летней давности (едиственного норм компа с видеокартой) вложить в этот лидар Livox MID 360 что-бы не выкинуть квадроцикл, этакое повышение ставок ведь нет горантии что всё заработает((. Лидар пришёл я проверил его работоспособность:



Решил тестировать его с довольно популярным фремимврком FAST-LIO [github.com], мне удалось запустить его у себя на Ubuntu 20.04 с ROS Noetic переделав под драйвер моего лидара livox_ros_driver2 благо они почти идентичны.





Теперь нужно сделать крепление лидара на квадроцикл, желательно что-бы оно могло фильтровать вибрации т.е. было на подобие этого:



Иначе есть риск повреждения прибора от сильной тряски если квадроцикл поедет по дорожным ямам или просто столкнётся с чем-то. А если перевернётся ему вообще будет кают т.к. он должен быть расположен выше всего и что-бы ничего не прекрывало его обзор на 360 градусов. И при этом к нему должна обязательно крепится камера что-бы движения imu от камеры было механически согласованно с imu от лидара т.е. крепление сложное. Лидарная навигация должна дать высокую точность курса и перемещения квадроцикла в прострастве а также работать ночью, солнце тоже влияет на лидары меньше чем на визуальную навигацию.

Квадроциклы smp robotics кстати не используют лидары для навигации как у вездеходов Яндекса она у них работает по камерам, визуальная, в прочем и такие фреимворки которые я тестировал или подобные они тоже не используют. На ихних роботах навигация планарная, что можно понять прочитав их страницу технологии [smprobotics.com] когда камера обращена на дорогу перед роботом [smprobotics.com], не смотрит далеко вперёд и считывает направление движения текстур дороги что-то на подобие оптического потока что я упоминал, у этого есть свои как плюсы так и минусы…



?si=fTXS2hnGH1btC6eu

Пример работы на столе планарной навигационной системы на базе OpenVINS модуль ov_plane:

?si=CHV6_lBVjjfUCO-G

Краткий принцип работы планарной навигации:

?si=TSSlTcD1cFHTdK1x

Главный плюс этой системы работа на плоскости без каких-либо 3д ориентиров (только по рисункам, текстуре на столе) что является несовненно большим плюсом т.к. плоскостей больше чем 3д ориентиров это дороги, газоны, пол в помещении. Для БПЛА летающей на большой высоте где всё кажется планарным это едиственный способ автономной навигации. Однако в 3х мерной среде где плоскостей нет например полёт в кронах деревьев она не работает:

?si=IhSI2zbzy5uYeP-l

?si=4qYCQidtFnVbzNWf

Аналогично если поверхность не слишком плоская например поверхность океана при волнениях такая навигация будет работать плохо и только комплексирование нескольких методов можно добится работы в различных средах на улице.

И ещё как можно заметить во всём мире разрабатывают системы визуальной навигации кроме этой страны! Проводятся международные челенджи и форумы но это то, что по телевизору не покажут то где Российских участников нет!

?si=6sHvPn434jP0OuJM

Первый тест FAST-LIO:
Однометрия достаточно надёжна и работает на квадроцикле двигающимся своим ходом.

Тест Fast-lio на улице. Однометрия хорошо работает на квадроцикле двигающемся своим ходом. Управлять с компьютера и двигать карту одновременно не удобно приходится делать остановки. Также дохлые свинцово-кислотные аккумуляторы не позволяют ехать быстро и иногда квадрику требуется помощь пинком. Цветовая кодировка карты это интенсивность отраженного от поверхности излучения тёмные поверхности изображены тёмными тонами а светлые светлыми т.е. по факту это панарамное Ч/Б изображение + форма.

?si=EVnARx33da_lmNww

Тест лидарно-инерциальной навигационной системы в поле, как и следовало ожидать в плоской геометрически вырожденной сцене лидарная навигация тоже начала дрейфовать:

?si=6_UkI-gsq1kCAJJE

Пример работы планарной навигационной системы на самоуправлямой модели авто с камерой ADNS 3080 тест замыкания петли:

?si=jCJZ7CoPe1CsSmYk

Благодоря высокой частоте кадров 6400 кадров/с датчика от компьютерной мыши он способен успешно отслеживать движение по асфальтному покрытию без потери отслеживания на больших скоростях.