Автоматизация системы управления квадрокоптера. Дипломная (ВКР). Информатика, ВТ, телекоммуникации. 2017-06-25

Квадрокоптеры

Система управления квадрокоптером на основе адаптивной нейронной сети

Наука й Образование

МГТУ им. Н.Э. Баумана

Сетевое научное издание

1ЭЗМ

Наука и Образование. МГТУ им. Н.Э. Баумана. Электрон. журн. 2023. № 07. С. 262-277.

Б01: 10.7463/0717.0001282

Представлена в редакцию: 11.06.2023 Исправлена: 25.06.2023

© МГТУ им. Н.Э. Баумана

УДК 62-503.57

Система управления квадрокоптером на основе адаптивной нейронной сети

Ющенко А.С.1, Лебедев К.Р.1′», ‘:к^еЬе(1еу@уапаех:ш

Забихафар С.Х.1

:МГТУ им. Н.Э. Баумана, Москва, Россия

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

Ключевые слова: БПЛА, квадрокоптер, нелинейное управление, скользящее управление, адаптивная нейронная сеть

Введение

В настоящее время неуклонно растет интерес к использованию беспилотных муль-тироторных летательных аппаратов (БПЛА), предназначенных для широкого спектра задач, в основном, из-за простоты конструкции и высокой грузоподъемности, по сравнению с классическими вариантами вертолетов.

К сожалению, решение задачи управления мультикоптерами осложняется ввиду существенной нелинейности и действия внешних возмущений. Наиболее распространенные ПИД-регуляторы, а также линейно-квадратичные регуляторы, недостаточно хорошо справляются с этой задачей [1, 2]. Возникает необходимость оперативной подстройки ПИД-коэффициентов регуляторов в процессе работы, или же полной перенастройки в случаях изменения параметров объекта управления.

Одним из способов управления в изменяющихся условиях является применение скользящего режима [3]. Эта технология позволяет добиться стабилизации и работы управляемой системы даже при случайных внешних воздействиях и отсутствии достаточ-

но точной математической модели объекта управления. Принцип скольжения заключается в обеспечении движения системы в непосредственной близости от поверхности скольжения в фазовом пространстве. С другой стороны, скользящий режим имеет существенные недостатки. Наиболее существенным является высокочастотное дрожание системы около поверхности скольжения. Также, скользящий режим предполагает полное знание динамики системы. Для устранения этих недостатков были предложены разные методы. Например, в работе A. G. Aissaoui, H. Abid и M. Abid описано использование нечеткой логики для управления приводом [4], а в работе Lon-Chen Hung и Hung-Yuan Chung искусственная нейронная сеть применяется в управлении манипулятором [5].

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

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

1. Динамика квадрокоптера

О

Рис.1. Расчетная модель квадрокоптера

Используя углы Эйлера и второй закон Ньютона, мы можем определить уравнение динамики как [1]:

ф =

0 =

Ml^lzZ Iz;,ä IrПг (t) à kfrx„,2

К L

-фв—

J e—f- Ф dф

m> (Iy- ) wn ф- f n 2

i, i,, i, i,,

в2 da

.. ^ (t) (Ix — Iy ) f ,2

w = —— -— фв—1— w dm

Г Iz Iz ф Iz w w

.. (CwSeCv SVSW )Ui (t) kftx .

x = —w-ф-^^—— x dx

m m

.. (SwSвСф SфСг )Ul(t) kfty — cj

У y

(1)

У = ■

m

m

.. CeCvUx{t) f . z =-ф—— z — g dz

mm

где, m — масса квадрокоптера, Ir — сумма моментов инерций роторов, взятых относительно оси z, Ix, Iy, Iz — моменты инерции относительно главных осей, kftx, kfty, kftz — коэффициенты аэродинамического сопротивления перемещению, kfrx, kfry, kfrz — коэффициенты аэродинамического сопротивления вращению, ф, 0, w — углы крен, тангаж и рысканье, x, y, z — координаты центра масс, dx,d ,dz,d ,de,d — внешние возмущения по координатам (на-

4

пример, ветер), Qr = ^ (-1)1 1 , w; — угловая скорость i-го мотора.

i=1

Существует три вида движения квадрокоптера:

1) подъем-спуск достигается одновременным увеличением и уменьшением силы тяги моторов;

2) тангаж как результат разницы силы тяги переднего и заднего моторов, аналогично крен;

3) рысканье достигается с помощью разницы в скорости разноименно вращающихся моторов, исходя из теоремы о сохранении кинетического момента механической системы.

Управляющие сигналы относительно угловых скоростей моторов, учитывающие три возможные вида движения:

(2)

где, kp — коэффициент тяги, kd — моментный коэффициент.

Одним из основных режимов управления квадрокоптером является движение по траектории. Рассматривая уравнения динамики системы (1), заметим, что в переносном

Ui(t ) » ‘ kP kP kP kP » œ2

Uv (t ) 0 — lkp 0 lkp Ю22

Ue (t ) — lkp 0 lkp 0 œ32

Uw (t ) _ kd — kd kd — kd _ œ42

движении системы управляющими сигналами для перемещения центра масс в неподвижной системе координат 0ХУ2 являются [6]:

= ЗДСф )Ul(t) Uy(t) = ^Сф SФCV )Ul(t) (3)

Uz(t) = CeCчA(t)

где ^ = sin(ф),Cф = cos(ф),Se = sin(e), Ce = ^(Ю^у = ^пО),Су = .

Желаемую траекторию переносного движения определяют параметры: ф, e и и1(1). Выразим уравнения желаемых крена и тангажа из (3):

. , — иу(1)С^а

ф = агс8ш(^ =)

л/иХ (1) иу(‘) и2(1)

ва = агс’апЛ^» — иу(‘)5″ ) ‘ ( и»(‘) ‘ (4)

где фd, ed, у d — желаемые параметры движения, ^ = sm(уd ),С а = cos(уd )

2. Постановка задачи

В общем случае, динамическая модель квадрокоптера в форме Эйлера-Лагранжа записывается как [7] (матричная форма):

M(q)q Vm (я, q )q G(q) F(q) = U (5)

где М^) — матрица инерции, )я — матрица кориолисовых и центростремительных

сил, G(q) — вектор гравитации, F(q) — вектор сил аэродинамического сопротивления, и — вектор управляющих сил.

Рассмотрим режим движения по траектории и ориентации в пространстве. Определим текущую ошибку как

е = Яа — Я,

где = [ха уа zd фа ed уа ]т — вектор желаемых координат, а

Я = [х У z ф e у^ — вектор текущих координат.

Зададим поверхность скольжения, преобразовав ошибку линейным фильтром Б(‘):

S = е (6)

где Я > 0 — диагональная матрица, представляющая наклон поверхности скольжения.

При движении системы вдоль поверхности скольжения в фазовой плоскости ошибка S и ее производная стремятся к нулю.

Уравнение динамики квадрокоптера в терминах преобразованной ошибки S может быть записано как:

мS = ед — и (7)

где Г (х) — неизвестная функция, определяемая динамикой квадрокоптера:

ВД = М(я)(4й ^ё) Ут(я, я)(я й А,е) ) О(я) (8)

Согласно универсальной теореме аппроксимации, искусственная нейронная сеть с одним скрытым слоем может аппроксимировать любую нелинейную, непрерывную, неизвестную функцию с любой точностью [8]. Исходя из данного утверждения, введем двухслойную нейронную сеть с сигмоидальной активационной функцией нейронов скрытого слоя (рис. 2) для аппроксимации функции Д(х), которая описывается соотношением:

ВД = ^^а^х) в (9)

где W, V — неизвестные весовые множители, при ограничении, наложенном на ошибку аппроксимации: |У| < ( % — неотрицательное ограничение).

Рис. 2. Структура нейронной сети

Обозначим оценку функции Д(х) с помощью введенной нейронной сети:

£ (х) = УУ та(У тх) (10)

Если V,М — текущие значения весовых множителей, то, можно вычислить погрешности V = V — V, У = W — УУ, с помощью которых проводится настройка нейронной сети.

3. Синтез регулятора

При движении системы вдоль поверхности скольжения возникает эффект высокочастотного дрожания, который негативно влияет на качество следования желаемой траектории движения. Введем вторую нейронную сеть для устранения данного эффекта — корректирующую. Структура нейросетвого регулятора, состоящего из основной и корректирующей нейронных сетей, представлена на рисунке 3.

Рис. 3. Структура нейросетевого регулятора

Представим управляющий сигнал в виде двух слагаемых:

и = и* и„ (11)

где — основной управляющий сигнал для движения системы в непосредственной близости от поверхности скольжения, исог — корректирующий сигнал. Выберем основной управляющий сигнал в виде [7]:

и* = У та(/тх) К^ (12)

Корректирующий управляющий сигнал введен для сглаживания эффекта высокочастотного дрожания около поверхности скольжения, и имеет в основе непрерывную функцию Р(.):

Р(х) =

1 — е-

(13)

1 е-2х

Тогда идеальный корректирующий управляющий сигнал:

И„ = Б*Р(^) (14)

где S* — линейная комбинация е и е. Вторая нейронная сеть должна быть добавлена в схему для аппроксимации И^г (рис. 4).

Рис. 4. Структура корректирующей нейронной сети

Выход данной сети:

UOT = BP(aV) 8С (15)

