Глава 4. Python API
4.1. Установка Python на Windows
На сайте https://www.python.org/ скачиваем версию Python не ниже 3.10, не выше 3.13, пролистав главную страницу ниже и нажав на кнопку «Downloads» напротив необходимой версии.

Скачивание Python с официального сайта
Открывшееся окно пролистываем ниже и нажимаем на ссылку установщика Python, соответствующего характеристикам вашего компьютера.

Установщики Python для Windows
Запускаем установщик и ставим галочку «Add python.exe to PATH», далее нажимаем «Install Now».

Запуск установщика Python
Отключаем ограничение длины пути.

Отключение ограничения длины пути после установки
Для более удобного программирования можно установить различные интегрированные среды разработки для различных программ (Visual Studio Code, PyCharm и т.д.).
Чтобы установить Visual Studio Code:
- Переходим на сайт https://code.visualstudio.com/ и нажимаем «Downloads for Windows»;

Скачивание Visual Studio Code с официального сайта
- Запускаем установщик;

Выбор дополнительных задач при установке VS Code
- После установки запускаем приложение, заходим в расширения, вводим Python и устанавливаем его;

Установка расширения Python в VS Code
- После установки переходим в проводник и создаем новый Python файл, после чего можно начинать программировать.

Стартовый экран VS Code
4.2. Установка Python на Linux
- Для начала необходимо обновить список пакетов. Для этого открываем терминал, пишем команду и вводим пароль:
sh
sudo apt update![]()
- В большинстве случаев Python уже установлен в Linux. Чтобы установить последнюю версию Python, выполняем:
sh
sudo apt install python3![]()
- Устанавливаем pip.
sh
pip – это пакетный менеджер для Python. Его установка производится с помощью команды:sh
sudo apt install python3-pip![]()
- Устанавливаем venv.
Проверяем версию python командой:
sh
python3 --version![]()
Затем в соответствии с версией устанавливаем venv командой:
sh
sudo apt install python3.<версия>-venv![]()
Можно также установить Visual Studio Code. Для этого переходим к списку установленных приложений (находится в левом нижнем углу) и находим Ubuntu Software.

Переход в Ubuntu Software
Теперь необходимо найти VS Code. Нажимаем кнопку «Поиск» и вводим «vscode» в строке поиска. Нажимаем «code» в списке, чтобы открыть страницу приложения.

Страница поиска приложения VS Code
Нажимаем кнопку «Установить» и при появлении запроса вводим пароль, чтобы начать установку.

Установка VS Code в Ubuntu Software
Теперь Visual Studio Code должен появиться в списке ваших установленных приложений. Запускаем его, заходим в расширения, ищем Python и устанавливаем его.

Установка расширения Python в VS Code
После установки переходим в проводник и создаем новый Python файл, после чего можно начинать программировать.

Переход для создания нового Python файла
4.3. Установка Python API и библиотек
- Для получения API необходимо перейти по ссылке и скачать пакет «drone_control_api». Пакет возможно установить с основным репозиторием «ARA-EDU»:
https://github.com/AppliedRobotics/ARA-EDU/tree/main/drone-control-api
После скачивания разархивируем архив.
Открываем папку с API в установленном IDE. В случае, если работа производится в VS Code нажимаем File Open Folder и выбираем папку «drone-control-api» в открывшемся окне проводника/файлового менеджера:

Импорт папки в рабочую область VS Code

Открытая папка «drone-control-api» в VS Code
Для Linux:
- Следом создаем виртуальное окружение. Для этого в терминале среды программирования вводим следующую команду:
sh
python3 -m venv.venv
После ввода команды в папке «drone-control-api» появится папка «.venv». В нижней части экрана может появиться сообщение, изображенное на иллюстрации В случае его отображения нажимаем «Yes».

Директория «.venv» виртуального окружения в проекте

Уведомление о выборе виртуального окружения в VSCode
- Активируем созданное виртуальное окружение.
sh
source.venv/bin/activate
Если все было сделано правильно, то в командной строке появится префикс (.venv) с окружением
- Далее необходимо установить библиотеки.
sh
python3 -m pip install -e.
Установятся библиотека «Drone» и необходимые для нее зависимости, а именно:
websockets
opencv-python
numpy
Эти зависимости автоматически устанавливаются вместе с пакетом. Однако их также можно установить вручную, используя файл «requirements.txt».
Для этого в терминале выполняем команды:
pip3 install -r requirements.txt

Для Windows:
- Следом создаем виртуальное окружение. Для этого в терминале среды программирования вводим следующую команду:
sh
python -m venv.venv
После ввода команды в папке «drone-control-api» появится папка «.venv». В нижней части экрана может появиться сообщение, изображенное на иллюстрации В случае его отображения нажимаем «Yes».

Директория «.venv» виртуального окружения в проекте

Уведомление о выборе виртуального окружения в VSCode
- Активируем созданное виртуальное окружение.
..venv\Scripts\activate

