Scientific journal
Fundamental research
ISSN 1812-7339
"Перечень" ВАК
ИФ РИНЦ = 1,674

QUADCOPTER POSE STABILISATION AND CONTROL METHOD USING FUSION OF VISUAL AND INERTIAL SENSORS DATA

Logachev V.G. 1 Minin I.V. 1
1 Tyumen State Oil and Gas University
This article focuses on the approach, which allows to stabilize the position and compensate quadcopter odometry drift and to perform autonomous flights to selected destinations using visual navigation in previously unknown environments without external navigation. It consists of three components – a system of visual SLAM, Kalman filter and PID to generate control commands. The status of ​​the system contains 10 variables, the current position in quadcopters dimensional coordinate system, expressed in meters, the velocity in meters per second on each of the coordinates, as well as roll, pitch, yaw in degrees and the angular speed in degrees per second. The approach is tested using the Parrot AR.Drone 2.0 quadcopter as laboratory quadcopter in different environments, indoors and outdoors. It allows to control quadcopter with accuracy up to within 4 cm in indoor environment and to 20 cm and outdoor, and system is robust with link at delays of up to 500 ms. Described and tested possibility of accurate and reliable flight in visual figures.
pose estimation
pose keeping
stabilization system
visual navigation
SLAM
gyroscope
accelerometer
Kalman filter
PID controller
1. Achtelik M., Weiss S., Siegwart R. Onboard IMU and monocular vision based control for MAVs in unknown in-and outdoor environments // Robotics and automation (ICRA), 2011 IEEE international conference on. IEEE, 2011. рр. 3056–3063.
2. Blosch M. et al. Vision based MAV navigation in unknown and unstructured environments // Robotics and automation (ICRA), 2010 IEEE international conference on. IEEE, 2010. рр. 21–28.
3. Brand C. et al. Stereo-vision based obstacle mapping for indoor/outdoor SLAM // Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on. IEEE, 2014. рр. 1846–1853.
4. Fu C. et al. Monocular visual-inertial SLAM-Based collision avoidance strategy for fail-safe UAV using fuzzy logic controllers // Journal of Intelligent & Robotic Systems. 2014. Vol. 73. no. 1–4. рр. 513–533.
5. Klein G., Murray D. Parallel tracking and mapping for small AR workspaces // Mixed and Augmented Reality, 2007. ISMAR 2007. 6th IEEE and ACM International Symposium on. IEEE, 2007. рр. 225–234.
6. Krajník T. et al. AR-drone as a platform for robotic research and education // Research and Education in Robotics-EUROBOT 2011. Springer Berlin Heidelberg, 2011. рр. 172–186.
7. Li R. et al. LIDAR/MEMS IMU integrated navigation (SLAM) method for a small UAV in indoor environments // Inertial Sensors and Systems Symposium (ISS), 2014 DGON. IEEE, 2014. рр. 1–15.
8. Lindsey Q., Mellinger D., Kumar V. Construction with quadrotor teams // Autonomous Robots. 2012. Vol. 33. no. 3. рр. 323–336.
9. Lupashin S. et al. A simple learning strategy for high-speed quadrocopter multi-flips // Robotics and Automation (ICRA), 2010 IEEE International Conference on. IEEE, 2010. рр. 1642–1648.
10. Mellinger D., Michael N., Kumar V. Trajectory generation and control for precise aggressive maneuvers with quadrotors // Experimental Robotics. Springer Berlin Heidelberg, 2014. рр. 361–373.
11. Muller M., Lupashin S., DAndrea R. Quadrocopter ball juggling // Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on. IEEE, 2011. рр. 5113–5120.
12. Zingg S. et al. MAV navigation through indoor corridors using optical flow // Robotics and Automation (ICRA), 2010 IEEE International Conference on. IEEE, 2010. рр. 3361–3368.

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

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