где весовые множители B и a = [^ À2]t — идеальные веса выходного и скрытого слоя сети, xe = [e èр — входной вектор, sc — ошибка аппроксимации и P(.) — активационная функция, определенная в формуле (13).

Так как веса B и a неизвестны, необходимо найти уравнения их настройки. Запишем оценку корректирующего управляющего сигнала:

uCor = B P(a Txe) (16)

При этом, ошибка оценки корректирующего сигнала:

Uc0r = Bip B P ‘ aTxe wg (17)

где 13, a — оценка B, a соответственно, B = B — B, a = a — a ошибка оценки, w — ошибка при-

ближения:

wg = sc bip a xe BO(axe)2

(18)

Сделаем допущение, что:

wr

< wg,B < вт^| a<an

гдеат, неизвестные положительные константы. Возьмем законы настройки, предложенные в [8]:

13 = БВР (атхе)8

а = Ба (хеБВ Р’ к а И(а-(а))

где = Рвт > 0^ = Бат > 0,ка> 0. Здесь обозначено:

«|Т

а =

‘ 1

,1,8 > 0

(20)

|е| 8

После подстановки в (11) управляющий сигнал примет окончательный вид:

и = ^ та(/ тх) КУБ 33 Р(а тхе) (21)

При использовании данного управления, уравнения динамики системы в терминах преобразованной ошибки представляют из себя:

МБ = -(Ку Чт)Б WTа(VTx) — та(/ тх) В Р(а тхе) 8 (22)

Прибавим и вычтем из правой части равенства (22) выражение Wтa ¥та, где а = а(/тх), 5 = а-а :

МБ = -(Ку ЧМ)Б а W т~ ВР 8 (23)

Ключевым шагом является разложение в ряд Тейлора сигмоидальной функции 5 и

непрерывной активационной функции Р . Ограничиваясь первыми двумя членами разложения, запишем:

мБ = -(ку чм)Б W т<а’ Vтx — ВР — ВзРР’ ахе ^ (24)

где ошибка приближения:

^(г) = ¥т а’/тх ВР(атхе) ^ WTO(VTx)2 8 (25)

Пусть в (24) равно нулю. Тогда можно показать, ошибка стремится к нулю с течением времени, если положить

= Б(аБт — кШ)

. . (26) V/ = а(хБт¥ т а’-кБЦчЧ)

для любых Б, О, к > 0 («e-modificatюntechшque» [8]). При этом весовые коэффициенты,

соответствующие оценкам /,М остаются ограниченными. Покажем, что при выбранном алгоритме настройки весовых коэффициентов движение системы в окрестности поверхности скольжения будет устойчивым.

Определим функцию Ляпунова как:

Ь = 1 БтМ(д)8 11г{¥тБ-1¥} 11г(/тО-1/} 1 гг{Вт^-1В} 1 М^а2 } (27)

Продифференцируем:

iL = STMS 1STM S tr(WTF-1W } tr{ VTG -1V} tr(BTFB-1B } tr{Fa-1Sä } (28) Подставляем уравнение (24), при wi=0:

Ii = -STKVS 1ST (M — 2Vm )S tr(WT (F-1W a ST)} tr{ VT (G -1V xST WT а’)} tr(BT(FB-1B — STP)} tr((Fa-1aа — axeSTBP’)}

Матрица ( M — 2 V ) — кососимметрическая, тогда второе слагаемое в уравнении (29) равно нулю. А также, из W = W — W, принимая во внимание, что W-константа, следует, что W = -W (аналогично для V,B,a). Тогда:

Ii = -StKVS < 0 (30)

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

4. Экспериментальное исследование

В ходе исследования в среде MATLAB была создана модель квадрокоптера согласно уравнениям динамики (1), а также регулятор для трех углов (крен, тангаж и рыскание). Регулятор состоит из нейронной сети для аппроксимации основных управляющих сигналов (входной вектор x = [фй фй cpd e e] на каждую ось (крен, тангаж и рысканье), 7 нейронов в скрытом слое), а также трех нейронных сетей для аппроксимации корректирующих управляющих сигналов (по одной на ось).

Внешние возмущения: d ,de,d = sin(t) 3; время моделирования: t=20 c. Параметры моделирования представлены в таблице 1 .

Таблица 1. Значение параметров при моделировании

Параметр Значение Параметр Значение

g, (м/с2) 9,81 kftz, (Н*с/м) 6e-4

m, (кг) 0,5 kfrxp, к&е,(кг*м2/рад) 5,5e-4

1, (м) 0,25 kfrv, (кг*м2/рад) 6e-4

кр,(Н*с2) 3e-5 F 100

кл(Н*м*с2) 3,5e-7 G 40

1г,(кг*м2) 2,5e-5 k 1

1х,(кг*м2) 3,5e-3 Fb 10

1у,(кг*м2) 3,5e-3 Fa 4

12,(кг*м2) 8e-3 ka 1

kftx, кГ4у,(Н*с/м) 5,5e-4

» | 1« Ц Н

Рис. 5. Крен: штриховая линия — фа, сплошная линия — ф.

Желаемый угол крена — периодическая функция. Перерегулирование стремится к 0%.

оь —

Рис. 6. Управляющий сигнал по крену — иф. Время подстройки регулятора к модели и внешним воздействиям — около 1~2 с.

т

Рис. 7. Тангаж: штриховая линия — 9а, сплошная линия — 9. При резком изменении желаемого значения угла перерегулирование стремится к 0%.

Рис. 8. Управляющий сигнал по тангажу — и9

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

Рис. 9. Рысканье: штриховая линия — сплошная линия — у.

Хорошее качество следования действительного значения угла желаемому закону изменения.

Рис. 10. Управляющий сигнал по рысканью —

Траектория центра масс при одновременном задании трех углов ориентации согласно желаемым законам изменения показана на рисунке 11:

Рис. 11. Координаты центра масс — x, y, z.

В результате моделирования поведения системы показана эффективность предлагаемого метода управления. Каждый из углов ориентации в пространстве (крен, тангаж и рысканье) следует желаемой траекторией с высокой точностью.

Заключение

В данной статье рассматривалась система управления квадрокоптером в скользящем режиме, основанная на двойном нейросетевом регуляторе. Основная нейронная сеть представляет из себя MIMO («Multiple Input Multiple Output») систему, аппроксимирующую управляющий сигнал для движения системы в непосредственной близости от поверхности скольжения. Вспомогательная нейронная сеть аппроксимирует корректирующий управляющий сигнал, необходимый для сглаживания эффекта высокочастотного дрожания около поверхности скольжения. Предлагаемый метод позволяет управлять системой без априорной информации о параметрах динамической модели управляемого объекта. Устойчивость движения системы в окрестности поверхности скольжения доказана с помощью метода Ляпунова. По результатам моделирования динамической модели квадрокоптера с нейросетевым регулятором и в среде MATLAB, можно сделать вывод о том, что предло-

женный способ управления обеспечивает устойчивое движение по заданной траектории несмотря на внешние возмущающие воздействия.

Список литературы

1. Bouabdallah S., Noth A., Siegwart R. PID vs LQ control techniques applied to an indoor micro quadrotor // Intelligent robots and systems: IEEE/RSJ Intern. conf. on intelligent robots and systems: IROS 2004 (Sendai, Japan, Sept. 28- Oct. 2, 2004): Proc. Vol. 3. N.Y.: IEEE, 2004. Pp. 2451-2456. DOI: 10.1109/IR0S.2004.1389776

2. Naidoo Y., Stopforth R., Bright, G. Quad-rotor unmanned aerial vehicle helicopter modelling and control // Intern. J. of Advanced Robotic Systems. 2023. Vol. 8. Iss. 4.

Pp. 139-149. DOI: 10.5772/45710

3. Spurgeon S.K. Sliding mode control: a tutorial // European control ^nf.: ECC 2023 (Strasbourg, France, June 25-27, 2023). Режим доступа:

https://kar.kent.ac.uk/41730/1/sliding_mode.pdf (дата обращения 17.07.2023).

4. Aissaoui A.G., Abid H., Abid M. Robust fuzzy sliding mode controller design for motors drives // Acta Electrotechnica et Informatica. 2009. Vol. 9. No. 2. Pp. 64-71. Режим доступа: http://www.aei.tuke.sk/papers/2009/2/10 Aissaoui.pdf (дата обращения 17.07.2023).

5. Lon-Chen Hung, Hung-Yuan, Chung. Hybrid neural sliding mode controller design for a robot manipulator // J. of Grey System. 2005. Vol. 17. No. 2. Pp. 183-200.

6. Bouhali O., Boudjedir H. Neural network control with neuro-sliding mode observer applied to quadrotor helicopter // Intern. symp. on innovations in intelligent systems and applications: INISTA 2023 (Istanbul, Turkey, June 15-18, 2023): Proc. N.Y.: IEEE, 2023. Pp. 8993.

7. Lewis F.L., Jagannathan S., Yesildirek A. Neural network control of robot manipulators and nonlinear systems. L.: Taylor & Francis, 1999. 442 p.

8. Stable adaptive neural network control / S.S. Ge a.o. Boston: Kluwer, 2002. 282 p.

Science ¿Education

of the Baumail MSTU

Science and Education of the Bauman MSTU, 2023, no. 07, pp. 262-277.

DOI: 10.7463/0717.0001282

Received: 11.06.2023

Revised: 25.06.2023

© Bauman Moscow State Technical Unversity

The Adaptive Neural Network Control of Quadrotor Helicopter

A.S. Yushenko1, K.R. Lebedev1*, S.H. Zabihafar1

kirileb edeviSvandexju

:Bauman Moscow State Technical University, Moscow, Russia

Keywords: UMAV, quadcopter, nonlinear control, sliding-mode control, adaptive neural network

The current steady-rising interest in using the unmanned multi-rotor aerial vehicles (UMAV) designed to solve a wide range of tasks is, mainly, due to their simple design and high weight-carrying capacity as compared to classical helicopter options.

Unfortunately, to solve a problem of multi-copter control is complicated because of essential nonlinearity and environmental perturbations. The most widely spread PID controllers and linear-quadratic regulators do not quite well cope with this task. The need arises for the prompt adjustment of PID controller coefficients in the course of operation or their complete re-tuning in cases of changing parameters of the control object.

One of the control methods under changing conditions is the use of the sliding mode. This technology enables us to reach the stabilization and proper operation of the controlled system even under accidental external exposures and when there is a lack of the reasonably accurate mathematical model of the control object. The sliding principle is to ensure the system motion in the immediate vicinity of the sliding surface in the phase space. On the other hand, the sliding mode has some essential disadvantages. The most significant one is the high-frequency jitter of the system near the sliding surface. The sliding mode also implies the complete knowledge of the system dynamics. Various methods have been proposed to eliminate these drawbacks. For example, A.G. Aissaoui’s, H. Abid’s and M. Abid’s paper describes the application of fuzzy logic to control a drive and in Lon-Chen Hung’s and Hung-Yuan Chung’s paper an artificial neural network is used for the manipulator control.

This paper presents a method of the quad-copter control with the aid of a neural network controller. This method enables us to control the system without a priori information on parameters of the dynamic model of the controlled object. The main neural network is a MIMO («Multiple Input Multiple Output») system approximating the control signal for the system motion in the immediate vicinity of the sliding surface. The auxiliary neural network approximates the corrective control signal required to smooth out the high-frequency jitter effect near the sliding surface.

In the course of the study a quad-copter model was designed in the MATLAB environment according to the dynamic equations as well as a controller for three angles (roll, pitch and yaw). The controller consists of a neural network for approximating the main control signals and three neural networks for approximating corrective control signals (one per the axis). Environmental perturbations are involved in model.

Based on the system behavior simulation the effectiveness of the proposed control method is shown. Each of the orientation angles (roll, pitch and yaw) follows the desired trajectory with high accuracy. The stability of the system motion in the sliding surface vicinity is proved by Lyapunov method. The simulation results of the neural network controller and a quad-copter dynamic model in the MATLAB environment allow us to draw conclusion that the proposed control method ensures the stable motion along a given trajectory even despite environmental perturbations.

References

1. Bouabdallah S., Noth A., Siegwart R. PID vs LQ control techniques applied to an indoor micro quadrotor. Intelligent robots and systems: IEEE/RSJIntern. conf. on intelligent robots and systems: IROS 2004 (Sendai, Japan, Sept. 28- Oct. 2, 2004): Proc. Vol. 3. N.Y.: IEEE, 2004. Pp. 2451-2456. DOI: 10.1109/IR0S.2004.1389776

2. Naidoo Y., Stopforth R., Bright, G. Quad-rotor unmanned aerial vehicle helicopter modelling and control. Intern. J. of Advanced Robotic Systems, 2023, vol. 8, iss. 4, pp. 139-149.

DOI: 10.5772/45710

3. Spurgeon S.K. Sliding mode control: a tutorial. European control conf.: ECC 2023 (Strasbourg, France, June 25-27, 2023). Available at: https://kar.kent.ac.uk/41730/1/sliding mode.pdf , accessed 17.07.2023.

4. Aissaoui A.G., Abid H., Abid M. Robust fuzzy sliding mode controller design for motors drives. Acta Electrotechnica et Informatica, 2009, vol. 9, no. 2, pp. 64-71. Available at: http://www.aei.tuke.sk/papers/2009/2/10_Aissaoui.pdf , accessed 17.07.2023.

5. Lon-Chen Hung, Hung-Yuan Chung. Hybrid neural sliding mode controller design for a robot manipulator. J. of Grey System, 2005, vol. 17, no. 2, pp. 183-200.

6. Bouhali O., Boudjedir H. Neural network control with neuro-sliding mode observer applied to quadrotor helicopter. Intern. symp. on innovations in intelligent systems and applications: INISTA 2023(Istanbul, Yurkey, June 15-18, 2023): Proc. N.Y.: IEEE, 2023. Pp. 89-93.

7. Lewis F.L., Jagannathan S., Yesildirek A. Neural network control of robot manipulators and nonlinear systems. L.: Taylor & Francis, 1999. 442 p.

8. Stable adaptive neural network control / S.S. Ge a.o. Boston: Kluwer, 2002. 282 p.

Автоматизация системы управления квадрокоптера. дипломная (вкр). информатика, вт, телекоммуникации. 2023-06-25

Министерство
образования и науки Российской Федерации

федеральное
государственное бюджетное образовательное учреждениевысшего образования

«Иркутский
государственный университет»

(ФГБОУ ВО
«ИГУ»)

Физический
факультет

Выпускная
квалификационная работа бакалавра по направлению 03.03.03 Радиофизика

«Радиоэлектронные
устройства, методы обработки сигналов и автоматизации»

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

Содержание

Введение

. Беспилотные мультироторные
летательные аппараты

.1 Базовые принципы полета
квадрокоптера

.2 ПИД регуляторы

.3 Полетный контроллер

.4 Фильтр Калмана

. Технология компьютерного зрения

.1 Введение в компьютерное зрение

.2 Методы обработки изображений в
компьютерном зрении

. Библиотека OpenCV

.1 Примитивные типы данных в OpenCV

.2 Классы библиотеки OpenCV

.Разработка системы управления
квадрокоптером на базе IMU и ультразвукового дальномера HC-504

.1 Сборка устройства и код для
получения данных

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

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

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

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

.3 Обработка изображения

.4 Выделение контуров

.5 Нахождение дефектов обводящего
контура

.6 Фильтрация точек дефектов

.7 Обработка результатов

. Формирование сигналов и передача
полученных данных

.1 Передача данных через серийный
порт

.2 Передача данных на бортовой
контроллер

Заключение

Список использованных источников

Приложение

Введение

На сегодняшний день беспилотные летательные
аппараты (БПЛА) — бурно развивающаяся область техники. БПЛА выполняют широкий
спектр как военных, так и гражданских задач. В последние годы большую
популярность приобрели мультироторные БПЛА (мультикоптеры) по причине
доступности и простоты эксплуатации. Мультикоптеры используются для доставки
грузов, фото- и видеосъемки, поисковых работ, наблюдения за природными явленими
и многого другого.

Разработки в этой области активно ведутся как
коммерческими предприятиями, так и любителями. В данной работе описан проект,
цель которого — автоматизировать систему управления мультикоптера с четырьмя
роторами, сделать управление интуитивно понятным и отзывчивым. В результате
было разработано устройство, способное передавать «с Земли» всю информацию,
необходимую для пилотирования мультикоптера, используя лишь жесты одной руки.
Разработка устройства от проектирования и до готового кода велась
самостоятельно и в ней, кроме всего прочего, были использованы источники с
открытым исходным кодом — платформа Arduino
ибиблиотека компьютерного зрения OpenCV.Устройство
является прототипом, но имеет перспективы к развитию и реальному использованию.

1.      Беспилотные мультироторные
летательные аппараты

Квадрокоптер (разновидность мультикоптера) — это
летательный аппарат построенный по вертолётной схеме с четырьмянесущими
винтами.

Многовинтовые вертолёты разрабатывались ещё в
первые годы вертолётостроения. Один из первых квадрокоптеров, который реально
оторвался от земли и мог держаться в воздухе, был создан Георгием Ботезатом и
испытан в 1922 году. Недостатком этих аппаратов была сложная трансмиссия,
передававшая вращение одного мотора на несколько винтов. Изобретение хвостового
винта и автомата перекоса положило конец этим попыткам. Новые разработки
начались в 1950-е годы, но дальше прототипов дело не продвинулось. беспилотный квадрокоптер контроллер

Новое рождение мультикоптеры получили в XXI
веке, уже как беспилотные аппараты. Благодаря простоте конструкции
квадрокоптеры часто используются в любительском моделировании. Мультикоптеры
удобны для недорогой аэрофото- и киносъёмки — громоздкая камера вынесена из
зоны действия винтов.

Квадрокоптеры имеют четыревинта постоянного шага
(автомата перекоса, в отличие от одно- и двухвинтовых аппаратов, нет). Каждый
винт приводится в движение собственным двигателем. Половина винтов вращается по
часовой стрелке, половина — против, поэтому хвостовой винт квадрокоптеру не
нужен. Маневрируют квадрокоптеры путём изменения скорости вращения винтов.
Например:

·        ускорить все винты — подъём;

·        ускорить винты с одной стороны и
замедлить с другой — движение в сторону;

·        ускорить винты, вращающиеся по
часовой стрелке, и замедлить вращающиеся против — поворот.

Микропроцессорная система переводит команды
радиоуправления в команды двигателям. Чтобы обеспечить стабильное зависание,
мультикоптеры в обязательном порядке снабжают тремя гироскопами, фиксирующими
крен аппарата. Как вспомогательный инструмент, иногда, также используется
акселерометр, данные от которого позволяют процессору устанавливать абсолютно
горизонтальное положение, и бародатчик, который позволяет фиксировать аппарат
на нужной высоте. Также, применяют сонар для автоматической посадки и удержания
небольшой высоты, а также для облёта препятствий.

Современные мультикоптеры используют
бесколлекторные электродвигатели и литий-полимерные аккумуляторы в качестве
источника энергии. Это накладывает определённые ограничения на их полётные
характеристики: типичная масса мультикоптера составляет от 1 до 4 кг, при
времени полёта от 10 до 30 минут (30-50 минут у уникальных единичных
экземпляров). Поднимаемый полезный груз моделями мультикоптеров среднего
размера и грузоподъёмности — от 500 г до 2-3 кг, что позволяет поднять в воздух
небольшую фото или видеокамеру. Существуют и достаточно крупные модели мультикоптеров,
с количеством роторов порядка 6-8 (гекса и октокоптеры), способные поднять в
воздух груз массой до 20-30 кг. Для увеличения грузоподъёмности применяют
соосное расположение несущих роторов, что в случае гексакоптера, например, даёт
12 моторов и 12 пропеллеров, расположенных попарно на 6 несущих лучах. Скорость
полёта мультикоптера может быть от нуля (неподвижное висение в точке) до
100-110 км/ч. Запас энергии батарей позволяет отдельным моделям мультикоптеров
улетать на расстояние до 7-12 км, на практике же радиус действия (максимальное
расстояние, на которое они способны улететь с последующим возвратом в точку
взлёта) обычно ограничено прямой видимостью (100-200 м при ручном управлении)
либо дальностью действия аппаратуры радиоуправления и видеолинка. При этом
лучшие образцы подобной аппаратуры, использующие усилители мощности
радиосигнала и систему направленных антенн, способны обеспечивать стабильные
радиоуправление и видеолинк на расстояния до 100 км. Таким образом, наибольшее
ограничение на радиус действия мультикоптеров накладывает именно время полёта.

1.1    Базовые принципы полета
квадрокоптера

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

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

Рис.1. Принципиальная схема квадрокоптера

Базовые принципы полетов любой техники описывает
аэродинамика и квадрокоптеры не являются исключением. Три оси вращения
абсолютно однозначно задают ориентацию квадрокоптера в пространстве и
направление его полета. Причем направление движения никак не зависит от
расположения самого квадрокоптера в воздухе.

Три перечисленных выше оси или угла принято
правильно называть тангажом (pitch),
креном (roll) и
рысканьем (yaw). Разберем
их более подробно.

Рис.2. Углы Эйлера — крен, тангаж и рыскание

Под тангажом понимают поворот аппарата вокруг
продольной оси, рысканием — вокруг вертикальной оси, а креном — продольной оси.

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

В случае квадрокоптера все обстоит иначе. Здесь
имеются целых четыре винта, два из которых вращаются по часовой стрелке, а два
других в противоположном направлении. Соответственно если все из винтов
квадрокоптера имеют одинаковую скорость вращения, то все параметры будут
скомпенсированы. При увеличении скорости вращения одного из винтов квадрокоптера
баланс нарушается. При этом, если скорость винта с обратным направлением
вращения будет пропорционально уменьшена, то рыскание не поменяется, но
изменится тангаж или крен[1].

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

Управление двигателями квадрокоптера, а,
следовательно, и скоростью вращения его винтов, осуществляется с пульта
дистанционного управления, сигнал с которых поступает на бортовой компьютер
квадрокоптера и к ним добавляются необходимые коррекции от гироскопа,
акселерометра и так далее.

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

1.2    ПИД регуляторы

Сигнал с полетного контроллера поступает не
напрямую на двигатель, а на так называемый ПИД-регулятор, который исходя из
входного сигнала выдает двигателю соответствующее напряжение. ПИД-регулирование
— необходимая технология для управления квадрокоптером.

Например, имеется дрон которому необходимо
повернуться на 60 градусов относительно центра масс. Движение вокруг центра
осуществляется подачей на его привода требуемой угловой скорости вращения. Для
того, чтобы повернуться именно на 60 градусов необходимую угловую скорость надо
подавать по определённому закону. В момент, когда разность между текущим углом
и требуемым ещё значительна, угловая скорость должна быть высокой. При
уменьшении же разности должна уменьшаться и скорость. Когда разность окажется
равна 0, угловая скорость также должна равняться 0. Однако, такой закон не
представляется возможным определить заранее, потому что квадрокоптер обладает
моментом инерции, и он способен просто пролететь необходимое положение, а в
этом случае придётся подавать скорость в обратном направлении. Самое
стандартное решение в такой ситуации — сообщать необходимую скорость как
разность требуемого положения и текущего, умноженную на определённую
неотрицательную постоянную. Подобный алгоритм называется П регулятором. Однако,
такой алгоритм обладает существенным недостатком: в случае, когда требуемое
угловой положение вращается с определённой угловой скоростью, то дрон никогда
не догонит эту точку. Рано или поздно наступит момент, когда угловая скорость,
выверенная П регулятором, будет равна угловой скорости вращения необходимого
положения. Для качественного решения данной задачи рекомендуется сложить
скорость от П регулятора со скоростью, рассчитанной как интеграл по времени
ошибки по углу, умноженный на определённую неотрицательную постоянную. В такой
ситуации необходимая скорость будет тем выше, чем дольше дрон не способен
занять требуемое угловое положение. Данный алгоритм является ПИ регулятором. Но
и этот алгоритм можно доработать. В момент, когда ошибка по углу будет
равняться 0, дрон под действием сил инерции пролетит нужное положение, при
условии, что его скорость до этого была относительно высокой. Чтобы этого
избежать необходимо совершать торможение чуть быстрее, когда ещё ошибка мала, а
именно — к требуемой скорости от ПИ регулятора добавить производную ошибки по
времени, которая в свою очередь умножается на положительную постоянную. Это и
есть ПИД регулятор.

Пропорционально-интегральное-дифференциальное
управление может быть представлено следующей схемой:

Рис.3. Принципиальная схема ПИД-регулятора

В блокеprocess
содержится описание объекта управления (его модель), в центральной части схемы
представлен сам регулятор, который отрабатывает сигнал ошибки(рассогласования)
задающего сигнала и измеренного значения с объекта управления. Ошибка в данной
схеме поступает одновременно на все три блока параллельно, при этом:

·        в блоке P происходит умножение соответствующего
коэффициента пропорциональной составляющей на величину ошибки;

·        в блоке I происходит интегрирование
ошибки и умножение полученной величины на коэффициент ;

·        в блоке D происходит
дифференцирование ошибки с умножением на коэффициент ;

Уравнение ПИД регулятора:

,

(t) — выходная величина регулятора;(t) -сигнал
рассогласования(ошибка);, I, D — пропорциональная, интегральная и дифференциальная
составляющие;

 — коэффициенты ПИД
регулятора;

Распространены также следующие модификации
уравнения ПИД регулятора:

,

,

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

Пользуясь преобразованием Лапласа при нулевых
начальных условиях, представим передаточную функцию ПИД регулятора в
операторной форме:

,

.

На рис. 4 изображены амплитудно-частотная(АЧХ) и
фазо-частотная(ФЧХ) характеристики операторной передаточной функции. В области
нижних частот АЧХ и ФЧХ определяются интегральным членом, в области средних
частот — пропорциональным, в области высоких — дифференциальным[3].

Рис.4. АЧХ и ФЧХ ПИД-регулятора

Вид АЧХ и ФЧХ регулятора определяет его точность
и запас устойчивости. Из рисунка 2 следует, что при уменьшении интегральной
составляющей , происходит
увеличение модуля коэффициента усиления регулятора на нижних частотах (то есть
при приближении к установившемуся режиму), следовательно, погрешность «e»
начинает снижаться.

При повышении дифференциальной компоненты
происходит усиление на высоких частотах, что влечёт за собой усиление шумов
измерений и внешних возмущений. Исходя из этого следует, что дифференциальную
составляющую используют только для улучшения формы переходного процесса в
системе, тогда как её практическая реализация зачастую содержит фильтр высоких
частот(ФВЧ).

Рис.5. ПИД-регулятор в системе с шумом и
внешними возмущениями f(t)

Повышение пропорционального коэффициента K влечёт
за собой увеличение модуля петлевого усиления контура регулирования, а также
увеличение точности во всём диапазоне частот, однако запас по фазе и усилению
начинает снижаться, что ухудшает робастность и качество регулирования системы,
а при дальнейшем увеличении пропорциональной составляющей K возникают
периодические колебания (система теряет устойчивость). Воздействие помех
измерений f и шума снижается с ростом петлевого усиления и пропорционального
коэффициента.

1.3    Полетный контроллер

Полетный контроллер — это основная плата
управления, обеспечивающая функционирование мультикоптера. В качестве
вычислительного центра платы управления используется микроконтроллер, зачастую
это либо маломощные Atmega328, либо более современные, Atmega2560 или ARM-контроллеры
(STM32)

К функциям полетного контроллера относятся:

·        Стабилизация аппарата в воздухе

·        Удержание высоты (при помощи
барометра) и позиции (при помощи GPS)

·        Автоматический полет по заданным
заранее точкам (опционально)

·        Передача на землю текущих параметров
полета с помощью модема или Bluetooth (опционально)

·        Обеспечение безопасности полета
(возврат в точку взлета при потере сигнала, автопосадка)

·        Подключение дополнительной
периферии: OSD, светодиодной индикации и пр.

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

Используемый в данном проекте полетный
контроллер ArduPilot Mega является полноценным решением БПЛА, который позволяет
помимо радиоуправляемого дистанционного пилотирования — автоматическое
управление по заранее созданому маршруту, т.е. полет по точкам, а также
обладает возможностью двухсторонней передачей телеметрических данных с борта на
наземную станцию (телефон, планшет, ноутбук, DIY) и ведение журнала во
встроенную память.

Он основан на автопилоте APM 2.x,
разрабатываемым сообществом DIY Drones и базирующийся на open-source проекте,
позволяющий превратить любой аппарат в автономное средство и эффективно
использовать его не только в развлекательных целях, но и для выполнение
профессиональных проектов.

Особенности:

·        3 осевой гироскоп, акселерометр,
магнитометр и высокоточный барометр

·        Система стабилизации с возможностью
воздушной акробатики

·        Удержание позиции по GPS, полет по
точкам и возврат на точку старта

·        Возможность использования
инфракрасного датчика для обхода препятствий

·        Поддержка ультразвукового датчика (sonar
sensor) для автоматического взлета и посадки

·        Автоматическое следование по
маршрутным точкам

·        Управление двигателями посредством
ШИМ (PWM) с использованием дешевых регуляторов скорости (ESC)

·        Собственная система стабилизации для
камеры (функция контроллера подвеса)

·        Радиосвязь и телеметрия с борта

·        Поддержка множества рам и
конфигураций аппаратов

·        Поддержка датчика уровня заряда
батареи

·        Настраиваемая световая индикация при
полетах

·        Совместим с многими
радиоуправляемыми приемниками PWM и PPM сигналов

·        Передача в реальном времени
телеметрических данных

·        Поддержка OSD телеметрии (наложение
на видеопередачу телемерических данных) используя протокол MAVLINK

·        Конфигурирования точек полета
посредством Google Maps

·        Бортовая флеш память 16Мбит для
автоматической регистрации данных

·        Цифровой компас работает на HMC5883L
(до версии 2.5.2)

·        6 степеней свободы в InvenSense
акселерометре , гироскоп MPU-6000

·        Контроллер Atmel ATmega2560-16AU и
ATMEGA32U-2 чип для обработки и функции USB

·        Возможно загрузка обновлений
встроенного программного обеспечения и конфигурации

Ниже описаны некоторые режимы полета, доступные
контроллеру:

·        Stabilize — удержание горизонта,

·        AltHold — удержание высоты,

·        Loiter -«замри и слоняйся»,

·        Return-to-Launch — вернуться на
точку старта,

·        Auto — выполнение заданного маршрута
в автоматическом режиме,

·        Acro — акробатика (отключение всех
стабилизационных систем),

·        Circle — облет по кругу заданного
радиуса,

·        Position — фиксация в воздухе с
ручным газом взлета

·        Land — автоматическая посадка

·        Simple — «легкий»
полет

Контроллер предусматривает возможность
самостоятельно программировать режимы полета.

1.4    Фильтр Калмана

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

Фильтр Калмана использует динамическую модель
системы (например, физический закон движения), известные управляющие
воздействия и множество последовательных измерений для формирования оптимальной
оценки состояния. Алгоритм состоит из двух повторяющихся фаз: предсказание и
корректировка. На первом рассчитывается предсказание состояния в следующий
момент времени (с учетом неточности их измерения). На втором, новая информация
с датчика корректирует предсказанное значение (также с учетом неточности и
зашумленности этой информации)

Рис.6. Схема фильтра Калмана

Уравнения представлены в матричной форме. В
случае с одной переменной матрицы вырождаются в скалярные значения.

В данных обозначениях: подстрочный индекс
обозначает момент времени: k
— текущий, (k-1) — предыдущий,
знак «минус» в верхнем индексе обозначает, что это предсказанное промежуточное
значение.
Описание переменных представлены на следующих изображениях[4]:

Рис.7. Уравнение предсказания

Рис.8. Уравнение корректировки

2.      Технология компьютерного
зрения

В данном разделе даны основные теоретические
сведения о рассматриваемом предмете.

2.1    Введение
в
компьютерное зрение

Компьютерное зрение — это общее название
совокупности теории и технологий создания машин, способных производить
обнаружение, отслеживание и классификацию объектов.

Компьютерное зрение — это очень молодая и
динамично развивающаяся область компьютерной технологии. Первые разработки в
этой области появились только в начале 1970-х годов. Главной причиной этому
служит то, что методы, обычно используемые в алгоритмах компьютерного зрения
достаточно требовательны к вычислительной мощности машин — почти всегда
приходится иметь дело с большими по временам рассвета цифровой эры массивами
данных. Так же по причине относительной новизны технологии и происхождения её
из многих других областей компьютерной науки, не существует стандартной
формулировки проблемы компьютерного зрения, и, более того, не существует
стандартного решения этой проблемы. Методы и алгоритмы компьютерного зрения
очень сильно зависят от конкретно поставленной задачи и могут кардинально
отличаться друг от друга. Компьютерное зрение во многом происходит от таких
областей, как статистика, методы оптимизации и обработка сигналов, а также
тесно пересекается с ними.

Максимально корректно задачу компьютерного
зрения можно сформулировать как извлечение полезных описаний из изображения или
последовательности изображений, а также в формировании полезных выводов исходя
из этих описаний. Критерий полезности описания и, соответственно, нужные методы
обработки выбираются исходя из конкретной задачи[5].

Главная сложность технологий компьютерного
зрения и, в то же время, причина необходимости их создания, состоит в том, что
любая электронно-вычислительная машина лишена абстрактных категорий и способна
работать только с конкретными величинами. Компьютер не «понимает», что такое
форма, текстура, размер или даже цвет, его можно только «научить» работать с
этой информацией, определенным образом закодировав её в последовательность
нулей и единиц.

2.2    Методы обработки изображений
в компьютерном зрении

Компьютерное зрение не рассматривает технологии
создания цифровых изображений, описывая лишь их обработку, поэтому опустим
подробности и остановимся на том, что устройство захвата (цифровая камера),
используя светочувствительные элементы формирует изображение (фото) или
последовательность изображений (видео) реальных объектов. «Изображение» для
компьютера — это определенным образом закодированный массив чисел.

Рис.9. Представление зрительной информации
компьютером

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

Однако широкая область методов компьютерного
зрения принимает в качестве входных данных бинарные изображения (состоящие
только из пикселов черного или белого цветов). В данной работе есть смысл
остановиться на этих методах.

Эти алгоритмы могут выполнять различные действия
— от очень простых, наподобие подсчета каких-либо признаков, до гораздо более
сложных действий, связанных с распознаванием, определением местоположения и
контролем объектов. Для формирования бинарного изображения B по данным
полутонового или цветного изображения I можно выполнить операцию, которая
выбирает некоторое подмножество пикселов изображения в качестве пикселов
переднего плана (foreground pixels). Эти пикселы представляют интерес для решаемой
задачи анализа изображений. Остальные пикселы игнорируются как фоновые
(background pixels). Операция отбора пикселов может быть простой (например,
пороговый оператор, который отбирает пикселы со значениями из заданного
яркостного диапазона или цветового подпространства) или представлять собой
сложный алгоритм классификации. [2]

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

Рис. 10. Бинарное изображение рукописных
символов

Пикселы бинарного изображения B принимают
значения 0 или 1. Будем полагать, что значения 1 соответствуют пикселам
переднего плана, а 0 — фоновым пикселам. Значение пиксела на пересечении строки
r и столбца c пиксельного массива обозначается как B [r, c]. Изображение
размерами M × N состоит из M строк,
пронумерованных от 0 до M − 1, и N столбцов, пронумерованных от 0 до N −
1. Таким образом, обозначение B [0, 0] соответствует значению левого верхнего
пиксела изображения, а B [M − 1, N − 1] — значению правого нижнего
пиксела. Во многих алгоритмах при обработке пикселов учитываются не только их
значения, но и значения соседних пикселов. При рассмотрении соседних пикселов
часто используются два определения окрестностей — четырехсвязная и
восьмисвязная окрестности. Четырехсвязная окрестность N4 [r, c] пиксела [r, c]
состоит из пикселов [r − 1, c], [r 1, c], [r, c − 1] и [r, c
1]. Они часто называются, соответственно, северным (north), южным (south),
западным (west) и восточным (east) соседями. Восьмисвязная окрестность N8 [r,
c] пиксела [r, c] содержит все пикселы четырехсвязной окрестности, а также
диагональные соседние пикселы [r − 1, c − 1], [r − 1, c 1],
[r 1, c − 1] и [r 1, c 1]. Их можно соответственно называть северо-
западным (northwest), северо-восточным (northeast), юго-западным (southwest) и
юго-восточным соседями (southeast). Расположение соседних пикселов в
окрестностях двух типов демонстрируется на рис. 11. [7]

Рис. 11. Четырехсвязная и восьмисвязная
окрестности пикселов

Во многих алгоритмах четырехсвязные или
восьмисвязные (или определенные иначе) окрестности часто называются просто
окрестностями пикселов. Анализ окрестностей позволяет перейти от пространству
пикселей к пространству контуров и производить обработку изображений так
называемыми скелетными методами.

Вообще методы обнаружения можно условно
разделить на три основные группы:

− скелетные методы;

− методы на основе 3D модели объекта;

− методы на основе 2D модели объекта.

В скелетных методах исследуется контур силуэта:
обычно отыскиваются углы, выступы, впадины и другие точки с высокими значениями
кривизны. Для получения информации о форме контура применяются различные
представления границы объекта. В методах на основе 3D модели руки представляют
в виде сложных трехмерных поверхностей и классифицируются с помощью нейронных
сетей Метод 2D распознавания схож с предыдущим, но использует двумерное
изображение вместо объемных моделей. Каждый из методов имеет преимущества и
недостатки. Недостатком метода на основе 3D модели является его ресурсоемкость.
Построение 3D модели, обучение нейронной сети и ее использование могут
потребовать значительных ресурсов, так же не стоит забывать, что для
использования данного метода требует камеры с возможностью определения глубины
изображения.

Остановимся на скелетных(структурных) методах,
которые используют для обработки изображений информацию о топологии объекта.
Ключевое понятие структурных методов — контур.

Контур — это внешние очертания (обвод) предмета
или объекта. При проведении контурного анализа полагается, что контур содержит
достаточную информацию о форме объекта, внутренние точки объекта во внимание не
принимаются. Вышеприведённые положения, разумеется, накладывают существенные
ограничения на область применения контурного анализа, которые, в основном,
связаны с проблемами выделения контура на изображениях: из-за одинаковой
яркости с фоном объект может не иметь чёткой границы, или может быть зашумлён
помехами, что приводит к невозможности выделения контура; перекрытие объектов
или их группировка приводит к тому, что контур выделяется неправильно и не
соответствует границе.

Однако, переход к рассмотрению только контуров
объектов позволяет уйти от пространства изображения к пространству контуров,
что существенно снижает сложность алгоритмов и вычислений. Резюмируя, контурный
анализ имеет довольно слабую устойчивость к помехам, и любое пересечение или
лишь частичная видимость объекта приводит либо к невозможности детектирования,
либо к ложным срабатываниям, но простота и быстродействие контурного анализа,
позволяют вполне успешно применять данный подход (при чётко выраженном объекте
на контрастном фоне и отсутствии помех).

После выделения контура в бинарный вид, он
подвергается процедуре скелетизации (утоньшению). Каждый последовательный
контур скелетного представления описывается в виде последовательного набора
особых точек и так называемого цепного кода, состоящего из точки привязки,
числа кодов и массива направлений из очередной точки на следующую точку. Особые
точки — это концевые точки и точки ветвления (триоды), т.е. точки, соседи
которых образуют не менее трех связных областей. На рис. 12 изображен образ,
обладающий двумя внутренними контурами, одной концевой точкой и тремя триодами
[8].

Рис. 12. Образ, прошедший процедуру скелетизации

В полученном описании производится огрубляющая
предобработка, состоящая в удалении коротких линий, объединении близких
триодов, уничтожении малых внутренних контуров. Для внешнего контура находится
его тип или топологический код. Для этого контур записывается в виде
последовательного набора номеров особых точек, соответствующих обходу по
часовой стрелке. Затем с помощью пере нумерации делается попытка отождествления
контура с одним из основных типов[6].

.        БиблиотекаOpenCV

В данном разделе представлены данные о
библиотеке OpenCV — основного инструмента для разработки проекта. OpenCV — это
одна из библиотек для работы с компьютерным зрением (библиотекой в
программировании называется сборник подпрограмм или объектов, используемых для
разработки программного обеспечения). OpenCV реализована на C/C , также
разрабатывается для Python, Java, Ruby, Matlab, Lua и других языков. Может
свободно использоваться в академических и коммерческих целях — распространяется
в условиях лицензии BSD. Библиотека довольно популярна, на текущий момент имеет
более 5 миллионов скачиваний и недавно была предложена в качестве основы для стандарта
Khronos по компьютерному зрению. Примечательно, что OpenCV был разработан
программистами из российского отделения компании Intel.

3.1    Примитивные типы данных в OpenCV

включает в себя множество примитивных типов
данных. Эти данные не являются примитивными с точки зрения языков
программирования высокого уровня, но являются самыми элементарными с точки
зрения OpenCV. [5]

Самый простой тип данных — Point. Эта простая
структура состоит только из двух полей x и y типа int. Point2D32f содержит два
поля x и y типа float. Point3D32f содержит три поля x, y и z типа float.
Используется для задания и обработки графических точек.содержит два поля width
и height типа int. Size2D32f содержит два поля width и height типа float. Size
содержит в себе информацию о размере того или иного массива.содержит четыре
поля x, y, width и height типа int и представляет собой прямоугольник.содержит
четыре переменные типа double. На самом деле Scalar включает в себя одно поле
val, которое является указателем на массив, содержащий четыре числа типа
double. Scalar хранит в себе данные о цвете того или иного объекта и, в отличие
от всех остальных структур содержит три конструктора. Первый может принимать
один, два, три или четыре аргумента и присваивать их соответствующим элементам val
[]. Второй конструктор, RealScalar, принимает один аргумент и устанавливает
соответствующее значение val [0], остальные элементы устанавливаются в 0.
Третий конструктор ScalarAll так же принимает один аргумент и инициализирует
все элементы val [] этим значением., Size, Rect и Scalar чрезвычайно полезны,
потому что существенно упрощают код. Они позволяют перейти от объектов,
привычных в численных вычислениях к объектам, необходимым для работы с
изображениями.

3.2    Классы библиотеки OpenCV

В данном подразделе обозначим основные классы
библиотеки OpenCV, причем функционал тех, которые будут широко использованы в
практической части, обговорим подробнее.-основная функциональность. Включает в
себя базовые структуры, вычисления (математические функции, генераторы
случайных чисел) и линейную алгебру, DFT, DCT, ввод/вывод для XML и YAML и т.
д.- работа с матрицами (многомерными массивами). Понятие матрицы в OpenCV
несколько более абстрактно, нежели в линейной алгебре. В частности, элементы
матрицы не обязательно должны быть просто числами, а могут представлять собой
любые объекты, в том числе и сторонних библиотек.- обработка изображений
(фильтрация, геометрические преобразования, преобразование цветовых пространств
и т. д.). Imgproc это базовая структура, используемая для кодирования того, что
мы называем «изображение». Эти изображения могут быть чёрно-белыми,
цветными, 4-х канальными (RGB Alpha), и каждый канал может содержать либо
целые, либо вещественные значения. Следовательно, этот тип является более
общим, чем стандартные 3-х канальные 8-битные изображения. OpenCV располагает
обширным арсеналом операторов для работы с этими изображениями, которые
позволяют изменять размеры изображений, извлекать отдельные каналы, складывать
два изображения, и т.д. В сущности, это объект Mat, но с некоторыми
дополнениями для интерпретации матрицы как изображения. Одно важное отличие
Imgproc от Mat заключается в поведении imageData. Данные Mat являются
объединением, поэтому существует возможность задать тип указателя. Указатель imageData
задается жестко как uchar*. При работе с матрицами, необходимо уменьшать
смещение, так как указатель данных не всегда может быть типа byte, в то время,
как указатель данных изображения всегда типа byte, смещение можно использовать
«как есть».

HighGUI
— простой UI, ввод/вывод изображений и видео. Функции OpenCV, которые позволяют
взаимодействовать с операционной системой, файловой системой и аппаратными
средствами, такими, как камера, собраны в библиотеке HighGUI (что означает
«высокоуровневый графический пользовательский интерфейс»). HighGUI
позволяет открывать окна для отображения изображений, читать и записывать
графические файлы (изображения и видео), обрабатывать простые события мыши,
указателя и клавиатуры. Данный класс также позволяет создавать такие полезные
элементы, как ползунок. HighGUI имеет достаточный функционал для разработки
различного рода приложений. При этом наибольшая польза от использования данной
библиотеки в её кроссплатформенности. библиотека HighGUI состоит из трех
частей: аппаратной, файловой и GUI. Аппаратная часть в первую очередь касается
работы с камерой. HighGUI предоставляет простые механизмы подключения и
последующего получения изображения с камеры. Все, что касается файловой
системы, в первую очередь связано с загрузкой и сохранением изображения.
Приятной особенностью библиотеки является наличие методов, которые одинаково
обрабатывают видеопоток и из файла, и с камеры. Та же идея заложена и в методы
обработки изображений. Функции просто полагаются на расширения файлов и
автоматически обрабатывают все операции по кодированию и декодированию
изображений. Третья часть HighGUI — GUI. Библиотека предоставляет несколько
простых функций, которые позволяют открывать окно и отображать в нем
изображения. Тут же (в окне) существует возможность обрабатывать события,
поступившие от мыши и клавиатуры [10].

ML — модели
машинного обучения (SVM, деревья решений, обучение со стимулированием и т. д.).

Features2d —
распознавание и описание плоских примитивов (SURF, FAST и другие, включая специализированный
фреймворк).

Video — анализ
движения и отслеживание объектов (оптический поток, шаблоны движения,
устранение фона).

Objdetect —
обнаружение объектов на изображении (нахождение лиц с помощью алгоритма
Виолы-Джонса, распознавание людей HOG и т. д.).

Calib3d — калибровка
камеры, поиск стерео-соответствия и элементы обработки трёхмерных данных.

4.      Разработка системы
управления квадрокоптером на базе
IMUи
ультразвукового дальномера
HC-504

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

Рис. 13. Принципиальная схема устройства

Принципиальная схема устройства показана на
рисунке. Ее ключевая часть — микроконтроллер AVRна
базе платформы Arduino.
На него приходят сигналы с датчиков, обрабатываются и с помощью радиомодулей
отправляются на борт. Каналом газа управляет ультразвуковой дальномер — чем
больше расстояние от него до руки, тем больше мощности подается на двигатели.
За рыскание, тангаж и крен отвечает так называемое инерционное измерительное
устройство (IMU),
закрепленное на руке, — гироскоп, акселерометр, компас и барометр, собранные в
одной плате. В определенном для каждого угла Эйлера интервале борт будет
копировать положение устройства.

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

Начнем сборку устройства с манипулятора, который
будет управлять первыми четырьмя каналами. В качестве логического процессора
будет использоваться микроконтроллер AtmelATMega2560 на базе Arduino.
Использование Arduino позволяет упростить разработку и отладку устройства, в
дальнейшем от него можно отказаться, ограничиваясь только микроконтроллером.

4.1    Сборка устройства и код для
получения данных

Как было сказано выше, беспилотный дрон управляется
четырьмя основными каналами — газ, рыскание, крен и тангаж. Полетный контроллер
принимает на своих входах значения каждого канала как целое число размером 1
байт (от 0 до 255 для газа и от -127 до 127 для рыскания, крена и тангажа).
Значит первая цель — получить эти значения как переменные в микроконтроллере. В
скетче можно сразу объявить эти переменные и назвать их throttle, yaw, pitch и
roll. В этих переменных будут хранится необработанные показатели датчиков. Для
данных, прошедших обработку и готовых к отправке, объявим еще четыре
переменных: sendThrottle, sendYaw, sendPitch, sendRoll.

Для получения значения газа будет использоваться
ультразвуковой дальномер HC-SR04. Необходимо подключить его к выводам 5V и GND
для обеспечения питания и два логических вывода соединить с любыми цифровыми
выводами Arduino. Подсоединим вывод trig к цифровому выводу с номером 8 и
определим его в программе как TRIGGER_PIN, а echo — к выводу 9 и определим, как
ECHO_PIN. Для работы с дальномером будет использоваться библиотека NewPing.h.
После ее импортирования следует ввести команду NewPing sonar(TRIGGER_PIN,
ECHO_PIN, MAX_DISTANCE), где MAX_DISTANCEустановим как 200 — это максимальное
измеряемое значение в сантиметрах. Эта команда создает объект sonar, методами
которого можно получить расстояние, измеряемое дальномером. Теперь в функции
loop можно присвоить переменной throttle значение sonar.ping и ее значение
будет пропорционально расстоянию до препятствия. В этом можно убедиться, если
вывести значение переменной в монитор серийного порта. В конце функции loop
также необходимо ввести задержку в 5мс для того чтобы периоды опроса не
«наслаивались» друг на друга и не возникало ошибочных значений.

В массиве представленных значений присутствует
достаточно сильный шум, который может негативно повлиять на результаты их
анализа. Необходимо включить в программу фильтрацию для переменной throttle.
Имеет смысл в данной работе использовать упрощенный фильтр Калмана. Принцип его
работы приведен выше. Функция фильтра принимает два аргумента — стандартное
отклонение величины от математического ожидания и скорость реакции на
изменение. Первый аргумент считается опытным путем — для этого нужно создать
массив данных достаточного размера на выходе датчика без изменения внешних
условий, а затем найти стандартное отклонение по генеральной совокупности.
Можно модифицировать программу для выполнения этого действия, но удобнее
выполнить его в программе MicrosoftExcel с помощью функции СТАНДОТКЛОН.Г. Из
выборки около 750 элементов получаем стандартное отклонение 21,47. Запишем это
значение при объявлении констант в начале кода. Подбор скорости реакции на
изменение производится вручную, эмпирическим путем. Нужно выставить значение
достаточно малое, чтобы дрон быстро воспринимал команду изменения газа и
достаточно большую, чтобы отфильтровать лишние значения. Для тестирования
добавим в программу функцию с алгоритмом упрощенного фильтра Калмана, которая
принимает значение и возвращает его отфильтрованным. Сразу после получения
переменной throttle в функции loop вызовем фильтрующую функцию, в качестве
аргумента которой будет значение throttle и значение которой будет снова
присваиваться переменной throttle. Далее эмпирическим путем определено, что
оптимальный коэффициент для данной задачи — это число 0,02.

Теперь произведем аналогичные действия для
остальных трех каналов, а именно считаем и отфильтруем данные с датчиков.
Показания рыскания, крена и тангажа будут считываться с инерционного
измерительного устройства, который имеет в составе гироскоп, акселерометр,
барометр и компас. Оно так же нуждается в питании с напряжением 5В и имеет два
логических выхода. Передача данных осуществляется по протоколу I2C, который
поддерживается Arduino, поэтому для передачи большого количества данных
достаточно двух проводов. Подсоединим устройство к I2C-совместимым портам
ArduinoMega, а именно 20 и 21. Они помечены метками SDA и SCL.

Для работы с инерционным измерительным
устройством необходимо импортировать в скетч две библиотеки — Wire.h для работы
с протоколом I2C и TroykaIMU.h для работы с самим устройством. Библиотека
написана по принципам объектно-ориентированного программирования, поэтому для
дальнейшей работы, вне функций необходимо создать по объекту для каждого
использованного модуля, а именно объекты классов Accelerometer, Gyroscope и
Compass. Все полученные величины будут проходить через фильтр Магвика, поэтому
для него тоже создать объект класса Magwick. Дадим ему имя filter, а объектам
для акселерометра, гироскопа и компаса — accel, gyro и compass соответсвенно.

Далее объявим переменные для данных с
акселерометра, гироскопа и компаса — каждый из них ориентируется в трехмерном
пространстве и поэтому выдает контроллеру по три значения за раз. Назовем их
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 окно
вызывается для взаимодействия с пользователем.

Выполнив эти действия, метод передает управление
классу controller.java, в котором содержится основной код программы. В этом
месте нужно создать объекты для всех элементов программы. Это объекты классов
Button (графическая кнопка включения и выключения камеры), ImageView (класс,
предназначенный для вывода изображений на экран), ScheduledExecutorService
(таймер для контроля видеопотока) и VideoCapture (класс OpenCV, захватывающий
видео с камеры).

Запрограммируем метод private Mat grabFrame,
захватывающий видеопоток. Как мы знаем, слово перед названием метода означает
тип данных переменной, которую возвращает этот метод. В данном случае это не
стандартная переменная, а объект класса Mat, содержащегося в библиотеке OpenCV.
В нашем случае это будет объект frame — одно изображение, взятое из
видеопотока. Итак, в методе private Mat grabFrame создаем объект frame класса
Mat. С помощью метода this.capture.isOpened программа проверяет, поступает ли с
порта камеры изображение, и, если да, то применяет метод this.capture.read
(frame), который записывает кадр в объект, а если нет, то выдает ошибку. Далее
идет проверка, содержит ли кадр какую-либо информацию, и, если содержит, то над
ним будут проводиться необходимые преобразования, которые мы пропишем позже.
Пустой же кадр метод возвращает без изменений.

Следующий необходимый метод обрабатывает
событие, возникающее при нажатии кнопки Start Camera. Назовем его void
startCamera (ActionEvent event). Как мы можем увидеть, этот метод принимает как
аргумент событие нажатия кнопки и не возвращает никаких значений.

Как только кнопка оказывается нажатой, метод
активируется и сначала применяет метод this.capture.open (cameraId), включающий
захват с камеры с номером cameraId. Если захват включён, то функция
frameGrabber с помощью упомянутого ранее таймера захватывает изображение с
потока раз в 33мс (30 кадров в секунду). Тут же производится и перенос
изображения из объекта класса Mat в объект класса Image. Так же в методе
предусмотрен вывод сообщений об ошибке, процедура закрытия программы, изменений
надписи на кнопке, в зависимости от состояния камеры (включена или выключена).

Теперь в FXML-файле нужно создать разметку: в
текстовом виде или с помощью утилиты SceneBuilder. В объекте класса Scene,
который является окном программы в операционной системе Microsoft Windows,
создадим связанные элементы GridPane и BorderPane. Они служат для удобной
расстановки элементов управления по рабочему пространству программы. Так же
необходимо создать объект класса ImageView для показа захватываемого изображения
на экран и объект класса Button для включения и выключения камеры.

Рис. 16. Интерфейс программы в утилитеSceneBuilder

Представленная на Рис. 17 программа — результат
на данный момент. Пока она умеет лишь захватывать видео с камеры и выводить его
на экран. Так же имеется кнопка Start/Stop Camera, которая соответственно
включает и выключает камеру.

Рис. 17. Программа, захватывающая видеосигнал с
камеры

5.3    Обработка изображения

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

В первую очередь, информация о цвете изображения
является избыточной и нет нужды тратить вычислительные мощности на ее
обработку. Тем более, она может не только оказаться ненужной, но и навредить,
уменьшая эффективность распознавания объекта. Так же многие методы библиотеки
OpenCV в принципе могут работать только с двухцветными изображениями. Поэтому в
большинстве проектов по распознаванию объектов изображение переводится в
бинарный вид (черные и белые пиксели без оттенков серого). Часто этот перевод
происходит с помощью семплирования цвета — в таком случае программа запоминает
определенный цвет и переводит все пикселы с цветами, имеющими похожий цвет, в
белый, а все остальные пикселы — в черный. Однако в данном случае этот метод
является излишним, так как предполагается только распознавание темного предмета
на светлом фоне. Итак, как мы знаем, результатом захвата видеосигнала является
объект frame класса Mat. Создадим метод, преобразующий его в двоичный вид и
назовем его imageEdit. В результате всех преобразований над изображением оно
должно максимально полно передавать информацию о положении объекта, то есть в
идеале полностью обозначать объект одним из цветов, а фон — другим.

Первое необходимое действие — перевод
изображения в монохромный вид. Оно производится методом Imgproc.cvtColor,
который принимает три аргумента: объект изображения-источника, объект, в
который записывается преобразованное изображение и тип преобразования. В
качестве первых двух выберем объект frame, а типом преобразования будет
COLOR_BGR2GRAY. Теперь для получения бинарного изображения, нужно произвести
пороговое преобразование. За это отвечает метод Imgproc.threshold, который
имеет пять аргументов: два первых повторяют аналогичные для Imgproc.cvtColor,
третий — величина порога выбора цвета, четвертый — максимальное значение
переменной, а пятый — тип преобразования (метод поддерживает различные
алгоритмы, из который мы выбираем находящийся под номером один, в котором
пиксел перекрашивается в белый цвет, если значение его яркости меньше
порогового значения, и в черный в другом случае). Значение порога не может быть
выбрано раз и навсегда, так как оно должно зависеть от внешних условий, таких
как освещение. Поэтому добавим объект класса Slider в класс controller и в файл
разметки. В нем же устанавливаются максимальное, минимальное значения и
значение по умолчанию для слайдера. Теперь команду slider.getValue можно
вставить в качестве третьего аргумента метода порогового преобразования и
управлять величиной порога непосредственно с экрана.

Теперь изображение имеет бинарный вид, но
необходимо произвести еще одно преобразование — размытие, защищающее от помех.
Оно производится методом Imgproc.medianBlur. Аналогично с пороговым
преобразованием, создадим слайдер, управляющий глубиной размытия и эмпирически
выберем оптимальный тип алгоритма для данной задачи.

Рис. 18. Внешний вид программы после
редактирования изображения

5.4    Выделение контуров

Добившись результата, в котором программа выдает
качественное бинарное изображение руки, можно приступать к анализу этого
изображения. Сейчас для компьютера оно представляет собой двоичный массив, в
котором черные пиксели можно считать за состояние true, а белые — за состояние
false. Наша задача — анализ информации, содержащейся в этом массиве и вынесение
решения согласно этой информации. Алгоритм должен состоять из двух частей:
первая каким-то образом структурирует информацию, содержащуюся в изображении,
представляет всю полезную часть информации в виде определенного блока данных.
Вторая часть сравнивает эти данные с определенными константами, заданными
программистом. Если обработанные данные соответствуют какой-то из констант, то
программа возвращает определенный результат, иначе — сообщение об ошибке или
отсутствии необходимого типа изображения.

Самый примитивный вариант реализации — полностью
игнорировать первый шаг, оставив данные в массиве двоичных переменных и
сравнивать этот массив с заранее записанным в программу изображением
какого-либо жеста. В таком случае используемый метод фактически представляет
собой корреляционный прием: значение каждого пиксела полученного изображения
перемножается со значением соответствующего пиксела заданного изображения.
Затем эти значения суммируются и по этой сумме можно делать вывод о степени
«похожести» двух изображений. Этот метод прост для программирования, но
чрезвычайно неэффективен: из-за различных вариаций захваченного изображения
(масштаб, положение руки, индивидуальные особенности каждого пользователя)
метод будет давать существенные ошибки [11]. Поэтому в данном проекте мы
перейдем от анализа пикселей к анализу контуров.

Стоит обговорить, какую информацию следует
выделить из изображения для дальнейшей обработки. Начать следует с самых
простых жестов — когда плоскость ладони перпендикулярна камере и в качестве
переменных для вычислений используется только длины пальцев (прижат тот или
иной палец или нет) и углы между пальцами. Для получения всей этой информации
достаточно найти на изображении несколько точек, называемых экстремальными:
кончик каждого пальца и каждую перепонку между пальцами. В таком случае
необходимо создать два контура: один точно очерчивает руку, а другой соединяет
крайние точки в многоугольник. В таком случае точки на кончиках пальцев — это и
есть точки, по которым следует построить обводящий контур, а точки между
пальцами можно найти как максимальные отклонения одного контура от другого.
Значит сначала нужно найти и нарисовать оба контура, а затем производить
операции над ними. Далее контур, повторяющий форму руки, будем называть
внутренним (в программе он значится как contour), а контур, соединяющий
экстремальные точки объекта в многоугольник, будем называть обводящим (или
convexHull). Естественно, на изображении может быть целое множество контуров, и
все контуры того или иного вида объединяются в массив, каждый элемент которого
является, в свою очередь, массивом точек или иных типов переменных. Таким
образом, и contour, и convexHull — это двухмерные массивы, и каждый отдельный
контур можно выделить с помощью метода get(i), где i — номер контура [9].

Дополним метод обработки изображения командами,
которые будут находить контуры изображения и графически выделять их на рабочем
поле. Для начала пропишем команду LinkedList<MatOfPoint> contours = new
LinkedList<>. Она создает массив объектов класса MatOfPoint — это тип
данных, хранящий в себе положение множества точек на двухмерном пространстве
изображения. Значит каждый контур будет обрабатываться программой, как массив
точек, находящихся на определенной кривой. Затем с помощью команды
contours.clear убедимся, что заданный массив пуст. Далее применяется метод
findContours из класса Imgproc. Он, используя определенные алгоритмы, выделит
контуры на порогах белого и черного цветов и запишет их в массив contours.
Настройки алгоритма задаются двумя атрибутами: мы выбираем
Imgproc.RETR_EXTERNAL для того, чтобы сообщить программе, что следует
игнорировать внутренние контуры и выделять следует только наружные. Второй
атрибут характеризует сам тип используемого алгоритма и эмпирическим путем было
выявлено, что оптимально подходит Imgproc.CHAIN_APPROX_TC89_KCOS.

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

В конце пропишем метод, рисующий все контуры на
рабочем поле, Imgproc.drawContours. Его параметры не представляют особого
интереса: это поле, в котором нужно рисовать, массив самих контуров, цвет и
толщина линий.

Нахождение обводящего контура — менее
тривиальная задача. Дело в том, что библиотека OpenCV умеет оперировать
множеством типов переменных и массивов, причем разные ее методы требуют
применения разных типов. Поэтому нередко приходится применять весьма нетривиальные
преобразования, особенно реализации библиотеки для языка Java: многие операции,
в отличие от реализаций для других языков программирования, здесь приходится
производить вручную.

Как мы помним, контур, очерчивающий объект,
программа воспринимает как массив класса MatOfPoint, содержащего в себе
координаты каждой точки данного контура. Однако объект обводящего контура
задается совершенно другим путем — метод Imgproc.convexHull находит не контур,
а каждую крайнюю точку изображения как элемент массива MatOfInt, состоящего из
целых чисел, тогда как для графического изображения пригоден только массив
MatOfPoint. Итак, для единственного оставшегося после фильтрации внутреннего
контура нужно создать элемент массива с помощью метода convexHull.add(new MatOfInt),
а также применить сам метод Imgproc.convexHull, отыскивающий обводящий контур
вокруг внутреннего, причем в атрибутах метода должны находиться нулевые
элементы массивов contour и convexHull, которые выделяются с помощью метода
get. В результате массив convexHull наполняется обводящим контуром.

Теперь преобразуем его в необходимый формат,
MatOfPoint. Преобразование идет сначала из формата библиотеки OpenCV MatOfInt в
обычный двухмерный массив переменных типа Point, и только потом непосредственно
в формат MatOfInt. Создадим массив класса Point и назовем его hullpoints. Затем
в каждом шаге цикла от нуля до количества обводящих контуров создадим новую
точку в массиве и еще одним циклом присвоим новое значение с помощью метода
convexHull.get. Массив же в формате MatOfPoint обозначим как hullmop и еще
одним циклом по всем элементам hullpoints проведем преобразование. Происходит
это тремя командами: создается объект MatOfPoint, затем метод
fromArray(hullpoints.get) передает ему соответствующее счетчику значение массива
hullpoints, затем метод hullmop.add записывает этот объект в соответствующий
массив. [5]

Теперь контур доступен для рисования методом
Imgproc.drawContours. Выберем для его отображения другой цвет и толщину линий.
В результате программа имеет данный вид.

В результате программа имеет вид, показанный на
рис. 19.

Рис. 19. Программа после выделения контуров

5.5    Нахождение дефектов
обводящего контура

Теперь найдем так называемые дефекты обводящего
контура. Создадим очередной массив-объект для хранения информации, на сей раз в
формате MatOfInt4, под названием convDef. Этот массив хранит данные
сгруппировано по четыре числа формата Integer. Используется этот формат потому
что для описания каждого дефекта метод выделяет по четыре величины: start, end,
defect, depth.

Рис. 20. Схематичное изображение возвращаемых
значений метода Imgproc.convexityDefects

Для полноценного описания в данном проекте
достаточно точек start, end
и defect. Для удобства создадим так же обычный одномерный массив типа Integer и
назовем его cdList. Методом Imgproc.convexityDefects выделим массив дефектов
обводящего контура. Метод имеет три атрибута: внутренний, контур, обводящий
контур, и объект в который будет записываться информация, convDef. Но формат
IntOfMat4 неудобен для использования и не принимается методами рисования,
поэтому переведем всю информацию в обычный массив data методом
convDef.get(0).toList. Также создадим массив cdList, который наполнится натуральными
числами от нуля до количества дефектов умноженное на четыре. Теперь информация
записана так: в нулевом элементе находится число, характеризующее start первого
дефекта, в первом — end первого дефекта, во втором defect первого дефекта, в
третьем — depth первого дефекта, в четвертом — start второго дефекта и так
далее. Теперь создадим три массива типа Point, в одном из них будут храниться
все точки start, во втором все точки end,
а в третьем — все точки defect.

После этого можно сразу создать цикл, рисующий
все точки из массивов на экране, на этот раз методом Imgproc.circle и
посмотреть на соответствующий результат.

Рис. 21. Вид программы с найденными
экстремальными точками

5.6    Фильтрация точек дефектов

Прежде чем приступать к анализу найденных точек,
необходимо отфильтровать результаты, так как не все из них несут полезную
информацию. Нужно, во-первых, исключить все точки defect, образующие с
соседними точками start и end
слишком большой угол, так как даже максимальный угол отклонения между
указательным и большими пальцами не может превышать π/2
и
эти точки не несут в себе информации о положении пальцев. Во-вторых, нужно
исключить точки start, которые находятся слишком близко друг к другу, так как
алгоритм может ошибочно выделять несколько рядом стоящих точек и один палец
будет считаться за два. И, в-третьих, нужно исключить точки, находящиеся в
непосредственной близости к краю изображения, ведь, как показывает практика,
алгоритм часто дает просчеты и в этих областях. Для работы с каждым из трех
массивов точек создадим еще по два массива — x и y координат соответственно.
Назовем их соответственно startX, startY,endX,
endY, defectX, defectY,
и присвоим им тип double. Наполнить их информацией о соответствующих
координатах каждой точки можно с помощью цикла по всем индексам точек. Далее
условными операторами будем производить фильтрацию. Сделаем внутри цикла
оператор if с тремя условиями прохождения фильтрации. Если координаты точек
defect и start с неким порядковым номером будут отвечать заданным условиям, то
эти точки будут записаны в новые массивы, названные defectFiltered и startFiltered.
Эти условия будут объединены как логические элементы «и», то есть присвоение
случится только если все три условия одновременно будут выполнены.

Первое условие — это достаточно малая величина
угла между двумя точками start и точкой defect, находящейся между ними. В java
и OpenCV не существует стандартной функции для нахождения угла, описанного
тремя точками, поэтому придется сделать ее самостоятельно. По теореме косинусов
можно найти любой угол треугольника, если известно три его стороны. Поэтому построим
воображаемый треугольник из точекstart,defectиendи
вычислим длины его сторон, зная координаты точек. Затем применим теорему
косинусов и запишем значение угла в переменную angle. Теперь можно создать
первое условие — точка будет записываться в конечный массив только когда
значение угла меньше определенного значения. Пока остановимся на значении π/2.

Второе условие — это достаточно большое
расстояние между точками startи
end. Если расстояние
между двумя точками будет меньше определенного значения, то соответствующая
точка start должна
записаться в конечный массив, поэтому с помощью цикла сравним каждую точку start
[i] с точкой end [i]. При сумме квадратов координат больше определенного
значения, точка записывается в конечный массив.

Третье условие наиболее просто в реализации:
необходимо чтобы каждая координата находилась в определенном интервале, это
можно реализовать с помощью оператора «и» (значение должно быть больше нижнего
значения и, в то же время, меньше верхнего значения). Выберем значения отступов
по 30 пикселей с каждого края экрана.

Рис. 22. Вид программы после фильтрации точек
дефектов

5.7    Обработка результатов

Итак, теперь мы имеем достаточно много
информации о положении руки: каждый из пяти пальцев обозначен точкой из
массива, причем номер пальца, считая слева направо, соответствует номеру
элемента массива. Четыре промежутка между пальцами так же обозначены точками из
другого массива. В дальнейшем, обрабатывая эту информацию, можно получать
разнообразные результаты. Для примера усовершенствуем код, чтобы программа
выводила на экран количество не прижатых пальцев, и, если пальцы соединены,
меняла цвет внутреннего контура на красный. Информация о количестве пальцев уже
присутствует в программе — это размер массива startFiltered. Преобразуем ее в
тип данных String и выведем на экран методом Imgproc.putText. Метод для
вычисления углов тоже уже был создан в процессе произведения фильтрации,
поэтому воспользуемся им, только на этот раз суммируем все углы циклом и выведем
их среднее арифметическое. Среднее арифметическое теперь снова можно сравнить с
порогом, только теперь при его значении меньше порога объект класса Scalar,
отвечающий за цвет внутреннего контура, будет менять свои атрибуты. Конечный
результат можно наблюдать на рис. 23.

Рис. 23. Конечный вариант программы

6.      Формирование сигналов и
передача полученных данных

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

6.1    Передача данных через
серийный порт

Для работы с серийным портом будем пользоваться
библиотекой RXTX. Добавим новый класс и назовем его Serial.java. После
импортирования всех необходимых элементов библиотеки в теле класса необходимо
инициализировать объекты классов serialPort, input, output, а также объявить
константы:TIME_OUT=2000
(время ожидания информации),DATA_RATE
= 9600 (скорость передачи серийного порта, должна совпадать с заданной в коде
для Arduino),
PORT_NAMES (массив имен порта для различных операционных систем). Также в
классе должно содержаться несколько методов.

Первый метод носит название initializeи
используется для подготовки порта и инициализации переменных и объектов.
Сначала в нем выбирается подходящее имя порта: в цикле по всем элементам PORT_NAMEсравнивается
значение элемента массива и имени порта, полученного из системы. Если не
требуется мультиплатформерность, то эта часть необязательная и для Windowsдостаточно
задать имя порта как, например, «COM4».

ЗатемвэтомжеметоденужнометодомportId.openоткрытьсерийныйпорти
установить параметры DATA_RATE,DATABITS_8,
STOPBITS_1 и PARITY_NONE.
МетодамиgetInputStreamиgetOutputStream
открываем входящие и исходящие потоки данных. Входящий поток не пригодится, так
как информация будет передаваться только в сторону Arduino,
но можно добавить этот метод для дальнейших усовершенствований. Также включим
так называемое прослушивание событий методом addEventListener
— тогда программа будет реагировать на входящие сигналы.

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

Для отправки данных будет использоваться метод sendSingleByte.
Он принимает один аргумент — байт, который будет передаваться, назовем его myByte.
Методом output.writeзначение
прописывается в серийный порт.

Метод serialEventбудет
вызываться автоматически, когда в серийный порт будут поступать данные. Для
этого нужно в его аргументы поставить событие прихода данных SerialPortEvent.
В этом методе значение считывается методом input.read,
преобразуется из типа byteв
тип и хранится в переменной value.
Также пропишем вывод переменной в консоль, предварительно проверив, входит ли
она в интервал от 0 до 255.

Теперь для класса можно создавать объекты и
вызывать их методы.Создадим объект serialкласса
Serialи запустим метод
его инициализации в функции start.
В методе setClosed,
выполняющемся при закрытии программы, пропишем serial.close.

Теперь закодируем информацию с двух переменных,
получаемых с камеры, в один байт. При разжатых пальцах в этом байте будет
содержатся просто количество отфильтрованных контуров. При сжатых байт будет
содержать значение {1, 1, 1, 1, 1, 1, 1, 1} (127).В конце функции,
анализирующей изображение следует добавить метод, осуществляющий отправку
данных — serial.sendSingleByteи
в аргументах ему задать полученный байт.

Следующий этап — модернизировать код для Arduinoтак,
чтобы он мог принимать данные, отправляемые в серийный порт.Добавим переменную
типа Byteи назовем ее
getByte. В каждом
повторении цикла loopтеперь
можно добавить команду Serial.read,
при условии, если Serial.avaliable.

Известно, что контроллер ArduPilotвоспринимает
сигнал смены режима полета так же, как и сигналы газа, рыскания, тангажа и
крена — в виде сигнала с широтно-импульсной модуляцией. Конкретные режимы,
которые будут переключаться этим сигналом, можно настроить в программе MissionPlanner.Одному
каналу можно присвоить функцию смены до 256 режимов, но чаще всего один канал
на пульте меняет два или три режима. В нашем случае подобный вариант является
оптимальным. Сделаем так, чтобы при сжатых пальцах первый канал передавал байт
11111111, а при разжатых — 0000000. Второй канал будет управляться количеством
пальцев — пусть при сжатых пальцах значение равно 0, при трех разжатых — 86,
при четырех 172, при пяти — 255.

Принятый байт нужно «разложить» обратно на два
сигнала, аналогичными условиями: если принятый байт равен 127, то переменная mode1
типа intравна 255, а
mode2 -нулю, иначе mode
1 равна нулю, а mode2
определяется по вышеописанному методу.

6.2    Передача данных на бортовой
контроллер

Теперь передадим данные на другой
микроконтроллер, который будет находиться на борту квадрокоптера. Для этой цели
будут использоваться два модуля nRF24L01. Это модули радиосвязи, работающие на
частоте 2,4 ГГц и имеющие встроенную библиотеку для платформ Arduino
и RaspberryPI. Модули могут
передавать сигнал на расстоянии до 100 м наскорости до 2 Мб/с. Модуль
взаимодействует с контроллером с помощью интерфейса SPI.

Кроме ножек питания, модуль имеет следующие
выводы:

·        SCK (Serial ClocK) — тактирование
(синхронизация).

·        MOSI / MI (Master
Out Slave In) — входданных.

·        MISO / MO (Master
In Slave Out) — выходданных.

·        CE/SS — Выбор ведомого на шине SPI
из нескольких устройств.

·        SCN — выбор режима приема/передача,
фактически тот же CE.

·        IRQ — выход прерывания,чаще всего не
используется. Необходим для немедленной реакции микроконтроллера при приеме
нового пакета данных.

В код для Arduinoкак
на приемной, так и на передающей стороне, необходимо импортировать библиотеки SPI.h,
nRF24L01.h и RF24.h, а также инициализировать несколько объектов и переменных и
запустить определенные методы.

Сначала создадим объект radioкласса
RF24, в качестве
аргументов он принимает номера выводов Arduino,
через которые идет передача данных. Далее следует объявить массив адресов типа byte,
в этом массиве хранится информация о наименованиях так называемых «труб» —
каналов по которым модуль производит передачу.

В функции setup
нужно активировать модуль методом .begin,
затем задать режим подтверждения приема на случай, если понадобится отправлять
данные с борта на землю методом. Затем методомsetRetries
устанавливается количество попыток передать пакет данных и время между ними.
Метод enableAckPayloadразрешает
отсылку данных в ответ на входящий сигнал.setPayloadSize
устанавливает размер пакета в байтах, (в нашем случае 6 байт).

openReadingPipe
включает для прослушивания «трубу» спредварительно объявленным номером Adress.
setChannel используется для
выбора канала в шестнадцатиричной системе счисления (можно выбрать любой, но на
некоторых частотах могут присутствовать шумы) setPALevelустанавливаетуровень
мощности передатчика. Аргументами могут быть RF24_PA_MIN,
RF24_PA_LOW,
RF24_PA_HIGH
или RF24_PA_MAX.
Метод setDataRateпозволяет
выбрать максимальную скорость обмена информацией. Выбираем 250 кбит/с, так как
при небольшой скорости передачи наблюдается максимальная чувствительность и
дальность. Метод powerUp
сигнализирует о начале работы модуля. Так же для приемника необходимо прописать
метод startListening,
а для передатчика — stopListening.

Код для передачи и приема так же прост, как и в
случае с серийным портом: для передачи используется метод writeобъекта
radio, а для приема —
метод read. Далее в
приемнике можно вывести каждую полученную величину на вывод, поддерживающий
широтно-импульсную модуляцию и направить непосредственно на входы полетного
контроллера.

В прил. 1 приведен полный код программы для
распознавания жестов, в прил. 2 — код программы для передатчика и приемника на
платформе Arduino.

Заключение

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

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

Тем не менее, устройство имеет реальные
перспективы развития и использования в практических целях.

Разработка устройства потребовала навыки в
областях радиофизики, программирования, обработки сигналов, робототехники,
электроники и аэродинамики.

Список использованных источников

1.       Герасимов В.Г.
Электротехника и электроника. Кн. 2. Электромагнитные устройства и
электрические машины. / В.Г. Герасимов, Э.В. Кузнецов, О.В. Николаева. — М:
Энергоатомиздат, 1997. — 288 с.

2.       Корнилов В. А. Система
управления мультикоптером / В.А. Корнилов, Д.С. Молодяков, Ю.А. Синявская. //
Электронный журнал «Труды МАИ». — 2023. — № 62. — С. 1-8.

3.       Ang K.H. PID control
system analysis, design, and technology / K.H. Ang, G. Chong, Y. Li. // IEEE
Transactions on Control Systems Technology. -2005.- № 4. — C. 559-576.

.        Kalman R.E. A new
approach to linear filtering and prediction problems / R.E. Kalman. // Journal
of Basic Engineering. — 1960. — №82.-С.
35-45

5.       ФорсайтД.А.
Компьютерноезрение. Современныйподход. / Д.А. Форсайт, Ж. Понс. — М
:Издательскийдом «Вильямс», 2004. — 982с.

.        Шапиро Л. Компьютерное
зрение. / Л. Шапиро,000 Дж. Стокман. — М : Бином. Лаборатория знаний, 2023. —
752 с.

.        Желтов С.Ю. Обработка и
анализ изображений в задачах машинного зрения. / С.Ю. Желтов [и др.]; ред. С.Ю.
Желтов. — М : Физматкнига, 2023. — 672 с.

9.       G. Bradski.
LearningOpenCV. / GaryBradskiandAdrianKaehler. — M :O’ReillyMedia, Inc, 2008. —
571 c.

.        OpenCVdocumentationindex
[Электронныйресурс]
:сайт.
— URL: https://docs.opencv.org.

.        E. R. Davies. Machine
Vision: Theory, Algorithms, Practicalities. M: — Morgan
Kaufmann, 2004. — 579 c.

12.     IMU-сенсор
на 10 степеней свободы [Электронныйресурс] :сайт. — URL:
http://wiki.amperka.ru/продукты:troyka-imu-10-dof

Приложения

Приложение 1

Main.java

package
sample;

// импортирование
JavaFX-классов

import
javafx.application.Application;

import
javafx.fxml.FXMLLoader;

import
javafx.scene.Parent;

import
javafx.scene.Scene;

import
javafx.scene.layout.BorderPane;

import
javafx.stage.Stage;

// импортирование
библиотеки
OpenCV

import
org.opencv.core.Core;

publicclass Main extends Application {

@Override

publicvoid start(Stage primaryStage) throws Exception{

// связываем
с
FXML-документомloader
= new FXMLLoader(getClass().getResource(«sample.fxml»)); //
load the FXML resourceroot =
FXMLLoader.load(getClass().getResource(«sample.fxml»));

// заголовок
окна.setTitle(«Gesture
Detection 0.1»);

// создаем окно с параметрами высоты и ширины

primaryStage.setScene(new
Scene(root, 517, 441));

// запрет изменения размеров окна.setResizable(false);

// создаем сцену и выводим на экран

primaryStage.show();

}

publicstaticvoid main(String[] args) {

// загружаем
OpenCV.loadLibrary(Core.NATIVE_LIBRARY_NAME);

// запускаем
JavaFX-код(args);

}

}.java

package
sample;

// импортирование средства для буферизации
изображений

import
java.awt.image.BufferedImage;

import
java.awt.image.DataBufferByte;

// импортирование
JavaFX-классов

import
javafx.application.Platform;

import
javafx.beans.property.ObjectProperty;

import
javafx.embed.swing.SwingFXUtils;

import
javafx.scene.image.Image;

// импортирование
библиотеки
OpenCV

import
org.opencv.core.Mat;

publicfinalclass Utils

{

//Конвертирует
Mat-объект
из
OpenCV в
Image из
JavaFX

publicstatic Image mat2Image(Mat frame)

{

try

{

return
SwingFXUtils.toFXImage(matToBufferedImage(frame), null);

}

catch
(Exception e)

{.err.println(«Cannot convert
the Mat obejct: » e);

returnnull;

}

}

// метод для импортирования элементов в
«нить» JavaFX

// взят
из
открытых
источников

publicstatic <T> void onFXThread(final
ObjectProperty<T> property, final T value)

{.runLater(() -> {.set(value);

});

}

// буферизация
изображения

privatestatic BufferedImage matToBufferedImage(Mat original)

{image = null;

int
width = original.width(), height = original.height(), channels =
original.channels();

byte[]
sourcePixels = newbyte[width * height * channels];.get(0, 0,
sourcePixels);

if
(original.channels() > 1)

{= new BufferedImage(width,
height, BufferedImage.TYPE_3BYTE_BGR);

}

else

{= new BufferedImage(width,
height, BufferedImage.TYPE_BYTE_GRAY);

}

finalbyte[] targetPixels = ((DataBufferByte)
image.getRaster().getDataBuffer()).getData();.arraycopy(sourcePixels, 0,
targetPixels, 0, sourcePixels.length);

return
image;

}

}.FXML

<?xml
version=»1.0″ encoding=»UTF-8″?>

<?import
javafx.scene.text.*?>

<?import
java.lang.*?>

<?import
javafx.geometry.*?>

<?import
javafx.scene.control.*?>

<?import
javafx.scene.image.*?>

<?import
javafx.scene.layout.*?>

<?import
javafx.geometry.Insets?>

<?import
javafx.scene.control.Button?>

<?import
javafx.scene.control.Slider?>

<?import
javafx.scene.image.ImageView?>

<?import
javafx.scene.layout.BorderPane?>

<?import
javafx.scene.layout.ColumnConstraints?>

<?import
javafx.scene.layout.GridPane?>

<?import
javafx.scene.layout.RowConstraints?>

<GridPane
alignment=»center» hgap=»10″ maxHeight=»600.0″
maxWidth=»726.0″ minHeight=»426.0″
minWidth=»517.0″ prefHeight=»441.0″
prefWidth=»517.0″ vgap=»10″
xmlns=»http://javafx.com/javafx/8″
xmlns:fx=»http://javafx.com/fxml/1″ fx:controller=»sample.Controller»>

<columnConstraints>

<ColumnConstraints/>

</columnConstraints>

<rowConstraints>

<RowConstraints/>

</rowConstraints>

<children>

<BorderPane
maxHeight=»600.0″ maxWidth=»700.0″
minHeight=»438.0″ minWidth=»530.0″
prefHeight=»438.0″ prefWidth=»530.0″>

<bottom>

<Button
fx:id=»button» mnemonicParsing=»false»
onAction=»#startCamera» text=»Start Camera»
BorderPane.alignment=»CENTER»>

<BorderPane.margin>

<Insets
bottom=»15.0″ />

</BorderPane.margin>

</Button>

</bottom>

<center>

<ImageView
fx:id=»currentFrame» fitHeight=»325.0″
fitWidth=»431.0″ pickOnBounds=»true»
preserveRatio=»true» BorderPane.alignment=»CENTER»>

<BorderPane.margin>

<Insets
right=»-300.0″ />

</BorderPane.margin></ImageView>

</center>

<top>

<Slider
fx:id=»slider» max=»300.0″ maxWidth=»400.0″
minWidth=»100.0″ prefHeight=»14.0″
prefWidth=»100.0″ value=»150.0″
BorderPane.alignment=»TOP_RIGHT»>

<BorderPane.margin>

<Insets
right=»5.0″ top=»10.0″ />

</BorderPane.margin></Slider>

</top>

<left>

<Label
alignment=»TOP_LEFT» text=»Threshold»
BorderPane.alignment=»TOP_LEFT»>

<font>

<Font
size=»17.0″ />

</font>

<padding>

<Insets
left=»30.0″ top=»-20.0″ />

</padding>

</Label>

</left>

<right>

<Slider
fx:id=»sliderBlur» max=»20.0″ min=»2.0″
prefHeight=»14.0″ prefWidth=»402.0″ value=»5.0″
BorderPane.alignment=»CENTER»>

<padding>

<Insets
right=»5.0″ top=»-350.0″ />

</padding>

</Slider>

</right>

</BorderPane>

<Label
text=»Blur»>

<GridPane.margin>

<Insets
left=»50.0″ top=»-365.0″ />

</GridPane.margin>

<font>

<Font
size=»17.0″ />

</font>

</Label>

</children>

</GridPane>.java

package
sample;

// импортирование
JavaFX-классов

import
javafx.event.ActionEvent;

import
javafx.fxml.FXML;

import
javafx.scene.control.Button;

import
javafx.scene.control.Slider;

import
javafx.scene.image.Image;

import
javafx.scene.image.ImageView;

// импортирование
библиотеки
OpenCV

import
org.opencv.core.*;

import
org.opencv.imgproc.Imgproc;

import
org.opencv.videoio.VideoCapture;

// импортирование
утилит
Java

import
java.util.ArrayList;

import
java.util.LinkedList;

import
java.util.List;

import
java.util.concurrent.Executors;

import
java.util.concurrent.ScheduledExecutorService;

import
java.util.concurrent.TimeUnit;

publicclass Controller {

@FXML

private
Button button; // кнопка

@FXML

private
Slider slider;
// слайдер выбора порога

@FXML

private
Slider sliderBlur; //слайдер
размытия

@FXML

private
ImageView currentFrame; //объект просмотра изображения

private
ScheduledExecutorService timer; // таймер
для
контроля
видеострима

private
VideoCapture capture = new VideoCapture(); // объект
OpenCV, захватывающий
видео

privateboolean cameraActive = false; // флаг,
меняющий
поведение
кнопки

privatestaticint cameraId = 0; // id используемой
камеры

Serial serial = new Serial(); // объект
для работы с серийным портом

// событие
нажатия
кнопки

@FXML

protectedvoid startCamera(ActionEvent event)

{

if (!this.cameraActive)
//если камера в данный момент выключена

{

// начинаем захват видео

this.capture.open(cameraId);

// видео
доступно?

if
(this.capture.isOpened())

{

this.cameraActive
= true; //флажок поднимается

// захватываем 30 кадров в секунду (33 мс)

Runnable frameGrabber = new
Runnable()

{

@Override

publicvoid run()

{

// захватываем отдельный кадрframe =
grabFrame();

// теперь
в
изображение
егоimageToShow
= Utils.mat2Image(frame);(currentFrame, imageToShow);

}

};

// настройка
таймера

this.timer
= Executors.newSingleThreadScheduledExecutor();

this.timer.scheduleAtFixedRate(frameGrabber,
0, 33, TimeUnit.MILLISECONDS);

// надпись
на
кнопке
меняется

this.button.setText(«Stop
Camera»);.initialize();

}

else

{

// ошибка.err.println(«Impossible
to open the camera connection…»);

}

}

else

{

// выключаем
камеру

this.cameraActive
= false;

// меняем
надпись
на
кнопке

this.button.setText(«Start
Camera»);

// останавливаем таймер

this.stopAcquisition();

}

}

//функция, захватывающая кадр из видео,
возвращает переменную frame

private
Mat grabFrame()

{

// создаем
объект
фреймаframe
= new Mat();

// объявляем переменную, которая будет
отправляться по серийному порту

Byte sendByte = 0;

// если
захват
работает

if
(this.capture.isOpened())

{

// прочитать
текущий
кадр

this.capture.read(frame);

// если кадр не пустой, обрабатываем его

if
(!frame.empty())

{

// переводим в оттенки серого

Imgproc.cvtColor(frame, frame,
Imgproc.COLOR_BGR2GRAY);

// переводим
в
черное
и
белое.threshold(frame,
frame, slider.getValue(), 5000, 1);

// сглаживаем
(избавляемся
от
шумов).medianBlur(frame,
frame, ((int) sliderBlur.getValue() * 2) — 1);

// объявляем
массив
контуров
и
очищаем<MatOfPoint>
contours = new LinkedList<>();.clear();

// вспомогательный
массивhierarchy
= new Mat();

// ищем
контуры.findContours(frame,
contours, hierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_KCOS);

// Выделяем
самый
большой
контур<MatOfPoint>
finalContours = new LinkedList<>();.addFirst(contours.get(0));

for
(int j = 0; j < contours.size(); j )

{

if
(Imgproc.contourArea(contours.get(j)) >
Imgproc.contourArea(finalContours.getFirst()))

{.removeFirst();.addFirst(contours.get(j));

}

}

// Находим
обводящие
контуры<MatOfInt>
convexHull = new LinkedList<>();.add(new
MatOfInt());.convexHull(finalContours.get(0), convexHull.get(0));

// конвертируем MatOfInt в Point для рисования
обводок

// цикл
по
всем
контурам<Point[]>
hullpoints = new ArrayList<>();

for
(int j = 0; j < convexHull.size(); j )

{[] points = new
Point[convexHull.get(j).rows()];

// цикл
по
всем
выпуклостям

for
(int k = 0; k < convexHull.get(j).rows(); k )

{

int
index2 = (int) convexHull.get(j).get(k, 0)[0];[k] = new
Point(finalContours.get(j).get(index2, 0)[0], finalContours.get(j).get(index2,
0)[1]);

}.add(points);

}

// конвертируем
массив
Point в
MatOfPoint<MatOfPoint> hullmop = new ArrayList<>();

for
(int j = 0; j < hullpoints.size(); j )

{m = new
MatOfPoint();.fromArray(hullpoints.get(j));

hullmop.add(m);

}

// ищем дефекты контуров

List<Integer>
cdList;<MatOfInt4> convDef = new LinkedList<>();.add(new
MatOfInt4());

// функция
ищет
дефекты.convexityDefects(finalContours.get(0),
convexHull.get(0),.get(0));

// оставляем
только
первый=
convDef.get(0).toList();data[] = finalContours.get(0).toArray();

// разбираем дефекты на состанвые точкиdefect[]
= new Point[20];

Point start[] = new
Point[20];end [] = new Point [20];

int
defectCounter = 0;

int
defectFilteredCounter = 0;

// переносим
в
массив
точек

for
(int j = 0; j < cdList.size(); j = j 4)

{[defectCounter] = data[cdList.get(j
2)];[defectCounter] = data[cdList.get(j 1)];[defectCounter] =
data[cdList.get(j)]; ;

}

// массивы
для
отфильтрованных
точекdefectFiltered[]
= new Point[15];

Point startFiltered[] = new Point[15];

// переменная для угла и ее счетчик

double
angle;

double
angleCount = 0;

// фильтрация

for
(int i = 0; i < defectCounter; i )

{

// координаты,
по
которым
считается
угол

double
dx1 = Math.abs (start[i].x — defect[i].x);

double
dy1 = Math.abs (start[i].y — defect[i].y);

double
dx2 = Math.abs (defect[i].x — end[i].x);

double
dy2 = Math.abs (defect[i].y — end[i].y);

// вычисление
угла=
(dx1*dx2 dy1*dy2)/Math.sqrt((dx1*dx1 dy1*dy1)*(dx2*dx2 dy2*dy2));

// вычисление расстояния между соответствующими
точками end и start

double
lenght = Math.sqrt (Math.pow((start[i].x — end[i].x), 2) Math.pow((start[i].y
— end[i].y), 2);

// условие
с
подобранными
значениями

if
((start[i].y < 450 || start[i].y > 50) && angle > 4.6
&& lenght < 5.1)

{

// записываем
точки
в
массив[defectFilteredCounter]
= defect [i];[defectFilteredCounter] = start [i];

// служит
количеством
отфильтрованных
контуров ;=
angleCount angle;

}

}

//выводим
количество
стартовnumberOfFingers
= Integer.toString(defectFilteredCounter);textPoint = new Point (20,
80);

// среднее
арифметическое
углов

double
angleThresh = angleCount/defectFilteredCounter;

//переводим
в
цвет.cvtColor(frame,
frame, Imgproc.COLOR_GRAY2BGR);

// если среднее арифметическое угла больше
определенного значения, цвет меняется

if
(angleThresh > 0.75 || defectFilteredCounter <= 5)

{.putText(frame, numberOfFingers,
textPoint, 1, 6, new Scalar(0, 255, 0), 4);.drawContours(frame,
finalContours, -1, new Scalar(255, 0, 0), 5);

}

else

{.drawContours(frame, finalContours,
-1, new Scalar(0, 0, 255), 5);

}

// обводящий
контур.drawContours(frame,
hullmop, -1, new Scalar(0, 0, 255), 2);

// точки старт и дефект обозначаются кругами
разных цветов

for
(int i = 0; i < defectFilteredCounter; i )

{.circle(frame, defectFiltered [i],
5, new Scalar(0, 255, 0), 5);

}

for
(int i = 0; i < defectFilteredCounter; i )

{.circle(frame, startFiltered [i],
5, new Scalar(0, 255, 255), 5);

}

//делаем
байт

if
(angleThresh > 0.75 || defectFilteredCounter <= 5)

{= (byte)127;

}

if
(angleThresh > 0.75)

{= (byte)defectFilteredCounter;

}

}

}.sendSingleByte(sendByte);

// возвращаем
обработанный
кадр

return
frame;

}

// остановить захват и отпустить все ресурсы

privatevoid stopAcquisition()

{

if
(this.timer!=null && !this.timer.isShutdown())

{

try

{

// остановить
таймер

this.timer.shutdown();

this.timer.awaitTermination(33,
TimeUnit.MILLISECONDS);

}

catch
(InterruptedException e)

{.err.println(«Exception in
stopping the frame capture, trying to release the camera now… » e);

}

if
(this.capture.isOpened())

{

// отпустить
камеру

this.capture.release();

}

}

// обновляем {@link ImageView} в основной тред
JavaFX

privatevoid updateImageView(ImageView view, Image image)

{.onFXThread(view.imageProperty(),
image);

}

// При закрытии приложения останавливаем захват:

protectedvoid setClosed()

{

this.stopAcquisition();.close();

}

}.java

package
sample;

// импортирование средств ввода-вывода

import
gnu.io.CommPortIdentifier;

import
gnu.io.SerialPort;

import
gnu.io.SerialPortEvent;

import
gnu.io.SerialPortEventListener;

import
java.io.InputStream;

import
java.io.OutputStream;

import
java.util.Enumeration;

publicclass Serial implements SerialPortEventListener

{

// создание объектов и константserialPort;

private
InputStream input;

private
OutputStream output;

privatestaticfinalint TIME_OUT = 2000;

privatestaticfinalint DATA_RATE = 115200;

privatestaticfinal String PORT_NAMES[] = {

«/dev/tty.usbserial-A9007UX1»,
// Mac OS X

«/dev/ttyUSB0», // Linux

«COM4», // Windows

};

// инициализация

publicvoid initialize() {portId = null;portEnum =
CommPortIdentifier.getPortIdentifiers();

while
(portEnum.hasMoreElements()) {currPortId = (CommPortIdentifier)
portEnum.nextElement();

for
(String portName : PORT_NAMES) {

if
(currPortId.getName().equals(portName)) {= currPortId;

break;

}

}

}

try
{

// открываем
серийный
порт=
(SerialPort) portId.open(this.getClass().getName(),TIME_OUT);

// устанавливаем
параметры
порта.setSerialPortParams(DATA_RATE,.DATABITS_8,.STOPBITS_1,.PARITY_NONE);

// открываем входные и выходные потоки=
serialPort.getInputStream();= serialPort.getOutputStream();

// добавляем слушаетелей событий

serialPort.addEventListener(this);.notifyOnDataAvailable(true);

} catch (Exception e)
{.err.println(e.toString());

}

}

// метод
для
закрытия
порта

publicsynchronizedvoid close() {

if
(serialPort != null) {.removeEventListener();

serialPort.close();

}

}

// метод для отправки одного байта

publicvoid sendSingleByte(byte myByte){

try
{.write(myByte);.flush();

} catch (Exception e)
{.err.println(e.toString());

}

}

// метод для приема данных (не используется)

publicsynchronizedvoid serialEvent (SerialPortEvent oEvent) {

if
(oEvent.getEventType() == SerialPortEvent.DATA_AVAILABLE) {

try
{

int
myByte=input.read();

// перевод из типа byte в int

int
value = myByte & 0xff;

if(value>=0
&& value<256){.out.println(value);

}

} catch (Exception e)
{.err.println(e.toString());

}

}

}

}

Приложение
2

передатчик

#include <NewPing.h>

// объявление пинов для дальномера

#define
TRIGGER_PIN
7

#define
ECHO_PIN
8

// максимальная дистанция в сантиметрах, на
которую реагирует дальномер

#define MAX_DISTANCE 200

// создаем объект дальномера

NewPing sonar(TRIGGER_PIN, ECHO_PIN,
MAX_DISTANCE);

// библиотеки для радиосвязи

#include <SPI.h> // библиотека для работы
с шиной SPI

#include «nRF24L01.h» // библиотека
радиомодуля

#include «RF24.h» // ещё библиотека
радиомодуля

// создать объект на пинах 9 и 10radio(9, 10);

//возможные номера труб

byte
address[][6] = {«1Node»,
«2Node»,
«3Node»,
«4Node»,
«5Node»,
«6Node»};

// Инициализация функции фильтрации

float
kalman (float val);

// функции обработки кнопок

void
buttons();

// Объявление переменных для фильтра Калмана

float
varVolt = 21.47; // среднее отклонение (ищем в excel)

float
varProcess = 0.02; // скорость реакции на изменение (подбирается вручную)

float
Pc = 0.0;

float
G = 0.0;

float
P = 1.0;

float
Xp = 0.0;

float
Zp = 0.0;

float
Xe = 0.0;

// Конец объявления переменных для фильтра
Калмана

// Переменные для кнопки

int
buttonPin = 6;

boolean button1S; // храним
состояния
кнопок
(S — State)button1F; // флажки кнопок
(F — Flag)button1R; // флажки кнопок
на
отпускание
(R — Release)button1P; // флажки
кнопок
на
нажатие
(P — Press)button1H; // флажки кнопок
на
удержание
(H — Hold)button1D; // флажки кнопок
на
двойное
нажатие
(D — Double)button1DP; // флажки
кнопок
на
двойное
нажатие
и
отпускание
(D — Double Pressed)

#define double_timer 100 // время (мс),
отведённое на второе нажатие

#define hold 500 // время (мс), после которого
кнопка считается зажатой

#define debounce 80 // (мс), антидребезгlong
button1_timer; // таймер последнего нажатия кнопки

unsigned long button1_double;
// таймер
двойного
нажатия
кнопки

// библиотека для работы I²C

#include <Wire.h>

// библиотека для работы с модулями IMU

#include <TroykaIMU.h>

// множитель фильтра Магвика

#define BETA 0.22

// создаём объект для работы с акселерометромaccel;

// создаём объект для работы с гироскопомgyro;

// создаём объект для работы с компасомcompass;

// создаём объект для фильтра Магвикаfilter;

// переменные для данных с гироскопа,
акселерометра и компаса

float
gx, gy, gz, ax, ay, az, mx, my, mz;

// получаемые
углы
ориентации
(Эйлера)

float
yaw, pitch, roll, throttle, yawCenter = 0;

// обработанные
значения
каналов

int
sendYaw, sendPitch, sendThrottle, sendRoll;

// переменная для хранения частоты выборок
фильтра

float
fps = 100;

// перменные для приема из серийного порта

byte getByte;

int
mode1, mode2;

//флаг готовности к передаче

bool
check = false;

// калибровочные значения компаса

// полученные в калибровочной матрице из примера
«compassCalibrateMatrixx»

constdouble compassCalibrationBias[3] = {

.21,

.214,

.236

};

constdouble compassCalibrationMatrix[3][3] = {

{1.757, 0.04, -0.028},

{0.008, 1.767, -0.016},

{-0.018, 0.077, 1.782}

};

void
setup()

{

// задание
режимов
пинов

//пин
для
кнопки

pinMode(buttonPin, INPUT_PULLUP);

// открываем последовательный порт

Serial.begin(115200);.println(«Begin
init…»);

// инициализация акселерометра.begin();

// инициализация гироскопа.begin();

// инициализация компаса.begin();

// инициализация радиомодуля

// активировать модуль.begin();

// режим подтверждения приёма, 1 вкл 0
выкл.setAutoAck(1);

// (время между попыткой достучаться, число
попыток).setRetries(0, 15);

// разрешить отсылку данных в ответ на входящий
сигнал.enableAckPayload();

// размер пакета, в байтах.setPayloadSize(32);

// открываем канал для передачи данных для
«трубы» 0

radio.openWritingPipe(address[0]);

// выбираем
канал.setChannel(0x60);

// уровень мощности передатчика

radio.setPALevel
(RF24_PA_MAX);

// скорость обмена.setDataRate (RF24_250KBPS); .

// начать работу.powerUp();

// модуль не прослушивает эфир, а передает

radio.stopListening();

// калибровка
компаса.calibrateMatrix(compassCalibrationMatrix,
compassCalibrationBias);

// выводим сообщение об удачной
инициализации.println(«Initialization completed»);

}

void
loop()

{

// опрос
кнопокS
= digitalRead(buttonPin);

//отработка кнопок();

// отработка режимов кнопки

if (button1P)

{

// переключается режим готовности=
!check;.println(«pressed»);

// центрируется рыскание= yaw;P = 0;

}

// канал газа получает данные с дальномера

throttle = sonar.ping();

// данные
фильтруются=
kalman(throttle);

// запоминаем
текущее
времяlong
startMillis = millis();

// считываем данные с акселерометра в единицах G

accel.readGXYZ(&ax, &ay,
&az);

// считываем данные с гироскопа в радианах в
секунду.readRadPerSecXYZ(&gx, &gy, &gz);

// считываем данные с компаса в Гауссах

compass.readCalibrateGaussXYZ(&mx,
&my, &mz);

// устанавливаем
коэффициенты
фильтра.setKoeff(fps,
BETA);

// обновляем
входные
данные
в
фильтр.update(gx,
gy, gz, ax, ay, az, mx, my, mz);

// получение углов yaw, pitch и roll из фильтра

yaw = filter.getYawDeg();=
filter.getPitchDeg();

roll = filter.getRollDeg();

// преобразование данных

// рыскание

sendYaw = -3.175*(yaw — yawCenter);

if
(sendYaw >= 127) sendYaw = 127;

if
(sendYaw <= -127) sendYaw = -127;

// газ=
0.159375 * throttle — 63.75;

if
(throttle >= 2000) sendThrottle = 255;

if
(throttle <= 400) sendThrottle = 0;

// тангаж

if
(pitch >= -180 && pitch < -140) sendPitch = 3.175 * pitch
571.5;

if
(pitch > 140 && pitch <= 180) sendPitch = 3.175 * pitch — 571.5;

if
(pitch >= -140 && pitch < 0) sendPitch = 127;

if
(pitch <= 140 && pitch > 0) sendPitch = -127;

// крен

if
(roll >= — 180 && roll < -140) sendRoll = -3.175 * roll — 571.5;

if
(roll > 140 && roll <= 180) sendRoll = -3.175 * roll 571.5;

if
(roll <= 140 && roll > 0) sendRoll = 127;

if
(roll >= -140 && roll < 0) sendRoll = -127;

// выводим полученные углы в serial-порт

if
(check == false)

{.print(«!
«);.print(«yaw: «);.print(yaw);.print(«tt»);.print(«pitch:
«);.print(pitch);.print(«tt»);.print(«roll:
«);.print(roll);.print(«tt»);.print(«throttle:
«);.println(throttle);

}

else

{.print(«yaw:
«);.print(sendYaw);.print(«tt»);.print(«pitch:
«);.print(sendPitch);.print(«tt»);.print(«roll:
«);.print(sendRoll);.print(«tt»);.print(«throttle:
«);.println(sendThrottle);

}

// принимаем данные из серийного порта

if
(Serial.available())

{= (byte)Serial.read();

}

// «разбираем» байт на соответсвующие
величины

if
((int)getByte == 127) {= 255;= 0;

}

else
{= 0;= (int)getByte * 86;

if (mode2 >
255) {= 255;

}

}

// вычисляем затраченное время на обработку
данных

unsigned long deltaMillis =
millis() — startMillis;

// вычисляем частоту обработки фильтра= 1000 /
deltaMillis;

// при готовности отправляем данные на приемник

if
(check == true)

{.write(&sendThrottle, sizeof(sendThrottle));.write(&sendYaw,
sizeof(sendYaw));.write(&sendPitch, sizeof(sendPitch));.write(&sendRoll,
sizeof(sendRoll));.write(&mode1, sizeof(mode1));.write(&mode2,
sizeof(mode2));

}(5);

}

//функция
фильтрации

float
kalman (float val) {= P varProcess;= Pc/(Pc varVolt);= (1-G)*Pc;=
Xe;= Xp;

// «фильтрованное» значение

Xe = G*(val-Zp) Xp;

return(Xe);

}

void
buttons() {

// нажатие (с антидребезгом)

if
(button1S && !button1F && millis() — button1_timer >
debounce) {

button1F = 1;_timer = millis();

}

// если отпустили до hold, считать отпущенной

if
(!button1S && button1F && !button1R && !button1DP
&& millis() — button1_timer < hold) {R = 1;F = 0;_double = millis();

}

// если отпустили и прошло больше double_timer,
считать 1 нажатием

if
(button1R && !button1DP && millis() — button1_double >
double_timer) {

button1R = 0;P = 1;

}

// если отпустили и прошло меньше double_timer и
нажата снова, считать что нажата 2 раз

if
(button1F && !button1DP && button1R && millis() —
button1_double < double_timer) {

button1F = 0;R = 0;DP = 1;

}

// если была нажата 2 раз и отпущена, считать
что была нажата 2 раза

if
(button1DP && millis() — button1_timer < hold) {

button1DP = 0;D = 1;

}

// Если удерживается более hold, то считать
удержанием

if
(button1F && !button1D && !button1H && millis() —
button1_timer > hold) {

button1H = 1;

}

// Если отпущена после hold, то считать, что
была удержана

if
(!button1S && button1F && millis() — button1_timer > hold)
{F = 0;H = 0;

}

}приемник

// библиотеки
для
радиосвязи

#include <SPI.h> // библиотека для работы
с шиной SPI

#include «nRF24L01.h» // библиотека
радиомодуля

#include «RF24.h» // ещё библиотека
радиомодуля

// создать объект на пинах 7 и 8radio(7, 8);

//возможные номера труб

byte
address[][6] = {«1Node»,
«2Node»,
«3Node»,
«4Node»,
«5Node»,
«6Node»};

// инициализация выходов

#define THROTTLE_PIN 3

#define YAW_PIN 5

#define PITCH_PIN 6

#define ROLL_PIN 9

#define MODE1_PIN 10

#define MODE2_PIN 11

void
setup

{

// открываем последовательный
порт.begin(115200);

// инициализация радиомодуля

// активировать модуль.begin();

// режим подтверждения приёма, 1 вкл 0
выкл.setAutoAck(1);

// время между попыткой достучаться, число
попыток.setRetries(0, 15);

// разрешить отсылку данных в ответ на входящий
сигнал.enableAckPayload();

// размер пакета, в байтах.setPayloadSize(32);

// открываем канал для передачи данных для
«трубы» 0

radio.openWritingPipe(address[0]);

// выбираем
канал.setChannel(0x60);

// уровень мощности передатчика

radio.setPALevel
(RF24_PA_MAX);

// скорость обмена.setDataRate (RF24_250KBPS); .

// начать работу.powerUp();

// модуль не прослушивает эфир, а передает

radio.stopListening();

// калибровка
компаса.calibrateMatrix(compassCalibrationMatrix,
compassCalibrationBias);

// выводим сообщение об удачной
инициализации.println(«Initialization completed»);

// приемный модуль начинает прослушивать
эфир.startListening();

}

void
loop() {pipeNo, sendThrottle, sendYaw, sendPitch, sendRoll, mode1, mode2;

// слушаем эфир со всех «труб»

while(
radio.available(&pipeNo)){

{.read(&sendThrottle, sizeof(sendThrottle));.read(&sendYaw,
sizeof(sendYaw));.read(&sendPitch, sizeof(sendPitch));.read(&sendRoll,
sizeof(sendRoll));.read(&mode1, sizeof(mode1));.read(&mode2,
sizeof(mode2));

}

//выводим
данные(3,
sendThrottle);(5, sendYaw);(6, sendPitch);(9, sendRoll);(10, mode1);(11,
mode2);

}

}

Вывод уравнений движения

Ориентация квадрокоптера в пространстве задается тремя углами: рысканья —

ψ

, тангажа —

θ

, крена —

φ


Они вместе составляют вектор

Позиция устройства в инерциальной система отсчета задается радиус-вектором

Матрица перехода из системы координат квадрокоптера в инерциальную систему координат имеет следующий вид

Сила тяги, производимая каждым из четырех двигателей равна


Здесь

ωi

− угловая скорость двигателя, а

b

– коэффициент пропорциональности.


Теперь мы можем записать дифференциальное уравнение, описывающее ускорение квадрокоптера по вертикальной оси.

Сразу же запишем второе дифференциальное уравнение

Здесь

I

– матрица инерции,

M

– вращающий момент, приложенный к квадрокоптеру,

MG

– гироскопический момент.


Вектор

M

задается следующим образом:

Здесь

d

— коэффициент лобового сопротивления,

L

– длина плеча.


Гироскопические моменты, вызванные поворотом объекта с вращающимися роторами двигателей записываются так:









Эта система из 9 уравнений как раз и описывает динамику системы.

Смотрите про коптеры:  Квадрокоптер Phantom 2 DJI Vision Plus – полный обзор цена и где купить
Оцените статью
Радиокоптер.ру
Добавить комментарий