Глава 3. Программирование Blockly
3.1 Рекомендации по использованию Blockly
Интерфейс блочного программирования – удобный инструмент для изучения автономного полета БПЛА без знания языков программирования.
Для безопасной эксплуатации рекомендуется реализовывать автономный полет посредством двух эксплуатантов:
первый – оператор дрона, который переключает БВС в режим ручного управления в случае нештатной ситуации (ошибки программирования, сбой оборудования, недостаточность пространства для выполнения маневра);
второй – оператор персонального компьютера, который запускает программу и следит за ее выполнением.
Рекомендуется использовать задержку после вызова функции взлета, так как дрону необходимо время, чтобы набрать высоту.
Рекомендуется сигнализировать DXL модулем “Трехцветный светодиод” о выполнении какого-либо действия, чтобы иметь визуальный контроль исполнения кода.
Перед взлетом рекомендуется обнулять скорости:
При использовании в программе методов программирования, которые используют одометрию, рекомендуется обнулять одометрию в точках, относительно которых происходит позиционирование (обычно это точка взлета).
3.2 Алгоритм автономного запуска БПЛА через графический интерфейс
Установите дрон в защищенном кубе;
Включите пульт управления;
Подключите АКБ так, чтобы дрон не повернулся. Это важно, так как в момент включения инициализируется система координат дрона и направление осей;
Для программирования в интерфейсе Blockly подключитесь к Графическому интерфейсу DroneCam (см.Глава 2).
Затем нажмите на кнопку настройки в левом верхнем углу, а затем Программирование Блокли;
Рисунок 3.1. Переход в окно “Программирование Блокли”
Запрограммируйте автономный полет или загрузите файл;
Переведите дрон в режим ARM, моторы активируются;
Включите режимы AltHold и PosHold;
Переключите тумблер, закрепленный за 8 каналом в нижнее крайнее положение, это переключит квадрокоптер на управление с TrackingCam3 и введет его в режим автономного полета, управление с пульта будет недоступно;
Запустите скрипт через графический интерфейс;
ВНИМАНИЕ!
При возникновении нештатной ситуации рекомендуется перевести дрон в ручное управление, переведя тумблер, закрепленный за 8 каналом в верхнее крайнее положении. Удерживайте на пульте управления уровень газа на середине, чтобы при переключении дрон не упал.
После выполнения скрипта верните дрон в ручное управление, переведя тумблер 8 канала в верхнее крайнее положение. Управление через пульт снова станет доступно. Удерживайте на пульте уровень газа посередине, чтобы дрон при переключении не упал;
После завершения посадки переведите дрон в режим DISARM;
Отключите АКБ;
Выключите пульт.
3.3 Примеры использования Blockly
Ниже приведено несколько примеров. Скачивание и видео работы кейсов доступны по ссылке: https://github.com/AppliedRobotics/ARA-EDU/tree/main/drone-control-api/examples/Кейсы Блокли
Перечень примеров:
Takeoff_and_landing.json – пример взлета и посадки.
Takeoff_and_rotate.json – пример взлета, поворота и посадки.
Takeoff_and_fly_forward_odom.json – пример взлета, пролета вперед и посадки.
Square_odom.json – пример полета по квадрату с помощь одометрии.
ArucoSearch.json – пример поиска ArUco-маркера.
TrackingAruco_P_Reg.json – пример слежения за ArUco-маркером с помощью П-регулятора.
TrackingAruco_R_Reg.json – пример слежения за ArUco-маркером с помощью релейного регулятора.
Boarding_aruco_PD.json – пример посадки на ArUco-маркер с помощью П, Д-регулятора.
WallFollow.json – пример полета вдоль стены при помощи ультразвукового дальномера с релейным регулятором.
Blob_Follow_PID_Reg.json – пример слежения за цветным объектом с помощью ПИД - регулятора.
Алгоритм запуска примера:
Скачайте файл с примером;
Подключитесь к Графическому интерфейсу;
Перейдите в меню и выберете вкладку “Программирование Блокли”;
Для загрузки примера выберите “Открыть рабочее пространство”;
Рисунок 3.2. Переход в окно рабочего пространства для загрузки файла с примером
Выберете скачанный ранее файл;
Переведите дрон в автономный режим, запустите скрипт, нажав на кнопку Запуска программы.
3.4 Простые примеры
К простым примерам относятся следующие примеры: Взлет и посадка, Взлет и поворот, Полет вперед, Полет по квадрату, Полет вдоль стены.
3.4.1 Системы координат
1. Скоростная система координат
Используется для методов “Задать скорость XY” (setVelXY) и “Задать скорость XY и рыскание” (setVelXYYaw).
Оси координат задаются следующим образом: ось Х вперед, ось Y вправо, ось Yaw по часовой стрелке. Соответственно, если задать скорость X положительной, то дрон полетит прямо, если отрицательной полетит назад. Аналогично по оси Y и углу поворота Yaw.
Рисунок 3.3. Направление осей координат скоростной СК
2. Одометрическая система координат
Используется в методах “Лететь в XY дрон” (gotoXYdrone) и “Лететь в XY одометрию” (gotoXYodom).
Ось X направлена вперед, ось Y направлена влево. Необходимо учитывать, что в случае вызова “Лететь в XY дрон” данная система координат используется независимо от текущего угла поворота дрона, так как привязана к дрону. Если вызывать метод “Лететь в XY одометрию” без предварительного обнуления одометрии, то нулем (началом) координат будет являться точка включения дрона.
Рисунок 3.4. Направление осей координат одометрической СК
3.4.2 Описание простых примеров
Взлет и посадка (Takeoff_and_landing.json)
Дрон взлетит, а спустя 7000 миллисекунд (7 секунд) приземлится обратно.
Поворот (Takeoff_and_rotate.json)
Дрон взлетит, а через 7 секунд повернется на 1.57 радиан (90 градусов), поворот будет осуществляться по часовой стрелке. Через 5 секунд после поворота дрон начинает посадку.
Полет вперед (Takeoff_and_fly_forward_odom.json)
В самом начале программы рекомендуется обнулять все скорости, чтобы дрон не полетел в сторону при старте. После этого происходит взлет и спустя 5 секунд дрон начинает лететь прямо на 1.5 метра и совершает посадку.
ВНИМАНИЕ!
При запуске данного примера убедитесь что пролет вперед будет безопасен!
Полет по квадрату (Square_odom.json)
В самом начале программы рекомендуется обнулять все скорости, чтобы дрон не полетел в сторону при старте.
В этой программе дрон совершает пролет по квадрату. Для этого дрону задаются координаты 4 углов квадрата.
ВНИМАНИЕ!
При запуске данного примера убедитесь, что полет по квадрату будет безопасен!
Рисунок 3.5. Траектория полета дрона по квадрату
Полет вдоль стены (WallFollow.json)
Данный пример реализует полет вдоль стены при помощи ультразвукового датчика расстояния и релейного регулятора.
Регулятор – алгоритм, выполняющий задачу поддержания объекта управления в определённом состоянии.
Регулятор вычисляет рассогласование (ошибку) между заданным состоянием и текущим. Он получает информацию о состоянии объекта управления с помощью датчиков и вырабатывает управляющее воздействие для того, чтобы его корректировать.
Релейный регулятор – это регулятор, изменяющий скачком управляющее воздействие на систему при прохождении регулируемой величины через пороговое фиксированное значение.
В функции wall_follow задается постоянная скорость, с которой дрон будет лететь вдоль стены (roll_vel = -0.65). После чего в цикле определяется расстояние между дроном и стеной с помощью ультразвукового датчика, и считается ошибка между заданным расстоянием (distance = 0.5) и текущим.
Затем начинает действовать релейный регулятор: если дрон отлетит от стены больше, чем на 0.5 м (pitch_error > 0), то с помощью регулятора он подлетит к ней с заданной скоростью pitch_vel. Переменная pitch_vel равна значению, которое возвращает функция sign, умноженному на 0.65.
Если дрон подлетит слишком близко к стене (pitch_error < 0), то с помощью регулятора он отлетит от нее на заданную скорость pitch_vel, которая примет отрицательное значение, так как функция sign вернет значение -1.
Если вы хотите добавить в функцию параметры (как в данном примере – функция sign с параметром value), необходимо выполнить следующее:
- Нажмите на шестеренку в блоке функции. Вы увидите настройки функции:
- Введите название параметра и переместите его в блок параметры:
- Создавайте столько параметров, сколько необходимо.
3.5 Примеры с ArUco-маркером
ArUco маркеры — это двоичные квадратные маркеры, состоящие из сетки (4x4, 5x5, 6x6, 7x7), где каждая ячейка имеет значение 0 (черное) или 1 (белое). В центре маркера находится уникальный паттерн, который может быть легко распознан с помощью компьютерного зрения. Каждый маркер окружен черной рамкой и имеет свой уникальный идентификатор (ID), который позволяет различать маркеры между собой.
Рисунок 3.6. Примеры внешнего ArUco-маркеров
Камера захватывает изображение, после чего изображение обрабатывается, преобразуется в оттенки серого, затем происходит фильтрация шума (отбрасываются мелкие детали) и в конце изображение проходит бинаризацию (превращается в черно-белое).
Рисунок 3.7. Изображение до и после обработки
После чего идет поиск замкнутых контуров. Ищутся области с прямоугольными границами (квадратная форма маркера).
Далее извлечение внутреннего паттерна. Алгоритм "нарезает" внутреннюю часть маркера на ячейки и анализирует их, получая двоичный код. Извлеченный двоичный код сравнивается с библиотекой шаблонов (словарем), чтобы найти соответствующий ID:
Рисунок 3.8. Сегментирование ArUco маркера на ячейки и извлечение его двоичного кода
Если код совпадает с одним из шаблонов в словаре, маркер успешно распознан, если же код не совпадает, маркер считается нераспознанным.
Для правильного распознавания ID маркера независимо от его ориентации, определяется угол поворота маркера. Паттерн приводится к стандартной ориентации (например, с верхним левым углом как начальной точкой).
3.5.1 Настройка ArUco-маркеров
Для работы с ArUco маркерами необходимо выполнить настойку.
В боковом меню Графического интерфейса доступен раздел “Настройки”.
Рисунок 3.9. Окно “ Настройки”
Активируйте “Обнаружение маркеров ArUco”.
В данном меню можно выбрать используемый словарь ArUco маркеров 4х4, 5x5, 6x6, 7x7, после выбора нажмите «Применить выбранный словарь ArUco». Максимальный ID используемого маркера – 50.
Также можно настроить размер стороны маркера в метрах. Если вы хотите сгенерировать маркеры, нажмите кнопку “Создать ArUco-маркер”, после чего откроется бесплатный сайт по генерации маркеров. Для применения настроек необходимо нажать кнопку “Применить размер маркера”.
3.5.2 Система координат ArUco-маркеров
Система координат подразумевает наличие двух способов получения информации о видимых маркерах:
- Получение позиции ArUco-маркеров относительно дрона (для каждого из ArUco маркеров, getArucos).
Рисунок 3.10. Направление осей координат СК ArUco маркеров относительно дрона
Данный метод возвращает позиции задетектированных маркеров относительно камеры. Координаты маркеров измеряются относительно системы координат, представленной на рисунке. Таким образом, если камера направлена вперед, для стабилизации дрона относительно оси roll необходимо использовать координату Х, для оси pitch координату Z, а для высоты – координату Y. Если камера направлена в пол, то система координат поворачивается на 90 градусов относительно оси X по часовой стрелке, тогда ось Х направлена вправо, ось Y – назад, ось Z – вниз.
- Получение позиции дрона относительно ArUco-маркеров (для каждой позиции относительно ArUco-маркера, getCameraPoseAruco).
Рисунок 3.11. Направление осей координат СК для получения позиции дрона относительно ArUco маркеров
Данный метод возвращает позиции камеры относительно задетектированных маркеров. Координаты камеры измеряются относительно системы координат, центр которой находится в центре задетектированного маркера. Для выравнивания относительно маркера используются ось Z для pitch, ось X для roll, ось Y для высоты. Если камера направлена в пол, то система поворачивается на 90 градусов вокруг оси X по часовой стрелке.
3.5.3 Описание примеров с ArUco маркером
Поиск ArUco-маркера (ArucoSearch.json)
В самом начале вызывается функция main.
В функции main происходит взлет, вызывается функция search_aruco (поиск ArUco), после чего дрон совершает посадку.
Функция search_aruco принимает скорость, с которой дрон будет вращаться и искать ArUco-маркер. После вызова данной функции светодиод загорается красным, после чего дрон начинает вращаться с заданной скоростью и ищет ArUco маркер (цикл повторять, пока или while). Когда маркер будет задетектирован выполнится блок “для каждого из ArUco маркеров” и светодиод загорится зеленым, дрон остановится и цикл завершится.
Слежение за ArUco маркером при помощи релейного регулятора (TrackingAruco_R_Reg.json)
Функция tracking_aruco рассчитывает ошибки по расстоянию между дроном и маркером по трем осям x, y и z.
Если разница по высоте больше, чем допустимая (accuracy_height = 0.15), то вызывается функция height_regulation для регулировки высоты.
Данный пример реализует выравнивание на маркер при помощи самого простого регулятора - релейного. Для каждой из осей корректировки рассчитывается ошибка (в метрах), после чего, если ошибка выше заданной, то позиция дрона корректируется при помощи управления по скорости.
Слежение за ArUco маркером при помощи пропорционального регулятора (TrackingAruco_P_Reg.json)
В данном примере вместо релейного используется пропорциональный регулятор.
Функция aruco_regulation для выравнивания дрона относительно маркера, принимает ошибки по расстоянию и умножает их на коэффициенты, после чего полученные значения сравниваются с пороговыми значениями (threshold) в функции constrain, это делается для ограничения максимальной скорости.
Если дрон потеряет маркер из виду, то будет вызываться функция aruco_regulation без вращения, для того, чтобы дрон вернулся в позицию, где был виден маркер последний раз.
3.6 Примеры с цветными блобами
3.6.1 Настройка цветовых блобов (blobs)
Цветовые блобы детектируются в формате HSV (см. Приложение №1), подробнее про алгоритм детекции можно узнать в методическом пособии Учебное пособие «Модуль технического зрения TrackingCam v3».
В боковом меню Графического интерфейса доступен раздел “Настройки”. Активируйте “Обнаружение Блобов”.
Рисунок 3.12. Пример обнаружения ArUco маркера
Для настройки доступно 4 цветовых блоба, по каждому доступна настройка минимальных и максимальных H (Hue), S (Saturation), V (Value), при которых будет распознаваться блоб. Более подробно о цветовой модели HSV вы можете прочитать в Приложении 1.
Рисунок 3.13. Пример настройки цветового блоба
Диапазон измерения компонентов блоба:
H: 0 - 255
S: 0 - 255
V: 0 - 255
Для удобства настройки доступна визуализация цветовой маски, находящаяся справа от изображения. Стоит учитывать, что из-за задержки при выборе другого блоба маска изменится через 2-3 сек. В программировании (Python API и блокли) доступна информация об ID (номере Блоба), координатах его центра в системе координат изображения и его площади.
В Приложении 2 вы можете найти пример настройки цветового блоба.
3.6.2 Система координат цветовых Блобов
Для детектируемых цветовых блобов используется следующая система координат (для каждого блоба из массива блобов, getBlobs):
Рисунок 3.14. Направление осей системы координат камеры
Блобы детектируются относительно пиксельного центра координат камеры. Начало системы координат камеры расположено в левом верхнем углу. В соответствии с этим ось Х направлена вправо, а ось Y направлена вниз. Также в данном случае координаты объектов возвращаются не в метрическом, а в пиксельном формате (максимум 320 пикселей по Х и 240 пикселей по Y). Таким образом, чтобы найти координаты объекта относительно оптического центра, необходимо вычесть из координат по оси X 160 (320/2) и из координат по оси Y 120 (240/2).
3.6.3 Примеры Blockly программирования с цветными блобами
Слежение за цветным объектом с использованием пропорционального регулятора (Blob_Follow_P_Reg.json)
При запуске main программа обнуляет скорость и рыскание, затем квадрокоптер выполняет взлет. После этого активируется основная функция слежения за объектом. Дрон в цикле анализирует найденный blob, вычисляя координаты его центра в изображении и размеры. Эти параметры сравниваются с центром изображения и целевой площадью **target_area **объекта, чтобы понять, насколько текущая позиция дрона отклоняется от желаемой.
Если ошибка по высоте превышает допустимый порог (accuracy_height), вызывается функция height_regulation, которая корректирует текущую высоту дрона, пропорционально ошибке. В функции regulation выполняется расчет скоростей для регулировки положения дрона относительно центра блоба и расстояния до него.
После расчета управляющего воздействия полученное значение проверяется в функции constrain, чтобы в случае ошибки более 1 (threshold) управляющее воздействие снизилось до 1. Таким образом в случае большого расхождения между желаемым и действительным положением дрон не будет реагировать слишком резко.
Слежение за цветным объектом с использованием ПИД-регулятора(Blob_Follow_PID_Reg.json)
init_PID – функции для инициализации PID регуляторов (pitch и roll).
В функции blob_follow рассчитывается центр камеры в пикселях (камера имеет разрешение 360 на 240), после чего задается целевая площадь объекта слежения в пикселях (target_area = 15000), чем ближе мы к объекту, тем больше площадь, чем дальше, тем меньше площадь.
Затем задается предельная площадь в пикселях (blob_threshold), будут отслеживаться объекты, у которых площадь больше, чем данное значение.
Если мы находим объект заданного цвета, то считаем его площадь для определения ошибки по pitch, а также считаем ошибку по roll и высоте между центром объекта и центром камеры. После чего вызывается функция regulation.
В функции regulation рассчитываются скорости для выравнивания с помощью функций PID_PITCH и PID_ROLL, которые реализуют работу ПИД - регулятора. В функции constrain происходит ограничение значения ошибки, чтобы уменьшить резкость дрона при значении ошибки более 1 (threshold).
3.7 Советы по использованию алгоритмов технического зрения
Стоит учитывать, что включение двух алгоритмов одновременно (и детектирование маркеров, и детектирование цветовых блобов) могут ухудшить производительность системы, рекомендуется включать только один алгоритм детекции.
В случае детектирования ArUco маркеров, указанный размер маркера критично влияет на точность определения позиции БПЛА.
Не пренебрегайте настройкой фокуса камеры. Четкое изображение обеспечит более быстрое и правильное распознавание и позволит избежать непредвиденных ситуаций и лишних ошибок в выполнении кода.