Диссертация (Автономная система управления полетом квадрокоптера с возможностью облета препятствий и комплексной навигацией), страница 6

Это можетсвидетельствовать о правильности метода. Время реакции равно 0,138629 с, чтодоказывает быстродействие метода.На Рис. 1.13 показаны системы координат, которые были использованы вданной работе:OB X BYB Z BOV , k XV , k YV , k ZV , kOW , k XW , k YW , k ZW , k– базовая неподвижная система координат;– связанная система координат БПЛА в момент k;–системакоординаткамерывмоментk;OC1, k X C1, k YC1, k ZC1, k – система координат камеры для первого кадра изображения вмомент k; OC2, k X C2, k YC2, k Z C2, k – система координат камеры для второго кадраизображения в момент k.Рис.1.13.

Системы координат для момента времени k37По визуальной одометрии [37] можно вычислить координаты характерныхточек и центра масс БПЛА:X f  LB  LV  R B  RV  X fVkWVkWw, k b, k vBBvXb, k  LVk  RVk  X w, kBRVk  R k 1LBVk   L k 1где Xbf,k  xbf,k , ybf,k , zbf,kточекицентраX wf ,k  xwf ,k , ywf ,k , zwf ,kTмассTи Xbv ,k  xbv,k , ybv,k , zbv,kБПЛАвT– координаты характерныхнеподвижнойvvvи Xvw,k  xw, k , yw, k , zw,kT(1.33)системекоординат;– координаты характерныхточек и центра масс БПЛА в связанной системе координат; RVB и LBV , k – матрицывращения и перемещения системы координат БПЛА на неподвижную системуVкоординат; RVW и LW – матрицы преобразования и матрица перемещениясистемы координат камеры на систему координат БПЛА; R k и L k – матрицывращения и перемещения системы координат первого кадра изображения насистему координат второго кадра изображения.В начальный момент система координат БПЛА совпадает с неподвижнойсистемой координат.

В своей работе считаем, что движение бортовой камеры сдостаточной точностью совпадает с движением БПЛА, т.е. матрицу RVWпринимаем единичной, а LVW – нулевой. Использование только визуальногоалгоритма компьютерного зрения не позволяет точно получить масштабныйкоэффициент (какправило,используетсясочетаниеизмерительным блоком), поэтому ошибка матрицы LBVсинерциальнымувеличивается современем. Для решения этой проблемы в данной работе получение координатБПЛА проводится алгоритмом SLAM, а не визуальной одометрией, то естькоординаты характерных точек можно получить из уравнения:38BXbf,k  Xbv ,k  RVkXwf ,k(1.34)Для алгоритма планирования полѐта БПЛА целесообразно использоватьмодель, описывающую только кинематику траекторного движения.

При этомможно принять, что на каждом шаге алгоритма скорость полѐта не меняется, аизменения скорости от шага к шагу ограничены располагаемыми ускорениями.Вектор состояния модели движения камеры на БПЛА и матрицу координаториентиров в неподвижной системе координат можно записать следующимобразом:XV  Xbv , Vbvгде Vbv  vxbv , v y v , vz bvbXbf ,i  xbf ,i , ybf ,i , zbf ,iT ; XM  Xbf ,1, Xbf ,2 ,Xbf ,i ,, Xbf ,n T(1.35)T – скорость камеры в неподвижной системе координат; – координаты i-ого ориентира в неподвижной системеTкоординат; n – количество наблюдаемых ориентиров.Расширенный вектор состояния камеры на БПЛА и ориентиров карты внеподвижной системе координат:X  XV , XM T(1.36)По принципу визуальной одометрии [37], уравнения состояния движениякамеры на БПЛА и ориентиров карты можно записать в виде: Xv  V v  Ω  t b ,k b, kT vX k 1  XV ,k 1, X M ,k Vb ,k  ΩfvB Xb,k  RVk X w,k (1.37)где Ω – гауссовские белые шумы с нулевым средним.Вектор наблюдения:Zk  X wf ,k(1.38)Оценка вектора состояния в алгоритме одновременной локализации икартографирования (SLAM) для рассматриваемой модели, которая являетсянелинейной, но с «гладкими» нелинейными функциями в правых частях уравнений39состояний и наблюдений, может быть получена с помощью расширенного фильтраКалмана (Extended Kalman filter – EKF), в котором на каждом шаге проводитсялинеаризация посредством разложения в ряд Тейлора с отбрасыванием членов рядавыше первого порядка.Если нелинейную модель БПЛА и ориентиров карты записать в видеXk 1  f Xk , uk   ε kS k  hXk   ηk(1.39)где uk  ak – управляющие переменные; ε k , ηk – гауссовские белые шумы снулевым математическим ожиданием и ковариациями Q and R соответственно,то применение расширенного фильтра Калмана для алгоритма SLAM можнопредставить в виде следующей последовательности действий:А.