Если все было сделано правильно, то в командной строке появится префикс (.venv) с окружением
- Далее необходимо установить библиотеки.
sh
python -m pip install -e.
Установятся библиотека «Drone» и необходимые для нее зависимости, а именно:
websockets
opencv-python
numpy
Эти зависимости автоматически устанавливаются вместе с пакетом. Однако их также можно установить вручную, используя файл «requirements.txt».
Для этого в терминале выполняем команду:
sh
pip install -r requirements.txt
4.4. API для взаимодействия с дроном
API (Application Programming Interface) — это интерфейс программирования приложений. Он предоставляет набор методов и правил, с помощью которых одна программа может взаимодействовать с другой. API позволяет разработчикам использовать функциональность, которую предоставляет другая программа, библиотека, сервис или устройство, без необходимости вникать в ее внутреннюю реализацию.
Принцип работы API:
Запрос от клиента. Программа (клиент) отправляет запрос к API, указывая, какую информацию или действие она хочет получить/выполнить.
Обработка запроса сервером/системой. Сторона, предоставляющая API, обрабатывает запрос. Она проверяет данные, производит вычисления или взаимодействует с другими системами.
Ответ от API. После обработки запроса API возвращает ответ.
Рассмотрим функционал API для взаимодействия с дроном на языке программирования Python:
| connect (ip, port) — Вызов метода connect устанавливает соединение с дроном. ip – ip адрес, через который устанавливается соединение, в формате str. Допустимо использовать имя хоста, а именно «trackingcam3». port – порт, через который устанавливается соединение, в формате str, порт – 1233. Возвращает True или False в зависимости от того удалось ли установить соединение или нет. |
|---|
| takeoff () — Взлет дрона на на высоту 1 метр. Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| boarding () — Посадка дрона. Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| setZeroOdomOpticflow () — Выставляет точку в пространстве, относительно которой будет считаться одометрия (по умолчанию это точка взлета). Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| getOdomOpticflow() — Получение данных с датчика Opticflow (расстояние, на которое переместился дрон по координатам Xx и Yy относительно взлета или с точки, которая была установлена с помощью метода setZeroOdomOpticflow() ). Возвращает список из 3 значений в формате: [odom_x, odom_y, odom_z]. Значение odom приходит в формате float и измеряется в метрах. |
| getLidar() — Получение данных с лидара. Возвращает список из 360 значений [dist_1, dist_2, dist_3, …], где каждое значение, соответствует расстоянию на соответствующем угле. В случае если лидар установлен! Значение dist приходит в формате float и измеряется в метрах. |
| getRPY() — Получение углов Roll, Pitch, Yaw (эти углы определяют положение дрона в пространстве, то есть определяет на какой угол и по какой оси дрон был повернут относительно начальной точки). Возвращает список из 3 значений в формате: [roll, pitch, yaw]. Значение углов приходит в формате float и измеряется в радианах. |
| getHeightBarometer() — Получение высоты дрона с помощью Барометра. Возвращает значение высоты в формате float. |
| getHeightRange() — Получение высоты дрона с помощью лазерного дальномера. Возвращает значение высоты в формате float. |
| getLight() — Получение освещенности, с помощью датчика света. Возвращает значение уровня освещенности в формате float. |
| getUltrasonic() — Получение расстояния до препятствия с помощью ультразвукового датчика расстояния. Возвращает список в формате: [{"id": 0, "value": value_0}, {"id": 1, "value": value_1}, {"id": 2, "value": value_2}, {"id": 3, "value": value_3}] Значение value приходит в формате float и измеряется в сантиметрах. |
| getBlobs() — Получение параметров цветовых пятен. Возвращает список в формате: [{"id": "0", "center": {"x": center_x0, "y": center_y0}, "size": {"x": size_x0, "y": size_y0}}, {"id": "1", "center": {"x": center_x1, "y": center_y1}, "size": {"x": size_x1, "y": size_y1}}, {"id": "2", "center": {"x": center_x2, "y": center_y2}, "size": {"x": size_x2, "y": size_y2}}, {"id": "3", "center": {"x": center_x3, "y": center_y3}, "size": {"x": size_x3, "y": size_y3}}] |
| getImage() — Получение изображения с камеры. Возвращает изображение с камеры в формате numpy массива, совместимого с opencv. |
| getUtilsData() — Получение информации о параметрах дрона. Возвращает список в формате: [['ARMING_DISABLED_NAVIGATION_UNSAFE'], ['ACC', 'BARO', 'RANGEFINDER', 'OPFLOW', 'TEMP'], ['ANGLE_MODE', 'NAV_ALTHOLD_MODE', 'NAV_FW_AUTOLAND'], 49, [1500, 1500, 1500, 885, 1675, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500], [0.0, 0.0, 42949672.0], [-0.008726646259971651, -0.03665191262740524, -2.35619 44901923453], False, True] |
| getArucos() — Получение положения и ориентации ArUco-маркеров относительно камеры дрона и ID маркеров. Если маркер был обнаружен, то возвращаются координаты ArUco-маркеров относительно системы координат камеры в формате: [{'id': aruco_id, 'pose': {'position': {'x': aruco_pos_x, 'y': aruco_pos_y, 'z': aruco_pos_z}, 'orientation': {'x': aruco_orient_x, 'y': aruco_orient_y, 'z': aruco_orient_z}}}]. Параметры position измеряются в метрах, а orientation – в радианах. Если маркеры не обнаружены, то возвращается ответ «no markers». |
| getCameraPoseAruco() — Получение позиции и ориентации камеры дрона относительно ArUco-маркеров и ID маркеров. Возвращаются координаты камеры относительно систем координат ArUco-маркеров в формате: [{'id': aruco_id, 'pose': {'position': {'x': camera_pos_x, 'y': camera_pos_y, 'z': camera_pos_z}, 'orientation': {'x': camera_orient_x, 'y': camera_orient_y, 'z': camera_orient_z}}}] Параметры position измеряются в метрах, а orientation – в радианах. Если маркеры не обнаружены, то выдается ответ «no markers». |
| getArm() — Получение информации о состоянии дрона. Возвращает пустой список [], если дрон еще не был переведен в режим Arm после включения. Возвращает список ['ARMED', 'WAS_EVER_ARMED'], если в момент вызова метода, дрон переведен в режим Arm. Возвращает список ['WAS_EVER_ARMED'], если дрон не переведен в режим Arm, но после своего включения уже был когда-либо переведен в режим Arm. |
| setYaw(yaw) — Установка угла (yaw), на который дрону необходимо повернуться (угол задается относительно стартовой позиции дрона, а не от текущей. Если аргумент функции положительный, то дрон будет вращаться по часовой стрелке). yaw – целевой угол в радианах в формате float. Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| setVelXY(vel_x, vel_y) — Установка скорости по осям X и Y соответственно. vel_x – целевая скорость по оси X в формате float vel_y – целевая скорость по оси Y в формате float Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| setVelXYYaw(vel_x, vel_y, vel_yaw) — Установка скорости по осям X, Y и Yaw соответственно. vel_x – целевая скорость по оси X в формате float vel_y – целевая скорость по оси Y в формате float vel_yaw – целевая скорость по yaw в формате float (положительное число – вращение по часовой, отрицательное число – вращение против часовой стрелки) Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| gotoXYdrone(drone_x, drone_y) — Установка целевой точки для дрона относительно его текущего положения. drone_x – целевое значение положения относительно текущей координаты дрона по оси X в формате float drone_y – целевое значение положения относительно текущей координаты дрона по оси Y в формате float Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| gotoXYodom(odom_x, odom_y) — Установка целевой точки для дрона относительно его 0 по одометрии. odom_x – целевое значение положения относительно начальной точки одометрии по оси X в формате float odom_y – целевое значение положения относительно начальной точки одометрии по оси Y в формате float Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| setHeight(target_height) — Установка высоты дрона в метрах относительно земли. target_height – целевая высота для дрона в формате float. Высота задается только в границах от [0.05, 2.0] в метрах, иначе сервис выдаст ошибку и не выполнит изменение высоты. Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| setDiod(r, g, b) — Управление светодиодом. r – компонента, отвечающая за красный цвет; g – компонента, отвечающая за зеленый цвет; b – компонента, отвечающая за синий цвет; Каждая компонента принимает значение от 0 до 255 в формате int. Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| setBeeper(power, freq) — Управление пьезопищалкой. power – мощность звука. freq – частота звука. Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| setMagnet(val) — Управление магнитом. val – состояние магнита, принимает True или False (включение и выключение магнита соответственно) Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
| setServoAngle(angle) – управление сервоприводом. angle – угол поворота в радианах от 0 до 3.14, что соответствует повороту от 0 до 180 градусам. Возвращает «OK», если метод был вызван, и был получен ответ сервиса. Конфигурация дрона с сервоприводом является нестандартной и разработана специально для соревновательной деятельности. |
| setShoot(time) – совершение выстрела из игрушечного пистолета. time – время в секундах, на которое будет открыт огонь. Для совершения одного выстрела требуется от 0.1 до 0.2 секунд, у каждого пистолета это время индивидуально. Возвращает «OK», если метод был вызван, и был получен ответ сервиса. Конфигурация дрона с игрушечным пистолетом является нестандартной и разработана специально для соревновательной деятельности. |
| disconnect() — Метод, закрывающий соединение с сервером. Возвращает «OK», если метод был вызван, и был получен ответ сервиса. |
4.5. Ознакомление с методами API для взаимодействия с дроном
Рассмотрим программу example.py для взаимодействия с дроном. Данная программа позволит ознакомиться с методами API для взаимодействия с дроном.
python
from drone_control_api import Drone
from drone_control_api.pid import PID
import datetime
import cv2
ip = "10.42.0.1"
port = "1233"
last_time = None
def OnMes0(mes):
global last_time
current_time = datetime.datetime.now()
if last_time is not None:
delta_time = (current_time - last_time).total_seconds()
else:
delta_time = None
last_time = current_time
if delta_time is not None:
log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: {delta_time:.2f} seconds\n"
else:
log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: N/A\n"
def OnMesImage(img):
print(f"img = {img}\n")
cv2.imwrite('img', img)
client = Drone()
client.AddOnMessangeUtils(OnMes0)
client.AddOnMessangeImage(OnMesImage)
print("connected?", client.connect(ip, port), "\n")
# print("takeoff?", client.takeoff(), "\n")
# print("boarding?", client.boarding(), "\n")
# print("setZeroOdomOpticflow?", client.setZeroOdomOpticflow(), "\n")
# print("getOdomOpticflow?", client.getOdomOpticflow(), "\n")
# print("getLidar?", client.getLidar(), "\n")
# print("getUltrasonic?", client.getUltrasonic(), "\n")
# print("getRPY?", client.getRPY(), "\n")
# print("getHeightBarometer?", client.getHeightBarometer(), "\n")
# print("getHeightRange?", client.getHeightRange(), "\n")
# print("getArm?", client.getArm(), "\n")
# print("setYaw?", client.setYaw(0), "\n")
# print("setVelXY?", client.setVelXY(0, 0), "\n")
# print("setVelXYYaw?", client.setVelXYYaw(0, 0, 0), "\n")
# print("gotoXYdrone?", client.gotoXYdrone(0, 0), "\n")
# print("gotoXYodom?", client.gotoXYodom(0, 0), "\n")
# print("setHeight?", client.setHeight(0), "\n")
# print("getArucos?", client.getArucos(), "\n")
# print("getCameraPoseAruco?", client.getCameraPoseAruco(), "\n")
# print("setDiod?", client.setDiod(0, 0, 0), "\n")
# print("setBeeper?", client.setBeeper(0, 0), "\n")
# print("setMagnet?", client.setMagnet(False), "\n")
# print("setServoAngle?", client.setServoAngle(0.0), "\n")
# print("setShoot?", client.setShoot(0.15), "\n")
# print("getBlobs?", client.getBlobs(), "\n")
# print(f"getImage = {client.getImage()}")
# imgg = client.getImage()
# cv2.imshow("imgg", imgg)
# cv2.waitKey(0)
# utils = client.getUtilsData()
# print(utils)
print("disconnected?", client.disconnect(), "\n")frome drone_control_api import Drone — импортирует библиотеку Drone, которая предоставляет API для работы с дроном. Она содержит методы для управления дроном и получения данных с его датчиков.
import datetime — импортирует библиотеку для работы с датой и временем.
import cv2 — импортирует библиотеку OpenCV, которая широко используется для обработки изображений и компьютерного зрения. OpenCV предоставляет множество функций для работы с изображениями и видео.
В строках 6 и 7 указываются параметры подключения к дрону. IP-адрес, к которому подключается клиент. В данном случае 10.42.0.1 — это IP-адрес TrackingCam v3, и порт, через который идет связь. В данном случае – 1233.
Функция OnMes0 — служит для обработки сообщений.
Она принимает сообщение от дрона (mes), после чего фиксирует момент получения сообщения: current_time = datetime.datetime.now()
Если уже было получено сообщение, то вычисляет интервал времени в секундах: delta_time = (current_time - last_time).total_seconds()
После чего формирует текст для записи логов и сохраняет их в файл messages_log.txt.
В строке 29 создается объект клиента client, который используется для взаимодействия с дроном через API: client = Drone()
Далее вызывается метод из библиотеки Drone, который регистрирует функцию-обработчик сообщений: client.AddOnMessangeUtils(OnMes0).
Со строки 34 перечислены вызовы различных методов API.
4.6 ПИД-регулятор
Для удобства использования в API интегрирован модуль ПИД регулирования, который можно импортировать строкой:
python
from drone_control_api.pid import PIDНиже представлен его исходный код:
python
class PID:
def __init__(self, Kp, Ti, Td):
self.Kp = Kp
self.Td = Td
self.Ti = Ti
self.curr_error = 0
self.prev_error = 0
self.sum_error = 0
self.prev_error_deriv = 0
self.curr_error_deriv = 0
self.control = 0
def update_control(self, current_error, reset_prev=False):
self.prev_error = self.curr_error
self.curr_error = current_error
# Calculating the integral error
self.sum_error = self.sum_error + self.curr_error
# Calculating the derivative error
self.curr_error_deriv = self.curr_error - self.prev_error
# Calculating the PID Control
self.control = self.Kp * self.curr_error + self.Ti * self.sum_error + self.Td * self.curr_error_deriv
def get_control(self):
return self.controlДанный модуль реализует классический PID регулятор с заданными коэффициентами усиления. Его использование можно увидеть в примерах ниже.
4.7 Алгоритм автономного запуска БПЛА при использовании Python API
Подготовительный этап:
- Перед запуском программы открываем среду программирования, импортируем папку с API в рабочую область. На иллюстрации приведен пример импорта в VS Code.


Импорт папки «drone-control-api» в рабочую область IDE
- Виртуальное окружении после импорта проекта активируется самостоятельно – должен появиться префикс «(.venv)» в командной строке. Однако, если этого не произошло, открываем терминал IDE и активируем созданное виртуальное окружение командой:
Для Linux:
sh
source.venv/bin/activate
Для Windows:
..venv\Scripts\activate

- Проверяем, чтобы в запускаемой программе IP-адрес для подключения совпадал с IP-адресом камеры. Если IP не будут совпадать, соединение с дроном не произойдет, и автономный полет не запустится. О том, как узнать IP камеры, можно прочитать в подразделе 1.3.3.

Проверка совпадения IP-адреса камеры в запускаемой программе
Алгоритм автономного запуска БПЛА:
Устанавливаем дрон в защитном кубе.
Подключаем АКБ так, чтобы дрон не повернулся. Это важно, так как в момент включения инициализируется система координат дрона и направление осей.
Подключаемся к Wi-Fi сети, которую раздает (или подключается) TrackingCam v3.
Включаем пульт управления.
Переводим квадрокоптер в режим ARM, моторы активируются;
Включаем режимы AltHold и PosHold.
Переключаем тумблер, закрепленный за 8 каналом в нижнее крайнее положение, это переключит квадрокоптер на управление с TrackingCam v3 и введет его в режим автономного полета, управление с пульта будет недоступно.
Запускаем выполнение кода. Есть два способа запуска выполнения скрипта:
- Используем интерфейс VS Code, нажав на стрелочку в правой верхней части экрана:

