ПРОГРАММНЫЙ КОМПЛЕКС АЛГОРИТМОВ АВТОНОМНОГО ОПРЕДЕЛЕНИЯ ПАРАМЕТРОВ УГЛОВОЙ ОРИЕНТАЦИИ БЕСПИЛОТНЫХ ЛЕТАТЕЛЬНЫХ АППАРАТОВ Текст научной статьи по специальности «Медицинские технологии»

Труды МАИ. 2022. № 124 Trudy MAI, 2022, no. 124

Научная статья УДК 004.94

DOI: 10.34759/Ы-2022-124-17

ПРОГРАММНЫЙ КОМПЛЕКС АЛГОРИТМОВ АВТОНОМНОГО ОПРЕДЕЛЕНИЯ ПАРАМЕТРОВ УГЛОВОЙ ОРИЕНТАЦИИ БЕСПИЛОТНЫХ ЛЕТАТЕЛЬНЫХ АППАРАТОВ

Павел Григорьевич Ермаков1, Александр Андреевич Гоголев2:

1,2Государственный научно-исследовательский институт авиационных систем,

Москва, Россия

1pavel-ermakov-1998@mail.ru

2kirbizz8@yandex.щ:

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

Ключевые слова: навигационные алгоритмы, имитационное моделирование, беспилотный летательный аппарат, акселерометры, магнитометры, датчики угловой скорости, фильтр Махони, фильтр Маджвика, комплементарный фильтр Для цитирования: Ермаков П.Г., Гоголев А.А. Программный комплекс алгоритмов автономного определения параметров угловой ориентации беспилотных летательных аппаратов // Труды МАИ. 2022. № 124. DOI: 10.34759/Ы-2022-124-17

SOFTWARE COMPLEX OF ALGORITHMS FOR AUTONOMOUS DETERMINATION OF THE ANGULAR ORIENTATION PARAMETERS

OF UNMANNED AERIAL VEHICLES

Pavel G. Ermakov1, Alexander A. Gogolev2:

1,2State Institute of Aviation Systems, Moscow, Russia 1pavel-ermakov-1998@maiLru 2kirbizz8 @yandex.ru:

Abstract. The article "Software complex of algorithms for autonomous determination of the angular orientation parameters of unmanned aerial vehicles" by P.G.Ermakov and A.A.Gogolev compares three approaches used for an Attitude and Heading Reference System (AHRS), namely Mahony, Madgwick and Complementary filters.

For attitude determination on UAV's board widely used a magnetic and inertial measurement unit (MIMU). MIMU consists of a 3-axis MEMS gyroscope, accelerometer and magnetometer. The accelerometer measures the acceleration of a UAV, the gyrosocope measures the angular rate of a moving object, and the magnetometer measures the Earth's magnetic field.

So, Mahony uses a proportional and integral controller to correct the gyroscope bias, Madgwick uses the gradient-descent algorithm. Both approaches use a quaternion representation, which a four-dimensional complex number representing the orientation of an object. A Complementary filter is proposed that combines accelerometer output for low frequency attitude estimation with integrated gyroscope output for high frequency estimation. Madgwick obtains better heading orientation than Mahony and Complementary AHRS approach in respect of the root mean square error (RMSE) of the Euler angles when compared to the motion capture system.

Keywords: fusion algorithms, simulation modelling, unmanned aerial vehicle, magnetometers, accelerometers, gyros, Mahony filter, Madgwick filter, Complementary filter For citation: Ermakov P.G., Gogolev A.A. Software complex of algorithms for autonomous determination of the angular orientation parameters of unmanned aerial vehicles. Trudy MAI, 2022, no. 124. DOI: 10.34759/trd-2022-124-17

Введение

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

Сегодня наиболее бурное развитие получают БЛА малой массы (до 500 кг), при проектировании которых ставится задача по снижению стоимости и выполнению полетов в урбанизированной среде для чего требуется решать навигационную задачу БЛА на высоком уровне с использованием инерциальных МЭМС датчиков и приемников сигналов ГНСС низкого класса точности за счет применения алгоритмов комплексной обработки информации (Рисунок 1). Требуется определить собственное пространственное положение БЛА [1], проекции вектора скорости и угловую ориентацию БЛА.

ГЛОНАССI

| 145-232

Скорость. координаты, куроаадй увд1, иреуя

Рисунок 1 - Типовой навигационный контур БЛА

Определение угловой ориентации малогабаритного БЛА (без использования корректирующих сигналов наземных навигационных систем) может быть осуществлено на базе бортовых микроэлектромеханических (МЭМС) датчиков: 3-х осевой акселерометр, 3-х осевой датчик угловой скорости и 3-х осевой магнитометр. Выходная информация МЭМС датчиков низкого класса точности содержит помимо вектора истинных измерений совокупность ошибок, таких как внутренние шумы датчиков (возникающие, например, из-за нестабильности внешней среды или напряжения), смещения нулевого сигнала, дрейфы нуля, которые затрудняют решение задачи автономного определения параметров угловой ориентации БЛА [2,3].

Традиционно для повышения точности определения угловой ориентации БЛА используются следующие алгоритмы комплексной обработки информации:

> Фильтр Калмана [4];

> Метод наименьших квадратов [5];

> Метод максимального правдоподобия [6];

> Фильтр частиц [7];

> Фильтр Махони [8];

> Фильтр Маджвика [9];

> Комплементарный фильтр [10].

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

Для данных алгоритмов общим входным вектором является вектор вида:

^ =

ау

где:

> а)] - вектор показаний 3-х осевого ДУСа, размер [3 х 1];

