CC BY
16УДК 629.7.05
Разработка математического и алгоритмического обеспечения интегрированной
навигационной системы как элемента бортовой информационно-управляющей системы наземного подвижного объекта
А. В. Фомичев 1, Е. С. Лобусов 1, А. В. Пролетарский 1, В. В. Лукьянов 1, Е. Г. Одинцова 1, А. Д. Устюгов 1
1 МГТУ им. Н.Э. Баумана, Москва, 105005, Россия
Представлены результаты разработки подсистемы определения параметров движения (ПОПД) для наземного подвижного объекта в составе блока инерциальных датчиков и приемника спутниковой навигационной системы. Разработано математическое и алгоритмическое обеспечение ПОПД с учетом приборного состава и его размещения на объекте. Приведены результаты математического моделирования, демонстрирующие особенности и ограничения предлагаемой ПОПД.
E-mail: a.v.fomichev@bmstu.ru
Ключевые слова: блок инерциальных и навигационных датчиков, бортовая информационно-управляющая система, наземный подвижный объект, подсистема определения параметров движения, спутниковая навигационная система, схема обработки навигационной информации.
Для решения разнообразных задач, связанных с управлением наземным подвижным объектом (ПО), возникает потребность в определении наиболее важных переменных, характеризующих его движение. К их числу относятся такие переменные, как векторы ускорения и угловой скорости в связанной с объектом системе координат (ССК), вектор и модуль линейной скорости в локальной географической системе координат ENU, курсовой угол (угол рыскания), углы крена и тангажа.
Указанные переменные можно вычислять с помощью интегрированной навигационной системы (ИНС), в состав которой входят векторные датчики линейных ускорений (ВИЛУ) и угловой скорости (ВИУС), конструктивно оформленные в виде блока чувствительных элементов (БЧЭ) и жестко установленные в отведенных для них местах на ПО, а также приемник сигналов спутниковой навигационной системы (СНС). В общем случае на ПО может быть размещено несколько элементарных БЧЭ. Все отдельные БЧЭ и приемник СНС являются информационно-связанными.
Важную часть ИНС составляет математическое (алгоритмическое) обеспечение, которое и выполняет функцию вычисления указанных переменных движения.
Возможности вычисления переменных движения с помощью ИНС в составе средств инерциальной и спутниковой навигации являются предметом многочисленных исследований. Обработка информации, варианты построения, преимущества и недостатки различных схем интеграции достаточно подробно описаны как в отечественной литературе [1 — 3], так и в зарубежной [4, 5]. Оригинальность разработанного математического и алгоритмического обеспечения подсистемы определения переменных движения (ПОПД) заключается в том, что учитываются конструктивные особенности ПО и условия его эксплуатации.
Используемые системы координат. В связанной системе координат O0xyz ось О0у направлена на кабину ПО, ось O0z — вверх. В этой системе координат проводятся дальнейшие вычисления (рис. 1).
Рис. 1. Связанная система координат
Может быть использована и другая связанная с ПО система координат О0хрургр, которая отличается от системы О0ху1 тем, что ось О0хр направлена на корму ПО, ось Оогр — вверх. При необходимости между указанными системами координат легко осуществить соответствующее преобразование О0ху1 ^ О0хрур^р. Общее обозначение базиса ССК — В. Положение БЧЭ относительно начала ССК задается в
радиус-вектором гос.
Помимо ССК используют другую систему координат, связанную с Землей, — локальную географическую систему координат ENU, которая относится к классу навигационных систем координат (НСК). Общее обозначение базиса НСК — Б. Начало НСК располагается в текущем месте нахождения ПО (рис. 2). Углы ф и X — широта и долгота соответственно, ось Е (£) направлена на восток, ось N (п) — на север, ось и (0 — вверх.
При выводе уравнений движения ПО используют инерциальную система координат (ИСК), базис которой обозначают у. Переменные, характеризующие угловое движение ПО, приведены на рис. 3. К ним относятся углы последовательных поворотов 3-1-2, т. е. рыскание у, тангаж д и крен у.
E ($) ^
x
x
Рис. 3. Угловые переменные, характеризующие движение ПО
Структура математического обеспечения ИНС. Сигналы, которые подвергают обработке, являются сигналами измерения от трех типов измерительных датчиков, установленных в базисе ССК: от
а в
векторного измерителя угловой скорости ш , от векторного измерителя линейных ускорений яв, от приемника СНС ф (широта), X (долгота), им (составляющая скорости в направлении на север), иЕ
(составляющая скорости в направлении на восток).
Обработка указанных сигналов выполняется в соответствии с предлагаемой структурной схемой, приведенной на рис. 4.
уравнения
Рис. 4. Структурная схема обработки сигналов в ПОПД
Блоки, выделенные на рис. 4 цветом, обозначают измерительные устройства ИНС. Переменные, отмеченные сверху символом а
(Л. Л. Л /V N.
а, ш, £, и), характеризуют оценки соответствующих переменных.
Здесь обозначено: £ — оценка ускорения силы тяжести в месте
нахождения ПО (оценивается предварительно); = [и\с ,°2с ,0]т —
вектор горизонтальной скорости ПО в точке с (в относительном движении к Земле) в навигационной системе координат БЫи.; ВЕШ —
оператор углового положения ПО (чаще всего рассчитывается в ква-тернионном виде, однако можно сразу рассчитать и углы последовательных поворотов, так как их отклонения не превышают 30°);
= [—и2с, °1с, °1с ф]Т —1--транспортирующая скорость ПО в
Я® + к
точке с в осях ENU (рассчитывается по найденным оценкам горизонтальных проекций скорости и текущей широте места в блоке 2),
(R + h) ~ | r + го^| — геоцентрическая высота (оценивается предварительно); юф = юф [0, cos ф, sin ф]т — угловая скорость Земли в осях
системы координат ENU (определяется по известной широте, данные о которой поступают из приемника СНС).
В блоках 3 и 4 реализуется закон регулирования, позволяющий обеспечивать качество функционирования всей ИНС.
Движение ПО в вертикальном направлении может быть оценено по рассчитываемой компоненте вертикальной скорости (в точке с):
uB = tg а, иS =4 vi + u22c
с последующим интегрированием для получения оценки текущей высоты h
t
h = h0 + J uj dt.
t0
Следует обратить внимание на то, что интегрировать при наличии шумов не очень хорошо, поскольку их влияние может быть существенным. Поэтому необходимо аккуратно фильтровать сигнал
ЦВ. Более уместно оценивать высотную компоненту по скорости
центра масс (ЦМ) ПО ЦВ, а не по скорости в точке с.
В блоке 5 интегрируются уравнения кинематики углового движения ПО. В блоке 1 выполняется интегрирование ускорения либо процедурой интегрирования, либо более сложной процедурой, в которой учитывается влияние Земли.
Достоинство предложенной и рассмотренной структуры ИНС заключается в том, что реализуемая в ней схема обработки информации достаточно проста и понятна.
Особенности реализации контуров управления (коррекция по внешним источникам данных). Комплексирование связано с введением дополнительных сигналов управления (коррекции) в каналы ИНС. При этом считается, что все функционирующие сигналы являются непрерывными. В действительности внешние измерительные сигналы, рассматриваемые как эталонные, поступают в дискретном виде, причем такт дискретизации может существенно отличаться в большую сторону от основного такта обработки. Такое несоответствие тактов следует обязательно учитывать.
Рассмотрим схему согласования внешних сигналов и сигналов ИНС (рис. 5) и получения рассогласования. Сигнал ИНС можно считать практически непрерывным.
Если внешний сигнал поступает без запаздывания с известной дискретностью по времени, то сигнал от ИНС также преобразуется импульсным элементом с той же самой дискретностью. Разностный импульсный сигнал преобразуется в кусочно-непрерывный сигнал экстраполятором нулевого порядка.
Если внешний сигнал приходит с запаздыванием (известным) т, тогда сигнал ИНС получает такое же запаздывание т за счет введения специального блока запаздывания (на рис. 5 обозначен пунктирной линией).
I 'Сигнал ИНС
Сигнал ИНС
-тр;
Внешний сигнал от СНС
Без учета запаздывания С учетом запаздывания
Рис. 5. Схема согласования внешних сигналов и сигналов ПОПД
Построение имитатора внешней среды. Для отработки функционирования схемы, показанной на рис. 4, следует иметь возможность генерировать внешние сигналы, выступающие в качестве эталонных, линейного ускорения а, угловой скорости ш, линейной скорости и, которые воспринимаются соответствующими измерителями, а также гравитационного ускорения g.
Генерирование указанных внешних сигналов реализуется имитатором, в основу построения которого положены уравнения динамики и кинематики движения твердого тела (простейшая модель). Таким образом, программа-имитатор моделирует реальное движение ПО, осуществляемое под действием как управляющих, так и возмущающих воздействий. В качестве таких воздействий выступают величина скорости движения, вид траектории и текущее состояние полотна дороги.
Полное движение изделия складывается из рассмотрения его как сложного, состоящего из опорного пространственного движения ЦМ по картографической поверхности Земли и возмущенного движения, которое обусловлено неровностями дороги.
Для имитатора опорного движения предлагается использовать результаты натурных испытаний, в качестве которых служат оценки
e
переменных движения (угловые скорости последовательных поворотов и вектор линейной скорости и, начальное и конечное положения объекта) реальной траектории. Тем самым после обработки формируется траектория, характеризуемая набором радиусов кривизны и приближенная к реальной.
Предполагается, что траектория (кривая) определяется в трехмерном евклидовом пространстве и может характеризоваться определенным набором параметров, в качестве которых выступает радиус-вектор кривизны для выбранной точки траектории. При знании соответствующих оценок векторов линейной и угловой скоростей можно определить (рис. 6) положение радиуса кривизны, лежащего в плоскости, которая перпендикулярна векторам и и (9 + у). Здесь 9 — угол наклона траектории; у — угол пути. Вектор (9 + у) выделен на рис. 6 цветом (движение по крену в построении не участвует и удаляется предварительно из исходных данных). Теперь векторное соотношение, включающее радиус кривизны Якр, который направлен по вектору нормали п траектории, принимает следующий вид:
и = (9 + lj/)± х R
кр'
где символ ± у вектора обозначает проекцию этого вектора на плоскость (у', 2), перпендикулярную вектору и.
X x
Рис. 6. Схема определения радиуса кривизны
Единичные векторы t и Ь — это соответственно тангенциальный и бинормальный векторы траектории.
Данное векторное соотношение можно записать в системе координат (х'у^'):
и \j/ cos 0 0 -0"
0 = 0 0 0 *2 ; RKp = [ R1 R2 R3 Г »
0 0 -\j> cos 0 0 _ R3 _ кР
где Ri, R2, R3 — проекции радиус-вектора кривизны в системе координат (x'y'z').
Отсюда проекция R1 = 0.
Для нахождения проекций конкретных значений R2 и R3 следует дополнительно воспользоваться угловым положением вектора (9 + \j/) х и в плоскости (y 'z'). Проекции этого вектора равны
и = [0 0 -\{/ cos 0]т, а проекции радиус-вектора кривизны
R =
-и
R =
и
(ё2 + (у cos е)2 )/(ё У (ё2 + (у cos е)2 )Ду cos е)'
Модуль радиус-вектора кривизны RKp |=V R+R32. Угловые скорости при движении по траектории
■ a R3U \|/ cos ö= 3
R2 + R32
e= -r2u
R2 +
Таким образом, задача формирования траектории привязана к постановке реального эксперимента и обработке его результатов. Далее необходимо рассчитать навигационные переменные, соответствующие движению по сформированной траектории.
Для имитации возмущений опорного движения рассмотрено несколько подходов. Генерация возмущений как функции времени основана либо на традиционной идеологии формирующих фильтров, либо на использовании идеологии модуляции элементарных спектров, но их целесообразно применять в стационарных условиях (постоянная скорость движения). Другой подход предполагает генерацию возмущения как функции текущего расстояния и основан на замене координатной переменной «время» на координату «расстояние».
Результаты моделирования. Изложенный подход к оценке переменных движения (особенно угловых переменных) был отработан с помощью имитатора движения ПО. При этом исследовано влияние нецентрального расположения БЧЭ на борту ПО на показания ВИЛУ; проведены оценки функционирования автономного варианта схемы обработки измерительной информации в ИНС и варианта схемы обра-
ботки измерительной информации в ИНС с учетом комплексирования с приемником СНС.
Исходные данные для моделирования: широта места — 60°; шаг интегрирования — 0,005 с; расположение БЧЭ на ПО гов; движение ПО с разгоном до постоянной скорости — 10 м/с; погрешности ВИУС — [3-10-5, -2 10-5,2-10-5] с-1; погрешности ВИЛУ [-10-2, -10-2, 10-2] м/с2.
В качестве примера на рис. 7 приведены результаты моделирования одного из вариантов, для которого учитывали возмущения по измерительным и управляющим каналам, погрешности ВИУС и ВИЛУ, а также была проведена коррекция по СНС.
0 50 100 150 200 250 300
t, с
-4
0 50 100 150 200 250 300
б t, С
а
-3
x 10
4
3
2
Рис. 7. Результаты моделирования (начало):
а — изменение углового положения ПО, вызванное управлением по курсу 3 и возмущениями по скорости, углам тангажа 1 и крена 2; б — изменение угловой погрешности (с коррекцией от СНС) в кватерни-онном виде (1 — Хь 2 — Х2; 3 — Х3)
3.5 3 2.5 2 1.5 1
0.5 0
-1
0 50 100 150 200 250 300
t, с
в
15
10 5 0
-5 -10 -15
0 50 100 150 200 250 300
г, с
г
Рис. 7. Результаты моделирования (оокончание):
в — изменение угловой погрешности (с коррекцией от СНС) в кватернионном виде (1 — Хь 2 — Х2; 3 — Х3) без возмущений; г — изменение составляющих скорости по осям системы координат ЕЫи по результатам обработки (1 — иЕ; 2 — 3 — иЦ)
Выводы.
1. Показано, что использование только ВИЛУ и СНС не дает возможности оценивать с надлежащей точностью угловое положение ПО, что является важным в процессе движения.
2. Предложена схема оценки переменных движения ПО, использующая два вида измерительных устройств (ВИЛУ и ВИУС) сов-
Ч 3 I/
2
\ 1
местно с приемником СНС, обеспечивающим коррекцию их показаний. Данные от БЧЭ, установленного не в ЦМ ПО, оценивают поступательное движение ЦМ, возмущенное влиянием углового движения от нецентрального расположения БЧЭ.
3. Проведенное моделирование на разработанной модели движения ПО измерительных устройств и планируемых к использованию алгоритмов обработки показало практическую возможность оценивания переменных движения, включая и угловые переменные, как с коррекцией от СНС, так и в автономном режиме, но в ограниченном промежутке времени.
4. Реализация предлагаемой схемы оценивания позволит в дальнейшем разработать и ввести в практику чрезвычайно важный в процессе движения ПО режим горизонтирования платформы.
СПИСОК ЛИТЕРАТУРЫ
1. Бабич О.А. Обработка информации в навигационных комплексах. М.: Машиностроение, 1991. 512 с.
2. Лебедев Д.В., Ткаченко А.И. Системы инерциального управления: Алгоритмические аспекты. Киев: Наукова думка, 1991. 208 с.
3. Современные информационные технологии в задачах навигации и наведения беспилотных маневренных летательных аппаратов / под ред. М.Н. Красильщикова и Г.Г. Себрякова. М.: ФИЗМАТЛИТ, 2009. 562 с.
4. Savage P.G. Strapdown analytics / Maple Plain. Published by: Strapdown Associates, Inc. 2000.Vol. 1, 2.
5. Titterton D.H., Weston J.L. Strapdown inertial navigation technology, 2nd Ed. The Institution of Electrical Engineers, 2004.
Статья поступила в редакцию 25.10.2012
