Адаптивный навигационный комплекс

Полезная модель относится к области приборостроения и может быть использована для определения координат, скорости и курса подвижных объектов (ПО) в труднодоступных районах с пересеченным рельефом местности, экстремальными условиями передвижения, при воздействии электромагнитных помех, отсутствии сигналов спутниковых навигационных систем (СНС). Технический результат - повышение точности измерения параметров движения и быстродействия процесса непрерывного определения скорости, магнитного азимута направления движения и координат. Устройство содержит блоки: датчиков магнитного поля (1), датчиков линейного ускорения (2), коррекции показаний датчиков магнитного поля и линейного ускорения (3), расчета горизонтальных проекций магнитного поля земли (4), синхронизации (5); датчик перемещения (6); блоки расчета угла направления движения ПО (7), расчета приращения координат за единицу приращения величины перемещения ПО (8), суммирования (9, 14); интегратор (10); блоки: коррекции относительного смещения координат (11), расчета корректирующих коэффициентов величины перемещения (12), умножителя (13); приемник СНС (15); блоки: интерфейса информационного обмена (16), анализа фазовых интервалов и фронтов (17), фильтрации (18), коррекции неортогональности и компенсации погрешности, при конструктивных и технологических допусках изготовления блоков датчиков (19); регистр хранения коэффициентов лабораторной настройки (20), регистр хранения исходных значений координат (21); блоки: коррекции величины ускорения ПО (22), коррекции величины магнитного поля (23), расчета коэффициентов коррекции побочных магнитных полей (24), анализа содержимого матрицы коррекции угла направления движения (25), формирования и хранения матрицы коррекции угла направления движения (26), расчета разницы углов (27), расчета параметров вектора перемещения по СНС (28), расчета параметров вектора перемещения по автономной навигационной системе (29), анализа значения угла направления движения и величины перемещения (30), хранения значений координат по СНС (31), анализа величины вектора магнитного поля (32), формирования и хранения матрицы коррекции значений величины магнитного поля (33).

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

Известно устройство, описанное в патенте RU 2279039С1, 25.01.2005 «Навигационный комплекс», содержащее приемоиндикатор спутниковой навигационной системы, измерители скорости и курса, трехстепенный магнитный датчик направления, установленный в связанной системе координат подвижного объекта ПО, датчики углов крена, тангажа, датчики углов атаки и скольжения, датчики линейных ускорений и угловых скоростей, бортовой вычислитель, при этом косвенная стабилизированная платформа с тремя кардановыми рамками, на которых установлены три моментных электродвигателя с сервоприводом, два трехкомпонентных акселерометра с механизмом их перемещения и измеритель линейной скорости перемещения, функционально связана с бортовым вычислителем.

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

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

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

- в случае наличия преднамеренных радиотехнических помех, когда сигналы от СНС будут зашумлены (искажены) и расчет координат не состоится;

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

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

Для обеспечения определения координат местоположения объекта с высокой точностью, в вышеуказанных условиях может быть использована геомагнитная навигационная система, которая избавлена от влияния перечисленных выше факторов и принцип функционирования, которой заключается в определении величины пройденного пути объектом за единицу времени измерения, а также в определении направления перемещения на основе измерения действующего значения магнитного поля Земли (МПЗ). Применение метода определения координат на основе геомагнитного принципа позволяет достичь определения координат местоположения с погрешностью не более 1% от пройденного пути (на 100 метрах пути 1 метр ошибки) и погрешностью по азимутальному углу направления движения не более 0,5 градуса.

Известно устройство описанное в патенте RU 2221991, G01C 21/08, 17/38, 20.01.2004 г. «Способ определения местоположения подвижных наземных объектов и устройство для его реализации».

Устройство содержит датчики магнитного поля; датчики вертикали; датчик перемещения; блок преобразования и усреднения; блок расчета горизонтальных проекций магнитного поля; первый блок расчета коэффициентов коррекции магнитного поля; блок коррекции магнитного поля; навигационный блок; блок управления; пульт управления; блок индикации; блок расчета продольного ускорения; блок коррекции ускорения; блок контрольных значений горизонтальных проекций МПЗ; второй блок расчета коэффициентов коррекции магнитного поля.

В исходной точке маршрута оператор с выхода пульта управления вводит исходные данные: координаты начальной точки и значение магнитного склонения для данной местности, которые поступают на вход навигационного блока. В режиме измерения два датчика магнитного поля и датчики линейного ускорения формируют на своих выходах аналоговые сигналы, пропорциональные значениям проекций Н_X, Н_Y магнитного поля и проекций А_X, А_Y ускорения силы тяжести на оси приборной системы координат. Аналоговые сигналы датчиков поступают на первый и второй входы блока преобразования и усреднения, который обеспечивает их преобразование в цифровой код и усреднение в каждом рабочем цикле. Длительность периода усреднения определяется блоком управления по информации, поступающей с его выхода на вход блока преобразования и усреднения.

Усредненные значения Н_X , Н_у поступают на вход блока коррекции магнитного поля, в котором проводится коррекция значений Н_X, Н_у с учетом коэффициентов коррекции, полученных в калибровочном цикле.

Скорректированные значения Н_X, Н_Y поступают на вход блока расчета горизонтальных проекций магнитного поля, на другой вход которого с выхода блока преобразования и усреднения через блок коррекции ускорения поступают значения проекций А_X, А_У . По скорректированным значениям Н_Х, Н_У и значениям А_Х А_У в блоке расчета горизонтальных проекций магнитного поля производится расчет горизонтальных проекций МПЗ Н_XB, Н_YB, которые поступают на вход навигационного блока.

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

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

Перед началом измерения координат, проводят калибровочный цикл, в котором определяют значения горизонтальных проекций суммарного вектора Н_XB, Н_YB напряженности МПЗ и магнитного поля объекта. С выхода блока управления на вход блока расчета коэффициентов коррекции магнитного поля поступает команда, в соответствии с которой измеренные значения H_X, Н _Y с выхода блока преобразования и усреднения поступают без изменения через блок коррекции магнитного поля на вход блока расчета горизонтальных проекций магнитного поля, на другой вход которого поступают измеренные значения проекций А_X А_У. Блок расчета горизонтальных проекций проводит определение проекций Н_XB Н_YB, и пересылает их в первый блок расчета коэффициентов коррекции магнитного поля. После завершения измерений проводят расчет коэффициентов коррекции (годографа горизонтальной составляющей) магнитного поля - смещение центра эллипса Н_Х, Н_У, и полуоси эллипса а и b.

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

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