Задача точного измерения движения в произвольном и неизвестном заранее пространстве является одной из главных в области компьютерного зрения и робототехники и широко известна как проблема одновременной навигации и составления карты (simultaneous localization and mapping – SLAM). Идея очень проста – составлять карту окружающего пространства, используя сенсоры. Карта в дальнейшем используется для повторного определения текущей позиции через некоторый период времени. Таким образом SLAM отвечает на вопросы «Как выглядит окружающий мир?» и «Где я нахожусь?».

Для решения проблемы SLAM в данное время предлагается использовать самые различные сенсоры, такие как стереокамеры [3], лазерные сканеры – лидары [8], RGB-D камеры [4]. Данные решения неоправданно использовать для решения проблемы SLAM на малых БПЛА в качестве средства противопожарного мониторинга и локального пожаротушения в связи с их дороговизной и тяжелым весом – предполагаемая потеря БПЛА не должна принести значительного финансового ущерба.

Рассмотрим данную проблему на примере малого БПЛА с четырьмя фиксированными роторами – квадрокоптера. С нашей точки зрения, встроенные в квадрокоптер легкие и недорогие камеры имеют значительное преимущество в том, что их радиус действия не ограничен, позволяя применять однообъективный SLAM как внутри помещений, так и на открытых производственных площадках. Однако существует ряд ограничений, так как масштаб окружающего мира не может быть определен при помощи одного лишь изображения – необходимы дополнительные источники данных, и в случае с квадрокоптером наиболее целесообразно использовать данные инерциальных измерений IMU (Inertial measurements unit – устройство для осуществления инерциальных измерений).

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

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

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

pic_51.tif

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

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

Для компенсации накопления ошибки было предложено использование множества различных методов SLAM [1, 2], однако серьезной проблемой является определение масштаба, и оно не может быть совершено без привлечения дополнительных сенсоров, таких как IMU.

В качестве лабораторной платформы используется квадрокоптер Parrot AR.Drone 2.0, доступный в свободной продаже. В сравнении с квадрокоптерами фирм DJI или Ascending Technologies преимуществами являются невысокая цена, высокая надежность и доступность запасных частей. Стандартная комплектация также включает в себя защитную рамку, которая позволяет безопасно выполнять полеты внутри помещений. В то же время ни программное, ни аппаратное обеспечение данной модели невозможно заменить или изменить стандартными методами; связь с квадрокоптером доступна только через беспроводную сеть стандарта 802.11g. Вес со стандартной батареей и рамкой для полетов внутри помещений составляет около 420 грамм.

Квадрокоптер укомплектован 3-осевыми гироскопом и акселерометром, ультразвуковым альтиметром и двумя камерами. Первая камера направлена вперед, имеет разрешение 640×480, угол обзора составляет 93° и обладает дефектом rolling shutter с задержкой около 40мс между первой и последней захваченной линией. Видео с фронтальной камеры транслируется на компьютер с частотой 20 кадров в секунду. Вторая камера направлена вниз, имеет разрешение 320×240 и угол обзора 64°; используется встроенным программным обеспечением для определения горизонтальной скорости. Квадрокоптер транслирует данные гироскопа и горизонтальную скорость с частотой 200 Гц, ультразвуковой альтиметр передает данные с частотой 25 Гц. Исходные, не модифицированные встроенным программным обеспечением данные недоступны для использования. Встроенное программное обеспечение использует эти сенсоры для управления по каналам крена Φ и тангаж Ψ, в то время как скорость рыскания logachev01.wmf и вертикальная скорость logachev02.wmf задаются при помощи внешнего эталонного значения u. Это значение устанавливается каждые 10 мс отправкой управляющей команды logachev03.wmf

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

1. Однообъективный SLAM. Для реализации однообъективного SLAM наш подход использует библиотеку PTAM – Parallel Tracking and Mapping [5]. После инициализации карты производится вращение карты в плоскости x, y в соответствии с данными акселерометра, и масштабирование с параметром средней глубины ключевой точки равным 1. В дальнейшем масштабирование полученной карты выполняется при помощи метода, описанного ниже. Также определяется положение квадрокоптера в пространстве применением расширенного фильтра Калмана для определения и отбрасывания некорректных измерений.

