Глава 3. Программирование Blockly
3.1. Рекомендации по использованию Blockly
Интерфейс блочного программирования – удобный инструмент для изучения автономного полета БПЛА без знания языков программирования.
Для безопасной эксплуатации рекомендуется реализовывать автономный полет посредством двух эксплуатантов:
первый – оператор дрона, который переключает БВС в режим ручного управления в случае нештатной ситуации (ошибки программирования, сбой оборудования, недостаточность пространства для выполнения маневра);
второй – оператор персонального компьютера, который запускает программу и следит за ее выполнением.
Рекомендуется использовать задержку после вызова функции взлета, так как дрону необходимо время, чтобы набрать высоту. Перед взлетом рекомендуется обнулять скорости

Пример реализации рекомендации
Рекомендуется сигнализировать DXL модулем «Трехцветный светодиод» о выполнении какого-либо действия, чтобы иметь визуальный контроль исполнения кода.
При использовании в программе методов программирования, которые используют одометрию, рекомендуется обнулять одометрию в точках, относительно которых происходит позиционирование (обычно это точка взлета).

Обнуление оптической одометрии после взлета
3.2. Алгоритм автономного запуска БПЛА через графический интерфейс
Устанавливаем дрон в защищенном кубе.
Включаем пульт управления.
Подключаем АКБ так, чтобы дрон не повернулся. Это важно, так как в момент включения инициализируется система координат дрона и направление осей.
Для программирования в интерфейсе Blockly подключаемся к Графическому интерфейсу DroneCam (см. Глава 2).
Затем нажимаем на кнопку настройки в левом верхнем углу, а затем «Программирование Блокли».


Переход в окно «Программирование Блокли»
Программируем автономный полет или загружаем файл.
Включаем пульт.
Переводим дрон в режим ARM, моторы активируются.
Включаем режимы AltHold и PosHold.
Переключаем тумблер, закрепленный за 8 каналом в нижнее крайнее положение, это переключит квадрокоптер на управление с TrackingCam v3 и введет его в режим автономного полета. Управление с пульта будет недоступно.
Запускаем скрипт через графический интерфейс, нажав на соответствующую кнопку.

Запуск скрипта
При возникновении нештатной ситуации рекомендуется перевести дрон в ручное управление, переведя тумблер, закрепленный за 8 каналом в верхнее крайнее положении. Удерживаем на пульте управления уровень газа (стик Throttle) на середине, чтобы при переключении дрон не упал.При возникновении нештатной ситуации рекомендуется перевести дрон в ручное управление, переведя тумблер, закрепленный за 8 каналом в верхнее крайнее положении. Удерживаем на пульте управления уровень газа (стик Throttle) на середине, чтобы при переключении дрон не упал.
- После выполнения скрипта:
| Если квадрокоптер выполнил посадку в автономном режиме | – опускаем стик Throttle вниз; |
|---|---|
| – переводим тумблер 8 канала в верхнее крайнее положение, вернув дрон в ручное управление; | |
| – переходим к пункту 11 | |
| Если после завершения скрипта квадрокоптер находится в воздухе | – удерживаем на пульте стик Throttle посередине, чтобы при переходе в ручной режим квадрокоптер не упал. |
| – переводим тумблер 8 канала в верхнее крайнее положение, управление с пульта снова доступно. | |
| – выполняем посадку в ручном режиме, осторожно опуская стик Throttle. |
После завершения посадки переводим дрон в режим DISARM.
Выключаем режимы AltHold и PosHold, переведя тумблер, отвечающий за их включение в верхнее крайнее положение.
Отключаем АКБ.
Выключаем пульт.
3.3. Примеры использования Blockly
Ниже предоставлен список доступных примеров. Скачивание и видео работы примеров доступны по ссылке:
https://github.com/AppliedRobotics/ARA-EDU/tree/main/drone-control-api/examples/Кейсы%20Блокли
| Название примера | Название папки | Описание |
|---|---|---|
| Простые примеры | ||
| Takeoff_and_landing.json | Взлет и посадка | Пример взлета и посадки |
| Takeoff_and_rotate.json | Взлет и поворот | Пример взлета, поворота и посадки |
| Takeoff_and_fly_forward _odom.json | Пролет вперед | Пример взлета, пролета вперед и посадки |
| Square_odom.json | Полет по квадрату | Пример полета по квадрату с помощью одометрии |
| WallFollow.json | Полет вдоль стены (Релейный регулятор) | Пример полета вдоль стены при помощи ультразвукового дальномера с релейным регулятором. |
| Примеры с ArUco-маркером | ||
| ArucoSearch.json | Поиск Аруко Маркера | Пример поиска ArUco-маркера |
| TrackingAruco_R_Reg.json | Слежение за Аруко (Релейный регулятор) | Пример слежения за ArUco-маркером с помощью релейного регулятора |
| TrackingAruco_P_Reg.json | Слежение за Аруко (П-регулятор) | Пример слежения за ArUco-маркером с помощью П-регулятора |
| Boarding_aruco_PD.json | Посадка на Аруко | Пример посадки на ArUco-маркер с помощью П, Д-регулятора |
| Примеры с цветными блобами | ||
| Blob_Follow_P_Reg.json | Слежение за блобом (П-регулятор) | Пример слежения за цветным объектом с помощью П-регулятора. |
| Blob_Follow_PID_Reg.json | Слежение за блобом (ПИД-регулятор) | Пример слежения за цветным объектом с помощью ПИД -регулятора. |
| Примеры с полезной нагрузкой | ||
| Aruco_Sorting_Magnet.json | Сортировка груза по ID Аруко (электромагнит) | Пример использования электромагнита в автономном полете. В зависимости от ID распознанного ArUco-маркера загорится соответствующий цвет светодиода и дрон выполнит полет в необходимую точку. |
| Color_Cargo_Drop_Servo. json | Сброс груза по цвету (сервопривод) | Пример демонстрирует работу механизма сброса с сервоприводом. При обнаружении объекта определенного цвета меняется световая индикация и сервопривод поворачивается на заданный угол для сброса груза соответствующего цвета. |
| Aeroduel_Shoot_Arucos. json | Стрельба по Аруко | Пример выстрелов по двум ArUco-маркерам. После взлета квадрокоптер начинает поиск маркеров и выравнивание по оси yaw. ID целевых ArUco-маркеров не должны повторяться. |
Алгоритм запуска примера:
Скачиваем файл с примером.
Подключаемся к Графическому интерфейсу.
Переходим в меню и выбираем вкладку «Программирование Блокли».
Для загрузки примера выбираем «Открыть рабочее пространство».