Запуск программы через интерфейс VS Code
- В терминале вводим команду:
Для Linux:
sh
python3 <путь до файла со скриптом>
Например, python3 examples/Кейсы\ Python/Взлет\ и\ посадка/Takeoff_and_landing.py. Когда в названии папок есть пробелы, в Linux ставится обратный слеш перед ними;
Для Windows:
sh
python <путь до файла со скриптом>
Например, python '.\examples\Кейсы Python\Взлет и посадка\Takeoff_and_landing.py';
Нажимаем «Enter», дрон начнет выполнение кода.
- После выполнения скрипта:
| Если квадрокоптер выполнил посадку в автономном режиме | – опускаем стик Throttle вниз; |
|---|---|
| – переводим тумблер 8 канала в верхнее крайнее положение, вернув дрон в ручное управление; | |
| – переходим к пункту 10 | |
| Если после завершения скрипта квадрокоптер находится в воздухе | – удерживаем на пульте стик Throttle посередине, чтобы при переходе в ручной режим квадрокоптер не упал. |
| – переводим тумблер 8 канала в верхнее крайнее положение, управление с пульта снова доступно. | |
| – выполняем посадку в ручном режиме, осторожно опуская стик Throttle. |
После завершения посадки переводим дрон в режим DISARM, моторы перестанут вращаться.
Выключаем режимы AltHold и PosHold, переведя тумблер, отвечающий за их включение в верхнее крайнее положение.
Отключаем АКБ от дрона.
Выключаем пульт.
4.8. Примеры программ с использованием API
В репозитории представлены примеры использования Python API, которые можно найти в папке examples/Кейсы Python. Системы координат в Python API такие же, как описаны в примерах Blockly (подразделы 3.4.1, 3.5.2 и 3.6.2). Также информация об используемых системах координат находится в Приложении 1.
Ниже приведен список доступных примеров. Все перечисленные примеры находятся в папке /drone-control-api/examples/Кейсы Python.
| Название примера | Название папки | Описание |
|---|---|---|
| Простые примеры | ||
| Takeoff_and_landing.py | Взлет и посадка | Пример взлета и посадки |
| Takeoff_and_rotate.py | Взлет и поворот | Пример взлета, поворота и посадки |
| Takeoff_and_fly_forward _odom.py | Пролет вперед | Пример взлета, пролета вперед и посадки |
| Square.py | Полет по квадрату | Пример полета по квадрату с помощью одометрии |
| ObstacleDetector.py | Детектор препятствий | Пример обнаружения препятствий при помощи ультразвукового дальномера с сигнализацией трехцветным светодиодом о состоянии дрона. Используется ПИД регулятор. |
| Wall_Follower_R_reg.py | Полет вдоль стены (Релейный регулятор) | Пример полета вдоль стены при помощи ультразвукового дальномера c релейным регулятором. |
| Wall_Follower_PID_reg.py | Полет вдоль стены (ПИД-регулятор) | Пример полета вдоль стены при помощи ультразвукового дальномера c ПИД-регулятором. |
| Примеры программ с использованием ArUco-маркеров | ||
| TrackingAruco_P_Reg.json | Слежение за Аруко (П-регулятор) | Пример слежения за ArUco-маркером с помощью П-регулятора |
| TrackingAruco_PID_Reg.py | Слежение за Аруко (ПИД-регулятор) | Пример слежения за ArUco-маркером с помощью ПИД-регулятора |
| ArucoBoarding.py | Посадка на Аруко Маркер (ПИД-регулятор) | Пример выполнения посадки на ArUco-маркер при помощи ПИД регулирования. |
| TwoArucos.py | Облет двух маркеров | Пример полета по двум ArUco-маркерам, наклеенным на стенах полетной зоны. После достижения каждого из маркеров дрон подает сигнал светодиодом. Выравнивание по маркерам происходит при помощи PID-регулятора. |
| Примеры программ с использованием цветового блоба | ||
| Blob_Follow_PID_Reg.py | Слежение за блобом (ПИД-регулятор) | Пример слежения за цветным объектом с помощью ПИД-регулятора. |
| Примеры программ с использованием полезной нагрузки | ||
| Aruco_Sorting_Magnet.py | Сортировка груза по ID Аруко (электромагнит) | Пример использования электромагнита в автономном полете. В зависимости от ID распознанного ArUco-маркера загорится соответствующий ему цвет светодиода и дрон выполнит полет в необходимую точку. |
| Color_Cargo_Drop_Servo.py | Сброс груза по цвету (сервопривод) | Пример демонстрирует работу механизма сброса на основе сервопривода. При обнаружении блоба определенного цвета меняется световая индикация и сервопривод поворачивается на заданный угол для сброса груза соответствующего цвета. |
| Aeroduel_Tir.py | Стрельба по Аруко | Пример выстрелов по ArUco-маркерам с ID 1 и 2. В примере реализовано выравнивание сначала на ArUco-маркер с ID 0, затем поиск и выравнивание по оси yaw на маркеры с ID 1 и 2. |
В программах ниже используется базовый IP-адрес камеры 10.42.0.1. Если вы поменяли IP или камера подключена к сети рабочего пространства, введите IP-адрес своей камеры (см. подраздел 1.3.3).В программах ниже используется базовый IP-адрес камеры 10.42.0.1. Если вы поменяли IP или камера подключена к сети рабочего пространства, введите IP-адрес своей камеры (см. подраздел 1.3.3).
Обратим внимание, что в некоторых примерах используется PID-регулирование. Приведенные в примерах коэффициенты PID-регулятора подобраны с учетом полной сборки дрона и предварительных настроек полетного контроллера, загрузка которых описана выше (см. подраздел 1.1–1.2). Следует учитывать, что роботизированные системы могут иметь небольшие отличия, поэтому коэффициенты PID-регулятора, приведенные в примерах, могут не подойти конкретной сборке.
В этом случае требуется выполнить индивидуальный подбор коэффициентов. Рекомендуется начинать настройку с пропорциональной составляющей P при нулевых значениях I и D, затем подобрать дифференциальную составляющую D, после чего, при необходимости, интегральную составляющую I.
4.8.1. Простые программы на Python
Взлет и посадка (Takeoff_and_landing.py)
Дрон устанавливает соединение. Взлет, задержка 10 секунд, чтобы дрон успел выполнить взлет, посадка, разрыв соединения.
python
from drone_control_api import Drone
import time
ip = "10.42.0.1"
port = "1233"
client = Drone()
print("connected?", client.connect(ip, port), "\n")
print("takeoff?", client.takeoff(), "\n")
time.sleep(10)
print("boarding?", client.boarding(), "\n")
print("disconnected?", client.disconnect(), "\n")Взлет и поворот (Takeoff_and_rotate.py)
Дрон устанавливает соединение. Взлет, задержка на 10 секунд, поворот по часовой стрелке на 90○ относительно начального положения, в котором был включен дрон. Угол поворота указывается в радианах. Последняя команда – разрыв соединения.
python
from drone_control_api import Drone
import time
ip = "10.42.0.1"
port = "1233"
client = Drone()
print("connected?", client.connect(ip, port), "\n")
print("takeoff?", client.takeoff(), "\n")
time.sleep(10)
print("setYaw?", client.setYaw(1.57), "\n") #поворот по часовой стрелке относительно положения, в котором дрон был включен (угол задается в радианах)
print("boarding?", client.boarding(), "\n")
print("disconnected?", client.disconnect(), "\n")Взлет и пролет вперед (Takeoff_and_fly_forward.py)
Дрон устанавливает соединение. Взлет, задержка 10 секунд. Командой client.setZeroOdomOpticflow() ‒ обнуляется одометрия. Далее задается полет вперед по одометрии на 1,5 метра. Затем посадка, разрыв соединения. При запуске данного примера убеждаемся, что пролет вперед будет безопасен!
python
from drone_control_api import Drone
import time
ip = "10.42.0.1"
port = "1233"
client = Drone()
print("connected?", client.connect(ip, port), "\n")
print("takeoff?", client.takeoff(), "\n")
time.sleep(10)
print("setZeroOdomOpticflow?", client.setZeroOdomOpticflow(), "\n")
print("gotoXYdrone?", client.gotoXYdrone(1.5, 0), "\n") #полет вперед по одометрии на 1.5м
#print("gotoXYodom?", client.gotoXYodom(1.5, 0), "\n") #полет вперед относительно текущей позиции дрона
print("boarding?", client.boarding(), "\n")
print("disconnected?", client.disconnect(), "\n")Пролет по квадрату (Square.py)
Дрон устанавливает соединение. Взлет, задержка 10 секунд, обнуление одометрии.
Далее задается полет по одометрии на 1 метр: client.gotoXYodom (1, 0). Метод повторяется 4 раза. Значения координат изменяются в соответствии с необходимым положением – координатой квадрата. Началом координат считается положение, в котором обнулилась одометрия. Далее идет посадка, разрыв соединения.
Строки 22–25 закомментированы и показывают пример полета по квадрату, используя метод gotoXYdrone(), где система координат начинается от текущего положения дрона.
При запуске данного примера убеждаемся, что полет по квадрату будет безопасен!
python
from drone_control_api import Drone
import time
ip = "10.42.0.1"
port = "1233"
client = Drone()
print("connected?", client.connect(ip, port), "\n")
print("takeoff?", client.takeoff(), "\n")
time.sleep(10)
print("setZeroOdomOpticflow?", client.setZeroOdomOpticflow(), "\n")
print("gotoXYodom?", client.gotoXYodom(1, 0), "\n")
print("gotoXYodom?", client.gotoXYodom(1, 1), "\n")
print("gotoXYodom?", client.gotoXYodom(0, 1), "\n")
print("gotoXYodom?", client.gotoXYodom(0, 0), "\n")
# print("gotoXYdrone?", client.gotoXYdrone(1, 0), "\n")
# print("gotoXYdrone?", client.gotoXYdrone(0, 1), "\n")
# print("gotoXYdrone?", client.gotoXYdrone(-1, 0), "\n")
# print("gotoXYdrone?", client.gotoXYdrone(0, -1), "\n")
print("boarding?", client.boarding(), "\n")
print("disconnected?", client.disconnect(), "\n")Обнаружение препятствия (ObstacleDetector.py)
Для обнаружения препятствия используется ультразвуковой датчик расстояния, с помощью него мы будем получать расстояние от дрона до стены. В этом примере также появляется управление светодиодом и пьезопищалкой. Перед запуском выполняем настройку DXL-устройств по инструкции подраздела 1.4.
Программа предназначена для автономного полета дрона вперед до обнаружения препятствия. При обнаружении полет прекращается, включается красный светодиод, подается звуковой сигнал и квадрокоптер выполняет посадку.
python
import time
from drone_control_api import Drone
from drone_control_api import pid
ip = "192.168.1.197"
port = "1233"
# Функция обнаружения препятствий
def obstacle_detector():
distance = 0.7
while True:
distance_to_wall = client.getUltrasonic()[0]['value']
distance_to_wall /= 100
print('dist to wall', distance_to_wall)
# Если расстояние до препятствия больше целевого, продолжаем полет впередif distance_to_wall > distance:
client.setDiod(0, 255, 0)
client.setVelXY(0.9, 0)
client.setBeeper(0, 0)
python
# Если расстояние меньше или равно целевому, сбрасываем скорости и выходим из циклаelse:
client.setDiod(255, 0, 0.0)
client.setBeeper(50, 1000)
client.setVelXY(0, 0)
break
python
client = Drone()
print("connected?", client.connect(ip, port), "\n")
client.setDiod(0,0,0)
print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")
print("takeoff?", client.takeoff(), "\n")
time.sleep(5)
client.setHeight(0.5)
obstacle_detector()
time.sleep(3)
print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")
print("boarding?", client.boarding(), "\n")
time.sleep(3)
client.setDiod(0.0, 0.0, 0.0)
client.setBeeper(0.0, 0.0)В функции obstacle_detector cоздается переменная distance, равная 0.7 метров. Все, что ближе этого расстояния, считается препятствием. Расстояние, измеренное ультразвуковым датчиком, выводится в консоль для удобства. Дрон выполняет взлет на 1 метр, а затем опускается на высоту 0.5 метров client.setHeight (0.5) и летит вперед с постоянной скоростью client.setVelXY(0.9, 0).
Если расстояние между дроном и препятствием больше distance, то светодиод горит зеленым цветом, и дрон продолжает лететь с заданной скоростью. Если же препятствие обнаружено, то загорается красный светодиод, подается звуковой сигнал и дрон останавливается. Цикл завершается и выполняется посадка.
Полет вдоль стены с использованием релейного регулятора (Wall_Follower_R_reg.py)
В данном примере также используется ультразвуковой датчик расстояния, с помощью которого можно получить расстояние от дрона до стены, а также светодиод для сигнализации.
Релейный регулятор не учитывает величину ошибки (разность между текущим и заданным положениями). Управляющее воздействие имеет фиксированное по модулю значение и определяется знаком ошибки, что, как правило, приводит к колебаниям системы вблизи заданного значения.
python
from drone_control_api import Drone
import time
ip = "10.42.0.1"
port = "1233"
def wall_follow():
distance = 0.5
while True:
distance_to_wall = client.getUltrasonic()[0]['value']
distance_to_wall /= 100
print('dist to wall', distance_to_wall)
pitch_error = distance_to_wall - distance
print('pitch_error', pitch_error)
if distance_to_wall > distance:
client.setDiod(0, 255, 0)
else:
client.setDiod(255, 0, 0)
r_regulator(pitch_error)
def r_regulator(pitch_error):
max_speed_roll = -0.65
pitch_speed = sign(pitch_error) * 0.65
client.setVelXY(pitch_speed, max_speed_roll)
def sign(value):
if value >= 0:
value = 1
else:
value = -1
return value
client = Drone()
print("connected?", client.connect(ip, port), "\n")
print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")
print("takeoff?", client.takeoff(), "\n")
time.sleep(5)
wall_follow()Квадрокоптер выполняет взлет на 1 метр, далее вызывается функция wall_follow(). Во время ее выполнения дрон будет лететь влево и выравниваться относительно стены, удерживая определенное расстояние до нее.
Функция wall_follow() запускается в бесконечном цикле, поэтому посадку необходимо выполнять в ручном режиме. Расстояние, измеренное ультразвуковым дальномером, преобразуется из сантиметров в метры и выводится в консоль для удобства. Переменная distance, равная 0.5 метров, является целевым расстоянием до стены. В переменной pitch_error рассчитывается ошибка – разница между текущим и целевым расстоянием. Если расстояние между дроном и стеной больше целевого, загорается зеленый светодиод, иначе – красный. В цикле вызывается функция r_regulator с переданной в нее ошибкой pitch_error.
В функции r_regulator задаются и рассчитываются скорости для движения дрона (max_speed_roll и pitch_speed). В функции sign определяется направление движения, куда в качестве value передается переменная pitch_error. Если ошибка больше или равна 0 (то есть дрон не достиг целевого расстояния или имеет нужное положение), то направление движения дрона будет положительным, и он полетит вперед. Если же pitch_error меньше 0 (то есть дрон находится ближе к стене, чем это нужно), то функция sign вернет значение -1, и дрон полетит назад с заданной скоростью.
Для выполнения посадки переводим дрон в ручной режим, переключив 8 канал в верхнее положение, после чего совершаем посадку вручную. Для завершения программы нажимаем сочетание клавиш Ctrl+C в терминале среды программирования.
Полет вдоль стены с использованием ПИД-регулятора (Wall_Follower_PID_reg.py)
Для полета вдоль стены также используется ультразвуковой датчик расстояния.
python
import time
from drone_control_api import Drone
from drone_control_api.pid import PID
import datetime
ip = "10.42.0.1"
port = "1233"
def WallFollow():
pid_pitch= PID(2.5, 0.0, 0.5)
distance = 0.5
pitch_error = 0.0
response_distance = 1.0
while True:
distance_to_wall = client.getUltrasonic()[0]['value']
distance_to_wall /= 100
print('distance to wall', distance_to_wall)
if distance_to_wall < response_distance:
pitch_error = distance_to_wall - distance
print('pitch_error', pitch_error)
DefaultRegulation(pitch_error, pid_pitch)
else:
client.setVelXY(1.0, 0)
def DefaultRegulation(pitch_error, pid_pitch: PID):
accuracy = 0.1
max_pid = 1.0
max_speed_roll = 0.9
if abs(pitch_error) > accuracy:
pid_pitch.update_control(pitch_error)
PID_PITCH = pid_pitch.get_control()
PID_PITCH = constrain(PID_PITCH, max_pid)
else:
PID_PITCH = 0
print(f"PID Control: PITCH={PID_PITCH}, ROLL={max_speed_roll}")
client.setVelXY(PID_PITCH, max_speed_roll)
def constrain(value, threshold):
if value > threshold:
value = threshold
if value < -threshold:
value = -threshold
return value
client = Drone()
print("connected?", client.connect(ip, port), "\n")
print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")
print("takeoff?", client.takeoff(), "\n")
time.sleep(4)
client.setHeight(0.5)
time.sleep(5)
print("Wall Follow: ", WallFollow(), "\n")Функция WallFollow запускается в бесконечном цикле, где дрон постоянно измеряет расстояние до стены с помощью ультразвукового дальномера. Это расстояние преобразуется из сантиметров в метры. Как только дрон получает данные о расстоянии до стены, он проверяет, насколько близко он к ней. Если расстояние меньше заданного (response_distance = 1.0 метра), это сигнализирует о том, что дрон слишком близко к стене, и ему нужно начинать лететь в сторону и выравниваться относительно стены.
Здесь вступает в действие функция DefaultRegulation. Функция принимает ошибку по pitch, которая определяется как разница между текущим расстоянием до стены и заданным значением distance (0.5 метра). Если ошибка превышает определенный порог точности accuracy (0.1 метр), PID-регулятор обновляет свои управляющие параметры и вычисляет новое управляющее воздействие для дрона. Помимо этого, при вызове функции DefaultRegulation дрону присваивается скорость для полета вдоль стены max_speed_roll.
Если же дрон находится на безопасном расстоянии от стены (больше 1.0 метра), программа просто задает дрону скорость для полета вперед.
Функция constrain следит за тем, чтобы значения управляющих параметров не выходили за пределы допустимых значений (max_pid), обеспечивая безопасность и стабильность управления.
Так как алгоритм полета вдоль стены запускается в цикле без прерывания, посадку необходимо совершить в ручном режиме. Для завершения программы нажимаем сочетание клавиш Ctrl+C в терминале среды программирования.
4.8.2. Примеры программ с использованием ArUco-маркеров
Перед выполнением рассматриваемых ниже примеров проверяем раздел «Настройки» Графического интерфейса. Включаем «Обнаружение маркеров ArUco» и выполняем настройку маркеров: выбираем словарь и размер маркера, соответствующие вашему маркеру.