> Оу - вектор показаний 3-х осевого акселерометра, размер [3 х 1];

> гпу - вектор показаний 3-х осевого магнитометра, размер [3 х 1];

> ] - текущая итерация работы алгоритма автономного определения угловой ориентации БЛА.

Фильтр Махони

Фильтр Махони [11] осуществляет коррекцию показаний 3-х осевого ДУСа с помощью измерений 3-х осевого акселерометра, 3-х осевого магнитометра и пропорционально - интегрального регулятора [12]. Функциональная схема работы фильтра Махони представлена ниже.

Рисунок 2 - Функциональная схема работы фильтра Махони

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

> анорм у - нормированный вектор показаний 3-х осевого акселерометра, размер

[3 x 1];

> тнорм у - нормированный вектор показаний 3-х осевого магнитометра, размер

[3 x 1];

^ - оценка кватерниона ориентации БЛА, размер [4 x 1];

> (¡норм у - нормированная оценка кватерниона БЛА, размер [4 x 1];

> (¡норм - нормированная оценка кватерниона БЛА на предыдущей итерации работы алгоритма автономного определения угловой ориентации БЛА, размер

[4 х 1].

Обозначения на представленной функциональной схеме работы фильтра Махони:

> Мт у - нормированный вектор относительного направления магнитного поля Земли в связанной системе координат, размер [3 х 1];

> ] - вектор угловой скорости БЛА, построенный по показаниям 3 - х осевого магнитометра и 3 - х осевого акселерометра, размер [3 х 1];

> К1 - интегральный коэффициент усиления;

> КР - пропорциональный коэффициент усиления;

> ¿окорру - скорректированный вектор показаний 3-х осевого ДУСа, размер [3 х 1];

> ^(о ■ - кватернион скорректированного вектора показаний 3-х осевого ДУСа, размер [4 х 1];

^ - приращение оценки кватерниона ориентации БЛА, размер [4 х 1]. Алгоритм работы фильтра Махони:

1. Нормирование показаний 3-х осевых акселерометров и 3-х осевых магнитометров для подавления шумов в измерениях данных сенсоров:

а ■ = ,, ■

"-норм ] / ||¿1-1| '

-» —Щ/ (2)

^Шнорм /\\mjW

2. Компенсация магнитного искажения:

Магнитные искажения в показаниях магнитометров возникают вследствие того, что БЛА во время полета может подвергаться воздействию ферро - магнитных источников, а также собственное бортовое оборудование обладает магнитными массами, магнитные поля которых искажают измерения магнитного поля Земли. Следовательно, ввиду наличия вышеприведенных проблем возникает необходимость в компенсации магнитных искажений. Далее будет описан подход, гарантирующий воздействие магнитных возмущений только на курс БЛА, а также устраняющий необходимость задания заранее направления магнитного поля Земли [13].

Ошибочное определение наклона магнитного поля НЕ может быть исправлено, если относительное направление магнитного поля Земли ЬЕ будет иметь тот же самый наклон. Это достигается путем вычисления нормалей Н^ и на оси X и У в Земной системе координат (2-3).

¡Ь] — (0 ¡х] Ну] = ¿¡норм ]-1 ® (0 тнорм })Т ® Чиорм ]-!> (3)

ь^ — (о ^НН^р . (4)

Нормированный вектор относительного направления магнитного поля Земли в связанной системе координат Мт у запишется как:

Vгп1 ] — (0 ^тп1х ] Vгп1у ] ^гп1г ]) — Чнорм ]-1 ® ® (¡норм ]-1> (5)

$ . — (уЬ уЬ уЬ )т. (6)

Э Ут1 (7)

ут]— /у* у.

У Ут1 ] У

3. Определение угловой скорости БЛА по показаниям 3-х осевого акселерометра и 3-х осевого магнитометра:

Уа1 ; — (0 У^1Х 1 У^1У ; ) — $-1рм 1-1 ®(0 0 0 д(<р))Т ® ¡ннорм ¡-ъ (8)

у . — (уь . уъ . уъ Лт. (9)

— (уа1х] Уа1у] уа1г]) > П^ ■ — а ■ х V +т ■ х 7 ■ (10)

} "-норм } Л уа} ~ "ьнорм } Л ут у у 7

д(ф) - значение ускорения свободного падения на текущей географической широте ^ БЛА, м / сек2.

4. Коррекция показаний 3-х осевого ДУСа:

^корр] j + Кр^а,т }>

где:

& - время дискретизации фильтра Махони, сек.

Второе слагаемое в выражении (11) является оценкой смещения нуля 3-х осевого ДУСа.

5. Вычисление приращения оценки кватерниона ориентации БЛА:

А --- ы (12)

Ч] 2 Чнорм]-! ^ О.Шкорр)ш

6. Численное интегрирование приращения оценки кватерниона ориентации БЛА:

Ч] = Ч]-1 + Ч]^. (13)

7. Нормирование оценки кватерниона ориентации БЛА:

Чнорм] ' /||а II.

' \\"]\\

Пункты 1 - 7 алгоритма работы фильтра Махони повторяются до тех пор, пока поступают измерения с датчиков.

Фильтр Маджвика

В данном алгоритме коррекция показаний 3-х осевого ДУСа осуществляется с помощью метода градиентного спуска [14, 15]. Далее представлен принцип работы фильтра Маджвика.

Рисунок 3 - Функциональная схема работы фильтра Маджвика Обозначения на представленной функциональной схеме работы фильтра

Маджвика:

^ Ь^у - кватернион относительного направления магнитного поля Земли в Земной