По измеренным таким образом значениям проекций H_X, Н_У блок контрольных значений горизонтальных проекций МПЗ проводит определение усредненных значений горизонтальной составляющей МПЗ и угла направления движения, значения которых пересылаются во второй блок расчета коэффициентов коррекции магнитного поля, на вход которого с первого блока расчета коэффициентов коррекции магнитного поля поступают значения коэффициентов коррекции и значения горизонтальной составляющей МПЗ, полученные в калибровочном цикле.

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

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

Наиболее близким по технической сущности к предлагаемому известно устройство, описанное в патенте RU 100232 U1 GO1С 21\08, 07.07.2010 «Комплексированная навигационная система для определения координат подвижных наземных объектов».

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

1 - блок датчиков магнитного поля;

2 - блок датчиков линейного ускорения;

3 - блок коррекции показаний датчиков магнитного поля и линейного ускорения;

4 - блок расчета горизонтальных проекций МПЗ;

5 - блок синхронизации;

6 - датчик перемещения;

7 - блок расчета угла направления движения подвижного объекта (ПО);

8 - блок расчета приращения координат за единицу приращения величины перемещения;

9 - первый блок суммирования;

10 - интегратор;

11 - блок коррекции относительного смещения координат;

12 - блок расчета корректирующих коэффициентов величины перемещения и угловой ориентации;

13 - блок умножителя;

14 - второй блок суммирования;

15 - приемник спутниковой навигационной системы СНС;

16 - блок интерфейса информационного обмена;

17 - блок анализа фазовых интервалов и фронтов;

18 - блок фильтрации;

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

20 - регистр хранения коэффициентов лабораторной настройки;

21 - регистр хранения исходных значений координат;

22 - блок коррекции величины ускорения ПО;

23 - блок коррекции величины магнитного поля;

24 - блок расчета коэффициентов коррекции побочных магнитных полей.

Устройство-прототип содержит блок датчиков магнитного поля 1, блок датчиков линейного ускорения 2, блок коррекции показаний датчиков магнитного поля и линейного ускорения 3, блок расчета горизонтальных проекций МПЗ 4, блок синхронизации 5, датчик перемещения 6, блок расчета угла направления движения ПО 7, блок расчета приращения координат за единицу приращения величины перемещения 8, первый 9 и второй 14 блоки суммирования, интегратор 10, блок коррекции относительного смещения координат 11, блок расчета корректирующих коэффициентов величины перемещения и угловой ориентации 12, блок умножителя 13, приемник СНС 15, блок интерфейса информационного обмена 16, блок анализа фазовых интервалов и фронтов 17, блок фильтрации 18, блок коррекции неортогональности и компенсации погрешности при конструктивных и технологических допусках изготовления устройств 19, регистр хранения коэффициентов лабораторной настройки 20, регистр хранения исходных значений координат 21, блок коррекции величины ускорения ПО 22, блок расчета коэффициентов коррекции побочных магнитных полей 24 и блок коррекции величины магнитного поля 23, выход которого соединен со вторым входом блока расчета горизонтальных проекций МПЗ 4. При этом, выходы блоков датчиков магнитного поля 1 и датчиков линейного ускорения 2 соединены соответственно с первым и вторым входами блока коррекции показаний датчиков магнитного поля и линейного ускорения 3, выход которого через блок фильтрации 18 соединен с первым входом блока компенсации погрешности при конструктивных и технологических допусках изготовления устройств 19, первый выход которого соединен с первым входом блока коррекции величины ускорения ПО 22, второй выход - с первым входом блока коррекции величины магнитного поля 23, а второй вход - со вторым выходом регистра хранения коэффициентов лабораторной настройки 20, первый выход которого соединен с третьим входом блока коррекции показаний датчиков магнитного поля и линейного ускорения 3; выход датчика перемещения 6 соединен с первым входом блока анализа фазовых интервалов и фронтов 17, выход которого соединен со вторым входом блока коррекции величины ускорения ПО 22, выход которого соединен с первым входом блока расчета горизонтальных проекций МПЗ 4 и вторым входом блока расчета угла направления движения ПО 7, третий вход которого соединен со вторым выходом блока расчета корректирующих коэффициентов величины перемещения и угловой ориентации 12, а также с первым входом блока расчета коэффициентов коррекции побочных магнитных полей 24, выход которого соединен со вторым входом блока коррекции величины магнитного поля 23; первый выход блока синхронизации 5 соединен со вторым входом блока анализа фазовых интервалов и фронтов 17, второй выход - с третьим входом блока анализа фазовых интервалов и фронтов 17 и вторым входом блока расчета приращения координат за единицу приращения величины перемещения 8, а третий выход блока синхронизации 5 соединен с третьим входом блока расчета горизонтальных проекций МПЗ 4, выход которого соединен с третьим входом блока интерфейса информационного обмена 16, вторым входом блока расчета коэффициентов коррекции побочных магнитных полей 24 и первым входом блока расчета угла направления движения ПО 7, выход которого соединен с первым входом блока расчета приращения координат за единицу приращения величины перемещения 8 и четвертым входом блока интерфейса информационного обмена 16, первый выход которого через регистр хранения исходных значений координат 21 соединен с четвертым входом блока расчета корректирующих коэффициентов величины перемещения и угловой ориентации 12, третьим входом блока коррекции относительного смещения координат 11 и вторым входом первого блока суммирования 9, выход которого соединен со вторым входом блока интерфейса информационного обмена 16, второй выход которого соединен со вторым входом блока расчета корректирующих коэффициентов величины перемещения и угловой ориентации 12, первый выход которого соединен со вторым входом блока умножителя 13, первый вход которого соединен с выходом блока анализа фазовых интервалов и фронтов 17, а выход - соединен с третьим входом блока расчета приращения координат за единицу приращения величины перемещения 8, выход которого соединен с первым входом второго блока суммирования 14, выход которого соединен с входом первого блока суммирования 9 и первым входом блока коррекции относительного смещения координат 11, выход которого соединен со вторым входом второго блока суммирования 14, а второй вход - с первым входом блока интерфейса информационного обмена 16, выходом приемника СНС 15 и третьим входом блока расчета корректирующих коэффициентов величины перемещения и угловой ориентации 12; третий выход блока интерфейса информационного обмена 16 соединен с входом регистра хранения коэффициентов лабораторной настройки 20 и третьим входом блока расчета коэффициентов коррекции побочных магнитных полей 24, кроме того, выход блока расчета приращения координат за единицу приращения величины перемещения 8 через интегратор 10 соединен с первым входом блока расчета корректирующих коэффициентов величины перемещения и угловой ориентации 12.