Проверка настроек ArUco-маркера
Слежение за ArUco-маркером при помощи П-регулятора (TrackingAruco_P_Reg.py)
В данном примере реализуется слежение и выравнивание дрона относительно маркера, а также используется трехцветный светодиод для сигнализации о распознавании маркера.
python
from drone_control_api import Drone
from drone_control_api.pid import PID
import time
ip = "10.42.0.1"
port = "1233"
# Функция поиска маркера
def search_aruco (yaw_vel):
client.setDiod(255, 0, 0)
client.setVelXYYaw(0, 0, yaw_vel)
aruco_state = True
while aruco_state:
errors = client.getArucos()
if errors:
aruco_state = False
client.setDiod(0, 255, 0)
client.setVelXYYaw(0, 0, 0)
# Функция расчета ошибок
def tracking_aruco():
pitch_error = 0
roll_error = 0
yaw_error = 0
distance = 1
accuracy_height = 0.15
while True:
errors = client.getArucos()
if errors:
client.setDiod(0, 255, 0)
distance_to_marker = errors [0]['pose']['position']['z']
pitch_error = distance_to_marker - distance
roll_error = errors[0]['pose']['position']['x']
height_error = errors[0]['pose']['position']['y']
yaw_error = -1 * errors[0]['pose']['orientation']['z']
print(f"Дистанция до маркера: {distance_to_marker}")
if abs(height_error) > accuracy_height:
height_regulation(height_error)
aruco_regulation (pitch_error, roll_error, yaw_error)
else:
client.setDiod(255, 0, 0)
aruco_regulation (pitch_error, roll_error, 0)
print (f"Маркер потерян! Поиск:")
print (f"pitch_error = {pitch_error}, roll_error = {roll_error}")
# Выравнивание по высоте
def height_regulation(height_error):
current_height = client.getHeightRange()
new_height = current_height - height_error
print(f"Новая высота {new_height}")
client.setHeight(new_height)
# Выравнивание относительно маркера
def aruco_regulation(pitch_error, roll_error, yaw_error):
PID_pitch = pitch_error * 1.5
PID_pitch = constrain (PID_pitch, 1)
PID_roll = roll_error * 1.5
PID_roll = constrain (PID_roll, 1)
PID_yaw = constrain (yaw_error, 0.3)
print(f"PID_pitch={PID_pitch}, PID_roll={PID_roll}, PID_yaw={PID_yaw}")
client.setVelXYYaw(PID_pitch, PID_roll, PID_yaw)
# Функция ограничения величины
def constrain (value, threshold):
if value > threshold:
value = threshold
if value < -threshold:
value = -threshold
return value
client = Drone()
# Подключаемся
print("connected?", client.connect(ip, port), "\n")
# Сбрасываем скорости
print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")
# Взлет
print("takeoff?", client.takeoff(), "\n")
time.sleep(7)
# Ищем маркер
search_aruco(0.5)
# Выравнивание относительно маркера
tracking_aruco()Квадрокоптер выполняет взлет на 1 метр, затем вызывается функция search_aruco. В ней происходит вращение дрона вокруг своей оси со скоростью yaw_vel, которая передается с вызовом функции. При обнаружении маркера загорается зеленый светодиод, после чего значения скоростей обнуляются, обеспечивая остановку дрона. Выполнение функции search_aruco завершается.
В функции tracking_aruco происходит расчет ошибок, которые затем передаются в функции для выравнивания относительно маркера (height_ regulation и aruco_regulation). Здесь в переменную errors записывается положение маркера относительно квадрокоптера.
В ошибку по pitch (переменная pitch_error) записывается разница между истинным расстоянием до маркера и желаемым (distance). Расстояние рассчитывается исходя из размера маркера в кадре, поэтому важно ввести правильный размер маркера при настройке. В ошибку по roll (переменная roll_error) записывается положение маркера относительно дрона по оси X, в ошибку по высоте (height_error) – положение маркера относительно дрона по оси Y (см. Приложение 1 «Системы координат ArUco-маркеров»), в ошибку по yaw (yaw_error) – отрицательный поворот маркера.
Переменная accuracyheight хранит пороговое значение. Если ошибка по высоте будет больше него, дрон перейдет к выполнению функции **height regulation** – выравниванию по высоте.
Функция aruco_regulation принимает ошибки по pitch, roll и yaw и рассчитывает скорости по этим осям, достаточные для устранения ошибок. Затем полученные значения сравниваются с threshold в функции constrain для ограничения максимальной скорости и передаются в метод setVelXYYaw.
Так как алгоритм полета вдоль стены запускается в цикле без прерывания, посадку необходимо совершить в ручном режиме. Для завершения программы нажимаем сочетание клавиш Ctrl+C в терминале среды программирования.
Для выполнения посадки необходимо переключится в ручной режим управления. Чтобы завершить программу нажимаем сочетание клавиш Ctrl+C в терминале среды программирования.
Слежение за ArUco-маркером при помощи ПИД-регулятора (TrackingAruco_PID_Reg.py)
В данном примере реализовано выравнивание дрона по ArUco-маркеру при помощи ПИД-регулятора.
python
from drone_control_api import Drone
from drone_control_api.pid import PID
import time
import sys
ip = "10.42.0.1"
port = "1233"
# Функция поиска маркера
def search_aruco (yaw_vel):
client.setDiod(255, 0, 0)
client.setVelXYYaw(0, 0, yaw_vel)
aruco_state = True
while aruco_state:
errors = client.getArucos()
if errors:
client.setDiod(0, 255, 0)
client.setVelXYYaw(0, 0, 0)
aruco_state = False
# Функция расчета ошибок
def tracking_aruco():
pid_pitch = PID(2, 0.0034, 0.47)
pid_roll = PID(2, 0.0034, 0.47)
pid_yaw = PID(1.3, 0.0, 0.55)
pitch_error = 0
roll_error = 0
yaw_error = 0
distance = 1
accuracy_height = 0.15
accuracy_yaw = 0.15
accuracy = 0.1
while True:
errors = client.getArucos()
if errors:
client.setDiod(0, 255, 0)
distance_to_marker = errors [0]['pose']['position']['z']
pitch_error = distance_to_marker - distance
roll_error = errors[0]['pose']['position']['x']
height_error = errors[0]['pose']['position']['y']
yaw_error = -1 * errors[0]['pose']['orientation']['z']
print(f"Дистанция до маркера: {distance_to_marker}")
if abs(height_error) > accuracy_height:
height_regulation(height_error)
aruco_regulation (pitch_error, roll_error, yaw_error, pid_pitch, pid_roll, pid_yaw)
# # проверка выравниванивания относительно маркера по осям
# if abs(pitch_error) <= accuracy and abs(roll_error) <= accuracy and abs(yaw_error) <= accuracy_yaw:
# client.setVelXYYaw(0, 0, 0)
# client.setDiod(0, 0, 255)
# print("Выравнивание произошло")
# break
else:
client.setDiod(255, 0, 0)
aruco_regulation (pitch_error, roll_error, 0, pid_pitch, pid_roll, pid_yaw)
print (f"Маркер потерян! Поиск:")
print (f"pitch_error = {pitch_error}, roll_error = {roll_error}")
# Выравнивание по высоте
def height_regulation(height_error):
current_height = client.getHeightRange()
new_height = current_height - height_error
print(f"Новая высота {new_height}")
client.setHeight(new_height)
# Выравнивание относительно маркера
def aruco_regulation(pitch_error, roll_error, yaw_error, pid_pitch:PID, pid_roll:PID, pid_yaw: PID):
pid_pitch.update_control(pitch_error)
PID_pitch = pid_pitch.get_control()
PID_pitch = constrain (PID_pitch, 1)
pid_roll.update_control(roll_error)
PID_roll = pid_roll.get_control()
PID_roll = constrain (PID_roll, 1)
pid_yaw.update_control(yaw_error)
PID_yaw = pid_yaw.get_control()
PID_yaw = constrain (yaw_error, 0.35)
print(f"PID_pitch={PID_pitch}, PID_roll={PID_roll}, PID_yaw={PID_yaw}")
client.setVelXYYaw(PID_pitch, PID_roll, PID_yaw)
# Функция ограничения величины
def constrain (value, threshold):
if value > threshold:
value = threshold
if value < -threshold:
value = -threshold
return value
client = Drone()
# Подключаемся
print("connected?", client.connect(ip, port), "\n")
# Сбрасываем скорости
print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")
# Взлет
print("takeoff?", client.takeoff(), "\n")
time.sleep(10)
# Ищем маркер
search_aruco(0.5)
# Выравнивание относительно маркера
tracking_aruco()
print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")
print("boarding?", client.boarding(), "\n")
print("disconnected?", client.disconnect(), "\n")Порядок работы данной программы схож с предыдущим примером. Отличие состоит в том, что функция aruco_regulation() принимает не только ошибки по pitch, roll и yaw, но и объекты PID-регуляторов для осей этих осей. Эти регуляторы создаются как экземпляры класса PID внутри функции tracking_aruco() в строках 23–25 и передаются в aruco_regulation() для расчета управляющих воздействий на основе текущих ошибок позиционирования дрона относительно ArUco-маркера.
Далее вызывается метод update_control(), который принимает текущее значение ошибки, рассчитывает управляющее воздействие для ее устранения с учетом заданных коэффициентов PID-регулятора. После этого метод get_control() возвращает результат расчета, который сохраняется в переменные PIDpitch, PID_roll и PID yaw. Затем в функции constrain полученные значения сравниваются с threshold для ограничения максимальной скорости и передаются в метод setVelXYYaw().
Алгоритм слежения за маркером запускается в бесконечном цикле. Посадку осуществляем в ручном режиме.
Обратим внимание на закомментированное условие прерывания бесконечного цикла в функции tracking_aruco() в строках 51–56. Если модули ошибок по pitch, roll и yaw становятся меньше или равны заданным порогам accuracy и accuracy_yaw, скорости обнуляются, загорается синий светодиод, и выполнение функции tracking_aruco() прекращается. После чего дрон выполняет посадку.
Если стоит задача выполнить посадку или другое действие в автономном режиме, необходимо раскомментировать данный блок программы.
Посадка на ArUco-маркер (ArucoBoarding.py)
Перед выполнением данного примера необходимо перекрутить TrackingCam v3 так, чтобы она была направлена вниз. Обратим внимание, что система координат также поворачивается на 90○ (см. Приложение 1). При запуске примера не рекомендуется печатать маркер размером более 20 см.
Посадка на ArUco-маркер производится следующим способом:
Камера, направленная вниз получает данные о положении ArUco-маркера с помощью метода getArucos().
С помощью этих данных производится выравнивание дрона относительно маркера.
После того, как дрон попадает в некоторую область, регламентирующую точность выравнивания, он снижает высоту.
При определенном значении высоты, полученной с помощью метода getHeightRange() дрон осуществляет посадку.
Пример кода для реализации посадки на ArUco маркер представлен ниже:
python
# Библиотеки для работы со временем
import time
from datetime import datetime
# Класс для работы с API дрона
from drone_control_api import Drone
# Импортируем класс PID
from drone_control_api.pid import PID
# Задаем IP и порт для подключения к дрону
ip = "10.42.0.1"
port = "1233"
# Переменная для хранения времени последнего сообщения
last_time = None
# Функция посадки дрона на аруко-маркер
def Aruco_boarding():
# Время начала отсчета
time_begin = datetime.now()
# Инициализируем переменную для текущего времени
time_now = time_begin
# Находим их разность
delta_time = time_now - time_begin
# Инициализируем переменную для текущей высоты
current_height = client.getHeightRange()
# Выводим текущую высоту
print(f"Started Height is: {current_height}")
# Инициализация PID-контроллеров для крена и тангажа
pid_pitch = PID(0.7, 0.0, 0.1)
pid_roll = PID(0.7, 0.0, 0.1)
# Инициализируем переменные ошибок
pitch_error = 0.0
roll_error = 0.0
# Устанавливаем время выполнения, по истечении которого дрон совершит посадку
while delta_time.total_seconds() < 100:
try:
# Обновляем текущее время и рассчитываем время выполнения
time_now = datetime.now()
delta_time = time_now - time_begin
# С помощью сервиса getArucos получаем координаты маркера относительно дрона — это и есть ошибки
errors = client.getArucos()
# Если камера не видит аруко-маркеров
if errors == []: # Потерян маркер
print('I dont see Aruco')
# Вызываем функцию регулировки
ArucoRegulation(pitch_error, roll_error, pid_pitch, pid_roll)
# Продолжаем выполнение цикла
continue
else:
# Если камера видит аруко-маркеры, записываем в переменные ошибки
roll_error = errors[0]['pose']['position']['x']
pitch_error = errors[0]['pose']['position']['y'] - 0.1
# Вызываем функцию регулировки
ArucoRegulation(pitch_error, roll_error, pid_pitch, pid_roll)
# Если ошибки меньше 30 по x и y - снижаем высоту
if abs(pitch_error) < 0.3 and abs(roll_error) < 0.3:
current_height *= 0.75
print("Снижение")
# Посылаем в сервис setHeight новую высоту
client.setHeight(current_height)
print(f"Current Height is: {current_height}")
if current_height < 0.4:
# Останавливаем дрон и завершаем посадку
client.setVelXY(0, 0)
print("Landing")
client.boarding()
break
except KeyboardInterrupt:
# Обрабатываем прерывание программы с клавиатуры
print("KeyboardInterrupt detected, landing the drone...")
break
# Функция регулировки положения дрона относительно аруко-маркера
def ArucoRegulation(pitch, roll, pid_x: PID, pid_y: PID):
print(f"Adjusting position: pitch={pitch}, roll={roll}")
# Обновление контроллеров по осям
pid_x.update_control(pitch)
pid_y.update_control(roll)
PID_PITCH = -pid_x.get_control()
PID_ROLL = pid_y.get_control()
# Ограничение управляющих сигналов
PID_PITCH = constrain(PID_PITCH, 1.1)
PID_ROLL = constrain(PID_ROLL, 1.1)
print(f"PID Control: PITCH={PID_PITCH}, ROLL={PID_ROLL}")
# Устанавливаем скорости для дрона
client.setVelXY(PID_PITCH, PID_ROLL)
return (PID_PITCH, PID_ROLL)
# Функция для ограничения значений
def constrain(value, threshold):
if value > threshold:
value = threshold
if value < -threshold:
value = -threshold
return value
# Функция обработки входящих сообщений
def OnMes0(mes):
global last_time
current_time = datetime.now()
if last_time is not None:
delta_time = (current_time - last_time).total_seconds()
else:
delta_time = None
last_time = current_time
if delta_time is not None:
log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: {delta_time:.2f} seconds\n"
else:
log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: N/A\n"
# Запись сообщения в лог
with open("messages_log.txt", "a") as file:
file.write(log_message)
# Создаем объект клиента для взаимодействия с дроном
client = Drone()
# Добавляем обработчик сообщений
client.AddOnMessangeUtils(OnMes0)
# Подключаемся к дрону и выполняем команды
print("connected?", client.connect(ip, port), "\n")
print("VelCorrect", client.setVelXY(0, 0), "\n")
print("takeoff?", client.takeoff(), "\n")
print("Pose aruco: ", client.getArucos(), "\n")
time.sleep(8)
client.setHeight(1.5)
time.sleep(5)
print("getHeightRange?", client.getHeightRange(), "\n")
print("getOdomOpticflow?", client.getOdomOpticflow(), "\n")
print("Aruco Boarding: ", Aruco_boarding(), "\n")
print("VelCorrect", client.setVelXY(0, 0), "\n")
print("boarding?", client.boarding(), "\n")
print("disconnected?", client.disconnect(), "\n")Выполняется подключение, обнуление скоростей, взлет и получение информации об ArUco-маркерах, которая выводится в консоль. Далее происходит подъем на высоту 1.5 метров и получение информации о текущей высоте и положении дрона относительно точки взлета.
Затем вызывается функция Aruco_boarding(), в которой с помощью метода getArucos() рассчитываются ошибки по осям pitch и roll. При этом из ошибки по pitch вычитается 0,1 м, так как камера смещена примерно на 10 см вперед относительно центра дрона. Поскольку ошибка определяется относительно положения камеры, а не геометрического центра дрона, без этой поправки квадрокоптер при посадке будет оказываться немного позади маркера.
Также в функции Aruco_boarding() реализованы условия и алгоритм посадки дрона на ArUco-маркер. Выполнение функции ограничено по времени и продолжается не более 100 секунд с момента ее запуска.
В ArucoRegulation() отправляются значения ошибок, где с использованием PID-регуляторов рассчитываются управляющие воздействия и устанавливаются скорости для движения дрона к центру ArUco-маркера.
Функция constrain нужна для установления максимальных значений скоростей с выхода ПИД-регулятора (в данном примере значения могут находиться в диапазоне от -1.1 до 1.1).
Обратим внимание, что управляющее воздействие для оси pitch умножается на -1 (строка 86). Это обусловлено ориентацией осей системы координат ArUco-маркеров (см. Приложение 1 «Системы координат ArUco-маркеров»). Когда ArUco-маркер находится позади дрона, центр ArUco-маркера располагается в нижней части кадра.


