CC BY
128
УДК 527:519.8
МАТЕМАТИЧЕСКИЕ МОДЕЛИ КОРРЕКЦИИ БЕСКАРДАННОЙ ИНЕРЦИАЛЬНОЙ НАВИГАЦИОННОЙ СИСТЕМЫ БЕСПИЛОТНИКА НА ОСНОВЕ НЕПОЛНОЙ ИНФОРМАЦИИ О ТРАЕКТОРИИ ЕГО ДВИЖЕНИЯ
А.А. Кудряш
В статье составляются уравнения ошибок для задачи о движении беспилотника при условии привлечения информации о высоте, а также формируются уравнения на основе измерений дальности до объекта и угла. Задача оценивания ошибок решается при помощи фильтра Калмана. Рассматривается два вида движения: по окружности и по прямой
Ключевые слова: беспилотник, математические модели, коррекция, инерциальная навигационная система, ошибки, фильтр Калмана
Уравнения, которые выводятся в работе, используются для коррекции грубой бескарданной инерциальной навигационной системы (БИНС), расположенной на борту беспилотника. БИНС состоит из трёх датчиков линейных ускорений (акселерометров), трёх датчиков угловых скоростей и бортового вычислителя, задачей которого является интегрирование динамических уравнений Ньютона, причём плата, с которой жёстко связаны датчики, крепится на корпусе движущегося объекта. Коррекция проводится при помощи информации о дальности до объекта с известными координатами и угле между направлением на объект и одной из осей трёхгранника, связанного с БИНС. Для упрощения выводимых уравнений предполагается, что объект находится на экваторе, а также привлекается информация о высоте (например, при помощи баро-высотомера).
В работе приняты следующие обозначения:
• М - точка, показывающая местоположение чувствительной массы акселерометров. Измеряются действующие на эту массу силы. За счёт нормировки чувствительную массу можно сделать единичной;
• Мг - приборный трёхгранник. В проекциях на оси этого трёхгранника измеряется внешняя сила, приложенная к точке М, и проекции его угловой
скорости. Результаты измерений = £ + Д£,
где = - вектор погрешностей
измерений акселерометров, у2 = (у21,у22, у2з)т -вектор погрешностей измерений датчиков угловых скоростей;
• 0% - инерциальный трёхгранник. Ось 0%3 направлена на северный полюс, плоскость 0(1(2 -экваториальная, ось 0%1 направлена в точку весеннего равноденствия;
• 0^ - гринвичский трёхгранник, жёстко связанный с Землёй. Точка 0 - геометрический центр Земли, ось 0^3 совпадает с осью вращения Земли,
Кудряш Андрей Андреевич - ВГТУ, аспирант, тел. 8(473)246-42-22
плоскость 0ц1ц2 - экваториальная плоскость, плоскость - плоскость гринвичского (нулевого) меридиана. Ал - матрица его ориентации относительно инерциального;
• Мх - географический трёхгранник. Ориентация этого трёхгранника полагается полусвободной, то есть компонента вектора относительной угловой скорости этого трёхгранника относительно гринвичского в проекциях на его оси П3 равна нулю. Тогда в начальный момент он выбирается следующим образом: ось Мх3 направлена по внешней нормали к поверхности Земли, Мх2 лежит в плоскости текущего меридиана и направлена на Север, а Мх1 направлена по касательной к параллели на Восток, а в последующие моменты времени этот трёхгранник выбирается таким образом, чтобы проекция его собственной относительной угловой скорости на ось Мх3 была равна -Ябшр, где р - географическая северная широта - угол между Мх3 и экваториальной плоскостью 0^1^2, отсчитываемый к Северу, Я - географическая долгота - угол между проекцией орта Х3 на экваториальную плоскость и осью 0^1, отсчитываемый к Востоку. Это вытекает из тех соображений, что когда объект движется по поверхности Земли, проекция относительной угловой скорости трёхгранника, ориентированного в географической сетке (ориентация которого задана в начальный момент времени), на ось Мх3 равна ЯБтр. Географический трёхгранник выбирается как опорный, то есть в проекциях на оси этого трёхгранника определяются координаты и скорости объекта (под объектом понимается единичная приведённая чувствительная масса акселерометров);
• Му - модельный трёхгранник - числовой образ приборного трёхгранника. Предполагается, что он мало отличается от приборного. Угловая скорость этого трёхгранника в проекциях на его оси шу = ш'2. Ау - матрица его ориентации относительно инерциального трёхгранника, определяется путём решения кинематических уравнений Пуассона Ау = ш2'Ау, где начальное значение матрицы Ау определяется начальной выставкой [2]. Смысл обозначения ш2' приведён ниже;
• 0ух(0х ) - квазимодельный трёхгранник -числовой образ опорного трёхгранника Мх. Пред-
полагается, что он мало отличается от географического. В проекциях на оси этого трёхгранника в бортовом вычислителе определяются координаты и скорости модельной точки M '. Аух - матрица ориентации этого трёхгранника относительно инерциаль-ного;
• L - матрица ориентации модельного трёхгранника относительно квазимодельного, L = АуАухт;
• В ' - матрица ориентации квазимодельного трёхгранника относительно гринвичского. Её элементы находятся при решении модельных уравнений [1]. После её определения определяется матрица Аух = В 'Ал, откуда находится матрица L;
• Mzx - квазиприборный трёхгранник. Его ориентация относительно приборного трёхгранника задаётся матрицей LT. На оси квазиприборного трёхгранника перепроектируются результаты измерения внешней силы fz. Результат такого перепроектирования - величина fzx служит входной информацией при интегрировании модельных динамических уравнений Ньютона: fzx = LTfz ;
• 1х, ly, lz - векторы, составленные из координат вектора l в осях трёхгранников Mx,Oyx,Mzx соответственно;
• а - кососимметрическая матрица /О а3 -а2\
(—а3 О а1 ), поставленная в соответствие V а2 —а1 О ) вектору-столбцу (а1,а2,а3)т ;
• Сх - матрица ориентации квазиприборного трёхгранника относительно квазимодельного, причём Сх = E+Pх, где Рх = (рх1,рх2,рх3)т - вект°р малого поворота;
• и - вектор угловой скорости вращения Земли: |и| = 7.29 ■ 1О"5 с-1, и = (О, |u|cos^, |u|sin^)T;
• а - большая полуось эллипсоида Земли;
• ш0 - частота Шулера, ш2 = - = 1.2 5 ■ 1 О -3 с- 1;
Уравнения ошибок выводятся в осях квазимодельного трёхгранника. Пусть и - координаты точек и соответственно в осях квазимодельного трёхгранника. Имеются соотношения:
у ' = v; + Пу у',
y = vy+ Qy у,
где Vy и Vy - относительные угловые скорости точек M ' и M соответственно, а Qy - относительная угловая скорость квазимодельного трёхгранника относительно гринвичского трёхгранника. Вычитая из первого равенства второе, получим:
Ау = AVy + îïyAy,
где вектор Ау = (Ау1, Ау2, Ау3)т - ошибка в определении местоположения, определяется равенством Ау = у ' — у, AVy = (AVyi, AVy2, AVy3)T - ошибка в определении скорости, определяется равенством AVy = Vy — Vy. Полные ошибки в определении коор-
динат и скоростей раскладываются в сумму кинематической и динамической ошибки:
Му = 8Уу+РуУу,
где 5Уу = Уу — У* - динамическая ошибка, ¡уУу -кинематическая ошибка. Аналогично:
Ау = 5у + ¡¡уу ',
где можно положить у = (0,0,а)т, поскольку мало отличается от , соответственно /?уу отличается от ¡Зуу' на величину второго порядка малости. Уравнения ошибок представляют собой систему девяти дифференциальных уравнений первого порядка относительно трёх компонент вектора Ау, трёх компонент вектора динамической ошибки скорости 5Уу и трёх координат вектора малого поворота:
Ау = 5Уу + руУу + йуАу ; 8Уу = (йу + 2 йу)5Уу — ш20(5у1,5у2,5у3) т + А£ (1) к = ¡х +
5 у1, 5у2,5у3 выражаются через Ау1,Ау2 ,Ау3. После привлечения информации о высоте , где
р* - погрешность информации, порядок системы уменьшится до 7, а после пренебрежения малыми второго порядка уравнения ещё более упростятся. Подробно о выводе уравнений можно узнать из [1]. Окончательный вид уравнений будет такой:
Ау1 = 8 V1+P3 V2 ,
Ау 2 = S V2 — P3V1, S V1 = 2 U3SV2 — с 2 Ау1 — Р2 g + Af1 , S V2 = — 2 U3 SV1 — с 2 Ау2 + Р 1 g + Af2, Рх = ûy Рх + Vz,
(2)
где первые 4 уравнения - скалярные, а в последнем содержится 3 уравнения.
Для того, чтобы решить задачу коррекции БИНС, надо сформировать измерение. Формируется оно следующим образом: пусть измеряется дальность до маяка и угол. Первоначально измеряется угол между одной из осей приборного трёхгранника (Мг1 или Мг2) и направлением на маяк, но с помощью матрицы 17 находятся положения осей квазиприборного трёхгранника, соответственно появляется возможность измерить этот угол как угол между направлением на маяк и одной из осей квазиприборного трёхгранника. Результаты измерения обозначим соответственно й* и р*. Тогда й* = й + ра, р* = р + р.ф. Сформируем вектор
к* = (й*соБр*,й*5тр*)т,
к** = (й + рй) соб(р + рр) = йсоБр + расоБр — —йрфБтр = К1* + р1*,
где члены второго порядка малости и выше отбрасываются. Аналогично
К** = (й + ра) Б\п(р + рр) = йБ\пр + р^тр + йррСОБр = К2*+ р2*,
где и - инструментальные погрешности. Далее, пусть имеются координаты маяка ( (Я0, р 0, Л 0) т. Для упрощения положим Л 0 = 0. Можно посчитать гринвичские координаты маяка:
7 0 = Д со б р0со бЯ0,
7 0 = Д со б р0Б1пЯ0, 7 0 = Д Бтр0,
где - радиус Земли. Зная эти координаты, с помощью матрицы ориентации В' квазимодельного трёхгранника относительно гринвичского можно найти координаты маяка в осях квазимодельного трёхгранника: у 0 = В'770. Тогда можно сформировать вектор
ку = у0 — у ' = у 0 — у — Ду,
где находится при решении модельных уравнений.
Получается, что и - это вектор,
направленный из точки, в которой находится чувствительная масса в точку, в которой расположен маяк (а точнее две его горизонтальные координаты), заданный в осях квазимодельного и квазиприборного трёхгранников соответственно. Тогда:
к2 = + /?ж) Ку.
Вектор измерений формируется следующим образом:
И = ку — к* = Ку — Ду — (Я + Дж)ку — р2 =
- Ду - pz - /?жк
(3)
Модели погрешностей измерений формируются следующим образом:
Pd=Pd + Ра
(s)
(4)
где первые составляющие - постоянные ошибки, а вторые слагаемые - белые шумы известной интенсивности.
Выведенные уравнения ошибок и уравнения измерений используются для оценивания двух горизонтальных компонент вектора полной ошибки координат и двух горизонтальных компонент динамических ошибок скоростей. Оценивание производится при помощи фильтра Калмана. Рассматривается два вида движения: движение по лучу и движение по окружности. Для простоты будем считать, что движение по лучу - движение по экватору по направлению к маяку, находящемуся на экваторе, а движение по окружности - движение вокруг маяка с постоянной скоростью по окружности известного радиуса. В первом случае широта постоянна и равна 0, т.е. географический трёхгранник не меняет ориентацию во времени. Будем считать, что векторы линейных и угловых скоростей проектируются на оси географического трёхгранника (в уравнениях ошибок это можно сделать, так как взаимная ориентация квазимодельного и географического трёхгранников определяется матрицей малого поворота. Тогда чле-
ны в уравнениях ошибок (2), содержащие линейные и угловые скорости, записанные в проекциях на оси географического и квазимодельного трёхгранников, будут отличаться на величины второго порядка малости). В этом случае в уравнениях ошибок и3 = 0 , Vi = V, где V - скорость летательного аппарата. Вектор абсолютной угловой скорости квазимодельного трёхгранника в проекциях на собственные оси имеет вид: = ( 0 , и + П2 , 0 ) г, П2 = V/Д - относительная угловая скорость аппарата. Тогда система уравнений ошибок запишется в виде:
Ду1 = s Vi ДУ2 = S 72 - /?3 V
s 71 = - ш 2 Дух - /?2 g + Д/i s V2 = - ш 2 Ду2 + /ig + Д/2 (5) ft = - (и + П2 )/ + Vi /2 = V2
//3 = (и + П2 )//i + V3
Что касается уравнений измерений, то из уравнения (3):
W = - Ду - pz - Джку ,
или в скалярном виде:
= — Дух - pdcos\jj + dsmi/jp^p - ß3K2y W2 = - Ду2 - pds irr/ - d с о s /р^ + / к i/
где // = 0 , d = d о - V t (d 0 - расстояние до маяка в начальный момент времени), а и
с точностью до малых второго порядка. Тогда система уравнений измерений примет вид:
г Wl = — Дух - pd
{ W2 = - Ду2 - (d о - V t) р^ + /3 ( d о - V t) •
(6)
Во втором случае можно считать, что угол измеряется между одной из осей географического трёхгранника и направлением на маяк (так как географический и квазиприборный трёхгранник отличаются на матрицу малого поворота, соответственно в уравнениях измерений члены, зависящие от угла, измеренного в осях квазиприборного трёхгранника отличаются от членов, зависящих от угла, измеренного в осях географического трёхгранника, на величину второго порядка малости). Кроме того, величина Яб1гг( в данной задаче оказывается очень малой, следовательно географический трёхгранник и в этом случае можно считать неизменно ориентированным во времени. Будем считать, что окружность достаточно малого радиуса по сравнению с радиусом Земли, поэтому Землю можно считать плоской, т.е. . Пусть - угол поворота летательного
аппарата вокруг маяка, // = —, £ - время движения, d - радиус окружности. Тогда компоненты скорости имеют вид: . Вектор аб-
солютной угловой скорости выглядит следующим образом: о)у = (П^и + П2 ,0 ) т, где П = — б 1 гг/, П-^ = — — со б //. Уравнения ошибок имеют вид:
f Ay i = 8 Vi + p3 ( - Vs i mfj) Ay2 = 5 V2 — P3( - Vco s p) 8V1 = -ш20АУ1 — p2g + Af± « 8V2 = — ш2 Ay2 + Pig + Af2. (7)
p± = - (u + П2) P3 + Vi p2 = CLip3 + V2 {p3 = (u + i ) pi — ili P2+ v3
В уравнениях измерений величины d = const, p = ", соответственно p Зк2y = p 3dsin",p3Kiy =
P3dcos" с точностью до малых второго порядка. Уравнения измерений принимают вид:
= —Ауг - pdcosp + dsimppp - p3ds\n гр { W 2 = — Ay 2 — pdsiml) — dcosp pp + p 3dcosp ( )
Модели инструментальных погрешностей A fz и v z выбираются следующим образом:
Afz = Afz° + Afz(s)
0 , (s) , (9)
vz = + vy
где первые слагаемые в моделях погрешностей это неизвестные постоянные составляющие, а вторые слагаемые - белые шумы известной интенсивности. В дальнейшем индекс при использовании ошибок датчиков будет опускаться.
Задача оценивания искомых параметров производится при помощи дискретного фильтра Калма-на. Основные соотношения фильтра Калмана здесь не приводятся, о них можно прочитать в [2]. Пусть имеется вектор состояния системы . Он состоит из двух компонент ошибок определения местоположения объекта в горизонтальной плоскости (Ayi, Ay 2 ), двух компонент вектора динамических ошибок скорости (8Vi, 8V2), трёх компонент вектора малого поворота ( , , ), задающего матрицу ориентации квазиприборного трёхгранника относительно квазимодельного, двух компонент постоянной составляющей ошибки акселерометров ( , ), трёх компонент постоянной составляющей ошибки датчиков угловых скоростей ( , , ), а также двух величин, отвечающих за постоянные составляющие ошибок определения дальности и угла ( , Ро) - всего 14 компонент.
Также имеется вектор измерений
z = (WiW) т.
В общем виде система записывается следующим образом:
х = А х + q, z = Нх + г,
(уравнения, соответствующие постоянным составляющим погрешностей, т.е. последние 7 уравнений системы записываются в виде х г = 0 ).
Здесь - матрица состояния системы, -матрица измерений, q,r - векторы шума динамической системы и помехи измерения соответственно. Предполагается, что шум в данной задаче - дискрет-
ный случайный процесс с нулевым средним и известной интенсивностью.
Возвращаясь к конкретной задаче, рассмотрим вид матриц А и Н для конкретных двух случаев. В первом случае (движение по лучу) матрица A определяется системой (5) с учётом модели ошибок датчиков (9), а матрица Н - системой (6) с использованием модели ошибок измерений (4).
Для случая движения по окружности матрица А определяется соотношениями (7) и (9), а матрица Н - соотношениями (8) и (4).
Далее записываются соотношения фильтра Калмана и оценивается вектор состояния системы. В данной работе не привлекается информация о векторе и используются только ковариационные соотношения (другими словами, алгоритм фильтра Калмана состоит только из этапа коррекции, этап прогноза не рассматривается)
Стандартные соотношения для фильтра Кал-мана записываются для случая, когда погрешности измерений не коррелируют между собой, т.е. матрица R = М [ггт] является диагональной, однако в случае, когда беспилотник движется по окружности, это условие нарушается. В этом случае невырожденным преобразованием вводится вектор z * = Cz = С (Нх + = Н*х + г*, такой, что координаты г* не будут коррелировать между собой. К примеру, если выбрать так, чтобы матрица равнялась единичной, то М[г* г*т] = СтRС = Е, т.е. i
С = R ~2. Матрица R в данной задаче зависит от дисперсий случайных составляющих погрешностей определения дальности и угла, которые полагаются известными.
В качестве примера можно привести результаты численного моделирования. Параметры выбирались следующими: скорость аппарата постоянна и равна 50 м/с, начальное расстояние до маяка при движении по лучу равно 100 км, радиус окружности во втором случае - 50 км. Время полёта аппарата - 10 мин. Численные значения ошибок БИНС следующие (имеются ввиду среднеквадратические отклонения величин): aAf<> = 0. 0 5 м/с2, а^и = 0.0 1 м/с 2, av0 = 0 . 1 °/с, av(S) = 0. 1 °/ч. Для задания начальных кова-риаций вектора состояния использовались следующие величины: м, м с,
, , м, . Для слу-
чайных составляющих ошибок определения дальности и угла соответственно взяты следующие модели:
м, . Для оценивания влияния
Pd Р-ф
использования дополнительных измерений использовались среднеквадратические отклонения ошибки оценки двух координат ( ) до и после добав-
ления измерений.
Для получения результатов использовалась программа реализации навигационных алгоритмов на языке C, написанная сотрудниками лаборатории Управления и Навигации кафедры Прикладной Механики и Управления МГУ.
Результаты представлены в следующей таблице.
Тип Оценива- С использо- Без ис-
движе- емая пе- ванием из- пользова-
ния ременная (СКО) мерений [м] ния измерений [м]
Движение по лучу Дух 11.23 37901
ду2 3083.6 37906
Движение по Д ух 2.4038 6181.9
окружности Ау2 41.079 6185
Отсюда можно сделать вывод, что лучше исследуемые переменные оцениваются при движении по окружности, откуда можно сделать вывод, что задача лучше обусловлена, когда в траектории дви-
жения аппарата присутствуют элементы кругового движения.
Автор выражает благодарность проф. Парус-никову Н.А. (МГУ), с.н.с. Попеленскому М.Ю. (МГУ), проф. Шунину Г.Е. (ВГТУ) за обсуждение результатов работы и ценные замечания.
Литература
1. Голован, А.А. Математические основы навигационных систем. Ч. 1. Математические модели инерци-альной навигации [Текст] / А.А. Голован, Н.А. Парусников. - М.: МАКС Пресс, 2011. - 136 ^
2. Голован А.А., Парусников Н.А. Математические основы навигационных систем. Ч. 2. Приложение методов оптимального оценивания к задачам навигации. [Текст] / А.А. Голован, Н.А. Парусников. - М.: Издательство Московского Университета, 2008. - 128 с.
Воронежский государственный технический университет
MATHEMATICAL MODELS OF CORRECTION OF STRAPDOWN INERTIAL NAVIGATION SYSTEM ON THE UNMANNED AERIAL VEHICLE BY USING PARTIAL INFORMATION
ABOUT THE TRAJECTORY OF MOTION
A.A. Kudryash
This article includes constitution of error equations for a problem of craft motion provided by using information about altitude as well as making equations based on measures of distance to the object and an angle. This article describes solution of a problem of estimation of craft position error. The problem of error estimation is underway by means of Kalman filter. Two types of motion are considered: full circle driving and driving on the straight
Key words: unmanned aerial vehicle, mathematical models, correction, inertial navigation system, errors, Kalman filter
