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

Таким образом, QY  QT ,что позволяет эффективно повысить скороть вычисления алгоритма.Разработка алгоритма отслеживания спланированного маршрута наоснове улучшенного метода «L1» с адаптивным выбором опорной точкиотслеживания.Принцип метода «L1» для решения задачи отслеживания заданного маршрутаквадрокоптера показан на Рис. 1, где V – вектор скорости; L1 – вектор отквадрокоптера до опорной точки;  – угол упреждения; d – кратчайшеерасстояние от центра массы квадрокоптера до траектории (точек маршрута); a c –вектор нормального ускорения, необходимого для отслеживания маршрута; D –опорная точка; O – центр вспомогательной окружности; R – радиусвспомогательной окружности; C – точка центра массы квадрокоптера.Рис.

1. Принцип алгоритма отслеживанияНормальное ускорение, необходимое для отслеживания заданного маршрута,определяется следующим образом:ac  V 2 R  V 2 L1 2 sin   2V 2 sin  L1Процесс выбора опорной точки можно разделить на два этапа:1) выбор ближайшей точки маршрута D1 при условии: L1  d ; 1   / 2 ;2) определение порядкого номера опорной точки N в множестве точекмаршрута:0, m  1;r  1 m1N  n  ;   pq   floor m  1  sin 1  1  d  1      i   , m  2i  7где n  порядкий номер точки D1 ;   функция опорной точки; p, q, r показатели влияния угла упреждения, расстояния, углов поворота маршрута(определяются путем предварительного моделирования); m – количество точек вэтой области;   угол между точками маршрута.Разработка нового алгоритма облета различных препятствий.

Дляопределения координат, скоростей и размеров препятствий, предлагаетсяиспользовать прогноз движения на основе известного сигма-точечного ФК(Uncented Kalman Filter – UKF).Уравнения состояний и наблюдения системы в момент k :X k  f X k 1, u k , w k Z k  hX k , v k где X k  [Xb _ k , Vb _ k , R k ]T - вектор состояния движения препятствий; Xb _ k координаты препятствий;Vb _ k  скорости препятствий;Rk радиусыпрепятствий; Z k  [Dk , α k , ηk ]T - вектор наблюдения; D k  расстояния; α k углы между касательными; η k  углы между вектором скорости и направлениемна препятствие (линией визирования препятствия); u k - управляющее воздействие;w k и v k - шумы процесса и измерений.На Рис.

2 показан принцип предлагаемого алгоритма облета неподвижныхпрепятствий в двумерной среде.Рис. 2. Стратегия облета неподвижного препятствияДля включения процесса облета неподвижных препятствий, необходимыопределенные условия:- di  d r , d r  порог расстояния облета;- вектор скорости робота Vc находится в области наблюдения scope _ i и8внутри угла  i .Для облета неподвижных препятствий нужно вычислить минимальныеускорения, необходимые для исключения столкновений, и умножить накоэффициенты безопасности облета.

Эти коэффициенты выбираются, исходя изстепени безопасности, требуемой в конкретной задаче. Выражения минимального ибезопасного нормальных ускорений, необходимых для исключения столкновения спрепятствием i , можно записать следующим образом:Vc2Vc22  Vc2  sin ai ; aci  ki  aiRG d AD 2  sin  d ADгде ki  коэффициент безопасности для препятствия i .В качестве нормального ускорения для облета всех препятствий выбираетсясамое большое из множества безопасных нормальных ускорений, т.е.ac  maxac1, ac2 ,, aci ,, acn  .Подвижные препятствия определяются из условия vbi _ k  vt , где vtвыбирается, исходя из погрешности визуальной системы. Облет подвижныхпрепятствий производится путем предсказания местоположений столкновения,после чего на этих местах размещаются виртуальные неподвижные препятствия вкачестве объекта облета.Преимущества подобной стратегии:1) Уменьшение ненужных действий при облете препятствий, что делает полетболее стабильным, а скорость вычисления алгоритма более высокой;2) Снижение влияния визуального слепого поля на эффективность алгоритмаоблета подвижных препятствий.Разработка многорежимной системы стабилизации для квадрокоптера всреде с атмосферным влияниям.

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