Положение ArUco-маркера относительно дрона и в кадре камеры
В данном случае ошибки по оси Y будут положительными, однако, чтобы отрегулировать дрон относительно центра маркера, необходимо выполнить полет назад, а, значит, в метод setVelXY() отправить отрицательное значение скорости по оси X (см. Приложение 1 «Скоростная система координат»). Аналогично для обратного случая, когда маркер находится в верхней части кадра.
Как только абсолютные значения ошибок по pitch и roll будут меньше допустимого порога (в примере это 0.3 метра), дрон начнет снижение. Когда дрон достигнет высоты менее 0.4 метров, осуществляется посадка и выполнение функции Aruco_boarding() завершается.
Облет двух ArUco маркеров (TwoArucos.py)
Данный алгоритм реализует пошаговый поиск и стабилизацию дрона относительно двух ArUco-маркеров. Основная логика строится на взаимодействии двух функций: поиска (SearchForArucos) и регулирования (ArucoRegulation).
Используемые маркеры должны иметь уникальные ID. Если на площадке будет установлено два маркера с одинаковым ID, дрон успешно выровняется на первый из них, но второй маркер он проигнорирует.
python
import time
from drone_control_api import Drone
from drone_control_api.pid import PID
import datetime
import math
ip = "10.42.0.1"
port = "1233"
last_time = None
id_markers = []
def OnMes0(mes):
global last_time
current_time = datetime.datetime.now()
if last_time is not None:
delta_time = (current_time - last_time).total_seconds()
else:
delta_time = None
last_time = current_time
if delta_time is not None:
log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: {delta_time:.2f} seconds\n"
else:
log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: N/A\n"
with open("messages_log.txt", "a") as file:
file.write(log_message)
# Функция поиска маркера
def SearchForArucos(vel):
client.setDiod(255.0, 0.0, 0.0)
client.setVelXYYaw(0, 0, vel)
global id_markers
while True:
errors = client.getArucos()
if errors:
for marker in errors:
current_id = marker['id']
if current_id not in id_markers:
client.setVelXYYaw(0, 0, 0)
id_markers.append(current_id)
print(f"Found new marker ID: {current_id}")
return current_id
# Выравнивание относительно маркера
def ArucoRegulation(target_id):
client.setDiod(0.0, 255.0, 0.0)
pid_pitch= PID(1.0, 0.002, 0.2)
pid_roll = PID(1.0, 0.002, 0.2)
pid_yaw = PID(0.6, 0.0, 0.5)
distance = 1.0
pitch_error = 0.0
roll_error = 0.0
height_error = 0.0
yaw_error = 0.0
accuracy = 0.15
while True:
all_markers = client.getArucos()
# Поиск целевого маркера среди всех видимых
target_marker = None
if all_markers:
for m in all_markers:
if m['id'] == target_id:
target_marker = m
break
if target_marker:
yaw_error = target_marker['pose']['orientation']['z']
height_error = target_marker['pose']['position']['y']
roll_error = target_marker['pose']['position']['x']
distance_to_marker = target_marker['pose']['position']['z']
pitch_error = distance_to_marker - distance
print(f"Дистанция до маркера: {distance_to_marker}")
if abs(height_error) > 0.15:
print(f'height_error = {height_error}')
HeightRegulation(height_error)
DefaultRegulation(pitch_error, roll_error, pid_pitch, pid_roll, yaw_error, pid_yaw)
if abs(pitch_error) < 0.15 and abs(roll_error) < 0.15 and abs(yaw_error) < 0.15:
print("Выравнивание произошло успешно")
break
else:
print("Dont see aruco")
DefaultRegulation(pitch_error, roll_error, pid_pitch, pid_roll, 0, pid_yaw)
def DefaultRegulation(pitch_error, roll_error, pid_p: PID, pid_r: PID, yaw_error, pid_yaw: PID):
accuracy = 0.0
max_pid = 0.8
if abs(pitch_error) > accuracy:
pid_p.update_control(pitch_error)
PID_PITCH = pid_p.get_control()
PID_PITCH = constrain(PID_PITCH, max_pid)
else:
PID_PITCH = 0
if abs(roll_error) > accuracy:
pid_r.update_control(roll_error)
PID_ROLL = pid_r.get_control()
PID_ROLL = constrain(PID_ROLL, max_pid)
else:
PID_ROLL = 0
if abs(yaw_error) > 0.1:
pid_yaw.update_control(yaw_error)
PID_YAW = - pid_yaw.get_control()
PID_YAW = constrain(PID_YAW, 0.4)
else:
PID_YAW = 0
print(f"PID Control: PITCH={PID_PITCH}, ROLL={PID_ROLL}, YAW = {PID_YAW}")
client.setVelXYYaw(PID_PITCH, PID_ROLL, 0)
# Выравнивание по высоте
def HeightRegulation(height_error):
current_height = client.getHeightRange()
target_height = current_height - height_error
print(f'current_height is {current_height}')
print(f'target_height is {target_height}')
client.setHeight(target_height)
# Функция ограничения величины
def constrain(value, threshold):
if value > threshold:
value = threshold
if value < -threshold:
value = -threshold
return value
def main():
# Подключаемся
print("connected?", client.connect(ip, port), "\n")
# Сбрасываем скорости
print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")
# Взлет
print("takeoff?", client.takeoff(), "\n")
time.sleep(10)
for _ in range(2):
# Ищем ArUco-маркер
found_id = SearchForArucos(0.5)
print(found_id)
# Выравнивание относительно маркера
ArucoRegulation(found_id)
# Устанавливаем цвет светодиода на красный
client.setDiod(255.0, 0.0, 0.0)
# Сбрасываем скорости
print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")
# Посадка
print("boarding?", client.boarding(), "\n")
print("Main function completed successfully!")
client = Drone()
client.AddOnMessangeUtils(OnMes0)
if __name__ == "__main__":
main()Квадрокоптер выполняет взлет, после чего переходит к поиску ArUco-маркера. В процессе поиска он вращается по часовой стрелке. Как только обнаруживается маркер, который ранее не распознавался, вращение прекращается — скорости обнуляются, и функция завершает работу с помощью return.
ID найденного маркера добавляется в список id_markers, который используется для хранения уже обработанных целей, чтобы исключить повторное выравнивание по одному и тому же маркеру.
В функции ArucoRegulation() задаются коэффициенты PID-регулятора, вычисляются ошибки по тангажу (pitch_error), крену (roll_error), рысканию (yaw_error) и высоте (height_error), а также определяется условие выхода из функции при завершении выравнивания.
В отличие от аналогичных функций из предыдущих примеров, которые реагируют на любой обнаруженный маркер, здесь используется работа с конкретным маркером. Функция принимает ID целевого маркера (target_id) и обрабатывает только его. В каждой итерации цикла while программа перебирает массив всех видимых маркеров (all_markers). Если маркер с нужным ID найден, его данные (координаты X, Y, Z и угол Yaw) извлекаются для расчетов ошибок. Все остальные маркеры в кадре игнорируются.
Рассчитанные ошибки по pitch, roll и yaw передаются в функцию DefaultRegulation(), где задаются скорости для выравнивания, рассчитанные при помощи PID-регулятора. Эти значения ограничиваются функцией constrain(), чтобы избежать перерегулирования.
Для каждой из осей реализовано условие обнуления ошибки, если она не превышает определенный порог (accuracy для pitch и 0,2 для yaw). В примере accuracy равно 0.0, однако данное значение можно увеличить. Тогда дрон, попадая в определенную зону вокруг маркера, не будет совершать движений, так как скорости будут обнулены.
Функция выравнивания по высоте height_regulation() вызывается только в случае, если ошибка по высоте превышает 0.15 м.
После выравнивания по одному маркеру цикл for (строка 158) идет на вторую итерацию, выполняется поиск второго маркера, и алгоритм выравнивания повторяется вновь.
Количество маркеров, на которые дрон будет выравниваться, настраивается в цикле for в 158 строке. Достаточно заменить «2» на нужное количество маркеров. Однако каждый цикл выравнивания с использованием PID-регулятора требует времени. При увеличении количества маркеров необходимо учитывать общее время полета дрона и среднее время на выравнивание по ArUco-маркеру.
Для ускорения работы при большем количестве маркеров рекомендуется скорректировать коэффициенты ПИД-регуляторов (строки 53–55) или увеличить допустимый порог ошибок accuracy (строка 64).
4.8.3. Примеры программ с использованием цветового блоба
Перед запуском примеров выполняем настройку цветового блоба, который будет распознавать дрон. Проверяем раздел «Настройки» Графического интерфейса. Включаем «Обнаружение Блобов», переходим во вкладку «Блобы» и выполняем настройку цветового маркера: настраиваем компоненты HSV так, чтобы желаемый блоб распознавался полностью.
Настройка компонентов HSV рассмотрена в подразделе 3.6.1, а также в Приложении 3.

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


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