Устройство-прототип работает следующим образом.

В производственно-лабораторных условиях, при помощи трехкоординатного поворотного стенда происходит настройка датчиков магнитного поля Hx, Нy блока 1, и датчиков Аx, Аy блока 2.

При проведении операции лабораторной настройки значения проекций МПЗ поступают с выхода блока 4 на вход 3 блока 16 для проведения анализа и расчетов корректирующих коэффициентов, затем по интерфейсу информационного обмена, массив коэффициентов лабораторной настройки поступает от ЭВМ на блок 16 и с выхода 3 блока 16 далее поступает на вход блока 20.

Аналоговые сигналы с блока 1 датчиков магнитного поля и блока 2 датчиков линейного ускорения поступают на соответствующие входы 1, 2 блока 3, где происходит преобразование аналоговых сигналов в цифровую форму.

Далее откорректированные значения с выхода блока 3 поступают на вход блока 18, где происходит фильтрация сигнала.

С выхода блока 18 фильтрованные показания датчиков поступают на вход блока 19, в котором по значениям корректирующих коэффициентов поступающих на вход 2 блока 19 с выхода 2 блока 20, производится математическая коррекция.

Для определения действующего значения магнитного поля в пункте измерения, в устройстве применены блоки 23, 4, 24 при этом: фильтрованные и откорректированные сигналы Нx, Нy с выхода 2 блока 19 поступают на вход 1 блока 23, в котором происходит коррекция измеренного значения величин Нx и Нy в соответствии с сигналами, поступающими на вход 2 блока 23 с выхода блока 24. Затем значения величин Нx, Нy поступают с выхода блока 23 на вход 2 блока 4 для проведения расчета горизонтальных проекций МПЗ. Результаты расчета горизонтальных проекций МПЗ далее поступают с выхода блока 4 одновременно на три блока: на вход 3 блока 16 в котором информация о проекциях передается пользователю, на вход 2 блока 24 и на вход 1 блока 7 в котором осуществляется расчет дирекционных углов ориентации блока датчиков.

Рассчитанные значения дирекционного угла направления с выхода блока 7 поступают на вход 1 блока 8, в котором производится расчет смещения координат за последний временной интервал измерения, а так же поступают на вход 4 блока 16 для передачи информационных параметров пользователю.

В блоке 24 по значениям горизонтальных проекций величин Нх, Нy происходит расчет действующего вектора магнитного поля Нb, и дальнейший его анализ, в сравнении со значением вектора магнитного поля (Нb0), определенным в условиях производственно-лабораторной настройки и хранимым в качестве постоянной величины в энергонезависимой памяти блока 24.

По величине отклонения значения Нb от Нb0, а также, по величине изменения значений проекций Нx и Нy относительно значений, полученных в предыдущем рабочем цикле измерений, а также по величине изменений поправки угла направления, поступающей с выхода 2 блока 12 на вход 1 блока 24, производится оценка значений поправочных коэффициентов.

В блоке 22 происходит преобразование значений ускорений Аx, Аy, поступающих с выхода 1 блока 19 на вход 1 блока 22, с учетом изменения значений величины перемещения, пройденного объектом и поступающей с выхода блока 6 на вход 1 блока 17.

Синхронизацией рабочих циклов измерений управляет блок 5, с выхода 3 которого синхронизирующий сигнал начала отсчета горизонтальной проекции МПЗ поступает на вход 3 блока 4, также с выхода 1 блока 5 на вход 2 блока 17 поступает сигнал, по которому происходит анализ длительности фазовых интервалов сигнала, поступающего от блока 6 датчика перемещения.

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

Значения приращения координат с выхода блока 8 одновременно поступают на вход 1 блока 14 и вход блока 10. Блок сумматора 10 и блок сумматора 14 - осуществляют вычисление проекций суммарного вектора перемещения объекта относительно координат исходного пункта.

По значениям величины суммарного вектора перемещения ПО относительно координат исходного пункта, рассчитанной по значениям автономного режима, которые поступают на вход 1 блока 12 и с выхода блока 10, и по значениям координат, полученным от СНС поступающим, с выхода блока 15 на вход 3 блока 12, в блоке 12 производится расчет вектора, образованного перемещением объекта, вычисленным по показаниям спутниковых координат относительно координат исходного пункта, значение которого поступает на вход 4 блока 12 с выхода блока 21.

Значение вектора, накопленного перемещения с выхода блока 14 поступает на вход 1 блока 9 в котором, по этому значению и по значению исходных координат поступающих на вход 3 блока 11 с выхода блока 21 вычисляются координаты автономной системы и по значениям этих координат, а также по значению координат спутниковой навигационной системы, поступающих на вход 2 блока 11 с выхода блока 15, вычисляется величина отклонения векторов и производится коррекция суммарного вектора перемещения.

С выхода блока 11 значение коррекции поступает на вход 2 блока 14 в результате, чего в блоке 14 изменяется значение накопленной величины суммарного вектора перемещения и уменьшается ошибка отклонения векторов.

Сигнал откорректированного значения суммарного перемещения с выхода блока 14 поступает на вход 1 блока 9, в котором производится расчет координат текущего местоположения ПО с учетом координат исходного пункта, поступающих с выхода блока 21 на вход 2 блока 9. Сигналы действительных координат местоположения, полученных в блоке 9, с его выхода поступают на вход 2 блока 16 для отправки пользователю.

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

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

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

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

Функциональная схема предлагаемого адаптивного навигационного комплекса представлена на фиг.2, где приняты следующие обозначения:

1 - блок датчиков магнитного поля;

2 - блок датчиков линейного ускорения;

3 - блок коррекции показаний датчиков магнитного поля и линейного ускорения;

4 - блок расчета горизонтальных проекций МПЗ;

5 - блок синхронизации;

6 - датчик перемещения;

7 - блок расчета угла направления движения;

8 - блок расчета приращения координат за единицу приращения величины перемещения;

9 - первый блок суммирования;

10 - интегратор;

11 - блок коррекции относительного смещения координат;

12 - блок расчета корректирующих коэффициентов величины перемещения;

13 - блок умножителя;

14 - второй блок суммирования;

15 - приемник СНС;

16 - блок интерфейса информационного обмена;

17 - блок анализа фазовых интервалов и фронтов;

