Материал: Автоматизация системы управления квадрокоптера

Далее объявим переменные для данных с акселерометра, гироскопа и компаса - каждый из них ориентируется в трехмерном пространстве и поэтому выдает контроллеру по три значения за раз. Назовем их ax, ay, az, gx, gy, gz, mx, my, mz соответственно. Также необходимо объявить переменную fps, которая будет хранить частоту выборок фильтра. В конце каждого цикла она будет обновляться, но для первого цикла ей при инициализации нужно присвоить значение, например, 100. Далее инициализируем массивы постоянных величин, полученные в калибровочной матрице из примеров на сайте производителя [12]. Это наборы констант для калибровки компаса и compassCalibrationBias представляет собой одномерный массив из трех элементов, а compassCalibrationMatrix - двухмерный массив из девяти элементов.

В функции setup необходимо произвести инициализацию всех элементов инерционного измерительного устройства и откалибровать компас. В начале функции выведем в монитор порта сообщение «Begin init». Затем нужно для объектов accel, gyro и compass запустить метод .begin. Для объекта compass также запускаем метод calibrateMatrix с аргументами compassCalibrationBias и compassCalibrationMatrix, он откалибрует компас для данного запуска устройства. В конце функции выводим в монитор порта сообщение об удачной инициализации, его можно будет увидеть только если все методы завершили свою работу без ошибок.

В функции loop нужно сначала вычислить частоту обработки фильтра, fps. Для этого в функции, после всех команд, связанных с ультразвуковым дальномером нужно записать в переменную startMillis значение времени, прошедшего с начала работы контроллера с помощью функции millis, которая возвращает это время. В конце функции, но перед командой задержки delay нужно сначала вычислить затраченное время на обработку данных, millis - startMillis и записать его в новую переменную, deltaMillis. Затем вычисляем частоту обработки фильтра в герцах, для этого нужно величину, обратную deltaMillis домножить на 1000, так как время в переменной deltaMillis измеряется в миллисекундах. Между присваиванием startMillis и вычислением fps находится весь остальной код, обрабатывающий информацию с инерционного измерительного устройства.

Начинается он со считывания данных с датчиков. Метод readGXYZ объекта accel считывает данные с акселерометра в единицах G и записывает их в переменные ax, ay, az с помощью указателей. Метод readRadPerSecXYZ объекта gyro считывает данные с гироскопа в радианах в секунду и записывает их в переменные gx, gy, gz. Метод readCalibrateGaussXYZ объекта compass считывает данные с компаса в Гауссах и записывает их в переменные mx, my, mz.

После того, как данные получены, библиотека позволяет поместить их в объект фильтра и извлекать из него конкретные значения рыскания, крена и тангажа. Для начала нужно обновить коэффициенты фильтра - это делается с помощью метода setKoeff соответствующего объекта. Метод принимает два аргумента - вычисленное в предыдущем цикле значение fps и числовой коэффициент BETA, в примерах производителя равный 0,22. При необходимости его можно будет изменять эмпирическим путем, вне функций запишем это число в константу BETA с помощью команды #define.

Далее с помощью метода update обновляем входные данные в фильтр. Аргументами служат значения g* a* и z*, полученные ранее. Теперь из объекта фильтра методами getYawDeg, getPitchDeg и getRollDeg можно получить значения рыскания, тангажа и крена и записать их в соответствующие переменные yaw, pitch и deg. После вывода их в монитор серийного порта, можно наблюдать следующее.

Рис. 14. Данные из монитора порта

Перед преобразованием данных в необходимый для полетного контроллера вид, нужно учесть еще два нюанса. Первый - то, что в течение определенного промежутка времени после запуска контроллера, датчики не могут выдавать правильные показания и отправлять такие данные на полетный контроллер недопустимо. Второй состоит в том, что рыскание должно принимать значения от -127 до 127, где 0 - это положение равновесия, при котором корректировка рыскания не осуществляется. Проблема состоит в том, что нулевое значение рыскания с инерционного измерительного устройства выставляется по компасу и соответствует направлению на север. Естественно, что в реальном устройстве не должно быть привязки к сторонам света, а нулевое значение должно быть любым, удобным пользователю.