Переход в окно рабочего пространства для загрузки файла с примером
Выбираем скачанный ранее файл.
Переводим дрон в автономный режим, запускаем скрипт, нажав на кнопку «Запуск программы».
Примеры содержат методы программирования, использующие различные системы координат (СК). Далее представлено описание используемых СК, алгоритмы работы примеров, а также подготовка к их выполнению.
3.4. Простые примеры
3.4.1. Системы координат
1. Скоростная система координат. Используется для методов «Задать скорость XY» (setVelXY) и «Задать скорость XY и рыскание» (setVelXYYaw).
Оси координат задаются следующим образом: ось Х вперед, ось Y вправо, ось Yaw по часовой стрелке. Соответственно, если задать скорость X положительной, то дрон полетит прямо, если отрицательной – полетит назад. Аналогично по оси Y и углу поворота Yaw.

Направление осей координат скоростной СК
2. Одометрическая система координат. Используется в методах «Лететь в XY дрон» (gotoXYdrone) и «Лететь в XY одометрию» (gotoXYodom).
Ось X направлена вперед, ось Y направлена влево. Необходимо учитывать, что в случае вызова «Лететь в XY дрон» данная система координат используется независимо от текущего угла поворота дрона, так как привязана к дрону. Если вызывать метод «Лететь в XY одометрию» без предварительного обнуления одометрии, то нулем (началом) координат будет являться точка включения дрона.

Направление осей координат одометрической СК
В Приложении 1 данного руководства продублированы используемые системы координат с направлением осей, а также методами, которые их используют.
3.4.2. Описание простых примеров
Взлет и посадка
(Takeoff_and_landing.json)

Взлет и посадка
Дрон взлетит, а спустя 10000 миллисекунд (10 секунд) приземлится обратно. Квадрокоптер может немного сместиться в сторону при взлете, но при наборе высоты он вернется в зону над точкой взлета.
Взлет и поворот
(Takeoff_and_rotate.json)

Взлет, поворот и посадка
Дрон взлетит, через 10 секунд повернется на 1.57 радиан (90 градусов). Поворот будет осуществляться по часовой стрелке. После поворота дрон начинает посадку. Если задать угол рыскания -1,57 радиан, дрон повернется влево на 90 градусов, поворачиваясь против часовой стрелки.
Если перезапустить данный пример, дрон не повернется еще на 90○, так как углы считаются исходя из его начального положения. Соответственно, если необходимо, чтобы квадрокоптер повернулся еще на 90○ после запуска, требуется ввести значение 3,14 радиан, что соответствует 180-и градусам и запустить пример.

Положение квадрокоптера при перезапуске примера со значением угла рыскания 1,57 радиана и 3,14
Полет вперед
(Takeoff_and_fly_forward_odom.json)
иллюстрации Взлет, полет и посадка
В самом начале программы рекомендуется обнулять все скорости, чтобы дрон не полетел в сторону при старте. После этого происходит взлет и спустя 10 секунд дрон начинает лететь прямо на 1.5 метра, затем совершает посадку.
При запуске данного примера убеждаемся, что пролет вперед будет безопасен!
Полет по квадрату
(Square_odom.json)

Взлет, полет по квадрату с использованием метода «Лететь в XY (одометрия)», посадка
В самом начале программы рекомендуется обнулять скорости, чтобы дрон не полетел в сторону при старте.
В этой программе дрон совершает пролет по квадрату с использованием метода полета по одометрии. Для этого дрону задаются координаты 4-х углов квадрата. Координаты и траектория полета дрона по квадрату изображены на иллюстрации
При запуске данного примера необходимо убедиться, что полет по квадрату будет безопасен!

Траектория и координаты полета дрона по квадрату
с использованием метода «Лететь в XY (одометрия)»
Данный пример также можно реализовать при помощи метода «Лететь в XY (дрон)». При полете в точку учитывается нынешнее положение дрона. Таким образом, программа с использованием метода «Лететь XY (дрон)» будет выглядеть следующим образом:

Взлет, полет по квадрату с использованием метода «Лететь XY (дрон)», посадка
Полет вдоль стены
(WallFollow.json)
Данный пример реализует полет вдоль стены при помощи ультразвукового датчика расстояния и релейного регулятора.
Регулятор – алгоритм, выполняющий задачу поддержания объекта управления в определенном состоянии.
Регулятор вычисляет рассогласование (ошибку) между заданным состоянием и текущим. Он получает информацию о состоянии объекта управления с помощью датчиков и вырабатывает управляющее воздействие для того, чтобы его корректировать.
Релейный регулятор – это регулятор, изменяющий скачком управляющее воздействие на систему при прохождении регулируемой величины через пороговое фиксированное значение.

