CC BY
30
УДК 531.383
DOI: 10.24412/2071-6168-2022-12-85-92
РЕАЛИЗАЦИЯ АЛГОРИТМОВ ОРИЕНТАЦИИ БЕСПЛАТФОРМЕННЫХ ИНЕРЦИАЛЬНЫХ
НАВИГАЦИОННЫХ СИСТЕМ
В.В. Матвеев, М.Г. Погорелов, В.В. Лихошерст, А.В. Каликанов, М.Д. Кирсанов, Д.С. Стрельцов, А.Г. Колесникова
Рассматриваются алгоритмы ориентации бесплатформенной инерциальной навигационной системы. Дана характеристика кинематических параметров, используемых для описания положения подвижного объекта: углов Эйлера-Крылова, направляющих косинусов, параметров Родрига-Гамильтона, компонентов вектора Эйлера. Приводятся кинематические уравнения, связывающие параметры ориентации и проекции вектора угловой скорости подвижного объекта и численная реализация алгоритмов ориентации. Представлены результаты моделирования алгоритмов ориентации при коническом движении объекта.
Ключевые слова: кинематические параметры, алгоритм ориентации, бесплатформенная инерциальная навигационная система.
В настоящее время бесплатформенные инерциальные навигационные системы (БИНС)имеют ряд несомненных преимуществ по сравнению с платформенными системами, главным из которых является отсутствие гиростабилизированной платформы. Отсутствие прецизионного карданова подвеса позволяет значительно снизить массогабаритные характеристики, стоимость и энергопотребление, повысить надежность, упростить сборку. В БИНС датчики первичной информации непосредственно устанавливаются на борту подвижного объекта, а функции гиростабилизированной платформы выполняет вычислительное устройство, позволяющее алгоритмически пересчитать показания акселерометров из связанной системы координат в опорную, на основе решения задачи ориентации. Прямым интегрированием получить углы поворота объекта не представляется возможным, так как проекции угловой скорости не являются голономными координатами [1]. Корректный способ определения углов отклонения подвижного объекта относительно опорной системы координат состоит в интегрировании кинематических уравнений относительно искомых параметров ориентации. В качестве параметров ориентации могут быть использованы углы Эйлера-Крылова, направляющие косинусы, параметры Родрига-Гамильтона, вектор конечного поворота и др. [2-4]. От эффективности алгоритмов ориентации зависит точность определения координат местоположения объекта. Анализу точности алгоритмов ориентации посвящена настоящая статья.
Обзор алгоритмов ориентации БИНС. Разработка алгоритмов ориентации начинается с выбора параметров, используемых для описания положения подвижного объекта относительно центра масс. Для математического описания алгоритмов ориентации введем инерциальную систему координат i и связанную с подвижным объектом систему координат b. Переход от инерциальной системы координат ж связанной Ьможно задать при помощи различных кинематических параметров: трех углов Эйлера-Крылова, матрицы направляющих косинусов СЬ, кватерниона Qb. или вектора Эйлера фЬ. Задача ориентации подвижного объекта будет решена, если будут найдены любые из перечисленных выше параметров ориентации по информации о проекциях вектора угловой скорости связанной системы координат b на свои же оси относительно инерциальной системы координат i.
При разработке алгоритмов ориентации часто применяются параметры Родрига-Гамильтона [3- 6]. В монографии [5] приведен способ решения задачи ориентации на основе параметров Родрига— Гамильтона, которые в свою очередь определяются численным алгоритмом второго порядка, построенным на основе метода последовательных приближений Пикара:
Ql«(k +1) = Qbfi(k) - Qbfi(k )e(k +1) / 8 -
-0,5[ Qb1 (k) Да x (k +1) + Qba(k) Да y (k +1) + Qb^ k) Да z (k +1)],
Qb, (k +1) = Qb (k) - Qb (k)e(k +1) / 8 +
+0,5[ Qbo(k) Да x (k +1) - Qb3(k) Да y (k +1) + ^(k) Да z (k +1)], (1)
Qba(k +1) = Qba(k) - Qba(k)e(k +1) / 8 + +0,5[Qb3(k)Даx(k +1) + Qbo(k)Даy(k +1) - Q* (k)Даz(k +1)], Qb3(k +1) = Qb3 (k) - Qb3(k )e(k +1) / 8 + +0,5[-Qba(k) Да x (k +1) + Qb, (k) Да y (k +1) + Q^k) Да z (k +1)], где Qb (m = 0,1,2,3)- параметры Родрига-Гамильтона; Да , Да , Да - приращения интегралов от
,m x y z
проекций вектора угловой скорости объекта на оси чувствительности гироскопов, определяемые так:
(к+1)70
Да х (к +1) = (I )Л
щ
(к+1)7) Да у (к +1) = |
ю/ь „ (? )&,
кТо (к+1)То Ю
Да г (к +1) = ¡юЬь,2 (I )Л.
(2)
(3)
(4)
кТ0
Параметр е(к +1) определяется по формуле
е(к +1) = Да 2 (к +1) + Да у (к +1) + Да2 (к +1). (5)
Повышение точности алгоритмов ориентации с кватернионами основано на представлении кинематического уравнения в виде степенного ряда [6]. Кинематическое уравнение с параметрами Родрига-Гамильтона для непрерывного времени имеет вид [3]:
20 Ь = [ДЬ °]0Ь, (6)
где 0Ь = Ц^^ |Т - матрица размера 4х1, составленная из элементов кватерниона (пара-
метров Родрига-Гамильтона);
[ДЬ °] =
0 -юьь,х -юьь, у -Ю?ь,г
ЮЬ, X 0 юьь, г -юьь, у -кватернионная матрица,
< у -юьь, г 0 ю'ь,х
юьь,2 ЮЬ, у - юьь,X 0
составленная из проекции вектора угловой скорости ю/Ь .
Решение матричного уравнения (6) на периоде дискретизации Т0 можно представить в виде [6]
и -©(к) ,
0Ь (к +1) = е 2 (k),
(7)
где
(к+1)то
©(к) = | [Дь °]я-
к = 0, 1,
кТо
0 -Да х (к) -Да у (к) -Да г (к) Да х (к) 0 Да г (к) -Да у (к) Да у (к) -Да г (к) 0 Да х (к) Да г (к) Да у (к) -Да х (к) 0 Показательную функцию в уравнении (7) можно представить в виде степенного ряда
2©(к) _ ©(к) 1 © 2(к) 1 ©п (к) + —— +--— +... +--— +....,
= Е
4x4
(8)
2 2! 4 п! 2п
где Е4х4 - единичная матрица размера 4x4.
Таким образом, точность алгоритма может быть повышена выбором соответствующего числа членов ряда (8).
Другой способ решения задачи ориентации подвижного объекта связан с численной реализацией уравнения Бортца [7]
1
фЬ
-ФЬ X юЬ
(9)
где ю'ь - вектор абсолютной угловой скорости подвижного объекта, выраженный своими проекциями в связанной системе координат Ь; ф' - вектор Эйлера, характеризующий переход из инерциальной системы координат/ в связанную систему Ь.
Матрица направляющих косинусов перехода от инерциальной координат к связанной, выраженная посредством вектора фь, может быть вычислена следующим образом:
СЬ = Езхз - №ь, X]+Л[фЬ х]2,
кососимметрическая матрица, составленная из проекций вектора
0 -Ф Ф ', у
где [ФЬ X] = ФЬ 0 -ФЬ /, X
-ФЬ у Ф /, X 0
ориентации ф.; fx, f2 - вспомогательные функции, определяемые так
sin | Фь I | Фь |2 | Фь |4 „ 1 - cos | фь | 1 | фь |2 | фь |4
f =1 -..., /2 =
| ФЬ | 6 120 | ФЬ |2 2 24 720
| ф41= ^(Ф4 )2 + (Фь, )2 + (Ф4 )2 - модуль вектора Эйлера.
Уравнение (5) может быть представлено в виде
Фь, (t) «Да(0 + Ap(t), (10)
t i t где Aa(t) = Jwbbdt -интеграл от вектора угловой скорости подвижного объекта, Ap(t) = — JФь х wbbdt"
0 2 0 коническая поправка (Coning).
Для дискретного времени уравнение (10) можно представить в виде:
ФЬ(£ +1) «Aa(k +1) + Ap(k +1), (11)
где компоненты вектора Aa(k +1) определяются в соответствии с соотношениями (2) - (4). В работе [8] коническая поправка вычисляется так
AP(k +1) = 12 Aa(k) х Aa(k +1). (12)
В работе [9] расчет конической поправки определяется выражением:
AP(k +1) = ^ Aa(k +1) х Aa П (k +1) + 24 Aa(k +1) х Aa ш (k +1), (13)
где Aan (k +1) = Aa(k +1) - Aa(k) - вторая разность,
Aaш (k +1) = Aa n (k +1) - Aan (k) - третья разность. В работе [9] также изложен дискретный алгоритм для пошагового интегрирования кинематического уравнения для вектора Эйлера, основанный на использовании первичной информации о квазикоординатах, представляющих собой кратные интегралы от угловой скорости на шаге опроса датчиков. При использовании первичной информации о квазикоординатах в виде первого и второго интегралов от угловой скорости юЬ . Данный алгоритм 4-го порядка относительно T0 имеет следующий вид:
ф. (k +1) = Aa(k +1) - — • [ Aa(k +1) х Aa11 (k +1) ],
(14)
T ±0
где Aa(k +1) определяется в соответствии с (2)-(4), а Aa11 (k +1) следующим образом
(k+1)7о
Aa11 (k +1) = |Aa(k + 1)dt' (15)
kTo
В работе [9] отмечается, что «основным достоинством алгоритма (15) в отличие от традиционных, является то, что он обладает одинаковой точностью вычислений как в условиях конического, так и произвольного углового движения объекта». Оценка точности алгоритмов
С целью анализа алгоритмов ориентации проводилось моделирование, в соответствии с которым формировалось движение подвижного объекта близкое к коническому. Изменение углов рыскания ),
тангажа &(t) и крена y(t) относительно инерциальной системы координат задавались следующими
функциями времени:
) = A sin <v,
&(t) = A cos <V, (16)
y(t) = ю/,
где A, A, ю^, юэ - амплитуды и круговые частоты колебаний по рысканию и тангажу соответственно; юу - угловая скорость вращения подвижного объекта по крену.
Проекции вектора угловой скорости подвижного объекта задавались следующим образом:
h
®b,x = У + VSin &
'ib.
шЪЪу = \у cos & cos У + && sin у, (17)
шЪЪг = & cos у-1^ cos&sin у. Далее формировались приращения гироскопов за период дискретизации T0 методом Рунге-Кутта на основании соотношений (2) - (4).Сравнению подлежали алгоритмы, приведенные в табл. 1.
87
Для алгоритмов «К1» - «К4» углы рыскания, тангажа и крена рассчитывались по соотношени-
ям:
\(к +1) = arctg
( 2$Лк + 1^з(к +1) -2Qlо(k + ^(к +1)^
д(к +1) = агС§
2Qbл(k +1)2 + 2Qbо(k +1)2 -1
2Qtl(k + Ы2{к +1) + 2Qlо(k + 1Шк +1)
■¡№10(к +1)2 + 2QгЬ,2(к +1)2 -1]2 + [20^ + 1^1з(к +1) -2QЬо(k + 1>0,Ь1(к +1)]2 ' 2Ql2(k + 1)QЬ,з(к +1) -2Qlо(k +(к +1)^
у(к +1) = атС§
2QIЬо(к +1)2 + 2QIЬ2(к +1)2 -1 Алгоритмы ориентации_
Таблица 1
Название алгоритма
Математическое описание
К1
Соотношения (1)
К2
Входные данные: Дах (к), Да (к), Да2 (к).
0Ь (к +1) = [Е4Х4 + +2 ]0Ь (к)
0 -Да х (к) -Дау (к) -Да г (к) Да 2 (к) -Да у (к) 0 Да х (к)
©(к) =
Да х (к) 0 Дау (к) -Да г (к) Да 2 (к) Да у (к) -Да х (к)
0
Выходные данные: кватернион О' (к).
кз
Входные данные: Да х (к), Да у (к), Да 2 (к). Формирование ©(к) аналогично алгоритму «К2».
+ ©(к) + ! ©(к) + 1 ©к ]0Ь (к)
0Ь (к +1) = [Е4х4
г 4x4 2 2! 4 з! 8
©( к) - аналогично алгоритму «К2».
Выходные данные: кватернион О' (к).
К4
Входные данные: Да х (к), Да у (к), Да 2 (к). Формирование ©(к) аналогично алгоритму «К2».
0Ь (к +1) = [Е
4x4
з
©(к) 1 ©2(к) 1 ©з(к)
+ +--— +--— +
2 2! 4
з! 8
+1 ©т ]оь (к).
4! 16
Выходные данные: кватернион 0/ (к) .
Э1
Входные данные: Да х (к), Да у (к), Да 2 (к).
Да11 (к +1) =
Да(к +1) + Да(к) 2
Т0
ФЬ (к +1) = Да(к +1) - — -[Да(к +1) хДа11 (к +1)]. Т0
Выходные данные: компоненты вектора Эйлера фь (к +1).
Э2
Входные данные: Да х (к), Да у (к), Да 2 (к).
ДР(к +1) = 112 Да (к) х Да (к +ФЬ (к +1) = Да(к + 1) + Др(к +1). Выходные данные: компоненты вектора Эйлера ф' (к +1).
Окончание таблицы 1
Название алгоритма Математическое описание
Э3 Входные данные: Дах (к), Да (к), Даг (к). Да П (к +1) = Да(к +1) - Да(к), Даш (к +1) = Дая (к +1) - Дая (к), ДР(к +1) = -1 Да(к +1) х Да п (к +1) + -1 Да(к +1) х Да ш (к +ГЬ фЬ (к +1) = Да (к +1) + Др(к +1). Выходные данные: компоненты вектора Эйлера ф^ (к +1).
Для алгоритмов Э1, Э2, Э3 по элементам вектора Эйлера фь(к +1) формировалась матрица направляющих косинусов
С (к +1) = Е -
sin(|Фf (к +1)|
[фЬ (к + 1)х]-
1 - cos(| Фь (к +1)|
[фЬ (к + 1)х]2
(18)
|фЬ(к +1)| ' |фЬ(к +1)|2
где | фЬ(к +1)|- модуль вектора Эйлера, [ф^ (к + 1)х]2 - кососимметрическая матрица, составленная из компонент вектора Эйлера.
По элементам матрицы (18) вычислялись углы рыскания, тангажа и крена
(к +1) . сдз (к +1) I (к +1) = -.
У (к +1) = аг^
Си (к +1)
(20)
у1[С*22(к + 1)]2 + [С1з2(к + 1)]
С'(к +1)
у. (к +1) = -ат^- , .
ГД ' С^,22 (к + 1)
Погрешности алгоритмов определялись как модуль разности расчетных по алгоритмам углов с эталонными углами (16).
При моделировании задавались следующие исходные данные: А = АУ = 2°, ю =ш„ = 001 Гц, ю = 1 Гц, период дискретизации Т0 =0,01 с.Погрешности углов рыскания и тангажа приведены на рис. 1, 2.
§
о
2 &
й 5
Л
13 о Я
а
о
о С
0.01
Ы0"
1х10-
Ы0"
1х10
10
200
400
600
800
Время, с
Рис. 1. Погрешности алгоритмов ориентации в определении угла рыскания
Абсолютные погрешности алгоритмов ориентации, накопленные за 600 с моделирования приведены в табл. 2.
Из анализа табл. 2 следует заключить, что наиболее высокой точностью обладает алгоритм с уравнением Бортца с конической поправкой в форме (13), который можно считать, как наиболее эффективным для реализации задач ориентации в БИНС.
2
4
6
8
0
к
ев H ев
5 ¡-
о
s в
о а
и О
С
10
0.1
1x10
1x10
1x10
1x10
1x10~
0
200
600
800
400 Время, с
Рис. 2. Погрешности алгоритмов ориентации в определении угла тангажа
Абсолютные погрешности алгоритмов^ ориентации
Таблица 2
Название алгоритма Погрешность угла рыскания, ° Погрешность угла тангажа, °
К 1 1,22 1,23
К 2 1,22 1,23
К 3 9,57е-3 1,02е-2
К 4 3,44е-3 6,63е-3
Э 1 3,44е-3 6,63е-3
Э 2 4,18e-005 2,17e-005
Э 3 8,12e-007 1,49e-006
Заключение. Дано математическое описание алгоритмов ориентации бесплатформенных инерциальных навигационных систем с кватернионами и вектором ориентации Эйлера. Приводится численная реализация алгоритмов и их моделирование при коническом движении объекта. Установлено, что наилучшей точностью обладает алгоритм на основе уравнения Бортца с конической поправкой, учитывающей вторую и третью разность от приращения интегралов от проекций вектора угловой скорости.
Работа выполнена при финансовой поддержке Министерства науки и высшего образования РФ в рамках государственного задания по теме «Развитие теории инерциальных датчиков первичной информации для навигационных систем высокоманевренных летательных аппаратов (FEWG-2022-0002)».
Список литературы
1. Магнус К. Гироскоп. Теория и применения. М.: Мир, 1974. 516 с.
2. Навигация летательных аппаратов в околоземном пространстве. Августов Л. И., Бабиченко А.В., Орехов М.И., Сухоруков С.Я., Шкред В.К.; под редакцией доктора технических наук, Заслуженного деятеля науки Российской Федерации, профессора Джагджгавы Гиви Ивлиановича. М. : ООО «Научтех-литиздат», 2015. 592 с.
3. Бранец В.Н., Шмыглевский И.П. Применение кватернионов в задачах ориентации твердого тела. М.: Наука, 1973. 320 с.
Бранец В.Н., Шмыглевский И.П. Введение в теорию бесплатформенных инерциальных навигационных систем. М.: Наука, 1992. 280 с.
5. Управление и наведение беспилотных маневренных летательных аппаратов на основе современных информационных технологий, Веремеенко К.К., Головинский А.Н., Инсаров В.В., Красильщиков М.Н., Себряков Г.Г. М.: ФИЗМАТЛИТ, 2003. 280 с.
6. Микросистемыориентациибеспилотныхлетательныхаппаратов / Р.В. Алалуев [и др.]; под. ред. В.Я. Распопова. М.: Машиностроение, 2011. 184 с.
7. Лебедев Р.К. Стабилизация летательного аппарата бесплатформенной инерциальной системой. М.: Машиностроение, 1977. 144 с.
8. SavegeP. G. Strapdown analytics. Strapdown Association, Part 1. Inc. Maple Plain, Minnesota, 2000. 817 p.
9. ЕмельянцевГ.И., Степанов А.П. Интегрированные инерциально-спутниковые системы ориентации и навигации/ Под общ. ред. акад. РАН В.Г. Пешехонова. СПб.: ГНЦ РФ АО «Концерн «ЦНИИ «Электроприбор», 2016. 394 с.
Матвеев Валерий Владимирович, д-р техн. наук, доцент, ведущий научный сотрудник лаборатории, matweew.valery@yandex.ru, Россия, Тула, Тульский государственный университет,
Погорелов Максим Георгиевич, канд. техн. наук, заведующий лабораторией, mgpogoreloff@yandex.ru, Россия, Тула, Тульский государственный университет,
Лихошерст Владимир Владимирович, канд. техн. наук, старший научный сотрудник лаборатории, lvv_01@inbox.ru, Россия, Тула, Тульский государственный университет,
Каликанов Алексей Владимирович, младший научный сотрудник, kalikanov.aleksei@mail.ru, Россия, Тула, Тульский государственный университет,
Кирсанов Максим Дмитриевич, младший научный сотрудник, KirsanovMD@yandex.ru, Россия, Тула, Тульский государственный университет,
Стрельцов Дмитрий Сергеевич, лаборант-исследователь, 30st01@mail.ru, Россия, Тула, Тульский государственный университет,
Колесникова Александра Геннадьевна, лаборант-исследователь, gentle_visual@mail.ru, Россия, Тула, Тульский государственный университет
IMPLEMENTA TION OF ORIENTATION ALGORITHMS FOR NON-PLATFORM INERTIAL
NAVIGATION SYSTEMS
V.V. Matveev, M.G. Pogorelov, V.V. Likhosherst, A.V. Kalikanov, M.D. Kirsanov, D.S. Streltsov, A.G. Kolesnikova
Algorithms for orientation of a strapdown inertial navigation system are considered. The characteristic of the kinematic parameters used to describe the position of a moving object is given: Euler-Krylov angles, direction cosines, Rodrigues-Hamilton parameters, components of the Euler vector. Kinematic equations are given that relate the orientation parameters and projections of the angular velocity vector of a moving object and the numerical implementation of orientation algorithms. The results of modeling of orientation algorithms for the conical motion of a moving object are presented.
Key words: kinematic parameters, attitude algorithm, strapdown inertial navigation system.
Matveev Valery Vladimirovich, doctor of technical sciences, docent, leading researcher of the laboratory, matweew.valery@yandex.ru, Russia, Tula, Tula State University,
Pogorelov Maxim Georgievich, candidate of technical sciences, head of the laboratory, mgpogore-loff@yandex.ru, Russia, Tula, Tula State University,
Likhoshurst Vladimir Vladimirovich, candidate of technical sciences, senior researcher of the laboratory, lvv_01@inbox.ru, Russia, Tula, Tula State University,
Kalikanov Alexey Vladimirovich, junior researcher, kalikanov.aleksei@mail.ru, Russia, Tula, Tula State University,
Kirsanov Maxim Dmitrievich, junior researcher, KirsanovMD@yandex.ru, Russia, Tula, Tula State University,
Streltsov Dmitry Sergeevich, laboratory researcher, 30st01@mail.ru, Russia, Tula, Tula State University,
Kolesnikova Alexandra Gennadievna, laboratory researcher, gentle_visual@mail.ru, Russia, Tula, Tula State University