Пример распознавания блоба в полете при корректной настройке HSV
Слежение за блобом (Blob_Follow_PID_Reg.py)
Данный пример демонстрирует слежение и выравнивание дрона относительно блоба. Программа запускается в бесконечном цикле без прерывания, посадку необходимо выполнить в ручном режиме.
python
from drone_control_api import Drone
from drone_control_api.pid import PID
import time
import cv2
ip = "10.42.0.1"
port = "1233"
def blob_follow():
# Настройка PID регулятора для компоненты ROLL и PITCH
pid_pitch = PID(0.0001101, 0.0, 0.0000108)
pid_roll = PID(0.01151, 0.0, 0.001)
# Зона сверху кадра, которую занимает защита дрона
top_offset = 40
# Зона границы
border_area = 20
# Центральный пиксель изображения, приходящего с камеры
camera_center_x = 320/2
camera_center_y = top_offset + (240 - top_offset) / 2\
# Целевая площадь блоба в кадре
target_area = 13000
# Минимальная площадь блоба
threshold_area = 3000
# Допустимая ошибка по высоте
accuracy_height = 10
# Инициализация переменных для избежания ошибки
roll_error = 0
pitch_error = 0
height_error = 0
current_area = 0
while True:
blobs = client.getBlobs()
if isinstance(blobs, str):
print("Ошибка получения блобов:", blobs)
break
# Фильтрация блобов, центры которых находятся в зоне ниже защиты дрона
valid_blobs = [b for b in blobs if b["center"]["y"] > top_offset]
if len(valid_blobs) <= 0:
client.setDiod(255, 0, 0)
print("Блобы не обнаружены")
print(f"pitch_error = {pitch_error}, roll_error = {roll_error}, height_error = {height_error}")
# Посылаем в регулятор последние успешно сохраненные значения ошибок
Blob_Regulation(pitch_error, roll_error, pid_pitch, pid_roll)
# Регулируем высоту по последнему сохраненному значению ошибки по высоте
height_regulation(height_error)
Continue
max_blob = max(valid_blobs, key=lambda b: b["size"]["x"] * b["size"]["y"])
print(f"Максимальный блоб: {max_blob}")
current_area = max_blob['size']['x'] * max_blob['size']['y']
if current_area > threshold_area:
roll_error = max_blob["center"]["x"] - camera_center_x
height_error = max_blob["center"]["y"] - camera_center_y
if abs(roll_error) >= border_area or abs(height_error) > border_area:
print("Блоб сильно смещен! Обнуляю Pitch.")
client.setDiod(0, 0, 255)
pitch_error = 0.0
else:
print("Блоб смещен не сильно! Рассчитываю Pitch.")
client.setDiod(0, 255, 0)
pitch_error = target_area - current_area
# Мертвая зона для Pitch
if abs(pitch_error) < 100:
pitch_error = 0.0
print(f"pitch_error = {pitch_error}, roll_error = {roll_error}, height_error = {height_error}")
# Передаем актуальные ошибки в ПИД-регулятор
Blob_Regulation(pitch_error, roll_error, pid_pitch, pid_roll)
if abs(height_error) > accuracy_height:
height_regulation(height_error)
else:
# Блоб не подходит по площади или не обнаружен
client.setDiod(255, 255, 255)
print(f"Блоб не подходит по площади, current_area = {current_area}" if valid_blobs else "Блоб не обнаружен")
print(f"pitch_error = {pitch_error}, roll_error = {roll_error}")
# Посылаем в регулятор последние успешно сохраненные значения ошибок
Blob_Regulation(pitch_error, roll_error, pid_pitch, pid_roll)
# Регулируем высоту по последнему сохраненному значению ошибки по высоте
height_regulation(height_error)
# Выравнивание по высоте
def height_regulation(height_error):
height_error *= 0.01
current_height = client.getHeightRange()
new_height = current_height - height_error
print(f"new_height = {new_height}")
client.setHeight(new_height)
# Выравнивание относительно цветового маркера
def Blob_Regulation(pitch_error, roll_error, pid_pitch:PID, pid_roll:PID):
pid_pitch.update_control(pitch_error)
PID_pitch = pid_pitch.get_control()
PID_pitch = constrain (PID_pitch, 0.8)
pid_roll.update_control(roll_error)
PID_roll = pid_roll.get_control()
PID_roll = constrain (PID_roll, 0.8)
print(f"PID_pitch={PID_pitch}, PID_roll={PID_roll}")
client.setVelXYYaw(PID_pitch, PID_roll, 0)
# Функция для ограничения значения переменной
def constrain(value, threshold):
if value > threshold:
value = threshold
if value < -threshold:
value = -threshold
return value
client = Drone()
print("connected?", client.connect(ip, port), "\n")
print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")
print("takeoff?", client.takeoff(), "\n")
time.sleep(8)
blob_follow()После установки соединения и взлета дрона на 1 метр вызывается функция слежения blob_follow(). В ней происходит фильтрация зоны сверху кадра размером 40 пикселей (top_offset), которую занимает защита дрона. Все объекты, чьи координаты выше этого значения, отбрасываются.
Среди оставшихся в нижней части кадра объектов выбирается самый большой – max_blob. Если площадь этого объекта меньше пороговой (threshold_area), светодиод загорается белым светом, в консоль выводится сообщение о том, что объекты не обнаружены, либо распознаваемый блоб не проходит фильтрацию по пороговой площади.
Если же подходящий объект найден, программа вычисляет ошибки, на основе которых дрон будет корректировать свое положение. Ошибки по крену (roll_error) и высоте (height_error) рассчитываются относительно пиксельного центра изображения. Обратим внимание, что пиксельный центр по оси Y (camera_center_y) рассчитан с учетом зоны сверху кадра, которую занимает защита дрона. В ошибку по тангажу (pitch_error) записывается разница между желаемой площадью блоба (target_area) в кадре и текущей (current_area).
В случае, когда объект смещается вверх, вниз или в стороны, его площадь в кадре может уменьшиться, что приведет к корректировке по оси pitch, так как выравнивание по тангажу основано на площади блоба в кадре. Чтобы избежать этого эффекта задается приоритетность выравнивания. Когда блоб сильно смещен по горизонтали или вертикали, ошибка по pitch обнуляется и выполняется корректировка по крену и высоте. В это время светодиод загорается синим светом.
Также для осей pitch и roll введена «мертвая зона»: если ошибка меньше заданных порогов (100 для pitch и 10 для roll), она принудительно обнуляется. Это необходимо из-за нестабильности распознавания блоба — его границы могут незначительно колебаться, что приводит к шуму в ошибках и, как следствие, к лишним управляющим воздействиям. Мертвая зона позволяет уменьшить влияние этих колебаний и сделать полет более стабильным.
Рассчитанные значения ошибок передаются в функции управления дроном: по высоте — в height_regulation, по крену и тангажу — в Blob_Regulation. Функция Blob_Regulation также использует коэффициенты ПИД-регуляторов для соответствующих осей:
pid_pitch = PID(0.0001101, 0.0, 0.0000108);
pid_roll = PID(0.01151, 0.0, 0.001).
Следует отметить, что коэффициенты ПИД-регулятора для оси pitch значительно меньше, чем для roll. Это связано с тем, что значения ошибок по pitch существенно больше по масштабу, чем по roll, так как pitch_error вычисляется с использованием площади блоба. Поэтому для получения корректного управляющего воздействия эти ошибки необходимо сильнее масштабировать за счет меньших коэффициентов регулятора.
Функция constrain() ограничивает величину выходных управляющих воздействий, что позволяет снизить риск перерегулирования дрона.
Перед запуском данного примера необходимо самостоятельно вычислить величину переменной target_area. Это значение задает площадь блоба на изображении, которая соответствует нужному расстоянию до объекта, и используется для управления положением дрона. Если блоб будет небольшим, а целевая площадь – наоборот, дрон будет пытаться достичь целевой площади и вскоре врежется в блоб.Перед запуском данного примера необходимо самостоятельно вычислить величину переменной target_area. Это значение задает площадь блоба на изображении, которая соответствует нужному расстоянию до объекта, и используется для управления положением дрона. Если блоб будет небольшим, а целевая площадь – наоборот, дрон будет пытаться достичь целевой площади и вскоре врежется в блоб.
Для расчета target_area выполняем настройку распознавания блоба (см. подраздел 3.6.1). Далее создаем Python-файл, например, в директории «examples» и пишем небольшую программу, которая будет выводить в терминал площадь видимого блоба, а также выведет изображение с камеры. Программа учитывает зону сверху кадра, занятую защитой квадрокоптера и окрашивает эту часть в черный цвет для удобства.
Пример программы:
python
from drone_control_api import Drone
import cv2
ip = "10.42.0.1"
port = "1233"
# Зона сверху кадра, которую занимает защита дрона
top_offset = 40
client = Drone()
print("Connected?", client.connect(ip, port))
try:
while True:
# Получение изображения с камеры
image = client.getImage()
if image is not None:
# Закрашиваем верхнюю часть (от 0 до top_offset пикселей по высоте) в черный цвет
image[0:top_offset,:] = 0
# Вывод изображения
cv2.imshow("Blob", image)
else:
print("Не удалось получить изображение с камеры")
cv2.waitKey(1)
blobs = client.getBlobs()
if blobs and isinstance(blobs, list):
valid_blobs = [b for b in blobs if b["center"]["y"] > top_offset]
if len(valid_blobs) > 0:
max_blob = max(valid_blobs, key=lambda b: b["size"]["x"] * b["size"]["y"])
blob_area = max_blob['size']['x'] * max_blob['size']['y']
print(f"Площадь блоба: {blob_area}")
else:
print("Блобы в рабочей зоне не обнаружены")
except KeyboardInterrupt:
print("Программа остановлена")
finally:
cv2.destroyAllWindows()Запускаем квадрокоптер в ручном режиме, взлетев на высоту 1 метра и подлетаем к блобу на расстоянии, которое он будет удерживать.
Рекомендуется выполнять расчет целевой площади посредством двух операторов. Первый будет управлять положением дрона, второй – координировать действия первого и следить за тем, чтобы блоб распознавался полностью.
Запускаем выполнение программы, следим за тем, чтобы блоб распознавался полностью и фиксируем измеряемую площадь