Полет вдоль стены
В функции wall_follow задаются желаемое расстояние до стены (distance = 0.5) и постоянная скорость, с которой дрон будет лететь вдоль стены (roll_vel = -0.65). В данном случае полет вдоль стены будет производиться в левую сторону, так как значение скорости отрицательное.
После чего в бесконечном цикле определяется расстояние между дроном и стеной с помощью ультразвукового дальномера (переменная distance_to_wall) и считается ошибка между заданным расстоянием и текущим (pitch_error = distance_to_wall - distance).
Затем начинает действовать релейный регулятор:
Если дрон отлетит от стены больше, чем на 0.5 м (pitch_error > 0), то с помощью регулятора он подлетит к ней с заданной скоростью pitch_vel. Переменная pitch_vel равна значению, которое возвращает функция sign, умноженному на 0.65.
Если дрон подлетит слишком близко к стене (pitch_error < 0), то с помощью регулятора он отлетит от нее на заданную скорость pitch_vel, которая примет отрицательное значение, так как функция sign вернет значение -1.
Далее значения переменных pitch_vel и roll_vel передаются в метод «Задать скорость XY», и квадрокоптер выполняет движение с заданными скоростями.
В примере реализовано сигнализирование о событиях при помощи модуля «Трехцветный светодиод». Когда квадрокоптер находится на расстоянии до стены больше заданного, светодиод загорается зеленым. Если расстояние меньше заданного – красным.
Данная программа запускается в бесконечном цикле без прерывания. Чтобы выполнить посадку, необходимо перейти в ручной режим и посадить квадрокоптер.
Если необходимо добавить параметры в функцию (как в данном примере – функция sign с параметром value), необходимо выполнить следующее:
- Нажимаем на шестеренку в блоке функции, откроются настройки функции.

Настройки функции
- Вводим имя параметра в левой части окна настроек функции вместо «х», затем перемещаем его в блок параметры.

Настройка параметра функции
- Можно создавать столько параметров, сколько необходимо.
3.5. Примеры с ArUco-маркером
ArUco-маркеры — это двоичные квадратные маркеры, состоящие из сетки (4x4, 5x5, 6x6, 7x7), где каждая ячейка имеет значение 0 (черное) или 1 (белое). В центре маркера находится уникальный паттерн, который может быть легко распознан с помощью компьютерного зрения. Каждый маркер окружен черной рамкой и имеет свой уникальный идентификатор (ID), который позволяет различать маркеры между собой.

Примеры ArUco-маркеров
Камера захватывает изображение, после чего изображение обрабатывается, преобразуется в оттенки серого, затем происходит фильтрация шума (отбрасываются мелкие детали) и в конце изображение проходит бинаризацию (превращается в черно-белое).
Изображение до и после обработки
После чего идет поиск замкнутых контуров. Ищутся области с прямоугольными границами (квадратная форма маркера).
Далее извлечение внутреннего паттерна. Алгоритм "нарезает" внутреннюю часть маркера на ячейки и анализирует их, получая двоичный код. Извлеченный двоичный код сравнивается с библиотекой шаблонов (словарем), чтобы найти соответствующий ID:

Сегментирование ArUco маркера на ячейки и извлечение его двоичного кода
Если код совпадает с одним из шаблонов в словаре, маркер успешно распознан, если же код не совпадает, маркер считается нераспознанным.
Для правильного распознавания ID маркера независимо от его ориентации, определяется угол поворота маркера. Паттерн приводится к стандартной ориентации (например, с верхним левым углом как начальной точкой).
3.5.1. Настройка ArUco-маркеров
Для работы с ArUco-маркерами необходимо выполнить настойку. В боковом меню Графического интерфейса доступен раздел «Настройки».

Окно «Настройки»
Активируем ползунок «Обнаружение маркеров ArUco».
Справа в меню «ArUco» можно выбрать используемый словарь ArUco-маркеров 4х4, 5x5, 6x6, 7x7, после выбора нажимаем «Применить выбранный словарь ArUco». Максимальный ID используемого маркера – 49. Также можно настроить размер стороны маркера в метрах.
Чтобы сгенерировать маркеры, нажимаем кнопку «Создать ArUco-маркер», после чего откроется бесплатный сайт по генерации маркеров. Для применения настроек необходимо нажать кнопку «Применить размер маркера».
3.5.2. Система координат ArUco-маркеров
Система координат (СК) ArUco-маркеров подразумевает наличие двух способов получения информации о видимых маркерах:
- Получение позиции ArUco-маркеров относительно дрона (метод «Для каждого из ArUco-маркеров», getArucos).

СК метода «Для каждого из ArUco-маркеров»
Координаты маркеров измеряются относительно системы координат, представленной на иллюстрации Таким образом, если камера:
направлена вперед, то для стабилизации дрона относительно ArUco-маркера по оси roll необходимо использовать координату Х, для оси pitch – координату Z, а для высоты – координату Y.
направлена вниз, то система координат поворачивается на 90 градусов относительно оси X по часовой стрелке, тогда ось Х направлена вправо, ось Y – назад, ось Z – вниз. В таком случае для стабилизации дрона относительно ArUco-маркера по оси roll используем координату Х, по оси pitch – координату Y, а по высоте – координату Z. На иллюстрации изображен пример информации, которую возвращает метод «Для каждого из ArUco-маркеров» и каждый из его параметров отдельно. Позиция по осям (pos*, position) измеряется в метрах, а ориентация (orient*, orientation) – в радианах.

Информация, которую возвращает метод
«Для каждого из ArUco-маркеров» и каждый из параметров
- Получение позиции дрона относительно ArUco-маркеров (метод «Для каждой позиции относительно ArUco-маркера», getCameraPoseAruco).

СК метода «Для каждой позиции относительно ArUco-маркера»
Данный метод возвращает позиции камеры относительно задетектированных маркеров. Координаты камеры измеряются относительно системы координат, центр которой находится в центре за детектированного маркера. Для выравнивания относительно маркера используются ось Z для pitch, ось X для roll, ось Y для высоты.
Если камера направлена в пол, то система поворачивается на 90 градусов вокруг оси X по часовой стрелке. Тогда для выравнивания относительно маркера используются ось Y для pitch, ось X для roll, ось Z для высоты.
Единицы измерения параметров метода «Для каждой позиции относительно ArUco-маркера» такие же, как и для параметров метода «Для каждого из ArUco-маркеров».

