CC BY
67УДК 621.396.969.181.23 DOI:10.31854/1813-324X-2021-7-2-51-67
Позиционирование транспортных средств
с комплексированием дальномерных, угломерных и инерциальных измерений в расширенном фильтре Калмана
Г.А. Фокин1®, А.Г. Владыко1®
!Санкт-Петербургский государственный университет телекоммуникаций им. проф. М.А. Бонч-Бруевича, Санкт-Петербург, 193232, Российская Федерация *Адрес для переписки: grihafokin@gmail.com
Информация о статье
Поступила в редакцию 08.05.2021 Принята к публикации 31.05.2021
Ссылка для цитирования: Фокин Г.А., Владыко А.Г. Позиционирование транспортных средств с комплексированием дальномерных, угломерных и инерциальных измерений в расширенном фильтре Калмана // Труды учебных заведений связи. 2021. Т. 7. № 2. С. 51-67. D0I:10.31854/1813-324X-2021-7-2-51-67
Аннотация: Настоящая работа посвящена исследованию моделей и методов повышения точности позиционирования в сверхплотных сетях радиодоступа V2X/5G при маневрах транспортных средств за счет комплексирования дальномерных и угломерных первичных измерений с измерениями инерциальных навигационных систем в расширенном фильтре Калмана. Бортовая бесплатформенная инерциальная навигационная система представлена модулями трехосевого акселерометра и гироскопа. Интеграция первичных инерциальных измерений ускорения и угловой скорости с первичными радиотехническими измерениями угла и времени прихода сигнала осуществляются путем преобразования инерциальной системы координат акселерометра и гироскопа в связанную систему координат транспортного средства с использованием кватернионов. Вторичная обработка инерциальных и радиотехнических измерений осуществляется в расширенном фильтре Калмана. Результаты комплексирования показывают повышение точности оценки траектории транспортного средства с нескольких метров до одного метра при повороте на перекрестке.
Ключевые слова: позиционирование, сверхплотные сети 5G, vehicle to everything, дальномерные и угломерные измерения, инерциальная навигационная система, расширенный фильтр Калмана, кватернионы.
1. ВВЕДЕНИЕ
Одними из основных первичных измерений в радиотехнических системах позиционирования являются дальномерные (ДМ, аббр. от англ. Time of Arrival - TOA), разностно-дальномерные (TDOA, аббр. от англ. Time Difference of Arrival и угломерные (УМ, аббр. от англ. Angle of Arrival - AOA) [1, 2]. В сетях мобильной связи четвертого поколения разностно-дальномерные измерения получили распространение для определения местоположения (МП) абонентских станций [3], а УМ измерения -для пеленгации базовых станций [4]. Для позиционирования внутри помещений распространение получили также методы радиокарты, основанные на первичных измерения индикатора уровня принимаемого сигнала RSSI (аббр. от англ. Received Signal Strength Indication) [5]. Радиокарту внутри помещений можно построить по сигналам RSSI,
принимаемым подвижными устройствами от точек доступа Wi-Fi [6], однако определение МП исключительно методом радиокарты по соотношению трудозатрат и достижимой точности на сегодняшний день оказывается неконкурентоспособным. Для повышения точности позиционирования служит подход, основанный на комплексировании измерений RSSI с измерениями инерциальной навигационной системы (ИНС, аббр. от англ. Inertial Navigation Systems - INS), которой оборудованы современные абонентские устройства [7]. Распространенным инструментом комплексирования разнородных измерений служит подход, основанный на фильтре Калмана (ФК) [8, 9].
Предложенный в 1960 г. [10] Р. Калманом рекурсивный фильтр оценки вектора состояния динамической системы (объекта определения МП) получил широкое распространение в системах позицио-
нирования для вторичной обработки зашумленных первичных измерений; при этом размерность искомого вектора состояния объекта определения МП может превосходить размерность вектора первичных измерений. ФК осуществляет рекурсивную оценку вектора состояния априорно известной динамической системы: вектор текущего состояния рассчитывается на основе текущих измерений с использованием предыдущего состояния ФК [10]. Алгоритм ФК включает два этапа: на первом этапе ФК прогнозирует значения переменных вектора состояния; на втором этапе результаты прогноза уточняются [11, 12]. С точки зрения позиционирования подвижных объектов пошаговая работа ФК позволяет прогнозировать траекторию в реальном времени, используя исключительно текущие измерения и вектор предыдущего состояния [13]. Для частного случая гауссовского распределения ошибок первичных измерений ФК дает оптимальную оценку искомых параметров состояния динамической системы по критерию среднеквадратической ошибки (СКО) [14].
При формализации ФК для конкретного практического приложения используют понятие вектора состояния, включающего параметры, описывающие состояние динамической системы в каждый момент времени. Зная математическую модель наблюдений (измерений) и модель априорного изменения вектора состояния, можно записать уравнение ФК. При нелинейной зависимости оценок координат позиционируемого объекта от первичных измерений для их вторичной обработки используется расширенный фильтр Калмана (РФК, аббр. от англ. Extended Kalman Filter - EKF), который осуществляет линеаризацию нелинейных моделей наблюдений с помощью разложения в ряд Тейлора [15].
Использование фильтра Калмана при позиционировании транспортных средств (ТС, аббр. от англ. Vehicular User Equipment - VUE) позволяет получать их оптимальные оценки координат (ОК), скорости и/или ускорения в результате вторичной обработки зашумленных первичных измерений в режиме реального времени с непрерывным обновлением оценок по мере движения. В интеллектуальных транспортных системах для сверхплотных сетей радиодоступа V2X/5G решению подлежит задача оценки координат с точностью до 1 м и скорости движения до 150 км/ч [9]. Здесь оценка траектории ТС, включая определение его местоположения (МП), скорости и/или ускорения, осуществляется на основе дальномерных и угломерных первичных измерений, которые собираются опорными пунктами приема (ПП) дорожной инфраструктуры (RSU, аббр. от англ. Roadside Unit) с определенной погрешностью измерений времени и угла прихода сигнала. ФК, используя вторичную обработку ДМ/УМ измерений, позволяет прогнозировать траекторию движения ТС с требуемой точностью.
Позиционирование транспортных средств в сверхплотных сетях радиодоступа V2X/5G с использованием комбинирования ДМ и УМ первичных измерений в расширенном фильтре Калмана было исследовано в [9]. Результаты имитационного моделирования (ИМ) показали возможность достижения точности до 1 м для сценария движения ТС по прямой при погрешности в 50 нс для ДМ и 2° для УМ измерений. При маневрировании транспортных средств, в том числе при поворотах на перекрестках, точность позиционирования снижается до нескольких метров (независимо от погрешности первичных измерений), что объясняется фиксированным интервалом их сбора. Проведенный анализ показал, что для повышения точности первичных измерений можно уменьшать периодичность сбора первичных измерений. Однако это не всегда реализуемо на практике. Альтернативным вариантом компенсации ошибок позиционирования на перекрестках является ком-плексирование радиотехнических измерений с данными бортовых ИНС, которые на интервале поворота позволяют компенсировать неточность ДМ и УМ измерений.
Целью настоящей работы является разработка и исследование моделей и методов повышения точности позиционирования в сверхплотных сетях радиодоступа V2X/5G при повороте транспортных средств на перекрестках за счет ком-плексирования ДМ и УМ первичных измерений с измерениями ИНС в в РФК.
Для достижения поставленной цели далее решаются следующие частные задачи. В разделе 2 представлена формализация обобщенной модели ФК. В разделе 3 приводятся особенности использования ИНС применительно к задаче позиционирования ТС, в том числе, преобразование инерци-альной системы координат (СК) акселерометра и гироскопа в связанную СК ТС с использованием кватернионов. Раздел 4 содержит математические модели комплексирования радиотехнических ДМ и УМ измерений с измерениями ИНС в РФК. В разделе 5 представлены результаты ИМ позиционирования ТС с использованием ДМ/УМ/ИНС измерений в РФК для сценария поворота на перекрестке. Раздел 6 содержит выводы и направления дальнейших исследований.
2. ОБОБЩЕННАЯ МАТЕМАТИЧЕСКАЯ МОДЕЛЬ
ФИЛЬТРА КАЛМАНА
Обобщенная математическая модель ФК формализует работу дискретизированной по времени линейной динамической системы, которая описывается вектором состояния. Линейный оператор, воздействуя в каждый дискретный момент времени на вектор состояния, переводит его в другой вектор состояния, добавляя вектор шума и вектор управления. Оценка вектора состояния динамиче-
скои системы производится на основе зашумлен-ных измерении и может быть формализована структурой фильтра, описываемого матричным уравнением (рисунок 1). Переменными уравнения в каждый дискретный момент времени к являются
следующие матрицы: ¥к - матрица эволюции системы; - матрица измерений; Qfc - ковариационная матрица системы; - ковариационная матрица шума измерений; - матрица коэффициентов управляющих воздействий.
Наблюдаемые
Рис. 1. Обобщенная математическая модель ФК
Fig. 1. Generalized Mathematical Model of the Kalman Filter
Матричное уравнение ФК связывает вектор состояния динамической системы хк в дискретный момент времени к с ее состоянием хк-1 в момент к-1:
= + Якик + шк, (1)
где - вектор управляющих воздействий, к которому прикладывается матрица ; шк - вектор эволюции системы, имеющий нормальное распределение с нулевым математическим ожиданием и ковариационной матрицей Qfc:
«Ъ-N(0, Цк ). (2)
Вектор состояния динамической системы хк в дискретный момент времени к связан с вектором первичных измерений гк соотношением:
гк = Н кхк + \к, (3)
где матрица измерений связывает вектор состояния хк с вектором измерений гк; \к - вектор шума измерений с нулевым математическим ожиданием и ковариационной матрицей
Ук~Я(0, Кк). (4)
Рекурсивное свойство ФК проявляется в зависимости оценки вектора состояния системы хк в текущий момент времени k от оценки вектора состояния %к-1 и погрешности этой оценки в предыдущий момент времени к-1, а также от вектора измерения гк в текущий момент времени к. Введем следующие переменные для формализации состояний ФК: х+ - апостериорная оценка состояния системы в момент к, полученная по результатам измерений вплоть до момента к; Р+ - апостериорная ковариационная матрица ошибок оценки вектора состояния х+, включающая дисперсии погрешности вычисленного состояния и ковариации параметров состояния системы. В каждый момент времени к работа ФК включает два этапа: этап прогноза и этап коррекции. На этапе прогноза (предсказания) предварительная оценка состояния х- в момент к получается по (апостериорной) оценке состояния хк_1 в предыдущий момент вре-
мени к-1; оценка х- называется априорной, так как не использует текущие измерения. На этапе коррекции априорная оценка дополняется текущими измерениями; скорректированная оценка х+ называется апостериорной оценкой состояния. С точки зрения позиционирования ТС по радиотехническим и инерциальным измерениям прогноз текущего состояния ТС может производиться на основе измерений, собранных в предыдущий момент времени, а коррекция осуществляется по текущим доступным измерениям. При этом, если в какой-то момент времени радиотехнические (апостериорные) измерения становятся недоступными, этап коррекции может быть пропущен, а прогноз выполнен по (априорным) измерениям ИНС.
Этап прогноза (предсказание) априорной оценки вектора состояния х- и ковариационной матрицы Р- в момент времени к определяются выражениями:
X- = ^х^! + Вкик, (5)
Р- = + Цк. (6)
Этап коррекции определяется следующими выражениями.
Вектор отклонения (ошибки) полученного в момент к измерения гк от априорной оценки х-:
Ук = гк- Нкх-. (7)
Ковариационная матрица вектора отклонения (ошибки) ук:
^ = НЛРЙ Н£ + (8)
Матрица коэффициентов усиления по Калману получается из ковариационной матриц этапа предсказания Р- и вектора отклонения
Кк = Р-Н^-1. (9)
Коррекция априорной оценки вектора состояния х- и ковариационной матрицы Р- с получением апостериорной оценки вектора состояния хк и ковариационной матрицы Р^ в момент времени к определяются выражениями:
х+ = Х- + Ккук, (10)
Р+ = (I - КкНк)Р_, (11)
где I - единичная матрица.
Для позиционирования ТС при его прямолинейном движении с постоянным ускорением ак (имеющим нормальное распределение с нулевым математическим ожиданием и СКО оа) между измерениями к и к-1 и периодом их сбора Д£ оценка вектора состояния хк определяется (значок шляпки далее опускается):
х-к = + Сак, (12)
где вектор состояния ФК в Ю хк = [х, ух]г включает координату х и скорость ух, вектор управления вырождается в скаляр ак, матрица эволюции системы представлена в виде:
матрица управления (коэффициентов управляющих воздействий) - вектор:
G = [^/2]. (14)
Представленную в настоящем разделе модель ФК далее уточним для РФК при комплексирова-нии ДМ/УМ/ИНС измерений в разделе 4.
3. ИСПОЛЬЗОВАНИЕ ИНС В ЗАДАЧАХ
ПОЗИЦИОНИРОВАНИЯ 3.1. Особенности использования ИНС
Рассмотрим особенности использования ИНС в задачах позиционирования [16-23]. Оценка координат ТС осуществляется с использованием РФК на основе ДМ/УМ измерений, а также измерений от модулей ИНС (IMU, аббр. от англ. Inertial Measurement Unit). Первичные ДМ/УМ измерения собираются стационарными ПП дорожной инфраструктуры, положение которых известно; число ПП в каждый момент движения ТС определяется дальностью связи между ПП и ТС.
В качестве модулей IMU используются микроэлектромеханические системы, объединяющие в себе микроэлектронные и микромеханические устройства: механическим устройством может быть инерциальный датчик (акселерометр, датчик угловых скоростей, гироскоп, барометрический датчик). В настоящей работе в качестве датчиков рассматривается применение микромеханических гироскопов и микромеханических акселерометров. В РФК ИНС используется на этапе формирования априорной оценки на основе первичных измерений 3-осевого акселерометра и 3-осевого гироскопа; при этом гироскопы обеспечивают ориентацию акселерометров в пространстве.
Комплексирование радиотехнических ДМ/УМ измерений с измерениями ИНС оправдано в случаях, когда использование глобальной навигационной
спутниковой системы (ГНСС) невозможно или затруднено, а также в сценариях, когда ДМ/УМ измерения оказываются недоступными; примером является движение ТС в тоннелях, а также применение промышленного транспорта на предприятиях, складах и иных объектах, где в явном виде использование радиосигналов проблематично в силу отсутствия условий прямой видимости [24-29].
В отличие от неавтономных радиотехнических методов ДМ/УМ измерений ИНС функционирует без использования внешних источников/приемников радиосигналов; оценка траектории объекта с использованием ИНС основана на его инерции и реализуется бортовыми средствами 1Ми автономно. Наиболее распространенными модулями IMU являются датчики линейного ускорения (акселерометры) и угловой скорости (гироскопы); измерив ускорение и угловую скорость вычислительный модуль IMU путем интегрирования может определить координаты подвижного объекта. ИНС, получив углы ориентации (курс, тангаж и крен) может определить отклонение локальной СК, связанной с корпусом датчика М^ от глобальной СК, связанной с Землей.
Таким образом, работа ИНС заключается в измерении ускорений и угловых скоростей подвижного объекта относительно трех осей СК для того, чтобы, исходя из этих данных, определить МП и скорость объекта; акселерометры измеряют линейное ускорение, а гироскопы определяют углы тангажа, рысканья и крена подвижного объекта относительно осей СК.
В зависимости от реализации горизонтальной СК, на объекте различают платформенные ИНС с гиростабилизированной платформой и бесплатформенные. Различие между ними заключается в том, что у платформенных ИНС есть карданов подвес - шарнирная опора, позволяющая закрепленному в ней телу вращаться в нескольких плоскостях одновременно; при этом закрепленное вращающееся тело сохраняет направление оси вращения независимо от ориентации подвеса. Из достоинств платформенных ИНС можно отметить ее высокую точность и отсутствие необходимости пересчета из связанной с корпусом датчика IMU (локальной) СК в необходимую (связанную) систему координат. Недостатком платформенных ИНС являются ограничение в измерении углов поворота, увеличенные массогабаритные показатели, усложнение конструкции, а также большая стоимость. В бесплатформенных ИНС отсутствует карданов подвес, измерительные датчики микромеханических гироскопов и акселерометров жестко связанны с подвижным объектом позиционирования. В настоящее время наибольшее распространение получили бесплатформенные ИНС, обладающие преимуществами неограниченных углов измере-
ний, высокой информативности, надежности и устойчивости к вибрационным и ударным воздействиям, а также уменьшенными массогабаритны-ми параметрами и энергопотреблением [23]. Недостатком бесплатформенных ИНС является рост погрешности даже для неподвижного объекта позиционирования. Далее рассмотрим особенности комплексирования инерциальных с радиотехническими измерениями (РТИ) для компенсации нарастающей погрешности ИНС.
3.2. Особенности комплексирования измерений ИНС
Повышение точности позиционирования с использованием бесплатформенных ИНС реализуется интеграцией автономных ИНС измерений с неавтономными ДМ/УМ измерениями, что позволяет компенсировать свойственные бесплатформенным ИНС накапливающиеся погрешности; ком-плексирование ДМ/УМ/ИНС измерений при этом осуществляется с использованием расширенного ФК. Достаточно хорошо исследованным направлением комплексирования является компенсация неточностей бесплатформенных ИНС за счет сигналов ГНСС, так как последние не имеют накапливающихся ошибок. Выделяют следующие схемы комплексирования бесплатформенных ИНС с ГНСС: а) раздельная; б) слабо связанная; в) жестко связанная; г) глубоко интегрированная [24]. Приведем особенности данных схем для уточнения рассматриваемого далее способа комплексирования РТИ/ИНС измерений [23, 24].
В раздельной схеме компенсация нарастающей со временем погрешности бесплатформенной ИНС осуществляется ее периодической инициализацией новыми оценками координат и скорости от ГНСС. Недостатком раздельной схемы для высокоподвижных объектов является редкая периодичность сигналов коррекции ГНСС.
В слабо связанной схеме бесплатформенная ИНС и спутниковая навигационная система (СНС) работают независимо. Для коррекции ОК, полученных от ГНСС, используется интегральный ФК, который, работая независимо, позволяет уточнять текущие оценки координат СНС при утрате сигналов ГНСС за счет измерений бесплатформенной ИНС, собранных в предыдущий момент. Недостаток слабо связанной схемы - взаимная корреляция ошибок ОК на выходе приемника СНС.
В жестко связанной схеме бесплатформенной ИНС осуществляет сбор первичных измерений поступательного и вращательного движений, которые вместе с первичными измерениями СНС поступают на фильтр Калмана для вторичной обработки. Данная схема позволяет повысить точность позиционирования по сравнению с раздельной и слабо связанной схемой, исключает взаимную корреляцию ОК, а также снимает проблему син-
хронизации работы бесплатформенной ИНС/СНС. Недостатком жестко связанной схемы является ее низкая надежность, так как при отсутствии первичных измерений ИНС в результате выхода из строя бесплатформенной ИНС система вторичной обработки ФК перестает работать.
В глубоко интегрированной схеме, в отличие от жестко связанной, обработка в ФК, помимо вторичных оценок координат и скорости подвижного объекта, осуществляет вычисление погрешностей бесплатформенной ИНС и первичных ДМ измерений СНС, что позволяет достичь максимальной точности. Недостатком глубоко интегрированной схемы является высокая размерность вектора состояния ФК, что увеличивает вычислительную нагрузку на процедуры вторичной обработки.
Таким образом, рассмотрев известные схемы комплексирования бесплатформенной ИНС с радиотехнической системой позиционирования ГНСС, можно отнести исследуемую далее схему комплексирования ДМ/УМ/ИНС к жестко связанной схеме, в которой ИНС и радиотехнические измерения ДМ/УМ обеспечивают вектор первичных измерений ъ (3) для вторичной обработки в РФК. Рисунок 2 иллюстрирует обобщенную схему ком-плексирования первичных измерений в РФК для случая оценки траектории ТС на плоскости в 2й.
Рис. 2. Обобщенная схема комплексирования ДМ/УМ/ИНС в РФК
Fig. 2. Generalized Scheme for Integrating TOA/AOA/INS in EKF
Для интеграции первичных инерциальных измерений ускорения и угловой скорости с первичными радиотехническими измерениями угла и времени прихода сигнала в единый вектор первичных измерений z (3) необходимо преобразование инерциальной системы координат акселерометра и гироскопа в связанную систему координат транспортного средства.
3.2. Инерциальная и связанная СК
В рассматриваемой далее бесплатформенной ИНС акселерометры и гироскопы жестко связаны с корпусом датчика IMU объекта, поэтому для комплексирования ИНС и ДМ/УМ измерений необходим пересчет из локальной инерциальной СК в связанную СК ТС. Связанная СК традиционно исполь-
зуется для анализа движения летательных аппаратов и может быть применима к прогнозу траектории ТС. Связанная СК в общем случае включает продольную 0х, поперечную 0y и вертикальную 0z оси, проходящие через центр масс объекта - микроэлектромеханические системы датчика или ТС (рисунок 3а).
Продольной 0х является ось транспортного средства (рисунок 3b); при вращении вокруг нее ТС совершает движение под углом крена (roll). Вертикальной 0z является ось, лежащая в плоскости симметрии ТС и перпендикулярная его продольной оси 0х; при вращении вокруг вертикальной оси ТС совершает движение под углом рыскания (yaw) - поворачивает влево или вправо. Поперечной 0y является ось, перпендикулярная плоскости симметрии ТС, направленная в сторону его правого крыла; при вращении вокруг поперечной оси ТС совершает движение под углом тангажа (pitch) - опускает и поднимает нос. Рисунок 3 иллюстрирует преобразование бесплатформенной инерциальной СК IMU в связанную СК подвижной платформы - транспортного средства VUE.
рыскание, Ж^^ yaw v
тангаж, К I......
pitch 9 ^ j
, крен,
roll ф ^^^^ ^ ^
А I ^^^ \
a b
Рис. 3. Преобразование из инерциальной СК IMU (a) в связанную СК VUE (b)
Fig. 3. Conversion from IMU Center-of-Mass System to VUE Frame of Reference
Рассмотрим особенности преобразования СК в специальном программном обеспечении (СПО) Matlab Sensor Fusion and Tracking Toolbox [30-32], которое позволяет отслеживать ориентацию, положение и траекторию позиционируемого подвижного объекта, называемого далее платформой.
Ориентация в Matlab Sensor Fusion and Tracking Toolbox определяется угловым смещением/перемещением и может быть описана в терминах поворота отдельной точки или всей СК. В первом
В СПО Matlab Sensor Fusion and Tracking Toolbox координаты обрабатываются массивами размерности RN*3, где число строк N - число выборок первичных измерений, взятых с интервалом At, а число столбцов 3 соответствует переменным СК (x, y, z).
случае СК статична - поворачивается точка. При повороте СК точка статична, поворачивается СК. При заданных осях и углах поворота угловое смещение точки эквивалентно угловому смещению СК с учетом противоположных направлений смещения. В СПО Matlab по умолчанию используется поворот СК: ориентация определяется поворотом исходной/начальной/родительской СК в целевую/ конечную/дочернюю СК. Выбор родительской СК зависит от практического приложения. Например, поворот СК независимых датчиков МЫ необходим для выравнивания их измерений в единой инер-циальной СК. Отслеживание СК платформы используется в задачах их стабилизации. Глобальная СК используется для одновременного сопровождения нескольких платформ.
Для обработки измерений, собранных в разных изначальных ориентациях, используют поворот их СК в терминологии связанной СК. Таблица 1 содержит соотношение осей и углов связанной СК.
ТАБЛИЦА 1. Соотношение осей и углов связанной СК
TABLE 1. Inter-Relation of Axes and Angles for Frame of Reference
Ось поворота Угол поворота (Эйлера) Обозначение Диапазон значений
z Рыскание/Yaw ф -180° < ф < 180°
y Тангаж/Pitch 8 -90° < 8 < 90°
x Крен/Roll Ф -180° < ф < 180°
Поворот по часовой стрелке вокруг оси в ее положительном направлении образует положительный угол вращения относительно данной оси. Поворот СК в пространстве осуществляется последовательным поворотом относительно каждой из осей СК. Так, например, для поворота родительской СК в последовательности ZYX следует выполнить ее поворот сначала вокруг вертикальной оси 0z на угол ф с использованием матрицы поворота (ф), затем вокруг поперечной оси 0у с использованием матрицы поворота (0) и в заключении вокруг продольной оси 0х с использованием матрицы поворота (ф); при этом после каждого поворота оси СК пересчитываются перед очередным поворотом.
Итоговая матрица поворота вектора из родительской в дочернюю СК для заданных углов поворота (рыскание ф, тангаж 0, крен ф) определяется выражением:
Получение вектора achild в дочерней СК из вектора aparent в родительской СК определяется формулой:
«child = (К(Ф- 0- Ф) X parent)7■ (16)
К(ф, 8, ф) = Rx (ф)Ку(8)Кг(ф) =
cos ф cos 8 sin ф cos 8 — sin 8 (15)
cos ф sin 8 sin ф — sin ф cos ф sin ф sin 8 sin ф + cos ф cos ф cos 8 sin ф
cos ф sin 8 cos ф + sin ф cos ф sin ф sin 8 sin ф + cos ф cos ф cos 8 cos ф.
Для исключения использования встроенных функций СПО Matlab Sensor Fusion and Tracking Toolbox и реализации процедур вторичной обработки через примитивы рассмотрим далее особенности использования кватернионов при повороте систем координат [33-37].
3.3. Использование кватернионов при повороте систем координат
Использование кватернионов представляется более удобным для ориентации и поворота СК в пространстве по сравнению с матрицей поворота на углы Эйлера. Кватернионы формализуют алгебраическое представления поворота в 3D и, как система гиперкомплексных чисел, образуют векторное пространство размерностью R4 над полем вещественных чисел и могут быть формализованы:
q = w + xi + у/ + zfc,
(17)
где w, x, y, z - вещественные числа; i, j, k - мнимые единицы со свойством:
¿2 = у2 = fc2 = ¿yfc = -1, (18)
при этом результат их попарного произведения зависит от порядка следования (не является коммутативным):
i; = fc, y¿ = -fc, (19)
мнимая часть x¿ + у/ + zfc кватерниона q ведет себя так же, как и вектор (х, у, z) 6 R3, а вещественная часть w ведет себя так же, как скаляр в R.
Определим пространство вращения: пусть точка (w, х, у, z) представляет вращение вокруг осей (х, у, z) на угол:
а =2 cos-1 w = 2 sin-1 ^х2 + у2 + z2, (20)
тогда кватернион (17) можно представить выражением:
q = w + xi + у/ + zfc = w + (x, y, z) = = cos a/2 + u sin a/2,
(21)
где и - единичный вектор.
Поворот вектора V вокруг оси и на угол а по часовой стрелке по направлению вектора и можно представить произведением:
Уа = qvq-1, (22)
Обратный кватернион ц-1 соответствует вращению вектора уав противоположном направлении:
Ч"1(ЧуЧ"1)Ч = V. (23)
Обратный кватернион определяется формулой: Ч"1 = чУ11ч112, (24)
где ||q|| - модуль кватерниона, определяемый как:
ичи = Тчч7 = + +у2 +z2- (25)
Для единичного кватерниона присуще: ||ц|| = 1.
Кватернион ц также можно определить как вещественную матрицу:
W -х -у
х w -г у
у г w -у . (26)
I г -у х w-
При матричном представлении сопряженному кватерниону соответствует транспонированная матрица ц* ^ QГ:
Q =
w
—X
—У —z
У z w
у — у
W
—z
z 1
-у
X
w.
(27)
Va
w —х —у —z- / -Vl- ■ W X У z -
X w —z У i —x w z —y
У z w —y 1 —y —z w X
z —у X w. V .—z У —y w.
Из (22, 24-26) для нормированного кватерниона ||ц|| =1 поворот вектора V = [у1,у2,у3,у4]г можно представить через кватернионы в матричном виде выражением:
(28)
Матричное представление (28) поворота вектора далее будет использовано в преобразовании из инерциальной СК IMU в связанную СК VUE при комплексировании ДМ/УМ/ИНС измерений в РФК.
4. МАТЕМАТИЧЕСКИЕ МОДЕЛИ
КОМПЛЕКСИРОВАНИЯ ДМ/УМ/ИНС В РФК
РФК позволяет оценить вектор состояния динамической системы (1), используя модель изменения состояния динамической системы Ffc и ряд измерений (3), косвенно описывающих состояние системы. Алгоритм работы РФК может быть описан в два этапа.
Этап 1. Получение априорной оценки вектора состояния x (5) и ковариационной матрицы ошибок оценки вектора состояния P (6) для шага k на основе данных с предыдущего шага k-1.
Этап 2. Обновление, на котором формируется апостериорная оценка вектора состояния x (10) и ковариационной матрицы ошибок P (11) с учетом новых измерений z (3).
Априорная оценка вектора состояния x формируется на основе измерений ИНС:
- измерения угловой скорости w по трем осям, полученные от гироскопа, позволяют получить ориентацию объекта q путем интегрирования этих измерений;
- измерения ускорений а по трем осям, полученные от акселерометра, позволяют получить скорость перемещения v и само перемещение объекта по трем осям путем двойного интегрирования.
Измерения гироскопа w и акселерометра а формируются в локальной инерциальной СК; предпо-
лагается, что начальная ориентация и местоположение ТС в глобальной СК известны с некоторой точностью. Вектор состояния включает в себя следующие величины:
х = [ч, р, уГ = о>2, ^з, У, , Уу]Т, (29]
где ц = д2, оз, д4]г - кватернион, описывающий ориентацию ТС; р = [х,у]г и V = [ух,уу]Г - векторы координат и скоростей ТС, соответственно.
Априорная оценка ориентации ТС формируется путем интегрирования измерений угловой скорости ш в форме кватернионов [26]:
- + At +
Ч- = Ч+-1 + у wfc®q+-i,
wfc = [0, w^ wy, wzf, Ч+-1 = toi, ?2, qif.
(30)
(31)
(32)
где и^ - кватернион, составленный из измерений гироскопа wx, wy, Wz по трем осям; Д£ - период измерений ИНС; ® - операция умножения кватернионов; здесь и далее верхние индексы «-» и «+» обозначают априорную и апостериорную оценки, соответственно.
Второе слагаемое выражения (30) является дифференциальным уравнением кватерниона, которое можно выразить через умножение кватернионов в матричном виде (для упрощения индексы не показаны) [26]:
(33)
Априорная оценка вектора ориентации (30) с учетом (31-33) примет вид:
+ — - д+м/у - м/г)
М , ,
+ у № ^ + q+wz - и/у)
1
q*c = 2 Wfc^qfc- 1=
0 -wx -Wy -Wz" roi
1 Wx 0 -Wy
2 Wy -wz 0 wx 4з+
Wy -wx 0
qïc =
+ At У
+ у (% At
- q+wz + q+wj
(34)
+ у + о+и/у - q+wx)
В процессе интегрирования амплитуда кватерниона ц может стать отличной от 1, поэтому для корректного выполнения последующей операции вращения необходимо выполнить операцию нормировки кватерниона (25):
llqll = Jqf + qf + qf + qf. (35)
Для интегрирования измерения акселерометра afc должны быть представлены в СК VUE - необхо-
димо кватернионом повернуть вектор ускорений = [0, аж, ау, а2 ] согласно выражению:
а£ = q-*®(q-®afc) = [0, af,a«,af] ,
(36)
где af - вектор ускорений в СК VUE; qfc - кватернион; q-* - комплексно-сопряженный кватернион.
Сначала выполняется умножение кватернионов (q-®afc) в матричном виде (28), результатом которого является вектор размерности 4x1:
q-®afc =
0 -«x -az- "ЧТ
«X 0 az - ay
ay -az 0 43
az S 0 q-
(37)
"42 ах~Чз ау —^4 аг д-аж + - ч-ау
+ д-ау-^-аж
Далее выполняется умножение q-*®(q-®afc), в котором сопряженной кватернион ц-* записывается в виде матрицы:
Я-*®(Я-®а^) =
-?4
q2
-q-
-q4 q-
q-
-q-q-
q-ax + q-az - q-ay q-ay-q-az + q-ax Lq-az + q-ay-q-ax
(38)
Результатом умножения в (38) является 4-эле-ментный вектор а^, (36) первый элемент которого не используется в дальнейших вычислениях, а элементы а^, ау, а^ определяются выражениями:
-^-(-Ч-ах-Ч-ау-Ч-аг) +
-^4 + ^4"аж),
= q,r(q,ray-q,i"az + q-ax) -(чТаг + q-a^-q-a^) --^-(-Ч-ах-Ч-ау-Ч-аг) + +q4-(q-ax + q-az - q4"ay), af = + q-ay-q-aj +
+4-(чГау-+ 44"ax) -+ 43"az - qi^O --^-(-^-ax-q-ay-^iaz).
(39)
(40)
(41)
Априорная оценка местоположения ТС определяется путем интегрирования оценки скорости V-:
Р+ + At р+ + Vj-At
Pfc = Р+-1 + v+At = где P+-1 = [Р+ Р+Г- v+ = [vx, V"]
(42)
Априорная оценка скорости движения ТС определяется путем интегрирования показаний акселерометра а£:
= V,
fc_i+ a^At=
< + af At ^ + a^At
(43)
pfc = Ffcpfc-i
FJ + GfcUG[ ;
(44)
где Р+-1 - апостериорная ковариационная матрица ошибок оценки вектора состояния в предыдущий момент времени к-1; - матрица частных производных уравнений модели априорной оценки (30, 42, 43) по элементам вектора состояния (29); и - диагональная матрица ошибок процесса, учитывающая шум измерений акселерометра и гироскопа; Т - операция транспонирования; -матрица частных производных уравнений модели априорной оценки (30, 42, 43) по элементам входного воздействия [м/ж, и/у, м/2, аж, ау, а2]. В общем случае матрицы Р и в зависят от значений а£, и^ и
в момент времени к, но далее для упрощения индексы не показаны.
Матрица частных производных уравнений модели априорной оценки (30, 42, 43) по элементам вектора состояния определяется выражением:
Г ôq- ôq- -
p ô4 P ôP P ôf
4X4 4X2 4X2
др- ôp- ôp-
F = p Г2Х4 ôv- p Lr2X4 P ôP 2X2 ôv-pôP 2X2 P ôf 2X2 ôv-P ôf 2X2
1 1 3qf dç- 0 0 0
dq- 1 1 dg- 0 0 0
ôqi
dg- 1 1 dq- 0 0 0
ôqi 94
3q£ 1 1 0 0 0
ôqi
0 0 0 0 1 0
0 0 0 0 0 1
0 0 0
ôqi
0 0
.ôqi ^2 ^3 0^4
0 0
0 0
00
00
At 0
0
At
(45)
кватернион априорной
где а£ = [аж,аж]г; у+_1 = [<, ; для случая на плоскости учитываются только координаты х и у.
Априорная оценка вектора состояния х- = [Ч-,Р-,у-] для момента времени к (5) построена на основе апостериорной оценки х+-1 = = [Ч+-1, Р+-1, у+-1] в предыдущий момент времени к-1.
Априорная ковариационная матрица ошибок оценки вектора состояния в момент времени к (6) вычисляется по формуле:
где Ч- = Ч2, оценки ориентации ТС (30)(30); р- = [р-, р-] -априорная оценка МП ТС (42); у^ = , Уу ]г -априорная оценка скорости ТС.
Далее рассмотрим составные подматрицы матрицы (45).
ЁЯ!
*4Х4 - матрица частных производных уравнений априорной оценки ориентации (30) по компонентам кватерниона ц, размерность К4*4 (далее слово размерность будем подразумевать):
З^- З^- З^-" 3^ 3^ З^з 3^
7 ôq _
At
У
At
У
At
—w7 L 2 z
ôqi
dg-
dq-
At 2
1
3q£
^2
At
3q£
0^4
At
(46)
1 — — W^ — — w,, — — w.
2
At
■w, -
2 At
2
-w.
At
- У
At
У ^
1
At
"У
At
-wr
w,.
-. ар
• 4Х2 матрица частных производных уравнений априорной оценки ориентации (30) по компонентам вектора координат р, К4*2:
да!
, ар
= 04
(47)
ач-
*4Х2 - матрица частных производных уравнений априорной оценки ориентации (30) по компонентам вектора скорости V, К4*2: ач-
F df = П *4X2 U4X2-
(48)
ар-р ач
'2Х4 _ матрица частных производных уравнений априорной оценки положения (42) по компонентам кватерниона ц, К2*4: ар-
р ач = п (49)
ар-
Р2х2 - матрица частных производных уравнений априорной оценки положения (42) по компонентам вектора координат р, К2*2:
"др" дх
др-ду.
др-F др =
0
0
= Î1 011-0 iJ
(50)
др-
^2x2 - матрица частных производных уравнений априорной оценки положения (42) по компонентам вектора скорости V, К2*2:
др-
др-р ду =
0
0
др ди
= 0 1 10 дС-Г
(51)
ду-дд
ду-рдЧ = Г2Х4
дУу- дУу- дУу- дУу-
.3^ ^2 ^3 3^
(52)
<^3
У \
-— = дг^^ау + 2^аж - 2^ а2),
У Л
-— = д£:(-2д2ау + 2^аж - 2^ аг),
У \
-— = Д£:(-292аж - 2^зау - 2^4^),
у Л
-— = д^^а^ - 2^1 ау - 2^^),
0^4
У \
-— = ДС(-2^аж + 2^4ау - 2^^)'
0^4
(53)
^ - матрица частных производных уравнений априорной оценки скорости (43) по компонентам вектора координат р, К2*2:
ЭУ-
р ар = п (54)
Эу-Р Э» 2Х2
ЗУ 0
ЗУ ЗУ.
У ^
= р 01' 1-0 1
(55)
Матрица частных производных уравнений модели априорной оценки (30, 42, 43) по элементам входного воздействия [м/ж, и/у, м/2, аж, ау, а2] определяется по выражению:
. 2Х4 матрица частных производных уравнений априорной оценки скорости (43) по компонентам кватерниона ц, К2*4:
Элементы матрицы (52) определяются выражениями:
д^ж у Л
-— = ДС(-2^1аж + 2^0^ - 2^0^),
^у у л
-— = Д£:(-2о4аж - 2^1 ау + 2^^),
у л
-— = д£(-2^2^ - - 2^4а2),
У \
-— = дс(2ч2% + 2^аж - 2^ аг),
у Л
-— = дс(-2^2% + 2^аж - 2^1 аг),
г эд-1
°4Х3 За °4Х3
эр- эр-
в = 2Х3 0У- ^ 2Х3 р За 2Х3 ЭУ-р За
"д9Г д9Т д9Т 0 0 0
Зм/у Зм/2
0 0 0
Зм/у Зм/2
0 0 0
Зм/у Зм/2
д9£ д9£ д9£ 0 0 0
Зм/у Зм/2
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 д^ж- д^ж- д^ж-
Заж Зау Зау
0 0 0 ЗУ- 3^- 3^-
Заж Зау За2.
(56)
Далее рассмотрим составные подматрицы матрицы С в (56).
эд-
С4ЭХ3 - матрица частных производных уравнений априорной оценки ориентации (30) по компонентам вектора угловой скорости ш, К4*3:
зч;
СЗш _
„ч
^2Х2 - матрица частных производных уравнений априорной оценки скорости (43) по компонентам вектора скорости V, К2*2:
дс
- У дс
У < дс
У ^ дс
- У *
"д9Т
Зм/Ж 5wz
5wж 5wz
5wж 5wz
^94"
.3wж Зwz.
дс дс
92 - У ^3 - У4
дс дс
1- У ^4 У ^3
(57)
дс
У дс
91
дс
У дс
- У ^
"92
-91
0Ч-
^4x3 - матрица частных производных уравнений априорной оценки ориентации (30) по компонентам вектора ускорения а, К4*3:
U =
221 G,8" = 04
(58)
0Р-
С/Х3 - матрица частных производных уравне-
где а
2
WX
= а
"aw 0 0 0 0 0 ■
0 aw 0 0 0 0
0 0 aw 0 0 0
0 0 0 a2 0 0
0 0 0 0 a2 0
. 0 0 0 0 0 a2-l
= 2 а2 = aS, - дисперсия
(64)
ний априорной оценки положения (42) по компо- го белого гауссового шума (АБГШ) измерений ги-
нентам вектора угловой скорости ш, 1
эр-
СЗш _ л
-,„0 — и2у3.
(59)
0Р-
С28х- - матрица частных производных уравнений априорной оценки положения (42) по компонентам вектора ускорения а, К2*3:
эр-С/хаз = «
2X3-
(60)
ЭУ-
С2Х3 - матрица частных производных уравнений априорной оценки скорости (43) по компонентам вектора угловой скорости ш, К2*3:
ЭУ-
С®^ — 02
2X3
2X3-
(61)
ЭУ-
С2Х3 - матрица частных производных уравнений априорной оценки скорости (43) по компонентам вектора ускорения а, размерности а, К2*3:
ЭУ-
G®" =
dvr ôvr ôvr
ôax ôay ôaz
,dax ôay ôaz.
(62)
Элементы матрицы (62) определяются выражениями:
дг?х ôa,.
= -At(qi + - - q2),
ôvy
— = -At(2qi^4 + 2^2Чз)-
ôa„
ÔVv
(63)
ôa
= -At(qi - + - q|),
y
ôaz 9a,
= -At(2qi^3 - 2^2Ч,з)-
= At(2qi^2 - 2^з^4)-
Диагональная матрица ошибок процесса и, учитывающая шум измерений акселерометра и гироскопа, формализует модель инструментальных погрешностей бесплатформенной ИНС и определяется выражением:
роскопа, а;
222 ^ — т — гт —
■"ау
дисперсия измере-
К = P-HrS-S = НРГН7 + R;
ний акселерометра.
На этапе обновления апостериорная оценка вектора состояния х+ и ковариационной матрицы ошибок Р+ формируется на основе х-, Р- и ДМ/УМ измерений, полученных в момент к. Этап обновления описывается уравнениями:
х+ — х- + Ку; (65)
у — [Ьгол; Ьлол ]; (66)
Р+ — (I - КН)Р,-; (67)
(68) (69)
где К - матрица коэффициентов усиления РФК; у -вектор обновления; 5 - ковариационная матрица ошибок вектора у; Н - матрица частных производных вектора измерений по составляющим вектора состояния; Я - ковариационная матрица ошибок измерений; I - единичная матрица.
Вектор обновления включает в себя два вектора Ь, которые описывают разность прямых измерений дальности и угла прихода с соответствующими величинами, полученными на основе априорной оценки х-+1.
Вектор обновления ДМ измерений
Ьгол — К - П)- ¿1 - 71,..., - г„_1]; (70)
где С/ - измеренная дальность от ТС до /-го ПП/; / = 0...п - 1, п - число ПП, измерения дальности от которых доступны в момент времени к; г/ - оценка дальности от ТС до /-го ПП на основе х-:
П — 11Р_ - ^II — 7(*-*г)2 + (У-Уг)2; (71)
где р_ — [х, у] - априорная ОК ТС; в; — [х;, уг ] - координаты ПП/.
Вектор обновления УМ измерений
Ьлол — [Фо - фo, Ф1 - ^ ., Фп-1 - Фп-1]; (72)
где Ф; - измеренный угол прихода сигнала от ПП/; / = 0...п - 1, п - число ПП, измерения угла прихода от которых доступны в момент времени к; Ф; -оценка угла прихода сигнала от ПП/ на основе х-:
Фф = z(pfc, Sj ) = arctan
(IZli)
\x - xj '
(73)
Матрица наблюдений Н - матрица частных производных векторов Г — [г0,71,..., ги-1] и ф —
= [ф0, ф1(..., срп-1] по элементам вектора состояний х:
Н =
Эг
04
ПХ4
Эф
04
ПХ4
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0000
Эг
н0р
ПХ2 Эф
н0р
ПХ2
дго Зх Зг^ Зх
дгп-1 Зх 50о Зх д01 Зх
Эг ийр ПХ2 Эф
Н
nХ2J
дго Зу Зт^ Зу
дгп-1 Зу 50о Зу 301 Зу
30„
30„
Зх
Зу
00 00
00 00 00
00
(74)
Далее рассмотрим составные подматрицы матрицы Н в (74).
Эг
Н0ЧХ4 - матрица частных производный уравнения дальности (71) по компонентам кватерниона
Я, К"*4:
Эг
н04 = п
ППХ4 _ ПХ4"
(75)
Н/Х2 - матрица частных производных уравнения дальности (71) по компонентам вектора координат р, К"*2 (76).
Эг
Н0Х2 - матрица частных производных уравнения дальности (71) по компонентам вектора скорости V, К"*2:
Эг
НЦ„ =
(77)
н0^4 - матрица частных производный уравнения угла прихода сигнала (73) по компонентам кватерниона я, К"*4:
Эф
н0 4 = п
П ПХ4 _ ""Х4-
(78)
Эр
Х2 матрица частных производный уравнения угла прихода сигнала (73) по компонентам вектора координат р, К"*2 (79).
Эф
Н 0Х2 - матрица частных производный уравнения угла прихода сигнала (73) по компонентам вектора скорости V, К"*2:
Эф
I Э V
ЦО V — л
П ПХ2 _ " "Х2-
(80)
Ковариационная матрица ошибок измерений Я равна:
Г< 0 0 0 0 0
0 < 0 0 0 0
0 0 0 'П-1 0 0
0 0 0 0 аФо 0
0 0 0 0 0 <
, 0 0 0 0 0 0
0 = < = а2 'п- -1 = = (^с)2-
0 0
0 0
0 0
0 0
0 0
0
фл—1
.(81)
ки измерения дальности; - СКО измерения вре-
мени прихода; с
22
скорость света; аф0 = аф =
= афп-1 = аФ - дисперсия ошибки измерения угла прихода.
Дале в разделе 5 рассмотрим реализацию представленной математической модели в имитационной модели (ИМ). Аналогично [9] примем шум первичных РТИ измерений ДМ/УМ АБГШ, а точность измерений зададим через СКО: для дальности, измеряемой через время прихода, СКО = 50 нс, для угла СКО ае = 2°.
Г дГо Зго 1 г х-хо У-Уо
Эг Н0р = ПХ2 Зх Зг^ Зу Зг^ 7(х-Хо)2 + (у-уо)2 7(х-хо)2 + (у-У-Уо Уо)2
Зх Зг„-1 Зу Зг„-1 7(х-Х1)2 + Су-У1)2 X ^п-1 7(х-хо)2 + (у-У - Уп-1 Уо)2 , (76)
. Зх ду . [V (х - х„-1)2 + (у - у„-1)2 7(х-х„-1)2 + (у- Уп-1)2-1
дфо дфо 1 г У-Уо х-хо
Эф н0р = ПХ2 Зх Зу (х - Хо)2 + О - Уо)2 У-У1 (х - Хо)2 + (у - X Х1 Уо)2
дх ду (х - Х1)2 + (у - У1)2 (х - Х1)2 + (у - У1 )2 ■ (79)
дфп-1 дфп- 1 У - Уп-1 X х^-1
. Зх Зу [. (х - х„-1)2 + (у - у„-1)2 (х - х„-1)2 + (у - Уп-1)2-
гироскопа g^ = 1,25 шума акселерометра чения меняющихся
5. ИМИТАЦИОННАЯ МОДЕЛЬ
КОМПЛЕКСИРОВАНИЯ ДМ/УМ/ИНС В РФК
В исследуемой ИМ шум первичных ИНС измерений, как и РТИ измерений, также принимается АБГШ: спектральная плотность мощности шума 10-4 рад/с/УГц, мощность g3 =2 • 10-4 м/с2//Тц. Зна-ошибок бесплатформенной ИНС можно считать постоянными [22]; примем смещение измерений гироскопа 3,5 • 10-4 рад/с, акселерометра 5 • 10-3 м/с2.
Рисунок 4 иллюстрирует работу РФК с комплек-сированием TOA/AOA/INS в сравнении с РФК на основе только измерений TOA/AOA: рисунок 4а иллюстрирует полную траекторию движения ТС в городе по модели сценария METIS Madrid Simulation Scenario, которая была разработана в [9]; рисунок 4b иллюстрирует оценку траектории ТС на отдельно взятом повороте.
Качественный анализ оценок траекторий (рисунок 4b) позволяет сделать вывод о том, что ком-плексирование РТИ-измерений с измерениями ИНС позволяет повысить точность позиционирования ТС на перекрестке. В рассматриваемом сценарии ИМ VUE движется со скоростью v « 5 м/с, интервал сбора ДМ/УМ измерений равен T = 0,1 с; интервал сбора ИНС измерений равен At = 0,01 с.
Рисунок 5 детализирует количественно точность ОК ТС для каждой точки траектории его движения; ось х показывает пройденное ТС расстояние; вертикальными штриховыми линиями показано примерное положение начала поворота в траектории движения ТС. Анализ точности ОК ТС для каждой точки траектории его движения (рисунок 5) показывает, что использование ИНС позволяет существенно сократить ошибку на участках траектории с изменением направления движения ТС.
Рисунок 6 показывает оценку траектории ТС на основе только измерений ИНС, т. е. при отсутствии этапа обновления на основе ДМ/УМ измерений.
Анализ точности ОК ТС только на основе измерений ИНС подтверждает накопление ошибки, так как для ИНС характерен дрейф измерений отдельных датчиков, что приводит к невозможности оценки координат ТС методом двойного интегрирования измерений на длительном интервале времени. Так, в алгоритме TOA/AOA/INS по измерениям ИНС строится оценка на относительно коротком интервале времени между этапами обновления на основе дальномерных/угломерных измерений, которые необходимы для периодической корректировки.
В ИМ предполагается, что измерения доступны в каждой точке траектории движения ТС. Ошибка оценки траектории ТС RMSE (аббр. От англ. Root Mean Square Error) вычисляется как среднее зна-
чение евклидова расстояния между истинными координатами и их оценкой по совокупности К точек измерений:
к
= 1 )2 + )2 < (82)
где [х^, у^] - к-я оценка координат ТС; [х, у] - истинные значения координат ТС; К - количество оценок координат.
у, м
0
I 1 i
300 х, м
у, м
20 10 0 -10 -20 -30
220 230
210 a AN сLOS ▲ ANcNLOS Истинная траектория
240 250 260 270 280 х, м Оценка траектории UN EKF TOA/AOA/INS Оценка траектории UN EKF TOA/AOA п Здания
о Эллипс ошибок оценки юординат UN b)
Рис. 4. Оценка траектории ТС: а) полная траектория; b) участок поворота
Fig. 4. Estimation of the VUE Trajectory: a) Full Trajectory; b) Intersection
EKF TOA/AOA/INS EKF,TOA/AOA
о
5 4
5
го" 3
о s
2 1
00 1 00 200 300 400 500 600
Пройденное расстояние по траектории движения UN Рис. 5. Точность ОК в каждой точке траектории движения ТС
Fig. 1. Location Estimation Accuracy at Each Point of the VUE Trajectory
У, 100
ДМ5Я
0 100 200 300 400 х, м
Рис. 6. Оценка траектории ТС только на основе измерений ИНС
Fig. 6. Estimation of the VUE Trajectory Based on INS Measurements Only
В ИМ предполагается, что измерения доступны в каждой точке траектории движения ТС. Ошибка оценки траектории ТС RMSE (Root Mean Square Error) вычисляется как среднее значение евклидова расстояния между истинными координатами и их оценкой по совокупности K точек измерений при движении ТС:
к
= 1 ^ VOio - хи)2 + (Уш - Уи)2-fc=1
(82)
где [xfc, yfc ] - k-я оценка координат ТС; [х, у] - истинные значения координат ТС; K - количество оценок координат.
При усреднении оценки траектории движения ТС в городе по модели METIS Madrid ошибка ОК составила порядка 0,7 м для ДМ/УМ/ИНС и 1,15 м для ДМ/УМ без ИНС; выигрыш от комплексирова-ния РТИ-измерений с ИНС особенно проявляется на поворотах при маневрировании ТС.
6. ВЫВОДЫ
Настоящая работа, являясь продолжением исследования [9], развивает методологию комплек-сирования радиотехнических дальномерных и угломерных измерений с измерениями инерциаль-ной навигационной системы с использованием расширенного фильтра Калмана. Разработанная математическая и имитационная модели, включая преобразование инерциальной системы координат акселерометра и гироскопа в связанную систему координат транспортного средства с использованием кватернионов, подтверждают возможность повышения точности позиционирования транспортного средства при изменении направления движения на перекрестках с точностью до одного метра.
м
ИСТОЧНИК ФИНАНСИРОВАНИЯ
Исследование выполнено в рамках исполнения Государственного контракта № П33-1-26/9.
Список используемых источников
1. Фокин Г.А. Технологии сетевого позиционирования: монография. СПб.: СПбГУТ, 2020. 558 с.
2. Фокин Г.А., Кучерявый А.Е. Сетевое позиционирование в экосистеме 5G // Электросвязь. 2020. № 9. С. 51-58. D01:10.34832/ELSV.2020.10.9.006
3. Киреев А.В., Фокин Г.А. Позиционирование объектов в сетях LTE посредством измерения времени прохождения сигналов // Труды учебных заведений связи. 2016. Том 2. № 1. С. 68-72.
4. Киреев А.В., Фокин Г.А. Пеленгация источников радиоизлучения LTE мобильным пунктом радиоконтроля с круговой антенной решеткой // Труды Научно-исследовательского института радио. 2015. № 2. С. 68-71.
5. Sivers M., Fokin G., Dmitriev P., Kireev A., Volgushev D., Al-Odhari A. Indoor Positioning in WiFi and NanoLOC Networks // Proceedings of the 16th International on Next Generation Wired/Wireless Networking (NEW2AN 2016) and 9th Conference on Conference on Internet of Things and Smart Spaces (ruSMART 2016), St. Petersburg, Russia, 26-28 September 2016. Lecture Notes in Computer Science. Cham: Springer, 2016. Vol. 9870. PP. 465-476. D0I:10.1007/978-3-319-46301-8_39
6. Sivers M., Fokin G., Dmitriev P., Kireev A., Volgushev D., Al-odhari A.H.A. Wi-Fi Based Indoor Positioning System Using In-ertial Measurements // Proceedings of the 17th International Conference on International Conference on Next Generation Wired/Wireless Networking (NEW2AN 2017), 10th Conference on Conference on Internet of Things and Smart Spaces (ruSMART 2017), Third Workshop on International Workshop on Nano-scale Computing and Communications (NsCC 2017), St. Petersburg, Russia, 28-30 August 2017. Lecture Notes in Computer Science. Cham: Springer, 2017. Vol. 10531. PP. 734-744. D0I:10.1007/978-3-319-67380-6_69
7. Киреев А.В., Фокин Г.А. Оценка точности локального позиционирования мобильных устройств с помощью радиокарт и инерциальной навигационной системы // Труды учебных заведений связи. 2017. Том 3. № 4. С. 54-62.
8. Духовницкий О.Г., Рагило М.А., Сиверс М.А., Фокин Г.А. Применение фильтра Калмана в задачах позиционирования // Электросвязь. 2016. № 1. С. 78-81.
9. Фокин Г.А., Владыко А.Г. Позиционирование транспортных средств в сверхплотных сетях радиодоступа V2X/5G с использованием расширенного фильтра Калмана // Труды учебных заведений связи. 2020. Т. 6. № 4. С. 45-59. D0I:10.31854/1813-324X-2020-6-4-45-59
10. Kalman R.E. A New Approach to Linear Filtering and Prediction Problems // Journal of Basic Engineering. 1960. Vol. 82. Iss. 1. PP. 35-45. D01:10.1115/1.3662552
11. Фомин В.Н. Рекуррентное оценивание и адаптивная фильтрация. М.: Наука, 1984. 286 с.
12. Балакришнан А.В. Теория фильтрации Калмана. М.: Мир, 1988. 168 с.
13. Эльясберг П.Е. Определение движения по результатам измерений. М.: Наука, 1976. 416 с.
14. Браммер К., Зиффлинг Г. Фильтр Калмана-Бьюси. Детерминированное наблюдение и стохастическая фильтрация. М.: Наука, 1982. 200 с.
15. Ройтенберг Я.Н. Автоматическое управление. М.: Наука, 1992. 576 с.
16. Кузовков Н.Т., Салычев О. С. Инерциальная навигация и оптимальная фильтрация. М.: Машиностроение, 1982. 216 с.
17. Андреев В.Д. Теория инерциальной навигации. Корректируемые системы. М.: Наука, 1967. 648 с.
18. Мак-Клур К.Л. Теория инерциальной навигации. М.: Наука, 1964. 300 с.
19. Ишлинский А.Ю. Ориентация, гироскопы и инерциальная навигация. М.: Наука, 1976. 670 с.
20. Горенштейн Н.А., Шульман И.А., Сафарян А.С. Инерциальная навигация. М.: Советское радио, 1962. 248 с.
21. Доннел О. Инерциальная навигация. Анализ и проектирование. М.: Наука, 1969. 592 с.
22. Васильев П.В., Мелешко А.В., Пятков В.В. Повышение точности корректируемой инерциальной навигационной системы // Известия высших учебных заведений. Приборостроение. 2014. Т. 57. № 12. С. 15-21.
23. Трефилов П.М. Сравнительный анализ улучшения точностных характеристик инерциальных навигационных систем // XIII Всероссийское совещание по проблемам управления ВСПУ-2019 (Москва, 17-20 июня 2019). М.: Институт проблем управления им. В.А. Трапезникова РАН, 2019. С. 470-474. D0I:10.25728/vspu.2019.0470
24. Munguia R. A GPS-aided Inertial Navigation System in Direct Configuration // Journal of Applied Research and Technology. 2014. Vol. 12. Iss. 4. PP. 803-814.
25. Lai Q., Yuan H., Wei D., Wang N., Li Z., Ji X. A Multi-Sensor Tight Fusion Method Designed for Vehicle Navigation // Sensors. 2020. Vol. 20. Iss. 9. P. 2551. D0I:10.3390/s20092551
26. Zhao F., van Wachem B.G.M. A novel Quaternion integration approach for describing the behaviour of non-spherical particles // Acta Mechanica. 2013. Vol. 224. PP. 3091-3109. D0I:10.1007/s00707-013-0914-2
27. Chen X., Song S., Xing J. A ToA/IMU indoor positioning system by extended Kalman filter, particle filter and MAP algorithms // Proceedings of the 27th Annual International Symposium on Personal, Indoor, and Mobile Radio Communications (PIMRC, Valencia, Spain, 4-8 September 2016). IEEE, 2016. D0I:10.1109/PIMRC.2016.7794980
28. Chen Xc., Chu S., Li F., Li F., Chu G. Hybrid ToA and IMU indoor localization system by various algorithms // Journal of Central South University. 2019. Vol. 26. PP. 2281-2294. D0I:10.1007/s11771-019-4173-9
29. Lategahn J., Müller M., Röhrig C. Extended Kalman filter for a low cost TDoA/IMU pedestrian localization system // Proceedings of the 11th Workshop on Positioning, Navigation and Communication (WPNC, Dresden, Germany, 12-13 March 2014). IEEE, 2014. D0I:10.1109/WPNC.2014.6843307
30. Sensor Fusion and Tracking Toolbox™ Getting Started Guide. MathWorks. 2018. URL: https://www.mathworks.com/ help/releases/R2020b/pdf_doc/fusion/fusion_gs.pdf (дата обращения 30.06.2021)
31. Estimate pose with nonholonomic constraints. MathWorks. 2021. URL: https://www.mathworks.com/help/fusion/ref/ insfilternonholonomic.html (дата обращения 30.06.2021)
32. 0rientation, Position, and Coordinate. MathWorks. 2021. URL: https://www.mathworks.com/help/fusion/gs/spatial-representation-coordinate-systems-and-conventions.html (дата обращения 30.06.2021)
33. Andrle M.S., Crassidis J.L. Geometric Integration of Quaternions // Journal of Guidance Control and Dynamics. 2013. Vol. 36. Iss. 6. PP. 1762-1767. D0I:10.2514/1.58558
34. Sola J. Quaternion kinematics for the error-state Kalman filter // Preprint arXiv:1711.02508. 2017.
35. Ханукаев Ю.И. Введение в теоретическую механику. М.: МФТИ, 2017. 240 с.
36. Ефремов А.П. Кватернионы: алгебра, геометрия и физические теории // Гиперкомплексные числа в геометрии и физике. 2004. Т. 1. № 1-1. С. 111-127.
37. Гордеев В.Н. Кватернионы и трехмерная геометрия. Киев: Сталь, 2012. 60 с.
* * *
Positioning of Vehicles with the Fusion of Time
of Arrival, Angle of Arrival and Inertial Measurements in the Extended Kalman Filter
G. Fokin1^, A. Vladyko1
JThe Bonch-Bruevich State University of Telecommunications, St. Petersburg, 193232, Russian Federation
Article info
DOI:10.31854/1813-324X-2021-7-2-51-67 Received 8 th May 2021 Accepted 31st May 2021
For citation: Fokin G., Vladyko A. Positioning of Vehicles with the Fusion of Time of Arrival, Angle of Arrival and Inertial Measurements in the Extended Kalman Filter. Proc. of Telecom. Universities. 2021;7(2):51-67. (in Russ.) D0I:10.31854/1813-324X-2021-7-2-51-67
Abstract: This work is devoted to the study of models and methods for improving posi-tioning accuracy in ultradense V2X/5G radio access networks for vehicles during maneuvers by combining range and angle primary measurements with measure-ments of inertial navigation systems in the extended Kalman filter. Onboard plat-formless inertial navigation system is represented by three-axis accelerometer and gyroscope modules. Integration of primary inertial measurements of acceleration and angular velocity with primary radio measurements of range and angle is carried out by converting the inertial coordinate system of the accelerometer and gy-roscope into coordinate system of vehicle using quaternions. Secondary pro-cessing of inertial and radio measurements is carried out in the extended Kalman filter. The integration results show an increase in the accuracy of estimating the trajectory of a vehicle from several meters to one meter when turning at an inter-section.
Keywords: positioning, ultra-dense 5G networks, vehicle to everything, range & angle measurements, inertial navigation system, extended Kalman filter, quaternions.
FUNDING
This research was performed as part of the State of the contract number P33-1-26 / 9. References
1. Fokin G.A. Technologies of Network Positioning. St. Petersburg: The Bonch-Bruevich State University of Telecommunications Publ.; 2020. 558 p. (in Russ.)
2. Fokin G.A., Koucheryavy A.Y. Network Positioning in 5G Ecosystem. Electrosvyaz. 2020;9:51-58. (in Russ.) D01:10.34832/ ELSV.2020.10.9.006
3. Kireev A., Fokin G. Positioning of Objects in LTE Networks by Measuring Signal Time of Arrival. Proc. of Telecom. Universities. 2016;2(1):68-72. (in Russ.)
4. Kireev A., Fokin G. Radio Direction-Finding of LTE Emissions Using Mobile Spectrum Monitoring Station with Circular Antenna Array. Trudy NIIR. 2015;2:68-71. (in Russ.)
5. Sivers M., Fokin G., Dmitriev P., Kireev A., Volgushev D., Al-Odhari A. Indoor Positioning in WiFi and NanoLOC Networks. Proceedings of the 16th International on Next Generation Wired/Wireless Networking, NEW2AN 2016, and 9th Conference on Conference on Internet of Things and Smart Spaces, ruSMART 2016, St Petersburg, Russia, 26-28 September 2016. Lecture Notes in Computer Science. Cham: Springer; 2016. vol.9870. p.465-476. D0I:10.1007/978-3-319-46301-8_39
6. Sivers M., Fokin G., Dmitriev P., Kireev A., Volgushev D., Al-odhari A.H.A. Wi-Fi Based Indoor Positioning System Using Inertial Measurements. Proceedings of the 17th International Conference on International Conference on Next Generation Wired/Wireless Networking (NEW2AN 2017), 10th Conference on Conference on Internet of Things and Smart Spaces (ruSMART 2017), Third Workshop on International Workshop on Nano-scale Computing and Communications (NsCC 2017), 28-30 August 2017, St Petersburg, Russia. Lecture Notes in Computer Science. Cham: Springer; 2017. vol.10531. p.734-744. DOI:10.1007/978-3-319-67380-6_69
7. Kireev A.V., Fokin G.A. Accuracy Evaluation of Local Positioning by Radiomap Building and Inertial Navigation Systems. Proc. of Telecom. Univercities. 2017;3(4):54-62. (in Russ.)
8. Dukhovnitskiy O.G., Ragilo M.A., Sivers M.A., Fokin G.A. Kalman Filtering for Positioning Applications. Electrosvyaz. 2016;1:78-81. (in Russ.)
9. Fokin G., Vladyko A. The Vehicles Positioning in Ultra-Dense 5G/V2X Radio Access Networks Using the Extended Kalman Filter. Proc. of Telecom. Universities. 2020;6(4):45-59. (in Russ.) D0I:10.31854/1813-324X-2020-6-4-45-59
10. Kalman R.E. A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering. 1960;82(1): 35-45. D0I:10.1115/1.3662552
11. Fomin V.N. Recurrent Estimation and Adaptive Filtering. Moscow: Nauka Publ.; 1984. (in Russ.)
12. Balakrishnan A.V. Kalman Filtration Theory. Moscow: Mir Publ.; 1988. (in Russ.)
13. Elyasberg P.E. Determination of Motion Based on Measurement Results. Moscow: Nauka Publ.; 1976. (in Russ.)
14. Brammer K., Ziffling G. Kalman-Bucy Filter. Deterministic Observation and Stochastic Filtering. Moscow: Nauka Publ.; 1982. (in Russ.)
15. Roytenberg Y.N. Automatic Control. Moscow: Nauka Publ.; 1992. (in Russ.)
16. Kuzovkov N.T., Salychev O.S. Inertial Navigation and Optimal Filtration. Moscow: Mashinostroenie Publ.; 1982. (in Russ.)
17. Andreev V.D. The Theory of Inertial Navigation. Corrected Systems. Moscow: Nauka Publ.; 1967. (in Russ.)
18. McClure K.L. The Theory of Inertial Navigation. Moscow: Nauka Publ.; 1964. (in Russ.)
19. Ishlinsky A.Y. Orientation, Gyroscopes and Inertial Navigation. Moscow: Nauka Publ.; 1976. (in Russ.)
20. Gorenstein N.A., Shulman I.A., Safaryan A.S. Inertial Navigation. Moscow: Sovetskoe radio Publ.; 1962. (in Russ.)
21. Donnell O. Inertial Navigation. Analysis and Design. Moscow: Nauka Publ.; 1969. (in Russ.)
22. Vasiliev P.V., Meleshko A.V., Pyatkov V.V. Accuracy Improvement of Correctable Inertial Navigation System. Journal of Instrument Engineering. 2014;57(12):15-21. (in Russ.)
23. Trefilov P.M. Comparative Analysis of Improving the Accuracy Characteristics of Inertial Navigation Systems. Proceedings of the XIIIth All-Russian Meeting on Control Problems, 17-20 June 2019, Moscow, Russia. Moscow: V.A. Trapeznikov Institute of Control Sciences RAS Publ.; 2019. p.470-474. (in Russ.) DOI:10.25728/vspu.2019.0470
24. Munguia R. A GPS-aided Inertial Navigation System in Direct Configuration. Journal of Applied Research and Technology. 2014;12(4):803-814.
25. Lai Q., Yuan H., Wei D., Wang N., Li Z., Ji X. A Multi-Sensor Tight Fusion Method Designed for Vehicle Navigation. Sensors. 2020;20(9):2551. DOI:10.3390/s20092551
26. Zhao F., van Wachem B.G.M. A novel Quaternion integration approach for describing the behaviour of non-spherical particles. Acta Mechanica. 2013;224:3091-3109. DOI:10.1007/s00707-013-0914-2
27. Chen X., Song S., Xing J. A ToA/IMU indoor positioning system by extended Kalman filter, particle filter and MAP algorithms. Proceedings of the 27th Annual International Symposium on Personal, Indoor, and Mobile Radio Communications, PIMRC, 4-8 September 2016, Valencia, Spain. IEEE; 2016. DOI:10.1109/PIMRC.2016.7794980
28. Chen Xc., Chu S., Li F., Li F., Chu G. Hybrid ToA and IMU indoor localization system by various algorithms. Journal of Central South University. 2019;26:2281-2294. DOI:10.1007/s11771-019-4173-9
29. Lategahn J., Müller M., Röhrig C. Extended Kalman filter for a low cost TDoA/IMU pedestrian localization system. Proceedings of the 11th Workshop on Positioning, Navigation and Communication, WPNC, 12-13 March 2014, Dresden, Germany. IEEE; 2014. D01:10.1109/WPNC.2014.6843307
30. MathWorks. Sensor Fusion and Tracking Toolbox™ Getting Started Guide. 2018. Available from: https://www.math works.com/help/releases/R2020b/pdf_doc/fusion/fusion_gs.pdf [Accessed 15th June 2021]
31. MathWorks. Estimate pose with nonholonomic constraints. 2021. URL: https://www.mathworks.com/help/fusion/ref/ insfilternonholonomic.html [Accessed 15th June 2021]
32. MathWorks. Orientation, Position, and Coordinate. 2021. URL: https://www.mathworks.com/help/fusion/gs/spatial-representation-coordinate-systems-and-conventions.html [Accessed 15th June 2021]
33. Andrle M.S., Crassidis J.L. Geometric Integration of Quaternions. Journal of Guidance Control and Dynamics. 2013;36(6):1762-1767. D0I:10.2514/1.58558
34. Sola J. Quaternion kinematics for the error-state Kalman filter. Preprint arXiv:1711.02508. 2017.
35. Khanukaev Y.I. Introduction to Theoretical Mechanics. Moscow: Moscow Institute of Physics and Technology Publ.; 2017. (in Russ.)
36. Efremov A.P. Quaternions: Algebra, Geometry and Physical Theories. Hypercomplex Numbers in Geometry and Physics. 2004;1(1-1): 111-127. (in Russ.)
37. Gordeev V.N. Quaternions and 3D Geometry. Kiev: Stal' Publ.; 2012. (in Russ.)
ФОКИН Григорий Алексеевич
Сведения об авторах:
кандидат технических наук, доцент, доцент кафедры радиосвязи и вещания Санкт-Петербургского государственного университета телекоммуникаций им. проф. М.А. Бонч-Бруевича, grihafokin@gmail.com © https://orcid.org/0000-0002-5358-1895
ВЛАДЫКО Андрей Геннадьевич
кандидат технических наук, директор НИИ «Технологии связи» Санкт-Петербургского государственного университета телекоммуникаций им. проф. М.А. Бонч-Бруевича, vladyko@sut.ru © https://orcid.org/0000-0002-8852-5607