Прогноз («одношаговый») состояния и ковариационной матрицы ошибок сиспользованием локальной линеаризации нелинейной системы:Xˆ k , uk , M k T ˆ k 1|k  f XPk 1|k  Fk Pk FkT  G k Q k GTkгде Fk (1.40)ˆ k , ukf Xf Xk , uk , Gk матрицы частных производныхˆkuXˆkˆ k ,u kX k ,u kXпервого порядка (матрица Якоби) при разложении нелинейных функций f Xk , uk в ряд Тейлора в окрестности оценки на k-ом шаге ( Xˆ k , uk ) :ˆ k , uk  f Xk , uk ˆkf Xk , uk   f X Xk  XXkˆ k ,u kX(1.41)Как видно из приведенного выше анализа, ковариационная матрица может бытьзаписана следующим образом:Pvm6  3n   P 6  6Pk 3n  6  3n  6   vvPmv 3n  6 Pmm (3n  3n)(1.42)40где Pvv – ковариационная матрица положения и ориентации робота; Pmv , Pvm –кросс-ковариационная матрица робота и ориентиров; Pmm – ковариационнаяматрица положения ориентиров.Б.

Определение ошибки прогноза и ковариационной матрицы по наблюдениямна шаге k+1:~Tˆ k 1|k ; Jy k 1  sk 1  h Xk 1  H k 1Pk 1|k H k 1  R k 1(1.43)где H k 1 – матрица Якоби при разложении нелинейных функций hXk  в рядˆ k 1|k .Тейлора в окрестности Xˆ k 1  Xˆ k 1|k  K k 1~y k 1 и ковариационнойВ.

Коррекция оценок состояния Xматрицы Pk 1  I  K k 1Hk 1 Pk 1|k , где K k 1  Pk 1|k HTk 1Sk11.Г. Расширение вектора состояния и ковариационной матрицы.Ориентиры, обнаруженные датчиками аппарата на каждом шаге, включаюториентиры,ужесуществующиенакарте,атакженовыеориентиры.Существовавшие ориентиры были использованы для оценок состояния навышеприведѐнных этапах. Новые ориентиры добавляются в вектор состояниясистемы через процесс инициализации.

Пусть на шаге k j-ый наблюдаемыйориентир в векторе наблюдения с координатами X wf ,,kj является новым. Егокоординаты в неподвижной системе координат:BXvf,,kj  g XV ,k , Zk   Xbv ,k  RVkX wf ,,kj(1.44)Новый вектор состояния системы после расширения: Xk Xnew,k   f , j  X w,k (1.45)Ковариационная матрица нового вектора состояния:PvvPnew,k   Pmvg X Pvv TPvmPmmg ZPmm Tg X Pvvg Z PmmTTg X Pvvg X  g Z Pmmg Z (1.46)41где g X , g Z – матрица Якоби g XV ,k , Xwf ,,kj для XV ,k и Z k .Признано, что ассоциация данных является одной из основных проблем длярешения задачи SLAM. Под ассоциацией данных понимается установлениеотношения эквивалентности между наблюдаемыми ориентирами и ориентирами ввекторе состояния.

Даже небольшое число ложно ассоциированных совпаденийможет приводить к расхождению алгоритма SLAM. Таким образом, алгоритмассоциации данных оказывает важное влияние на точность и надежностьалгоритма SLAM.В последние годы было предложено много способов ассоциации данных дляалгоритма SLAM. Наиболее часто используемым алгоритмом ассоциации данных вних является алгоритм ICNN (Individual Compatibility Nearest Neighbor –Индивидуальной Совместимости Ближайшего Соседа). В этом алгоритме дляассоциации каждого наблюдаемого ориентира производится сравнение расстояниядо существующих ориентиров с заранее заданным на основании экспертныхоценок порогом. Если расстояние между наблюдаемым и уже существующим накарте ориентирами меньше определенного порога, то наблюдаемый ориентирсовмещается с этим существующим ориентиром.

Когда на карте имеется несколькосовместимых с наблюдаемым ориентиров, то в качестве ассоциированноговыбирается ориентир с минимальным расстоянием.Учитывая ошибки измерений, в качестве расстояния между i-м наблюдаемымориентиром и j-м ориентиром на локальной карте целесообразно использоватьрасстояние Махаланобиса [49]:Dij  yijT J 1 yij(1.47)где yij  yi  yˆ j – вектор разности координат по результатам измерений; yi –вектор измерений координат i -го наблюдаемого ориентира;ŷ j – векторкоординат j -го ориентира в локальной карте; J – ковариационная матрица ошибокизмерений.42Если Dij  Gate , то считаем i -й наблюдаемый ориентир совпадающим с j -мсуществующим. Здесь Gate – заданный порог.Если более чем один ориентир из присутствующих на локальной картеудовлетворяет условиям Dij  Gate , то в качестве ассоциированного ориентиравыбираем ориентир с минимальным нормированным расстоянием.Этот алгоритм характеризуется высокой скоростью расчѐта и простотой вреализации, но не учитывает корреляцию между ориентирами.