Информация, которую возвращает метод «Для каждой позиции относительно ArUco-маркера» и каждый из его параметров
В Приложении 1 данного руководства продублированы используемые системы координат с направлением осей, а также методами, которые их используют.
3.5.3. Описание примеров с ArUco-маркером
Поиск ArUco-маркера
(ArucoSearch.json)

Сборка на поиск ArUco маркера
В самом начале вызывается функция main.
В функции main происходит взлет, далее вызывается функция search_aruco (поиск ArUco), после выполнения которой дрон висит две секунды и совершает посадку.
Функция search_aruco принимает скорость, с которой дрон будет вращаться и искать ArUco-маркер (vel = -0,5). После вызова данной функции светодиод загорается красным, затем дрон начинает вращаться с заданной скоростью против часовой стрелки, так как значение переменной vel, которое передается в метод «Задать скорость XY и рыскание», – отрицательное.
Далее запускается цикл «повторять, пока» (while), который выполняется, пока aruco_state = «истина». Когда маркер будет задетектирован, метод «Для каждого из ArUco-маркеров» вернет не пустой список, условие цикла перестанет выполняться (aruco_state = «ложь»). Светодиод загорится зеленым, дрон остановится и цикл, как и функция search_aruco, завершится.
Слежение за ArUco-маркером при помощи релейногорегулятора (TrackingAruco_R_Reg.json)
Данный пример реализует выравнивание на маркер при помощи самого простого регулятора – релейного. Для каждой из осей корректировки (X, Y, Z) рассчитывается ошибка (в метрах). После чего позиция дрона корректируется при помощи управления по скорости.
Ошибка – это разница между заданным и действительным положениями.


Слежение за ArUco-маркером при помощи релейного регулятора
В начале программы вызывается функция main, в которой происходит взлет, далее вызывается функция поиска ArUco-маркера (search_aruco) с вращением квадрокоптера по часовой стрелке, так как переданный параметр vel – положительный. Когда маркер распознан, светодиод загорается зеленым, затем вызывается функция выравнивания tracking_aruco. Выравнивание запускается в бесконечном цикле без прерывания, поэтому посадку необходимо выполнить в ручном режиме.
Функция tracking_aruco рассчитывает ошибки по расстоянию между дроном и ArUco-маркером по трем осям X (roll_error), Y (height_error) и Z (pitch_error), а также ошибку по ориентации (yaw_error).
Выравнивание по высоте
Если разница по высоте больше, чем допустимая (accuracy_height = 0.15), то вызывается функция height_regulation для регулировки высоты. Из текущей высоты вычитается ошибка height_error, и полученное значение передается в метод «Задать высоту».
Вычитание, а не сложение происходит по следующей причине: когда квадрокоптер находится выше ArUco-маркера, координата по оси Y (pos_y) примет положительное значение, так как ось Y при использовании метода «Для каждого из ArUco-маркеров» направлена вниз (см. подраздел 3.5.2). В этом случае, чтобы выровняться относительно маркера, дрону необходимо снизить высоту, следовательно, вычесть из текущей высоты ошибку. Аналогично для случая, когда квадрокоптер расположен ниже маркера.


Положение дрона относительно ArUco-маркера и значение координаты по оси Y
Выравнивание по крену, тангажу и рысканию
Вызывается функция регулировки aruco_regulation, в которую передаются рассчитанные ошибки по pitch, roll и yaw. На основе этих ошибок формируются управляющие воздействия (скорости: pitch_vel, roll_vel, yaw_vel), обеспечивающие корректировку положения квадрокоптера относительно ArUco-маркера.
Функция sign определяет знак выходного значения, то есть направление, в котором должен двигаться квадрокоптер для уменьшения текущей ошибки. Величина ошибки не учитывается: функция sign возвращает только три возможных значения: 0, 1 или -1. Это означает, что управляющее воздействие по модулю остается постоянным и не зависит от величины ошибки. Далее это значение масштабируется коэффициентами (0,6 для pitch и roll и 0,2 для yaw), задающими фиксированную скорость движения.
Такой принцип управления соответствует релейному (двухпозиционному) регулятору, при котором система переключается между фиксированными уровнями воздействия. Следствием применения релейного регулятора является наличие колебаний вокруг заданного значения, поскольку система постоянно перерегулируется и не может точно зафиксироваться в целевой точке.
Условие «если aruco_state» в функции tracking_aruco реализует поиск ArUco-маркера в случае его потери в процессе выравнивания. Переменная aruco_state принимает значение «ложь» (False) при каждой новой итерации цикла и меняет его на «истина» (True), если маркер распознается. В случае потери маркера светодиод загорается красным и вызывается функция aruco_regulation, в которую передаются последние зафиксированные значения ошибок по pitch и roll перед тем, как маркер был потерян.
Слежение за ArUco-маркеромпри помощи пропорционального регулятора
(TrackingAruco_P_Reg.json)