В среде с умеренным ветром можно выбиратьрегуляторы с меньшим временем регулирования. Когда ветер превышаетдопустимый предел, следует выбирать регуляторы с наименьшим временемрегулирования, а подсистему траекторного управления переводить в режимвозврата к исходной точке.В третьей главе представлена КНС на основе ФК с возможностьюобнаружения неисправности и слияния нескольких источников НИ свозможностью изоляции неисправности. На Рис. 3 показана схема КНС, где Z A – выходы ВНС с ошибками; Z G – выходы СНС с ошибками; h B , h R – выходывысотомеров с ошибками; Z S – выходы БИНС с ошибками; Z O – наблюдаемые НСпосле слияния данных датчиков; X r – НС состояния системы после слияния9данных датчиков; M – матрица обнаружения неисправности.Рис.

3. Схема КНСПреобразователь режимов навигации. ВНС используется в случаеотсутствия сигнала СНС или при необходимости более точной информации о среде(например, для облета препятствий или посадки на указанном месте), т. е.выбирается режим навигации БИНС/ВНС/BA/RA или БИНС/СНС/ВНС/BA/RA. Востальных случаях выбирается режим навигации БИНС/СНС/BA/RA.ВНС с локальным диапазоном наблюдения и локальной ассоциациейданных. В открытой среде, одновременно с приращением числа наблюдаемыхориентиров, будут также постоянно увеличиваться размер вектора состояниясистемы и сложность вычисления алгоритма.

Чтобы избежать этого, предлагаетсяввести известный EKF-SLAM алгоритм адаптации зоны наблюдения. Сутьалгоритма состоит в использовании локальной круговой карты для текущей оценкикоординат аппарата и локализации зоны используемых ориентиров в глобальнойсистеме координат. Локализация зоны наблюдения показана на Рис. 4.На Рис. 4: S –круговая локальная карта;S1 – диапазон наблюдения;дополнительныйS2 –диапазон; чёрные точки –ориентиры.Рис. 4.

Круговая локальная карта и диапазоннаблюденияЕсли число ориентиров Num в наблюдаемой области S1 меньше, чемминимально необходимое для надежной коррекции прогнозируемого векторасостояния Num min ( Num  Num min ), и радиус наблюдения R меньше, чем10максимальный радиус Rmax надежного наблюдения ( R  Rmax ), то предлагаетсяувеличить радиус локальной карты. Если число ориентиров больше, чеммаксимальное число ориентиров Num max , позволяющее избегать чрезмернойизбыточности коррекции ( Num  Num max ), или радиус наблюдения больше, чеммаксимальный радиус надежного наблюдения ( R  Rmax ), то предлагаетсяуменьшатьрадиуслокальнойкарты.КогдачислоориентировNum min  Num  Num max и R  Rmax , то радиус локальной карты остаётсянеизменным.Для дальнейшего повышения скорости вычисления алгоритма SLAMпредлагается алгоритм локальной ассоциации данных SLAM на основеулучшенного МА, реализуемый в два этапа, как показано на Рис.

5.Рис. 5. Схема алгоритма ассоциацииЧтобы уменьшить объём вычислений алгоритма, вместо глобальнойассоциации используем локальную. Поскольку в процессе наблюдения ипредсказания состояния существуют ошибки, необходимо добавить в пространствосовпадения область за пределами области наблюдения.Улучшения МА для избегания локального минимума, увеличенияпространства поиска и повышения скорости вычисления МА включают:- добавление хаотической последовательности в процесс обновленияглобальногоферомонаввидевозмущения: ij t  1  1    ij t    ij t   zij ;- установление границы феромона  ij   min , max  .На первом этапе работы алгоритма определяются ориентиры в пространствесовпадений и наблюдаемые ориентиры, имеющие возможности ассоциации покритерию индивидуальной совместимости (Individual Compatibility – IC).