Для исправления обеих вышеописанных проблем добавим к схеме кнопку запуска. Она должна замыкать вывод питания 5В и один из логических выводов. Объявим номер ее вывода как константу с именем BUTTON_PIN и функцией pinMode зададим выводу значение INPUT_PULLUP, то есть вход с включением подтягивающего резистора, встроенного в плату Arduino. Создадим функцию, в которую включены различные типы отработки нажатий кнопок. В данном коде будут использоваться только одинарные нажатия с применением защиты против дребезга контактов, но функция поддерживает так же двойные нажатия, нажатия с удержанием и т.д. Все эти режимы могут пригодиться при дальнейшей разработке программы. Присвоим одной из переменных, button1S значение digitalRead (BUTTON_PIN), а затем вызовем функцию обработки кнопок. После этого нужно поставить условие, было ли выполнено одинарное нажатие, то есть является ли выражение button1P истиной. Если да, то этой переменной следует задать значение false и выполнить необходимые действия, а именно «перевернуть» флаг check (то есть присвоить значение true переменной check, если до этого она имела значение false и наоборот) и задать переменной yawCenter значение yaw. Это нулевой отсчет рыскания, теперь в программе ориентация будет приходиться не на север, а на градус, записанный в переменную yawCenter при нажатии кнопки. Также в блоке, в котором происходит печать значений в монитор порта, добавим условие: при значении check равному false пусть в начале каждой строки печатается восклицательный знак - это будет символизировать то, что устройство находится в режиме ожидания и не передает данные.

4.2    Преобразование данных

Приступим к преобразованию данных. Как уже говорилось выше, каждое значение должно занимать один байт и газ должен принимать значения от 0 до 255, рыскание, тангаж и крен - от -127 до 127. Причем канал тангажа должен быть инвертирован, то есть при наклоне вперед выдавать отрицательное значение, а при наклоне назад - положительное.

Начнем с преобразования рыскания. Как оговаривалось ранее, его значение центрируется, то есть задается определенный угол yawCenter, который считается нулевым. Значит для получения конечного результата следует пользоваться не углом yaw, а углом yaw - yawCenter, он равен нулю если датчик находится в положении, при котором была нажата кнопка пуска и отклоняется от нуля при изменении этого положения. Нас интересует изменение положения максимум на 40 градусов в каждую сторону, на больший угол повернуть руку трудно физически. Экспериментально выявлено, что значение угла является положительным при повороте влево и отрицательным при повороте вправо. Значит нужно составить такую функцию sendYaw (yaw - yawCenter), чтобы

·        При yaw - yawCenter = 0, sendYaw = 0;

·        При yaw - yawCenter = 40, sendYaw = -127;

·        При yaw - yawCenter = -40, sendYaw = 127;

Решив простую систему уравнений, выясняем, что такой функцией является sendYaw = -3,175 (yaw - yawCenter).

Теперь данные изменяются в нужном виде, но значения могут быть больше 127 и меньше -127. Можно исправить это простыми условиями: если sendYaw> 127, значит sendYaw нужно принудительно присвоить значение 127. Если же sendYaw< -127, значит sendYaw нужно принудительно присвоить значение -127.тангажом и креном необходимо произвести аналогичные преобразования, учитывая характер показаний с датчиков.

Для тангажа:

·        При pitch = 180 или -180, sendPitch = 0;

·        При pitch = 140, sendPitch = -127;

·        При pitch = -140, sendPitch = 127;

Получившееся уравнение:= 3,175 * pitch - 571,5, если -180 <= pitch < -140;

sendPitch = 3,175 * pitch + 571,5, если 140 < pitch <= 180;= 127, если 0 > pitch >= -140;

sendPitch = -127, если 0 <= pitch <= 140;

Для крена:

·        При roll = 180 или -180, sendRoll = 0;

·        При roll = -140, sendRoll = -127;

·        При roll = 140, sendRoll = 127;

Получившееся уравнение:

sendRoll= -3,175 * roll - 571,5, если -180 <= roll < -140;= -3,175 * roll + 571,5, если 140 < roll <= 180;= 127, если 0 < roll =< 140;= -127, если 0 >= roll >= -140;

Для газа же нужно произвольно выбрать значение исходной величины, которое будет соответствовать максимальному. Также нужно учесть, что датчик не выдает значения, равные нулю, поэтому точку нуля стоит выбрать другую. Выберем 2000 для максимальной точки и 400 для минимальной и тогда условия выглядят так:

·        При throttle = 400, sendThrottle = 0;

·        При throttle = 2000, sendThrottle = 255;

Тогда,= 0.159375 * throttle - 63,75, если 400 < throttle < 2000;= 0, если throttle <= 400; = 255, еслиthrottle>= 2000;

Теперь данные преобразованы и их можно отправлять на бортовой контроллер. Но перед этим следует написать программу для управления пятым и шестым каналами - режимами полетов.