Слежение за ArUco-маркером
В данном примере вместо релейного используется пропорциональный регулятор. Его отличие заключается в том, что управляющее воздействие зависит от величины ошибки и не является фиксированным (меньше ошибка – ниже воздействие). В результате система может испытывать колебания, однако они выражены значительно слабее, чем при релейном управлении.
Функция aruco_regulation для выравнивания дрона относительно маркера принимает ошибки по расстоянию и умножает их на коэффициенты. Полученные значения сравниваются с пороговыми (threshold) в функции constrain. Если входное значение больше порогового, функция возвращает пороговое. Это делается для ограничения максимальной скорости, чтобы минимизировать вероятность перерегулирования. Если же значение находится в допустимых пределах, оно не изменяется. Таким образом, управляющее воздействие остается пропорциональным ошибке, но с ограничением по максимальному значению.
Если дрон потеряет маркер из виду, то будет вызываться функция aruco_regulation без вращения для того, чтобы дрон вернулся в позицию, где был виден маркер последний раз.
Посадка на ArUco-маркер
(Boarding_aruco_PD.json)
Пример реализует посадку на ArUco-маркер. Перед его выполнением необходимо перекрутить модуль TrackingCam v3 так, чтобы он был направлен вниз. Система координат ArUco-маркеров также поворачивается на 90○ (см. подраздел 3.5.2 или Приложение 1). При запуске примера рекомендуется печатать маркер размером не более 20 см.
Данный пример использует выравнивание при помощи PD-регулятора. P-составляющая регулятора формирует управляющее воздействие, пропорциональное величине ошибки. D-составляющая учитывает скорость изменения ошибки и формирует дополнительное воздействие, направленное на уменьшение перерегулирования. Если система слишком быстро приближается к заданному значению, D снижает управляющее воздействие, обеспечивая более плавное поведение.

Посадка на ArUco-маркер
В начале программы квадрокоптер выполняет взлет, далее инициализируются переменные. В бесконечном цикле реализовано распознавание маркера, выравнивание по нему, условия снижения и посадки на маркер.
Алгоритм работы программы:
Камера, направленная вниз получает данные о положении ArUco-маркера с помощью метода «Для каждого из ArUco-маркеров».
По этим данным производится выравнивание дрона относительно маркера при помощи PD-регулятора в функции ArucoRegulation.
После того, как дрон попадает в некоторую область, регламентирующую точность выравнивания (ошибки по pitch и roll меньше accuracy и не равны нулю), он снижает высоту.
Если текущая высота, полученная с помощью метода «Получить высоту по дальномеру», меньше accuracy_height, цикл прерывается и дрон осуществляет посадку.
Из ошибки по pitch вычитается 0,1 м, так как объектив камеры смещен относительно геометрического центра дрона примерно на 10 см вперед. Это необходимо учитывать, так как расчет ошибки выполняется относительно положения камеры, а не центра дрона. Без внесения этой поправки фактическое положение дрона оказывается смещенным назад относительно маркера при посадке.
В функции ArucoRegulation реализована классическая работа PD-регулятора. Рассчитанные ошибки сначала умножаются на П-коэффициент (p_coef), формируя основное управляющее воздействие, которое затем суммируется с воздействием Д-составляющей. Она рассчитывается путем умножения Д-коэффициента (d_coef) на разницу между текущей и прошлой ошибками. Прошлые ошибки по крену и тангажу хранятся в переменных prev_error_roll и prev_error_pitch, которые обновляются после расчета и установки управляющих скоростей.
Обратим внимание, что управляющее воздействие для оси pitch умножается на –1. Это обусловлено ориентацией осей системы координат ArUco-маркеров (см. Приложение 1 «Системы координат ArUco-маркеров»). Когда ArUco-маркер находится позади дрона, центр ArUco-маркера располагается в нижней части кадра.


Положение ArUco-маркера относительно дрона и в кадре камеры
В данном случае ошибки по оси Y будут положительными, однако, чтобы отрегулировать дрон относительно центра маркера, необходимо выполнить полет назад, а, значит, в метод «Задать скорость XY» отправить отрицательное значение скорости по оси X (см. Приложение 1 «Скоростная система координат»). Аналогично для обратного случая, когда маркер находится в верхней части кадра.
Как только абсолютные значения ошибок по pitch и roll будут меньше допустимого порога (в примере это accuracy = 0.15 метра), дрон начнет снижение. Когда дрон достигнет высоты менее 0.4 метров, произойдет выход из цикла и посадка.
В ситуации, когда маркер не распознается определенное количество итераций (time_count > 35) и текущая высота, умноженная на 1.25, больше максимальной, квадрокоптер поднимается выше, чтобы увеличить площадь обзора.
3.6. Примеры с цветными блобами
3.6.1. Настройка цветовых блобов (blobs)
Цветовые блобы детектируются в формате HSV (см. Приложение №1), подробнее про алгоритм детекции можно узнать в учебном пособии «Модуль технического зрения TrackingCam v3».
В боковом меню Графического интерфейса доступен раздел «Настройки». Активируем «Обнаружение Блобов» и переходим в меню «Блобы» в правой части экрана.

Настройка распознавания блобов
Для настройки доступно 4 цветовых блоба, по каждому доступна настройка минимальных и максимальных H (Hue), S (Saturation), V (Value), при которых будет распознаваться блоб. Более подробно о цветовой модели HSV можно прочитать в Приложении 2.

Пример настройки цветового блоба
Диапазон измерения компонентов блоба:
H: 0 – 255
S: 0 – 255
V: 0 – 255
Для удобства настройки доступна визуализация цветовой маски, находящаяся справа от изображения. Стоит учитывать, что из-за задержки при выборе другого блоба маска изменится через 2-3 сек. В программировании (Python API и Blockly) доступна информация об ID (номере Блоба), координатах его центра в системе координат изображения и его площади.
В Приложении 3 можно найти пример настройки цветового блоба.
3.6.2. Система координат цветовых Блобов
Для детектируемых цветовых блобов используется следующая система координат (метод «Для каждого блоба из массива блобов», getBlobs):