pic_52.tif

Рис. 2. Предсказание положения. Измерения и управляющие команды поступают со значительными задержками. Для их компенсации предлагается хранить историю измерений и посылать управляющие команды между t – Δtвиз и t – Δtупр

2. Расширенный фильтр Калмана. Для комбинирования и фильтрации всех получаемых данных применяется расширенный фильтр Калмана (EKF). Нами была рассчитана и откалибрована комплексная модель динамики полета квадрокоптера и его реакции на управляющие команды, детали которой описаны далее. Также EKF используется для компенсации различных временных задержек в системе, появляющихся в результате UDP/IP коммуникации в беспроводной сети, а также во время вычисления визуального трекинга (рис. 2).

3. PID управление. ПИД регулятор применяется для вычисления управляющих команд и направления квадрокоптера к желаемой позиции logachev04.wmf в глобальной системе координат. В соответствии с текущим состоянием мы преобразуем полученные управляющие команды в систему относительно самого робота и передаем команды квадрокоптеру. Для каждой из 4 степеней свободы используется отдельный ПИД контроллер, тип которого был экспериментально определен как наиболее подходящий.

Графическое представление предложенного подхода изображено на рис. 1.

Применение EKF к наблюдению и предсказанию. Область состояния состоит из 10 переменных:

logachev05.wmf (1)

где (xt, yt, zt) обозначает текущую позицию квадрокоптера в метрах и logachev06.wmf скорость в метрах в секунду в мировых координатах, крен Φt, тангаж Θt, рыскание Ψt и угловую скорость logachev07.wmf в градусах/с. В дополнение для каждого сенсора определяется функция наблюдения h(xt), которая описывает, как вектор наблюдений zt вычисляется из исходных данных сенсоров.

Квадрокоптер измеряет свою горизонтальную logachev08.wmf и logachev09.wmf своих локальных координатах, которые трансформируются в глобальные logachev10.wmf и logachev11.wmf. Угол крена logachev12.wmf и тангажа logachev13.wmf, измеряемые акселерометром, являются прямыми измерениями Φt и Θt соответственно. Для вычисления дрейфа рыскания производится дифференцирование измерения высоты logachev14.wmf и рыскания logachev15.wmf, и считаем их наблюдениями соответствующих скоростей. Следовательно, функцию наблюдения и вектор измерений можно записать как

logachev16.wmf (2)

logachev17.wmf (3)

где δt обозначает время, которое прошло с момента t до момента t + 1.

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

hP(xt) := (xt, yt, zt, Φt, Θt, Ψt,)T; (4)

zP, t := f(EDC EC, t), (5)

где EC, t ∈ SE(3) является вычисленным положением камеры (масштабировано с λ); EDC ∈ SE(3) – статическое преобразование из координатной системы камеры в координатную систему квадрокоптера, и f:SE(3) → 6 преобразование из пространства SE(3) к представлению крен – тангаж – рыскание.

Модель предсказания показывает, как вектор состояния xt изменяется со временем. В частности, аппроксимируется горизонтальное ускорение квадрокоптера logachev18.wmf, основанное на текущем состоянии xt, и вычисляется вертикальное ускорение logachev19.wmf, ускорение вращения рыскания logachev20.wmf и скорость вращения по углам крена logachev21.wmf и тангажа logachev22.wmf основанная на комбинации текущего состояния xt и активного управляющего состояния ut.

Горизонтальное ускорение пропорционально горизонтальной силе, действующей на квадрокоптер, задаваемой как

logachev23.wmf (6)

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

logachev24.wmf (7)

logachev25.wmf (8)

где R(•)i, j обозначает записи в матрице вращения, определенные углами крена, тангажа и рыскания.

Данная модель подразумевает постоянную тягу всех четырех роторов. Также аппроксимируется влияние отправляемых управляющих команд logachev26.wmf c помощью линейной модели:

logachev27.wmf (9)

logachev28.wmf (10)

logachev29.wmf (11)

logachev30.wmf (12)

