CC BY
5Труды МАИ. Выпуск № 101_ http: //trudymai .ru/
УДК 519.876.5
Определение собственного положения микробеспилотного летательного аппарата в условиях замкнутого пространства
Гоголев А.А.*, Горобинский М.А.**
Государственный научно-исследовательский институт авиационных систем, ул. Викторенко, 7, Москва, 125319, Россия, *e-mail: kirbizz8@yandex.ru **e-mail: migo2405@gmail.com
Аннотация
Микробеспилотные летательные аппараты (микроБЛА) типа мультикоптер благодаря своей сравнительной компактности и возможности маневрировать в ограниченном пространстве способны осуществлять целевую задачу в условиях замкнутого пространства (помещениях). Современные комплексы микроБЛА используют в качестве основного канала навигационной информации приемник спутниковых навигационных систем (СНС) ГЛОНАСС/GPS, сигналы которого не проникают в помещения. В статье представлено решение задачи определения собственного положения в условиях замкнутого пространства на основе бесплатформенной инерциальной навигационной системы (БИНС) и алгоритмов визуальной навигации по кодовым меткам при отсутствии сигналов ГЛОНАСС/GPS. Представленный аппаратно-программный комплекс позволяет определять собственное положение с помощью бортового вычислителя в режиме
реального времени с точностью, составляющей около 10 сантиметров и составляющей 1/5 линейных размеров летательного аппарата.
Ключевые слова: фильтр Махони, навигационная система, беспилотный летательный аппарат, мультикоптер, квадрокоптер, кодовые метки, автономная навигационная система.
Введение
Первый летательный аппарат типа мультикоптер был сконструирован еще на заре авиации в 1922 году Георгием Александровичем Ботезатом и смог подняться всего лишь на 4 метра [1]. Сегодняшние беспилотные летательные аппараты способны выполнять различные транспортные задачи [2], но ограничиваются пространствами с доступными сигналами СНС ГЛОНАСС/ОРБ [3]. Современные навигационные системы используют в качестве датчиков пространственного положения - приемник СНС, а в качестве датчиков углового положения - БИНС. Сферы применения микроБЛА в условиях замкнутого пространства (отсутствия сигналов СНС) достаточно обширны и представляют собой решения как транспортных и обзорных задач, так и спасательных работ в труднодоступных помещениях [4].
Постановка и схема решения задачи
Современные мультикоптеры (рисунок 1) оснащены достаточно обширным числом различных навигационных датчиков (БИНС, магнитометр, барометр, приемник СНС ГЛОНАСС/ОРБ, видеокамерами и другими датчиками различной
физической природы), однако нет готовых решений, позволяющих выполнять автономную обзорную задачу в условиях замкнутого пространства. Существующие образцы техники имеют телекомандный тип управления по сигналам оператора с персонального компьютера (ПК). При автономных режимах полета, таких как поиск и стабилизация над заданной точкой или пролет по точкам маршрута используются приемники сигналов ОРБ/ГЛОНАСС, которые не применимы в замкнутом пространстве.
Рисунок 1. Современный мультикоптер.
Определить пространственное положение микроБЛА позволяют имеющиеся на борту измерительные средства, но в таком случае получается высокая погрешность. Точность вычисления собственного позиционирования на основе БИНС носителя путем двукратного интегрирования ускорений от акселерометров будет составлять около 2-3 метров. Барометр имеет точность около 0.5 м, чего недостаточно для решения задачи автономного пилотирования. Магнитометр имеет значительные выбросы показаний в случае наличия больших металлических предметов.
Для решения задачи пространственного позиционирования в замкнутом пространстве предлагается использовать архитектуру программно-аппаратного
комплекса (Рисунок 2) состоящую из БИНС, магнитометра, барометра, системы визуальной навигации и алгоритмов комплексирования навигационной информации. Составные части алгоритма программного комплекса описаны ниже.
Рисунок 2. Схема применяемого программно-аппаратного комплекса.
Фильтр Махони
Традиционный подход по применению расширенного фильтра Калмана требует больших вычислительных ресурсов, отсутствующих в бортовом вычислителе микроБЛА. Фильтр Махони используется для определения углового положения микроБЛА в пространстве и фильтрации ускорений микроБЛА в земной связанной системе координат (ЗССК) [5]. Фильтр занимается обработкой массивов данных, поступающих с датчиков и снимает проблемы точности и настройки параметров фильтра Калмана. Вычислительная сложность алгоритма снижена за
счет использования математических расчетов на основе кватернионов, что
позволяет избегать трудностей, связанных с описанием пространственного положения при помощи углов Эйлера. Алгоритм работы фильтра (Рисунок 3) состоит из следующих этапов [6]:
• Определении ориентации по гироскопу;
• Коррекции гироскопа на основе магнитометра.
Рисунок 3. Схема работы фильтра Махони.
Оценки среднеквадратичного отклонения (СКО) по углу на основе фильтра
Махони и фильтра Калмана представлены в таблице 1 [7].
СКО по углу Алгоритм на основе Фильтр Махони
фильтра Калмана
0 для неподв. 0,789 0,581
0 для подв. 0,769 0,625
у для неподв. 0,819 0,502
у для подв. 0,847 0,668
¥ для неподв. 1,150 1,073
¥ для подв. 1,344 1,110
Таблица 1. Оценки точности фильтра Махони.
Магнитное поле
Получение информации о магнитном поле Земли (МПЗ) необходимо для увеличения точности определения ориентации летательного аппарата. В микроБЛА для измерения МПЗ используется 3-х осевой магнитометр. Сложность работы с информацией о магнитном поле состоит в том, что измерения МПЗ будут искажены, так как микроБЛА и его оборудование обладают магнитными массами, магнитные поля которых влияют на неправильное определение составляющих МПЗ [8]. Также показания магнитометра могут быть искажены наличием ферро-магнитных источников вокруг микроБЛА.
Направление магнитного поля Земли в земных координатах Б Е, может быть вычислено при помощи уравнения (1). Эффект определения ошибочного наклона магнитного поля может быть исправлен, если относительное направление магнитного поля Земли ЬЕ все время имеет тот же самый наклон [7]. Это достигается путем вычисления нормалей ЬЕ и Б Е на оси X и У в земной связанной системе координат, что описано в уравнении (2).
ПЕ = [0 кх ку = * ш5 * (1)
Б£ = [ О о к2] ;(1)
кватернион, определяющий направление магнитного поля Земли в земных координатах;
кх, ку, кг — составляющие вектора направления магнитного поля Земли;
ЬЕ — кватернион относительного направления магнитного поля Земли в земных координатах;
показания магнитометра;
кватернион, предназначенный для коррекции ориентации для инерциальной навигационной системы;
еЧ е3 г — кватернион, противонаправленный кватерниону 5 ъ;
Компенсация магнитных искажений при использовании описанного подхода гарантирует воздействие магнитных возмущений только на курс микроБЛА. Данный метод также устраняет необходимость задавать заранее направление магнитного поля Земли, что является потенциальным недостатком других подходов в фильтрах ориентации [5].
Комплементарный фильтр
Комплементарный фильтр - фильтр, применяемый к двум величинам, измеряемым разными датчиками, корректирующий одну из величин так, чтобы она медленно стремилась ко второй. Алгоритм работы представлен на основе бароинерциального комплементарного фильтра. Бароинерциальный комплементарный фильтр использует показания высоты с барометра и ускорения с БИНС. Барометр имеет малую точность измерений и сильную зашумленность сигнала, что не позволяет использовать его в качестве единственного источника данных о высоте. Высоту можно получить путем двойного интегрирования линейного ускорения по оси 7, полученного от акселерометра. Показания
акселерометра напрямую можно использовать только при горизонтальном полете микроБЛА, так как наклон аппарата вызовет изменение проекции ускорения микроБЛА на его ось Ъ. Помимо этого любой шум в результате двойного интегрирования может привести к серьезной ошибке. Если применить к двум полученным значениям комплементарный фильтр, то в результате ошибки как акселерометра, так и баровысотомера взаимно компенсируются, и полученная высота будет наиболее близка по значению к истинной. В результате выражение для нахождения искомого ускорения принимает следующий вид:
аг = аг —д + А ; (2)
аг — ускорение от акселерометра; а — линейное вертикальное ускорение;
- сила тяжести; А — оцениваемая величина ошибки.
Работа регулятора основана на оценке величины ошибки. Сделаем следующие допущения и найдем величину : параметры микроБЛА, влияющие на показания акселерометра зафиксированы и микроБЛА находится в горизонтальном положении.
А = АрР + А¡1 + АаО; (3)
Р, I, Б - коэффициенты РГО-регулятора;
Ар, А ¿, А а — Р, I, Б составляющие РГО-регулятора.
Пропорциональная составляющая регулятора - непосредственно сама ошибка определения высоты. Интегральная ( ) составляющая регулятора находится путем интегрирования ошибки. Если в качестве дифференциальной (Л а) составляющей регулятора взять продифференцированную ошибку, то вместе с ней в регулятор попадут и шумы от баровысотомера и акселерометра, поэтому за дифференциальную составляющую возьмем найденную с помощью акселерометра скорость с отрицательным знаком. Величина описывает полную величину действующей ошибки, поэтому не задействована и используется только ее I-составляющая регулятора:
а = аг—д — Л ¿; (4)
Нахождение Р-составляющей:
д _ Н—Ньдуо
ЛР = кг ;(5) высота от баровысотомера; к/ — коэффициенты фильтра. Нахождение 1-составляющей:
Л 1= Л ¿+ Л р*1; (6)
Нахождение Э-составляющей:
Л й= — Ъ; (7)
скорость, найденная путем интегрирования ускорения ;
После нахождения и идет вычисление скорости путем интегрирования:
К = К + ( а2 — АрР + А аБ ) г; (8) - шаг интегрирования (дельта между расчетами); Найденная скорость используется для нахождения высоты:
Н = Н + У^* уб; (10) Для получения конечного значения высоты вычитается :
(9)
коэффициенты фильтра.
/с/ = Ю; Р = 20; / = 0.1; Б = 0.2; уб = 7
1000000'
результирующая высота.
Система визуальной навигации
Система предназначена для поиска, распознавания и определения пространственного положения кодовой метки [9]. Во избежание ошибочного распознавания был применен метод устранения несовпадений, основанный на информации о движении камеры [10]. Схема работы алгоритма поделена на 5 этапов, таким образом, чтобы суммарное время работы алгоритма не превышало 20 миллисекунд. В качестве кодовых меток были выбраны метки типа АгЦео [11], имеющие 255 вариантов и позволяющие идентифицировать номер кодовой метки.
Рисунок 4. Схема работы системы визуальной навигации
Работа системы происходит следующим образом:
1. Инициализация изображения с бортовой камеры (частота 30 кадров/сек);
2. Поиск и распознавание кодовой метки на изображении;
3. Определение экранного положения кодовой метки на изображении;
4. Прогнозирование и фильтрация экранных координат на основе фильтра
Калмана;
5. Определение пространственного положения кодовой метки
относительно микроБЛА.
(я) 00
Рисунок 5.Алгоритм поиска и распознавания кодовой метки
Поиск и распознавание метки на изображении происходит следующим образом [11]:
1. Перевод изображения в черно-белое с сегментацией краев на основе оператора Кэнни [12];
2. Выделение границ меток на основе алгоритма Сузуки и Абе [13];
3. Распознавание метки [11] путем разбиения метки на элементарные бинарные области.
Фильтр Калмана используется для фильтрации и прогнозирования координат метки в случае ее кратковременного исчезновения и описывается с помощью соотношений, указанных ниже [14]. Вектор состояния:
х = (х, у, х, у, 5)т; (12)
координаты маркера; х, у — скорость перемещения маркера;
размер маркера; Модель оценок:
хь+1 = (13)
А =
-1 0
0 1
0 0
0 0
-0 0
О
О 01
ат о 10 0 0 10 0 0 1
;(14)
матрица перехода; Модель измерений:
н =
-1 0 0 0 о-
0 1 0 0 0
0 0 0 0 1
0 0 0 0 0
-0 0 0 0 0-
;(1б)
матрица измерений;
Уравнения работы фильтра Калмана:
^¿+1 — [(А+1) 1 + #¿+1^+1 ^¿+1] ;
хЬ + 1 = ^¿ + 1ЛХ1 >
^¿+1 = ^¿+1^+1^1+1 + Qi+l'>
вектор оценки;
х — вектор прогноза; Р* — корреляционная матрица оценки; корреляционная матрица прогноза; матрицы ковариации шумов;
В качестве камеры выступает так называемая пинхол-камера, в которой отсутствует объектив, а его роль выполняет отверстие размером 1 мм. В данном случае картинка формируется путем проецирования 3D точки в плоскости изображения с помощью перспективного преобразования:
где:
• М' - точка в реальном мире;
• (X , У, Я) - координаты 3Э точки в реальном мире;
• т' - проекция точки в плоскости изображения;
• (и, у) - координаты точки проекции в экранных координатах;
• А - матрица внутренних параметров;
• (сх, су) - центр изображения;
т' = Л[Д|^М';(18)
или
-и-1 Г/х О СХЛ ГГц г12 г13 V = О /у су Г21 Т22 г23 -1-' 1-0 0 1 \ У31 г32 г33
V
• fx,fy - фокусные расстояния, выраженные в пикселях;
• [ Я \ ¿\ - матрица перевода;
Таким образом, если изображение с камеры масштабируется с каким-либо коэффициентом, то все эти параметры также изменяются с таким же коэффициентом. Матрица внутренних параметров не зависит от наблюдаемого изображения, и остается неизменной при фиксированном фокусном расстоянии. Матрица [Я \ используется для описания движения камеры вокруг статичной картинки, или наоборот, движения объекта перед закрепленной камерой. То есть данная матрица переводит координаты точки в систему координат,
фиксированную по отношению к камере. Такое преобразование эквивалентно следующему:
-х- X
У = Я У + V, (20)
2
где матрица перевода, вектор перевода.
У = У->{ 22)
и = /х * X + сх; (23) V = [У*у + су) (24) г Ф 0; (25)
Рисунок 6. Модель камеры
Результаты комплексирования навигационной информации
При взлете микроБЛА, вследствие увеличения скорости вращения винтов на барометр оказывается сильное шумовое воздействие, и его показания искажаются, становясь отрицательными. Высота, полученная путем двойного интегрирования показаний акселерометра, также становится отрицательной.
Рисунок 7. Показания высоты при взлете микроБЛА
При подъеме на определенную высоту видно, что барометр выдает зашумленные показания с амплитудой в пределах 30 см. При использовании акселерометра, при подъеме высота получена с достаточно высокой точностью, хотя имеет место перерегулирование. Однако при стабилизации высоты ошибки в акселерометре продолжают накапливаться, и показания начинают «уходить» от истинного значения высоты.
Рисунок 8. Показания высоты при выводе микроБЛА на высоту 2.5 метра
Высота, полученная при помощи комплексирования, практически не имеет шумов, а скорость схождения к истинному значению меньше 0.5с. Точность работы алгоритма составляет 15 сантиметров.
Оценки СКО и математического ожидания по измерениям высоты представлены в таблице 2.
Статистическая характеристика Значение
Математическое ожидание 0,1103
Среднеквадратичное отклонение 0,1281
Таблица 2. Статистические характеристики алгоритма.
Библиографический список
1. Первый квадрокоптер, 2014. URL: https://geektimes.ru/post/240916
2. UPS wants to turn its delivery trucks into motherships for autonomous drones, 2017, available at: http://www.businessinsider.com/ups-test-delivery-truck-autonomous-drones-2017-2? ga=1.135060529.1274288254.1488796477%2F
3. Chovancova A., Fico T., Hubinsky P., Duchon F. Comparison of various quaternion-based control methods applied to quadrotor with disturbance observer and position estimator // IEEE Robotics and Autonomous Systems, 2016, no. 79, pp. 87 - 98.
4. Огольцов И.И., Рожнин Н.Б., Шеваль В.В. Разработка математической модели пространственного полета квадрокоптера // Труды МАИ. 2015. № 83. URL: http://trudymai.ru/published.php?ID=62031
5. Фильтр Маджвика, 2015. URL: https://habrahabr.ru/post/255661
6. 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, рр. 179 - 185.
7. Sebastian O.H. Madgwick. An efficient orientation filter for inertial and inertial/magnetic sensor arrays, April 30, 2010, available at: http://x-io.co.uk/res/doc/madgwick internal report.pdf
8. Колосовская Т.П. Субоптимальный алгоритм оценивания и параметрической идентификации для навигационных систем летательных аппаратов и других
подвижных объектов на основе информации магнитного поля Земли // Труды МАИ. 2016. № 88. URL: http://trudymai.ru/published.php?ID=70666
9. Аксенов А.Ю., Зайцева А.А., Кулешов С.В., Ненаусников К.В. Варианты обеспечения посадки при автономном управлении беспилотными мультироторными летательными аппаратами. Труды МАИ // 2017. № 96. URL: http: //trudymai .ru/published.php?ID=85880
10. Chuang Li, Hongxin Dong, Quan Quan. A Mismatching Eliminating Method Ba sed on Camera Motion Information // Proceedings of the 34th Chinese Control Conference, July 28-30, 2015, Hangzhou, China, available at: http://rfly.buaa.edu.cn/doc/2015 LiChuang AMismatchingEliminating.pdf
11. Garrido-Jurado S., Muñoz-Salinas R., Madrid F.J. Automatic generation and detection of highly reliable fiducial markers under occlusion // Pattern Recognition, June 2014, vol. 6, no. 47, pp. 2280 - 2292.
12. John Canny. A Computational Approach to Edge Detection // IEEE Transactions on Pattern Analysis and Machine Intelligence, November 1986, vol. PAMI-8, no. 6, pp. 679 -698.
13. Satoshi Suzuki, Keiichi Abe. Topological Structural Analysis of Digitized Binary // Computer Vision Graphics and Image Processing, 1985, no. 30, issue 1, pp. 32 - 46.
14. Paul Zarchan, Howard Musoff. Fundamentals of Kalman Filtering: A Practical Approach, American Institute of Aeronautics and Astronautics, Incorporated, 2000, 664 p.
15. Nagesh Y., Bleakley C. Accurate Orientation Estimation Using AHRS under Conditions // Sensors, 2014, no. 14, pp. 20008 - 20024, doi:10.3390/s141120008.
16. Valenti R.G., Dryanovski I., Jizhong Xi. Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs // Sensors, 2015, no. 15, issue 8, pp. 19302 - 19330, available at: https://doi.org/10.3390/s150819302
17. Гоголев А.А. Полунатурное моделирование беспилотных летательных аппаратов типа мультикоптер // Труды МАИ. 2017. № 92. URL: http://trudymai.ru/published.php?ID=77238
18. Yeo D., Sydney N., Paley D., Sofge D. Onboard flow sensing for downwash detection and avoidance with a small quadrotor helicopter // AIAA Guidance Navigation and Control Conference, 2015, pp. 1769.
19. Daniel P. Magree, Gerald J.J. van Dalen, Stephen Haviland и Eric N. Johnson. Lightweight Quadrotor with On-board Absolute Vision-aided Navigation // Unmanned Aircraft Systems (ICUAS), 2015 International Conference on, Denver, 2015, pp. 1158 - 1167.
20. Magree D., Johnson E.N. A Monocular Visionaided Inertial Navigation System with Improved Numerical Stability // AIAA Guidance, Navigation, and Control Conference, Florida, January 2015, pp. 0097. DOI: 10.2514/6.2015-0097.