Навтором этапе определяются совпадающие ориентиры и координаты совпадающихнаблюдаемых ориентиров на множестве состояний при помощи улучшенного МА.Разработка ФК с возможностью обнаружения неисправности. В даннойработе была осуществлена разработка ФК с возможностью обнаружения11неисправности. В качестве примера на Рис. 6 показана схема обработкиинформации о высоте.Рис. 6. Схема ФК с возможностью обнаружения неисправностиУравнение состояний интегрированной навигационной системы имеет вид:XO  t   F  t  XO  t   G  t  W  t XO   E , N ,U , v E , v N , vU , L, , h,  x ,  y ,  z ,  x ,  y ,  z T – векторсостояния; буквами E , N и U обозначены направления на восток, север ивверх; φE , φN и φU – ошибки определения углов ориентации в инерциальнойсистеме координат (ИСК); δvE , δvN и δvU – ошибки определения скоростиБПЛА в ИСК; δL , δλ и δh – ошибки определения широты, долготы и высоты;ε x , ε y и ε z – случайные погрешности датчиков угловой скорости (ДУС) в ССК;где x ,  y и  z – случайные погрешности акселерометров; F – матрица перехода;G – матрица влияния шумов системы; W – вектор шумов системы (предполагается– «белый» шум).Уравнение измерений основного ФК « O » можно записать в виде:ZO (t )  Z S (t )  ZO (t )  H o (t )Xo (t )  Vo (t )где Z O - информация об измерениях после слияния; Z S - состояния БПЛА попоказаниям БИНС с ошибками; Vo – вектор шумов наблюдения; H o – матрицаизмеренийУравнения измерений для суб-фильтра Калмана « A », « G »,« B » и « R »можно записать в виде:Z ah (t )  hS  hA  H ah (t ) X(t )  Vah (t )Z gh (t )  hS  hG  H gh (t ) X(t )  Vgh (t )Zbh (t )  hS  hB  Hbh (t ) X(t )  Vbh (t )Z (t )  h  h  H (t ) X(t )  V (t )SRrhrh rhгде hS , hG , h A , hB и hR – информация о высоте по показаниям БИНС, СНС,SLAM, барометрического высотомера и радиовысотомера; Vah , Vgh , Vbh и Vrh –12вектор шумов наблюдения высоты СНС, SLAM, барометрического высотомера ирадиовысотомера с дисперсиями R ah , R gh , R bh и R rh ; H ah , H gh , Hbh и H rh –матрицы измеренийЧтобы повысить вероятность обнаружения неисправности, в данной работебыл предложен обнаружитель неисправности с параллельной структурой,включающей тест  2 по остаточной ошибке и тест  2 по состоянию с двойнымпрогнозом состояния.В качестве примера приведем схему обнаружителя неисправности G на Рис.7.Рис.

7. Обнаружитель недостоверности информации о высотеАналогичные фильтры были применены и для других видов НИ.Использование нескольких источников НИ с возможностью изоляциинеисправности. Если используем несколько датчиков для измерения одногопараметра, то можно слить выходы всех датчиков, назначая веса для выходакаждого датчика.Результат слияния измеренной информации можно записать следующимобразом:Y  WMX  w1, w2 ,, wn diagm1, m2 ,, mn x1, x2 ,, xn Tгде Y – результат слияния; W  w1, w2 ,, wn  – матрица весов; X  x1, x2 ,, xn –выходы датчика; n - количество датчиков; M – матрица обнаружениянеисправности.Принцип назначения весов для выхода каждого датчика можно записатьследующим образом:nw j    2j 1  i2 i1n;wj 1jгде  i и  j – дисперсия ошибки выхода i –ого и j –ого датчика.Если в момент k датчик j получает измеренное значение T j k  , то:1 N1 N2T j k   T j k   Tˆ j k ; T j T j k ;  j k  T j k   T j , k  1,2,3,, NN k 1N k 1где T j k  – ошибка измерения j -ого датчика в момент k ; T j – среднее значениеj -ого датчика в момент k ;  j k  – дисперсия ошибки выход j -ого датчика вмоменте k ; Tˆ j k  – прогностическая величина, полученная с помощью ФК; N –число измерений с каждого датчика.13В четвертой главе проводится макетирование навигационной системы внаружной среде и комплексное моделирование разработанной автономной системыуправления полетом квадрокоптера в программной среде Matlab.Схема структуры комплексной математической модели автономной системыуправления полетом квадрокоптера показана на Рис.