18 - блок фильтрации;

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

20 - регистр хранения коэффициентов лабораторной настройки;

21 - регистр хранения исходных значений координат;

22 - блок коррекции величины ускорения ПО;

23 - блок коррекции величины магнитного поля;

24 - блок расчета коэффициентов коррекции побочных магнитных полей;

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

26 - блок формирования и хранения матрицы коррекции угла направления движения;

27 - блок расчета разницы углов;

28 - блок расчета параметров вектора перемещения по СНС;

29 - блок расчета параметров вектора перемещения по автономной навигационной системе;

30 - блок анализа значения угла направления движения и величины перемещения;

31 - блок хранения значений координат по СНС;

32 - блок анализа величины вектора магнитного поля;

33 - блок формирования и хранения матрицы коррекции значений величины магнитного поля.

Заявляемое устройство содержит блок датчиков магнитного поля 1, блок датчиков линейного ускорения 2, блок коррекции показаний датчиков магнитного поля и линейного ускорения 3, блок расчета горизонтальных проекций МПЗ 4, блок синхронизации 5, датчик перемещения 6, блок расчета угла направления движения подвижного объекта ПО 7, блок расчета приращения координат за единицу приращения величины перемещения 8, первый блок суммирования 9, интегратор 10, блок коррекции относительного смещения координат 11, блок расчета корректирующих коэффициентов величины перемещения 12, блок умножителя 13, второй блок суммирования 14, приемник СНС 15, блок интерфейс информационного обмена 16, блок анализа фазовых интервалов и фронтов 17, блок фильтрации 18, блок коррекции неортогональности и компенсации погрешности при конструктивных и технологических допусках изготовления блоков датчиков 19, регистр хранения коэффициентов лабораторной настройки 20, регистр хранения исходных значений координат 21, блок коррекции величины ускорения ПО 22, блок коррекции величины магнитного поля 23, блок расчета коэффициентов коррекции побочных магнитных полей 24, блок анализа содержимого матрицы коррекции угла направления движения ПО 25, блок формирования и хранения матрицы коррекции угла направления движения 26, блок расчета разницы углов 27, блок расчета параметров вектора перемещения по СНС 28, блок расчета параметров вектора перемещения по автономной навигационной системе 29, блок анализа значения и угла направления движения и величины перемещения 30, блок хранения значений координат по СНС 31, блок анализа величины вектора магнитного поля 32, блок формирования и хранения матрицы коррекции значений магнитного поля 33, при этом выходы блоков 1, 2 соответственно подключены к первому и второму входам блока 3, третий вход которого подключен к первому выходу блока 20, второй выход которого соединен с вторым входом блока 19, выход блока 3 соединен с входом блока 18, выход которого подключен к первому входу блока 19, первый выход которого подключен к первому входу блока 22, а второй выход соединен с первым входом блока 23, второй вход которого подключен к первому выходу блока 24, первый вход которого соединен с входом блока 20 и с третьим выходом блока 16, выход блока 6 соединен с первым входом блока 17, выход которого соединен со вторым входом блока 22 и с первым входом блока 13, второй вход которого подключен к выходу блока 12, а выход соединен с третьим входом блока 8, первый вход которого соединен с выходом блока 7, а второй вход подключен к второму выходу блока 5 и третьему входу блока 17, второй вход которого соединен с первым выходом блока 5, третий выход которого соединен с третьим входом блока 4, первый вход которого подключен к выходу блока 22 и второму входу блока 7, первый вход которого соединен с выходом блока 4 и с третьим входом блока 16, четвертый вход которого соединен с выходом блока 7, второй вход блока 4 соединен с выходом блока 23, выход блока 8 подключен к первому входу блока 10 и первому входу блока 14, выход которого подключен к первому входу блока 9, второй вход которого соединен с выходом блока 21, вход которого соединен с первым выходом блока 16, второй вход которого соединен с выходом блока 9, а первый вход соединен с выходом блока 15 и вторым входом блока 11, выход которого подключен к второму входу блока 14, второй выход блока 16 подключен к первому входу блока 25, второй вход которого соединен с первым выходом блока 26 и с третьим входом блока 32, первый выход блока 25 подключен к четвертому входу блока 8, а второй выход блока 25 соединен с первым входом блока 26, второй выход которого соединен с третьим входом блока 30, а второй вход подключен ко второму выходу блока 30 и к третьему входу блока 33, первый вход которого соединен с выходом блока 32, второй вход подключен к второму выходу блока 24, а выход соединен со вторым входом блока 24, первый вход блока 32 соединен с выходом блока 4 и с первым входом блока 7 и третьим входом блока 16, второй вход подключен к третьему выходу блока 24, четвертый вход блока 32 подключен к выходу блока 7 и к четвертому входу блока 16, первому входу блока 8 и второму входу блока 30, первый выход которого соединен с первым входом блока 31, с четвертым входом блока 33, с третьим входом блока 26 и вторым входом блока 10, а первый вход блока 30 соответственно подключен к выходу блока 13 и третьему входу блока 8, второй вход блока 16 подключен к первому входу блока 11, выход блока 10 соединен с входом блока 29, выход которого соответственно подключен к первому входу блока 12 и второму входу блока 27, выход которого соединен с четвертым входом блока 26, первый вход блока 27 подключен к выходу блока 28 и к второму входу блока 12, первый вход блока 28 соединен с выходом блока 31, второй вход которого подключен к второму входу блока 28 и выходу блока 15, выход блока 21 соединен с третьим входом блока 14.

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

Заявляемый адаптивный навигационный комплекс работает следующим образом.

В производственно-лабораторных условиях, при помощи трехкоординатного поворотного стенда происходит настройка датчиков магнитного поля Нx, Нy блока 1, и датчиков линейного ускорения по взаимно-ортогональным координатным осям Аx, Аy блока 2, причем блок 1 и блок 2 размещены в едином конструктивном корпусе.

Настройка датчиков производится посредством подбора цифровых коэффициентов, учитывающих и корректирующих влияние магнитного поля таким образом, чтобы в приборной системе координат, которая совмещена с основанием крепления корпуса блока датчиков, при любой угловой ориентации, значение проекции действующего вектора магнитного поля Нb на оси приборной системы координат (Нх, Нy), образовало магнитный годограф, который имеет форму идеального круга с центром в точке начала координат и определенное фиксированное значение величины проекции магнитного поля (Нb0), значение которого хранится в памяти блока 32.