Из-за этогоправильность и надежность ассоциации недостаточно высока, что можетприводить к расхождению алгоритма SLAM. Алгоритмы JCBB (Joint CompatibilityBranch and Bound – Совместной Совместимости Ветвей и Границ) [49] и MHT(Multiple Hypothesis Tracking – Отслеживания Нескольких Гипотез) [47] имеютвысокую точность и надежность, но их вычислительная сложность резковозрастает с увеличением количества ориентиров, поэтому они не могут бытьприменены к окружающей среде с большим количеством ориентиров. Длярешения задачи ассоциации применялись также алгоритмы оптимизации [99], втом числе – целочисленного программирования [119], а также теория графов [28].Эти алгоритмы являются слишком сложными и не могут быть применены дляБПЛА в режиме реального времени в открытой трѐхмерной среде.1.3.3.

Основные особенности интегрированных БИНСс применением барометрического высотомера и радиовысотомераДля определения высоты можно также использовать GPS-приѐмник.Принцип действия основан на одновременном измерении расстояния донескольких (как правило, от четырѐх до шести) вещающих спутников,находящихся на известных и специально корректируемых орбитах. Но точностьпозиционирования высоты очень низкая, иногда даже ошибка может достигатьболее 1000 метров, особенно когда все доступные спутники в позиции, близкой кгоризонту. В этих случаях с помощью GPS невозможно получить точную43информацию о высоте. Поэтому нужно пользовать высотомер для коррекцииошибки предсказанной высоты БИНС в КНС в режиме реального времени.Высотой полѐта называется расстояние от поверхности земли до аппарата,рассчитанное по вертикали от некоторого уровня, принятого за начало отсчета.Для измерения высоты обычно используют альтиметры или высотомеры.

Попринципуустройствавысотометрыделятсянабарометрическиеирадиотехнические.Принципбарометрическоговысотомераоснованнаиспользованиизакономерного изменения атмосферного давления с высотой.Существует много источников ошибок, которые вызывают отклонениепоказателей высоты барометрического высотомера от истинного. Основными изних являются ошибки шумов измерения и ошибки модели атмосферы [35].Принцип барометрического высотомера является правильным, только еслиусловия реальной атмосферы соответствуют стандартную атмосфере [74, 96].Если реальная атмосфера не соответствует стандартной атмосфере, то подобныйметод неизбежно производит ошибку измерения, которую можно называтьметодической или принципиальной ошибкой.

Эта ошибка не может бытьустранена при фильтрации.Принцип действия радиовысотомера [55] основан на определении временипрохождения радиосигнала от передающей антенны до отражающей поверхности иобратно, к приемной антенне.К недостаткам барометрического высотомера можно отнести неверноеизмерение давления на борту, большую погрешность (до 10 м), измерениеотносительной высоты, а не абсолютной, и сложность работы с высотомером. Кплюсамподобноговысотомераотноситсяэкономичностьресурсовдляопределения высоты вне зависимости от рельефа.Кнедостаткамрадиовысотомераможноотнестивыраженнуюнаправленность измерений, необходимость в мощном источнике излучения.Положительным свойством радиотехнических средств является высокая точность44определения навигационных параметров и возможность их измерения в любыхпогодных условиях.Ввиду того, что барометрический высотомер и радиовысотомер имеют своипреимущества и недостатки, в целях повышения точности измерения высоты, всвоей работе используем эти два вида высотомеров совместно в качествевспомогательного навигационного инструмента для КНС.1.4.

Выводы по главе 1Таким образом, в первой главе диссертационной работы представлен анализметодов и ключевых технологий для достижения автономного полѐта БПЛА.1. Построена более подробная ММ квадрокоптера с учѐтом влияния ветра,экранногоэффектаповерхностиземлиигироскопическихмоментоввинтомоторных групп, и т.д. Разработаны системы траекторного и угловогоуправленияполѐтомквадрокоптеранаосновеПИД-регуляторовибэкстеппинг-регуляторов.2. Анализ результатов моделирования полѐта квадрокоптера по заданномумаршруту показывает, что изменения координат центра масс квадрокоптера длядвух регуляторов почти одинаковые, а диапазон изменения углового положения сбэкстеппинг-регуляторами заметно меньше, чем соответствующий диапазон дляПИД-регуляторов,икриваяболеегладкая.Такимобразом,выборПИД-регуляторов для траекторного и бэкстеппинг-регуляторов – для угловогоуправления позволяет упросить структуру системы управления и повыситьстабильность полѐта квадрокоптера.3.