> For the complete documentation index, see [llms.txt](https://copter-space.gitbook.io/nebo-v-karmane/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://copter-space.gitbook.io/nebo-v-karmane/v-ray-teleupravlenie-dronom/opredelenie-polozheniya-drona-v-prostranstve-s-pomoshyu-web-kamery.md). # Определение положения дрона в пространстве с помощью Web-камеры Для определения положения дрона в пространстве относительно центра камеры нужно последовательно решить две задачи при обработке каждого кадра: {% hint style="success" %} 1. **Обнаружение контуров дрона "Жужа Nano" на изображении** 2. **Расчёт 3D-координат дрона "Жужа Nano" относительно оптического центра камеры.** {% endhint %} Ниже подробно рассмотрен алгоритм решения указанных задач. ### Обнаружение контуров дрона на изображении Для фильтрации изображения кадров Web-камеры используется библиотека **OpenCV**. Фильтрация изображения реализована в тестовой программе **DroneDetectorLAB.py**.\ Скачать её можно по ссылке на [Яндекс Диск](https://yadi.sk/d/hG409UkRSL7uyA) и запустить с помощью программы \ **Visual Studio Code** на вашем ПК. Цветное изображение с Web камеры разбивается на 3 канала в соответствии с [цветовой моделью LAB](https://ru.wikipedia.org/wiki/LAB) (опционально, например, для других цветов, можно использовать другие цветовые пространства: [HSV](https://ru.wikipedia.org/wiki/HSV_\(%D1%86%D0%B2%D0%B5%D1%82%D0%BE%D0%B2%D0%B0%D1%8F_%D0%BC%D0%BE%D0%B4%D0%B5%D0%BB%D1%8C\)) и прочие). Цветовое пространство кадра преобразуется с помощью`cv2.cvtColor(img, cv2.COLOR_BGR2LAB)`, затем разбивается на 3 канала с помощью `cv2.split(hsv)`. Каналы фильтруются с помощью параметров, задаваемых ползунками программы, которые можно интерактивно менять: ![Канал L](/files/-MDd2XcG1VQnp1by7IfH) ![Канал A](/files/-MDd2eR_RKZlv3hl6R9B) ![Канал B](/files/-MDd2po53M3MQECl1ngJ) ![Настройки фильтрации](/files/-MDd2v1_zYXQ43blSwqa) ![Результат фильтрации](/files/-MDd31ltayoD4yJxDtwd) ![Контуры дрона на исходном кадре](/files/-MDd397IMzD99FZS0CLL) На отфильтрованном изображении выбираются контуры (с помощью `cv2.findContours( thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)`), среди которых выбирается контур с максимальной площадью - это и есть **контур нашего дрона**. \ После нахождения контура дрона переходим к расчёту пространственных координат. ### Расчёт 3D-координат дрона относительно центра камеры При расположении камеры на потолке, она смотрит на дрон сверху, и оси координат направлены следующим образом: ![Оси координат камеры на потолке](/files/-MDd3RWy0roktB40fB8D) Ось X смотрит на изображении вправо, ось Y - вниз, ось Z - в плоскость изображения. Самый простой способ рассчитать расстояние от камеры до объекта на основании координат изображения объекта - [принцип подобия треугольников.](https://www.math10.com/ru/geometria/podobnye-treugolniki.html) Например, мы берём объект известной ширины 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 метр от камеры. Для других объектов возможно исправить указанные параметры в тексте программы самостоятельно. ![Автоматический расчёт фокусного расстояния](/files/-MDd3qWFuqgj0NFxPY8c) Эти принципы расчёта использованы в классе [**ZuzaNanoMocap**](https://yadi.sk/d/sugnZP2G61iAvg), который предназначен для обеспечения автономного управления дроном на основании видео-потока с камеры. ### Класс для автономного управления дроном ZuzaNanoMocap Класс **Python ZuzaNanoMocap** запускается в отдельном потоке и выполняет следующие функции: 1. Получение и фильтрация видеопотока с камеры, выделение контуров дрона 2. Расчёт координат дрона в пространстве, а также скорости и ускорения, фильтрация погрешностей с помощью фильтра скользящего среднего 3. Расчёт отклонения стиков дрона для удержания дрона в заданной точке, с помощью алгоритма [PID регулирования](https://ru.wikipedia.org/wiki/%D0%9F%D0%98%D0%94-%D1%80%D0%B5%D0%B3%D1%83%D0%BB%D1%8F%D1%82%D0%BE%D1%80) 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**](https://yadi.sk/d/xWFRN4CqmdMinQ) в качестве основной программы - на экран выводится видеопоток с камеры с отображением обнаруженного дрона и его координат: ![](/files/-MDd4Nkv1m6jMhHHW-6j) Перед запуском автономного дрона в полёт необходимо проверить, что рассчитанные координаты соответствуют реальным.