системе координат, размер [4 х 1]; ^ - приращение оценки кватерниона ориентации БЛА, размер [4 х 1];

> ^анорму -кватернион, составленный на основе нормированного вектора показаний 3-х осевого акселерометра анорм у, размер [4 х 1];

> ^тнорму -кватернион, составленный на основе нормированного вектора

показаний 3-х осевого магнитометра шнорм у, размер [4 х 1];

> норм- целевая функция использующаяся для

вычисления оценки кватерниона ориентации БЛА фу .В процессе работы фильтра Маджвика минимизируется с помощью метода градиентного спуска [16, 17], размер [6 х 1];

^ 1Т(Чнорм - транспонированный Якобиан целевой функции

f(Чнорм ]-1, ЧаНорМ], ЧтНорм]) размер [4 х 6];

> V/ - градиент целевой функции /(Чнорм ¡-^ Чaнормj, Чrnнормj), размер [4 х 1];

> Vf* - нормированный градиент целевой функции

Г(Чнорм 1-1, Чанорм], Ъ?р Чттт-норм^, размер [4 х 1];

> сокорр] - скорректированный вектор показаний 3-х осевого ДУСа, размер [3 х 1];

> Цшкорр] - кватернион скорректированного вектора показаний 3-х осевого ДУСа, размер [4 х 1];

> ( - коэффициент усиления фильтра Маджвика, предназначенный для устранения дрейфа 3-х осевого ДУСа;

> д - коэффициент усиления фильтра Маджвика, предназначенный для устранения смещения нуля 3-х осевого ДУСа.

Алгоритм работы фильтра Маджвика:

1. Нормирование показаний 3-х осевых акселерометров и 3-х осевых магнитометров происходит по соотношениям (1);

2. Компенсация магнитного искажения осуществляется по формулам (2 - 3);

3. Вычисление целевой функции /($норм ¡-^ Чaнормj, Ь?р Чтнорм]):

f (<7hopm y-1' ^a^pM^^t У' ^m^pM^

/{Л

/2 /3 /4

Zs Ve/

где:

/1 - 2 (¿7HOPM ; — 1(1)¿7HOPM ;—1(3) ¿LopM ; — 1(0)¿7HOPM ;—1(2)) ^aHopM ;(1); /2 = 2 (¿7hopm ; — 1(0)¿7HOPM ;—1(1) — ¿LopM ; — 1(2)¿7HOPM ;—1(3)) — ^aHopM ;(2); /3 = 2 (0,5 - ^м ,—1(1) - SopM ,—1(2)) - ^opM ;(3);

/4= 2b*-(1) (0,5 - ^м 7-1(2) - ^м 7-1(3)) +

+ 2^fу(3) (¿?HopM ; — 1(1)¿7HOPM _/—1(3) — ¿LopM ; — 1(0)¿7HOPM ;—1(2)) — ^mHopM ;(1);

/s= 2^f j (1) (¿7hopm ; — 1(1)¿7HOPM ;—1(2) ¿LopM ; — 1(0)¿7HOPM ;—1(3))

+

(15)

+ 2^f j (3) (¿7hopm ; — 1(0)¿7HOPM ;—1(1) + ¿LopM ; — 1(2)¿7HOPM ;—1(3)) ^rnHopM ; (2);

/e= 2bf, (3)) + 2bfy(3) (0,5-

a2

THOpM ]

— 1(1) ^HopM ; — 1(2)) ^rnHOpM ;(3).

4. Вычисление Якобиана /(¿7HopM y—1, bfy):

]{Янорм j-1, bt /О —

/Jl,l J 1,2 J 1,3 Jl,4\

J 2,1 Í2,2 h,3 J2,4

]з,1 1з,2 !3,3 J 3,4

Ja,1 J 4,2 J4,3 J 4,4

Js,l J 5,2 Js,3 J5,4

У6,1 J 6,2 Je,3 Je,4)

где:

J 1,1 — -2Чнорм j-l(2); Jl,2 — 2Чнорм j-l(3>);

Jl,3 — -2Чнорм _/-l(0); Jl,4 — 2Чнорм j-l(1);

hl— ' 2Чнорм j-l(1); J2,2 — 2Чнорм y-l(0);

J23 — ' 2Чнорм j-l(3>); J2,4 — 2Чнорм j-l(2);

hl — 0; J3,2 — ' ~4Чнорм j-l(1);

J3,3 — 4Чнорм j-l(2); J4,1 — -2bt ](3)Чнорм j-l(2);

J4,3 — -4b*j(1)qHopM-l(2) -

-2b¡^j(3)qHoVM j-l(0); J 5,1 — -2b¡^j(1)qHoVM j-l(3) + + 2bt ](3)Чнорм j-l

(1);

J 5,3 — -2b¡^j(1)qHoVM j-l(1) + + 2bt ](3)Чнорм j-l

(3);

!б,1 — 2bt j(1)4HopM j-l(2);

h,4 — 0; J4,2 — 2bt j(3)(¡HopM j-l(3);

J 4,4 — -4bçj(1)qHopM j-l(3) +

+ 2bt ](3)Чнорм j-l

(1);

J 5,2 — 2b?j(1)qHoVM j-l(2) +

+ 2b*j(3)qHopM j-l(0); J 5,4 — -2b*j(1)qHopM j-l(0) + + 2bt j(3)4HopM j-l

(2);

J6,2 — 2b?j(1)qHoVM j-l(3) -

(16)

-4Ь*(3)фнорм ;-1(1);

У6,3 = 2Ьс£;(1)фнорм ;-1(0) - Уб,4 = 2Ьс£;(1)фнорм ;-1(1).

