CC BY
187
УДК 629.7.05
Б.О. Качанов, В.С. Кулабухов, Н.А. Туктарёв, Д.В. Гришин
Адаптивный алгоритм вычислителя гировертикали беспилотного летательного аппарата
Предлагается алгоритм определения крена и тангажа беспилотного летательного аппарата по совместным измерениям гироскопов и акселерометров. Новым является способ маятниковой коррекции, интенсивность которой адаптируется к отклонениям кажущейся вертикали от гравитационной с учётом оценивания скорости. Алгоритм настраивается на обучающих последовательностях движения аппарата и обеспечивает робастность к дрейфам гироскопов. Ключевые слова: гироскоп, акселерометр, гировертикаль, маятниковая коррекция, кажущаяся вертикаль, робастность, алгоритм.
В настоящее время все большее применение находит беспилотная авиация, в которой значительное место занимают малоразмерные летательные аппараты (ЛА) самолётного типа. Важную составляющую их авионики представляют инерциальные системы ориентации [1—3]. При их проектировании возникает проблема, заключающаяся в необходимости использования миниатюрных и коммерчески оправданных микромеханических, в том числе микрооптоэлектромеханических гироскопов, относящихся к низкому классу точности [4, 5]. Снижение вредного влияния дрейфов таких гироскопов достигается путём комплексирования с измерениями датчиков иного типа [3, 6-10]. Каждый из способов комплексирования имеет свои достоинства и недостатки.
Использование сигналов акселерометров относится к задаче маятниковой коррекции гировертикали. Под гировертикалью понимается устройство, состоящее из датчиков первичной информации и вычислителя и предназначенное для определения углов наклона объекта относительно гравитационной вертикали места. При маятниковой коррекции гировертикаль обладает полной автономно -стью и не требует информации о других геофизических полях, кроме гравитационного, а состав датчиков включает в себя всего три гироскопа и три акселерометра. Однако если ЛА ускоряется, тормозит или вращается, то акселерометры измеряют так называемое кажущееся ускорение, которое равно разности абсолютного ускорения ЛА и ускорения свободного падения. Поэтому основной задачей при реализации маятниковой коррекции является выделение из измерений акселерометров составляющих, обусловленных влиянием гравитационного поля.
В данной работе рассматривается алгоритм вычислителя, выполняющего расчёт текущих значений углов крена и тангажа по информации от микромеханических гироскопов и акселерометров. Показано, что классический алгоритм счисления кватерниона ориентации при столь сильных дрейфах приводит к расходящимся оценкам и коррекция необходима. Предлагается адаптировать маятниковую коррекцию к уровню искажений измерений ускорения свободного падения. Реализация этого подхода позволяет придать алгоритму определения крена и тангажа ценное для практического применения свойство робастности к дрейфам гироскопов. Исследована потенциально достижимая точность оценивания крена и тангажа на траекториях возмущённого движения малоразмерного беспилотного ЛА.
Модели ориентации. Используются две модели ориентации. Модель 1 предназначена для учета изменения ориентации аппарата на шагах дискретизации измерений гироскопов. Она является динамической, детерминированной и представлена в параметрах Родрига-Гамильтона
4г +1 = Чг + Чг & , % = (Чг ° Чт )/2 . (1)
Здесь - кватернион ориентации аппарата; дтг - кватернион угловых скоростей; г - номер дискретного момента времени; At - шаг дискретизации измерений; «о » - операция произведения кватернионов.
Кватернион 4 определяет ориентацию связанной системы координат (СК) аппарата относительно инерциальной СК, роль которой играет нормальная земная СК. С точки зрения теории бесплатформенных инерциальных систем [1-3] соотношения (1) являются одношаговым алгоритмом ориентации.
Модель 2 предназначена для коррекции модели 1 с учётом сигналов акселерометров. Её вектор включает три компоненты: крен, тангаж и скорость ЛА. Далее под скоростью понимается скорость относительно Земли. Модель 2 является стохастической и статической. Её состояние учитывается для текущего момента дискретного времени ^ .
X = Ъд + Wi, х(?о) = хо е #{хо ,Ро },
хТ =[ Л V], хтд Лд V], М^wf] = О,, Ог = ^{ст2г,ст2,^}. (2)
Здесь х, - вектор состояния; #{хо,Ро} - его априорное нормальное распределение; V - скорость ЛА; wг■ - вектор возмущений с ковариационной матрицей О, ; хгд - вспомогательный вектор,
рассчитываемый по кватерниону ориентации с помощью матрицы поворота, связанной СК относительно инерциальной. Кватернион ориентации модели 1 связан с компонентами крена и тангажа вектора состояния модели 2 известными соотношениями [2].
Уравнение наблюдений модели 2 принимается в виде
Я = [Пхг пуг Щг ]Т + V = / /у, /гА ]Т + V,, М^ V-' ] = И,, К,- = ^{ст2^ ,стИу( ,ст2^}. (3)
Здесь Я, - вектор наблюдений; пх1, Иу,, п^ - проекции вектора перегрузки; V, - вектор ошибок измерений с ковариационной матрицей К, .
Функции /х, /у, /2 в (3) определяют связь измерений перегрузок с параметрами полёта и следуют из дифференциальных уравнений для скорости ЛА [11]
/х = 8Ш(») + (Ух +Vzюу-(й2Уу)/я ,
/у = ео8(В)ео8(у) + (Уу + VxЮz -ю^ )/Я , fz = -со8(»>ш(у) + V +VyЮх -ю^х)/я . (4)
Здесь Vx, Vy, Vz - проекции скорости на связанные оси аппарата; юх, юу, юz - проекции вектора угловой скорости.
В рамках ограничения состава датчиков акселерометрами и гироскопами используются упрощенные соотношения, правомерные при Vx = V, Vy = Vz = о, V = о, что соответствует допущению о постоянстве скорости и малости углов атаки и скольжения
/х -8т(3), /у -со8(3)со8(у) + Vюz/Я , /2 ~-со8(3>т(у)-ЮyV/я . (5)
При этом матрица Якоби вектора наблюдений имеет вид
" сов(») о о ~
-8т($)со8(у) -со8(&)8т(у) Юz /Я . (6)
8т($)8т(у) -со8($)со8(у) -Юу / Я
Модель 2 и её наблюдение являются приближенными. Степень приближения учитывается с помощью возмущения wг■ в (2) и ошибки измерения V, в уравнении наблюдения (3), за счет чего выполнение указанного допущения не является обязательным.
Заметим, что возмущение wг■ и ошибка измерения V, намеренно введены в (2) и (3) с целью регулирования меры доверия к состоянию модели 2 и её наблюдению путём варьирования ковариационных матриц О, и К, соответственно. Идея алгоритма состоит в том, чтобы О, и К, адаптивно подстраивать в соответствии с уровнем отклонения кажущегося ускорения от ускорения свободного падения, возникающего при маневрировании ЛА. Заметим также, что при |п| = 1 кажущееся ускорение совпадает с гравитационным. Поэтому представляется оправданным задавать дисперсии ст^,
ст2 , ст2 , стИх, ст2у, ст2у в О, и К, функциями модуля перегрузки, формируемыми по правилу: чем
больше модуль перегрузки отличается от единицы, тем больше данные дисперсии. И напротив: чем ближе модуль перегрузки к единице, тем больше доверия к модели 2 и тем меньше должны быть дисперсии возмущения и ошибки измерения.
Н = ® =
Эх
Обозначим , f, fy, fn четыре функции модуля перегрузки, предназначенные для определения текущих значений дисперсий возмущений и ошибок измерений модели 2
ста = f3 (n*), °2= /у (n*), стУ = fy ^^ ст2 =ст2=с =<j^y =<зП = fn (n*), n = ||n|-1|. (7)
Вид данных функций рассмотрим ниже на этапе обучения алгоритма.
Для достижения желаемой точности ориентации одного отсчета измерений акселерометров недостаточно. Поэтому оценки крена и тангажа уточняются рекуррентно по множеству измерений. Для пересчета оценок на очередной момент дискретного времени используется модель 1. При этом на каждом шаге Д? апостериорные математические ожидания крена и тангажа пересчитываются в априорные математические ожидания с помощью модели 1, а оценка скорости замораживается.
Алгоритм гировертикали. Обозначим N{x¿,P¿} - априорное нормальное распределение вектора состояния модели 2 для момента времени ti; N{x¿,P¿} - апостериорное распределение, подлежащее оцениванию; N{Xi+1,Pi+i} - априорное распределение для следующего момента времени ?i+1. Тогда алгоритм гировертикали, решаемый на одном интервале дискретизации измерений Д?, представляется в виде последовательности следующих шагов:
Шаг 1. Определение статистик апостериорной плотности N{xi,Pi} вектора состояния модели 2
с учётом N{Xi, Рг-} и текущих измерений гироскопов и акселерометров. Они имеют вид известных соотношений байесовского оценивания вектора состояния по вектору его дискретных измерений
[12, 13]. _
Pi = р-1 + Qi, K i = Pi HT (Hi PHT + Ri )-1, Pi = (I - KiHi )Pi (I - KiHi )T + KiRiKT ,
Xi = Xi-1 + Ki (Zi - Zi). (8)
Здесь Zi - оценка вектора наблюдений (3), которая имеет вид
" _ sin(Si) _ " cos(% )cos(Yi) + Цazi / g . (9)
-OOs(Si )sln(Yi ) - Vi<Byi / g Шаг 2. Расчет оценки кватерниона ориентации qi.
Шаг 3. Вычисление априорного кватерниона ориентации qi+1 для следующего момента дискретного времени. Выполняется по соотношениям (1).
Шаг 4. Вычисление статистик априорной плотности N{xi+1,Pi+1} . Включает в себя расчёт априорного математического ожидания и ковариационной матрицы. Априорное математическое ожидание компоненты скорости определяется с учетом допущения о постоянстве скорости и принимается равным её оценке: Ц +1 = Vi. Априорные математические ожидания тангажа и крена $г+ь Yi+1 вычисляются из кватерниона qi+1. Априорная ковариационная матрица приближенно принимается равной апостериорной ковариационной матрице: Pi+1 = Pi. Применение более сложных соотношений для ее расчета представляется неоправданным в силу приближенности модели 2.
Указанные вычисления выполняются на каждом шаге Д? по мере поступления новых измерений гироскопов и акселерометров.
Обучение алгоритма. Обучение алгоритма преследует цель определения таких функций f$, fy, fy, fn, которые бы обеспечивали наиболее точное определение крена и тангажа в условиях
наличия дрейфов гироскопов, уровень которых соответствует классу точности используемых датчиков. Заметим, что влияние дрейфов гироскопов зависит не только от их величины и знаков, но также от вида движения аппарата. Поэтому обучение алгоритма должно проводиться на процессах, характерных для движений аппарата в реальном полете. Для этого из экспериментальных полетных данных формируются обучающие последовательности, включающие в себя согласованные между собой
Zi =
процессы изменения скорости, углов ориентации, сигналов акселерометров и гироскопов. Затем к сигналам гироскопов прибавляются смещения нулей с максимально возможными для данного типа датчиков значениями.
Всего для данных одного полета формируется девять вариантов обучающих последовательностей, из которых восемь вариантов соответствуют различным сочетаниям знаков смещений нулей трех гироскопов плюс еще один вариант для случая точных измерений.
Для простоты отыскания функций /$, /, /у, /п
2
^ 2
С 2
они задаются в кусочно-линейном виде, представленном на рис. 1, и ограничиваются некоторыми предельными значениями. При этом в каждой функции удерживается два узла * 2 * 2
интерполяции (п1 ), (п2,^). Абсцисса левого узла ин-
n = 0
n2 n
Перегрузка, g Рис. 1. Зависимость дисперсии возмущений и измерений от n*
терполяции принимается равной нулю n = 0, что соответствует |n| = 1. Ось ординат имеет размерность дисперсии тангажа, крена, скорости и перегрузки для функций f$, /у, fy, fn соответственно.
В качестве критерия качества назначалась взвешенная среднеквадратическая ошибка (СКО) ориентации по крену и тангажу, усредненная по времени и по множеству всех девяти обучающих последовательностей.
CT(J) = а$ста (J) + а,уСТу(J), J = {J$,Jу,Jy,Jn }. (10)
Здесь ста - СКО оценивания тангажа; СТу - СКО оценивания крена; а$ и ау - весовые коэффициенты; J = {J$, Jy, Jy, Jn } - множество абсцисс и ординат узлов интерполяции функций f$ , /у,
*
fy, fn . При удержании двух узлов интерполяции в каждой из функций и при ni = 0 множество J содержит по три параметра на функцию, т.е. всего двенадцать параметров.
Минимизация ошибки (10) на множестве J выполнялась численно при многократном решении алгоритма гировертикали на девяти обучающих последовательностях. Окончательная оценка качества алгоритма производилась на контрольных последовательностях, сформированных из экспериментальных данных других полётов.
Результаты моделирования. Исследование точности разработанного алгоритма выполнялось с привлечением данных нескольких полётов, из которых один играл обучающую роль, а другие - контрольную. Результирующие данные приводятся для одного из контрольных полётов.
На рис. 2 представлены зависимости изменения тангажа (тонкая линия) и ошибка его оценивания по представленному алгоритму (толстая линия) при смещениях нулей трех гироскопов, равных +0,05 град/с (180 град/ч). Аналогичные зависимости можно получить для крена.
Рисунок 3 иллюстрирует адаптацию коррекции к изменению перегрузки в процессе полёта. На рисунке изображено изменение нормированной дисперсии ошибки наблюдения модели 2. При слишком больших значениях модуля перегрузки дисперсия
20 J
1,2 п
50
100
250
150 200 Время, с
Рис. 2. Тангаж и ошибка его оценивания
300
0,8
О о
0,4
150 200
Время, с
Рис. 3. Изменение нормированной дисперсии ошибки наблюдения модели 2
и ft tl 2
О «
О
0
0,01
0, 02 0, 03 0, 04 Смещение нулей, град/с
0,05
Рис. 4. Зависимость СКО тангажа от смещения нулей гироскопов
ошибки наблюдения выходит на ограничение, а при малых убывает, что соответствует виду функции /п , представленному на рис. 1.
Рисунок 4 иллюстрирует робастные свойства алгоритма. На нём изображены зависимости СКО оценивания тангажа (град) от смещения нулей гироскопов (град/с) для алгоритма ориентации с коррекцией (толстая линия) и без коррекции (тонкая линия). Аналогичная зависимость может быть получена для крена.
Отсюда следует, что предложенный алгоритм ориентации выдерживает стабильность ошибок оценивания крена и тангажа в достаточно широких пределах изменения дрейфов гироскопов. При малых дрейфах он уступает по точности алгоритму без коррекции, но при больших дрейфах превосходит в несколько раз. Заметим, что при отсутствии коррекции процессы ошибок ориентации являются неустойчивыми и при увеличении продолжительности полета выигрыш алгоритма с коррекцией возрастает.
Погрешности масштабных коэффициентов, неортогональности осей датчиков и шумовая погрешность в данной работе не рассматриваются, поскольку на несколько порядков меньше дрейфа, характерного для микромеханических гироскопов. Медленноменяющаяся погрешность гироскопов воспринимается алгоритмом как текущее смещение нуля и компенсируется за счет адаптивной коррекции по сигналам акселерометров точно так же, как и постоянная.
Заключение. Получен новый результат, заключающийся в создании алгоритма определения крена и тангажа летательного аппарата, обеспечивающего робастность по отношению к дрейфам гироскопов за счет маятниковой коррекции, адаптивной к отклонениям кажущейся вертикали от гравитационной. Применяя к данному алгоритму классификацию адаптивных систем управления [14], правомерно отнести его к классу субоптимальных в отношении частного критерия, используемого при обучении.
Достоинством предложенного алгоритма являются его автономность от внешних источников информации, отсутствие накопления ошибок, обусловленных вредным влиянием дрейфов гироскопов, а также упругих колебаний и вибраций конструкции ЛА. Поскольку коррекция ориентации выполняется относительно текущего направления гравитационной вертикали, то алгоритм сохраняет полезное свойство маятниковой коррекции, заключающееся в отсутствии необходимости учета местоположения летательного аппарата и его перемещения относительно Земли, а также учёта угловой скорости её вращения.
Практическая ценность заключается в том, что результат достигается минимальными техническими, а следовательно, и экономическими средствами, в смысле ограничения состава датчиков тремя гироскопами и тремя акселерометрами. Платой за робастность являются ненулевые ошибки оценивания на переходных процессах движения аппарата при отсутствии дрейфов гироскопов.
Литература
1. Степанов О.А. Применение теории нелинейной фильтрации в задачах обработки навигационной информации / О.А. Степанов. - СПб: ГНЦ РФ - ЦНИИ «Электроприбор», 1998. - 370 с.
2. Salychev O. Applied Inertial Navigation: Problems and Solutions / O. Salychev. - M.: BMSYU Press, 2004. - 304 c.
3. Матвеев В.В. Основы построения бесплатформенных инерциальных навигационных систем / В.В. Матвеев, В.Я. Распопов; под общ. ред. В.Я. Распопова. - СПб.: ГНЦ РФ ОАО «Концерн «ЦНИИ «Электроприбор», 2009. - 278 с.
4. Бусурин В.И. Исследование характеристик микрооптоэлектромеханического преобразователя угловых скоростей / В.И. Бусурин, А.В. Казарьян, А.Т. Фам // М.: Вестник МАИ. - 2015. - Т. 22, № 1. - C. 29-37.
5. Бусурин В.И. Анализ влияния конструктивных параметров на характеристики микрооптоэлектромеханического преобразователя угловых скоростей / В.И. Бусурин, А.Т. Фам, П.С. Ахламов // Труды МАИ. - 2015. - № 81. - C. 1-17.
6. Алалуев Р.В. Бесплатформенная система ориентации и навигации мини-беспилотного летательного аппарата / Р.В. Алалуев, Ю.В. Иванов, В.В. Матвеев, В.Я. Распопов, А.П. Шведов // Приложение к журналу «Мехатроника, автоматизация, управление»; под ред. В.Я. Распопова. - 2008. -№ 10. - С. 14-18.
7. Патент на полезную модель 96235 РФ. Бесплатформенная инерциальная гировертикаль /
A.П. Шведов, Ю.В. Иванов, В.Я. Распопов; заявитель и патентообладатель ГОУ ВПО «Тульский государственный университет»; заявл. 04.03.2010; опубл. 20.07.2010.
8. Белоглазов И.Н. Обработка информации в иконических системах навигации, наведения и дистанционного зондирования местности / И.Н. Белоглазов, С.Н. Казарин, В.В. Косьянчук. -М.: Физматгиз, 2012. - 368 с.
9. Ильясов С.П. Комплексирование инерциального измерительного модуля бесплатформенной системы ориентации / С.П. Ильясов, А.В. Корнилов, Д.В. Свяжин // Научн.-техн. вестник Поволжья. - 2013. -№ 4. - С. 174-177.
10. Качанов Б.О. Контроль углов ориентации летательного аппарата с помощью спутниковой навигационной системы / Б.О. Качанов, Е.Ю. Толстолуженский // Приборы и системы. Управление, контроль, диагностика. - 2010. - № 5. - С. 33-43.
11. Динамика полёта: учеб. для студентов высш. учеб. завед. / А.В. Ефремов, В.Ф. Захарченко,
B.Н. Овчаренко и др.; под ред. Г.С. Бюшгенса. - М.: Машиностроение, 2011. - 776 с.
12. Кочетков Ю.А. Основы автоматики авиационного оборудования / Ю.А. Кочетков. -М.: ВВИА им. Н.Е. Жуковского, 1995. - 574 с.
13. Сейдж Э.П. Теория оценивания и ее применение в связи и управлении / Э.П. Сейдж, Дж. Мелса. - М.: Связь, 1976.
14. Справочник по теории автоматического управления / под ред. А. А. Красовского. - М.: Наука. Гл. ред. физ.-мат. лит., 1987.
Качанов Борис Олегович
Д-р техн. наук, профессор, главный специалист ОАО МНПК «Авионика», Москва Тел.: +7 (495) 771-66-07, доб. 17-65 Эл. почта: nit@mnpk.ru
Кулабухов Владимир Сергеевич
Канд. техн. наук, доцент, гл. конструктор ОАО МНПК «Авионика», Москва Тел.: +7 (495) 771-66-07, доб. 17-66 Эл. почта: kulabuhov@mnpk.ru
Туктарёв Николай Алексеевич
Канд. техн. наук, ст. науч. сотрудник, гл. специалист ОАО МНПК «Авионика», Москва Тел.: +7 (495) 771-66-07, доб. 17-66 Эл. почта: nit@mnpk.ru
Гришин Дмитрий Викторович
Аспирант каф. «Системы автоматического и интеллектуального управления» МАИ, Москва Тел.: +7 (495) 771-66-07, доб. 10-39 Эл. почта: mai@dgri.ru
Kachanov B.O., Kulabuhov V.S., Tuktarjov N.A., Grishin D.V.
Adaptive algorithm of gyrovertical computing unit for unmanned aerial vehicle
A new algorithm for determining roll and pitch of the unmanned aerial vehicle (UAV) on joint measurement of gyroscopes and accelerometers is proposed. The novelty consists in the way of the pendulum correction. Its intensity adapts to deviations of apparent vertical from gravitational one considering assessment of velocity. The algorithm gets adjusted through the training sequences and provides robustness to gyroscopes drifts. Keywords: gyroscope, accelerometer, gyroscopic vertical, pendulum correction, apparent vertical, robustness, algorithm.