5.      Разработка ПО для распознавания жестов руки с видеосигнала методами библиотеки OpenCV

В данном разделе поставлена задача создания компьютерной программы, реализующей распознавание жестов руки. Эту программу можно использовать как один из модулей системы управления беспилотным дроном. Ее выходные данные могут активировать дополнительные функции и режимы дрона.

В контроллерах, управляющих беспилотными дронами сообщение, сигнализирующее о необходимости переключить режим, передается по отдельным каналам, которые так же обрабатывается средствами временного уплотнения. Таким образом, общий сигнал состоит из нескольких широтно-импульсно промодулированных сигналов, разделенных по времени. Сначала передаются импульсы, исходя из длины которых контроллер подает напряжение на двигатели, а за ними - импульсы, длина которых сигнализирует о активности того или иного режима полета.

Программа должна каким-то образом распознавать жест, показанный пользователем, возвращать результат (определенный код для каждого предусмотренного жеста), затем этот результат передавать в программу, анализирующую показания датчиков и формирующую сигнал для передачи на квадрокоптер. Эта задача является достаточно сложной в плане производительности - жест руки снимается на видео и затем видеосигнал анализируется сложными алгоритмами, требующими значительных вычислительных мощностей. Микроконтроллер AVR для этих целей не подойдет, а персональный компьютер обладает слишком низкой мобильностью для практического применения устройства. Поэтому идеальным вариантом бы стал мобильный телефон на системе Android: мы можем создать приложение, захватывающее видеосигнал с камеры устройства, обрабатывающее его и передающее результат по протоколу Bluetooth на основной модуль устройства. Поэтому для разработки был выбран язык программирования, используемый в разработке приложений для Android - Java. Первый вариант программы, для упрощения отладки и тестирования, создан как приложение с графическим интерфейсом на языке JavaFX для платформы IBM PC, но в будущем перенос приложения на Android не займет много сил и времени, так как Java является мультиплафтормерным языком программирования. Функционал первой версии программы пока будет ограничен подсчетом загнутых пальцев и проверкой состояния руки (соединены ли пальцы вместе или раздвинуты).

5.1    Постановка задачи и проектирование программного обеспечения

Рис. 15. Принципиальная схема программы

Разработку программы имеет смысл разделить на несколько шагов. Сначала программа должна получить видеосигнал с камеры и выделить из него одно изображение. Все остальные действия ведутся над анализом этого изображения, и, после завершения всех процедур, программа возвращает результат и захватывает новое изображение. Следующий после захвата изображения этап - его редактирование. Программа должна максимально упростить себе задачу анализа: изображение должно быть максимально сжато по разрешению и не должно содержать лишних цветов. В данном случае нужно произвести пороговое преобразование в бинарный вид, так как будут использоваться именно бинарные методы. Далее программа должна выделить необходимые контуры изображения с помощью алгоритмов компьютерного зрения и найти их дефекты. Подробнее об этом пойдет речь в соответствующем подразделе. Далее нужно найти и отфильтровать так называемые экстремальные точки - они содержат большинство информации об объекте. Затем исходя из положения этих точек нужно произвести необходимые вычисления и сделать вывод о том, какой именно жест демонстрируется. Так же вся графическая информация должна быть показана на экране. После всех этих действий цикл следует повторить.

Для программы был создан проект на платформе JavaFX в среде разработки IntelliJ IDEA. JavaFX по своей сути есть платформа для создания приложений на языке Java с графическим интерфейсом. Для создания интерфейсов существует файл на языке разметки FXML, который «общается» с программой на Java с помощью класса controller.java. FXML-файл описывает положение, размеры, графическое оформление всех элементов программы, а controller.java связывает элементы в программный код.

5.2    Захват видеосигнала

Первой задачей стал вывод на экран компьютера видеопотока с камеры. Сделать это можно и без использования библиотеки OpenCV, но мы сразу будем использовать её возможности. В классе Main создадим метод start, который будет производить действия для предварительной подготовки программы. В этом методе мы создаем объект класса FXMLLoader, указываем ему путь на FXML-файл с разметкой и назначаем его «родителем» (Parent). Отныне каждый объект, находящийся в программе будет являться классом-«ребенком» (Child) и подчиняться FXML-файлу и его контроллеру. Затем создается объект класса Stage (а так же дочерний объект класса Scene, который является окном программы в системе Microsoft Windows) и задаются его параметры - заголовок, размеры, возможность изменять размеры окна. Затем методом Stage.show окно вызывается для взаимодействия с пользователем.