При проведении операции лабораторной настройки, цифровые значения проекций МПЗ поступают с выхода блока 4 на третий вход блока 16 и с него передаются на ЭВМ для проведения анализа и расчетов корректирующих коэффициентов, затем массив коэффициентов лабораторной настройки поступает от ЭВМ на блок 16 и с третьего выхода блока 16 далее поступают на вход блока 20, где в дальнейшем сохраняются в его энергонезависимой памяти.

В рабочем цикле измерений блок 1 и блок 2 непрерывно формируют на своих выходах аналоговые сигналы, измерения действующего магнитного поля по ортогональным осям Нx, Нy приборной системы координат и измерения проекции действующего ускорения на оси Аx, Аy приборной системы координат.

Аналоговые сигналы с выходов блока 1 и блока 2, пропорциональные действующему значению магнитного поля и линейного ускорения поступают соответственно на первый и второй входы блока 3, где происходит преобразование аналоговых сигналов в цифровую форму, коррекция показаний с установкой нулевых значений, масштабирование в соответствии с величиной Нb0, и выравнивание показаний по координатным осям приборной системы координат. При этом коэффициенты коррекции и настройки поступают с первого выхода блока 20 на третий вход блока 3.

Параметры Нx, Нy вектора магнитного поля Н и параметры Аx, Аy вектора ускорения А вычисляются из показаний значений датчиков магнитного поля и линейного ускорения в блоке 3. В процессе вычислительных операций учитываются следующие коэффициенты:

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

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

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

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

Далее откорректированные значения с выхода блока 3 поступают на вход блока 18, где происходит фильтрация сигнала на основе частотного интегрирования показаний (в зависимости от частоты появления измеренного значения сигнала накапливается сумма количества появившихся измерений и по максимальному значению суммы выбирается соответствующее значение).

При этом с высокой оперативностью получаются достоверные значения выходных величин Нx, Нy, Аx, Аy при любых быстроменяющихся показаниях, поступающих с выходов блоков 1, 2 при воздействии разнонаправленных угловых ускорений.

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

Магнитное поле, измеряемое датчиками блока 1, представляет собой сумму величины магнитного поля, действующего в пункте измерения и величины магнитного поля, излучаемого объектами, имеющими ферромагнитные свойства и расположенными поблизости от блока датчиков 1 и 2. Для исключения влияния побочных магнитных полей при измерении действующего значения магнитного поля в пункте измерения, используются блоки 23, 4, 24, работа которых организована следующим образом: фильтрованные и откорректированные сигналы Нx, Нy с второго выхода блока 19 поступают на первый вход блока 23, в котором происходит коррекция измеренного значения величин Нx и Нy в соответствии с коэффициентами, поступающими на второй вход блока 23 с первого выхода блока 24. Коррекция происходит в соответствии с выражениями (1) и (2).

где Нxa и Нya - скорректированные уточненные составляющие величины магнитного поля по координатным осям X, Y приборной системы координат. Причем коэффициенты КМx и КМy, которые входят в выражения (1) и (2), являются коэффициентами, которые масштабируют величину магнитного поля для компенсации сжатия или растяжения полуосей магнитного годографа, искажение которых возникает под воздействием побочных магнитных полей, а также при изменении магнитного наклонения полного вектора магнитного поля, что связано с географическим местоположением ПО.

Компоненты SHx и SHy которые входят в формулы (1) и (2), являются коэффициентами, которые смещают измеренные значения магнитного поля к центру приборной системы координат для обеспечения симметрии относительно осей приборной системы координат при различной ориентации ПО в пространстве.

Смещение и искажение формы годографа магнитного поля (годограф получаем при изменении ориентации блока датчиков вследствие его вращения в горизонтальной плоскости от 0 до 360 градусов) возникает при переходе из лабораторных условий настройки к установке блока датчиков на объект эксплуатации, вследствие наличия на объекте эксплуатации металлических предметов и их взаимного месторасположения.

Описанные компоненты КМx, КМy и SHx, SHy входящие в выражения (1) и (2) являются медленно меняющимися величинами, которые зависят от совокупного фона магнитных помех, создаваемых металлическими предметами, находящимися в месте размещения блоков датчиков 1 и 2. Величина этих коэффициентов определяется после выполнения процедуры устранения девиации при вводе устройства в эксплуатацию и далее корректируется адаптивным алгоритмом компенсации магнитных помех в процессе эксплуатации.

Процедура устранения девиации заключается в измерении действующего магнитного поля при различных ориентациях ПО, с поочередным изменением положения ПО с дискретностью изменения угла на 30-60 градусов (6-12 положений). При выполнении процедуры устранения девиации показания измеренного магнитного поля с выхода блока 4 подаются на третий вход блока 16 и далее передаются пользователю. По окончании процедуры устранения девиации расчетные коэффициенты коррекции с третьего выхода блока 16 подаются на вход 1 блока 24, где сохраняются в энергонезависимой памяти.

Компоненты DHx и DHy, которые входят в формулы (1) и (2), являются коэффициентами, которые компенсируют величину отклонения магнитного годографа от формы правильного круга, возникающие вследствие неполной компенсации побочных магнитных полей, а также при перемагничивании ПО во время эксплуатации устройства.

Компоненты DHx и DHy являются табличными величинами, значение которых определяется при учете отклонения постоянно вычисляемого значения величины магнитного поля (Нb) от лабораторно установленного (Нb0) и углового отклонения между векторами, первый из которых образован направлением движения ПО, рассчитанного по географическим координатам, вычисленным по параметрам полученным от датчиков перемещения 6, магнитного поля 1 и ускорения 2, второй из которых образован направлением движения ПО, рассчитанного по географическим координатам, полученным от спутникового приемника (что для нашего устройства считается достоверным значением координат).

Часть комплекса включающая блоки датчиков и блоки обработки параметров магнитного поля, ускорения, перемещения, коррекции, фильтрации, расчета координат представляет автономную навигационную систему, а часть комплекса, в которой обрабатываются параметры, полученные от спутникового приемника, является спутниковой навигационной системой.

Таблица, в которой хранятся корректирующие коэффициенты DHx и DHy сформирована на основе 24-х дискретных величин (для каждого из двух коэффициентов отдельный набор значений), которые представлены как номера угловых секторов (величина углового сектора 15 градусов), на который ориентирован блок датчиков ПО. Соответственно, при вращении блока датчиков в диапазоне от 0 до 360 градусов образуется 24 сектора каждый из которых имеет величину 15 градусов.

