Определение положения дрона в пространстве с помощью Web-камеры

Python библиотека ZuzaNanoMocap

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

1. Обнаружение контуров дрона "Жужа Nano" на изображении

2. Расчёт 3D-координат дрона "Жужа Nano" относительно оптического центра камеры.

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

Обнаружение контуров дрона на изображении

Для фильтрации изображения кадров Web-камеры используется библиотека OpenCV.

Фильтрация изображения реализована в тестовой программе DroneDetectorLAB.py. Скачать её можно по ссылке на Яндекс Диск и запустить с помощью программы Visual Studio Code на вашем ПК.

Цветное изображение с Web камеры разбивается на 3 канала в соответствии с цветовой моделью LAB (опционально, например, для других цветов, можно использовать другие цветовые пространства: HSV и прочие).

Цветовое пространство кадра преобразуется с помощьюcv2.cvtColor(img, cv2.COLOR_BGR2LAB), затем разбивается на 3 канала с помощью cv2.split(hsv). Каналы фильтруются с помощью параметров, задаваемых ползунками программы, которые можно интерактивно менять:

Канал L

Канал A

Канал B

Настройки фильтрации

Результат фильтрации

Контуры дрона на исходном кадре

На отфильтрованном изображении выбираются контуры (с помощью cv2.findContours( thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)), среди которых выбирается контур с максимальной площадью - это и есть контур нашего дрона. После нахождения контура дрона переходим к расчёту пространственных координат.

Расчёт 3D-координат дрона относительно центра камеры

При расположении камеры на потолке, она смотрит на дрон сверху, и оси координат направлены следующим образом:

Оси координат камеры на потолке

Ось X смотрит на изображении вправо, ось Y - вниз, ось Z - в плоскость изображения.

Самый простой способ рассчитать расстояние от камеры до объекта на основании координат изображения объекта - принцип подобия треугольников.

Например, мы берём объект известной ширины W и помещаем его перед камерой на расстояние D. На картинке с камеры мы измеряем ширину изображения объекта в точках P. Это позволяет нам измерить длину фокуса нашей камеры F:

F = (P * D)/W

Например, диагональ рамы Жужи Nano W = 14,14 см (0.1414 м), мы помещаем раму перед камерой на расстояние D=1 метр. Диагональ квадрата на изображении, при этом, P=147 точек.

Таким образом, длина фокуса камеры F = 147точек * 1.0м / 0.1414м = 1039.603960

Зная длину фокуса камеры, мы можем теперь рассчитать расстояние до дрона, на основании его размеров на изображении:

D' = (W * F)/P

Кроме того, методом пропорции мы можем рассчитать координаты X,Y центра дрона относительно центра камеры:

X = (point_x-FRAME_WIDTH/2)/PixelSize*DroneFrameSize

Y = (point_y-FRAME_HEIGHT/2)/PixelSize*DroneFrameSize, где

point_x, point_y - координаты точки X, Y на картинке,

FRAME_WIDTH, FRAME_HEIGHT - ширина и высота кадра в пикселях,

PixelSize - размер рамы дрона в пикселях,

DroneFrameSize - размер рамы дрона в метрах.

Программа DroneDetectorLAB рассчитывает фокусное расстояние автоматически, исходя из предположения что диагональ объекта = 0.1414 метра и объект помещён на расстояние 1 метр от камеры. Для других объектов возможно исправить указанные параметры в тексте программы самостоятельно.

Автоматический расчёт фокусного расстояния

Эти принципы расчёта использованы в классе ZuzaNanoMocap, который предназначен для обеспечения автономного управления дроном на основании видео-потока с камеры.

Класс для автономного управления дроном ZuzaNanoMocap

Класс Python ZuzaNanoMocap запускается в отдельном потоке и выполняет следующие функции:

1. Получение и фильтрация видеопотока с камеры, выделение контуров дрона

2. Расчёт координат дрона в пространстве, а также скорости и ускорения, фильтрация погрешностей с помощью фильтра скользящего среднего

3. Расчёт отклонения стиков дрона для удержания дрона в заданной точке, с помощью алгоритма PID регулирования
4. Логирование основных показателей системы управления дроном в формате csv

Параметры управления задаются при инициализации класса:

Параметр	Описание
Device_ID	ID устройства web камеры
FRAME_WIDTH	Ширина кадра (в точках)
FRAME_HEIGHT	Высота кадра (в точках)
FPS	Скорость видеопотока камеры, кадров в секунду
camera_F	Фокусное расстояние камеры (см.выше)
thresh_min	Кортеж (tuple) нижних пороговых значений для фильтрации контура дрона, для каждого из 3 каналов. Значения подбираются с помощью вспомогательной программы DroneDetectorLAB
thresh_max	Кортеж (tuple) верхних пороговых значений для фильтрации контура дрона, для каждого из 3 каналов. Значения подбираются с помощью вспомогательной программы DroneDetectorLAB
blur	Сила размытия картинки перед фильтрацией
color_space_code	Способ преобразования цветового пространства кадра. По умолчанию - cv2.COLOR_BGR2LAB
DroneFrameSize	Размер стороны квадрата рамы дрона в метрах. Для Жужи Nano = 0.1м
WriteLog	Флаг записи логов. Если установлен в True - в каталоге программы при запуске класса создаётся новый лог файл с указанием даты и времени
FilterDepth	Глубина фильтра скользящего среднего для фильтрации координат, вычисляемых по изображению
dd_Target	Массив координат целевого положения дрона [X,Y,Z]. Расстояние указывается в метрах
dd_HoverThrottle	Значение стика газа по умолчанию (газ висения) - среднее положение стика
dd_XY_PID	Массив коэффициентов P, I, D для удержания дрона в плоскости XY
dd_Z_PID	Массив коэффициентов P, I, D для удержания высоты дрона по оси Z
k_PID_XY	Коэффициент, на который умножаются отклонения стиков дрона при удержании дрона с помощью крена (по оси Y), относительно значений, посчитанных PID регулятором
write_avi	Логическая переменная, отвечающая за запись видео в файл. True - включена, False - выключена.

При запуске файла ZuzaNanoMocap2.py в качестве основной программы - на экран выводится видеопоток с камеры с отображением обнаруженного дрона и его координат:

Перед запуском автономного дрона в полёт необходимо проверить, что рассчитанные координаты соответствуют реальным.