-4Ь?;(3)^орм у-1(2);

5. Получение транспонированного Якобиана /т((7норм у-1, Ь^у );

6. Вычисление градиента V/ целевой функции /(фнорм у-1 ^aнOpM;, Ьfy, ^шнорм;):

= Ут(^/норм у-1 (^/норм у-1 ^«норм^ ^нормД (17)

7. Нормирование градиента ^ целевой функции /(фнорм у-1 ^aноpмУ, ^ 9тнорму):

-V// (18) V/ = Л^лг

8. Коррекция показаний 3-х осевого ДУСа:

^коррУ _ ^У — С^* дрейф у; (19)

<^дрейф У = <£>дрейф ;-1 + 2<7норм ;-1 ® (20)

где:

б^дрейф у - оценка дрейфа 3-х осевого ДУСа;

- время дискретизации фильтра Маджвика, сек.

9. Вычисление приращения оценки кватерниона ориентации БЛА:

4; = 1 Знорму-1 0 ^орру - ^Г. (21)

10. Численное интегрирование приращения оценки кватерниона ориентации БЛА происходит по формуле (12);

11. Нормирование полученной оценки кватерниона ориентации БЛА осуществляется при помощи соотношения (13).

Шаги 1 - 11 алгоритма работы фильтра Маджвика повторяются до тех пор, пока поступают измерения с датчиков.

Комплементарный фильтр

Данный фильтр использует обработанную высокочастотным фильтром оценку кватерниона ориентации БЛА, построенного на основе показаний 3-х осевого ДУСа, а также фильтрованную низкочастотным фильтром оценку кватерниона ориентации БЛА, построенного на основе показаний 3-х осевого акселерометра и 3-х осевого магнитометра для повышения точности определения угловой ориентации БЛА [18, 19]. Функциональная схема работы комплементарного фильтра представлена ниже.

Рисунок 4 - Функциональная схема работы комплементарного фильтра Обозначения на представленной функциональной схеме работы комплементарного фильтра:

> уа у - оценка угла крена БЛА по показаниям 3-х осевого акселерометра;

/V

> *дау - оценка угла тангажа БЛА по показаниям 3-х осевого акселерометра;

17

> 1Рат у - оценка угла курса БЛА по показаниям 3-х осевого магнитометра и оценки углов крена и тангажа БЛА;

^ Оы; - приращение оценки кватерниона ориентации БЛА, построенного на основе показаний 3-х осевого ДУСа, размер [4 х 1];

> - оценка кватерниона ориентации БЛА, построенного на основе показаний 3-х осевого ДУСа, размер [4 х 1];

> фнорм - нормированная оценка кватерниона ориентации БЛА, построенного на основе показаний 3-х осевого ДУСа, размер [4 х 1];

^ Чат ] - оценка кватерниона ориентации БЛА, построенного на основе показаний 3-х осевого акселерометра и 3-х осевого магнитометра, размер [4 х 1];

^ ¿?норм аш; - нормированная оценка кватерниона ориентации БЛА, построенного на основе показаний 3-х осевого акселерометра и 3-х осевого магнитометра, размер [4 х 1];

> а - коэффициент усиления комплементарного фильтра.

Алгоритм работы комплементарного фильтра:

1. Нормирование показаний 3-х осевых акселерометров и 3-х осевых магнитометров происходит по соотношениям (1);

2. Вычисление оценки углов крена и тангажа БЛА по показаниям 3-х осевого акселерометра:

= а1ап2

"-норм ] (0)

\

анорм ](1) + анорм 1(2)

норм]'

3. Вычисление оценки угла курса БЛА по показаниям 3-х осевого магнитометра и оценки углов крена и тангажа БЛА:

¥ат ] = агап2 ( Ьу/ ъ

(23)

где:

Ш-норм

](2) sm(y

а ] ) ^норм

Ьу = ГПнорм ](0) С0Кда ]) + ])(тнорм уфвЦЯд ]) + ™-норм ](2) ^Уа ])).

4. Вычисление оценки кватерниона ориентации БЛА, построенного на основе показаний 3-х осевого акселерометра и 3-х осевого магнитометра:

Т

Чат } = \Чат ] (0) Чат ](1) Чат ](22) Чат ](3)) ;

(24)

где:

Чат ]'(0') СхСуСг + БхБуБг,

Чат ]'(1') 5хСуСг Сх5уБ2,

Чат ](22) СхБуСг + БхСуБг,

Чат СхСуБг БхБуСг,

Сх = СОБ^Ра ¡/2);

Бх = Бт(га ]/2);

Су = соз(да]/2);

Бу = соБ(да}/2);

Сг = С0Б(Ч)ат ]/2);

Бг = Бт(Уат }/2).

5. Нормирование оценки кватерниона ориентации БЛА, построенного на основе показаний 3-х осевого акселерометра и 3-х осевого магнитометра:

* (25)

Чнорм ат] /\\п II"

6. Вычисление приращения оценки кватерниона БЛА, построенного на основе показаний 3-х осевого ДУСа:

А --- <ъ (26)

^у 2 Чнорм]-1 Яш].

7. Численное интегрирование приращения оценки кватерниона ориентации БЛА,

построенного на основе показаний 3-х осевого ДУСа:

+ (27)

8. Нормирование оценки кватерниона ориентации БЛА, построенного на основе показаний 3-х осевого ДУСа:

* = и/ (28)

Чнорм ш] / \ \л II"

' 11™у||

9. Вычисление оценки кватерниона ориентации БЛА:

Ч) = (1 — а)Чнорм ш] + аЧнорм ат]"

(29)

