CC BY
53
УДК 681.513.3
ОПРЕДЕЛЕНИЕ ПРОСТРАНСТВЕННОГО ПОЛОЖЕНИЯ ШЛЕМА В НАШЛЕМНОЙ ИНФОРМАЦИОННО-УПРАВЛЯЮЩЕЙ
СИСТЕМЕ
О.В. Павлов
Приведены способы реализации систем позиционирования и их сравнительная оценка. Изложен алгоритм определения координат шлема по размещенным на нем четырем реперным светодиодным излучателям с использованием одной камеры. Предложен метод комплексирования оптико-электронной и инерциальной систем позиционирования с применением комплементарных фильтров Калмана. Показано, что данный метод обеспечивает необходимую точность определения пространственного положения шлема во всем рабочем диапазоне угловых перемещений.
Ключевые слова: гироскоп, акселерометр, реперные светодиоды, телевизионная камера, алгоритмы РпР, метод Гаусса-Ньютона, метод Левенберга-Марквардта, комплементарный фильтр Калмана.
При считывании информации с индикаторов на приборной панели кабины летательного аппарата (ЛА) летчик вынужден отвлекаться от визуального наблюдения наружной пилотажной обстановки. С целью непрерывного представления летчику оперативной информации, в какую бы сторону он ни смотрел, необходимо разместить индикатор на летном шлеме непосредственно перед глазами [1]. Поскольку нашлемный индикатор вместе с головой постоянно перемещается в пространстве кабины ЛА, а ряд пилотажных, навигационных и прицельных символов, таких как авиагоризонт, линия тангажа, курсовые метки, отметки целей и т.д., связаны с текущим положением ЛА, положение этих символов в поле индикации нашлемного индикатора должно изменяться в зависимости от текущего положения головы (шлема) в системе координат ЛА. Таким образом, необходимо с достаточной точностью постоянно отслеживать угловые координаты положения шлема в системе координат ЛА и использовать эту информацию при формировании информационных кадров [2].
Другой задачей, требующей точного определения пространственного положения шлема, является задача целеуказания управляемым авиационным средствам поражения. Летчик поворотом головы совмещает отображаемое на нашлемном индикаторе прицельное перекрестие с визуально наблюдаемой целью, которая может находиться не только прямо по курсу, но и в любой зоне обозреваемого пространства. Поскольку шлем и колли-маторный нашлемный индикатор жестко связаны конструктивно, угловые координаты линии визирования летчика и шлема совпадают, при этом угловые координаты положения шлема передаются как целеуказание в комплекс бортового вооружения.
Задачу определения положения шлема в пространстве кабины решает система позиционирования (СП), а конструктивное объединение нашлемного индикатора и СП образует нашлемную информационно-управляющую систему (НИУС) (рис. 1).
Рис. 1. Нашлемная информационно-управляющая система
СП могут строиться на различных физических принципах. В настоящее время существуют и нашли применение электромагнитные, инерци-альные и оптико-электронные СП.
Независимо от вида, СП должны обеспечивать высокую точность измерений. Основные параметры СП включают:
- диапазон возможных линейных и угловых перемещений головы;
- точность определения угловых координат;
- угловое разрешение;
- темп и латентность выдачи координатной информации.
Электромагнитные СП основаны на измерении параметров магнитного поля, создаваемого электрическим током [3]. Существует две разновидности электромагнитных СП, известные как системы переменного и постоянного тока. В обеих разновидностях используется передатчик, устанавливаемый жестко относительно неподвижной системы координат, и приемник, установленный на нашлемном модуле. Передатчик создает магнитное поле в рабочем объеме перемещений головы. Положение и ориентация приемника и, следовательно, головы определяется путем сравнения компонентов магнитного поля, измеренных приемником, с моделью поля, создаваемого источником.
К достоинствам электромагнитных СП следует отнести достаточно высокое быстродействие, снижающее динамическую погрешность, возможность измерения угловых координат головы в практически неограниченных диапазонах углов (±180°), а также весьма небольшие размеры и массу нашлемных приемников.
Существенным недостатком является чувствительность электромагнитных СП к искажениям сигнала вследствие взаимодействия магнитного поля с окружающими металлическими конструкциями. Для учета этих искажений в рабочем объеме перемещений головы должна проводиться трудоемкая процедура картографирования результирующего магнитного поля (mapping).
Инерциальные СП основаны на использовании трёх устройств: трехосных датчика угловой скорости (ДУС), акселерометра и магнитометра [4, 5]. ДУС позволяет измерять угловую ориентацию платформы, на которую он закреплен, относительно земной системы координат. Акселерометр измеряет ускорение платформы в определенном направлении, которое после двойного интегрирования дает величину линейного перемещения относительно исходного положения, а также позволяет оценить начальное угловое положение по крену и тангажу [6]. Магнитометр позволяет оценить начальное угловое положение по курсу, однако в нашлемных СП его применение не оправдано, так как требует проведения процедуры картографирования кабины по аналогии с электромагнитными СП.
Инерциальные СП обладают высоким быстродействием, что позволяет довольно точно отслеживать пространственное положение головы в движении. Поскольку система не имеет магнитных компонентов, на ее работу не оказывают влияние окружающие металлические предметы. Инер-циальные СП имеют наибольший из всех известных систем рабочий объем перемещений головы, в котором обеспечиваются измерения, они наиболее просты по конструкции, имеют малые габариты и массу при использовании микроэлектромеханических (MEMS) гироскопов и акселерометров.
К недостаткам инерциальных СП следует отнести наличие дрейфа сигналов ДУС и, как следствие, сравнительно высокую погрешность измерения координат, а также необходимость установки исходного положения системы.
Оптико-электронные СП являются на сегодняшний день наиболее предпочтительными для реализации в НИУС. Они не требуют картографирования рабочего пространтсва перемещения головы и имеют высокую чувствительность к малым угловым перемещениям, что обеспечивает их высокую точность.
Наибольшее распространение получили оптико-электронные СП, содержащие несколько точечных источников излучения, расположенных на нашлемном модуле, и один или несколько приемников излучения (датчиков), жестко связанных с системой координат кабины ЛА. В качестве источников излучения используются светодиоды с полусферической диаграммой направленности. Приемниками могут быть телевизионные камеры с матрицей или линейкой фоточувствительных элементов, позиционно-чувствительные фотоприемники и др.
128
Рассмотрим оптико-электронную СП включающую один приемник оптического излучения и 4 точечных источника излучения (репера). Геометрические построения для решения задачи трехмерной реконструкции по совокупности реперных излучателей с априорной известной конфигурацией, образующих кластер, приведены на рис. 2. Связь однородных пространственных координат п реперных точек объекта Модш- = =[Мг-, 1]т = [X, Yi, Zi, 1]т, - = 1,п , в системе координат камеры OXYZ с фокусным расстоянием объектива / и однородными пиксельными координатами их образов в плоскости изображения шг- = [и, V, 1]т при использовании математической модели проективной камеры определяется матрицей проекции Р [7]:
Ш- = -^РМодш, (1)
где wi = Р<3>Модш - масштабный коэффициент, <3> - третья строка матрицы.
При компенсации дисторсионных искажений, характерных для объектива реальной камеры, решение задачи восстановления 3Б-координат реперных точек Мг- по их изображениям шг-, - = 1, п, п > 3, обеспечивают алгоритмы РегересНуе-п-РоШ (РпР). Идея алгоритмов Р4Р основана на решении системы нелинейных уравнений:
$12 + $2 - 2сОБа12^1^2 = ^122, $12 + $32 - 2соБа13$1$3 = Д132,
$22 + $32 - 2сОБа23$2$3 = ^232, (2)
$12 + $42 - 2соБа14$1$4 = Щ142, $22 + $42 - 2соБа24$2$4 = Щ242, $32 + $42 - 2соБа34$3$4 = Щ342.
В системе (2) символами а/ обозначены углы между направлениями на реперы с координатами М- и М/, собо/ = шгш/7[||шг-||||ш/||], - декартово расстояние от начала СКК до точки Мг-, = ||Мг||, Щ = ||М0- - М0/|| ли-
нейное расстояние между реперами г и у, М0г = [Х0г, 70г, - априорно известные однородные ЭБ-координаты реперных точек 1-4 в системе координат кластера, г, у = 1,4, г Ф у, |||| - два-норма вектора.
Решение системы (2) может быть найдено путем перехода к аффин-но инвариантным барицентрическим координатам Мёбиуса [8] и сведения (2) к системе линейных уравнений, либо путем поиска минимума суммы квадратов невязок в (2), что приводит данную систему к полиному 8-го порядка [9]. Поскольку оба подхода позволяют найти Р < 4 решений задачи Р4Р, то выбирается тот из них, для которого квадрат два-нормы ошибки репроекции минимален:
4
шт X
к г=1
ш,
РМ (к)
к = 1, Р .
Найденное решение служит начальной точкой для поиска численного решения (Э) по методу Гаусса - Ньютона или Левенберга - Марк-вардта.
По известным ЭБ-координатам реперных точек М0г в начальный момент времени и текущим ЭБ-координатах Мг можно оценить параметры евклидова ЭБ преобразования - матрицу поворота Я и вектор параллельного переноса 1 [10]:
^10 0
Я = и
0 1
0
0 0 ёе^иут)
Ут
^ = М 0 - Я М
1
п
1
п
где М 0 = — X М0г, М = — X Мг, матрицы и и У получаются в результате
пг=1 пг =1
8УБ-разложения матрицы
п
Н = ШУ1 = £ М м М
т
сг
г=1
где М0сг = М0г - М0 и Мсг = Мг - М - центрированные ЭБ координаты в начальный и текущий моменты времени соответственно, п - количество реперов.
^11 12 Г1Э
Оценка матрицы поворота Я =
позволяет вычислить
Г21 г22 Г2Э
31 ГЭ2 ГЭЭ. значения углов Эйлера - курса ф, тангажа 6 и крена у:
ф = а1ап2(-гЭ1, г11), 6 = агевт(г21), у = а1ап2(-г2Э, г22), (Э)
за исключением случаев 6 = ±90°, когда Г21 = ±1 и ф = а1ап2(-Г1Э, гээ).
1Э0
2
Для снижения погрешности 3Б реконструкции реперного кластера требуется оценка координат центров отметок от реперных излучателей (рис. 3) с субпиксельной точностью.
Рис. 3. Изображение кластера светодиодных реперов (диоды 3Л153Б ближнего инфракрасного диапазона); в нижнем углу показано увеличенное изображение правого крайнего светодиода
Для этого вычисляют средневзвешенные величины (и(/), V (/)) центров
свечения:
и(]) V(])
I I uY(u, V)
и(/) V(] / I I vY (и, V)
и
(/) _ и=1 V=1
и () V () I IY (и, V)
и=1 У_1
(/) _ и=1 V=1
и (/) V ()
I I Y(u, V)
и=1 У=1
где Y(u, V) - яркость пикселя с координатами (и, V), и (/) и V (/) - размеры отметки изображения от /-го сегмента кластера по горизонтали и вертикали соответственно, оцениваемые по результатам бинаризации кадра с камеры.
Комплексирование СП различных типов. Каждый тип СП имеет те или иные характерные достоинства и недостатки. Одним из возможных способов решения проблемы взаимной компенсации недостатков СП, основанных на различных физических принципах, является их комплексиро-вание.
При комплексировании оптико-электронной и инерциальной СП появляется возможность снизить требования к оптико-электронной СП в части рабочего диапазона перемещений головы, сократить количество ре-перных излучателей и, следовательно, уменьшить габариты и вес нашлем-ного модуля.
Классическим способом комплексирования данных от различных источников является применение фильтра Калмана [11], описываемого в общем случае в виде двух последовательно выполняемых операций экстраполяции (4) и коррекции по результатам измерений (5):
T
(4)
(5)
х k = Ах к-1 + Ви k-l, Рк = Apk-1А + ^ kk = р^нг (нр^н7 + я)-1, Pk =(1 - Кк Н )Р*,
хк = х- + Кк(г к - Нх-) где х и г - векторы состояния и наблюдений соответственно, А, В и К -соответственно матрицы динамики системы, применения управляющего воздействия и и коэффициентов усиления фильтра, Р - апостериорная матрица ковариаций ошибок предсказания, Н - матрица взаимосвязи между векторами х и г, г = Нх, Q и Я - матрицы ковариации шума процесса и шума измерений, I - единичная матрица, а символом « - » обозначено априорное состояние.
Известно [12], что при объединении навигационных данных о значениях углов Эйлера взаимосвязь векторов состояний и наблюдений является нелинейной:
z = h(x)=
2 2 + X3
atan2 2(^2 x3 + xq x1), xq — x1
arcsin[- 2(xjX2 + xq x3)] atan2 2( x1x2 + xq x3), xq + x2 — x2" + x2
(6)
где вектор состояния x является текущим кватернионом поворота [13]. По этой причине использовать линейный фильтр Калмана (4), (5) для оценки вектора xk затруднительно. Возможными путями решения данной проблемы являются применение либо расширенного фильтра Калмана с соответствующим вычислением частных производных функции h() в (6), либо использование различных численных аппроксимаций апостериорной плотности распределения вероятностей с применением ансцентного фильтра Калмана [14].
На практике для объединения навигационной информации от MEMS датчиков получило распространение применение упрощенного варианта реализации фильтра Калмана - альфа-бета фильтра [15], уравнения которого имеют вид:
xk = Fi(p)zk + F2(p)xk /p, zk = H(p)xk + nk, (7)
где H(p) и F1(p) - передаточные функции для фильтров нижних, а F2(p) -верхних частот, nk -гауссовский шум с нулевым средним
При F1(p) + F2(p) = 1 фильтр (7) является комплементарным фильтром Калмана (КФК) с весовым коэффициентом 5:
xk = 5zk + (1 - 5) x k /p. (8)
При объединении данных от инерциальной и оптико-электронной СП применение КФК позволяет осуществлять постоянную коррекцию угловых координат положения шлема, вычисляемых инерциальной СП, по результатам работы оптико-электронной СП, чтобы в случае нештатных
132
режимов работы последней (пропадание оптической связи или солнечная засветка) временно выдавать координаты положения шлема только по данным от инерциальной СП. Если ввести в рассмотрение логический признак условий работы оптико-электронной СП у,
Г1, штатные условия работы оптико - электронной СП, У = \
[0, нештатный режим работы оптико - электронной СП, то алгоритм работы комбинированной СП описывается выражениями
[ф-, у-, 6г]т = у5[фг'_опт, 6г-_опт, уг-_опт]т + (1-у5)[ф,_МЕМЗ, Ъ'МЕМБ, У'МЕМБ^, (9)
[ф/_ЛВ, У_ЛВ, 0/_Лв]Т =
Т
[ф/, q, y ] , g=1,
[ф/ MEMS, 0/ MEMS, V / MEMS g= 0
Т
(10)
где [ф/_ЛВ, 0/_ЛВ, у/_ЛВ]Т - вектор-столбец текущих угловых координат линии визирования (ЛВ), [ф/_опт, 0/_опт, У/_опт]Т и [j/_MEMS, 0/_MEMS, V/_MEMs]T -векторы-столбцы текущих угловых координат, вычисленных оптико-электронной и инерциальной СП соответственно.
Структурная схема, реализующая комплексирование согласно (9) и (10), приведена на рис. 4, где приняты следующие обозначения: ФПА -функциональный преобразователь, выполняющий операцию оценивания крена уа и тангажа 0а по сигналам MEMS акселерометра [ax, ay, az]T:
уа = atan2(ay, az), 0а = atan2[-ax, (ay2 + az2)0,5], ФПГ - функциональный преобразователь сигналов MEMS гироскопа w = [Wx, wy, Wz]T для оценивания вектора угловых скоростей в системе координат шлема:
*
W = XWXc,
где xc - сопряженный кватернион, КФКмем:? и КФКопт - комплементарные фильтры Калмана, реализующие операции объединения данных от MEMS акселерометра и гироскопа по (7) и (8) и операции объединения данных от инерциальной и оптико-электронной СП по (9) и (10) соответственно.
Рис. 4. Структурная схема комплексирования информации от оптико-электронной и инерциальной СП
133
Необходимо отметить, что использование результатов вычислений по (9) позволяет повысить темп выдачи угловых координат ЛВ в случае, если частота следования кадров датчиков оптико-электронной СП ниже частоты дискретизации сигналов MEMS датчика. Блок ФПГ также выполняет операции оценивания постоянных смещений сигналов MEMS гироскопа (дрейфа) для их компенсации. Для компенсации угловых перемещений ЛА в блок ФПГ также поступают данные от бортовой инерциальной навигационной системы (БИНС).
Экспериментальные исследования комбинированной СП. Точность оценивания координат комбинированной СП, реализованной в опытном образце НИУС (рис. 1), исследовалась с помощью координатно-измерительной машины (КИМ) CimCore Infinite 5030SC с погрешностью измерения линейных координат менее 50 мкм. В ходе эксперимента нашлемный модуль НСЦИ перемещался на поворотной платформе в азимутальной и угломестной плоскостях в диапазонах ±90° и -60...+30° соответственно. По его трехмерным координатам, измеренным КИМ, оценивались угловые координаты относительно исходного положения и сравнивались с показаниями СП. Результаты эксперимента графически представлены на рис. 5.
-90° -80° -70° -60° -50° -40° -30° -20° -10° 0° 10° 20° 30° 40° 50° 60° 70° 80° 90°
30°
20°
10°
0°
-10°
-20°
-30°
-40°
-50°
-60°
Рис. 5. Рабочая область СП НСЦИ
На рис. 5 темно-серым цветом показана область работы оптико-электронной СП, в которой среднеквадратическая погрешность определения угловых координат не превышает 15'; светло-серым цветом - область работы оптико-электронной СП с одной камерой с погрешностью не более 20'; без заливки - область работы инерциальной СП с погрешностью не более 50' на временном интервале порядка десятков секунд.
Заключение. Комбинированная СП позволяет объединить достоинства оптико-электронной (высокая точность, отсутствие дрейфа, нечувствительность к изменению температуры) и инерциальной (малая инерционность,
высокая скорость обновления данных, нечувствительность к засветкам) систем. Необходимая для инерциальной СП периодическая подстройка (выставка) осуществляется по данным от оптико-электронной СП. При временных нарушениях в работе оптико-электронной СП вследствие солнечной засветки или прерывания оптической связи между излучателями и приемниками, координаты шлема продолжают определяться инерциальной СП.
Список литературы
1. Костяшкин Л.Н., Павлов О.В., Трофимов Д.В. Проблемные аспекты разработки нашлемных систем целеуказания и индикации для вертолетов // Мехатроника, автоматизация, управление. 2009. № 3. С. 57-63.
2. Обработка изображений в авиационных системах технического зрения / под ред. Л.Н. Костяшкина и М.Б. Никифорова. М.: Физматлит, 2016. 234 с.
3. Филатов О.Г., Солдатенков В.А. Электромагнитная система позиционирования для нашлемной системы целеуказания и индикации // Электроника: наука, технология, бизнес. 2003. № 5. С. 62-67.
4. Mahony R., Hamel Т., Pflimlin J.-M. Complementary filter design on the special orthogonal group SO(3) // Proceedings of the IEEE Conference on Decision and Control. Seville, 2005. P. 1477-1484.
5. Madgwick S., Harrison A., Vaidyanathan R. Estimation of IMU and MARG orientation using a gradient descent algorithm // IEEE International Conference on Rehabilitation Robotics Rehab Week. Zurich, 2011.
6. Будков С. А., Ларкин Е.В. Определение пространственного положения рабочего органа // Известия Тульского государственного университета. Технические науки. 2013. Вып. 10. С. 197-202.
7. Prince S. Computer vision: models, learning and inference. Cambridge: Cambridge University Press, 2012. 665 р.
8. Lepetit V., Moreno-Noguer F., Fua P. EPnP: An Accurate O(n) Solution to the PnP Problem // International Journal of Computer Vision. 2009. Vol. 81, No. 2. P. 155-166.
9. Li S., Xu C., Xie M. A Robust O(n) Solution to the Perspective-n-Point Problem // IEEE Trans. on Pattern Analysis and Machine Intelligence. 2012. Vol. 34, Is. 7. P. 1444-1450.
10. Umeyama S. Least-Squares Estimation of Transformation Parameters Between Two Point Patterns // IEEE Trans. on Pattern Analysis and Machine Intelligence. 1991. Vol. 13, Is. 4. P. 376-380.
11. Welch G., Bishop G. An Introduction to the Kalman Filter. Technical Report TR 95-041. Chapel Hill: University of North Carolina at Chapel Hill, 1995.
12. Белокуров В.А. Система угловой ориентации на основе гауссов-ского парциального фильтра // Вестник Рязанского государственного радиотехнического университета. 2016. № 56. С. 11-16.
135
13. Челноков Ю.Н. Кватернионные и бикватернионные модели и методы механики твердого тела и их приложения. Геометрия и кинематика движения. М.: ФИЗМАТЛИТ, 2006. 512 с.
14. LaViola J. A Comparison of Unscented and Extended Kalman Filtering for Estimating Quaternion Motion // Proceedings of the American Control Conference. Denver, 2003. P. 2435-2440.
15. Mahony R., Hamel T. Attitude estimation on SO(3) based on direct inertial measurements // Proc. Int. Conf. Robotics Automation (ICRA). Orlando, 2006. P. 2170-2175.
Павлов Олег Вячеславович, начальник лаборатории научно-конструкторского центра видеокомпьютерных технологий, [email protected], Россия, Рязань, АО ««Государственный Рязанский приборный завод»
DETERMINATION OF HELMET SPATIAL POSITION IN HELMET-MOUNTED INFORMATION AND CONTROL SYSTEM
O.V. Pavlov
Methods of implementing positioning systems and their comparative evaluation are presented. The algorithm for determining helmet coordinates according to four reference LED emitters placed on it using a single camera is described. A methodfor integrating optoelectronic and inertial positioning systems using complementary Kalman filters is proposed. It is shown that this method provides the necessary accuracy of determining the spatial position of the helmet in the entire working range of angular displacements.
Key words: gyroscope, accelerometer, reference LEDs, television camera, PnP algorithms, Gauss-Newton method, Levenberg-Marquardt method, complementary Kalman filter.
Pavlov Oleg Vyacheslavovich, head of laboratory of Scientific and Design Center of Video Computing Technologies, oleg.pavlov@mail. ru, Russia, Ryazan, JSC «Ryazan State Instrument-making Enterprise»
