CC BY
8
УДК 621.396.969 ЭС!: http://doi.org/10.25728/pu.2018.5.10
ПРИМЕНЕНИЕ МЕТОДА ПОЛУОПРЕДЕЛЕННОЙ РЕЛАКСАЦИИ ДЛЯ ОПРЕДЕЛЕНИЯ ОРИЕНТАЦИИ ТВЕРДОГО ТЕЛА В ПРОСТРАНСТВЕ1
Л.Б. Рапопорт
В данной работе метод полуопределенной релаксации применен к задаче об определении ориентации твердого тела относительно локального горизонта с помощью спутниковой навигации. Показано, как исходная невыпуклая задача квадратичного программирования погружается в более широкий класс выпуклых оптимизационных задач, допускающих эффективное решение. Вместо исходной вычислительно сложной задачи решена выпуклая задача, дающая приближенное решение исходной задачи. Предложенный подход применен к обработке экспериментальных данных.
Ключевые слова: спутниковая навигация, полуопределенная релаксация, задача 'М'аЬЪа.
ВВЕДЕНИЕ
Пусть на твердом теле закреплены четыре навигационные антенны, обозначенные через А1, А2, А3 и М(см. далее в § 4 рис. 1). На рис. 1, приведен пример расположения антенн на крыше автомобиля, использованного при проведении натурных испытаний. Антенна М служит подвижной базовой антенной при решении задач определения базовых линий между антеннами А1 и М, / = 1, 2, 3. Напомним, что в спутниковой навигации под базовой линией понимается вектор, соединяющий две навигационные антенны [1]. Таким образом, имеем три базовые линии (вектора), соединяющие антенну М с антеннами А1, А2 и А3. Обозначим эти векторы через Х(1), Х(2) и Х(3). С твердым телом связана система координат, называемая связанной.
Выразим векторы Х(1), Х(2) и Х() в координатах связанной системы и разместим их по столбцам в матрице Х0 размера 3x3. Поскольку связанная система координат вращается вместе с твердым телом, с которым она связана, то матрица Х0 от времени не зависит.
Вторая система координат связана с Землей и называется локальным горизонтом. Первая ось локального горизонта касательна к геоиду в точке,
1 Работа выполнена при финансовой поддержке Программы № 29 Президиума РАН «Актуальные проблемы робототех-
определенной антенной М, и направлена на север (т. е. касательна к меридиану). Вторая ось также касательна к геоиду и направлена на восток (каса-тельна к параллели). Третья ось ортогональна первым двум и направлена вниз. Три вектора базовых линий, выраженных в координатах локального горизонта, образуют матрицу Х(?), зависящую от времени. Ортогональная матрица, переводящая матрицу Х0 в матрицу Х(?), называется матрицей ориентации и обозначается О(?). В дальнейшем символ зависимости от времени для простоты опущен. Матрица О, выраженная через углы Эйлера, определяется выражением
СОБ у СОБ 9
соб9Бт у
V - Бт 9
О =
соб у Бт ф Бт 9 -- соб ф Бт у СОБ ф СОБ у + Л Бт ф Бт у Бт 9^ соб9Бт ф
Бт ф Бт у + Л + соб ф соб у Бт 9^ соб ф Бт у Бт 9 - соб у Бт ф СОБфСОБ9
,(1)
нических систем».
где у, 9 и ф обозначают азимут, тангаж и крен соответственно. Матрица Х определяется с помощью методов относительной фазово-дифференциаль-ной навигации, описанных например, в работе [1]. Исходными данными для вычислений служат кодовые и фазовые измерения, полученные для всех антенн, нескольких спутников и нескольких частотных диапазонов. Векторы определяются с ошибками, обусловленными влиянием шумов и многолучевых искажений на кодовые и фазовые измере-
ния. Для решения задачи определения матрицы Q пользуются распространенным приемом решения задачи Wahba, описанным в работах [2—4].
Этот прием описан далее в § 1 и состоит в минимизации суммы квадратов нормы отклонения векторов базовых линий, измеренных в координатах локального горизонта, от тех же векторов, но определенных поворотом из связанной системы координат к локальному горизонту. При решении задачи минимизации искомой является оценка матрицы поворота (1). Достоинство метода Wahba состоит в простоте реализации. В выражении для квадрата нормы невязок исчезает слагаемое, завит
сящее от Q Q, в силу ортогональности матрицы Q.
Недостаток, на который обращается внимание в настоящей работе, состоит в том, что в такой формулировке задачи о наименьших квадратах не учитываются корреляционные связи между ошибками оценки компонент векторов базовых линий. Нетрудно переформулировать задачу Wahba в терминах обобщенной задачи о наименьших квадратах, введя в рассмотрение весовую матрицу Ж. Однако при этом исчезает главное достоинство метода Wahba, поскольку теперь слагаемое, квадратично зависящее от искомой ортогональной
т
матрицы (точнее, от Q Ж^), не исчезает. Решение задачи Wahba с учетом весов рассмотрено также в работе [5], где применены метод погружения задачи в параметрический класс и метод продолжения по параметру. Использованию результатов фазовых измерений для определения ориентации твердого тела посвящено большое число работ (см., например, [6—10]). Однако эффективные и быстрые методы для использования результатов взвешенных фазовых измерений в этих работах не рассматриваются.
В настоящей работе предложен метод решения взвешенной задачи Wahba. В качестве формального вычислительного аппарата применяется метод полуопределенной релаксации, сводящий задачу к выпуклой оптимизации, для решения которой существуют эффективные инструменты, работающие в реальном времени. Поскольку соответствующие алгоритмы итеративны, то для решения задачи в следующий момент времени можно использовать решение, относящееся к предыдущему моменту времени. В работе приведены результаты натурных испытаний.
1. ЗАДАЧА WAHBA И МЕТОД ЕЕ РЕШЕНИЯ
Далее символ I будет обозначать единичную матрицу нужного размера. Нулевая матрица будет обозначаться 0. Для двух квадратных матриц М и N одинакового размера определяется скалярное
т
произведение выражением (М, N = 1г(М N = что в правой части выражения (4) матрицу
= tr(N M), где символ tr( •) означает след матрицы. Выражение (M, M) = ||M| |р определяет квадрат нормы Фробениуса матрицы M. Матрица Q, доставляющая минимум выражению ||QX0 - X|| Р, есть
решение задачи Wahba. Для нахождения решения преобразуем последнее выражение. Получим
IIQX0 - X Р = tr[(QXo - X)T(QXo - X)] =
= tr( QTQX0) - 2tr( QTX) + tr(XTX). (2)
Принимая во внимание, что под знаком tr(-) в произведении матриц сомножители можно менять местами, получим
tr( XT QTX) = tr(XX° QT). Тогда задача минимизации квадрата нормы невязки ||QX0 - X| р по ортогональной матрице Q
равносильна, в силу выражения (2), максимизации T T
следа матрицы tr(XX0 Q ) ^ max . Согласно ме-
QTQ = I
т
тоду [2, 3] вычислим SVD-разложение XX0 = UZ V,
где матрицы U и V ортогональны, а диагональная матрица £ составлена из сингулярных чисел мат-
т
рицы XX0 . Из линейной независимости столбцов матрицы X0 следует, что среди сингулярных чисел нет нулевых. Тогда оптимальная оценка матрицы Q, обеспечивающая максимум выражения tr( UZ VQT) = tr(2^VQTU) определяется выражением
VQTU = I'или QT = VTUT, Q = UV (3)
Таким образом решается задача Wahba c помощью SVD-разложения. Для каждого нового момента времени требуется вычисление SVD-разло-жения.
2. МЕТОД БОР-РЕЛАКСАЦИИ ДЛЯ РЕШЕНИЯ ЗАДАЧИ WAHBA
т
Заметим, что в выражении (2) слагаемое может быть опущено, поскольку оно не зависит от искомой матрицы Q. Поэтому выражение (2) можно переписать в виде
((
||QX0 - XIР = tr
X0X(° -X0XT -xxT 0 .
QQT QT
Q0
(4)
Обозначим G =
r T T Л
X0XT -X0X -XX0T 0
. Легко видеть,
00 0 I можно без изменения смысла заме-0 0 )
( т нить на У = 10
^ 0 I
Имеем ||ОХ0 - Х| ^ = Иг(СУ).
Далее выражение М > 0 обозначает неотрицательную определенность матрицы М. Условие
М > N означает М — N > 0. Условие ортогональ-т
ности 00 = I заменим на более слабое (релакси-
т
рованное) условие 00 — I > 0. Используя лемму Шура (см., например, работу [11]), получим условие неотрицательной определенности (полуопределенности) в виде
Y P 0.
(5)
Таким образом, задача Wahba может быть переписана в релаксированном виде как задача выпуклого полуопределенного программирования [11, 12]. При этом требуется минимизировать линейную форму
(G, Y) ^ min
(6)
при выпуклом ограничении (5). Релаксированное ограничение (5) вместо строгого исходного огра-
т
ничения 00 — I = 0 предполагает приближенное решение задачи Wahba. Однако вычислительные эксперименты с реальными данными показывают, что на практике решение релаксированной выпуклой задачи (5), (6) совпадает с точным решением исходной задачи (3).
3. ВЗВЕШЕННАЯ ЗАДАЧА WAHBA И ЕЕ РЕШЕНИЕ С ПОМОЩЬЮ БОР-РЕЛАКСАЦИИ
Наряду с задачей (2) рассмотрим задачу
II0Х) -Х||2р> ж = М(0Х) - Х)тЖ(0Х0 - Х)] = = 1г( хТ 0ТЖ0Х0) - 21г( хТ 0ТЖХ) +
+ tr(XT WX) ^ min
(7)
минимизации квадрата взвешенной нормы невязки. Здесь № — положительно определенная весовая матрица. Применение обобщенного метода наименьших квадратов предполагает использование в качестве весовой матрицы, обратной к ковариационной матрице С ошибок оценки векторов Х-г). Эта матрица получается одновременно с вычислением матрицы X. При этом в формуле (7)
№ = С-1.
В отличие от ранее рассмотренных методов те-
т т
перь слагаемое 1;г(Х0 0 №0Х0) в выражении (7) не
может быть опущено, поскольку оно не является
т
константой (матрица Q WQ не является единичной). В данной работе предложен двухэтапный метод решения задачи
|| QX0 - F, W ^ min при условии QTQ = I.
На первом этапе решается задача Wahba с единичной весовой матрицей любым из рассмотренных методов. Получим оценку Q *. На втором этапе находим коррекцию решения Q *, обусловленную введением весовой матрицы. Предполагая, что коррекция мала, представим Q в виде
Q = Q *(I + S),
(8)
где ^ = —^ это малая кососимметричная матрица. Обозначим X = X — 0 *Х0. Тогда выражение (7) может быть переписано в виде
II0X0 -XIд ж = II0*¿X -XIд ж = 1Г[(0*£Х, -
X )TW(Q *SX0 - X)] = tr( x( STQ *TWQ *SX0)
2tr(x(STQWX) + tr(XT WX) =
Т-rT „rT-r-
= tr
ff _T \f XoXT -XoX W
-WXX
0
0
STQ*TWQ* SST Q*T
Q* S
+ tr( XT WX) ^ min.
Обозначив G =
( XoX( -XoXTW^
-WXXT
0
0
(9)
и отбросив
— Т —
слагаемое 1г( X ЖХ), не зависящее от искомой матрицы перепишем выражения (9) в виде
ЮХ0 - X д ж =
f (
= tr
G
STQ*TWQ*SST Q*T
Q* S
= tr
G
ST Q*TWQ* SST Q*T
7-1
0 jj W
^ min.
jj
Q*S W" Последнюю задачу запишем в виде
tr( G Y) ^ min, Y =
f \
P STQ*T Q*S W-1
(10)
где P - STQ*TWQ*S
0. Заменив последнее ра-
венство на более слабое Р - ^ *т *£ > 0 и воспользовавшись леммой Шура, получим
Y P 0.
(11)
Задача (10), (11) представляет собой задачу полуопределенного программирования, которая решается любым из известных [11, 12] методов выпуклой оптимизации с полуопределенным ограничением. После решения задачи (10), (11) относительно £ и получения Q по формуле (8), воспользуемся одним шагом ортогонализации
Q = 1Ш + Ш-1)т).
4. РЕЗУЛЬТАТЫ НАТУРНЫХ ИСПЫТАНИЙ
На рис. 1 изображена крыша автомобиля, на которой закреплены антенны. Связанная система координат определена следующим образом. Первая ось направлена параллельно центральной оси автомобиля вперед. Вторая ось направлена ортогонально первой в горизонтальной плоскости и ориентирована в правую сторону. Третья ось направлена вниз ортогонально первым двум. На рис. 2 показано расположение антенн в горизонтальной плоскости связанной системы координат. Первая координата совпадает с осью ординат, вторая координата совпадает с осью абсцисс. Матрица Х0 имеет вид:
X,
0,693 0,339 0,349 0 -0,354 0,345 -0,233 -0,232 -0,228
Автомобиль ездил 30 мин в городе по плоской траектории, показанной на рис. 3. При этом крен и тангаж изменялись незначительно. Данные собирались на частоте 10 Гц. Четыре антенны PG-F1
Рис. 2. Расположение антенн в горизонтальной плоскости связанной системы координат (по осям абсцисс и ординат отложены первая и вторая координата, измеренные в метрах)
Рис. 1. Расположение антенн на крыше автомобиля
Рис. 3. Траектория движения автомобиля в координатах «широта — долгота» (координаты отложены в градусах северной широты и восточной долготы)
были подключены к приемникам N^^5 производства компании «Торсоп». Сырые данные, записанные в памяти приемников, были использованы для расчета матрицы Х(/). Поездке предшествовала статическая калибровка, проводившаяся в течение 30 мин. В процессе калибровки была определена матрица Х0. Графики изменения углов изображены на рис. 4. Задача полуопределенного программирования решалась с помощью пакетов 8ББиМ1 и УЛЬМ1Р для Matlab.
Весовая матрица была определена при расчете матрицы Х(/) для каждого момента времени t в результате применения алгоритма ЯТК [1] вместе с получением оценок векторов базовых линий. Алгоритм ЯТК реализован автором на языке С++.
ЛИТЕРАТУРА
1. Leick A., Rapoport L., Tatarnikov D. GPS Satellite Surveying. — Wiley & Sons, 2015.
2. Cohen C.E. Attitude Determination, in: Global Positioning System: Theory and Applications, Vol. 2, ed. by B.W. Parkinson and J.J. Spilker; Vol. 164, Progress in Astronautics and Aeronautics, AIAA, Reston, VA, 1994.
3. Wahba G. A Least Squares Estimate of Spacecraft Attitude // SIAM Review. — 1965. — Vol. 7, N 3. — P. 409.
4. Степанов О.А., Кошаев Д.А. Исследование методов решения задачи ориентации с использованием спутниковых систем // Гироскопия и навигация. — 1999. — № 2 (25). — С. 30—55.
5. Рапопорт Л.Б. Метод определения относительной ориентации // Проблемы управления. — 2010. — № 5. — C. 57—64.
6. Mogilnitsky V.G., Rapoport L.B., Khvalkov A.A., et al. Attitude/ RTK/INS Integrated System: Experimental Results // Proc. of the 17th Intern. Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS 2004), Long Beach, CA, September, 2004. — P. 792—800.
7. Liu S., Lei Zhang, Jian L., Luo Y. Dual Frequency Long-short Baseline Ambiguity Resolution for GNSS Attitude Determination // Proc. of IEEE/ION PLANS 2016, Savannah, GA, April. — 2016. — P. 630—637.
8. Shnaufer B., McGraw G., Phan H., Joseph A., GNSS-Based Dual-Antenna Heading Augmentation for Attitude and Heading Reference Systems // Proc. of the 29th Intern. Technical Meeting of The Satellite Division of the Institute of Navigation (ION GNSS + 2016), Portland, Oregon, September, 2016. — P. 3669—3691.
9. Garcia J.G., Axelrad P., Roncagliolo P.A., Muravchik C.H. Fast and Reliable GNSS Attitude Estimation Using a Constrained Bayesian Ambiguity Resolution Technique (C-BART) // Proc. of the 28th Intern. Technical Meeting of The Satellite Division of the Institute of Navigation (ION GNSS + 2015), Tampa, Florida, September. — 2015. — P. 2809—2820.
10. Nadarajah N, Teunissen P.J.G. Instantaneous GPS/BeiDou/ Galileo Attitude Determination: A Single-Frequency Robustness Analysis under Constrained Environments // Proc. of the ION 2013 Pacific PNT Meeting, Honolulu, Hawaii, April. — 2013. — P. 1088—1103.
11. Vandenberghe L., Boyd S. Semidefinite Programming // SIAM Review. — 1996. — Vol. 38. — P. 49—95.
12. Ben-Tal A., Nemirovski A. Lectures on Modern Convex Optimization: Analysis, Algorithms, and Engineering Applications / MOS-SIAM Series on Optimization. — Philadelphia, 2001.
Статья представлена к публикации членом редколлегии Б.В. Павловым.
Рапопорт Лев Борисович — д-р физ.-мат. наук, зав. лабораторией, Институт проблем управления им. В.А. Трапезникова РАН, г. Москва; профессор, Московский физико-технический институт (государственный университет), г. Долгопрудный, Н LBRapoport@gmail.com.
Не забудьте подписаться!
Подписку на журнал «Проблемы управления» можно оформить в любом почтовом отделении (подписной индекс 81708 в каталоге Роспечати или 38006 в объединенном каталоге «Пресса России»), а также через редакцию с любого месяца, при этом почтовые расходы редакция берет на себя. Отдельные номера редакция высылает по первому требованию.
Рис. 4. Графики зависимости углов ориентации от времени (по
оси абсцисс отложено время; единицей времени служит 0,1 с; всего на графиках представлены 18 000 отсчетов, соответствующие 1800 с)
Расчеты сделаны для двух вариантов: с весовой матрицей и без нее. Сравнение показывает примерно 10% уменьшения погрешности при использовании взвешивания. Сравнение точности проводилось на тех же статических данных, на которых проводилась калибровка.
ЗАКЛЮЧЕНИЕ
Показано, что задача определения ориентации твердого тела с помощью методов спутниковой навигации может быть сведена к выпуклой полуопределенной оптимизации. Выводы подтверждены натурным экспериментом.
Автор выражает признательность сотрудникам компании «Topcon» М.М. Костяному и В.М. Дмитриеву за помощь в проведении эксперимента.
W