Определение целевой площади блоба
В примере значения целевой площади находятся в диапазоне от 12900 до 13200 пикселей (диапазон значений в ваших условиях будет другим). Выбираем значение из интервала и вписываем его в переменную target_area. Рекомендуется взять значение близкое к минимальному при условии, что блоб распознавался полностью.
4.8.4. Примеры программ с использованием полезной нагрузки
Данный подраздел рассматривает описание работы примеров, использующих бортовой модуль захвата груза, сервопривод (не входит в комплект), игрушечный пистолет (не входит в комплект). В Приложении 4 приведены их подключение к квадрокоптеру и настройка модулей для управления ими. Перед запуском примеров необходимо выполнить указанные в приложении шаги.
Примеры, использующие сервопривод и игрушечный пистолет, реализованы на базе нестандартных конфигураций дрона ARA-EDU. Данные модули, сопутствующие крепления и провода для подключения не входят в базовый комплект поставки и служат примером целевого расширения платформы для решения специализированных соревновательных задач. Использование подобных модификаций осуществляется в рамках соревнований и под строгим контролем экспертов. Примеры, использующие сервопривод и игрушечный пистолет, реализованы на базе нестандартных конфигураций дрона ARA-EDU. Данные модули, сопутствующие крепления и провода для подключения не входят в базовый комплект поставки и служат примером целевого расширения платформы для решения специализированных соревновательных задач. Использование подобных модификаций осуществляется в рамках соревнований и под строгим контролем экспертов.
Примером применения нестандартной сборки ARA-EDU в соревновательной деятельности выступает дисциплина «Аэродуэль».
Сортировка груза по ID Аруко при помощи электромагнита (Aruco_Sorting_Magnet.json)
Пример демонстрирует использование электромагнита в автономном полете для транспортировки груза. С помощью камеры дрон считывает ArUco-маркеры, которые определяют конечную точку доставки. В зависимости от ID распознанного ArUco-маркера загорится соответствующий ему цвет светодиода, и квадрокоптер выполнит полет в точку. Для полета используется метод gotoXYdrone().
В программе реализовано ожидание готовности оператора перед отправкой команды на взлет. Перед запуском программы необходимо поместить под электромагнитом металлический объект для захвата и доставки. Также следует убедиться, что закрепленный груз не перекрывает датчик оптического потока (Optical Flow).
Пример работает корректно при стандартной настройке каналов пульта и режимов полета. Не изменяйте настройки режимов в конфигураторе «AR-configurator» и назначение каналов в настройках пульта управления. Если изменения уже были внесены, верните настройки в исходную рекомендуемую конфигурацию.Пример работает корректно при стандартной настройке каналов пульта и режимов полета. Не изменяйте настройки режимов в конфигураторе «AR-configurator» и назначение каналов в настройках пульта управления. Если изменения уже были внесены, верните настройки в исходную рекомендуемую конфигурацию.
python
import time
from drone_control_api import Drone
ip = "10.42.0.1"
port = "1233"
client = Drone()
rc_threshold = 1900
x_target = 1
# Функция распознавания маркера
def get_flight_config():
while True:
aruco = client.getArucos()
if aruco:
marker_id = aruco[0]['id']
print(f"ID ArUco-маркера: {marker_id}")
if marker_id == 0:
return {'y': 0, 'color': (255, 0, 0)} # Красный
elif marker_id == 1:
return {'y': 1, 'color': (0, 255, 0)} # Зеленый
else:
return {'y': -1, 'color': (0, 0, 255)} # Синий
# Функция ожидания активации режимов
def wait_for_safety_switches():
print('ОТОЙДИТЕ ОТ ДРОНА И ЗАКРОЙТЕ ПОЛЕТНУЮ ЗОНУ', '\n')
print('ВКЛЮЧИТЕ ARM')
print('ВКЛЮЧИТЕ ALTHOD+POSHOLD')
print('АКТИВИРУЙТЕ 8 КАНАЛ', '\n')
while True:
telemetry = client.getUtilsData()
if telemetry:
rc_channels = telemetry[4]
if rc_channels:
''' В списке rc_channels (при условии, что настройки режимов полета не изменялись):
- 5 канал, отвечающий за ARM, находится под индексом 4
- 6 канал, отвечающий за переключение режимов полета, находится под индексом 5
- 8 канал, отвечающий за включение автономного режима, находится под индексом 7 '''
is_armed = rc_channels[4] > rc_threshold
is_mode = rc_channels[5] > rc_threshold
is_automode = rc_channels[7] > rc_threshold
if is_armed and is_mode and is_automode:
break
def main():
print("connected?", client.connect(ip, port), "\n")
# Распознавание маркера
config = get_flight_config()
y_target = config['y']
r, g, b = config['color']
client.setDiod(r, g, b)
print(f"Целевая точка: X = {x_target}, Y = {y_target}")
print("setMagnet?", client.setMagnet(True), "\n")
# Ожидание готовности оператора
wait_for_safety_switches()
# Выполнение полета
print("setVelXYYaw?", client.setVelXYYaw(0, 0, 0), "\n")
print("takeoff?", client.takeoff(), "\n")
time.sleep(10)
print("setZeroOdomOpticflow?", client.setZeroOdomOpticflow(), "\n")
print("gotoXYdrone?", client.gotoXYdrone(x_target, y_target), "\n")
print("boarding?", client.boarding(), "\n")
print("setMagnet?", client.setMagnet(False), "\n")
print("disconnected?", client.disconnect(), "\n")
if __name__ == "__main__":
main()Основной алгоритм программы реализован в функции main(). После установления соединения вызывается функция get_flight_config(), где происходит ожидание распознавания ArUco-маркера. В зависимости от считанного ID, программа определяет параметры полета:
ID 0: целевая координата Y = 0, цвет индикации — красный.
ID 1: целевая координата Y = 1, цвет индикации — зеленый.
Другие ID: целевая координата Y = –1, индикация — синяя.
После успешного распознавания маркера функция get_flight_config() завершается и возвращает словарь, содержащий целевую координату
Далее вызывается функция wait_for_safety_switches(), блокирующая выполнение программы до подтверждения готовности оператором. В терминал выводятся подсказки о действиях, которые необходимо произвести: освободить и закрыть полетную зону, после чего активировать режимы для запуска автономного полета.
С помощью метода getUtilsData() программа запрашивает текущую телеметрию дрона. В переменную rc_channels помещается список значений каналов управления, после чего проверяется их состояние. Если уровень сигнала в канале превышает пороговое значение rc_threshold, данный режим считается активированным. Для старта полетного задания должны быть включены тумблеры ARM, режимы удержания (Althold + Poshold) и восьмой канал управления.
Как только все условия безопасности выполнены, функция wait_for_safety_switches() завершается. Программа передает команду на взлет, после чего совершается полет в заданную точку с координатами X = 1 и вычисленным значением Y.
Сброс груза по цвету при помощи сервопривода (Color_Cargo_Drop_Servo.py)
Пример демонстрирует работу механизма сброса на основе сервопривода в автономном полете. Квадрокоптер выполняет взлет, начинает вращение и ищет цветовые объекты (блобы). При обнаружении определенного блоба светодиод загорается соответствующим цветом и поворачивает сервопривод на заданный угол для сброса груза. Выполнение полетного задания завершается посадкой после обработки двух различных цветовых зон.
Перед запуском примера подвешиваем грузы зеленого и синего цвета на сброс и убеждаемся, что их отсоединение без команды невозможно. Также выполняем настройку блобов: для блоба ID 0 настраиваем распознавание синего объекта, для блоба ID 1 – распознавание зеленого объекта, а также следим, за тем, чтобы объекты распознавались полностью и не пересекались.