Направление осей системы координат камеры
Блобы детектируются относительно пиксельного центра координат камеры. Начало системы координат камеры расположено в левом верхнем углу. В соответствии с этим ось Х направлена вправо, а ось Y направлена вниз. Также в данном случае координаты объектов возвращаются не в метрическом, а в пиксельном формате (максимум 320 пикселей по Х и 240 пикселей по Y). Таким образом, чтобы найти координаты объекта относительно оптического центра, необходимо вычесть из координат по оси X 160 (320/2) и из координат по оси Y 120 (240/2).
В Приложении 1 данного руководства продублированы используемые системы координат с направлением осей, а также методами, которые их используют.
3.6.3. Примеры Blockly программирования с цветными блобами
Перед запуском данного примера выполняем настройку цветового блоба, который будет распознавать дрон (см. подраздел 3.6.1). После настройки следует выполнить полет в ручном режиме и оценить корректность распознавания: убедиться, что блоб стабильно распознается, в его границы не попадают посторонние объекты. Примеры некорректно настроенного цветового блоба приведены на иллюстрации


Примеры распознавания блоба в полете при плохой настройке
Некорректная настройка блоба может привести к непредсказуемому поведению дрона при запуске примера, приведенного ниже. Следует выполнить донастройку HSV, чтобы в полете границы блоба совпадали с границами объекта.
На иллюстрации приведен пример распознавания блоба при хорошей настройке HSV: в кадре отсутствуют или незначительны шумы от объектов вокруг, блоб распознается с ближнего и дальнего расстояния и при обзоре с разных сторон.


Пример распознавания блоба в полете при корректной настройке HSV
Слежение за цветным объектом с использованием пропорционального регулятора
(Blob_Follow_P_Reg.json)

Слежение за цветным объектом с использованием пропорционального регулятора
При запуске функции main квадрокоптер выполняет взлет, далее активируется основная функция слежения за объектом blob_follow. В бесконечном цикле происходит распознавание блоба, вычисление его размеров и координат центра в изображении.
Поскольку площадь блоба в изображении зависит от расстояния до объекта, этот параметр может использоваться для оценки удаления до цветного маркера. Полученные значения, измеряемые в пикселях, сравниваются с центром изображения (camera_center_x, camera_center_y) и целевой площадью объекта (target_area) в кадре, чтобы определить, насколько текущее положение дрона отклоняется от желаемого. Таким образом вычисляются ошибки.
Если ошибка по высоте превышает допустимый порог (accuracy_height), вызывается функция height_regulation, которая корректирует текущую высоту дрона пропорционально величине ошибки. В функции regulation выполняется расчет скоростей для регулировки положения дрона относительно центра блоба и расстояния до него.
После вычисления управляющего воздействия его значение ограничивается функцией constrain: если рассчитанная скорость превышает пороговое значение threshold, она принудительно снижается до допустимого уровня (threshold). Это предотвращает чрезмерно резкую реакцию дрона при значительном расхождении между заданным и текущим положением.
Для выполнения посадки необходимо перейти в ручной режим, так как программа запускается в бесконечном цикле без прерывания.
Перед запуском данного примера необходимо самостоятельно вычислить величину переменной target_area. Это значение задает площадь блоба на изображении, которая соответствует нужному расстоянию до объекта, и используется для управления положением дрона. Если блоб будет небольшим, а целевая площадь – наоборот, дрон будет пытаться достичь целевой площади и вскоре врежется в блоб.Перед запуском данного примера необходимо самостоятельно вычислить величину переменной target_area. Это значение задает площадь блоба на изображении, которая соответствует нужному расстоянию до объекта, и используется для управления положением дрона. Если блоб будет небольшим, а целевая площадь – наоборот, дрон будет пытаться достичь целевой площади и вскоре врежется в блоб.
Для расчета target_area выполняем настройку распознавания блоба (см. подраздел 3.6.1).
Запускаем квадрокоптер в ручном режиме, взлетев на высоту 1 метра и подлетаем к блобу на расстоянии, которое он будет удерживать.
Рекомендуется выполнять расчет целевой площади посредством двух операторов. Первый будет управлять положением дрона, второй – координировать действия первого и следить за тем, чтобы блоб распознавался полностью.
- Выполняем расчет целевой площади блоба в кадре одним из способов:
Способ 1: переходим в раздел «Настройки» графического интерфейса и переключаемся в меню «Блобы». В блоке «Обнаруженные блобы» будет выводиться информация о видимом цветном маркере.

Информация об обнаруженном блобе
Обращаем внимание на размеры маркера в кадре и вычисляем площадь блоба в кадре:
В примере это 108,74 * 199.06 = 21 645,78. Округлив, берем значение в диапазоне от 21000 до 22000, например, 21500 (диапазон значений в ваших условиях будет другим). Принятое значение вписываем в переменную target_area.
Способ 2: после настройки блоба переходим в раздел «Программирование Блокли» и создаем короткую программу с бесконечным циклом.
Запускаем программу. В стеке вызовов каждую секунду будет появляться красная срока с искомым значением целевой площади. В примере это диапазон значений от 22000 до 22670. Выбираем значение в этом диапазоне и вписываем его в переменную target_area.
Останавливаем выполнение программы.

Программа и вывод в стеке вызовов после запуска программы для определения target_area
Рекомендуется обращать внимание на несколько значений. Например, значение площади в разные моменты было 19734, 20815, 21928. В таком случае рекомендуется взять минимальное значение при условии, что блоб распознавался полностью. Тогда определяем значение в диапазоне 19000–21000.
Слежение за цветным объектом с использованием ПИД-регулятора
(Blob_Follow_PID_Reg.json)