Коэффициенты c3…c8 определяются тестовыми полетами квадрокоптера. Общее состояние системы задается как

logachev31.wmf (13)

Модель, описанная в формулах (7)–(13), не претендует на физическую правильность из за многочисленных допущений, но очень хорошо работает на практике, в основном за счет своей полноты: поведение всех параметров состояния и действие всех команд управления аппроксимируется, позволяя «слепой» прогноз, т.е. предсказание без наблюдений в течение короткого периода времени (~125 мс на практике).

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

Для анализа точности оценки масштаба квадрокоптеру было поручено пролететь заданную фигуру, в то же время каждую секунду брался образец данных и масштаб оценивался повторно. В первом тестовом наборе квадрокоптеру требовалось перемещаться только по вертикали, так как образцы данных в большей степени состояли только из измеренной высоты. Во втором наборе тестов квадрокоптеру требовалось пролететь горизонтальный прямоугольник, и в данном случае образцы данных состоят из измерений скорости при помощи встроенного IMU. После каждого полета мы измеряли истинную высоту logachev32.wmf над поверхностью вручную, устанавливая квадрокоптер к двум точкам измерения и сравнивая известную дистанцию с измеренной системой визуальной навигации. Касательно первоначальной оценки значения logachev33.wmf соотносятся со средней «глубиной» контрольных точек первого ключевого кадра, которая в наших экспериментах имела значение от 2 до 10 м. Для большего удобства сравнения, мы проанализировали и визуализировали ошибку logachev34.wmf в соответствующую оцененной длине в 1 м.

На рис. 3 представлена средняя ошибка, а также расхождение на протяжении 10 полетов. Как видено, наш метод быстро и точно определяет масштаб для двух типов движения квадрокоптера. В связи с повышенной точностью альтиметра в сравнении с измерением горизонтальной скорости, определение положения в пространстве происходит быстрее и точнее при вертикальном движении квадрокоптера, то есть схождение происходит через 2 секунды в сравнении с 15 секундами при горизонтальном движении, и финальная точность ±1,7 % в сравнении с ±5 %. Но в практическом применении мы рекомендуем использовать произвольные движения, что даст большее количество информации и значительно повысит точность и время сходимости.

Точность позиционирования. Также было произведено тестирование с точки зрения стабилизации положения квадрокоптера. Было задано удержание позиции квадрокоптером в течение 60 с в различных окружениях и измерена среднеквадратичная ошибка. Результаты представлены на рис. 3: измеренная ошибка находится между 4,9 см (внутри помещений) и 18 см (снаружи).

pic_53.tif

pic_54.tif

Рис. 3. Точность оценки расстояния и стабильность полета в различных окружениях

pic_55.tif

Рис. 4. Горизонтальная траектория полета квадрокоптера при выполнении полета по фигуре и удержании заданной позиции

Минимизация ухода измерений. Для проверки того, что визуальный SLAM минимизирует уход показателей одометрии, было произведено сравнение оцененной траектории с и без использования системы визуального SLAM. Рис. 4 показывает результирующие траектории с пролетом по фигуре и с удержанием заданной позиции, причем квадрокоптер был перемещен вручную с исходной позиции. Оба полета занимали около 40 с, и квадрокоптер был приземлен не более чем в 15 см от его исходного положения. В сравнении с исходными данными одометрии, накопленная ошибка составила 2,1 м для полета по фигуре и 6 метров для второго случая. Этот эксперимент демонстрирует то, что комбинирование визуального SLAM и инерциальных измерений эффективно минимизирует уход измерений положения во время совершения маневров.

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

Рецензенты:

Борзых В.Э., д.ф.-м.н., профессор, заведующий кафедрой информатики и информационных технологий, ФГБОУ ВПО «Тюменский государственный архитектурно-строительный университет», г. Тюмень;

Якубовский Ю.Е., д.т.н., профессор, заведующий кафедрой прикладной механики, ФГБОУ ВПО «Тюменский государственный нефтегазовый университет», г. Тюмень.