Настройка блобов
Таким образом, при распознавании синего блоба светодиод загорится синим цветом, сервопривод повернется на 0 градусов, сбросив синий груз. При распознавании зеленого блоба светодиод загорится зеленым цветом сервопривод повернется на 180 градусов (3,14 радиан), сбросив зеленый груз.
python
from drone_control_api import Drone
from drone_control_api.pid import PID
import time
import math
ip = "10.42.0.1"
port = "1233"
def search_blobs(vel):
treshold_area = 20000
client.setVelXYYaw(0, 0, vel)
id_blobs_list = []
while not len(id_blobs_list) == 2:
blobs = client.getBlobs()
if blobs:
for blob in blobs:
max_blob = max(blobs, key = lambda b: b['size']['x'] * b['size']['y'])
current_area = max_blob['size']['x'] * max_blob['size']['y']
id = blob["id"]
if id not in id_blobs_list and current_area > treshold_area:
# Если обнаружен синий блоб
if id == '0':
id_blobs_list.append(id)
client.setDiod(0, 0, 255)
client.setVelXYYaw(0, 0, 0)
print('Сброс синего груза')
client.setServoAngle(0.0)
# Если обнаружен зеленый блоб
if id == '1':
id_blobs_list.append(id)
client.setDiod(0, 255, 0)
client.setVelXYYaw(0, 0, 0)
print('Сброс зеленого груза')
client.setServoAngle(3.14)
client.setVelXYYaw(0, 0, vel)
client = Drone()
print("connected?", client.connect(ip, port), "\n")
client.setServoAngle(1.57)
print("setVelXYYaw?", client.setVelXYYaw(0, 0, 0), "\n")
print("takeoff?", client.takeoff(), "\n")
time.sleep(12)
search_blobs(0.5)
print("setVelXYYaw?", client.setVelXYYaw(0, 0, 0), "\n")
print("boarding?", client.boarding(), "\n")После установки соединения сервопривод устанавливается на 90 градусов (1,57 радиан) для удержания грузов, далее производится взлет и вызов функции search_blobs(vel). Здесь квадрокоптер начинает вращение вокруг своей оси для поиска блобов.
Для отслеживания результатов создается пустой список id_blobs_list, а за поиск отвечает цикл while, который выполняется до тех пор, пока в список не будут занесены два идентификатора (ID) целевых цветных объектов. При обнаружении блобов для фильтрации шумов вычисляется текущая площадь блоба (current_area) и сравнивается с пороговым значением treshold_area. Если объект слишком мал (находится далеко), дрон игнорирует его и продолжает поиск.
Когда в поле зрения камеры попадает объект, превышающий пороговую площадь, считывается его ID. Воспроизводится один из следующих сценариев:
ID «0» (Синий объект): список найденных целей пополняется текущим ID, светодиод загорается синим, а вращение временно останавливается. Команда client.setServoAngle(0.0) поворачивает сервопривод в крайнее положение (0°), открывая защелку для сброса синего груза.
ID «1» (Зеленый объект): текущий ID заносится в список, светодиод загорается зеленым цветом, вращение прекращается. Сервопривод поворачивается в положение 180° (3,14 радиан), сбрасывая зеленый груз.
После отработки любого из сценариев снова подается команда на вращение client.setVelXYYaw(0, 0, vel), чтобы продолжить поиск оставшегося объекта.
Как только в списке id_blobs_list оказываются оба идентификатора, цикл while завершается. Вращение квадрокоптера останавливается и происходит посадка.
Стрельба по Аруко (Aeroduel_Tir.py)
В данном примере после взлета квадрокоптер выполняет выравнивание по ArUco-маркеру с ID 0 на расстоянии 1 м. Далее дрон осуществляет последовательный поиск маркеров с ID 1 и 2: сначала выполняется поиск маркера с ID 1, выравнивание по оси yaw и выстрел по нему, после чего аналогичные действия выполняются для маркера с ID 2.
Нулевой (ID 0) ArUco-маркер используется в качестве точки стабилизации дрона. Рекомендуется размещать маркеры с ID 1 и 2 на прилегающей стене полетной зоны относительно стены, на которой установлен маркер с ID 0.
Перемещая его, можно регулировать расстояние, с которого выполняются выстрелы по ArUco-маркерам с ID 1 и 2, так как после завершения выравнивания на нулевой маркер, квадрокоптер сразу переходит к поиску следующих маркеров.
ArUco-маркеры рекомендуется располагать на высоте ~1 м.
python
from drone_control_api import Drone
from drone_control_api.pid import PID
import time
import math
ip = "10.42.0.1"
port = "1233"
def search_aruco(marker_id, calculating_angle=False, shoot=False):
pid_yaw = PID(0.5, 0.0, 0.0)
accuracy = 0.15
client.setVelXYYaw(0.0, 0.0, 0.5)
search = True
while search:
marker_info = client.getArucos()
if marker_info:
for marker in marker_info:
if marker['id'] == marker_id:
if calculating_angle:
roll_error = marker['pose']['position']['x']
pitch_error = marker['pose']['position']['z']
yaw_error = math.atan2(roll_error, pitch_error)
else:
yaw_error = -1 * marker['pose']['orientation']['z']
print("yaw_error", yaw_error)
pid_yaw.update_control (yaw_error)
PID_yaw = pid_yaw.get_control()
PID_yaw = constrain(PID_yaw, 0.5)
client.setVelXYYaw(0.0, 0.0, PID_yaw)
if abs(yaw_error) < accuracy:
client.setVelXYYaw(0.0, 0.0, 0.0)
print(f"Выравнивание по yaw на маркер {marker_id} завершено")
if shoot:
client.setShoot(0.15)
print("Выстрел")
search = False
def aruco_reg():
pid_pitch = PID(0.5, 0.0, 0.0)
pid_roll = PID(0.5, 0.0, 0.0)
accuracy = 0.2
while True:
marker_info = client.getArucos()
if marker_info:
if marker_info [0]['id'] == 0:
distance_to_marker = marker_info [0]['pose']['position']['z']
pitch_error = distance_to_marker - 1
roll_error = marker_info[0]['pose']['position']['x']
pid_pitch.update_control(pitch_error)
PID_pitch = pid_pitch.get_control()
PID_pitch = constrain (PID_pitch, 0.3)
pid_roll.update_control(roll_error)
PID_roll = pid_roll.get_control()
PID_roll = constrain (PID_roll, 0.3)
# print(f'PID_pitch: {PID_pitch}, PID_roll: {PID_roll}')
client.setVelXYYaw(PID_pitch, PID_roll, 0.0)
if abs(pitch_error) < accuracy and abs(roll_error) < accuracy:
client.setVelXYYaw(0.0, 0.0, 0.0)
print("Выравнивание на маркер с id 0 завершено")
break
def constrain (value, threshold):
if value > threshold:
value = threshold
if value < -threshold:
value = -threshold
return value
client = Drone()
print("connected?", client.connect(ip, port), "\n")
client.setVelXYYaw(0.0, 0.0, 0.0)
client.takeoff()
time.sleep(15)
client.setHeight(0.9)
print("Поиск маркера с id 0")
search_aruco(marker_id=0)
print("Выравнивание на маркер с id 0")
aruco_reg()
for i in range(1, 3):
print("Поиск маркера с id", i)
search_aruco(marker_id=i, calculating_angle=True, shoot=True)
print("Посадка")
client.boarding()
client.disconnect()Квадрокоптер взлетает, снижается до высоты 0,9 м, следом запускается функция search_aruco() для поиска ArUco-маркера с ID 0. Данная функция предназначена не только для поиска маркеров. Здесь выполняется расчет ошибки по yaw и выравнивание дрона по этой оси относительно маркеров. Так как при первом вызове этой функции параметр calculating_angle равен False, ошибка будет рассчитываться исходя из ориентации маркера относительно дрона.
В результате выравнивания по ориентации дрон повернется так, чтобы его корпус стал параллелен плоскости маркера. Сам маркер при этом может находиться далеко сбоку от центра камеры. Именно по этой причине выравнивание по ориентации не подходит для совершения выстрелов из точки, но подходит для полного выравнивания относительно маркера.
После завершения выравнивания по yaw вызывается функция aruco_reg(), где реализован стандартный алгоритм выравнивания дрона относительно ArUco-маркера с использованием PID-регулятора по осям pitch и roll, а также условие выхода из функции.
Завершив выравнивание по маркеру с ID 0, дрон перейдет к поиску маркера с ID 1. Функция search_aruco() вызывается с параметрами calculating_angle и shoot, равными True. Теперь ошибка по yaw рассчитывается при помощи арктангенса.
Происходит вычисление угла, на который необходимо повернуться, чтобы маркер оказался в центре кадра камеры. Значение передается в метод установки скорости setVelXYYaw(0.0, 0.0, PID_yaw). Таким образом, корпус дрона не параллелен плоскости маркера, а направлен в его сторону для совершения выстрела.
Выполнив выравнивание и выстрел по ArUco-маркеру ID 1, выполняется тот же алгоритм для маркера с ID 2. Завершив второе выравнивание и выстрел, квадрокоптер выполнит посадку.