Способ оценки пространственного и углового положения объекта Российский патент 2024 года по МПК G01C21/00 

Изобретение относится к области радиотехнических систем и может быть использовано, например, в системах определения координат и углового положения подвижных объектов с использованием комплексного способа навигации, объединяющего инерциальный и спутниковый способы.

Известны способы оценки пространственного и углового положения объектов, основанные на применении навигационной аппаратуры потребителя глобальной навигационной спутниковой системы, инерциальных МЭМС-датчиков и фильтра Калмана (1 - Lai, Ying-Chin, and Jan, Shau-Shiun, Attitude estimation based on fusion of gyroscopes and single antenna GPS for small UAVs under the influence of vibration, GPS solutions, 2011, vol. 15, no. 1, pp. 67-77.), (2 - Wu, Yuanxin, et al., Strapdown inertial navigation system algorithms based on dual quaternions, IEEE Transactions on Aerospace and Electronic Systems, 2005, vol. 41, no. 1, pp. 110-132), (3 - Рехман А., Шахид X., Афзал M.A., Бхатти X.M.A. Пешеходная навигация на основе комплексирования ГНСС-приемника и МЭМС-датчиков смартфона с помощью расширенного фильтра Калмана // Гироскопия и навигация. - 2020. - Т. 28. - №2 - С. 91-108.). Эти способы позволяют определить оценки пространственного и углового положения объектов, основанные на применении навигационной аппаратуры потребителя глобальной навигационной спутниковой системы и инерциальных МЭМС-датчиков.

Недостатками этих способов является необходимость точного знания моделей и оценок случайных погрешностей используемых датчиков, в значительной степени влияющих на точность оценок пространственного и углового положения объектов.

Известен способ инерциально-спутникового позиционирования подвижных объектов (4 - Патент РФ №2595328, МПК: (2006.01) G01C 21/00. Способ инерциально-спутникового позиционирования подвижных объектов, Закрытое акционерное общество «МЕРА». Иванов Ю.М., Филимонов А.П. Публ. - 27.08.2016 г.). Способ заключается в совместной обработке входных данных о положении подвижных объектов, формируемых независимо инерциальными датчиками, вырабатывающими векторы угловой скорости ускорения подвижных объектов, барометрическим высотомером и спутниковым приемником глобальной навигационной спутниковой системы с известным альманахом навигационных спутников и составом рабочего созвездия, с учетом информации о положении и ориентации подвижных объектов, при этом между орбитами навигационных спутников и подвижными объектами на высоте 35-60 км от поверхности Земли размещают аэростатную подвеску, радио- и оптически связанную с контрольно-корректирующей станцией, являющейся геодезически точно привязанной к принятой системе координат базовой станцией и формирующей локальную дифференциальную навигационную систему с зоной обзора радиусом примерно 50-200 км, и использующей радиооптические каналы синхронизации генераторов, работающих на двух близких частотах, а также в области длин волн света 0,4-0,6 мкм, и осуществляющих временную (фазовую) синхронизацию разнесенных синтезаторов передатчиков аэростатной подвески, а также передающих информацию подвижных объектов о метеопараметрах и своих координатах перемещения.

Реализация способа позволяет повысить точность, доступность и непрерывность навигационного обеспечения подвижных объектов.

Недостатком способа является высокая техническая сложность и зависимость от дополнительного оборудования - аэростатной подвески, зона действия которой ограничена. Кроме того, способ не позволяет учесть потенциальный положительный эффект предоставляемый возможностью формирования оценки пространственного и углового положения объекта в отложенном времени.

Известен способ ориентации подвижных объектов (5 - Волков В.Л., Жидкова Н.В. Обработка информации в системе ориентации на основе МЭМС // Труды Нижегородского государственного технического университета им. Р.Е. Алексеева. - 2015 - №3(110).- С. 279-286.). Способ заключается в определении углов ориентации на основе кинематических уравнений Пуассона для параметров Родрига-Гамельтона, при этом корректирующие углы курса, крена и тангажа вычисляются по информации трехосевого магнитометра и трехосевого акселерометра. Для компенсации ошибок применяется схема на основе пропорционально-интегральных регуляторов, позволяющая определять и компенсировать ошибки дрейфа трехосевого датчика угловой скорости.

Недостатком способа является ограничение коррекции углов курса, тангажа и крена только во время равномерного прямолинейного движения подвижного объекта, а также неопределенность в выборе значений коэффициентов пропорциональности пропорционально-интегральных регуляторов.

Известен способ автономной ориентации подвижного объекта с помощью инерциального измерительного модуля (6 - Патент РФ №2738342, МПК: (2006.01) G01M 7/00. Способ автономной ориентации подвижного объекта с помощью инерциального измерительного модуля, Федеральное государственное бюджетное образовательное учреждение высшего образования «Саратовский государственный технический университет имени Гагарина Ю.А.». Алешкин В.В., Здражевский Р.А., Голованов П.Н., Марусич В.О. Публ. - 11.12.2020 г.). Способ заключается в измерении проекций векторов напряженности магнитного поля трехосным блоком магнитометров, кажущегося ускорения объекта трехосным блоком акселерометров и абсолютной угловой скорости вращения объекта трехосным блоком гироскопов, входящих в инерциальный измерительный модуль, вычислении корректирующих сигналов позиционной коррекции и решении кинематических уравнений для вычисления оценок углов ориентации. При этом к корректирующим сигналам позиционной коррекции добавляют перекрестные сигналы коррекции в каждом канале, коэффициенты которых задаются исходя из необходимых динамических свойств системы, а собственные коэффициенты коррекции в кинематических уравнениях не равны коэффициентам перекрестной коррекции, один из перекрестных коэффициентов коррекции имеет противоположный по отношению к остальным знак.

Способ позволяет повысить точность решения задачи автономной ориентации подвижных объектов за счет асимптотической устойчивости системы по всем трем углам ориентации, возможности настройки системы на период Шулера для компенсации баллистических погрешностей без введения интегральной коррекции, возможности настройки системы как фильтр нижних частот по всем входам и выходам.

Недостатком способа является неопределенность в выборе значений собственных коэффициентов коррекции в кинематических уравнениях и перекрестных коэффициентов коррекции.

Известен способ оценки пространственного и углового положения объекта (7 - Аль-Мансур М., Шуаиб И., Джафар А., Потапов А.А. Аналитический алгоритм оценки пространственного положения и курса объекта // Гироскопия н навигация. - 2019. - Т. 27. - №1- С. 72-92.), принятый за прототип, который включает:

- последовательное получение по сигналам датчиков - трехосевого магнитометра, трехосевого акселерометра, трехосевого датчика угловой скорости, навигационной аппаратуры потребителя глобальной навигационной спутниковой системы - выборок данных;

- оценку условия возможности формирования оценки пространственного и углового положения объекта в одном из вариантов;

- если условие возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, не выполняется, то для каждой новой выборки данных датчиков вычисляют кинематические углы Эйлера на основании выходных значений трехосевого датчика угловой скорости, вычисляют углы наклона (крена и тангажа) по выходным данным трехосевого акселерометра, трехосевого магнитометра и расчетным значениям абсолютной скорости, полученной по сигналам навигационной аппаратуры потребителя глобальной навигационной спутниковой системы, вычисляют угол курса на основе сигналов трехосевого магнитометра, полученные измеренные и вычисленные величины комплексируют и формируют оценки пространственного и углового положения объекта;

- если условие возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, выполняется, то для каждой новой выборки данных датчиков на основе этих сведений формируют вектора состояния и измерений, для них определяют уравнение формирующего фильтра и уравнение наблюдения, по ним определяют выражения для математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания, задают начальные значения математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания, последовательно повторяют шаги прогноза по времени и шаги коррекции по сигналам датчиков;

- сохранение результатов формирования оценок

пространственного и углового положения объекта в реальном времени.

Способ обеспечивает формирование оценки пространственного и углового положения объекта при различных вариантах результатов проверки условия наличия и достоверности сведений о модели и случайных погрешностях используемых датчиков.

Недостатком способа-прототипа является низкая точность как в случае наличия и достоверности сведений о модели и случайных погрешностях используемых датчиков, так и в случае наличия и достоверности сведений о модели и случайных погрешностях используемых датчиков, но при влиянии местных условий на показания магнитометра.

Кроме того в практике встречаются задачи, когда оценка пространственного и углового положения объекта не требуется в процессе перемещения объекта по маршруту, а необходима по его окончанию. К таким задачам можно отнести, например, «геопривязку» зарегистрированных в процессе движения измерений, сформированных в связанной с объектом системе координат. В этом случае учитывая особенности инерциальных МЭМС-модулей и большие погрешности алгоритмов, основанных на применении фильтра Калмана на участках маневра, актуальным является выявление и использование некоторых особых участков пространственно-временной траектории объекта. К таким участкам можно отнести участки покоя (УП) или состояния «Покой», на которых пространственное положение объекта не изменяется, а также следующие за ними участки прямолинейного движения (УПД) или состояния «Прямолинейное движение», которые как участки имеют длину по величине значительно превышающую погрешности определения координат датчиками глобальной навигационной спутниковой системы.

Технической проблемой, на решение которой направлено предлагаемое изобретение, является повышение точности оценки пространственного и углового положения объекта.

Для решения указанной технической проблемы предлагается способ оценки пространственного и углового положения объекта, для реализации которого по сигналам датчиков - трехосевого магнитометра, трехосевого акселерометра, трехосевого датчика угловой скорости, навигационной аппаратуры потребителя глобальной навигационной спутниковой системы последовательно получают выборки данных, оценивают условия возможности формирования оценки пространственного и углового положения объекта в одном из вариантов,

если условие возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, не выполняется, то для каждой новой выборки данных датчиков

вычисляют кинематические углы Эйлера на основании выходных значений трехосевого датчика угловой скорости, вычисляют углы наклона (крена и тангажа) по выходным данным трехосевого акселерометра, трехосевого магнитометра и расчетным значениям абсолютной скорости, полученной по сигналам навигационной аппаратуры потребителя глобальной навигационной спутниковой системы,

вычисляют угол курса на основе сигналов трехосевого магнитометра,

полученные измеренные и вычисленные величины комплексируют и формируют оценки пространственного и углового положения объекта,

если условие возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, выполняется, то для каждой новой выборки данных датчиков

на основе этих сведений формируют вектора состояния и измерений, для них определяют уравнение формирующего фильтра и уравнение наблюдения, по ним определяют выражения для математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания,

задают начальные значения математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания,

последовательно повторяют шаги прогноза по времени и шаги коррекции по сигналам датчиков,

по получаемым значениям вектора состояния формируют оценки пространственного и углового положения объекта,

сохраняют результаты формирования оценок пространственного и углового положения объекта в реальном времени.

Согласно изобретению, после оценки условий возможности формирования оценки пространственного и углового положения объекта в одном из вариантов

если условие возможности формирования оценки пространственного и углового положения объекта в отложенном времени соблюдается, то задают интервал времени задержки и на этом интервале накапливают выборки данных,

после накопления выборок данных на интервале времени задержки по сигналам датчиков - выборкам данных выявляют особые элементы траекторий - состояния «Покой» и «Прямолинейное движение»,

по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Покой» определяют значения оценок углов тангажа и крена на этих участках траектории,

по выборкам данных - сигналам трехосевого магнитометра для каждого состояния «Покой» определяют значения предварительных оценок углов курса на этих участках траектории,

по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в связанной системе координат,

по выборкам данных навигационной аппаратуры потребителя глобальной навигационной спутниковой системы для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в нормальной системе координат,

из уравнений связи векторов в связанной и нормальной системах координат определяют углы курса на каждом участке траектории «Прямолинейное движение»,

по найденным углам курса на каждом участке траектории «Прямолинейное движение» и по предварительным оценкам углов курса на предстоящих им участках траектории «Покой» определяют поправки по углам курса и далее используют их как калибровки магнитометра по магнитному склонению и влиянию внешней среды,

по выборкам данных для элементов траекторий, не относящихся к состояниям «Покой» и «Прямолинейное движение», определяют оценки пространственного и углового положения объекта с учетом краевых условий по углам курса, тангажа и крена, а также калибровкам магнитометра по магнитному склонению и влиянию внешней среды, полученных для элементов траекторий, относящихся к состояниям «Покой» и «Прямолинейное движение»,

по результатам формирования оценок пространственного и углового положения объекта в отложенном времени выполняют проверку правильности полученных оценок в реальном времени.

Техническим результатом является повышение качества оценки пространственного и углового положения объекта.

Технический результат достигают за счет введения следующих новых операций:

если условие возможности формирования оценки пространственного и углового положения объекта в отложенном времени соблюдается, то задают интервал времени задержки и на этом интервале накапливают выборки данных,

после накопления выборок данных на интервале времени задержки по сигналам датчиков - выборкам данных выявляют особые элементы траекторий - состояния «Покой» и «Прямолинейное движение»,

по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Покой» определяют значения оценок углов тангажа и крена на этих участках траектории,

по выборкам данных - сигналам трехосевого магнитометра для каждого состояния «Покой» определяют значения предварительных оценок углов курса на этих участках траектории,

по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в связанной системе координат,

по выборкам данных навигационной аппаратуры потребителя глобальной навигационной спутниковой системы для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в нормальной системе координат,

из уравнений связи векторов в связанной и нормальной системах координат определяют углы курса на каждом участке траектории «Прямолинейное движение»,

по найденным углам курса на каждом участке траектории «Прямолинейное движение» и по предварительным оценкам углов курса на предстоящих им участках траектории «Покой» определяют поправки по углам курса и далее используют их как калибровки магнитометра по магнитному склонению и влиянию внешней среды,

по выборкам данных для элементов траекторий, не относящихся к состояниям «Покой» и «Прямолинейное движение», определяют оценки пространственного и углового положения объекта с учетом краевых условий по углам курса, тангажа и крена, а также калибровкам магнитометра по магнитному склонению и влиянию внешней среды, полученных для элементов траекторий, относящихся к состояниям «Покой» и «Прямолинейное движение»,

по результатам формирования оценок пространственного и углового положения объекта в отложенном времени выполняют проверку правильности полученных оценок в реальном времени.

На фигуре приведена структурная схема устройства, реализующего предлагаемый способ оценки пространственного и углового положения объекта.

Сочетание отличительных признаков и свойств предлагаемого способа из литературы не известны, поэтому он соответствует критериям новизны и изобретательского уровня.

При описании способа используются следующие системы координат и их обозначения: связанная систем координат (ССК) O^CCKX^CCKY^CCKZ^CCK, нормальной топоцентрической системе координат (НСК) O^HCKX^HCKY^HCKZ^HCK.

Взаимное расположение систем координат определяется углами Эйлера - курса ψ, тангажа θ и крена ϕ.

Для левых ССК и НСК представление вектора из НСК в ССК через последовательные матрицы поворотов с учетом промежуточных осей O^CCKX*^ССК, O^CCKY*^ССК и O^CCKZ*^ССК принимает вид

где С_ψ=cosψ, S_ψ=sinψ, C_θ=cosθ, S_θ=sinθ, С_ϕ=cosϕ, S_ϕ=sinϕ, а через матрицу поворотов

Для правых ССК и НСК соответствующие выражения принимают вид

при этом полагается, что ось O^HCKZ^HCK направлена к центру Земли, т.е. НСК является правой системой координат. В этом случае выражение (5) совпадает с выражением, полученным в описании способа-прототипа [7].

Способ оценки пространственного и углового положения объекта реализуется следующим образом:

1 По сигналам датчиков - трехосевого магнитометра, трехосевого акселерометра, трехосевого датчика угловой скорости, навигационной аппаратуры потребителя глобальной навигационной спутниковой системы последовательно получают выборки данных.

2 Оценивают условия возможности формирования оценки пространственного и углового положения объекта в одном из вариантов.

3 Если условие возможности формирования оценки пространственного и углового положения объекта в отложенном времени соблюдается, то задают интервал времени задержки и на этом интервале накапливают выборки данных.

4 Если условие возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, не выполняется, то для каждой новой выборки данных датчиков

4.1 вычисляют кинематические углы Эйлера на основании выходных значений трехосевого датчика угловой скорости,

4.2 вычисляют углы наклона (крена и тангажа) по выходным данным трехосевого акселерометра, трехосевого магнитометра и расчетным значениям абсолютной скорости, полученной по сигналам навигационной аппаратуры потребителя глобальной навигационной спутниковой системы,

4.3 вычисляют угол курса на основе сигналов трехосевого магнитометра,

4.4 полученные измеренные и вычисленные величины комплексируют и формируют оценки пространственного и углового положения объекта.

5. Если условие возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, выполняется, то для каждой новой выборки данных датчиков

5.1 на основе этих сведений формируют вектора состояния и измерений, для них определяют уравнение формирующего фильтра и уравнение наблюдения, по ним определяют выражения для математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания,

5.2 задают начальные значения математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания,

5.3 последовательно повторяют шаги прогноза по времени и шаги коррекции по сигналам датчиков,

5.4 по получаемым значениям вектора состояния формируют оценки пространственного и углового положения объекта.

6. Сохраняют результаты формирования оценок пространственного и углового положения объекта в реальном времени.

7.1 После накопления выборок данных на интервале времени задержки по сигналам датчиков - выборкам данных выявляют особые элементы траекторий - состояния «Покой» и «Прямолинейное движение»,

7.2 по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Покой» определяют значения оценок углов тангажа и крена на этих участках траектории,

7.3 по выборкам данных - сигналам трехосевого магнитометра для каждого состояния «Покой» определяют значения предварительных оценок углов курса на этих участках траектории,

7.4 по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в связанной системе координат,

7.5 по выборкам данных навигационной аппаратуры потребителя глобальной навигационной спутниковой системы для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в нормальной системе координат,

7.6 из уравнений связи векторов в связанной и нормальной системах координат определяют углы курса на каждом участке траектории «Прямолинейное движение»,

7.7 по найденным углам курса на каждом участке траектории «Прямолинейное движение» и по предварительным оценкам углов курса на предстоящих им участках траектории «Покой» определяют поправки по углам курса и далее используют их как калибровки магнитометра по магнитному склонению и влиянию внешней среды,

7.8 по выборкам данных для элементов траекторий, не относящихся к состояниям «Покой» и «Прямолинейное движение», определяют оценки пространственного и углового положения объекта с учетом краевых условий по углам курса, тангажа и крена, а также калибровкам магнитометра по магнитному склонению и влиянию внешней среды, полученных для элементов траекторий, относящихся к состояниям «Покой» и «Прямолинейное движение».

8. По результатам формирования оценок пространственного и углового положения объекта в отложенном времени выполняют проверку правильности полученных оценок в реальном времени.

Для реализации пункта 1 по сигналам датчиков - трехосевого магнитометра, трехосевого акселерометра, трехосевого датчика угловой скорости (ДУС), навигационной аппаратуры потребителя глобальной навигационной спутниковой системы (ГНСС) последовательно получают выборки данных. Для этого формируют следующие вектора по сигналам трехосевых:

- магнитометра

где B_x|_t=t.=B_xi, B_y|_t=ti=B_yi, B_z|_t=ti.=B_zi, t_t - i-й момент времени, i= 1,2,…, при этом t_i - t_i-1=Δt_i=Δt,i=2,3,…,

- акселерометра

где a_x|_t=ti = a_xi, a_y|_t=ti.=a_yi, a_z|_t=ti=a_zi,

- ДУС

где ω_x|_t=ti.=ω_xi, ω_y|_t=ti=ω_yi, ω_z|_t=ti=ω_zi,

а также данные ГНСС датчика в виде значений времени t_i и координат для них

где ς_B|_t=ti=ς_Bi - геодезическая широта, ς_L|_t=ti=ς_Li - геодезическая долгота, ς_H|_t=ti=ς_Hi - геодезическая высота.

Для реализации пункта 2 оценивают условия возможности формирования оценки пространственного и углового положения объекта в одном из вариантов. Для этого, как и в способе-прототипе [7], на основе имеющейся исходной информации определяется наличие и достоверность сведений о модели и случайных погрешностях используемых датчиков.

Кроме того на основе требований к оперативности получения результата уточняют возможность формирования оценки пространственного и углового положения объекта в реальном или отложенном времени.

Для реализации пункта 3 проверяют условие возможности формирования оценки пространственного и углового положения объекта в отложенном времени. Если условие возможности формирования оценки пространственного и углового положения объекта в отложенном времени соблюдается, то задают длительность интервала времени задержки ΔT₃ и на k-ом интервале накапливают выборки данных (6) - (9) для t_i ∈ [t_нач./k, t_нач./k + ΔТ₃], где t_нач.k - момент времени начала k-го интервала времени задержки.

Для реализации пункта 4 предварительно в зависимости от выбора систем координат ССК и НСК (правых или левых) на основе формирования кинематических уравнений Эйлера с последующим выражением из них вектора производных углов Эйлера формируют систему конечно-разностных уравнений относительно текущих значений углов Эйлера ω_i+1, θ_i+1, ψ_i+1 по их предыдущим значениям ω_i, θ_i, ψ_i и измерениям текущих значений угловых скоростей ДУС ω_x(i+1), ω_y(i+1), ω_z(i+1). Для левых систем координат:

а для правых систем координат:

Начальные значения углов Эйлера ω_i, θ_i, ψ_i при t_i=t₁ для (10) или (11) можно получить по данным магнитометра (6) и акселерометра (7) в начальный момент времени, когда объект находится в состоянии покоя.

Для вычисления углов крена и тангажа по выходным данным трехосевого акселерометра, трехосевого магнитометра и расчетным значениям абсолютной скорости, полученной по сигналам навигационной аппаратуры потребителя глобальной навигационной спутниковой системы, сначала определяют нормированные значения векторов (6) и (7)

Тогда для левых систем координат углы тангажа и крена по выходным данным трехосевого акселерометра принимают вид

Однако выражения (13) будут верны только в случае покоя или движения с нулевым ускорением. Поэтому в способе-прототипе [7] предлагается использовать коррекции по углам тангажа Δθ_i и крена Δϕ_i, формируемых на основе данных об ускорении объекта, полученных акселерометром и расчетным путем из данных датчика ГНСС (ς_i, ς_i-1, ς_i-2), при этом используются данные о текущем значении угла курса, получаемом на основе сигналов трехосевого магнитометра. Таким образом, получают

Вычисление угла курса на основе сигналов трехосевого магнитометра реализуется следующим образом.

Значение угла курса можно получить по данным магнетометра, приведенным к плоскости горизонта по уже известным углам θ_i и ϕ_i.

Для левых систем координат с учетом (4) получим

а для правых с учетом (5)

Тогда B_нθϕxi и В_нθϕyi - измерения в плоскости местного горизонта проекций вектора напряженности магнитного поля, поэтому значение угла курса равно магнитному курсу с учетом магнитного склонения Δψ_МСкл

где B_нθϕxi и В_нθϕyi для левой системы координат

а для правой

Полученные измеренные и вычисленные величины комплексируют, например, следующим образом.

Объединение значений угла курса, полученных по (10) или (11) по данным ДУС, т.е. величин со значениями угла курса, полученных по данным магнетометра ψ_i^Магн. по выражениям (17), (18), (19) или по выражениям (17), (20), (21), возможно на основе комплементарного фильтра, операторная форма выражения для которого приведена в способе-прототипе [7]

где Т_ψ - величина, подбираемая так, чтобы реализовать низкочастотную фильтрацию оценки ψ^Магн. и высокочастотную фильтрацию оценки ψ^ДУС.

Во временной области из (22) в дискретной форме получим

По выражению (23) осуществляется коррекция по углу курса на основе комплексирования данных магнитометра и ДУС.

Формирование оценок пространственного и углового положения объекта выполняют путем последовательного повторения шагов по времени в виде последовательных наборов величин

Для реализации пункта 5 в соответствии со способом-прототипом [7] и, например, [1] - [3], предварительно формируют вектора состояния и измерений, для них определяют уравнение формирующего фильтра и уравнение наблюдения, по ним определяют выражения для математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания, задают начальные значения математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания и далее на основе каждой новой выборки данных датчиков последовательно повторяют шаги прогноза по времени и шаги коррекции по сигналам датчиков, по получаемым значениям вектора состояния формируют оценки пространственного и углового положения объекта.

Для реализации пункта 6 сохраняют результаты формирования оценок пространственного и углового положения объекта в реальном времени также в виде последовательных наборов величин (24).

Для реализации пункта 7 после накопления выборок данных на интервале времени задержки по сигналам датчиков - выборкам данных сначала выявляют особые элементы траекторий - состояния «Покой» и «Прямолинейное движение».

УП характеризуется отсутствием линейного и углового перемещения, косвенным признаком может быть отсутствие линейного ускорения. Этот участок траектории легко выявляется по зарегистрированным сигналам трехосевого магнитометра (6), трехосевого акселерометра (7), трехосевого датчика угловой скорости (8), навигационной аппаратуры потребителя глобальной навигационной спутниковой системы (9). Измеряемые величины (6) и (9) не должны изменяться во времени, измеряемые величины (8) должны быть близки к нулю, а измеряемые величины (7) должны соответствовать постоянному значению ускорения свободного падения. С учетом внутренних шумов датчиков и внешних помех правила идентификации УП можно сформировать, например, в виде требования одновременного выполнения неравенств

где |g| - модуль ускорения свободного падения, δ_В, δ_ς, δ_ω, δ_а - малые величины.

УПД характеризуется коллинеарностью векторов линейной скорости. Для этого участка измеряемые величины (8) должны быть близки к нулю, т.е.

где - малая величина, а также для всего УПД их средние значения также должны быть близки к нулю.

Выявленные на основе выражений (25) - (29) УП и УПД запоминаются на основе разметки индексов последовательных моментов времени по множествам

Для УП (для каждого состояния «Покой»), определяемых множествами индексов моментов времени Ĩ^УП1, Ĩ^УП2, … определяют по выборкам данных - сигналам трехосевого акселерометра значения оценок углов тангажа и крена на этих участках траектории, например, по выражениям, аналогичным как для первого УП

Также для этих участков по выборкам данных - сигналам трехосевого магнитометра определяют значения предварительных оценок углов курса, например, по выражениям, аналогичным как для первого УП

где В_нθϕxУП1 и B_нθϕУП1 для левой системы координат

а для правой

Для УПД (для каждого состояния «Прямолинейное движение»), определяемых множествами индексов моментов времени Ĩ^УП1, Ĩ^УПД2, … определяют по выборкам данных - сигналам трехосевого акселерометра направления прямолинейного движения на этих участках траектории в связанной системе координат по выражениям, аналогичным как для первого УПД в виде единичного безразмерного вектора

где

- мощность множества индексов моментов времени, соответствующих разгону, - мощность множества индексов моментов времени, соответствующих торможению.

Также для этих участков по выборкам данных навигационной аппаратуры потребителя глобальной навигационной спутниковой системы определяют направления прямолинейного движения на этих участках траектории в нормальной системе координат, аналогичным как для первого УПД в виде единичного безразмерного вектора

где - вектор геоцентрических экваториальных прямоугольных координат объекта в i-й момент времени (8 - Машимов М.М. Геодезия. Теоретическая геодезия: Справочное пособие // Под ред. В.П. Савиных, В Р. Ященко. - М.: Недра, 1991.).

Безразмерные единичные вектора являются одним и тем же вектором, представленном, соответственно в связанной и нормальной системах координат, поэтому уравнение связи векторов в связанной и нормальной системах координат представляется в общем виде

Из этого уравнения связи (40) векторов в связанной и нормальной системах координат определяют углы курса на каждом участке траектории «Прямолинейное движение». В частности, по выражениям, аналогичным как для первого УПД для левой системы координат

где для упрощения записи верхние обозначения принадлежности к номеру участка УПД1 в правой части опущены,

а для правой системы координат

где также для упрощения записи верхние обозначения принадлежности к номеру участка УПД1 в правой части опущены.

По найденным углам курса на каждом участке траектории «Прямолинейное движение», то есть по выражениям, аналогичным, выражениям (41) или (42), и по предварительным оценкам углов курса на предстоящих им участках траектории «Покой», то есть по выражениям, аналогичным, выражениям (32), (33), (34) или (32), (35), (36), определяют поправки по углам курса и далее используют их как калибровки магнитометра по магнитному склонению и влиянию внешней среды, например, по выражениям, аналогичным как для первых последовательных УП и УПД

и далее используют их как калибровки магнитометра по магнитному склонению и влиянию внешней среды.

Для реализации пункта 8 сравнивают результаты оценок пространственного и углового положения объекта, реализованные при выполнении или не выполнении условия возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, с результатами оценок пространственного и углового положения объекта, реализованные при выполнении условия возможности формирования оценки пространственного и углового положения объекта в отложенном времени.

По результатам этого сравнения определяют правильность полученных оценок в реальном времени.

Таким образом, предлагаемый способ имеет следующие отличительные признаки в последовательности его реализации от способа-прототипа, которые представлены в таблице 1.

Из представленной таблицы сравнения последовательностей реализации способа-прототипа и предлагаемого способа видно, что в предлагаемом способе относительно способа-прототипа дополнительно введены операции:

- если условие возможности формирования оценки пространственного и углового положения объекта в отложенном времени соблюдается, то задают интервал времени задержки и на этом интервале накапливают выборки данных,

- после накопления выборок данных на интервале времени задержки по сигналам датчиков - выборкам данных выявляют особые элементы траекторий - состояния «Покой» и «Прямолинейное движение»,

- по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Покой» определяют значения оценок углов тангажа и крена на этих участках траектории,

- по выборкам данных - сигналам трехосевого магнитометра для каждого состояния «Покой» определяют значения предварительных оценок углов курса на этих участках траектории,

- по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в связанной системе координат,

- по выборкам данных навигационной аппаратуры потребителя глобальной навигационной спутниковой системы для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в нормальной системе координат,

- из уравнений связи векторов в связанной и нормальной системах координат определяют углы курса на каждом участке траектории «Прямолинейное движение»,

- по найденным углам курса на каждом участке траектории «Прямолинейное движение» и по предварительным оценкам углов курса на предстоящих им участках траектории «Покой» определяют поправки по углам курса и далее используют их как калибровки магнитометра по магнитному склонению и влиянию внешней среды,

- по выборкам данных для элементов траекторий, не относящихся к состояниям «Покой» и «Прямолинейное движение», определяют оценки пространственного и углового положения объекта с учетом краевых условий по углам курса, тангажа и крена, а также калибровкам магнитометра по магнитному склонению и влиянию внешней среды, полученных для элементов траекторий, относящихся к состояниям «Покой» и «Прямолинейное движение»,

- по результатам формирования оценок пространственного и углового положения объекта в отложенном времени выполняют проверку правильности полученных оценок в реальном времени, что приводит к положительному эффекту - повышению качества оценки пространственного и углового положения объекта.

Структурная схема устройства, реализующего предлагаемый способ, изображена на фигуре. В состав устройства входят: антенная система (АС) 01, навигационная аппаратура потребителя глобальной навигационной спутниковой системы (НАП ГНСС) 02, накопитель данных НАП (НД НАП) 03, модуль МЭМС (М МЭМС) 04, вычислительное устройство (ВУ) 05, устройство управления (УУ) 06.

АС 01 соединена с первым входом НАП ГНСС 02, выход которого подключен к первому входу НД НАП 03. НД НАП 03 соединен с первыми входами М МЭМС 04 и ВУ 05. Выход М МЭМС 04 подключен ко второму входу ВУ 05, выход которого соединен со входом УУ 06 и является выходом устройства. Выход УУ 06 соединен со вторыми входами НАП ГНСС 02, НД НАП 03, М МЭМС 04 и третьим входом ВУ 05.

Сигналы ГНСС поступают на АС 01, затем в НАП ГНСС 02. В НАП ГНСС 02 выполняется их обработка с выделением данных и синхронизирующих временных сигналов. Далее эти данные и сигналы поступают в НД НАП 03, где формируются выборки данных НАП ГНСС 02, а синхронизирующие сигналы подаются в М МЭМС 04. В М МЭМС 04 по сигналам датчиков - трехосевого магнитометра, трехосевого акселерометра, трехосевого датчика угловой скорости формируются выборки данных совместно с временными метками. Посредством УУ 06 производится оценка условия возможности формирования оценки пространственного и углового положения объекта в одном из вариантов. Если условие возможности формирования оценки пространственного и углового положения объекта в отложенном времени соблюдается, то посредством УУ 06 задают интервал времени задержки, УУ 06 формирует управляющий сигнал в НАП ГНСС 02, НД НАП 03, М МЭМС 04 и ВУ 05 для накопления выборок данных в НД НАП 03 и М МЭМС 04 на этом интервале и их обработки в ВУ 05.

Если условие возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, не выполняется, то посредством УУ 06 формируется управляющий сигнал в НАП ГНСС 02, НД НАП 03, М МЭМС 04 и ВУ 05 для передачи каждой новой выборки данных датчиков из НД НАП 03 и М МЭМС 04 в ВУ 05 и их обработки в ВУ 05. В частности, в ВУ 05 вычисляют кинематические углы Эйлера на основании выходных значений трехосевого датчика угловой скорости, вычисляют углы наклона (крена и тангажа) по выходным данным трехосевого акселерометра, трехосевого магнитометра и расчетным значениям абсолютной скорости, полученной по сигналам навигационной аппаратуры потребителя глобальной навигационной спутниковой системы, вычисляют угол курса на основе сигналов трехосевого магнитометра, полученные измеренные и вычисленные величины комплексируют и формируют оценки пространственного и углового положения объекта. Эти оценки из ВУ 05 поступают в УУ 06 и на выход устройства.

Если условие возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, выполняется, то посредством УУ 06 формируется управляющий сигнал в НАП ГНСС 02, НД НАП 03, М МЭМС 04 и ВУ 05 для передачи каждой новой выборки данных датчиков из НД НАП 03 и М МЭМС 04 в ВУ 05 и их обработки в ВУ 05. В частности, в ВУ 05 на основе этих сведений формируют вектора состояния и измерений, для них определяют уравнение формирующего фильтра и уравнение наблюдения, по ним определяют выражения для математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания, задают начальные значения математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания, последовательно повторяют шаги прогноза по времени и шаги коррекции по сигналам датчиков, по получаемым значениям вектора состояния формируют оценки пространственного и углового положения объекта. Эти оценки из поступают в УУ 06 и на выход устройства.

Все результаты формирования оценок пространственного и углового положения объекта в реальном времени сохраняются в ВУ 05.

После накопления выборок данных в НД НАП 03 и М МЭМС 04 на интервале времени задержки и поступления их в ВУ 05 по сигналам датчиков - выборкам данных в ВУ 05 выявляют особые элементы траекторий - состояния «Покой» и «Прямолинейное движение», по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Покой» определяют значения оценок углов тангажа и крена на этих участках траектории, по выборкам данных - сигналам трехосевого магнитометра для каждого состояния «Покой» определяют значения предварительных оценок углов курса на этих участках траектории, по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в связанной системе координат, по выборкам данных навигационной аппаратуры потребителя глобальной навигационной спутниковой системы для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в нормальной системе координат, из уравнений связи векторов в связанной и нормальной системах координат определяют углы курса на каждом участке траектории «Прямолинейное движение», по найденным углам курса на каждом участке траектории «Прямолинейное движение» и по предварительным оценкам углов курса на предстоящих им участках траектории «Покой» определяют поправки по углам курса и далее используют их как калибровки магнитометра по магнитному склонению и влиянию внешней среды, по выборкам данных для элементов траекторий, не относящихся к состояниям «Покой» и «Прямолинейное движение», определяют оценки пространственного и углового положения объекта с учетом краевых условий по углам курса, тангажа и крена, а также калибровкам магнитометра по магнитному склонению и влиянию внешней среды, полученных для элементов траекторий, относящихся к состояниям «Покой» и «Прямолинейное движение».

По результатам формирования оценок пространственного и углового положения объекта в ВУ 05 в отложенном времени по сигналу УУ 06 в ВУ 05 выполняется проверка правильности полученных оценок в реальном времени. Ее результаты поступают в УУ 06 и на выход устройства.

В качестве примера сравнения применения предлагаемого способа и способа-прототипа для левых систем координат рассмотрим оценку пространственного и углового положения объекта, перемещающегося по модельной траектории, заданной начальными условиями:

ϕ₀=0°, θ₀=2,8624°, ψ₀=0°, а_х0=a_y0=a_z0=0,

ω_х0=ω_y0 = ω_z0=0, Δt=0,01, Т_ψ=20

и значениями ускорений начальных скоростей в НСК. НСК с зафиксированным началом координат в точке ς₁ образует топоцентрическую систему координат (ТцСК). Начальное положение объекта в ТцСК определяется координатами

Ускорения в НСК определяются аналитически

через базовую функцию вида

где j - номер участка, а a_j, b_j, c_j - коэффициенты, значения которых приведены в таблице 2.

Таблица 2

Параметры функции сформированы исходя из непрерывности ее производной и равенства нулю в начальной и конечной точке значений функции и ее производной. При этом значения ускорений построенные на основе функции F_i обеспечивают на траектории участки «Покой» и «Прямолинейное движение».

Ускорения с заданными начальными условиями определяют траекторию в НСК (ТцСК) посредством последовательного интегрирования для проекций скоростей и перемещений (координат):

Положим, что для этой модельной траектории углы Эйлера определяются через проекции вектора скорости

ωϕ=0,

а моделируемые показания трехосевых МЭМС-датчиков принимают вид для:

- акселерометра

- ДУС

- магнитометра

где Δψ_МСкл.Д, Δψ_{МНакл.Д} ^_ действительные значения магнитного склонения и магнитного наклонения, Δψ_МСкл.Д=Δψ_МСкл.Т + Δψ_МСмСкл, Δψ_МСкл.Т - справочное (теоретическое) значения магнитного склонения, Δψ_МСмСкл - смещение значения магнитного склонения, обусловленное местными условиями (предметами), Δψ_МСмСкл =^-1,75°, - функция арктангенса, учитывающая возможные нулевые значения знаменателя,

n_axi, n_ayi ∈ N {0; 5 ⋅ 10^-2}, n_ωxi=n_ω ⋅ 0,0313 ⋅

n_ωyi = n_ω ⋅0,0313 ⋅ n_ωzi = nω ⋅ 0,0313 ⋅ n_ω ∈ N{0; 1}

n_Bxi, n_Byi, n_Bzi ∈ N{0;7 ⋅ 10^-8}, - обозначение нормального закона распределения с математическим ожиданием и среднеквадратическим отклонением

Моделируемые данные ГНСС датчика в виде значений координат ς_i= в моменты времени t_i формируются по выражению вида:

где ψ_{ТцСК→ГСК} - оператор перевода координат из ТцСК в геодезическую систему координат, например, в соответствии с [8], n_pxi, n_pyi, n_pzi ∈ N {0; 5 ⋅ 10^-2}.

Положим, что условие возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, не выполняется, т.е. параметры шумов в моделируемых сигналах (44) - (47) неизвестны. Таким образом, реализуются пп. 4, 6, 7 последовательности реализации предлагаемого способа (Таблица 1).

Для этих условий среднеквадратическая ошибка оценки основного для решения большинства практических задач угла курса способом-прототипом составила 1,975°, а для предлагаемого 0,599°. Таким образом, помимо возможности получения оценок в реальном времени (что предоставляет способ-прототип) применение предлагаемого способа с получением оценки пространственного и углового положения объекта в отложенном времени обеспечивает снижение среднеквадратической ошибки по углу курса более чем в 3 раза. Кроме того, только на начальном УП среднеквадратическая ошибка определения местоположения предлагаемым способом существенно снижается, так для способа-прототипа она составила 1,6236 м, а для предлагаемого способа - 0,0615 м.

Таким образом, предлагаемый способ, также как и способ-прототип, обеспечивает формирование оценки пространственного и углового положения объекта при различных вариантах результатов проверки условия наличия и достоверности сведений о модели и случайных погрешностях используемых датчиков. Кроме того, приведенная сравнительная оценка эффективности предлагаемого способа относительно способа-прототипа показывает повышение точности оценки пространственного и углового положения объекта.

Изобретение относится к области радиотехнических систем и может быть использовано, например, в системах определения координат и углового положения подвижных объектов с использованием комплексного способа навигации, объединяющего инерциальный и спутниковый способы. Способ оценки пространственного и углового положения объекта обладает следующими новыми операциями: если условие возможности формирования оценки пространственного и углового положения объекта в отложенном времени соблюдается, то задают интервал времени задержки и накапливают выборки данных. После накопления выборок данных на интервале времени задержки по сигналам датчиков - выборкам данных выявляют особые элементы траекторий - состояния «Покой» и «Прямолинейное движение». По выборкам данных навигационной аппаратуры потребителя ГНСС для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в нормальной системе координат, из уравнений связи векторов в связанной и нормальной системах координат определяют углы курса на каждом участке траектории, затем определяют поправки по углам курса. Далее используют их как калибровки магнитометра, по выборкам данных для других элементов траекторий определяют оценки пространственного и углового положения объекта с учетом краевых условий по углам курса, тангажа и крена, а также калибровкам магнитометра. По результатам формирования оценок пространственного и углового положения объекта в отложенном времени выполняют проверку правильности полученных оценок в реальном времени. Технический результат – повышение качества оценки пространственного и углового положения объекта. 1 ил., 2 табл.

Способ оценки пространственного и углового положения объекта, при котором по сигналам датчиков - трехосевого магнитометра, трехосевого акселерометра, трехосевого датчика угловой скорости, навигационной аппаратуры потребителя глобальной навигационной спутниковой системы последовательно получают выборки данных, оценивают условия возможности формирования оценки пространственного и углового положения объекта в одном из вариантов, если условие возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, не выполняется, то для каждой новой выборки данных датчиков вычисляют кинематические углы Эйлера на основании выходных значений трехосевого датчика угловой скорости, вычисляют углы наклона (крена и тангажа) по выходным данным трехосевого акселерометра, трехосевого магнитометра и расчетным значениям абсолютной скорости, полученной по сигналам навигационной аппаратуры потребителя глобальной навигационной спутниковой системы, вычисляют угол курса на основе сигналов трехосевого магнитометра, полученные измеренные и вычисленные величины комплексируют и формируют оценки пространственного и углового положения объекта, если условие возможности формирования оценки пространственного и углового положения объекта в реальном времени, связанное с наличием и достоверностью сведений о модели и случайных погрешностях используемых датчиков, выполняется, то для каждой новой выборки данных датчиков на основе этих сведений формируют векторы состояния и измерений, для них определяют уравнение формирующего фильтра и уравнение наблюдения, по ним определяют выражения для математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания, задают начальные значения математического ожидания вектора состояния и его ковариационной матрицы ошибок оценивания, последовательно повторяют шаги прогноза по времени и шаги коррекции по сигналам датчиков, по получаемым значениям вектора состояния формируют оценки пространственного и углового положения объекта, сохраняют результаты формирования оценок пространственного и углового положения объекта в реальном времени, отличающийся тем, что после оценки условий возможности формирования оценки пространственного и углового положения объекта в одном из вариантов, если условие возможности формирования оценки пространственного и углового положения объекта в отложенном времени соблюдается, задают интервал времени задержки и на этом интервале накапливают выборки данных, после накопления выборок данных на интервале времени задержки по сигналам датчиков - выборкам данных выявляют особые элементы траекторий - состояния «Покой» и «Прямолинейное движение», по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Покой» определяют значения оценок углов тангажа и крена на этих участках траектории, по выборкам данных - сигналам трехосевого магнитометра для каждого состояния «Покой» определяют значения предварительных оценок углов курса на этих участках траектории, по выборкам данных - сигналам трехосевого акселерометра для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в связанной системе координат, по выборкам данных навигационной аппаратуры потребителя глобальной навигационной спутниковой системы для каждого состояния «Прямолинейное движение» определяют направления прямолинейного движения на этих участках траектории в нормальной системе координат, из уравнений связи векторов в связанной и нормальной системах координат определяют углы курса на каждом участке траектории «Прямолинейное движение», по найденным углам курса на каждом участке траектории «Прямолинейное движение» и по предварительным оценкам углов курса на предстоящих им участках траектории «Покой» определяют поправки по углам курса и далее используют их как калибровки магнитометра по магнитному склонению и влиянию внешней среды, по выборкам данных для элементов траекторий, не относящихся к состояниям «Покой» и «Прямолинейное движение», определяют оценки пространственного и углового положения объекта с учетом краевых условий по углам курса, тангажа и крена, а также калибровкам магнитометра по магнитному склонению и влиянию внешней среды, полученных для элементов траекторий, относящихся к состояниям «Покой» и «Прямолинейное движение», по результатам формирования оценок пространственного и углового положения объекта в отложенном времени выполняют проверку правильности полученных оценок в реальном времени.