10. Нормирование полученной оценки кватерниона ориентации БЛА осуществляется при помощи соотношения (1 3).

По аналогии с рассмотренными выше фильтрами шаги 1 - 10 алгоритма работы комплементарного фильтра повторяются до тех пор, пока поступают измерения с датчиков.

Применительно ко всем вышеприведенным алгоритмам автономного определения угловой ориентации БЛА получение углов Эйлера происходит по следующим соотношениям [20]:

■р _ | 2(Цнорм](0^Чнорм]'(3 + Чнорм](1)Чнорм](2)) /

' ( /(1-2(ч2Норм](0) + Ч2Норм](1)

= ЧБт (2(Цнорм](1)Чнорм](3) — Чнорм](0)Чнорм](2)))>

~ Мал2 I 2(^норм]'(2^Чнорм]'(3 + Янорм](0)Янорм](1))) / где:

У] - оценка угла крена БЛА, рад; -д] - оценка угла тангажа БЛА, рад; ^ - оценка угла курса БЛА, рад.

Имитационная модель алгоритмов определения угловой ориентации БЛА

Имитационное моделирование алгоритмов комплексной обработки навигационных данных производилось на высокоуровневом языке программирования Python, характеризующимся кроссплатформенностью (возможно использовать на отечественных операционных системах и вычислителях), быстротой разработки программно - математического обеспечения (ПМО) и динамической типизацией данных (простота использования переменных). На рисунке 4 представлен программный комплекс имитационного моделирования в виде UML диаграммы (иерархии классов) [21, 22].

Рисунок 5 - Иерархия классов разработанного программного комплекса Центральным рабочим классом разработанного ПМО является класс Т81тиШот,

который связан со следующими классами:

> Абстрактный класс алгоритмов автономного определения угловой ориентации

БЛА ABCAHRSFilter. Имеет связь композицию с классом Quaternion;

> Абстрактный класс ABCPlotGraph, предназначенный для построения графиков различных данных;

> Абстрактный класс ABCFileWorker, предназначенный для работы с файлами; ^ Класс Quaternion, реализующий математические операции с кватернионами.

Наследниками класса ABCAHRSFilter являются:

> Класс Mahony, реализующий алгоритм работы фильтра Махони;

> Класс Madgwick, реализующий алгоритм работы фильтра Маджвика;

^ Класс Complementary, реализующий алгоритм работы комплементарного фильтра.

Наследником класса ABCPlotGraph является класс NavigationPlotGraph, реализующий построение графиков навигационных параметров.

Наследником класса ABCFileWorker является класс NavigationFileWorker, реализующий загрузку и сохранение навигационных параметров в файл формата csv.

djef Run (self} :

EEG2RAD = np.float32(np.pi/180) RÄD2DEG = np. f loat.32 (lBO/np. pi}

# Создание объектов навигационных фильтров raahony = Mahony(self. ÄBCAHRSFiiter, 0.5, 1) madgwick = Madgwick {self ,_ABCAHRSFi Iter, 0.32) ccmpl = Conplementary[self. ABCAKRSFii~er, 0.05)

# Создание объекта fileworker

fileWorker = Nävi gatioriF 11 eWor ke r (self. ABCFileWorker) data = fileWorker .LoadExcel (lrrepoIMU3. csv")

# Инициализация матрицы кватерниона ориентации dien = len[data[1 дуг 1])

q = np.zeros({dien, 4}) q[:r 0] = 1-0

f Вызов функций расчеса репе:-:ий фильтров EulerMahony = np.zeros{ [dien,3}) for t in range(1, dien):

q[t] = mahony.UpdateMARG[q [t - 1], DEG2RAD * data[1 дуг'; [t],

datafacc'] [t] , data ['nag1 ] ;t]) EulerMahony[t] = self._Quaternion.to_angles[q[t]) * RAD2DEG

EulerMadgwick = np.zeros({dien, 3)) for t in range{1, dien):

q[t] = madgwick.UpdateMÄRG(q[t - 1J, DEG2RAD * data[1 дугT] [t],

datafacc'] [t] , data [ 'naq1 ] ;t]) EulerMadgwick[t] = self._Quaternion.to_angles[q[t]) * RÄD2DEG

EulerCompl = np.zeros{ [dlen, 3)] for t in range{1, dlen):