Затем скорректированные значения величин магнитного поля Нха, Нya поступают с выхода блока 23 на второй вход блока 4 для проведения расчета горизонтальных проекций МПЗ. Результаты расчета горизонтальных проекций МПЗ далее поступают с выхода блока 4 одновременно на три блока: на третий вход блока 16, в котором информация о проекциях передается пользователю, на первый вход блока 32, где происходит расчет величины проекции вектора магнитного поля Нb и анализ соответствия величины этого вектора, вектору (Нb0), определенному ранее при лабораторной настройке, и на первый вход блока 7, в котором осуществляется расчет углов ориентации блока датчиков: магнитный азимут и углы наклонов (продольные, поперечные).

Углы наклонов приборной системы координат в продольном и поперечном направлении определяются в блоке 7 по соответствующим выражениями.

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

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

Az - значение линейного ускорения в проекции на ось Z приборной системы координат (значение рассчитано по выражению (4));

где А - постоянная величина соответствующая величине ускорения силы тяжести, равная 9,81 м/с², цифровое значение которой определяется на этапе лабораторной настройки устройства.

В блоке 22 происходит коррекция значений линейных ускорений Ах, Ау, поступающих с первого выхода блока 19 на первый вход блока 22, с учетом изменения значений величины перемещения, пройденного ПО, значение которого поступает на второй вход блока 22 с выхода блока - 17. То есть, при наличии ускорений, возникающих при перемещении ПО, которые определяются по изменению его скорости, производится анализ значений измеренных ускорений, получаемых в устройстве по измеренным значениям датчиков блока 2, и по результату анализа определяются корректирующие значения, которые компенсируют показания ускорений, связанные с маневрами ПО при совершении разгонов, торможений, поворотов. Компенсированные значения ускорений определяются в соответствии с выражением (5), и с выхода блока 22 поступают одновременно на первый вход блока 4 и на второй вход блока 7. Данные для расчета перемещений объекта за последний измеренный интервал поступают на первый вход блока 17 с выхода блока 6, который осуществляет формирование импульсов, длительности которых пропорциональны приращению скорости движения ПО.

где ⁱ-^i-1 - изменение скорости за промежуток времени, в котором проводится измерение ускорения,

t - временной интервал измерения скорости движения ПО,