Слежение за цветным объектом с использованием ПИД-регулятора
init_PID – функции для инициализации PID-регуляторов (pitch и roll). Обратим внимание, что приведенные в примере коэффициенты PID-регулятора подобраны с учетом полной сборки дрона и предварительных настроек полетного контроллера, загрузка которых описана выше (см. подраздел 1.1–1.2). Следует учитывать, что роботизированные системы могут иметь небольшие отличия, поэтому коэффициенты PID-регулятора, приведенные в примере, могут не подойти конкретной сборке.
В этом случае выполняем индивидуальный подбор коэффициентов. Рекомендуется начинать настройку с пропорциональной составляющей P при нулевых значениях I и D, затем подобрать дифференциальную составляющую D, после чего, при необходимости, интегральную составляющую I.
В функции blob_follow рассчитывается центр камеры в пикселях (камера имеет разрешение 320 на 240), после чего задается целевая площадь объекта слежения в пикселях (target_area = 15000), чем ближе мы к объекту, тем больше площадь, чем дальше, тем меньше площадь.
Затем задается предельная площадь в пикселях (blob_threshold): будут отслеживаться объекты, у которых площадь больше, чем данное значение. При обнаружении объекта заданного цвета и размера вычисляется его площадь для определения ошибки по pitch. Для вычисления ошибок по высоте (height) и крену (roll) рассчитывается разница между центром изображения и центром объекта. Далее вызывается функция regulation, принимая на вход ошибки по осям.
В функции regulation рассчитываются скорости для выравнивания с помощью функций PID_PITCH и PID_ROLL, которые реализуют работу ПИД-регулятора. В функции constrain происходит ограничение рассчитанных управляющих воздействий, чтобы уменьшить резкость дрона при значении более 1 (threshold).
Перед запуском примера необходимо также самостоятельно вычислить величину переменных target_area и blob_threshold для ваших условий. Расчет target_area рассмотрен в описании предыдущего примера.Перед запуском примера необходимо также самостоятельно вычислить величину переменных target_area и blob_threshold для ваших условий. Расчет target_area рассмотрен в описании предыдущего примера.
Переменную blob_threshold вычисляем аналогично с target_area. Необходимо выявить максимальную площадь объектов в кадре, которые не будут учитываться.
3.7. Советы по использованию алгоритмов технического зрения
Стоит учитывать, что включение двух алгоритмов одновременно (и детектирование маркеров, и детектирование цветовых блобов) могут ухудшить производительность системы, рекомендуется включать только один алгоритм детекции.
В случае детектирования ArUco-маркеров, указанный размер маркера критично влияет на точность определения позиции БПЛА.
Не стоит пренебрегать настройкой фокуса камеры. Четкое изображение обеспечит более быстрое и правильное распознавание и позволит избежать непредвиденных ситуаций и лишних ошибок в выполнении кода.
Если требуется распознавание только одного блоба, следует установить значения HSV для остальных блобов в ноль, чтобы исключить распознавание других объектов и, как следствие, избежать непредвиденных происшествий. Пример приведен на иллюстрации

Настройка HSV-параметров для выделения одного блоба
3.8. Примеры с полезной нагрузкой
Использование полезной нагрузки открывает возможность решения прикладных инженерных задач, таких как конструирование креплений и обвесов, чтение схем подключения, управление полезной нагрузкой и разработка сценариев ее использования.
В перечень полезной нагрузки входят такие устройства, как:
Электромагнит (бортовой модуль захвата груза);
Сервопривод – не входит в базовый комплект;
Игрушечный пистолет с мягкими пулями – не входит в комплект.
Примеры, использующие сервопривод и игрушечный пистолет, реализованы на базе нестандартных конфигураций дрона ARA-EDU. Данные модули, сопутствующие крепления и провода для подключения не входят в базовый комплект поставки и служат примером целевого расширения платформы для решения специализированных соревновательных задач. Использование подобных модификаций осуществляется в рамках соревнований и под строгим контролем экспертов. Примеры, использующие сервопривод и игрушечный пистолет, реализованы на базе нестандартных конфигураций дрона ARA-EDU. Данные модули, сопутствующие крепления и провода для подключения не входят в базовый комплект поставки и служат примером целевого расширения платформы для решения специализированных соревновательных задач. Использование подобных модификаций осуществляется в рамках соревнований и под строгим контролем экспертов.
Примером применения нестандартной сборки ARA-EDU в соревновательной деятельности выступает дисциплина «Аэродуэль». 1
Данный подраздел рассматривает описание работы примеров, использующих вышеперечисленные устройства. В Приложении 4 приведены их подключение к квадрокоптеру и настройка модулей для управления устройствами.
Сортировка груза по ID Аруко при помощи электромагнита
(Aruco_Sorting_Magnet.json)
Данный пример демонстрирует использование электромагнита для захвата и сброса груза, а также полет в определенную точку в зависимости от ID распознанного ArUco-маркера. Для полета в точку используется метод «Лететь в XY (дрон)».

Сортировка груза по ID Аруко при помощи электромагнита
Программа начинается с команды на активацию электромагнита. Перед запуском программы под него необходимо поместить груз с закрепленной металлической пластиной или деталью для захвата.
Далее происходит ожидание появления в поле зрения камеры ArUco-маркера. Как только маркер распознан, программа считывает его ID и фиксирует соответствующие параметры для выполнения миссии:
ID 0: устанавливает координату Y равной 0, включается красный цвет светодиода (R=255).
ID 1: устанавливает координату Y = 1, включается зеленый цвет светодиода (G=255).
Другие ID: программа устанавливает координату Y равной -1 и включает синий цвет светодиода (B=255).
Как только маркер будет распознан, цикл прервется, и светодиод загорится соответствующим цветом. После активации светодиода в стеке отобразится предупреждение об освобождении полетной зоны. Спустя 5 секунд поступит сообщение о включении моторов.
Не включайте моторы, пока полетная зона не будет освобождена и закрыта!
Как только режим ARM будет активирован, начнется отсчет 6-и секунд на активацию режимов Althold+Poshold и 8-го канала для переключения в автономный режим управления. Спустя это время поступит команды на взлет и выполнение полета в соответствующую точку.
Далее квадрокоптер выполнит посадку и отключит электромагнит. Груз будет сброшен.
Сброс груза по цвету при помощи сервопривода (Color_Cargo_Drop_Servo.json)
Пример демонстрирует работу механизма сброса на основе сервопривода в автономном полете. При обнаружении объекта определенного цвета светодиод загорается соответствующим цветом и поворачивает сервопривод на заданный угол для сброса груза.
Перед запуском примера подвешиваем грузы зеленого и синего цвета на сброс и выполняем настройку блобов: для блоба ID 0 настраиваем распознавание синего объекта, для блоба ID 1 – распознавание зеленого объекта, а также следим, за тем, чтобы объекты распознавались полностью и не пересекались.