q[t] = compl.UpdateMARG (q [t - 1], DEG2RAD * data[Tgyr'] [t] ,

data[1acc1;, data['naq1];t]) EulerCompl [t] = self._Quaternion. to angles [q [t] ) * RAD2DEG

Рисунок 6 - Часть программного кода главной процедуры Run класса TSimulator

Результаты моделирования

Проведение имитационного моделирования алгоритмов автономного определения параметров угловой ориентации БЛА осуществлялось на основании тестовых данных (датасета) инерциально - измерительного модуля ХБеш МТ - 10 [23]. Для определения корректности исходных данных ХБеш МТ - 10 была проведена их верификация (статистическая оценка данных) путем вычисления СКО проекций линейного ускорения и проекций угловой скорости БЛА в статическом режиме. Полученные значения СКО сравнивались с номинальными паспортными данными ХБеш МТ1 - 10 и обобщались в таблицу (Таблица 1). Отклонения реальных значений СКО навигационных параметров отличаются от номинальных значений меньше, чем на 10 %, в результате чего можно сделать вывод, что датасет инерциально - измерительного модуля ХБеш МТ1 - 10 считается верифицированным и корректным для дальнейшего использования.

Навигационный параметр Реальное значение СКО Номинальное значение СКО Отклонение СКО, %

Продольная угловая скорость БЛА, град/сек 0,6207 0,6112 1,53

Боковая угловая скорость БЛА, град/сек 0,6198 0,6112 1,38

Вертикальная угловая скорость БЛА, град/сек 0,6258 0,6112 2,33

Продольное ускорение БЛА, м/сек2 0,0119 0,0116 2,52

Боковое ускорение БЛА, м/сек2 0,0121 0,0116 4,13

Вертикальное ускорение БЛА, м/сек2 0,0126 0,0116 7,93

Таблица 1 - СКО навигационных параметров БЛА в статическом режиме

Ниже представлено описание углового движения БЛА по исходным данным инерциально - измерительного модуля ХБеш МТ1 - 10.

Эволюции проекций угловой скорости БЛА

— йугоХ СугоТ

А

п .....Л к

V /»»^""т-у,, ¡7

1

г

Рисунок 7 - График эволюции проекций угловой скорости БЛА

> Маневры БЛА относительно продольной оси (угол крена):

^ БЛА совершал разворот относительно продольной оси ОХ против часовой стрелки с 7 по 11 сек., максимальная угловая скорость составила 140 град/сек;

^ БЛА совершал разворот относительно продольной оси ОХ по часовой стрелке с 14 по 18 сек., максимальная угловая скорость составила 310 град/сек;

^ БЛА совершал разворот относительно продольной оси ОХ против часовой стрелки с 21 по 24 сек., максимальная угловая скорость составила 240 град/сек.

> Маневры БЛА относительно боковой оси (угол тангажа):

^ БЛА совершал разворот относительно боковой оси ОУ против часовой стрелки с 25 по 30 сек., максимальная угловая скорость составила 150 град/сек;

^ БЛА совершал разворот относительно боковой оси ОУ по часовой стрелке с 33 по 36 сек., максимальная угловая скорость составила 260 град/сек;

^ БЛА совершал разворот относительно боковой оси ОУ против часовой стрелки с 40 по 43 сек., максимальная угловая скорость составила 190 град/сек.

> Маневры БЛА относительно вертикально оси (угол курса):

^ БЛА совершал разворот относительно вертикальной оси 02 против часовой стрелки с 45 по 48 сек., максимальная угловая скорость составила 205 град/сек;

^ БЛА совершал разворот относительно вертикальной оси 02 по часовой стрелке с 53 по 56 сек., максимальная угловая скорость составила 420 град/сек;

^ БЛА совершал разворот относительно вертикальной оси 02 против часовой стрелки с 57 по 60 сек., максимальная угловая скорость составила 210 град/сек.

На представленных ниже рисунках показаны результаты работы алгоритмов автономного определения угловой ориентации БЛА.

_Эволюция угла крема БЛА_

- махони Маджвик - Комплементарный

йА

Г ' 1 и

О 10 20 30 40 50 60 70

Рисунок 8 - График эволюции угла крена БЛА

Эволюция ошибки угла крена БЛА

................... .. .. 1к 1 [1 и , . .

.............I ■ • ...........т...................

1

Г

о 1000 2000 3000 4000 5000 6000 7000

Рисунок 9 - График ошибки определения угла крена БЛА

Эволюция угла тангажа БЛА

О 10 20 30 40 50 60 70

Рисунок 10 - График эволюции угла тангажа БЛА

Рисунок 11 - График ошибки определения угла тангажа БЛА

Рисунок 12 - График эволюции угла курса БЛА

о 1000 2000 3000 4000 5000 6000 7000

Рисунок 13 - График ошибки определения угла курса БЛА Для оценки точности получаемых навигационных решений были использованы данные об угловом положении БЛА с оптической системы захвата движения [24 - 26].

Угол ориентации БЛА Фильтр Махони Фильтр Маджвика Комплементарный фильтр

МО СКО МО СКО МО СКО

Крен 2,62° 4,35° 2,25° 3,48° 5,3° 18,5°

Тангаж 2,18° 3,64° 1,55° 2,58° 2,41° 7,29°

Курс 5,12° 13,18° 3,26° 9,08° 7,25° 27,17°

Таблица 2 - Статистические характеристики алгоритмов автономного определения

угловой ориентации БЛА

Заключение

В статье рассмотрена задача автономного определения угловой ориентации БЛА на основе алгоритмов Маджвика, Махони и комплементарного фильтра.

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

1) На основе вышеуказанных алгоритмов был разработан программный комплекс алгоритмов автономного определения угловой ориентации БЛА;

2) Была проведена отработка разработанного программного комплекса на верифицированном датасете инерциально - измерительного модуля Xsens MTi - 10;

3) По результатам отработки разработанного программного комплекса можно сделать вывод о том, что точность определения углов ориентации БЛА по алгоритмам Махони и Маджвика выше минимум в 2 раза, чем у комплементарного фильтра.

Список источников

1. Ермаков П.Г., Гоголев А.А. Сравнительный анализ схем комплексирования информации бесплатформенных инерциальных навигационных систем беспилотных летательных аппаратов // Труды МАИ. 2021. № 117. URL: http://trudymai.ru/published.php?ID=156253. DOI: 10.34759/trd-2021-117-11

2. Jussi Collin. MEMS IMU carouseling for ground vehicles // IEEE Transactions on Vehicular Technology, 2014, no. 64 (6). DOI: 10.1109/TVT.2014.2345847

3. Pengfei Gui, Liqiong Tang, Subhas Mukhopadhyay. MEMS Based IMU for Tilting Measurement: Comparison of complementary and Kalman filter based data fusion // 10th Conference on Industrial Electronics and Applications (ICIEA), June 2015. DOI: 10.1109/ICIEA.2015.7334442

4. Kalman R.E. A new approach to linear filtering and prediction problems // Journal of Basic Engineering, 1960, vol. 82 (1), pp. 35-45. DOI:10.1115/1.3662552

5. Линник Ю.В. Метод наименьших квадратов и основы математико-статистической теории обработки наблюдений. - М.: Физматгиз, 1962. - 349 с.

6. Никулин М.С. Отношения правдоподобия критерий // Математическая энциклопедия / Виноградов И.М. (гл. ред). - М.: Советская энциклопедия, 1984. Т.4. С. 151- 1216.

7. Волков В.А., Кудрявцева И.А. Численное решение задач нелинейной фильтрации на основе алгоритмов фильтра частиц // Труды МАИ. 2016. № 89. URL: https://trudymai.ru/published.php?ID=73405

8. Robert E. Mahony, T.Hamel, Jean - Michel Pfimlin. Nonlinear Complementary Filters on the Special Orthogonal Group // IEEE Transactions on Automatic Control, July 2008, vol. 53(5), pp. 1203-1218. DOI: 10.1109/TAC.2008.923738

9. Sebastian O.H. Madgwick. An efficient orientation filter for inertial and inertial/magnetic sensor arrays, April 30, 2010. URL: https://x-

10.co. uk/res/doc/madgwick internal report.pdf

10. Simone A. Ludwig. Optimization of Control Parameter for Filter Algorithms for Attitude and Heading Reference Systems // IEEE Congress on Evolutionary Computation (CEC), July 2018. DOI: 10.1109/CEC.2018.8477725

11. Thibaud Michel, Hassen Fourati, Pierre Geneves, Nabil Layaida. A Comparative Analysis Estimation for Pedestrian Navigation with Smartphones // International Conference on Indoor Positioning and Indoor Navigation, October 2015. DOI: 10.1109/PIN.2015.7346767

12. Иванченко А.Я. Применение адаптивной настройки пропорционально -интегрально-дифференциального регулятора для управления антенной радиолокатора // Труды МАИ. 2013. № 66. URL: https://trudymai.ru/published.php?ID=40821

13. Гоголев А.А., Горобинский М.А. Определение собственного положения микробеспилотного летательного аппарата в условиях замкнутого пространства // Труды МАИ. 2018. № 101. URL: https://trudymai.ru/published.php?ID=97029

14. Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan. Estimation of IMU and MARG orientation using a gradient descent algorithm // 2011 IEEE International

Conference on Rehabilitation Robotics Rehab Week Zurich, ETH Zurich Science City, Switzerland, June 29 - July 1, 2011, pp. 179-185

15. Фильтр Маджвика. 2015. URL: https://habr.com/ru/post/255661/

16. Donghwan Kim, Jeffrey A. Fessler. Generalizing the optimized gradient method for smooth convex minimization // SLAM Journal on Optimization, 2016, vol. 28(2). DOI: 10.1137/17m112124x

17. Ефимов Е.Н., Шевгунов Т.Я. Разработка и исследование методики построения нейронных сетей на основе адаптивных элементов // Труды МАИ. 2012. № 51. URL: https://trudymai.ru/published.php?ID=29159

18. Parag Narkhede, Shashi Poddar, Rahee Walambe, George Ghinea, Ketan Kotecha. Cascaded Complementary Filter Architecture for Sensor Fusion in Attitude Estimation // Sensors, 2021, vol. 21(6). DOI: 10.3390/s21061937

19. Зо Мьо Наин, Щагин А.В., Ле Винь Тханг, Хтин Линн У. Комплементарный фильтр для оценки угла с использованием микроэлектромеханической системы гироскопа и акселерометра // Инженерный вестник Дона. 2020. № 3. URL: http://www.ivdon.ru/uploads/article/pdf/IVD 7 2 Zaw_MyoNaing_Shchagin.pdf_6749b2a 99a.pdf

20. Relationship Between Euler - Angle Rates and Body - Axis Rates. URL: http://www.stengel.mycpanel.princeton.edu/Quaternions.pdf

21. Буч Г., Рамбо Дж., Джекобсон А. Язык UML. Руководство пользователя. - M.: ДМК Пресс, 2007. - 496 с.

22. StarUML - The Open Source UML/MDA Platform. URL: https://staruml.io/

23. MTI User Manual. MTI 10 - series and MTI 100 - series 5th Generation. URL: https://www.xsens.com/hubfs/Downloads/usermanual/MTi usermanual.pdf

24. Gomes L.L., Leal L., Oliveira T.R., Cunha J.P.V.S., Revoredo T.C. Unmanned Quadcopter Control Using a Motion Capture System // IEEE Latin America Transactions, 16 December 2016. DOI: 10.1109/TLA.2016.7786340

25. Masiero A., Fissore F., Antonello R., Cenedese A. A Comparison of UWB and Motion Capture UAV Indoor Positioning // The International Archives Of The Photogrammetry, Remote Sensing And Spatial Informational Sciences, 2019. DOI: 10.5194/isprs-archives-XLII-2-W13-1695-2019

26. Delameare M., Duval F., Boutteau R.. A New Dataset of People Flow in an Industrial Site with UWB and Motion Capture Systems // Sensors, 2020, vol. 20 (16). DOI: 10.3390/s20164511

References

1.Ermakov P.G., Gogolev A.A. Trudy MAI, 2021, no. 117. URL: http://trudymai.ru/eng/published.php?ID=156253. DOI: 10.34759/trd-2021-117-11

2. Jussi Collin. MEMS IMU carouseling for ground vehicles, IEEE Transactions on Vehicular Technology, 2014, no. 64 (6). DOI: 10.1109/TVT.2014.2345847

3. Pengfei Gui, Liqiong Tang, Subhas Mukhopadhyay. MEMS Based IMU for Tilting Measurement: Comparison of complementary and Kalman filter based data fusion, 10th Conference on Industrial Electronics and Applications (ICIEA), June 2015. DOI: 10.1109/ICIEA.2015.7334442

4. Kalman R.E. A new approach to linear filtering and prediction problems, Journal of Basic Engineering, 1960, vol. 82 (1), pp. 35-45. DOI:10.1115/1.3662552

5. Linnik Yu.V. Metod naimen'shikh kvadratov i osnovy matematiko-statisticheskoi teorii obrabotki nablyudeniin (), Moscow, Fizmatgiz, 1962, 349 p.

6. Nikulin M.S. Otnosheniya pravdopodobiya kriterii. Matematicheskaya entsiklopediya (), Moscow, Sovetskaya entsiklopediya, 1984, vol. 4, pp 151- 1216.

7. Volkov V.A., Kudryavtseva I.A. Trudy MAI, 2016, no. 89. URL: https://trudymai.ru/eng/published.php?ID=73405

8. Robert E. Mahony, T.Hamel, Jean - Michel Pfimlin. Nonlinear Complementary Filters on the Special Orthogonal Group, IEEE Transactions on Automatic Control, July 2008, vol. 53 (5), pp. 1203-1218. DOI: 10.1109/TAC.2008.923738

9. Sebastian O.H. Madgwick. An efficient orientation filter for inertial and inertial/magnetic sensor arrays, April 30, 2010. URL: https://x-io.co.uk/res/doc/madgwick_internal_report.pdf

10. Simone A. Ludwig. Optimization of Control Parameter for Filter Algorithms for Attitude and Heading Reference Systems, IEEE Congress on Evolutionary Computation (CEC), July 2018. DOI: 10.1109/CEC.2018.8477725

11. Thibaud Michel, Hassen Fourati, Pierre Geneves, Nabil Layaida. A Comparative Analysis Estimation for Pedestrian Navigation with Smartphones, International Conference on Indoor Positioning and Indoor Navigation, October 2015. DOI: 10.1109/PIN.2015.7346767

12. Ivanchenko A.Ya. Trudy MAI, 2013, no. 66. URL: https://trudymai.ru/eng/published.php?ID=40821

13. Gogolev A.A., Gorobinskii M.A. Trudy MAI, 2018, no. 101. URL: https://trudymai.ru/eng/published.php?ID=97029

14. Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan. Estimation of IMU and MARG orientation using a gradient descent algorithm, 2011 IEEE International Conference on Rehabilitation Robotics Rehab Week Zurich, ETH Zurich Science City, Switzerland, June 29 - July 1, 2011, pp. 179 - 185

15. Fil'tr Madzhvika. 2015. URL: https://habr.com/ru/post/255661/

16. Donghwan Kim, Jeffrey A. Fessler. Generalizing the optimized gradient method for smooth convex minimization, SLAM Journal on Optimization, 2016, vol. 28(2). DOI: 10.1137/17m112124x

17. Efimov E.N., Shevgunov T.Ya. Trudy MAI, 2012, no. 51. URL: https://trudymai.ru/eng/published.php?ID=29159

18. Parag Narkhede, Shashi Poddar, Rahee Walambe, George Ghinea, Ketan Kotecha. Cascaded Complementary Filter Architecture for Sensor Fusion in Attitude Estimation, Sensors, 2021, vol. 21(6). DOI: 10.3390/s21061937

19. Zo M'o Nain, Shchagin A.V., Le Vin' Tkhang, Khtin Linn U. Inzhenernyi vestnik Dona, 2020, no. 3. URL: http://www.ivdon.ru/uploads/article/pdf/IVD 7 2 Zaw MyoNaing Shchagin.pdf 6749b2a 99a.pdf

20. Relationship Between Euler - Angle Rates and Body - Axis Rates. URL: http://www.stengel.mycpanel.princeton.edu/Quaternions.pdf

21. Buch G., Rambo Dzh., Dzhekobson A. Yazyk UML. Rukovodstvopol'zovatelya (), Moscow, DMK Press, 2007, 496 p.

22. StarUML - The Open Source UML/MDA Platform. URL: https://staruml.io/

23. MTI User Manual. MTI 10 - series and MTI 100 - series 5th Generation. URL: https://www.xsens.com/hubfs/Downloads/usermanual/MTi usermanual.pdf

24. Gomes L.L., Leal L., Oliveira T.R., Cunha J.P.V.S., Revoredo T.C. Unmanned Quadcopter Control Using a Motion Capture System, IEEE Latin America Transactions, 16 December 2016. DOI: 10.1109/TLA.2016.7786340

25. Masiero A., Fissore F., Antonello R., Cenedese A. A Comparison of UWB and Motion Capture UAV Indoor Positioning, The International Archives Of The Photogrammetry, Remote Sensing And Spatial Informational Sciences, 2019. DOI: 10.5194/isprs-archives-XLII-2-W13-1695-2019

26. Delameare M., Duval F., Boutteau R.. A New Dataset of People Flow in an Industrial Site with UWB and Motion Capture Systems, Sensors, 2020, vol. 20 (16). DOI: 10.3390/s20164511

Статья поступила в редакцию 10.02.2022 Статья после доработки 12.02.2022 Одобрена после рецензирования 24.04.2022 Принята к публикации 21.06.2022

The article was submitted on 10.02.2022; approved after reviewing on 24.04.2022; accepted for publication on 21.06.2022