Аy` - измеренные значения действующего ускорения после фильтрации и коррекции.

Математическая обработка, производимая в блоке 4, осуществляет компенсацию величин Нха, Нyа, поступающих на второй вход блока 4 с выхода блока 23, с учетом компенсированных показаний значений Ах и Аy, сигнал которых поступает на первый вход блока 4 с выхода блока 22, в результате которой при любых наклонах блоков 1 и 2, форма магнитного годографа остается неизменной (сохраняется идеальный круг радиуса Нb0, с центром в начале координат).

Рассчитанные значения углов ориентации, с выхода блока 7 поступают на первый вход блока 8, в котором производится расчет смещения координат за последний временной интервал измерения, а также поступают на четвертый вход блока 16 для передачи информационных параметров пользователю, также поступают на второй вход блока 30 для определения номера углового сектора, в направлении которого ориентирован ПО, и на четвертый вход блока 32, в котором по величине измеренного угла ориентации блока датчиков происходит анализ соответствия величины проекции вектора измеряемого магнитного поля с проекциями вектора, соответствующего лабораторным значениям для данной ориентации ПО.

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

Одновременно с определением угла ориентации, рассчитанного по параметрам датчиков блока 1, значения которых поступают с выхода блока 7 на первый вход блока 8, на четвертый вход блока 8 с первого выхода блока 25 поступает значение коэффициента коррекции угла, который корректирует угол ориентации блоков 1, 2 датчиков, для определения дирекционного угла направления движения ПО в системе географических координат. В значении коэффициента коррекции угла совокупно учтены поправки магнитного склонения для данной местности, также поправка вследствие технологической погрешности, возникающей при конструктивном креплении блоков датчиков 1, 2 на корпусе ПО, а также поправка девиационной погрешности при определении магнитного азимута. Исходное значение коэффициента коррекции угла поступает на первый вход блока 25 с второго выхода блока 16, которое в свою очередь поступает на блок 16 от управляющей ЭВМ.

Вторым параметром, по которому в блоке 8 происходит расчет смещения координат, является расстояние, пройденное ПО за последний временной интервал измерения; значение этого параметра поступает на третий вход блока 8 и на первый вход блока 30 с выхода блока 13. В блоке 30 производится анализ значения угла направления движения ПО, полученного с выхода блока 7, и величины перемещения полученной из блока 13. В блоке 13 производится расчет расстояния, пройденного ПО на основе показаний, поступающих на первый вход блока 13 с выхода блока 17 с учетом коэффициента коррекции пути, в котором совокупно учтены передаточные коэффициенты связи колесной оси и вала привода спидометра, состояние ходовой части и длинна окружности колеса, значение которого поступает на второй вход блока 13 с выхода блока 12. Расчет смещения координат, по проекциям GXS, GYS вектора суммарного перемещения ПО определяется в блоке 8 в соответствии с выражениями (6) и (7).

где KN - коэффициент учитывающий направление перемещения при движении: «вперед» - коэффициент "1", «назад» - коэффициент "-1";

S - величина перемещения ПО при движении за интервал времени измерения;

Аl - угол направления движения ПО относительно северного направления географического меридиана;

Gпр - угол ориентации продольной оси транспортного средства относительно уровня горизонта;

Lm - коэффициент учитывающий переход от линейного измерения перемещения к градусному. Это величина, соответствующая 1° меридиана, которая может принимать значения в диапазоне от 111120 до 111212 метров в зависимости от применяемой модели Земного эллипсоида, в предложенном комплексе применяется модель эллипсоида соответствующая международной системе координат WGS-84;

cos() - косинус угла сближения меридианов для различных широт текущего местоположения, - это угол широты текущего местоположения.

В блоке 5 производится синхронизация рабочих циклов измерений при этом с третьего выхода блока 5 синхронизирующий сигнал начала отсчета горизонтальной проекции МПЗ (период 1/16 сек.) поступает на третий вход блока 4; в то же время, с первого выхода блока 5 на второй вход блока 17 поступает высокочастотный сигнал (с длительностью 20 мкс), по которому производится анализ периода переключения фаз сигнала, поступающего на первый вход блока 17 с выхода блока 6. Выбранный период 1/16 секунды связан с особенностью реализации устройства при фильтрации измеренных показаний с применением двоичной системы. Выбранный период 20 мкс определяется типом переменной, используемой для счетчика импульсов для определения текущего состояния фазы датчика перемещения блока 6. Со второго выхода блока 5 сигнал управления (с периодом 1 сек.) поступает одновременно на третий вход блока 17, по которому происходит фиксация интервала пройденного расстояния за последний временной интервал наблюдения, и на второй вход блока 8, по которому запускается процедура расчета смещения координат за последний временной интервал измерения.

Рассчитанные значения приращения координат, которые являются значениями координат ПО, определенное по автономной системе, с выхода блока 8 одновременно поступают на первый вход блока 14 и на первый вход блока 10.

Блок 14 осуществляет вычисление проекций суммарного вектора перемещения ПО относительно координат исходного пункта, которые могут корректироваться для уточнения координат местоположения ПО.

Значение суммарного вектора перемещения, накопленного в блоке 14, с его выхода поступает на первый вход блока 9, в котором по этому значению и по значению исходных координат, определенных по автономной системе, поступающих на второй вход блока 9, с выхода блока 21, вычисляются текущие координаты местоположения ПО, которые с выхода блока 9 поступают на второй вход блока 16 для отправки результатов расчетов пользователю и на первый вход блока 11 для расчета корректирующей поправки значений суммарного вектора перемещения.

Расчет координат в блоке 9 проводится в соответствии с выражениями (8) и (9).

где GXA, GYA - географические координаты текущего положения ПО по оси X и оси Y соответственно, на плоскости географической системы координат, определенные по рассчитанным параметрам при работе системы в автономном режиме.

GXO, GYO - географические координаты исходного положения ПО откуда начался отсчет перемещения.

GXS, GYS - величина проекций вектора суммарного перемещения ПО относительно пункта установки исходных значений координат на оси X, Y в плоскости географической системы координат.

По значениям координат, полученных от спутниковой навигационной системы 15, поступающих на второй вход блока 11 с выхода блока 15, и по значениям координат автономной навигационной системы, поступающим на первый вход блока 11 с выхода блока 9, в блоке 11 вычисляется величина вектора отклонения автономной от спутниковой системы (координаты спутниковой системы считаем достоверными). При достижении вектора отклонения более заранее определенной пороговой величины, производится расчет поправочных значений коррекции относительного смещения координат автономной системы, причем вычисление поправочных значений происходит в период выдачи блоком 15 значений координат ПО принятых за достоверные. Также значения координат от приемника спутниковой навигации с выхода блока 15 поступают на вход 1 блока 16, для передачи пользователю с целью его информированности о местоположении ПО по параметрам спутниковой навигационной системы.

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

Накопление суммарного значения величины проекций вектора перемещения в блоке 14 начинает проводиться от момента установки координат исходного пункта расположения ПО, и каждый раз по установке координат исходного пункта суммарное значение величины вектора перемещения обнуляется. Такая операция проводится по команде, поступающей с выхода блока 21 на третий вход блока 14. Также координаты исходной точки с выхода 1 блока 16 поступают на вход блока 21, которые запоминаются в энергонезависимой памяти блока 21 и каждый раз по включению питания происходит восстановление их значений, до тех пор, пока оператор не установит их новые значения.

После включения электропитания комплекса, в блоке 30 проводится формирование команды определения номера углового сектора, в котором ориентирован ПО и при определении номера углового сектора происходит запуск цикла расчета коэффициентов коррекции параметров движения (ККПД). Во время цикла расчета ККПД проводится расчет и последующий анализ параметров движения ПО: определение величины коррекции магнитного азимута направления перемещения ПО и величины коррекции пройденного пути.

Цикл расчета ККПД начинается с выполнения трех операций, первая из которых производит запоминание текущих значений координат ПО полученных от СНС, значения которых поступают с выхода блока 15 на второй вход блока 31, при этом команда поступает с первого выхода блока 30 на первый вход блока 31, вторая операция проводит инициализацию в нулевое значение величины вектора накопления смещения по автономной навигационной системе, при этом команда поступает с первого выхода блока 30 на второй вход блока 10, третья операция производит обнуление счетчика пройденного расстояния от начала цикла расчета ККПД, при этом процесс операции производится внутри блока 30. Условием начала цикла расчета ККПД является выдача достоверных значений координат ПО от спутниковой навигационной системы (точность значений не хуже 30 метров), определяемых характеристиками СНС.

В течение времени цикла расчета ККПД в блоке 10 проводится определение проекций суммарного вектора перемещения, рассчитанного по показаниям приращения координат автономной навигационной системы, поступающих с выхода блока 8 на первый вход блока 10. С выхода блока 10 рассчитанные значения суммарного вектора перемещения поступают на вход блока 29, в котором проводится анализ величин проекций вектора перемещения и расчет его параметров: величины и ориентации направления движения ПО в системе географических координат. Одновременно с этим в блоке 28 также происходит анализ величин проекций вектора перемещения и расчет параметров вектора, образованного по показаниям спутниковой системы. Для этого на первый вход блока 28 с выхода блока 31 поступает хранимое значение координат спутниковой навигационной системы, зафиксированных в памяти в начале цикла ККПД, а также текущее значение координат, определенных по спутниковой навигационной системе, которые поступают на второй вход блока 28 с выхода блока 15. Причем вычисление происходит в тот момент, когда блок 15 выдает достоверные значения координат (точность значений не хуже 30 метров). Затем параметры угловой ориентации векторов, рассчитанных по автономной и спутниковой навигационной системам, с выхода блока 29 и выхода блока 28 соответственно поступают на вход 2 и вход 1 блока 27. Также параметры величины векторов, рассчитанных по автономной и спутниковой навигационным системам, с выхода блока 29 и с выхода блока 28 соответственно поступают на вход 1 и вход 2 блока 12, в котором происходит расчет коэффициента пропорциональности соотношений их величин - Ks (коэффициент коррекции пройденного пути).

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

Блок 27 производит расчет разницы угловых ориентаций, вышеуказанных векторов. Результат расчета поступает с выхода блока 27 на четвертый вход блока 26. В блоке 26 происходит анализ значений разницы углов вышеуказанных векторов, и в зависимости от рассчитанной величины производится поправка значений матрицы коррекции угла направления движения ПО, в соответствии с определенным номером углового сектора, причем номер углового сектора поступает с второго выхода блока 30 на второй вход блока 26. Матрица коррекции угла направления движения ПО формируется и хранится в блоке 26 и представляет собой набор ячеек памяти, в которых хранятся цифровые значения угловых коэффициентов, используемые для коррекции указанного параметра.

При выполнении условий завершения цикла расчета ККПД, блок 30 сформирует команду, завершения цикла, которая одновременно является командой начала нового цикла расчета ККПД, которая подается с первого выхода блока 30 одновременно на первый вход блока 31, на второй вход блока 10 и на четвертый вход блока 33.

Команда завершения цикла расчета ККПД также транслируется с первого выхода блока 26 на второй вход блока 25, в котором происходит запуск алгоритма анализа постоянной составляющей угла, который присутствует в матрице коррекции угла направления движения ПО.

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

Сформированные в блоке 26 коэффициенты коррекции углового направления фиксируют наличие искажений магнитного поля в данном угловом секторе ориентации ПО, которые возникают вследствие влияния остаточных значений побочных магнитных полей, которые не были устранены при проведении процедуры устранения девиации.

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

Для повышения точности определения углов ориентации направления перемещения ПО в процессе измерения магнитного поля и устранения влияния побочных магнитных полей введена дополнительная обработка, которая осуществляется в блоках 32, 33, 23, 24, которые работают следующим образом: горизонтальные проекции магнитного поля поступают с выхода блока 4 на первый вход блока 32; по ним осуществляется расчет величины действующего вектора магнитного поля Нb, и производится его сравнение с величиной вектора Нb0, определенного в условиях производственно-лабораторной настройки, который хранится в качестве постоянной величины в энергонезависимой памяти блока 32, который поступает на второй вход блока 32 с третьего выхода блока 24 на этапе производственно-лабораторной настройки. В случае отклонений величины векторов магнитного поля, производится анализ величины корректирующего коэффициента поправки углового направления, которая поступает на третий вход блока 32 с первого выхода блока 26, и по ней формируется корректирующие коэффициенты величин проекций магнитного поля, которые поступают в матрицу коэффициентов коррекции на первый вход блока 33 с выхода блока 32, в соответствии с номером углового сектора, который поступает на третий вход блока 33 со второго выхода блока 30, причем коррекция матрицы производится по команде начала нового цикла ККПД, которая поступает на четвертый вход блока 33 с первого выхода блока 30.

С выхода блока 33 коэффициенты коррекции проекции магнитного поля DHx и DHy, которые действительны при ориентации ПО в данном угловом секторе, поступают на второй вход блока 24, которые транслируются с первого выхода блока 24 на второй вход блока 23 для проведения коррекции магнитного поля в соответствии с выражениями (1) и (2). В блоке 24 также производится анализ содержимого матрицы коррекции магнитного поля и выделение из содержимого матрицы поправочных значений для коэффициентов КМx, КМy, SHx, SHy, которые были определены на этапе устранения девиации, при этом в случае коррекции вышеуказанных коэффициентов, по команде поступающей с второго выхода блока 24 на второй вход блока 33 в матрице, хранящейся в блоке 33 также проводится коррекция содержимого.

При выключении электропитания устройства происходит запоминание коэффициентов, хранящихся в блоке 33, в блоке 26, в блоке 25, в блоке 24 в их энергонезависимой памяти.

Поскольку в блоке 24 при формировании коэффициентов корректирующих измеряемое значение магнитного поля, учитываются значения матрицы коэффициентов коррекции магнитного поля формируемые и хранящиеся в блоке 33, и поступающие с его выхода на второй вход блока 24, а они в свою очередь изменяются под влиянием коэффициентов приходящих по обратной связи, с первого выхода блока 26 на третий вход блока 32, то происходит компенсационная подстройка коэффициентов коррекции магнитного поля в случае изменения магнитной обстановки вокруг первого блока датчиков магнитного поля.

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

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

Техническая реализация блока 1 возможна с применением магниточуствительных датчиков на основе феррозондов, или магниторезисторов в интегральном исполнении (например, НМС1002 фирмы Honeywell) с разрешением 27 ugauss, диапазон измерений ±6 gauss, чувствительностью 0,2 mV/gauss.

Техническая реализация блока 2 возможна с применением акселерометров формы STMicroelectonics (LIS3L06AL) или фирмы Analog Devices (ADXL325) с разрешением 0,5 mG, диапазон измерений ±2G, чувствительностью 0,6 V/G.

Датчик перемещения, входящий в состав устройства, реализован с применением трех датчиков Холла и четырех разнополюсных магнитов, закрепленных на оси, которая соединяется в разрыв тросика вала спидометра транспортного средства. Таким образом, образуется трехканальный датчик, у которого на один оборот колеса генерируется 6 импульсов состояния фазы угла поворота, что является достаточным для определения пройденного расстояния и скорости движения при перемещении на скорости выше 5 км/ч. При использовании электронного спидометра точность счисления пути будет ограничено значением расстояния пройденного колесом транспортного средства за один полный оборот.

Приемник спутниковой навигационной системы 15 реализован на основе отечественного приемника РНВ208, который выпускается фирмой ФГУП НИИ КП. Он работает с возможностью приема 30 каналов в системе ГЛОНАСС / GPS и имеет цифровой интерфейс информационного обмена RS-232 с реализацией многофункционального протокола информационного обмена.

Остальные блоки, входящие в состав заявляемого адаптивного навигационного комплекса, осуществляют математические преобразования, поэтому их можно реализовать на основе микроконтроллеров или цифровых сигнальных процессоров, в частности, макетный образец, на котором реализовано предлагаемое устройство, построен с применением микросхемы DSPIC33FJ128GP708 фирмы MicroChip (128 Кбайт память программ, 16 Кбайт память ОЗУ, встроенные контроллер АЦП 12 бит, два контроллера интерфейсов RS-232, интерфейс SPI, тактовая частота до 40 МГц, девять встроенных таймеров); также возможно применение сигнальных процессоров фирм Texas Instruments или Analog Devices, но в случае применения сигнальных процессоров необходимо применение внешнего АЦП с разрядностью не менее 12 бит.

В качестве особенности работы блока 3 следует заметить, что необходимо задействовать пять каналов АЦП или применить микросхему мультиплексирования каналов (например, отечественного производства К561КП2).

Технический результат от использования предлагаемого способа и устройства ограничен типом и характеристиками применяемой элементной базы. В макетном варианте были получены измерения дирекционных углов и углов наклона с дискретностью 0,1 градуса, с точностью определения углов не более 0,5 градуса, ошибка определения координат перемещения не более 1%.

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