Настройка блобов
Таким образом, при распознавании синего блоба светодиод загорится синим цветом, сервопривод повернется на 0 градусов, сбросив синий груз. При распознавании зеленого блоба светодиод загорится зеленым цветом сервопривод повернется на 180 градусов (3,14 радиан), сбросив зеленый груз.

Сброс груза по цвету при помощи сервопривода
Выполнение программы начинается в блоке main. Сервопривод устанавливается на 90 градусов, производится взлет, после чего вызывается функция search_blob со значением скорости вращения vel = 0.5. Создается пустой список id_blobs, в который будут заноситься идентификаторы (ID) найденных целей (блобов). Задается переменная threshold_area = 20000, определяющая минимальную площадь цветового пятна в пикселях. Это необходимо для исключения ложных срабатываний от мелких объектов.
Дрон начинает непрерывное вращение по рысканию. Поиск выполняется внутри цикла «повторять, пока не len = 2». Это означает, что сканирование прекратится только после того, как в список будут добавлены ID двух объектов.
Для каждого обнаруженного в кадре цветового блоба программа рассчитывает его текущую площадь current_area путем умножения сторон size_x и size_y. Если объект превышает пороговое значение площади и его ID еще нет в списке id_blobs, активируется один из сценариев:
ID «0» (синий блоб): идентификатор добавляется в список id_blobs, светодиод переключается на синий цвет, дрон останавливает вращение и сервопривод поворачивается на угол 0°, открывая защелку для сброса синего груза.
ID «1» (зеленый блоб): ID добавляется в список id_blobs, светодиод переключается на зеленый цвет, дрон останавливается и сервопривод поворачивается на угол 180°, открывая защелку для сброса синего груза.
После выполнения сброса переменная counter, предназначенная для пополнения списка id_blobs, увеличивается на 1, программа пересчитывает длину списка id_blobs и снова запускает вращение квадрокоптера со скоростью vel для поиска следующего объекта.
Как только найдены оба объекта, функция search_blob завершается. Управляющий алгоритм возвращается в блок main, где полностью останавливается вращение дрона и подается команда на совершение посадки.
Стрельба по Аруко
(Aeroduel_Shoot_Arucos.json)
Пример демонстрирует стрельбу по ArUco-маркерам с выравниванием по оси yaw. После взлета квадрокоптер начинает поиск ArUco-маркеров. В случае, если маркер ранее не был распознан, начинается выравнивание по нему. Когда квадрокоптер попадает в некоторую зону, регламентирующую точность выравнивания, совершается выстрел.
В примере реализована сигнализация о событиях трехцветным светодиодом.
ArUco-маркеры рекомендуется располагать таким образом, чтобы в кадре находился только один маркер. В случае, если в кадр попадет несколько маркеров, повышается риск выстрела мимо маркера. Для совершения выстрелов по всем ArUco-маркерам их ID не должны повторяться. Рекомендуемая высота расположения ~ 1 м.

Стрельба по Аруко
Программа запускается в блоке main. Сначала дрон взлетает на высоту 0,8 м. Для предотвращения повторных выстрелов по одному и тому же маркеру создаётся пустой список marker_list, в котором хранятся ID ArUco-маркеров, по которым уже был произведён выстрел.
В блоке main реализован цикл по переменной i от 0 до 1 с шагом 1. На первой итерации цикла переменной i присваивается значение 0, после чего вызывается функция search_aruco() с передачей этого значения.
В ней задается скорость вращения для поиска маркеров. Пока маркеры не обнаружены, дрон продолжает вращение, а светодиод горит красным цветом. После обнаружения маркера выполняется проверка: содержится ли его ID в списке marker_list. Если данный ID отсутствует в списке, светодиод загорается синим, и начинается наведение на маркер.
Производится расчет ошибки по yaw. В предыдущих примерах для выравнивания использовалось значение -orientation_z, что обеспечивало поворот корпуса дрона параллельно плоскости маркера. Однако в данном случае такое выравнивание не обеспечивает точного попадания по ArUco-маркеру.
Для наведения на центр маркера в программе используется расчет угла через координаты при помощи функции atan2(y, x). Результатом ее работы является угол в радианах, на который необходимо повернуться, чтобы достичь целевого положения. Полученное значение умножается на коэффициент 0,6 и ограничивается при помощи функции constrain, чтобы избежать перерегулирования. После чего значение передаётся в метод задания угловой скорости по рысканию.
Когда ошибка по yaw становится меньше допустимого порога accuracy, дрон останавливается, загорается зеленый светодиод, совершается выстрел, а список marker_list пополняется ID текущего ArUco-маркера по индексу number = i.
Далее цикл в блоке main переходит ко второй итерации, где переменной i присваивается значение 1. Для следующего ArUco-маркера выполняется аналогичный алгоритм, после чего список marker_list пополняется новым ID. По завершении второй итерации цикл заканчивается, и выполняется посадка дрона.
Следует отметить, что количество целевых маркеров можно увеличить, изменив верхнюю границу цикла в блоке main. Поскольку отсчёт начинается с нуля, необходимо указывать значение на единицу меньше количества маркеров. Например, для четырёх маркеров в цикле следует задать значение 3. При этом важно, чтобы в кадре одновременно находился только один маркер.