АВТОНОМНАЯ ПЕРСОНАЛЬНАЯ ИНФОРМАЦИОННО-ИЗМЕРИТЕЛЬНАЯ СИСТЕМА НАЗЕМНОГО ПОЗИЦИОНИРОВАНИЯ С КОРРЕКЦИЕЙ УГЛОВ НАКЛОНА ПО ОПОРНОЙ ПОВЕРХНОСТИ

Елена Купоросова

АВТОНОМНАЯ ПЕРСОНАЛЬНАЯ ИНФОРМАЦИОННО-ИЗМЕРИТЕЛЬНАЯ СИСТЕМА НАЗЕМНОГО ПОЗИЦИОНИРОВАНИЯ С КОРРЕКЦИЕЙ УГЛОВ НАКЛОНА ПО ОПОРНОЙ ПОВЕРХНОСТИ

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

Приборостроение

Диссертации

Вуз: Казанский национальный исследовательский технический университет им. А.Н. Туполева-КАИ (КНИТУ КАИ)

ID: 5f3004e2cd3d3e0001b5a22d

UUID: a14f8050-bc78-0138-153f-0242ac180006

Язык: Русский

Опубликовано: больше 3 лет назад

Просмотры: 67

64.44

Елена Купоросова

Казанский национальный исследовательский технический университет им. А.Н. Туполева-КАИ (КНИТУ КАИ)

Комментировать 0

Рецензировать 12

Скачать - 6,8 МБ

Поделиться работой

Министерство образования и науки Российской Федерации ФГБОУ ВО «Казанский национальный исследовательский технический университет им. А.Н. Туполева» (КНИТУ-КАИ) На правах рукописи Купоросова Елена Серафимовна АВТОНОМНАЯ ПЕРСОНАЛЬНАЯ ИНФОРМАЦИОННОИЗМЕРИТЕЛЬНАЯ СИСТЕМА НАЗЕМНОГО ПОЗИЦИОНИРОВАНИЯ С КОРРЕКЦИЕЙ УГЛОВ НАКЛОНА ПО ОПОРНОЙ ПОВЕРХНОСТИ Специальность 05.11.16 – Информационно-измерительные и управляющие системы (в приборостроении) ДИССЕРТАЦИЯ на соискание ученой степени кандидата технических наук Научный руководитель: кандидат технических наук, доцент Потапов А.А Казань, 2019

2 ОГЛАВЛЕНИЕ ВВЕДЕНИЕ………………………………………………………………………5 Глава АНАЛИТИЧЕСКИЙ 1. ПОВЫШЕНИЯ ТОЧНОСТИ ОБЗОР СОСТОЯНИЯ АВТОНОМНЫХ ИНФОРМАЦИОННО-ИЗМЕРИТЕЛЬНЫХ МЕТОДОВ ПЕРСОНАЛЬНЫХ СИСТЕМ НАЗЕМНОГО ПОЗИЦИОНИРОВАНИЯ…………………………………………………………….13 1.1 Анализ персональных требований и особенностей построения информационно-измерительных автономных систем наземного позиционирования…………………………………………………………………….13 1.2 Классификация персональных методов повышения точности информационно-измерительных автономных систем наземного позиционирования…………………………………………………………………….15 1.3 Методы компенсации систематических погрешностей датчиков первичной информации………………………………………………………………16 1.4 Методы компенсации случайных погрешностей датчиков первичной информации…………………………………………………………………………...17 1.5 Методы комплексирования………………………………………………..18 1.6 Методы повышения точности алгоритма счисления пути…………….24 1.7 Сравнительная характеристика методов повышения точности автономных персональных информационно-измерительных систем……………...28 ВЫВОДЫ И ПОСТАНОВКА ЗАДАЧИ НАУЧНОГО ИССЛЕДОВАНИЯ...32 Глава 2. РАЗРАБОТКА СПОСОБА ОПРЕДЕЛЕНИЯ УГЛОВ НАКЛОНА БЛОКА ДАТЧИКОВ ПЛОСКОСТИ ПЕРВИЧНОЙ ГОРИЗОНТА БЕЗ ИНФОРМАЦИИ ОТНОСИТЕЛЬНО НАКОПЛЕНИЯ ПОГРЕШНОСТИ ИЗМЕРЕНИЯ………………………………………………………………………….34 2.1 Постановка задачи………………………………………………………….34 2.2 Обзор датчиков расстояния (дальномеров)……………………………….35 2.3 Построение схемы дополнительной системы угловой ориентации с дальномерами………………………………………………………………………….37 2.4 Построение математической модели расстояния до опорной горизонтальной поверхности…………………………………………………………41

3 2.5 Построение алгоритма определения опорных углов наклона блока датчиков первичной информации относительно опорной горизонтальной поверхности для произвольного числа дальномеров………………………………..44 2.6 Получение аналитических выражений опорных углов наклона блока датчиков первичной информации с использованием показаний трех и четырех дальномеров…………………………………………………………………………...49 2.7 Построение математической модели расстояния до опорной поверхности, плоскость которой имеет уклон по отношению к плоскости горизонта……………56 2.8 Построение алгоритма определения углов уклона опорной поверхности относительно плоскости горизонта…………………………………………………..60 2.9 Построение алгоритма определения углов наклона блока датчиков первичной информации относительно плоскости горизонта……………………….64 ВЫВОДЫ……………………………………………………………………….68 Глава 3. ОЦЕНКА ПОГРЕШНОСТЕЙ ОПРЕДЕЛЕНИЯ УГЛОВ НАКЛОНА БЛОКА ДАТЧИКОВ ПЕРВИЧНОЙ ИНФОРМАЦИИ ОТНОСИТЕЛЬНО ОПОРНОЙ ПОВЕРХНОСТИ………………………………………………………...70 3.1 Постановка задачи………………………………………………………….70 3.2 Получение аналитической зависимости расстояния, измеряемого дальномерами, от погрешностей его установки на блоке датчиков первичной информации……………………………………………………………………………72 3.3 Получение уравнений чувствительности погрешностей определения опорных углов наклона к погрешностям установки дальномеров на блоке датчиков первичной информации……………………………………………………………….75 3.4 Проведение факторного эксперимента в пространстве конструктивных параметров установки дальномеров на блоке датчиков первичной информации…83 3.5 Разработка рекомендаций по дальнейшей алгоритмической компенсации возможных инструментальных погрешностей системы……………………………91 ВЫВОДЫ……………………………………………………………………….99 Глава 4. РАЗРАБОТКА МЕТОДОВ ПОВЫШЕНИЯ ТОЧНОСТИ РАБОТЫ АВТОНОМНОЙ ПЕРСОНАЛЬНОЙ ИНФОРМАЦИОННО-ИЗМЕРИТЕЛЬНОЙ СИСТЕМЫ НАЗЕМНОГО ПОЗИЦИОНИРОВАНИЯ…………………………….100 4.1 Постановка задачи………………………………………………………...100

4 4.2 Разработка схемы комплексной системы угловой ориентации………...100 4.3 Разработка алгоритма обнуления выходов интеграторов………………103 4.4 Разработка алгоритма коррекции путевой скорости подвижного объекта на приоритетных направлениях……………………………………………………..105 ВЫВОДЫ……………………………………………………………………...110 Глава 5. ОЦЕНКА РАЗРАБОТАННЫХ МЕТОДОВ ПОВЫШЕНИЯ ТОЧНОСТИ ОПРЕДЕЛЕНИЯ МЕСТОПОЛОЖЕНИЯ НА ИМИТАЦИОННОЙ МАТЕМАТИЧЕСКОЙ МОДЕЛИ ИНФОРМАЦИОННО-ИЗМЕРИТЕЛЬНОЙ СИСТЕМЫ…………………………………………………………………………...111 5.1 Постановка задачи………………………………………………………...111 5.2 Построение модели движения наземного подвижного объекта с блоком датчиков первичной информации…………………………………………………..111 5.3 Построение модели персональной информационно-измерительной системы……………………………………………………………………………….132 5.4 Представление результатов оценки точности определения координат местоположения подвижного объекта с помощью автономной персональной информационно-измерительной системы………………………………………….137 ВЫВОДЫ……………………………………………………………………...143 ЗАКЛЮЧЕНИЕ……………………………………………………………….144 СПИСОК ЛИТЕРАТУРЫ…………………………………………………….145 Приложение А. Оценка функционирования пешеходной навигационной системы Оsmium MIMU22TР при движении по различным прямоугольным маршрутам……………………………………………………………………………153 Приложение Б. Кинематические схемы и системы координат……………..163 Приложение В. Программа расчета коэффициентов чувствительности погрешностей вычисления опорных углов крена и тангажа………………………171 Приложение Г. Описание и структура имитационной математической модели автономной персональной информационно-измерительной системы……………182 Приложение Д. Акт внедрения……………………………………………….227

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

6 Степень разработанности темы. Задача разработки автономных персональных ИИС решается на основе классических методов проектирования БИНС, широко представленных в работах отечественных и зарубежных авторов (А.Ю. Ишлинский, Д.С. Пельпор, Ю.Н. Осокин, Е.Р. Рахтиенко, П.В. Бромберг, В.Я. Распопов, В.В. Матвеев, Шаймарданов И.Х., Дзуев А.А., W. Li, E. Foxlin и др.). Решением задач в области инерциальной навигации занимаются ведущие предприятия Российской Федерации, такие как АО «Инерциальные технологии «Технокомплекса» (г. Раменское), ОАО «АНПП «ТЕМП-АВИА» (г. Арзамас), ФГУП «Научно-производственное объединение автоматики им. академика Н.А. Семихатова» (г. Екатеринбург), ОАО «Радиоавионика» (г. Санкт-Петербург), «Концерн «Центральный научно-исследовательский институт «Электроприбор» (г. Санкт-Петербург) и другие, а также ряд зарубежных фирм, прежде всего Honeywell (США) и Inertial Elements (Индия). Подходы, возможные для применения при построении пешеходных навигационных систем, изложены в работах как отечественных научных школ: ФГБОУ ВПО государственный Московский университет), «Тульский государственный университет систем авиационный Московский институт университет», управления Томский и радиоэлектроники, (национальный исследовательский государственный технический университет им. Н.Э. Баумана, Сибирский федеральный университет (г. Красноярск), Национальный исследовательский Томский политехнический университет, Северный (Арктический) федеральный университет им. М.В. Ломоносова, Национальный исследовательский ядерный университет «МИФИ» (Москва), Саратовский национальный исследовательский государственный университет им. Н. Г. Чернышевского, так и зарубежных: Научный исламский университет Малайзии, г. Бандар Бару Нилаи (Малайзия), Геопространственный институт Университета Ноттингема (Великобритания), 26-й институт Китайской корпорации электронных технологий (Китай), Юго-восточный университет (Китай), Технический университет Тампере (Финляндия).

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

8 - разработка способа определения углов наклона блока ДПИ относительно плоскости горизонта без накопления погрешности измерения; - оценка погрешностей определения углов наклона блока ДПИ относительно опорной поверхности; - разработка методов повышения точности работы автономной персональной ИИС наземного позиционирования; - оценка разработанных методов повышения точности определения местоположения на имитационной математической модели ИИС. Методология и методы исследования. Для решения поставленных задач применялись основные положения теоретической механики, теории погрешностей технических измерений, методы статистической обработки и оптимальной фильтрации экспериментальных данных, методы проективной геометрии, методы анализа и исследования синтеза и измерительных оценки каналов, эффективности методы ИИС, экспериментального методы математического моделирования. Полученные результаты базируются на применении основных положений общей теории БИНС и теории построения комплексных навигационных систем. Научная новизна диссертации заключается в следующем: 1) Разработан новый способ и алгоритм определения углов наклона блока ДПИ относительно как опорной плоскости, так и плоскости горизонта, новизна которых заключается в компенсации накапливающейся с течением времени погрешности в определении этих углов посредством установки на блоке ДПИ нескольких дальномерных датчиков. Научная новизна подтверждена патентом РФ №2646941. 2) Разработано новое устройство определения углов наклона блока ДПИ относительно как опорной плоскости, так и плоскости горизонта, новизна которого заключается в компенсации накапливающейся с течением времени погрешности в определении угловой ориентации блока ДПИ за счет применения схемы комплексирования инерциальной и дальномерной систем угловой ориентации (СУО). Научная новизна подтверждена патентом РФ №2649026.

9 3) Разработана имитационная математическая модель автономной персональной ИИС, позволяющая: - исследовать работу системы в процессе моделирования движения блока ДПИ с заданными изменениями его линейных и угловых координат во времени; - оценить точность определения местоположения ПО при подаче на вход модели ИИС информации, полученной с реальных датчиков в ходе выполнения натурного эксперимента. Практическая ценность работы: 1) Разработан способ, который позволяет определять углы наклона блока ДПИ на основе информации, полученной посредством лучевого сканирования опорной поверхности, что обеспечивает возможность обнуления накапливающихся погрешностей измерений углов крена и тангажа блока ДПИ. 2) Получены формулы определения углов наклона блока ДПИ, позволяющие выполнить калибровку дальномерной СУО в лабораторных условиях методами математического планирования эксперимента с применением соответствующего контрольно–измерительного оборудования и алгоритмически скомпенсировать возможные инструментальные погрешности системы. 3) Построена имитационная математическая модель автономной персональной ИИС, позволяющая решать задачи, связанные с проектированием ИИС. Положения, выносимые на защиту: 1) Научно-обоснованная техническая разработка автономной персональной ИИС наземного позиционирования с коррекцией углов наклона по опорной поверхности за счет улучшения ее технических характеристик и расширения эксплуатационных возможностей (п. 1 паспорта специальности 05.11.16). 2) Новый способ и алгоритм определения углов наклона блока ДПИ относительно плоскости горизонта (п. 6 паспорта специальности 05.11.16). 3) Новое устройство определения углов наклона блока ДПИ относительно плоскости горизонта с применением комплексирования инерциальной и

10 дальномерной СУО, позволяющее повысить точность инерциальной ИИС (п. 6 паспорта специальности 05.11.16). 4) Имитационная математическая модель автономной персональной ИИС, которая позволяет задавать программу движения ПО и блока ДПИ, формировать текущие значения угловых и линейных параметров движения объекта, производить оценку точности работы ИИС при различных режимах (п. 5 паспорта специальности 05.11.16). Степень достоверности результатов определяется: - применением адекватных математических моделей и использованием современных методов анализа информационно-измерительных систем; - использованием для построения алгоритмов данных об изменении кинематических параметров движения ПО, полученных экспериментально; - результатами оценки эффективности применения разработанной системы современными методами математического моделирования; - опытом реализации и внедрения полученных научно-технических результатов. Реализация и внедрение результатов работы. Полученные научнотехнические результаты внедрены и использованы в ООО Специальное Конструкторское Бюро «Новые Технологии» (г. Казань) в виде способа и устройства определения углов наклона блока инерциальных измерителей комплексной системы угловой ориентации относительно плоскости горизонта. Имитационная математическая модель навигационной системы прошла испытания и подтвердила свою работоспособность. Результаты внедрения подтверждены соответствующим актом. Апробация работы. Основные положения и результаты диссертации докладывались и обсуждались на Международной молодежной научной конференции «Туполевские чтения» (г. Казань, 2015, 2017 гг.), на Внутривузовской молодежной научной конференции «Иностранный язык как средство профессиональной коммуникации» (г. Казань, 2016 г.), на международной молодёжной научной конференции «Гагаринские чтения-2017» (г. Москва, 2017 г.).

11 Публикации. По теме диссертации опубликовано 7 научных работ, из них две статьи в журналах, включенных в актуальный Перечень ВАК по специальности 05.11.16, два патента РФ на изобретение, три публикации в сборниках трудов и тезисов докладов на международных конференциях. Личный вклад автора заключается в научно-техническом обосновании разработки автономной персональной ИИС наземного позиционирования с коррекцией углов наклона по опорной поверхности; в разработке новых способа и алгоритма определения углов наклона блока ДПИ относительно плоскости горизонта; в разработке нового устройства определения углов наклона блока ДПИ относительно плоскости горизонта с применением комплексирования инерциальной и дальномерной систем угловой ориентации; в разработке математической модели движения автономной персональной ИИС, в апробации, опубликовании и внедрении результатов исследования. Все теоретические и экспериментальные результаты получены автором лично, либо при его определяющем участии. Публикации, отражающие основные результаты диссертации, написаны автором лично. Патенты разработаны совместно с научным руководителем. Диссертация соответствует паспорту специальности 05.11.16 «Информационно-измерительные и управляющие системы (в приборостроении)» по пунктам: 1) «Научное обоснование перспективных информационно-измерительных … систем, … повышение эффективности существующих систем» (разработаны принципы построения автономной персональной ИИС наземного позиционирования с коррекцией углов наклона по опорной поверхности) – п.1 паспорта специальности 05.11.16. 2) «Исследование возможностей и путей совершенствования существующих и создания новых … образцов информационно-измерительных … систем, улучшение их технических, эксплуатационных … характеристик, разработка новых принципов построения и технических решений» (разработаны новые способ

12 и алгоритм определения углов наклона блока ДПИ относительно плоскости горизонта) – п.6 паспорта специальности 05.11.16. 3) «Методы анализа технического состояния, диагностики и идентификации информационно-измерительных … систем» (разработана имитационная математическая модель для оценки точности работы ИИС) – п.5 паспорта специальности 05.11.16. Структура и объем работы. Диссертационная работа состоит из введения, пяти глав, заключения, списка литературы, включающего 74 наименования, и пяти приложений. Работа без приложений изложена на 152 страницах, включая 42 рисунка и 9 таблиц.

13 Глава 1. АНАЛИТИЧЕСКИЙ ОБЗОР ПОВЫШЕНИЯ СОСТОЯНИЯ ТОЧНОСТИ ПЕРСОНАЛЬНЫХ МЕТОДОВ АВТОНОМНЫХ ИНФОРМАЦИОННО-ИЗМЕРИТЕЛЬНЫХ СИСТЕМ НАЗЕМНОГО ПОЗИЦИОНИРОВАНИЯ 1.1 Анализ требований и особенностей построения автономных персональных информационно-измерительных систем наземного позиционирования Автономная персональная информационно-измерительная система (ИИС) наземного позиционирования предназначена для вычисления координат местоположения ПО с закрепленным на нем блоком инерциальных датчиков первичной информации (ДПИ) [1] при выполнении спасательных или других работ в опасных внешних условиях без использования сигналов глобальной навигационной спутниковой системы (ГНСС) [2]. В качестве наземного ПО носителя блока ДПИ может быть человек, выполняющий оперативную работу [3], служебная собака [4] или робот [5]. Основа автономной персональной ИИС наземного позиционирования традиционная [6]. Блок-схема приведена на рисунке 1.1. По классификации видов структурных схем ИИС схема на рисунке 1.1 относится к типовым структурным схемам с параллельными измерительными каналами [7]. Блок инерциальных ДПИ ВУ1 ВУ2 Рисунок 1.1 – Блок-схема схема автономной персональной ИИС наземного позиционирования

14 Обозначения на рисунке 1.1: ДПИ - датчики первичной информации (датчики угловых скоростей, акселерометры); ВУ1, ВУ2 – вычислительные устройства, реализующие алгоритм инерциального счисления пути;     a =i ⋅ ax + j ⋅ a y + k ⋅ az - вектор кажущегося ускорения блока ДПИ;     ω = i ⋅ ωx + j ⋅ ω y + k ⋅ ωz - вектор угловой скорости блока ДПИ; ax , ay , az – нормированные сигналы акселерометров, пропорциональные  проекциям вектора a на оси связанной системы координат;  x ,ω  y ,ω z ω – нормированные сигналы датчиков угловых скоростей,  пропорциональные проекциям вектора ω на оси связанной системы координат;  , γ , ψ  ин – нормированные сигналы с выходов ВУ1, пропорциональные ϑ ин ин углам тангажа ϑ, крена γ и рысканья ψ блока ДПИ соответственно; xин , yин , zин - нормированные сигналы с выходов ВУ2, пропорциональные координатам местоположения наземного ПО в навигационной системе координат; знак «∼» обозначает нормированный сигнал, приведенный к размерности соответствующей кинематической переменной. Блок ДПИ включает в себя трехосевой датчик угловых скоростей (гироскоп) и трехосевой акселерометр, каждый из которых измеряет одну из проекций вектора абсолютной угловой скорости или ускорения блока ДПИ на оси системы координат, связанной с корпусом блока ДПИ [8]. Измеренные ускорения и угловые скорости обрабатываются с помощью алгоритма инерциального счисления пути. Для инерциального определения координат местоположения ПО необходимо [6]: - непрерывно измерять проекции ускорения ПО с помощью акселерометров; - определять с помощью гироскопов ориентацию осей чувствительности акселерометров относительно навигационной системы координат; - дважды интегрировать сигналы акселерометров по времени;

15 - знать информацию о начальных значениях координат и скоростей в навигационной системе координат. На выходе ИИС формируется вектор состояния, который содержит все данные, необходимые для определения местоположения ПО, а именно: углы рысканья, крена и тангажа, угловые скорости, координаты, линейные скорости, абсолютные ускорения блока ДПИ. К персональным ИИС для наземных ПО предъявляются следующие требования: малые массогабаритные характеристики, малая стоимость элементной базы, универсальность, полная автономность и помехозащищенность [9]. Однако автономной работе ИИС свойственно значительное накопление ошибок определения местоположения ПО с течением времени из-за наличия погрешностей в сигналах ДПИ, а также в связи с проблемами согласования периода дискретности поступления информации с ДПИ и периодом обработки поступающей информации в ВУ, возникающими при реализации алгоритма инерциального счисления пути [10, 11]. 1.2 Классификация персональных методов повышения точности автономных информационно-измерительных систем наземного позиционирования При анализе основные методы повышения точности автономных персональных ИИС наземного позиционирования, применяемые сегодня на практике, удобно разделить на две основные группы: - методы повышения точности работы ДПИ; - методы повышения точности алгоритма счисления пути [74]. В свою очередь каждую из основных групп можно разделить на подгруппы. Подгруппы методов повышения точности работы ДПИ: - методы компенсации систематических погрешностей ДПИ; - методы компенсации случайных погрешностей ДПИ путем оптимальной фильтрации; - методы комплексирования [74].

16 Подгруппы методов повышения точности алгоритма счисления пути: - методы эвристического снижения дрейфа (HDR, iHDE); - методы коррекции по нулевой скорости (ZUPT). Классификация методов повышения точности автономных персональных ИИС наземного позиционирования в общем виде представлена на рисунке 1.2. Методы повышения точности (МПТ) автономных персональных ИИС наземного позиционирования МПТ алгоритма счисления пути МПТ работы ДПИ Методы компенсации систематических погрешностей ДПИ Методы эвристического снижения дрейфа (HDR, iHDE) Методы компенсации случайных погрешностей ДПИ путем оптимальной фильтрации Методы коррекции по нулевой скорости (ZUPT) Методы комплексирования Рисунок 1.2 - Классификация методов повышения точности автономных персональных ИИС наземного позиционирования 1.3 Методы компенсации систематических погрешностей датчиков первичной информации Для компенсации систематических погрешностей ДПИ используется калибровка [12, 13]. Наиболее распространенные в лабораторных условиях методы калибровки MEMS-датчиков по измеряемым параметрам: шестипозиционный метод – six-position method (SPM), модифицированный (MSPM), многопозиционный и модифицированный многопозиционный методы – multiposition method (MPM) и MMPM соответственно [14], [15]. С помощью метода SPM можно оценить смещение нуля ДПИ и их коэффициенты чувствительности. Параметры неортогональности осей ДПИ

17 оценивают, применяя метод MSPM. В этом случае измерение проводят в 9-ти и более положениях блока ДПИ с заданной угловой ориентацией, а для оценки параметров применяют метод наименьших квадратов. Методы МРМ и ММРМ основаны на том, что при отсутствии внешних воздействий (переносных угловых скоростей и ускорений) сумма квадратов показаний трехосевых ДПИ (гироскопов или акселерометров) равна квадрату опорного воздействия. Отличие между методами состоит в том, что при калибровке методом МРМ в качестве опорных сигналов используется угловая скорость Земли и ускорение свободного падения, а при калибровке методом ММРМ для задания опорного сигнала применяется дополнительная аппаратура с контролируемым значением опорного сигнала [14, 15]. 1.4 Методы компенсации случайных погрешностей датчиков первичной информации В существующих позиционирования автономных происходит персональных значительное увеличение ИИС наземного погрешности в определении местоположения объекта во времени из-за наличия шумовых составляющих в сигналах ДПИ – датчиков угловых скоростей и акселерометров [10, 11]. Случайные погрешности инерциальных ДПИ представляют в виде суммы составляющих: шум квантования (Quantization Noise); случайный уход (дрейф) угла (Angle Random Walk); нестабильность нулевого сигнала (Bias Instability); случайный уход (дрейф) выходного сигнала (Rate Random Walk); тренд выходного сигнала (Rate Ramp); коррелированный шум (Correlated Noise); синусоидальный шум (Sinusoidal Noise) [16, 17]. Компенсация случайных погрешностей ДПИ возможна путем оптимальной фильтрации. Наибольшее применение в технических системах нашел фильтр Калмана, предназначенный для оптимального подавления измерительного шума [18]. Различают следующие разновидности фильтров Калмана: классический, адаптивный и расширенный.

18 Классический вариант фильтра Калмана представляет собой рекуррентный алгоритм, определяющий оптимальную оценку параметров динамической системы. Применяется в случае, когда шум может быть охарактеризован как гауссовский [19]. Так как характер движения ПО заранее неизвестен, что как раз характерно для реальных задач, то приходится использовать различные методы адаптации фильтров Калмана [20]. Параметры таких фильтров корректируются в соответствии с входными данными. Другими словами, алгоритм фильтра «подстраивается» к оцениваемым параметрам модели поступающей информации. Во многих случаях зависимость между данными измерений (координаты, углы, скорости) и динамическими параметрами ПО имеет нелинейный характер. Для нелинейной системы применяется расширенный фильтр Калмана (Extended Kalman Filter, EKF), который является субоптимальным алгоритмом фильтрации [21, 22]. «Расширенность» по сравнению с линейным фильтром состоит в возможности применения нелинейной модели движения ПО и/или модели измерений [23]. 1.5 Методы комплексирования Для снижения погрешностей работы ДПИ используется внешняя по отношению к инерциальным ДПИ информация и обработка этой информации по способу компенсации или фильтрации [24], в том числе с применением оптимальных (калмановских) или субоптимальных фильтров [6, 18]. В качестве внешней информации для целей комплексирования используют: - информацию нескольких инерциальных блоков ДПИ с различной ориентацией измерительных осей относительно связанной системы координат (структурная избыточность) [25-27]; - информацию от системы радиомаяков [28, 29]; - видеоинформацию от визуальных средств коррекции [30] (например, видеокамеры) [31];

19 - информацию о плане улиц (зданий) [32] по схемам «точка-точка», «точкакривая», «кривая-кривая» [33]; - информацию о параметрах атмосферы [34, 35]; - информацию о напряженности магнитного поля Земли [35, 36]; - информацию от картографической базы данных значений магнитного поля с привязкой к координатам пространства, в котором планируется движение ПО [37]; - информацию о количестве шагов на основании результата детектирования блока датчика нагрузки [38]; - информацию о виртуальной скорости ПО с учетом особенностей походки [3, 8]. На рисунке 1.3 приведены методы комплексирования, которые преимущественно используют для получения более точного результата измерений с помощью персональных ИИС наземного позиционирования [74]. Снизить влияние независимых стохастических ошибок измерения наблюдаемых параметров движения позволяет установка нескольких блоков ДПИ таким образом, что одноименные оси систем координат, связанных с блоками, образуют неортогональную систему осей. Например, установка четырех блоков ДПИ на гранях тетраэдра в приборе Osmium MIMU22BTP (рисунок 1.4) [26, 27]. Данные с ДПИ поступают в блок обработки сигналов, где происходит их согласование и калибровка. Наличие устройства обработки данных с плавающей точкой позволяет определить наиболее достоверные значения измеряемых сигналов в системе координат, связанной с тетраэдром: проекций вектора  x,ω  y ,ω z и кажущегося ускорения a x , a y , a z и проекций вектора угловой скорости ω выполнить навигационные вычисления, формирующие выходные сигналы: приращения линейных координат ( dx, dy, dz ) и угловое положение прибора Osmium MIMU22BTP в неподвижной системе отсчета, а также ковариационную матрицу ошибок.

20 Методы комплексирования Методы комплексирования нескольких инерциальных блоков ДПИ с различной ориентацией измерительных осей относительно связанной системы координат (структурная избыточность) Методы комплексирования нескольких ДПИ с различными принципами действия Метод комплексирования инерциальных ДПИ с системой радиомаяков (RTLS) Метод комплексирования инерциальных ДПИ с визуальными средствами коррекции Метод комплексирования инерциальных ДПИ с базой картографических данных местности Метод комплексирования инерциальных ДПИ с бароальтиметром Метод комплексирования инерциальных ДПИ с магнитометрами Метод «фингепринтинга Метод комплексирования инерциальных ДПИ с лучевыми дальномерами Метод комплексирования инерциальных ДПИ с датчиком нагрузки Метод комплексирования инерциальных ДПИ с виртуальным датчиком скорости Рисунок 1.3 – Методы комплексирования

21 Навигационный алгоритм вычисления угловой ориентации и траектории прибора Osmium MIMU22BTP ДПИ 2 Обработка сигналов ДПИ ДПИ 3 Приращения координат Bluetooth и/или USB ДПИ 1 Угловая ориентация Ковариационная матрица ошибок ДПИ 4 Вычислитель Osmium MIMU22BTP Рисунок 1.4 - Блок-схема прибора Osmium MIMU22BTP Методы комплексирования нескольких ДПИ с различными принципами действия: 1) Метод комплексирования инерциальных ДПИ с системой определения траектории перемещения и текущего местоположения объекта внутри помещений в реальном времени, имеющей устоявшееся название Real Time Location System (RTLS), может существенно повысить точность инерциального счисления пути, если на объекте проведения оперативных работ развернута инфраструктура RTLS [29]. Основу RTLS составляют базовые радиоэлектронные станции, размещенные в реперных точках с фиксированными координатами и объединенные сетью передачи данных серверному программному обеспечению. Базовые радиоэлектронные станции взаимодействуют с активной радиоэлектронной меткой, прикрепленной к ПО, а серверное программное обеспечение по поступающей от базовых радиоэлектронных станций информации вычисляет координаты метки относительно реперных точек и накапливает полученные данные. Эту внешнюю по отношению к ИИС информацию можно комплексировать с данными инерциального счисления пути по методу компенсации или фильтрации.

22 2) Метод комплексирования инерциальных ДПИ с визуальными средствами коррекции заключается в размещении на торсе ПО блока Visual Compass, в который входят лазерный сканер, монокулярная камера и дополнительные инерциальные ДПИ, отслеживающие динамические движения этого блока [31]. Внешняя информация об ориентации блока Visual Compass извлекается из изображений, снятых видеокамерой. При обработке изображений по технологии компьютерного зрения используется факт наличия в зданиях множества элементов с перпендикулярными или параллельными краями (стены, двери, окна). Применение видеокоррекций параметров ориентации значительно улучшает точность оценок местоположения и ориентации. Кроме того, по изображениям рассчитываются и сохраняются в базе данных вместе с соответствующими местоположениями векторы-дескрипторы. При перемещении по зданию камера снимает изображения окружающего пространства и происходит извлечение дескрипторов. При этом осуществляется быстрое сравнение снимаемого камерой изображения с базой данных на предмет обнаружения повторного прохождения данного участка здания с целью коррекции местоположения ПО. Комплексирование данных измерения ориентации и местоположения ПО, полученных на выходах ИИС и блока Visual Compass, выполняют на основе фильтра Калмана или на основе робастной оптимизации с использованием графов. 3) Метод комплексирования инерциальных ДПИ с базой картографических данных местности представлен в работе [33]. Данный метод оценивает ошибки дрейфа гироскопа, сравнивая курс, полученный от ДПИ, с ближайшим доминантным направлением улицы или коридора в здании в картографической базе данных. Существует несколько алгоритмов согласования курса ПО с картографическими данными, построенных на базе метода максимального правдоподобия: «точка-точка», «точка-кривая», «кривая-кривая». Достоинство метода в том, что он устойчив к кратковременным отклонениям ПО от выбранного направления движения. Такие отклонения обычно происходят в случаях, когда ПО уклоняется от другого ПО, пересекает улицу или поворачивается. Трудности использования метода согласования с картой в

23 помещении связаны с тем, что траектория объектов в помещении не всегда совпадает с геометрией картографических данных. 4) Метод комплексирования инерциальных ДПИ с бароальтиметром предназначен для повышения точности определения высоты ПО относительно опорной поверхности. Современные миниатюрные бароальтиметры с цифровым выходом имеют чувствительность 0,02 мбар (2 Па), что обеспечивает при комплексировании их с ИИС определение высоты местоположения с погрешностью менее 1 м [34, 35]. 5) Метод комплексирования инерциальных ДПИ с магнитометрами используется для решения проблем повышения точности определения курса [35, 36]. Гироскопические измерения рыскания, содержащие достаточно большие ошибки, комплексируются с абсолютными курсовыми измерениями от магнитометров по способу компенсации или фильтрации. При комплексировании по способу фильтрации используют фильтр Калмана для оценивания ошибок рыскания, что улучшает точность выработки курса. 6) Метод «фингепринтинга» (fingerprinting) использует картографическую базу данных значений магнитного поля с привязкой к координатам, полученную в результате предварительного сканирования [37]. При измерении напряженности магнитного поля с помощью магнитометров происходит сравнение показаний магнитометров с картографической базой данных для уточнения местоположения ПО. 7) В патентном документе [38] приводится информация о методе получения информации о нулевой скорости на базе шагомера. Устройство включает в себя блок датчика нагрузки, который располагается в каблуке обуви, и измерительный блок, который измеряет количество шагов на основании результата детектирования. 8) Метод, учитывающий особенности походки человека, по мнению авторов литературных источников [3, 8], позволяет существенно уменьшить погрешности вычисления горизонтальных проекций вектора скорости ПО и, соответственно, уменьшить погрешности определения координат ПО относительно навигационной

24 системы координат. При вычислении проекций скорости используется информация о динамике походки человека, а вычисления выполняют с периодичностью, соответствующей не фиксированным интервалам времени, а частоте шагов. Для определения времени, соответствующего началу и окончанию шага, а также для вычисления длины и направления шага используются показания инерциальных ДПИ. Оценку средней скорости ПО на шаге получают путем деления длины шага на его длительность. Эту скорость используют в качестве внешнего измерения скорости для персональной ИИС. Путем комплексирования инерциальных ДПИ с кинетической моделью походки, а также используя модель аппроксимации погрешностей ускорений, вызванных ошибками горизонтирования, осуществляют снижение скоростных ошибок ИИС. Анализируя методы комплексирования ДПИ персональной ИИС при движении ПО внутри здания, приходим к выводу, что среди известных методов комплексирования нет метода, использующего в качестве внешней информации угловую ориентацию блока ДПИ относительно опорной поверхности, например, потолка. Эту информацию можно получить, например, с использованием миниатюрных современных лучевых дальномеров, установив их определенным образом на блоке ДПИ. На рисунке 1.5 название данного метода помещено в штриховую рамку. Теоретические основы данного метода комплексирования изложены в главе 3. 1.6 Методы повышения точности алгоритма счисления пути Для повышения точности алгоритма счисления пути применяют следующие методы (рисунок 1.5): - методы эвристического снижение дрейфа; - методы коррекции по нулевой скорости (ZUPT) на базе информации от инерциальных ДПИ.

25 МПТ алгоритма счисления пути Методы эвристического снижения дрейфа Метод HDR Методы коррекции по нулевой скорости (ZUPT) на базе алгоритмов: Алгоритм SHOE Метод iHDE Алгоритм MV Алгоритм MAG Алгоритм ARE Алгоритм на базе цепей Маркова Рисунок 1.5 - Методы повышения точности алгоритма счисления пути К методам эвристического снижения дрейфа относятся методы Heuristic Drift Reduction (HDR) и Improved Heuristic Drift Elimination (iHDE). Метод HDR учитывает тот факт, что многие улицы и коридоры частично имеют прямолинейную форму [39]. Упрощающее допущение о прямолинейности траектории ПО позволяет корректировать измеряемую угловую скорость блока ДПИ относительно вертикальной оси и угол направления шага в базовой системе отсчета, определяемый с учетом скорректированной угловой скорости. Применение двойной частотной фильтрации сигнала ДУС обеспечивает вполне эффективное сглаживание колебаний угловой скорости на доминантной частоте 1 Гц и выходного шума ДУС, который характерен для ходьбы. Скорректированная угловая скорость имеет существенное запаздывание по сравнению с кинематической переменной, вызванное действием двойного низкочастотного фильтра. Если пренебречь этой проблемой, то траектории крутых поворотов на угол ±90° строятся по расчетам как длинные кривые. Поэтому для компенсации запаздывания полезного сигнала на выходе системы установлено звено, реверсирующее эффект двойного низкочастотного фильтра.

26 Однако метод HDR имеет существенный недостаток, если алгоритм используется в зданиях сложной формы с криволинейными коридорами. В этом случае возможно даже ухудшение качества работы персональной ИИС. Указанный недостаток метода HDR устранен в iHDE [40]. Предлагаемая реализация метода iHDE предусматривает наличие двух дополнительных алгоритмов. Первый алгоритм, анализирующий движение, определяет направление большого шага и принимает решение об изогнутости траектории. Это решение формируется в виде двоичных кодов SS и SLP. Параметр SS контролирует длину шага, а параметр SLP – изгиб линии пути на основе анализа данных по курсу за 5 последних шагов. При коротком шаге и увеличении угла направления шага свыше некоторого граничного значения параметры SS и SLP принимают нулевые значения, отключающие коррекцию HDR. Второй алгоритм вычисляет абсолютную ошибку по курсу относительно доминантного направления, определенного селектором доминантного направления на основании данных вычисления угла курса, полученного путем комплексирования алгоритма инерциального счисления пути и информации от магнитометров, а также среднеквадратическую ошибку по углу курса. Выходные сигналы второго алгоритма поступают на вход расширенного фильтра Калмана для последующей коррекции маршрута и дрейфа гироскопа. Примечательно, что коррекция типа HDR в методе iHDE работает в случае, если доминантная траектория точно прямолинейна. Этим метод iHDE отличается от метода HDR, который всегда использует коррекцию, даже при изогнутых траекториях. Метод ZUPT на базе информации от инерциальных ДПИ использует данные о шагах ПО, проводя коррекции по нулевой скорости поступательного движения блока ДПИ [22, 31, 41-43]. В этом случае в алгоритм персональной ИИС определения местоположения ПО добавляется алгоритм обнуления скорости в момент завершения шага. Известны четыре алгоритма обнуления скорости [42]:

27 - алгоритм оптимального прогнозирования положения (The Stance Hypothesis Optimal Detector, SHOE); - алгоритм анализа дисперсии ускорения при изменении местоположения (The Acceleration Moving Variance Detector, MV); - алгоритм анализа амплитуды ускорения (The Acceleration Magnitude Detector, MAG); - алгоритм анализа интенсивности угловой скорости (The Angular Rate Energy Detector, ARE). Значения выходных сигналов, вычисленных по перечисленным выше алгоритмам обнуления скорости, сравниваются с так называемой пороговой границей, определяемой как 10х, где показатель степени х для каждого алгоритма индивидуален. Так для алгоритма SHOE х=4,7, для алгоритма MV х=3,5, для алгоритма MAG х=3,3, для алгоритма ARE х=4,6 [42]. Если результат меньше данной границы, то принимается решение, что ПО неподвижен (происходит обнуление скорости), если больше – то ПО движется. Такая коррекция заметно повышает точность определения местоположения, однако ошибки в определении курса сохраняются значительными. В результате исследования реальной системы Osmium MIMU22TP (Индия), где использован метод коррекции по нулевой скорости, показано, что погрешность счисления пути составила 26,6 %, что является недостаточным для длительного применения (приложение А). Для оценки эффективности работы алгоритмов MV, MAG, ARE, SHOE с их помощью выполнена обработка выходных сигналов блока ДПИ при двух способах его закрепления на ПО – на поясе и на обуви [43]. Результаты эксперимента показали, что алгоритмы SHOE и ARE имеют меньшую погрешность оценки состояния движения оператора как при креплении блока ДПИ на поясе ПО, так и на его обуви. Алгоритмы MV и MAG показали недопустимо большой процент погрешности, особенно при закреплении блока ДПИ на поясе. При этом эксперимент показал, что вклад алгоритма ARE, который является составной частью алгоритма SHOE, содержащего кроме информации по угловым скоростям

28 информацию по ускорениям, - в оценку состояния движения оператора более весом при закреплении блока ДПИ на обуви. При изменении темпа движения (быстрая ходьба, бег) погрешность определения местоположения ПО по алгоритмам ZUPT возрастает. С целью уменьшения этой погрешности авторы Park S. K., Suh Y.S. предлагают еще один вариант использования данных о шагах ПО, моделируя процесс выполнения шага цепью Маркова с четырьмя состояниями: 1 – подошва обуви с ДПИ неподвижна, 2 – подошва вращается по часовой стрелке (вид справа), начиная шаг, 3 – подошва вращается против часовой стрелки при маховом движении шагающей ноги вперед, 4 – подошва вращается по часовой стрелке при завершении шага и переходе в состояние 1 [44]. Смена состояний определяется путем сравнения показаний датчика угловой скорости с граничными областями допустимых значений, характерных для каждого из состояний цепи Маркова. При этом большую роль играет правильный выбор значений элементов матрицы вероятностей перехода цепи из одного состояния в другое, который варьируется в зависимости от темпа движения. Следует отметить, что при формировании сигнала с ДУС часть информации, относящаяся к промежуточным состояниям ДПИ, не принадлежащим состояниям цепи Маркова, не учитывается при оценке перехода цепи из одного состояния в другое. 1.7 Сравнительная характеристика методов повышения точности автономных персональных информационно-измерительных систем Сравнительная характеристика методов компенсации систематических погрешностей ДПИ приведена в таблице 1.1. Таблица 1.1 - Сравнительная характеристика методов компенсации систематических погрешностей ДПИ Название метода Калибровка ДПИ методом SPM Решаемая задача Компенсации систематических погрешностей ДПИ Достоинства Недостатки Позволяет оценить смещение Необходимость точного нуля ДПИ и их коэффициенты задания известной чувствительности с помощью угловой ориентации осей простых соотношений

29 Название метода Калибровка ДПИ методом MSPM Решаемая задача Достоинства Компенсации систематических погрешностей ДПИ Недостатки блока ДПИ относительно Позволяет оценить параметры системы отсчета неортогональности осей ДПИ Требует большого числа измерений для обеспечения заданной точности определения параметров. Применим для калибровки только высокоточных ИИС определения местоположения ПО Калибровка ДПИ методом MPM Компенсация систематических погрешностей ДПИ В каждом из п различных положений блока ДПИ его угловая ориентация относительно заданной системы отсчета произвольна. Не требует дополнительной аппаратуры для задания опорного сигнала Калибровка ДПИ методом MMPM Компенсации систематических погрешностей ДПИ Применим для калибровки Требует дополнительной ДПИ среднего и низкого класса аппаратуры для задания точности, к которым относятся опорного сигнала ДПИ на базе MEMS Сравнительная характеристика методов компенсации случайных погрешностей ДПИ приведена в таблице 1.2. Таблица 1.2 - Сравнительная характеристика методов компенсации случайных погрешностей ДПИ Название метода Решаемая задача Классический фильтр Калмана Компенсация шумовых параметров ДПИ Адаптивный фильтр Калмана Компенсация шумовых параметров ДПИ Расширенный фильтр Калмана Компенсация шумовых параметров ДПИ Достоинства Недостатки Необходимо иметь Позволяет получить подробное описание оптимальную оценку шумовых параметров ДПИ, полного вектора состояния, оцениванию доступны только т. е. оценку основных наблюдаемые компоненты ошибок ИИС вектора состояния Позволяет получить оптимальную оценку полного вектора состояния ИИС. Фильтр стремится Требуется время на компенсировать адаптацию влияние системы и восстановить исходный сигнал, устранив внесенные системой искажения Коэффициент усиления фильтра не может быть вычислен априорно. Все Применим для нелинейной параметры фильтра системы изменяются случайным образом, так как являются функциями оценки.

30 Сравнительная характеристика методов комплексирования нескольких ДПИ приведена в таблице 1.3. Таблица 1.3 - Сравнительная характеристика методов комплексирования нескольких ДПИ Название метода Структурная избыточность ДПИ Метод комплексирования ИИС с RTLS Метод комплексирования ИИС с визуальными средствами коррекции Метод комплексирования ИИС с базой картографических данных местности Метод комплексирования ИИС с бароальтиметром Метод комплексирования ИИС с магнитометрами Метод «фингепринтинга» Метод ZUPT на базе шагомера Решаемая задача Достоинства Снижение влияния независимых стохастических ошибок измерения наблюдаемых параметров движения Повышение надежности Коррекция местоположен ия и параметров ориентации Коррекция местоположен ия и параметров ориентации Коррекция ошибок положения курса Высокая точность Недостатки Увеличение массогабаритных характеристик, что не является существенным в виду миниатюрности конструкции ДПИ Требует специальной инфраструктуры (радиомаяки, Wi-Fi роутеры, ультразвуковые передатчики и т.д.) Высокая надежность Дополнительное определения оборудование доминантного видеокамера направления движения - Необходима карта Более точная коррекция местности с траектории в сравнении с и координатами опорных методами HDR и iHDE точек и направлений Необходимость Коррекция Бароальтиметр входит в тарировки показаний высоты состав большинства ДПИ прибора перед каждым применением Повышение Искажение измерений точности Магнитометры входит в магнитометров при определения состав большинства ДПИ наличии магнитных курса возмущений Картографическая база данных значений Коррекция Снижение ошибок магнитного поля должна местоположен определения быть составлена заранее. ия местоположения Коррекция по курсу не производится. Возможность комплексирования с Коррекция Необходимость другими методами ZUPT. фазы специальной установки Использование датчика детектировани датчика нагрузки на нагрузки в качестве я шага обуви генератора электрической энергии для питания ИИС

31 Название метода Учет особенностей походки человека Решаемая задача Ограничение роста погрешностей при вычислении углов ориентации и горизонт. составляющих скорости Достоинства Недостатки Не позволяет Возможность корректировать расположения ДПИ на погрешность курса. талии или на торсе. Увеличение Снижение скоростных погрешности при ошибок ИИС изменении динамики походки Сравнительная характеристика методов повышения точности алгоритма счисления пути приведена в таблице 1.4. Таблица 1.4 - Сравнительная характеристика методов повышения точности алгоритма счисления пути Название метода Метод HDR Метод iHDE Решаемая задача Коррекция ошибок положения и курса при движении по прямолинейной траектории Коррекция ошибок положения и курса при движении по прямолинейной траектории Метод ZUPT на базе алгоритма SHOE, ARE Уменьшение ошибки вычисления скорости по сигналам акселерометров Метод ZUPT на базе алгоритма MV, MAG Уменьшение ошибки вычисления скорости по сигналам акселерометров Метод ZUPT на базе алгоритма, использующего цепи Маркова Уменьшение ошибки вычисления скорости по сигналам акселерометров Достоинства Недостатки Простота реализации Увеличение погрешности из-за невозможности отключения коррекции в случае изогнутых траекторий Возможность отключения коррекции в случае изогнутых траекторий Работает только при наличии информации о доминантных направлениях возможного движения Повышение точности определения местоположения. Ошибки по курсу сохраняются Возможность установки значительными блока ДПИ на поясе или на обуви Ошибки по курсу сохраняются значительными. Увеличение погрешности определения Повышение точности местоположения ПО при определения изменении темпа его движения. местоположения. Установка ДПИ только на обуви. Большой процент погрешности по сравнению с алгоритмами SHOE, ARE. Повышение точности определения местоположения. Ошибки по курсу сохраняются Погрешность значительными. Установка определения ДПИ только на обуви. местоположения ПО не зависит от темпа его движения

32 ВЫВОДЫ И ПОСТАНОВКА ЗАДАЧИ НАУЧНОГО ИССЛЕДОВАНИЯ 1. Автономной работе ИИС свойственно значительное накопление ошибок определения местоположения ПО с течением времени из-за наличия погрешностей в сигналах ДПИ, а также в связи с проблемами согласования периода дискретности поступления информации с ДПИ и периодом обработки поступающей информации в ВУ, возникающими при реализации алгоритма инерциального счисления пути. Систематизированы и изучены существующие подходы, позволяющие повысить точность персональных ИИС без использования сигналов спутниковой навигации. Выполнена их классификация с выявлением достоинств и недостатков (таблицы 1.1 – 1.4), позволяющая провести сравнительную оценку методов с точки зрения их практического применения. 2. Анализ рассмотренных методов показал, что погрешности в определении местоположения складываются из двух составляющих: это погрешность определения угловой ориентации блока ДПИ и погрешность, связанная с интегрированием скорости движения объекта. Все рассмотренные методы в основном направлены на компенсацию погрешности, связанной с интегрированием скорости. Существующие подходы, касающиеся компенсации погрешностей определения угловой ориентации блока ДПИ, являются на сегодняшний день недостаточными. Актуально рассмотреть способ определения углов наклона блока ДПИ относительно плоскости горизонта с помощью дополнительных датчиков, использующих неинерциальный принцип измерения, свободных от радиотехнических помех и допускающих по своим геометрическим параметрам компактную установку на блоке ДПИ. Таким образом, постановка задачи заключается в научно-обоснованной технической разработке автономной персональной ИИС наземного позиционирования с коррекцией углов наклона по опорной поверхности. 3. Решение поставленной задачи научного исследования проводится по следующим основным направлениям:

33 - аналитический обзор состояния методов повышения точности автономных персональных ИИС наземного позиционирования; - разработка способа определения углов наклона блока ДПИ относительно плоскости горизонта без накопления погрешности измерения; - оценка погрешностей определения углов наклона блока ДПИ относительно опорной поверхности; - разработка методов повышения точности работы автономной персональной ИИС наземного позиционирования; - оценка разработанных методов повышения точности определения местоположения на имитационной математической модели ИИС.

34 Глава 2. РАЗРАБОТКА СПОСОБА ОПРЕДЕЛЕНИЯ УГЛОВ НАКЛОНА БЛОКА ДАТЧИКОВ ОТНОСИТЕЛЬНО ПЕРВИЧНОЙ ПЛОСКОСТИ ИНФОРМАЦИИ ГОРИЗОНТА БЕЗ НАКОПЛЕНИЯ ПОГРЕШНОСТИ ИЗМЕРЕНИЯ 2.1 Постановка задачи При выполнении оперативных работ строительные конструкции здания (пол, потолок, стены и т.д.) могут служить вспомогательной системой отсчета для альтернативного определения угловых координат блока ДПИ, используемых для повышения точности определения координат местоположения ПО. В данной главе предлагается способ использования системы отсчета такого рода. Для реализации этого способа необходимо установить на блоке ДПИ несколько датчиков (дальномеров - ДМ), измеряющих расстояния от мест их установки до опорной поверхности, измерительные оси которых ориентированы определенным образом относительно системы координат, связанной с блоком ДПИ. Поскольку результаты измерения ДМ зависят от углового положения блока ДПИ относительно опорной поверхности, ставилась задача построения алгоритма определения углов наклона блока ДПИ по результатам измерений ДМ. Основные подзадачи, которые при этом требуется решить: - обзор датчиков расстояния (дальномеров); - построение схемы дополнительной системы угловой ориентации (СУО) с ДМ; - построение математической модели расстояния до опорной горизонтальной поверхности; - построение алгоритма определения опорных углов наклона блока ДПИ относительно опорной горизонтальной поверхности для произвольного числа ДМ; - получение аналитических выражений опорных углов наклона блока ДПИ с использованием показаний трех и четырех ДМ; - построение математической модели расстояния до опорной поверхности, плоскость которой имеет уклон по отношению к плоскости горизонта;

35 - построение алгоритма определения углов уклона опорной поверхности относительно плоскости горизонта; - построение алгоритма определения углов наклона блока ДПИ относительно плоскости горизонта. 2.2 Обзор датчиков расстояния (дальномеров) Для измерения расстояний до объектов, координаты которых нас интересуют, используются датчики расстояний (дальномеры) [45]. Все они делятся на 2 типа: контактные и бесконтактные. Контактный метод основан на непосредственном контакте датчика с объектом [46]. Более универсальным является бесконтактный метод. По принципу работы бесконтактные датчики делятся на индуктивные, оптические, ультразвуковые и лазерные. Все они имеют электрический выходной сигнал, величина которого пропорциональна измеряемому расстоянию до объекта [47]. Индуктивные ДМ применяются для измерения расстояний до металлических объектов. Диапазон измерения: 1…26 мм [48]. Принцип измерения расстояний оптическим дальномером основан на решении прямоугольных либо равнобедренных треугольников, которые образуются между глазом наблюдателя и базисом дальномера (рейкой). Такие дальномеры применяются, в основном, в наблюдательных приборах и служат для измерения дистанции с небольшой точностью, ошибка при этом не менее 10% [45]. Ультразвуковой ДМ работает путем направления звукового сигнала на какой-то предмет, который, в свою очередь, отражает его. Факторы, влияющие на точность измерений ультразвуковым ДМ [49]: 1) Наличие паразитных эхо-сигналов: поскольку измерения проводятся в неидеальных условиях, отражение происходит не только от интересующего объекта. Отражение от вторичных целей, рельефа местности, подстилающей поверхности, а также вторичные переотражения порождают целый ряд сторонних сигналов.

36 2) Форма и материал отражающих объектов. 3) Частота: повышение частоты увеличивает разрешающую способность системы, но также увеличивается и затухание сигнала от расстояния. Тем более с ростом частоты возрастает влияние ветра на звуковую волну. 4) Сторонние помеховые сигналы, лежащие в рабочей области частот приемника. Ввиду перечисленных причин, малый по мощности полезный отраженный сигнал может быть пропущен. В то же время сильная помеха может быть воспринята в качестве искомого ответа. В связи с этим использовать ультразвуковые приборы можно в том случае, когда не требуется точных замеров [50]. В настоящее время высокоточное определение расстояния осуществляется с помощью лазерных дальномеров, которые подразделяются на [51]: 1) лазерные импульсные дальномеры [52], определяющие дальность по времени распространения лазерного импульса до объекта и обратно; 2) лазерные фазовые дальномеры [53], измеряющие дальность путем определения сдвига фазы гармонически модулированного оптического излучения лазера или светодиода по отношению к опорному колебанию. Точность измерения дальности импульсным ДМ определяется точностью измерения времени прохождения импульса энергии до объекта и обратно и составляет 0,1-0,2 м [54, 55]. Лазерные фазовые ДМ в отличие от импульсных обладают существенно меньшей дальностью измерения, но при этом гораздо большей точностью. Погрешность измерения расстояния ∆l не превышает 1 мм и определяется формулой [56]: ∆l =c ∆ϕ , 4π f где с – скорость света, м/с; ∆ϕ - погрешность разности фаз измеряемого и опорного сигналов; f – частота модуляции, Гц. (2.1)

37 Использование лазерных ДМ выбрано в качестве приоритетного. Данные датчики являются миниатюрными [57], что позволяет разместить их на блоке ДПИ, закрепленном на ПО. 2.3 Построение схемы дополнительной системы угловой ориентации с дальномерами Предлагается схема дополнительной СУО с ДМ, показанная на рисунке 2.1. Yоп Yg2 Опорная поверхность Xоп λz Xg2 О0* Плоскость горизонта Zg2 λx Zоп Li Блок ДМ L1 LN ВОУ Блок инерциальных ДПИ ВУУ U0 U0 К1 В1 В2 ЗУ ВУ2 ВУ1 Рисунок 2.1 – Схема дополнительной СУО с ДМ Обозначения на рисунке 2.1: Li ( ϑ, γ, ψ ) , i ∈1, N – расстояния до опорной поверхности, измеряемые ДМ;     a =i ⋅ ax + j ⋅ a y + k ⋅ az - вектор кажущегося ускорения блока ДПИ;

38     ω = i ⋅ ωx + j ⋅ ω y + k ⋅ ωz - вектор угловой скорости блока ДПИ; L1 , L2 ,..., LN – нормированные сигналы на выходах ДМ, пропорциональные соответствующим расстояниям до опорной поверхности; ax , ay , az сигналы на выходах акселерометров,  пропорциональные проекциям вектора a на оси связанной системы координат; – нормированные  x ,ω  y ,ω z ω – нормированные сигналы на выходах датчиков угловых  скоростей, пропорциональные проекциям вектора ω на оси связанной системы координат; ВОУ – вычислитель опорных углов, формирующий на выходе оценки углов опорного тангажа и опорного крена;  , γ – нормированные сигналы с выходов ВОУ, пропорциональные углам ϑ оп оп опорного тангажа ϑ оп и опорного крена γ оп , определяющим угловое положение блока ДПИ относительно негоризонтальной (в общем случае) опорной поверхности; ВУ1– вычислительное устройство, реализующее алгоритм инерциального вычисления углов ориентации блока ДПИ относительно неподвижной горизонтальной системы координат;  , γ , ψ  ин – нормированные сигналы с выходов ВУ1, пропорциональные ϑ ин ин углам тангажа ϑ, крена γ и рысканья ψ соответственно; ВУУ – вычислитель углов уклона λ z , λ x относительно плоскости горизонта; В1 – вычислитель углов тангажа ϑ а и крена γ а блока ДПИ по показаниям акселерометров при детектировании неподвижного состояния ПО; В2 – вычислитель углов уклона опорной поверхности относительно плоскости горизонта при детектировании неподвижного состояния ПО; * , λ  * - нормированные сигналы с выходов В2, пропорциональные углам λ z x уклона опорной поверхности λ z , λ x относительно плоскости горизонта в момент детектирования неподвижного состояния ПО;

39 ЗУ – запоминающее устройство, сохраняющее в процессе движения на своем выходе сигналы углов уклона опорной поверхности относительно плоскости  ,λ  , полученные в момент детектирования неподвижного состояния горизонта λ z х ПО;  ,λ  – нормированные сигналы с выходов ВУУ, пропорциональные углам λ z х уклона опорной поверхности λz, λx относительно плоскости горизонта, вычисленным в момент детектирования неподвижного состояния ПО; ВУ2 – вычислительное устройство, определяющее опорные углы отклонения блока ДПИ относительно плоскости горизонта;  , γ – нормированные сигналы с выходов ВУ2, пропорциональные углам ϑ дм дм опорного тангажа ϑдм и опорного крена γдм относительно плоскости горизонта. Принцип работы дополнительной СУО с ДМ заключается в следующем. В контрольных точках траектории ПО при неподвижном положении блока ДПИ ключ К1 замыкает линии связи с выходов акселерометров на входы вычислителя В1 блока ВУУ по сигналу U0. По сигналам акселерометров в вычислителе В1 определяются углы блока ДПИ относительно плоскости горизонта  , γ , информация о которых поступает на вход вычислителя В2. Сюда же ϑ а а  , γ с выхода блока ВОУ и сигнал по углу поступают значения опорных углов ϑ оп оп  ин с выхода ВУ1. Алгоритм, реализованный в блоке В2, на основе рысканья ψ поступившей информации вычисляет углы уклона опорной поверхности * , λ * , относительно плоскости горизонта, формируя на выходе сигналы λ z x поступающие на вход запоминающего устройства ЗУ. При размыкании ключа К1  ,λ  до следующей контрольной точки. на выходе блока ЗУ сохраняются сигналы λ z х Если опорная поверхность имеет уклон относительно плоскости горизонта, то результаты измерения дальностей зависят не только от углов отклонения блока ДПИ относительно плоскости горизонта, но и от азимутальной ориентации блока ДПИ относительно неподвижной системы координат, т.е. от угла рысканья. При

40  , γ на входы блоков ВУУ и ВУ2  ,λ  и углов ϑ вычислении углов уклона λ дм дм z х  ин , вычисленному в должна также поступать информация и по углу рысканья ψ блоке ВУ1. Алгоритм, реализованный в блоке ВУ2, на базе поступающей в блок  , γ и углах λ  ,λ  уклона опорной поверхности, информации об опорных углах ϑ оп оп z х вычисляет опорные углы блока ДПИ относительно плоскости горизонта, формируя  , γ . на выходе сигналы ϑ дм дм Для получения аналитических выражений углов наклона блока ДПИ с использованием показаний ДМ применены кинематические схемы и системы координат, приведенные в приложении Б. Матрицы, входящие в схемы, составляются в соответствии с понятием однородных координат проективного пространства [58, 59], согласно которому различные преобразования в трехмерном пространстве могут быть сведены к двум видам – вращению и переносу. Для компактного представления этих преобразований и связей между системами координат используются схемы в виде направленного графа. В вершинах графа располагаются системы координат, а на ребрах графа выносится информация о последовательности поворотов и об используемых углах поворота или о параллельном переносе систем координат и радиусе-векторе, определяющем координаты нового полюса [60]. Методика преобразования координат с применением формул (Б.1) – (Б.14), приведенных в приложении Б, используется для получения математических выражений при поэтапном решении поставленных задач в следующих разделах главы, вначале для частного случая с горизонтальной опорной поверхностью, а затем – при наличии уклона опорной поверхности относительно плоскости горизонта.

41 2.4 Построение математической модели расстояния до опорной горизонтальной поверхности Задача построения математической модели расчета расстояний, измеряемых ДМ, установленными на блоке ДПИ, до опорной горизонтальной поверхности решается с использованием теории однородных координат проективного пространства [58]. Исходными данными для решения задачи являются параметры кинематической модели движения ПО и геометрические параметры расположения опорной поверхности. Расстояние Li, измеряемое от места установки ДМ (от точек Odi) до точек Ki пересечения положительных полуосей Xdi с горизонтальной опорной поверхностью определяется по формуле [61]: Li = (x Ki g − xgOdi ) +(y 2 Ki g − ygOdi ) + (z 2 Ki g − z gOdi ) 2 , (2.2) где xgK , y gK , z gK – координаты точки Кi в системе O0XgYgZg; i i i xgOdi , y gOdi , z gOdi – координаты точки Odi в системе O0XgYgZg. Координаты точки Odi в нормальной земной системе координат O0XgYgZg определяются как координаты радиуса – вектора R O O = [ xgO di 0 di Координаты точки Odi в системе ОсXcYcZc: y gOdi z gOdi ]т . R OcOdi = [ xdi ydi zdi ] . т Однородные координаты точки Odi в системе ОсXcYcZc определяют вектор RO O  rOcOdi =  c di  . Для определения искомого вектора rO0Odi однородных координат  1  точки Odi в системе O0XgYgZg используется формула преобразования координат (приложение Б): = rO0Odi M = M g , c ⋅ rOcOdi , g , g 1 ⋅ M g 1,c ⋅ rOcOdi (2.3) = M g , g1 ⋅ M g1,c . где M g,c Выражение для матрицы M g ,c : M g , c = M g , g1 ⋅ M g1,c E = G 0 R O0Oc  C g1,c ⋅ 1   G0 G 0т  C g1,c = 1   G0 R O0Oc  ; 1  (2.4)

42 При подстановке выражения (2.4) в формулу (2.3) получается: R O0Oc   R OcOdi  C g1,c R OcOdi + R O0Oc   R O0Odi  = = ⋅     . 1   1   1 1    C g1,c rO0Odi =   G0 (2.5) Таким образом, координаты точки Odi в системе координат O0XgYgZg определяются формулой:  xgOdi   xdi   xg   Odi  y  + y  . C g c O 1,  y g=  C g1,c R OcOdi + R O= 0 c  di   g  Odi  zg   zdi   z g    (2.6) Координаты точки Кi в системе координат O0XgYgZg находятся из уравнений  проекций вектора RO di K i на координатные плоскости XgO0Yg, YgO0Zg (рисунок 2.2). Yg K”i K’i Ki Odi O0 Xg Zg  Рисунок 2.2 – Проекции орта idi измерительной оси i – того дальномера на координатные плоскости нормальной земной системы координат

43 Эти уравнения записываются как уравнения прямых, проходящих через проекции Odi' , Odi" точки Odi с угловыми коэффициентами, определяемыми     отношениями проекций xgidi , y gidi , z gidi орта idi оси Xdi.  y= y gOdi + y gidi x  idi g  ( x − x ), y= y gOdi + Odi g y gidi z  idi g (z − z ). Odi g (2.7) При подстановке в уравнения (2.7) вместо переменной у вертикальной координаты точки Кi, равной высоте помещения h0, горизонтальные координаты Ki = x x= z gKi точки Кi в нормальной земной системе координат определяются g , z формулами: xgKi = xgOdi + x y  idi g  idi g (h − y ), z gKi = z gOdi + Odi g 0 z  idi g  idi g y (h − y ) . 0 Odi g (2.8)  Проекции орта idi на оси системы координат O0XgYgZg получены из уравнения:   xgidi  = C g ,di C= где g 1,c Cc1,di   (с ) i kj 3×3 т z gidi  = C g ,di ⋅ [1 0  y gidi i 0] = c11 т i c21 т i  , c31 (2.9) ; i c11i , c21 , c31i – элементы матрицы направляющих косинусов C g ,di , определяемые по формулам: c11i = sin σi ( sin γ sin ψ − cos γ cos ψ sin ϑ) + cos ψ cos σi cos µi cos ϑ −   − cos σi sin µi ( cos γ sin ψ + cos ψ sin γ sin ϑ ) ;   i c21 = sin ϑ cos σi cos µi + cos ϑ sin γ sin µi cos σi + cos γ cos ϑ sin σi ;   i c31 = sin σi ( cos ψ sin γ + cos γ sin ψ sin ϑ) − cos σi cos µi cos ϑ sin ψ −   − cos σi sin µi ( cos γ cos ψ − sin γ sin ψ sin ϑ) .  (2.10) Формулы (2.8) после подстановки в них выражений (2.9) примут вид: ( ) c11i x = x + i h0 − y gOdi , c21 Ki g Odi g ( ) c31i z = z + i h0 − y gOdi . c21 Ki g Odi g (2.11)

44 Величина дальности Li определяется после подстановки в (2.2) координат (2.11). 2 Li = = (h 0  c11i  Odi  i  h0 − y g  c21  −y i c21 Odi g ) ( ) + (h 2 0 − y gOdi ) 2 2 i 2  c31  di +  i  h0 − y gO=  c21  ( ) (2.12) ( с11i ) + ( с21i ) + ( с31i ) . 2 2 2 При вычислении расстояния (2.12), измеряемого ДМ от точки Odi до опорной i должен иметь поверхности верхней (ОПВ - потолок), направляющий косинус с21 положительное значение, так как в противном случае измерительная ось ДМ будет направлена в опорную поверхность нижнюю (ОПН - пол). Полярность i направляющего косинуса с21 при выбранной схеме крепления блока ДПИ на ПО зависит от угла установки дальномера σi. Если ДМ измеряют расстояния от точек Odi до ОПН, вертикальная координата h0 точек Ki равна нулю и формула (2.2) примет в этом случае вид: y gOdi Li = − i c21 (с ) + (с ) + (с ) i 2 11 i 2 21 i 2 31 . (2.13) Так как при расчете по формуле (2.13) направляющий косинус с21i должен иметь отрицательное значение, знак расстояния Li будет положительным. 2.5 Построение алгоритма определения опорных углов наклона блока датчиков первичной информации относительно опорной горизонтальной поверхности для произвольного числа дальномеров В общем случае на блоке ДПИ установлено N>2 ДМ согласно схеме, приведенной на рисунке 2.1. Исходными данными для построения алгоритма являются конструктивная схема расположения ДМ на блоке ДПИ, выходные сигналы ДМ, пропорциональные измеряемым дальностям.

45 2.5.1 Вывод уравнений связи углов наклона блока датчиков первичной информации относительно опорной поверхности с расстояниями, измеряемыми дальномерами При построении алгоритма информация Li , i ∈1, N , поступающая с выходов ДМ, является исходной, на основании которой определяются опорные значения  блока ДПИ. углов крена γ дм и тангажа ϑ дм Проекция y   радиуса-вектора ROdi Ki = idi Li на нормаль к опорной  ROdi Ki g поверхности:  ROdi Ki g i i c21 = y c22  Li  i i    c21 c= Li , 23  ⋅  0   0  (2.14) i = sin ϑ cos σi cos µi + cos ϑ sin γ sin µi cos σi + cos γ cos ϑ sin σi . где c21 С другой стороны, y  ROdi Ki g = h0 − hi , (2.15) где h0 – расстояние от ОПН до ОПВ по нормали к ОПВ; hi – расстояние точки Odi до ОПН, измеренное по нормали к горизонтальной плоскости XgO0Zg. Расстояние hi определяется по формуле h= yg + y i  ROcOdi g , (2.16) где y g – расстояние точки Oс до ОПН, измеренное по нормали к горизонтальной плоскости XgO0Zg; y  ROcOdi g  R –проекция радиуса-вектора OcOdi на нормаль к ОПВ, определяемая формулой y  ROc Odi g = [ c21 c22  rdi cos ν i  c23 ]  ydi  ,    −rdi sin ν i  (2.17)

46 где c21 = sin ϑ, c22 = cos ϑ cos γ, c23 = − cos ϑ sin γ – направляющие косинусы матрицы C g1,c . При подстановке выражений (2.16), (2.17) в формулу (2.15) получается: y  ROdi Ki g =∆h − rdi cos ν i sin ϑ − ydi cos ϑ cos γ − rdi sin ν i cos ϑ sin γ , (2.18) где ∆h = h0 − y g . Учитывая, что информацию о кинематических переменных Li , i ∈1, N , определяемых по формуле (2.12) или (2.13), ДМ выдают в виде нормированных сигналов Li , i ∈1, N , при подстановке выражения (2.18) в формулу (2.14) получается: tgϑ ( cos σi cos µi Li + rdi cos νi ) + ( Li sin σi + ydi ) cos γ + ∆h + ( Li sin µi cos σi + rdi sin ν i ) sin γ = , cos ϑ π π π π − <γ< , − <ϑ< , i ∈1, N . 2 2 2 2 (2.19) Получена система уравнений (2.19), неизвестными в которой являются углы ϑ и γ, а исходной информацией – выходные сигналы ДМ Li , пропорциональные расстояниям Li, и конструктивные параметры установки ДМ на блоке ДПИ – rdi , ydi , ν i , µi , σi , i ∈1, N . 2.5.2 Алгоритм определения углов наклона блока датчиков первичной информации относительно опорной поверхности по показаниям дальномеров Исходными данными для решения системы уравнений (2.19) для произвольного числа ДМ N>2 являются координаты установки ДМ на блоке ДПИ в системе координат OcXcYcZc: радиус-вектор rdi полюса Odi, углы µi и σi . Рассмотрена следующая схема размещения ДМ на блоке ДПИ: - полюсы Odi размещены на поверхности цилиндра радиуса= rdi rd , i ∈1, N , осью которого является ось OcYc, на высоте yd над плоскостью XcOcZc, одинаковой для всех ДМ. Угловой шаг положения полюсов Odi одинаков для всех ДМ и равен ∆ν = ν i +1 − ν i = 2π / N , i ∈1, N − 1 ;

47 - системы координат Odi X diYdi Z di , i ∈1, N повернуты вокруг оси Yс1 2π относительно системы OdiXc1Yc1Zc1 на углы µi =ν i = ( i − 1) , i ∈1, N . В этом N случае координатные плоскости X diOdiYdi , i ∈1, N пройдут через ось OcYc; - измерительные оси ДМ повернуты вокруг оси Z di относительно плоскости Xc1OdiZc1 на углы σi, номинальное значение которых одинаково для всех ДМ. При принятых значениях конструктивных параметров уравнения (2.19) примут вид: tgϑ ( Li cos µi cos σi + rd cos ν i ) + cos γ ( Li sin σi + yd ) + rd sin ν i ) + sin γ ( Li sin µi cos σi += π π − <γ< , 2 2 µi = ν i = ∆h , cos ϑ π π − <ϑ< , 2 2 2π ( i − 1) , N (2.20) i ∈1, N . Уравнения (2.20) приводятся к виду: ∆h , f1i ( Li ) tgϑ + f 2i ( Li ) cos γ + f3i ( Li ) sin γ = cos ϑ π π π π − <γ< , − <ϑ< , i ∈1, N , 2 2 2 2 (2.21) где f1i ( Li ) =cos µi cos σi Li + rd cos νi , f 2i ( Li ) =sin σi Li + yd , f3i ( Li ) =sin µi cos σi Li + rd sin νi . Используя все N уравнений (2.21), составляется новая система путем приравнивания левых частей разных уравнений, т.к. правые части у всех уравнений одинаковые. ) ) sin γ ( f ( L ) − f ( L ) ) tgϑ, ( f ( L ) − f ( L )) cos γ + ( f ( L ) − f ( L = 2i i 2k π π − <γ< , 2 2 k 3i π π − <ϑ< , 2 2 i 3k k 1k k i ∈1, N − 1, k ∈ 2, N , k > i. 1i i (2.22) Количество уравнений вида (2.22) равно n1, определяемое как число 2 n1 C= сочетаний из N по два: = N N! [61]. Из полученных n1 уравнений 2!( N − 2 )! составляются системы уравнений вида:

48  f 2i ( Li ) − f 2 k ( Lk )  cos γ +  f3i ( Li ) − f 3k ( Lk= ) sin γ  f1k ( Lk ) − f1i ( Li ) tgϑ,      f 2 j ( L j ) − f 2 n ( Ln )  cos γ +  f3 j ( L j ) − f3n ( Ln= ) sin γ  f1n ( Ln ) − f1 j ( Lj ) tgϑ, (2.23)     i ∈1, N − 1, k > i, j ∈1, N − 1, n > j , n ≠ k .   Максимальное количество систем уравнений вида (2.23) n2 равно числу сочетаний из n1 по два: 2 = n2 C= n1 n1 ! . 2!( n1 − 2 )! Заметим, что число n2 систем из двух уравнений вида (2.23) следует выбирать с учетом конструктивной схемы расположения ДМ на блоке ДПИ. Полученные уравнения решаются как системы линейных алгебраических уравнений относительно переменных cos γ,sin γ . Получается несколько решений вида: = cos γ Fc ( Li , Lk , Lj , Ln ) tgϑ,   = sin γ Fs ( Li , Lk , L j , Ln ) tgϑ,  (2.24) из которых определяется тангенс опорного угла крена Fs ( Li , Lk , Lj , Ln ) tgγ оп = = ϕ ( Li , Lk , Lj , Ln )     Fc ( Li , Lk , L j , Ln ) (2.25) и сигнал, пропорциональный опорному углу крена γ оп = arctg ϕ ( Li , Lk , Lj , Ln )  . (2.26) В результате анализа полученных решений выбирается наилучший результат, исходя из особенностей конструктивной схемы и сложности математической формулы решения уравнений. Тангенс угла тангажа выражается через косинус угла крена из формулы (2.24), а затем через тангенс угла крена с последующей подстановкой формулы (2.25).

49 tgϑ = 1 1 1 cos γ . = ⋅ Fc ( Li , Lk , Lj , Ln ) Fc ( Li , Lk , Lj , Ln ) 1 + ϕ L , L , L , L  2  ( i k j n ) Сигнал, пропорциональный опорному углу тангажа:   1 1  = arctg ⋅ ϑ оп  2      Fc ( Li , Lk , L j , Ln ) 1 + ϕ ( Li , Lk , L j , Ln )       .   (2.27) Блок-схема алгоритма определения углов наклона блока ДПИ относительно горизонтальной опорной поверхности приведена на рисунке 2.3. Решение уравнений (2.23) найдено для частных случаев при N=3 и N=4. 2.6 Получение аналитических выражений опорных углов наклона блока датчиков первичной информации с использованием показаний трех и четырех дальномеров 2.6.1 Схема с тремя дальномерами Вывод формул определения углов наклона блока ДПИ относительно опорной горизонтальной поверхности для системы с тремя ДМ выполнен в соответствии с алгоритмом (рисунок 2.3). 1) При N=3 углы σi , µi , ν i примут значения: σ1 =σ2 =σ3 =σ, µ1 =ν1 =0, µ 2 =ν 2 = 2π 4π , µ3 =ν 3 = . 3 3 2) Уравнения (2.20) приводятся к виду: ∆h  tgϑ ( cos σL1 + rd ) + cos γ ( sin σL1 + yd ) = ,  cos ϑ  ∆ 3 1 h  cos γ ( L2 sin σ + yd ) + sin γ ( L2 cos σ + rd ) − tgϑ ( L2 cos σ + rd ) = , 2 2 cos ϑ  ∆h  3 1 cos γ ( L3 sin σ + yd ) − sin γ ( L3 cos σ + rd ) − tgϑ ( L3 cos σ + rd ) = .  2 2 cos ϑ  (2.28)

50 начало на 1 Задание исходных данных: Приведение уравнений (2.20) к виду: 1щщ щ1 (2.21) 2 Составление из уравнений (2.21) системы из уравнений: (2.22) 3 Составление из уравнений (2.22) систем уравнений общим количеством : 4 (2.23) 5 Решение уравнений (2.23). Результат решения – получение нескольких выражений вида: Определение опорного угла крена: 6 Определение опорного угла тангажа: 7 конец Рисунок 2.3 – Блок-схема алгоритма определения углов наклона блока ДПИ относительно опорной поверхности

51 Получена 3) система из 3! = 3 2!( 3 − 2 )! 2 = n1 C= 3 уравнений путем приравнивания левых частей первого и второго уравнений, первого и третьего уравнений, второго и третьего уравнений системы (2.28). ( ( ) ) 2 ( L2 − L1 ) sin σ cos γ + 3 ( L2 cos σ + rd ) sin γ= tgϑ ( 2 L1 + L2 ) cos σ + 3rd ,         2 ( L3 − L1 ) sin σ cos γ − 3 ( L3 cos σ + rd ) sin γ= tgϑ ( 2 L1 + L3 ) cos σ + 3rd ,         2 ( L2 − L3 ) sin σ cos γ + 3 ( L2 + L3 ) cos σ + 2rd sin γ =tgϑ ( L2 − L3 ) cos σ.   ( ) (2.29) 2 2 Из уравнений (2.26) составлены n= 3 системы уравнений C= C= 2 3 n 4) 1 вида (2.23). ( ( ) ) ( ) ( ) 2 ( L2 − L1 ) sin σ cos γ + 3 ( L2 cos σ + rd ) sin γ= tgϑ ( 2 L1 + L2 ) cos σ + 3rd ,   2 ( L3 − L1 ) sin σ cos γ − 3 ( L3 cos σ + rd ) sin γ= tgϑ ( 2 L1 + L3 ) cos σ + 3rd ,   2 ( L2 − L1 ) sin σ cos γ + 3 ( L2 cos σ + rd ) sin γ= tgϑ ( 2 L1 + L2 ) cos σ + 3rd ,   2 ( L2 − L3 ) sin σ cos γ + 3 ( L2 + L3 ) cos σ + 2rd sin γ= tgϑ ( L2 − L3 ) cos σ,   ( ) 2 ( L3 − L1 ) sin σ cos γ − 3 ( L3 cos σ + rd ) sin γ= tgϑ ( 2 L1 + L3 ) cos σ + 3rd ,   2 ( L2 − L3 ) sin σ cos γ + 3 ( L2 + L3 ) cos σ + 2rd sin γ= tgϑ ( L2 − L3 ) cos σ.   ( ) (2.30) (2.31) (2.32) Результат решения полученных систем уравнений относительно 5) переменных cos γ,sin γ :  ( L1L2 + L1L3 + L2 L3 ) cos 2 σ + 2rd ( L1 + L2 + L3 ) cos σ + 3rd2    cos γ tgϑ,   2 L2 L3 − L1 ( L3 + L2 ) cos σ − ( 2 L1 − L3 − L2 ) rd  sin σ      3 ( L3 − L2 )( L1 cos σ + rd )   sin γ tgϑ.   2 L2 L3 − L1 ( L3 + L2 ) cos σ − ( 2 L1 − L3 − L2 ) rd      ( ( ) ) 6) Определение тангенса опорного угла крена: 3 ( L3 − L2 )( L1 cos σ + rd ) sin σ tgγ оп = . ( L1L2 + L1L3 + L2 L3 ) cos 2 σ + 2rd ( L1 + L2 + L3 ) cos σ + 3rd2    (2.33)

52 Выражение для сигнала, пропорционального опорному углу крена:   3 ( L3 − L2 )( L1 cos σ + rd ) sin σ γ оп = . arctg   ( L1L2 + L1L3 + L2 L3 ) cos 2 σ + 2rd ( L1 + L2 + L3 ) cos σ + 3rd2    (2.34) 7) Выражение для тангенса опорного угла тангажа: Q ( L1 , L2 , L3 ) / V ( L1 , L2 , L3 ) , tgϑоп = (2.35) } { где Q ( L1 , L2 , L3= ) sin σ 2L2 L3 − L1 ( L3 + L2 ) cos σ − ( 2L1 − L3 − L2 ) rd ; V= ( L1, L2 , L3 ) ( L1L2 + L1L3 + L2 L3 ) cos2 σ + 2rd ( L1 + L2 + L3 ) cos σ + 3rd2  + 2 +3 ( L3 − L2 )( L1 cos σ + rd )  sin 2 σ. 2 Формула для сигнала, пропорционального опорному углу тангажа:  Q L , L , L   ( 1 2 3) .  = ϑ arctg оп  V L , L , L  ( 1 2 3)   (2.36) В частном случае при rd=0 формулы (2.34), (2.36) примут вид:  γ оп    = ϑ arctg  оп    −L  )L  tgσ   3(L 3 2 1 , = arctg  L  +L L  +L L  )  (L 1 3 2 3   1 2 (2.37)  2 L2 L3 − L1 ( L3 + L2 )    2 2 ( L1L2 + L1L3 + L2 L3 ) сtg 2σ + 3( L3 − L2 ) L12   .   (2.38) Частный случай rd =0 означает, что точка отсчета расстояний до опорной поверхности для всех ДМ одна и находится она на пересечении измерительных осей ДМ с осью OcYc в точке Ad. Если фактические точки отсчета Оdi находятся на окружности радиуса rd, то при вычислении опорных углов по формулам (2.34), (2.36) каждый из сигналов Li , i ∈1,3 есть сумма сигнала L*i , соответствующего расстоянию Li = Odi K i , измеренному ДМi, и сигнала расстоянию AdOdi = rd/cosσ, одинаковому для всех ДМ: L=i L*i + δL, i ∈1,3 . δL , соответствующего

53 2.6.2 Схема с четырьмя дальномерами Вывод формул определения углов наклона блока ДПИ относительно опорной горизонтальной поверхности для системы с четырьмя ДМ выполнен в соответствии с алгоритмом (рисунок 2.3). 1) При N=4 углы µi , ν i примут значения: 3π π σ1 =σ2 =σ3 =σ4 =σ, µ1 =ν1 =0, µ 2 =ν 2 = , µ3 =ν 3 =π, µ 4 =ν 4 = . 2 2 2) Уравнения (2.20) приводятся к виду: ∆h  tgϑ ( L1 cos σ + rd ) + cos γ ( L1 sin σ + yd ) = ,  cos ϑ  ∆h    , cos γ ( L2 sin σ + yd ) + sin γ ( L2 cos σ + rd ) = cos ϑ   ∆ h − tgϑ ( L3 cos σ + rd ) + cos γ ( L3 sin σ + yd ) = , cos ϑ  ∆h    cos γ ( L4 sin σ + yd ) − sin γ ( L4 cos σ + rd ) = .  cos ϑ  3) Получена система 4! = 6 2!( 4 − 2 )! 2 = n1 C= 4 из (2.39) уравнений путем поочередного приравнивания левых частей уравнений (2.39). ( L − L ) sin σ cos γ − ( L cos σ + r ) sin γ = −tgϑ( L cos σ + r ) ,   ( L − L ) sin σ cos γ = −tgϑ( ( L + L ) cos σ + 2r ) ,  ( L − L ) sin σ cos γ + ( L cos σ + r ) sin γ = −tgϑ( L cos σ + r ) ,   ( L − L ) sin σ cos γ + ( L cos σ + r ) sin γ = −tgϑ( L cos σ + r ) ,   ( L − L ) sin σ cos γ + ( ( L + L ) cos σ + 2r ) sin γ =0,      ( L − L ) sin σ cos γ + sin γ ( L cos σ + r )= tgϑ( L cos σ + r ).  4) 1 2 1 3 1 4 4 2 3 2 2 4 3 4 d 2 1 d d 1 d d 3 d 3 2 d 1 d 4 4 (2.40) d 3 d 2 2 Из уравнений (2.40) составляется n= C= C= 15 систем уравнений n 2 6 1 вида (2.23). Однако целесообразно выделить только те системы уравнений, которые содержат сигналы от всех ДМ, т.е. сигналы Li , i ∈1,4 :

54 ( L − L ) sin σ cos γ − ( L cos σ + r ) sin γ = −tgϑ( L cos σ + r ) ,  ( L − L ) sin σ cos γ + sin γ ( L cos σ + r )= tgϑ( L cos σ + r ) ,  (2.41) ( L − L ) sin σ cos γ = −tgϑ( ( L + L ) cos σ + 2r ) ,       σ γ + + σ + γ = sin cos cos 2 sin 0 , L − L L L r  ( ) ) (( )  (2.42) 1 3 2 2 d 4 1 3 2 4 1 d 4 3 2 d 3 1 d d 4 d ( L − L ) sin σ cos γ + ( L cos σ + r ) sin γ = −tgϑ( L cos σ + r ) ,   ( L − L ) sin σ cos γ + ( L cos σ + r ) sin γ = −tgϑ( L cos σ + r ) , 1 2 4 3 4 d 2 1 d d 3 (2.43) d 5) Результат решения полученных систем уравнений относительно переменных cos γ,sin γ имеет 3 варианта: Вариант 1. Решение уравнений (2.41):  ( L2 L3 − L1L4 ) cos σ + rd ( L2 + L3 − L1 − L4 )  c tgσ   cos γ tgϑ,            ( L1L4 − 2 L2 L4 + L2 L3 ) cos σ + rd ( L1 − L2 + L3 − L4 )       ( 2 L1L3 − L2 L3 − L1L4 ) cos σ + rd ( L1 − L2 + L3 − L4 )   tgϑ.  sin γ =   ( L1L4 − 2 L2 L4 + L2 L3 ) cos σ + rd ( L1 − L2 + L3 − L4 )     (2.44) Вариант 2. Решение уравнений (2.42): (( L + L ) cos σ + 2r ) tgϑ,   cos γ =   sin L − L σ ( 3 1)       cos 2 L − L L + L σ r + ( 2 4 ) ( 3 1)  d sin γ tgϑ. = ( L1 − L3 ) ( L2 + L4 ) cos σ + 2rd  3 1 ( ( d (2.45) ) ) Вариант 3. Решение уравнений (2.43):  ( L3 L4 − L1L2 ) cos σ + rd ( L3 + L4 − L1 − L2 )  cos σ   cos γ tgϑ,    L1L2 − 2 L2 L4 + L3 L4  cos σ + rd ( L1 − L4 − L2 + L3 ) sin σ   (2.46) ( L1L2 − 2 L1L3 + L3 L4 ) cos σ + rd ( L2 − L3 − L1 + L4 )    tgϑ. sin γ =    L1L2 − 2 L2 L4 + L3 L4  cos σ + rd ( L1 − L4 − L2 + L3 )  6) решений: { } { } Определение тангенса опорного угла крена для трех вариантов

55 ( 2 L1L3 − L2 L3 − L1L4 ) cos σ + rd ( L1 − L2 + L3 − L4 )  tgσ   , tgγ оп1 =         ( L2 L3 − L1L4 ) cos σ + rd ( L2 + L3 − L1 − L4 )    tgγ оп2 L − L ) sin σ ( = ( L + L ) cos σ + 2r 4 2 2 4 , d ( L1L2 − 2 L1L3 + L3 L4 ) cos σ + rd ( L2 − L3 − L1 + L4 )  tgσ   . tgγ оп3 = ( L3 L4 − L1L2 ) cos σ + rd ( L3 + L4 − L1 − L2 )    Выражения для сигнала, пропорционального опорному углу крена для трех вариантов решений:  ( 2 L1L3 − L2 L3 − L1L4 ) cos σ + rd ( L1 − L2 + L3 − L4 )  tgσ    , (2.47) γ оп1 = arctg             ( L2 L3 − L1L4 ) cos σ + rd ( L2 + L3 − L1 − L4 )    ( L4 − L2 ) sin σ γ оп2 =  arctg     ( L2 + L4 ) cos σ + 2rd  ,   (2.48)  ( L1L2 − 2 L1L3 + L3 L4 ) cos σ + rd ( L2 − L3 − L1 + L4 )  tgσ   . γ оп3 = arctg             ( L3 L4 − L1L2 ) cos σ + rd ( L3 + L4 − L1 − L2 )   (2.49) Наиболее простое выражение для сигнала, пропорционального опорному углу крена, (2.48). Выбирается данный вариант как наиболее удачный для выполнения дальнейших выкладок. Выражение для тангенса опорного угла тангажа: 7) ( L − L ) sin σ cos γ , (( L + L ) cos σ + 2r ) = tgϑоп 3 3 = cos γ где 1 = 1 + tg 2 γ оп2 1 1 (2.50) d ( L + L ) cos σ + 2r       ( L + L ) cos σ + 2r  + ( L − L ) sin 2 d 4 2 2 d 4 2 4 2 . 2 σ Подставив в формулу (2.50) выражение для косинуса угла крена, получим: tgϑдм L − L ) ( L + L ) cos σ + 2r  sin σ ( = (( L + L ) cos σ + 2r ) ( L + L ) cos σ + 2r  + ( L 3 1 2 4 d 2 3 1 d 2 4 d 4 − L2 ) sin σ 2 2 . (2.51)

56 Формула для сигнала, пропорционального опорному углу тангажа:    = arctg  ϑ оп      . (2.52) 2 2   4 − L2 ) sin σ  ( L − L ) ( L + L ) cos σ + 2r  sin σ (( L + L ) cos σ + 2r ) ( L + L ) cos σ + 2r  + ( L 3 1 2 4 d 2 4 d 2 3 1 d В частном случае при rd=0 формулы (2.48), (2.52) примут вид:  ( L4 − L2 ) tgσ  γ оп = , arctg   ( L2 + L4 )       = arctg  ϑ оп  L + L1 ) ( 3       ( L3 − L1 )( L2 + L4 ) sin σ   . 2 2 2 2     ( L2 + L4 ) cos σ + ( L4 − L2 ) sin σ   (2.53) Частный случай rd =0 означает, что точка отсчета расстояний до опорной поверхности Ad для всех ДМ одна и находится она на пересечении измерительных осей ДМ с осью OcYc. Если фактические точки отсчета Оdi находятся на окружности радиуса rd, то при вычислении опорных углов по формулам (2.53) каждый из сигналов Li , i ∈1,4 есть сумма сигнала L*i , соответствующего расстоянию Li = Odi K i , измеренному ДМi, и сигнала δL , соответствующего расстоянию AdOdi = rd/cosσ, одинаковому для всех ДМ: L=i L*i + δL, i ∈1,4 . Полученные аналитические выражения опорных углов наклона блока ДПИ с использованием показаний трех и четырех дальномеров инвариантны к высоте движения блока ДПИ, которая является переменным параметром в процессе движения ПО. 2.7 Построение математической модели расстояния до опорной поверхности, плоскость которой имеет уклон по отношению к плоскости горизонта Определение углов наклона блока ДПИ относительно опорной поверхности, выполненное при допущении, что плоскость опорной поверхности горизонтальна, является частным случаем. Если плоскость опорной поверхности имеет уклон

57  , вычисленные дальномерной относительно плоскости горизонта, то углы γ оп и ϑ оп СУО, будут характеризовать углы наклона блока ДПИ относительно негоризонтальной опорной поверхности, что внесет дополнительную погрешность при вычислении углов ориентации блока ДПИ. Поэтому необходимо решить задачу определения углов наклона блока ДПИ относительно плоскости горизонта. Для решения этой задачи с использованием теории однородных координат проективного пространства строится математическая модель расстояний, измеряемых ДМ, установленными на блоке ДПИ, до опорной поверхности, имеющей уклон относительно плоскости горизонта. Исходными данными для построения математической модели являются параметры кинематической модели движения ПО и геометрические параметры расположения опорной поверхности. Расстояние Li, измеряемое от места установки ДМ (от точек Odi) до точек K i* пересечения положительных полуосей Xdi с плоскостью опорной поверхности определяется по формуле [61]: Li = (x Ki* g −x Odi g ) +(y 2 Ki* g −y Odi g ) + (z 2 Ki* g −z Odi g ), 2 (2.54) где xgK , y gK , z gK – координаты точки K i* в системе O0XgYgZg; * i * i * i xgOdi , y gOdi , z gOdi – координаты точки Odi в системе O0XgYgZg. Координаты точки K i* в нормальной земной системе координат O0XgYgZg определяются как координаты радиуса – вектора R O K * =  xgKi  0 i * * Ki Координаты точки K i* в системе O0 X опYоп Z оп : R O*K * =  xоп  0 i * т zgKi  .  * ygKi * * Ki yоп T Ki  . zоп  * * Однородные координаты точки K i* в системе O0 X опYоп Z оп определяют вектор R * *  R * *  rO*K * =  O0 Ki  . Для определения искомого вектора rO*K * =  O0 Ki  однородных 0 i 0 i  1   1  * координат точки K i* в системе O0 X опYоп Z оп используется формула преобразования координат (приложение Б):

58 rO K * = M g , g 2 ⋅ M g 2,оп ⋅ rO*K * = M g ,оп ⋅ rO*K * , 0 i 0 i (2.55) 0 i = M g , g 2 ⋅ M g 2,оп . где M g ,оп Выражение для матрицы M g ,оп :  E R O0O0*  C g 2,оп G 0т  C g 2,оп R O0O0*  M g ,оп =M g , g 2 ⋅ M g 2,оп =  ⋅   = 1   G 0 1   G 0 1  G 0 (2.56) При подстановке выражения (2.56) в формулу (2.55) получается: C g 2,оп R O0O0*   R O0*Ki*  C g 2,оп R O0*Ki* + R O0O0*   R O0 Ki*  = rO K *  = ⋅ =      0 i 1 G 1  0   1     1  (2.57) Таким образом, координаты точки K i* в системе O0XgYgZg определяются формулой: Ki   xgKi   xоп 0  *  *   Ki Ki   yg  = C g 2,оп R O*K * + R O O* = C g 2,оп ⋅  yоп  + h0  0 i 0 0  K*   K*    i  zg   zопi   0  * * (2.58) * Координаты точки K i* в системе O0 X опYоп Z оп находятся из уравнения проекций прямой Odi Ki , проходящей через точки Odi, Ki и K i* (рисунок 2.4). Уравнения проекций прямой Odi Ki [61]: Ki Odi yоп − yоп Odi y= y + Ki x − xоп Odi xоп − xоп ( Odi оп y −y Odi y= yоп + z −z Ki оп Ki оп Odi оп Odi оп (z − z ) Odi оп  ) ,   ,   (2.59) * O O O где xоп – координаты точки Odi в системе O0 X опYоп Z оп ; , yоп , zоп di di di Кi Кi Кi – координаты точки Кi в системе xоп , yоп , zоп O0* X опYоп Z оп . K равна Так как точка Ki* принадлежит плоскости X опO0* Z оп , то ее ордината yоп * i нулю. При подстановке в уравнения (2.59) значения переменной y=0 получаются остальные координаты точки Ki* .

59 Ki Odi xоп − xоп Odi  y , оп Ki Odi − yоп yоп   Ki Odi zоп − zоп Odi  − Ki yоп . Odi  yоп − yоп  Ki Odi x= xоп − оп * Odi z= zоп оп Ki* (2.60) Yоп Zоп Xоп K”i K’i Ki Odi  Рисунок 2.4 - Проекции орта idi измерительной оси i – того ДМ на координатные плоскости системы координат, связанной с опорной поверхностью Координаты точек Odi и Кi в системе координат O0* X опYоп Z оп находятся аналогично по схеме (приложение Б): O0* X опYоп Z оп (1,3) M оп, g 2 −λ x , −λ z O0* X g 2Yg 2 Z g 2 ( p ) M g 2, g − R O O* O0 X gYg Z g , 0 0  R O0*Ki   R O0 Ki  = rO*K = M M = r M = r M ⋅ ⋅ ⋅ ⋅  , оп, g 2 оп, g оп, g  g 2, g O0 Ki O0 Ki 0 i 1  1     R O0*Odi   R O0Odi  = rO*O = M оп,= ⋅ r M ⋅    оп, g = g O0Odi 0 di  1   1  = M оп, g 2 ⋅ M g 2, g . где M оп, g ( C R − R O O* 0 0  оп, g 2 O0Odi  1 ) 

60 Выражение для матрицы M оп,g : M оп, g = M оп, g 2 ⋅ M g 2, g Cоп, g 2 G 0т   E −R O0O0*  = = ⋅ G 1 G 1    0   0 Cоп, g 2 −Cоп, g 2 R O0O0*    1  G 0  (2.61) Координаты точки Ki в системе координат O0* X опYоп Z оп : Ki  xоп   xgKi   Ki    yоп  Cоп, g 2 ⋅  0  = Ki  zоп   zgKi      (2.62) Координаты точки Odi в системе координат O0* X опYоп Z оп : Odi  xgOdi  xоп    Odi  O y = ⋅ C  − h0 − y g di g оп оп, 2   Odi   zоп  z gOdi    ( )   .   (2.63) Координаты точек Ki и Odi в нормальной земной системе координат определены в разделе 2.4 (формулы (2.11) и (2.6)). 2.8 Построение алгоритма определения углов уклона опорной поверхности относительно плоскости горизонта Задача определения углов уклона опорной поверхности относительно плоскости горизонта решается с использованием теории однородных координат проективного пространства. Исходными данными для решения задачи является информация с акселерометров блока ДПИ и информация с дальномерной СУО (опорные углы наклона блока ДПИ относительно горизонтальной опорной поверхности). = Соп, g1С g1,c и Cоп1,с определяют Матрицы вращения Cоп,с С= оп, g 2С g 2, g С g , g 1С g 1,c угловую связь между одними и теми же системами – системой ОсXcYcZc , связанной с блоком ДПИ, и системой O0* X опYоп Z оп , связанной с опорной поверхностью, поэтому направляющие косинусы матриц Cоп,с и Cоп1,с должны совпадать.

61 с При приравнивании сijоп, = сijоп1,с , i ∈1, j ∈1,3 , получается система линейных алгебраических уравнений: cos λ z cos ϑ cos ψ + sin λ z sin ϑ − cos ϑоп cos ψ оп = 0,   cos λ z ( sin γ sin ψ − cos γ cos ψ sin ϑ) + sin λ z cos γ cos ϑ +   + cos γ оп sin ϑоп cos ψ оп =sin γ оп sin ψ оп ,  cos λ z ( cos γ sin ψ + cos ψ sin γ sin ϑ) − sin λ z cos ϑ sin γ −    − cos ψ оп sin γ оп sin ϑоп =cos γ оп sin ψ оп . (2.64) Система (2.64) решается методом Крамера [61] относительно переменных cos λ z , sin λ z , cos ψ оп . Общий определитель системы (2.64): D= sin ϑоп cos ψ sin ( γ − γ оп ) + sin ψ cos ( γ − γ оп ) sin ϑ + cos ϑоп cos ϑ sin ψ. z (2.65) Частный определитель системы (2.64) по переменной cos λ z : D= sin ϑ sin ϑоп + cos ϑоп cos ϑ cos ( γ − γ оп )  sin ψ оп . cz (2.66) Частный определитель системы (2.64) по переменной sin λ z : D= cos ϑоп cos ψ sin ϑ cos ( γ − γ оп ) − sin ψ sin ( γ − γ оп )  sin ψ оп − sz − cos ϑ cos ψ sin ϑоп sin ψ оп . (2.67) Частный определитель системы (2.64) по переменной cosψ оп : = Dсψ cos ( γ − γ оп ) cos ψ − sin ( γ − γ оп ) sin ψ sin ϑ sin ψ оп . (2.68) Решение уравнений (2.64): ψ оп  Dcsin Dczsin ψоп Dszsin ψоп ψ = cos λ z sin ψ оп ,= sin λ z sin ψ оп , = cos ψ оп sin ψ оп ,  Dz Dz Dz  (2.69)  Dszsin ψоп Dz  = = tgλ z , tgψ оп , sin ψ оп sin ψ оп  Dcz Dcψ  где ψ оп – частные производные определителей Dcz , Dsz , Dcψ по Dczsin ψоп , Dszsin ψоп , Dcsin ψ переменной sin ψ оп :

62 Dczsin ψоп= sin ϑ sin ϑоп + cos ϑоп cos ϑ cos ( γ − γ оп ) ,   оп cos ϑоп cos ψ sin ϑ cos ( γ − γ оп ) − sin ψ sin ( γ − γ оп )  −  Dszsin ψ=  − cos ϑ cos ψ sin ϑоп ,  ψ оп  Dcsin ψ = cos ( γ − γ оп ) cos ψ − sin ( γ − γ оп ) sin ψ sin ϑ.  (2.70) Формулы для вычисления углов λ z , ψ оп :  Dz  Dszsin ψоп  , arctg = λ z arctg  = ψ  оп sin ψ оп   D sin ψоп  Dcz   cψ   .  (2.71) с При приравнивании сijоп,= сijоп1,с , i ∈ 2, j ∈1, 2 , получается система линейных алгебраических уравнений: cos λ x ( cos λ z sin ϑ − sin λ z cos ϑ cos ψ ) − sin λ x cos ϑ sin ψ = sin ϑоп ,   cos λ x ( cos λ z cos γ cos ϑ − sin λ z ( sin γ sin ψ − cos γ cos ψ sin ϑ) ) +  (2.72)  + sin λ x ( cos ψ sin γ + cos γ sin ψ sin ϑ= ) cos γ оп cos ϑоп .  Система (2.72) решается методом Крамера [61] относительно переменных cos λ х , sin λ х . Общий определитель системы (2.72): D= cos λ z ( sin ϑ cos ψ sin γ + cos γ sin ψ ) − sin λ z cos ϑ sin γ. x (2.73) Частный определитель системы (2.72) по переменной cos λ x : Dcx =sin ϑоп ( cos ψ sin γ + cos γ sin ψ sin ϑ) + cos γ оп cos ϑоп cos ϑ sin ψ. (2.74) Частный определитель системы (2.72) по переменной sin λ x : Dsx = cos λ z ( sin ϑ cos γ оп cos ϑоп − cos γ cos ϑ sin ϑоп ) + + sin λ z sin ϑоп ( sin γ sin ψ − cos γ cos ψ sin ϑ ) − cos ϑ cos ψ cos γ оп cos ϑоп ]. (2.75) Функции cos λ z , sin λ z с учетом решения (2.71) выражаются через tgλ z : = cos λ z 1 = 1 + tg 2λ z = sin λ z tgλ z = 1 + tg 2λ z  , 2 2 ( Dczsin ψоп ) + ( Dszsin ψоп )   Dszsin ψоп . 2 2  ( Dczsin ψоп ) + ( Dszsin ψоп )  Dczsin ψоп (2.76)

63 Выражения для определителей Dx , Dsx с учетом формул (2.76): Dx = Dsx Dczsin ψоп ( sin ϑ cos ψ sin γ + cos γ sin ψ ) − Dszsin ψоп cos ϑ sin γ (D 1 (D ) + (D sin ψ оп 2 cz ) + (D sin ψ оп 2 cz ) sin ψ оп 2 sz {D sin ψ оп cz ) sin ψ оп 2 sz , (2.77) ( sin ϑ cos γ оп cos ϑоп − cos γ cos ϑ sin ϑоп ) + (2.78) + Dszsin ψоп sin ϑоп ( sin γ sin ψ − cos γ cos ψ sin ϑ ) − cos ϑ cos ψ cos γ оп cos ϑоп ]} Решение уравнений (2.72): cos λ x = Dcx Dsx Dsx . , = sin λ x , = tgλ x Dx Dx Dcx (2.79) Угол уклона λ x вычисляется по формуле D  λ x =arctg  sx  .  Dcx  (2.80) Углы уклона опорной поверхности λ z , λ x вычисляются с целью коррекции выходных сигналов дальномерной СУО в контрольных точках маршрута ПО за короткие промежутки времени, когда блок ДПИ находится в неподвижном положении. Для вычисления углов уклона по формулам (2.71), (2.80) необходима исходная информация об истинном значении углов ориентации блока ДПИ γ, ϑ, ψ относительно нормальной системы координат O0XgYgZg и информация об углах наклона блока ДПИ γ оп , ϑоп относительно опорной поверхности. Углы ориентации блока ДПИ относительно плоскости горизонта γ и ϑ определяются по показаниям акселерометров [3] в ВУУ (рисунок 2.1), когда ПО с блоком ДПИ остается неподвижным в течение нескольких секунд, и оформляются  Угол в виде нормированных сигналов, которые обозначим символами γ a , ϑ a рысканья ψ относительно нормальной земной системы координат O0XgYgZg вычисляется в соответствии с алгоритмом инерциального счисления пути [3] и формируется на выходе вычислителя ВУ1 (рисунок 2.1) в виде нормированного сигнала ψ ин .

64 Дальномерная СУО формирует на выходе нормированные значения углов  , вычисляемые по наклона блока ДПИ относительно опорной поверхности γ оп , ϑ оп формулам, полученным в разделе 2.5. Таким образом, при подстановке нормированных значений сигналов  ,ψ  вместо соответствующих углов в формулы (2.71), (2.80):  ин , γ оп , ϑ γ а , ϑ а оп  , ψ=ψ   ин , γ оп = γ оп , ϑоп = ϑ γ=γ а , ϑ = ϑ а оп на выходе блока ВУУ в соответствии с формулами (2.71), (2.80) формируются нормированные сигналы λ z , λ x . Блок-схема алгоритма определения углов уклона опорной поверхности относительно плоскости горизонта приведена на рисунке 2.5. 2.9 Построение алгоритма определения углов наклона блока датчиков первичной информации относительно плоскости горизонта Исходными данными для решения задачи определения углов наклона блока ДПИ относительно плоскости горизонта с учетом негоризонтальности опорной поверхности являются: - нормированные сигналы на выходе ВУУ негоризонтальности опорной  ,λ  ; поверхности λ z x  , - нормированные сигналы на выходе вычислителя дальномерной СУО ϑ оп γ оп , представляющие собой опорные углы блока ДПИ относительно негоризонтальной опорной поверхности. 2.9.1 Определение угла тангажа относительно плоскости горизонта Определение угла тангажа относительно плоскости горизонта осуществляется путем решения уравнения сijоп,с = сijоп1,с при i=1, j=1 с учетом  , γ .  ,ϑ  ,λ исходных данных λ оп оп z x  sin ϑ оп  cos λ  tgϑ − cos ψ  cos λ  + sin λ  sin ψ   . cos λ sin λ = z x ин z x x ин cos ϑ ( ) (2.81)

65 Исходные данные: начало на - курс с выхода ИНС, - сигналы с акселерометров, 1 - сигналы дальномерной СУО. Определение углов крена и тангажа, если 1щщ щ1 : 2 Приравнивание элементов первых строк матриц 3 . Система (2.64) из трех уравнений . Решение уравнений (2.64) относительно переменных 4 5 Решение уравнений (2.70) относительно углов 6 Приравнивание элементов вторых строк матриц : . Система (2.72) из трех уравнений . 7 Решение уравнений (2.72) относительно переменных 8 Решение уравнений (2.77) относительно угла : конец Рисунок 2.5 – Блок-схема алгоритма определения углов уклона опорной поверхности относительно плоскости горизонта

66 Вводятся обозначения:  cos λ  , A =cos λ z x  cos λ  + sin λ  sin ψ  ин sin λ  ин B = cos ψ z x x (  . = C sin ϑ оп )   , .   (2.82) При подстановке в уравнение (2.81) выражений (2.82) и формулы 1 = 1 + tg 2ϑ получается уравнение cos ϑ Atgϑ −= B C 1 + tg 2ϑ . (2.83) При возведении уравнения (2.83) в квадрат получается (A 2 − C 2 ) tg 2ϑ − 2 ABtgϑ + B 2 − C 2 = 0 . (2.84) Решение уравнения (2.84): AB ± C A2 + B 2 − C 2 . tgϑ1,2 = 2 2 A − C ( ) (2.85)  =λ  =0 : A = 1, B = 0, C = sin ϑ  , ϑ=ϑ  , а формула (2.85) принимает При λ z x оп оп вид:  1 − sin 2 ϑ   ± sin ϑ ± sin ϑ оп оп оп  . tgϑ1,2 = = =± tgϑ оп 2  1 − sin ϑоп cos ϑоп (2.86) Так как формула (2.86) есть тождество, в решении (2.85) перед квадратным корнем выбирается знак плюс. Окончательно получается: AB + C A2 + B 2 − C 2 . tgϑ = ( A2 − C 2 ) (2.87)  на выходе вычислителя дальномерной СУО: Сигнал угла тангажа ϑ ДМ  AB + C A2 + B 2 − C 2  arctg  ϑдм =  ( A2 − C 2 )   .   (2.88)

67 2.9.2 Определение угла крена относительно плоскости горизонта Определение угла крена относительно плоскости горизонта осуществляется  , путем решения уравнения сijоп,с = сijоп1,с при i=2, j=2 с учетом исходных данных λ z  .  , γ , ϑ  ,ϑ λ дм оп оп x ( cos λ cos λ cos γ cos ϑ z x дм (   ин − cos γ cos ψ  ин sin ϑ − sin λ z cos λ x sin γ sin ψ дм ( )) + )  = cos γ cos ϑ  .  ин sin γ + cos γ sin ψ  ин sin ϑ + sin λ x cos ψ дм оп оп (2.89) Вводятся обозначения:  − sin λ  cos λ  sin ψ  ин sin λ  ин ,  P= cos ψ x z x   cos λ  cos ϑ  + sin λ  cos λ  cos ψ  sin ψ  ,    Q =cos λ + λ ϑ sin sin z x дм z x ин x ин дм   . R =cos γ оп cos ϑ  оп ( ) (2.90) При подстановке в уравнение (2.89) выражений (2.90) и формулы 1 = 1 + tg 2 γ получается cos γ Ptgγ += Q R 1 + tg 2 γ . (2.91) При возведении уравнения (2.91) в квадрат получается (P 2 − R 2 ) tg 2 γ + 2 PQtgγ + Q 2 − R 2 =0 . (2.92) Решение уравнения (2.92): − PQ ± R P 2 + Q 2 − R 2 . tgγ1,2 = 2 2 − P R ( ) (2.93)  =λ  =0 выражения (2.90) примут вид При λ z x  , R =cos γ cos ϑ  . P =0, Q =cos ϑ дм оп дм Формула (2.93) с учетом γ = γ оп станет тождеством: 2 2   ± cos γ оп cos ϑ дм cos ϑдм (1 − cos γ оп ) ± sin γ оп tg= γ = = tgγ оп . (2.94)   cos − cos 2 γ оп cos 2 ϑ − γ дм оп На основании (2.94) в решении (2.93) перед корнем выбирается знак минус. Окончательно получается:

68 − PQ − R P 2 + Q 2 − R 2 . tgγ = ( P2 − R2 ) (2.95) Сигнал угла крена γ дм на выходе вычислителя дальномерной СУО:  − PQ − R P 2 + Q 2 − R 2 γ дм =arctg   ( P2 − R2 )   .   (2.96) Таким образом, получен алгоритм определения углов наклона блока датчиков первичной информации относительно плоскости горизонта, блок-схема которого приведена на рисунке 2.6. ВЫВОДЫ 1. На основе теории однородных координат проективного пространства построены математические модели расстояний, измеряемых i-тым ДМ до опорной плоскости, которая горизонтальна или имеет уклон относительно плоскости горизонта. 2. Разработан способ определения углов наклона блока ДПИ относительно опорной поверхности на основе ДМ, позволяющий определить углы наклона без накопления погрешности по времени. На данный способ получен патент РФ на изобретение №2646941 [62]. 3. Разработан алгоритм определения углов наклона блока ДПИ относительно опорной поверхности с учетом количества ДМ и их расположения на блоке ДПИ. 4. Разработан алгоритм определения углов наклона блока ДПИ относительно плоскости горизонта без накопления погрешности измерения, который позволяет применить его для общего случая, когда плоскость опорной поверхности имеет уклон относительно плоскости горизонта. 5. Достоинством разработанных алгоритмов является инвариантность к высоте движения блока ДПИ, которая является переменным параметром в процессе движения ПО.

69 начало на Исходные данные: - курс с выхода ИНС, - сигналы с выхода ВУУ, 1 1щщ щ1 - сигналы дальномерной СУО. Приравнивание элементов первых строк матриц .: 2 3 Введение функций: A,B,C 4 (2.82) Решение уравнения (2.81) относительно угла ϑ: Приравнивание элементов вторых строк матриц 5 6 Введение функций: P,Q,R (2.90) 7 Решение уравнения (2.89) относительно угла γ: конец Рисунок 2.6 – Блок-схема алгоритма определения углов наклона блока ДПИ относительно плоскости горизонта

70 Глава 3. ОЦЕНКА ПОГРЕШНОСТЕЙ ОПРЕДЕЛЕНИЯ УГЛОВ НАКЛОНА БЛОКА ДАТЧИКОВ ПЕРВИЧНОЙ ИНФОРМАЦИИ ОТНОСИТЕЛЬНО ОПОРНОЙ ПОВЕРХНОСТИ 3.1 Постановка задачи По сравнению с погрешностями ДМ и методическими погрешностями алгоритма основной вклад в погрешность определения углов наклона блока ДПИ вносят инструментальные погрешности установки ДМ [63]. В данной главе рассматривается влияние отклонения геометрических характеристик кинематической схемы установки системы ДМ на платформе блока ДПИ от их номинальных значений на погрешность определения углов наклона блока ДПИ. Положение i - того ДМ в системе координат, связанной с блоком ДПИ, определяется пятью независимыми параметрами: координатами ydi , rdi , ν i центра Odi, являющегося началом отсчета расстояния до ОПВ или ОПН, и углами μ i , σi поворота системы координат, связанной с i - тым ДМ. В принятой схеме установки ном ДМ (п. 2.5.2 главы 2) номинальные значения координат= ydiном y= rd= , σiном σ d , rdi * ном одинаковы для всех ДМ, а номинальные значения углов= ν iном ν= μ*i i , μi индивидуальны для каждого ДМ, причем ν*i = μ*i . Для оценки влияния на точность вычисления углов наклона блока ДПИ именно инструментальных погрешностей установки ДМ принимается допущение об идеальности характеристик последних, т.е. текущее значение нормированного сигнала на выходе i - того ДМ точно равно измеряемому расстоянию: = Li ( t ) Li ( t ) , i ∈1, N . (3.1) При точном соответствии действительных значений параметров установки ДМ номинальным переменные Li , Li обозначаются символом «*»: = L*i ( t ) L*i ( t ) , i ∈1, N . (3.2)

71  * должны точно В этом случае вычисленные значения опорных углов γ *оп , ϑ оп совпасть с действительными значениями углов наклона блока ДПИ относительно опорной поверхности γ оп , ϑоп : γ *оп ( t ) = γ оп ( t ) , ϑ *оп ( t ) = ϑоп ( t ) . (3.3) При отличии действительных значений параметров установки системы ДМ на платформе блока ДПИ от номинальных получаются соотношения: Li = L*i + ∆Li ( ∆ydi , ∆rdi , ∆ν i , ∆μ i , ∆σi ) , i ∈1, N   γ оп = γ оп +∆γ оп ( ∆Li , i ∈1, N ) ,   ϑ оп = ϑоп + ∆ϑ оп ( ∆Li , i ∈1, N ) ,  где ∆ydi , ∆rdi , ∆ν i , ∆μ i , ∆σi – инструментальные погрешности (3.4) параметров установки ДМ; ∆Li ( ∆ydi , ∆rdi , ∆ν i , ∆μ i , ∆σi ) – отклонение текущего расстояния, измеряемого i - тым ДМ, от номинального значения L*i , обусловленное инструментальными погрешностями установки ДМ на платформе блока ДПИ;  – погрешности вычисления опорных углов крена и тангажа. ∆γ оп , ∆ϑ оп Требуется исследовать влияние инструментальных погрешностей установки ДМ на погрешности вычисления опорных углов крена и тангажа в ожидаемом диапазоне изменения кинематических углов ψ, ϑ, γ ориентации блока ДПИ и расстояния h0 от ОПН до ОПВ. Основные задачи, которые требуется решить: - получение аналитической зависимости расстояния, измеряемого ДМ, от погрешностей его установки на блоке ДПИ; - получение уравнений чувствительности погрешностей определения опорных углов наклона к погрешностям установки ДМ на блоке ДПИ; - проведение факторного эксперимента в пространстве конструктивных параметров установки ДМ на блоке ДПИ; - разработка рекомендаций по дальнейшей алгоритмической компенсации возможных инструментальных погрешностей системы.

72 3.2 Получение аналитической зависимости расстояния, измеряемого дальномерами, от погрешностей его установки на блоке датчиков первичной информации В главе 2 получена математическая модель расстояния, измеряемого i-тым ДМ до ОПВ (формула 2.12): (h = Li 0 − y gOdi ) i 21 c (с ) + (с ) + (с ) i 2 11 i 2 21 i 2 31 . (3.5) При неизменной величине h0 расстояние Li есть функция переменных i i , которые в свою очередь есть функции параметров установки ДМ: , с31 y gOdi , с11i , с21   c11i = sin σi ( sin γ sin ψ − cos γ cos ψ sin ϑ) + cos ψ cos σi cos µi cos ϑ −   − cos σi sin µi ( cos γ sin ψ + cos ψ sin γ sin ϑ ) ,   i c21= sin ϑ cos σi cos µi + cos ϑ sin γ sin µi cos σi + cos γ cos ϑ sin σi ,   i = sin σi ( cos ψ sin γ + cos γ sin ψ sin ϑ) − cos σi cos µi cos ϑ sin ψ −  c31  − cos σi sin µi ( cos γ cos ψ − sin γ sin ψ sin ϑ ) .  y gOdi= y g + rdi cos ν i sin ϑ + ydi cos γ cos ϑ + rdi sin νi cos ϑ sin γ, (3.6) Переменная y g есть ордината полюса Ос системы координат, связанной с блоком ДПИ. При разложении функции (3.5) в ряд Тейлора [61] в пространстве факторов ydi , rdi , ν i , μ i , σi в малой окрестности точки, координаты которой есть номинальные значения параметров установки ДМ, получается уравнение *  ∂Li   ∂y gOdi * Li = Li +  Odi    ∂y   ∂r  g   di *  ∂Li  ∆ + r  di  Odi   ∂y g *     ∂y gOdi   ∂ydi *  ∂Li  ∆ + y  Odi  di   ∂y g   ∂L *  ∂ci *  ∂L *  ∂ci *  ∂L *  ∂ci *  +   ii   11  +  ii   21  +  ii   31   ∆µi +   ∂c11   ∂µi   ∂c21   ∂µi   ∂c31   ∂µi       ∂L *  ∂ci *  ∂L *  ∂ci *  ∂L *  ∂ci *  +   ii   11  +  ii   21  +  ii   31   ∆σi ,   ∂c11   ∂σi   ∂c21   ∂σi   ∂c31   ∂σi     где *     ∂y gOdi   ∂νi *   ∆νi +  (3.7) L*i – расстояние от точки Odi до ОПВ при номинальных значениях параметров установки ДМ.

73 Частные вычисленные производные при функции номинальных Li значениях и переменных параметров i i , , с31 y gOdi , с11i , с21 установки ДМ, определяются выражениями:     *  ∂y gOdi   = γ ϑ cos cos ,   ∂y    di   *  ∂y gOdi  * *   =  rd ( cos ν i cos ϑ sin γ − sin ϑ sin νi ) .  ∂ν i    ∂y gOdi   ∂rdi *  * * = cos ν i sin ϑ + sin ν i cos ϑ sin γ,  (3.8) *   ∂c11i  * *   = − cos σ cos ψ cos ϑ sin µi + ( cos γ sin ψ + cos ψ sin γ sin ϑ ) cos µi  ,    ∂µi   *  ∂c2i 1   * *  (3.9)   = cos σ ( cos ϑ sin γ cos µi − sin ϑ sin µi ) ,  ∂µi   *   ∂c3i 1  * *   = cos σ sin µi cos ϑ sin ψ − cos µi ( cos γ cos ψ − sin γ sin ψ sin ϑ )  .    ∂µi   *   ∂c11i  *   = cos σ ( sin γ sin ψ − cos γ cos ψ sin ϑ) − cos ψ sin σ cos µi cos ϑ +    ∂σi   + sin σ sin µ*i ( cos γ sin ψ + cos ψ sin γ sin ϑ ) ,  *  i  ∂c21   * *    = cos γ cos ϑ cos σ − ( sin ϑ cos µi + cos ϑ sin γ sin µi ) sin σ,  ∂σi   *  i  ∂c31  *    = cos σ ( cos ψ sin γ + cos γ sin ψ sin ϑ) + sin σ cos µi cos ϑ sin ψ +   ∂σi   *  + sin σ sin µi ( cos γ cos ψ − sin γ sin ψ sin ϑ ) .  (310)

74  ∂Li  Odi  ∂y g *  2 1 i* 2 i* 2 − i* ( с11i* ) + ( с21 + ( с31  = ) ), c 21  *  ∂Li   i   ∂c11  )) , = c (с ) + (с ) + (с ) h −(y ) ) ( = − (с ) + (с ) + (с ) c ( ) ( i* 2 11 i* 21 *  ∂Li   i   ∂c21  ( с11i* h0 − y gOdi i* 2 21 i* 2 31 * Odi g 0 * i* 2 11 i* 2 21 i* 2 21 )) (с ) + (с ) + (с ) ( ( i* с31 h0 − y gOdi *  ∂Li   i  = i*  ∂c31  c21 i* 2 11 i* 2 31 + )) ( (с ) + (с ) + (с ) ( h0 − y gOdi i* 2 11 * i* 2 21 i* 2 31 * i* 2 21 i* 2 31 .           (3.11)  ,       i i , помеченные в (3.11) символом «*», Значения переменных y gOdi , с11i , с21 , с31 вычисляются по формулам: ( y )= y g + rd cos ν*i sin ϑ + yd cos γ cos ϑ + rd sin ν*i cos ϑ sin γ,   i*  с11 = sin σ ( sin γ sin ψ − cos γ cos ψ sin ϑ ) +  * * + cos σ cos ψ cos µi cos ϑ − sin µi ( cos γ sin ψ + cos ψ sin γ sin ϑ )  ,   i* * * с21 = ( sin ϑ cos µi + cos ϑ sin γ sin µi ) cos σ + cos γ cos ϑ sin σ,   i* с31 = sin σ ( cos ψ sin γ + cos γ sin ψ sin ϑ) −   − cos σ cos µ*i cos ϑ sin ψ + sin µ*i ( cos γ cos ψ − sin γ sin ψ sin ϑ )  .  Odi g * (3.12) Вводятся обозначения: * *  ∂Li   ∂y gOdi  y L , Li di =  =   Odi      ∂y g   ∂rdi  rdi i * *  ∂Li   ∂y gOdi  , Lνi i =   Odi      ∂y g   ∂ydi  * * * * * * * * * * * *  ∂Li  Odi  ∂y g *     ∂L   ∂ci   ∂L   ∂ci   ∂L   ∂ci  Lµi i =  ii   11  +  ii   21  +  ii   31  ,  ∂c11   ∂µi   ∂c21   ∂µi   ∂c31   ∂µi   ∂L   ∂ci   ∂L   ∂ci   ∂L   ∂ci  Lσi i =  ii   11  +  ii   21  +  ii   31  .  ∂c11   ∂σi   ∂c21   ∂σi   ∂c31   ∂σi   ∂y gOdi   ∂νi *     ,     (3.13)      С учетом обозначений (3.13) уравнение (3.7) примет вид: L=i L*i + Lridi ∆rdi + Li di ∆y di + Lνi i ∆νi + Lµi i ∆µi + Lσi i ∆σi , y (3.14)

75 Получено уравнение чувствительности расстояния Li , измеряемого i - тым ДМ, к инструментальным погрешностям параметров установки ДМ на платформе блока ДПИ. Ясно, что номинальное расстояние L*i и частные производные Li di , Lridi , Lνi i , Lµi i , Lσi i зависят от углов ψ, ϑ, γ пространственной ориентации блока y ДПИ и расстояния h0. 3.3 Получение уравнений чувствительности погрешностей определения опорных углов наклона к погрешностям установки дальномеров на блоке датчиков первичной информации 3.3.1 Схема с тремя ДМ В вычислительное устройство дальномерной СУО поступают сигналы с трех ДМ: Li= L*i + ∆Li , i= 1,2,3 . (3.15) Сигналы опорных углов крена и тангажа вычисляются по формулам, полученным в главе 2:   3 ( L3 − L2 )( L1 cos σ + rd )  sin σ , γ оп = arctg   ( L1L2 + L1L3 + L2 L3 ) cos 2 σ + 2rd ( L1 + L2 + L3 ) cos σ + 3rd2     Q L , L , L  ( 1 2 3)   ϑоп = arctg   V L , L , L  ( 1 2 3)   где . { (3.17) } Q ( L1 , L2 , L3= ) sin σ 2 L2 L3 − L1 ( L3 + L2 ) cos σ − ( 2 L1 − L3 − L2 ) rd ; ) V ( L1 , L2 , L= 3 = Uϑ F= ϑ (U 2 ϑ + 3Fϑ2 ) ; ( L L + L L + L L ) cos σ + 2r ( L + L ( L − L )( L cos σ + r ) sin σ . 2 1 2 3 1 3 2 2 3 1 Вводятся обозначения: d d 1 2 + L3 ) cos σ + 3rd2 ; (3.16)

76 3 ( L3 − L2 )( L1 cos σ + rd ) sin σ,    = Bγ ( L1L2 + L1L3 + L2 L3 ) cos 2 σ + 2rd ( L1 + L2 + L3 ) cos σ + 3rd2 ,  (3.18)    2  Bϑ ( L1L2 + L1L3 + L2 L3 ) cos 2 σ + 2rd ( L1 + L2 + L3 ) cos σ + 3rd2  +  =  1  2 2 2       +3 ( L3 − L2 )( L1 cos σ + rd )  sin σ .  (3.19) = Aγ { } sin σ  2 L2 L3 − L1 ( L3 + L2 )  cos σ − ( 2 L1 − L3 − L2 ) rd , A= ϑ { } С учетом обозначений (3.18), (3.19) уравнения (3.16), (3.15) примут вид: A γ оп = arctg  γ B  γ   .  (3.20) A   = ϑ arctg  ϑ  оп  Bϑ  (3.21) Разложим функцию (3.20) в линеаризованный ряд Тейлора [61] в пространстве переменных Li= L*i + ∆Li , i= 1,2,3 . ( A ) B − (B ) +∑ ( A ) + (B ) ' γ оп =γоп 3 ' * γ γ L i γ L i 2 * * 2 γ i =1 Aγ* ∆Li . (3.22) γ Второе слагаемое в уравнении (3.22) есть погрешность вычисления опорного угла крена: ( A ) B − (B ) = ∑ ( A ) + (B ) ' ∆γ оп 3 i =1 γ L i ' * γ γ L i 2 * * 2 γ Aγ* ∆Li . (3.23) γ Выражения для частных производных ( Aγ ) L , i = 1,2,3 : ' i    ' * ( Aγ )L2 = − 3 sin σ ( L1 cos σ + rd ) ,  ' * 3 sin σ ( L1 cos σ + rd ) .  ( A= γ )L 3  (A= ) ' γ L 1 3 sin ( 2σ ) ( L*3 − L*2 ) , 2 (3.24)

77 Выражения для частных производных ( Bγ ) L , i = 1,2,3 : ' i ( B )= ( L + L ) cos ( B )= ( L + L ) cos ( B )= ( L + L ) cos ' γ L 1 * 2 * 3 * 1 * 3 * 1 * 2 ' γ L 2 ' γ L 3 σ + 2rd cos σ,    2 σ + 2rd cos σ,   2 σ + 2rd cos σ.   2 (3.25) Переменные Aγ , Bγ при номинальных значениях параметров установки ДМ вычисляются по формулам: 3 ( L*3 − L*2 )( L*1 cos σ + rd ) sin σ,    Bγ* ( L*1L*2 + L*1L*3 + L*2 L*3 ) cos 2 σ + 2rd ( L*1 + L*2 + L*3 ) cos σ + 3rd2 . = Aγ* = (3.26) При подстановке в (3.23) вариаций ∆Li = Li − L*i , i = 1,2,3 (уравнение 3.14) получается выражение ( A ) B − (B ) =∑ ( A ) + (B ) ' ∆γ оп 3 i =1 ' * γ γ L i γ L i * 2 * 2 γ Aγ* ( L ∆r rdi i di ) + Li di ∆y di + Lνi i ∆ν i + Lµi i ∆µi + Lσi i ∆σi . (3.27) y γ Вводятся обозначения коэффициентов чувствительности погрешности вычисления опорного угла крена: ( A ) B − (B ) ( A ) B − (B ) A = L , γ ( A ) + (B ) ( A ) + (B ) ( A ) B − (B ) ( A ) B − (B ) A = L , γ + A B ( A ) + (B ) ( ) ( ) ( A ) B − (B ) A = L , i = 1,2, 3. ( A ) + (B ) ' γ rdi оп γ L i γ L i γ L i * γ ' rdi i ydi оп γ L i ' * γ γ L i 2 * * γ ' νi i γ ' * γ γ L i * 2 * 2 γ γ * γ σi i µi оп * γ * 2 γ γ * 2 γ ' σi γ оп γ L i * 2 * 2 γ ' νi γ оп ' * γ γ L i * γ * 2 γ ' γ L i * 2 γ ' γ L i 2 * γ  L ,    Aγ*  Lµi i ,        Aγ* y di i (3.28) С учетом обозначений (3.27) уравнение (3.26) примет вид: 3 ( ) ∆γ оп = ∑ γопrdi ∆rdi + γопydi ∆y di + γопνi ∆νi + γµопi ∆µi + γопσi ∆σi . i =1 (3.29)

78 Разложим функцию (3.21) в линеаризованный ряд Тейлора [61] в пространстве переменных Li= L*i + ∆Li , i= 1,2,3 . ( Aϑ )L Bϑ* − ( Bϑ )L ' 3  =ϑ + ϑ ∑ оп оп ' i ( A ) + (B ) * 2 ϑ i =1 i Aϑ* * 2 ϑ ∆Li . (3.30) Второе слагаемое в уравнении (3.30) есть погрешность вычисления опорного угла тангажа: = ∆ϑ оп 3 ∑ i =1 ( Aϑ )L Bϑ* − ( Bϑ )L ' ' i ( A ) + (B ) * 2 ϑ i Aϑ* * 2 ϑ ∆Li . (3.31) Выражения для частных производных ( Aϑ ) L , i = 1,2,3 : ' i ) ( = − sin σ cos σ ( L*2 + L*3 ) + 2rd ,   ' * * ( Aϑ )L=2 sin σ ( 2 L3 − L1 ) cos σ + rd  ,   ' * *    = σ − σ + A L L r sin 2 cos . ( ϑ )L3 d ( 2 1 )  ( Aϑ )L ' 1 (3.32) Выражения для частных производных ( Bϑ ) L , i = 1,2,3 : ' i 2U ϑ* ( L*2 + L*3 ) cos 2 σ + 2rd cos σ  + 3Fϑ* ( L*3 − L*2 ) sin ( 2σ )   , ( Bϑ ) L1 = * * *  2 V ( L1 , L2 , L3 )   2U ϑ* ( L*1 + L*3 ) cos 2 σ + 2rd cos σ  − 6 Fϑ* ( L*1 cos σ + rd ) sin σ  ' , ( Bϑ ) L2 = * * * 2 V ( L1 , L2 , L3 )   * * * 2 * *    U L L r F L r 2 cos 2 cos 6 cos sin + σ + σ + σ + σ ϑ ( 1 d ϑ ( 1 d) 2) '  . ( Bϑ ) L3 =  2 V ( L*1 , L*2 , L*3 )  ' U ϑ , Fϑ , V ( L1 , L2 , L3 ) , Aϑ , Bϑ Переменные при номинальных (3.33) значениях параметров установки ДМ вычисляются по формулам: ( L L + L L + L L ) cos σ + 2r ( L + L F= ( L − L )( L cos σ + r ) sin σ, V ( L , L= , L ) ( (U ) + 3 ( F ) ) , = U ϑ* * * 1 2 * ϑ * 3 * 1 * 2 * * 1 3 * 2 * 3 * * 2 3 * 1 2 d d * 2 ϑ * 2 ϑ * 1 * 2  + L*3 ) cos σ + 3rd2 ,       (3.34)

79 { } * A= sin σ  2 L*2 L*3 − L*1 ( L*3 + L*2 )  cos σ − ( 2 L*1 − L*3 − L*2 ) rd ,  ϑ    Bϑ* = V ( L*1 , L*2 , L*3 ) .  (3.35) При подстановке в (3.35) вариаций ∆Li = Li − L*i , i = 1,2,3 (уравнение 3.14) получается выражение 3 ∑  = ∆ϑ оп ( Aϑ )L Bϑ* − ( Bϑ )L i =1 ' ' i ) (A * 2 ϑ + (B ) i Aϑ* * 2 ϑ ( L ∆r rdi i di ) + Li di ∆y di + Lνi i ∆ν i + Lμi i ∆μ i + Lσi i ∆σi (3.36) y Вводятся обозначения коэффициентов чувствительности погрешности вычисления опорного угла тангажа: ( Aϑ )L Bϑ* − ( Bϑ )L '  rdi = ϑ оп ' Aϑ* r i i  ydi = Lidi , ϑ оп * 2 * 2 ( Aϑ ) + ( Bϑ ) ( Aϑ )L Bϑ* − ( Bϑ )L '  νi = ϑ оп ' Aϑ* ν i i  µi = Li i , ϑ оп * 2 * 2 ( Aϑ ) + ( Bϑ ) ( Aϑ )L Bϑ* − ( Bϑ )L '  = ϑ σi оп ' i ( A ) + (B ) * 2 ϑ i * 2 ϑ Aϑ*  L ,   Aϑ* µ  Li i ,       (3.37) ) (3.38) ( Aϑ )L Bϑ* − ( Bϑ )L ' ' i i ( A ) + (B ) * 2 ϑ * 2 ϑ ( Aϑ ) L Bϑ* − ( Bϑ )L ' ' i ( A ) + (B ) * 2 ϑ i * 2 ϑ Lσi i , i = 1,2, 3. Aϑ* y di i С учетом обозначений (3.37) уравнение (3.36) примет вид: = ∆ϑ оп Получены ∑ ( ϑ 3 i =1 rdi оп  ydi ∆y + ϑ  νi ∆ν + ϑ  µi ∆µ + ϑ  σi ∆σ . ∆rdi + ϑ i i i оп оп оп оп di уравнения чувствительности (3.29), (3.38) погрешностей вычисления опорных углов крена и тангажа дальномерной СУО с тремя ДМ к инструментальным погрешностям параметров установки ДМ на платформе блока ДПИ. 3.3.2 Схема с четырьмя ДМ В вычислительное устройство дальномерной СУО поступают сигналы с четырех ДМ: Li= L*i + ∆Li , i= 1,2,3,4 . (3.39) Сигналы опорных углов крена и тангажа вычисляются по формулам, поученным в главе 2:

80 A γ оп = arctg  γ B  γ где   ,  (3.40) A   = ϑ arctg  ϑ  , оп  Bϑ  (3.41) Aγ = ( L4 − L2 ) sin σ ; B= γ ( L A= ϑ ( L − L ) ( L = Bϑ ( 2 3 + L4 ) cos σ + 2rd ; 1 2 + L4 ) cos σ + 2rd  sin σ ; ) ( L3 + L1 ) cos σ + 2rd 2 ( L2 + L4 ) cos σ + 2rd  + ( L4 − L2 ) sin 2 σ .   2 Разложим функцию (3.40) в линеаризованный ряд Тейлора [61] в пространстве переменных Li= L*i + ∆Li , i= 2,4 . ( A ) B − (B ) = γ + ( A ) + (B ) ' γ оп γ L 2 оп ' * γ γ L 2 * 2 * 2 γ Aγ* ( A ) B − (B ) + ( A ) + (B ) ' ∆L2 γ ' * γ γ L 4 γ L 4 * 2 * 2 γ Aγ* ∆L4 . (3.42) γ Сумма второго и третьего слагаемых в уравнении (3.42) есть погрешность вычисления опорного угла крена: ( A ) B − (B ) = ( A ) + (B ) ' ∆γ оп γ L 2 * γ * 2 γ ' γ L 2 * 2 γ Aγ* ( A ) B − (B ) + ( A ) + (B ) ' ∆L2 γ L 4 ' * γ γ L 4 * 2 * 2 γ Aγ* ∆L4 . (3.43) γ Выражения для частных производных ( Aγ ) L , ( Bγ ) L , i = 2,4 : ' ' i (A ) (B ) ' γ L 2 ' γ L 2 = − sin σ, = cos σ, (A ) (B ) ' γ L 4 ' γ L 4 i = sin σ,    = cos σ.  (3.44) Переменные Aγ , Bγ при номинальных значениях параметров установки ДМ вычисляются по формулам: Aγ* = ( L*4 − L*2 ) sin σ,   . * * * B= ( L2 + L4 ) cos σ + 2rd .  γ (3.45)

81 При подстановке в (3.43) вариаций ∆Li = Li − L*i , i = 2,4 (уравнение 3.14) получается выражение: ( A ) B − (B ) A ∆γ = (L A B + ( ) ( ) ( A ) B − (B ) A L ∆r + ( ( A ) + (B ) ' оп ' * γ γ L 2 * 2 γ ' γ L 4 * γ γ L 4 * 2 * 2 γ rd 2 2 ) ∆rd 2 + L2d 2 ∆y d 2 + Lν22 ∆ν 2 + Lµ22 ∆µ 2 + Lσ22 ∆σ2 + y γ ' * γ * γ γ L 2 * 2 rd 4 4 d4 (3.46) ) + L4d 4 ∆y d 4 + Lν44 ∆ν 4 + Lµ44 ∆µ 4 + Lσ44 ∆σ 4 . y γ Вводятся обозначения коэффициентов чувствительности погрешности вычисления опорного угла крена: ( A ) B − (B ) ( A ) B − (B ) A = L , γ ( A ) + (B ) ( A ) + (B ) ( A ) B − (B ) ( A ) B − (B ) A = L , γ ( A ) + (B ) ( A ) + (B ) ( A ) B − (B ) A = L , ( A ) + (B )  L ,    Aγ*  Lµ22 ,        (3.47) ( A ) B − (B ) ( A ) B − (B ) A = L , γ ( A ) + (B ) ( A ) + (B ) ( A ) B − (B ) ( A ) B − (B ) A = L , γ + A B ( A ) + (B ) ( ) ( ) ( A ) B − (B ) A = L , ( A ) + (B )  L ,    Aγ*  Lµ44 ,        (3.48) ' γ rd 2 оп γ L 2 * 2 γ ' ν2 γ оп * γ γ L 2 * 2 γ ' γ σ2 оп * γ γ L 2 * 2 γ ' γ rd 4 оп γ L 2 γ L 2 * γ * 2 γ ' σ4 γ оп * γ * 2 γ ' ν4 γ оп * γ γ L 2 * γ * 2 γ ' γ L 2 * 2 * γ ' rd 2 2 γ L 2 yd 2 оп γ L 2 * 2 * γ ' ν2 2 µ2 оп ' * γ * γ γ L 2 * 2 γ γ γ L 2 * 2 γ L 2 * 2 * 2 γ γ ' ' * γ γ ' γ L 2 * 2 γ σ2 2 γ ' γ L 2 * 2 * γ ' rd 4 4 γ L 2 yd 4 оп ' * γ ' ν4 4 µ4 оп γ L 2 γ ' γ L 2 * 2 * γ γ L 2 * 2 * 2 γ γ γ L 2 2 * ' * γ * γ * 2 γ γ ' γ L 2 2 * γ σ4 4 γ Aγ* Aγ* yd 2 2 yd 4 2 С учетом обозначений (3.47), (3.48) уравнение (3.46) примет вид: rd 2 yd 2 ∆γ оп = γ оп ∆rd 2 + γ оп ∆y d 2 + γ νоп2 ∆ν 2 + γ µоп2 ∆µ 2 + γ σоп2 ∆σ2 + rd 4 yd 4 +γ оп ∆rd 4 + γ оп ∆y d 4 + γ νоп4 ∆ν 4 + γ µоп4 ∆µ 4 + γ σоп4 ∆σ4 . (3.49) Разложим функцию (3.41) в линеаризованный ряд Тейлора [61] в пространстве переменных Li= L*i + ∆Li , i= 1,2, 3, 4 .

82 ( Aϑ )L Bϑ* − ( Bϑ )L ' 4  =ϑ + ϑ ∑ оп оп ' i ( Aϑ* ) + ( Bϑ* ) 2 i =1 i Aϑ* 2 ∆Li . (3.50) Второе слагаемое в уравнении (3.50) есть погрешность вычисления опорного угла тангажа:  = ∆ϑ оп 4 ∑ ( Aϑ )L Bϑ* − ( Bϑ )L ' ' i ( A ) + (B ) * 2 ϑ i =1 i Aϑ* * 2 ϑ ∆Li . (3.51) Выражения для частных производных ( Aϑ ) L , i = 1, 2, 3, 4 : ' i = − ( L*2 + L*4 ) cos σ + 2rd  sin σ,    1 ' ( Aϑ )L2 =( L*3 − L*1 ) sin ( 2σ ) ,  2  ' ( Aϑ= )L3 ( L*2 + L*4 ) cos σ + 2rd  sin σ,   1 * '  ( Aϑ )L4 =( L3 − L*1 ) sin ( 2σ ).   2 ( Aϑ )L ' 1 (3.52) Выражения для частных производных ( Bϑ ) L , i = 1, 2, 3, 4 : ' i 2 2  cos σ ( L*2 + L*4 ) cos σ + 2rd  + ( L*4 − L*2 ) sin 2 σ ,  1  * * * * L + L cos σ + 2 r L + L cos 2 σ + 2 r cos σ ( ) d ) , ( 3 1) 4 d ( 2 ' ( Bϑ )L2 =  2 * * * * 2 2  ( L2 + L4 ) cos σ + 2rd  + ( L4 − L2 ) sin σ     2 ' * * * * 2 2 cos σ ( L2 + L4 ) cos σ + 2rd  + ( L4 − L2 ) sin σ ,  ( Bϑ )= L3   * * * * L3 + L1 ) cos σ + 2rd ( L2 cos ( 2σ ) + L4 + 2rd cos σ )  ( ' . ( Bϑ )L4 = 2 * * * * 2 2 ( L2 + L4 ) cos σ + 2rd  + ( L4 − L2 ) sin σ     ( Bϑ )= L ' ( ) ( (3.53) ) Переменные Aϑ , Bϑ при номинальных значениях параметров установки ДМ вычисляются по формулам: A= ϑ (L = Bϑ (( L + L ) cos σ + 2r ) * 3 − L*1 ) ( L*2 + L*4 ) cos σ + 2rd  sin σ, * 3 * 1 d    2 2 ( L*2 + L*4 ) cos σ + 2rd  + ( L*4 − L*2 ) sin 2 σ .    (3.54)

83 При подстановке в (3.51) вариаций ∆Li = Li − L*i , i = 2,4 (уравнение 3.14) получается выражение:  ∆ϑ оп ( Aϑ )L Bϑ* − ( Bϑ )L ' 4 ∑ i =1 ' i ( A ) + (B ) * 2 ϑ i Aϑ* * 2 ϑ ( L ∆r rdi i + Li di ∆y di + Lνi i ∆νi + Lµi i ∆µi + Lσi i ∆σi y di ) (3.55) Вводятся обозначения коэффициентов чувствительности погрешности вычисления опорного угла тангажа: '  = ϑ rdi оп '  νi = ϑ оп ' Aϑ* r i i  ydi = Lidi , ϑ оп 2 2 ( Aϑ* ) + ( Bϑ* ) * 2 ϑ  σi = ϑ оп ' i i ( A ) + (B ) * 2 ϑ Aϑ* * 2 ϑ ) (3.57) * 2 ϑ ( Aϑ )L Bϑ* − ( Bϑ )L ' Aϑ* ν i i  µi = Li i , ϑ оп 2 2 * * + A B ( ϑ) ( ϑ) ' i ( A ) + (B ) ' ( Aϑ )L Bϑ* − ( Bϑ )L (3.56) ' i ( Aϑ )L Bϑ* − ( Bϑ )L '  L ,   * Aϑ µ  Li i ,       ( Aϑ )L Bϑ* − ( Bϑ )L ( Aϑ )L Bϑ* − ( Bϑ )L ' i ( A ) + (B ) * 2 ϑ i * 2 ϑ Lσi i , i = 1,2, 3,4. Aϑ* y di i С учетом обозначений (3.56) уравнение (3.55) примет вид: = ∆ϑ оп Получены ∑ ( ϑ 4 i =1 rdi оп  ydi ∆y + ϑ  νi ∆ν + ϑ  µi ∆µ + ϑ  σi ∆σ . ∆rdi + ϑ i i i оп оп оп оп di уравнения чувствительности (3.49), (3.57) погрешностей вычисления опорных углов крена и тангажа дальномерной СУО с четырьмя ДМ к инструментальным погрешностям параметров установки ДМ на платформе блока ДПИ. 3.4 Проведение факторного эксперимента в пространстве конструктивных параметров установки ДМ на блоке ДПИ Уравнения чувствительности погрешностей вычисления опорных значений углов крена (3.29), (3.49) и тангажа (3.38), (3.57) есть уравнения с переменными коэффициентами, которые при выбранных номинальных значениях конструктивных параметров установки ДМ зависят от углов крена и тангажа блока ДПИ, а также от его расстояния от ОПВ (h 0 ) − y gOdi . Возникает задача оценки

84 предельных областей существования погрешностей вычисления опорных углов крена и тангажа блока ДПИ в пространстве конструктивных параметров установки ДМ при их возможных отклонениях от номинальных значений с учетом изменения углового положения блока ДПИ в допустимых пределах углов крена и тангажа. В группу независимых конструктивных параметров установки ДМ на блоке ДПИ входят пять величин: 1) угол σi, i=∈1, N наклона измерительной оси ДМ к плоскости XcOcZc системы координат, связанной с блоком ДПИ; 2) угол µi, i=∈1, N между плоскостями Xc1OdiYc1 и XdiOdiYdi; 3) радиус окружности rdi, i=∈1, N на которой располагается центр Odi; 4) ордината ydi, i=∈1, N центра Odi в связанной системе координат XcOcZc; 5) угол νi, i=∈1, N, определяющий положение радиуса-вектора в плоскости XcOcZc. Номинальные значения конструктивных параметров σ*, rd*, yd* одинаковы для всех ДМ. Номинальные значения параметров µi*, i=∈1, N и νi*=µi*, i=∈1, N индивидуальны и зависят от количества ДМ N. При размещении на блоке ДПИ N ДМ получается 5⋅N независимых конструктивных параметров, каждый из которых имеет случайное отклонение от своего номинала. rdi= rd* + ∆rdi , ν i = ν*i + ∆ν i , σi = σ* + ∆σi , ydi= yd* + ∆ydi ,   µi = µ*i + ∆µi ,   i ∈1, N .  (3.58) Учитывая габариты исследуемого объекта и технологию установки ДМ, принимаются следующие значения допусков конструктивных параметров, соответствующие современным реальным техническим возможностям: ∆rd = ±1мм, ∆yd = ±1мм, ∆ν = ±1о , ∆µ = ±1о , ∆σ = ±1о . (3.59) Для каждого из конструктивных параметров план всех возможных вариантов значений отклонений (3.59) соответствует матрице планирования полного факторного эксперимента (ПФЭ) типа 2N, приведенной в таблице 3.1 для N=3 [64].

85 Таблица 3.1 – Матрица планирования ПФЭ 23 k ( zi1 )k ( zi1 )k ( zi1 )k 1 2 3 4 5 6 7 8 -1 1 -1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1 1 -1 -1 -1 -1 1 1 1 1 Символами ( zi1 )k , i ∈1,5 обозначены нормированные факторы, связанные с действительными переменными формулами: rdi − rd* ydi − yd* ν i − ν*  = zi 2 = zi 3 , , , ∆rd ∆yd ∆ν   µ i − µ* σi − σ* , = , = zi 4 zi 5 i ∈1, N .  ∆µ ∆σ  = zi1 При выбранных значениях единиц варьирования (3.60) (3.59) значения нормированных факторов в таблице 3.1 совпадают со значениями действительных переменных. Номинальные значения факторов для случая N=3 приведены в таблице 3.2. Таблица 3.2 – Номинальные значения и единицы варьирования факторов № Номинальное Факторы п/п значение 1 rdi, i=∈1, 3 rd*=10 мм 2 ydi, i=∈1, 3 yd*=10 мм ν1*=0 3 νi, , i=∈1, 3 ν2*=2π/3 ν3*=4π/3 µ1*=0 4 µi, , i=∈1, 3 µ2*=2π/3 µ3*=4π/3 5 σi, , i=∈1, 3 σ*=π/3 Единица варьирования λ1=|∆rd|=1 мм λ2=|∆yd|=1 мм λ3=|∆ν|=1 градус λ4=|∆µ|=1 градус λ5=|∆σ|=1 градус

86 Диапазон изменения углов тангажа и крена при определении предельных границ области погрешностей определения опорных углов блока ДПИ приведен в таблице 3.3. Таблица 3.3 – Область поиска предельных значений погрешностей вычисления опорных углов наклона в пространстве угловых координат ϑ, γ . № п/п 1 2 3 4 5 6 7 ϑ,о -30 -30 -30 -30 -30 -30 -30 +30 +30 +30 +30 +30 +30 +30 γ,о -30 -20 -10 0 +10 +20 +30 Для каждой строки значений углов крена и тангажа (таблица 3.3) определяются значения коэффициентов чувствительности и погрешностей вычисления опорных углов при изменении каждого из конструктивных параметров установки ДМ в соответствии с таблицей 3.1, определяются минимальные и максимальные значения погрешностей, которые затем суммируются. Для системы с тремя ДМ расчет выполнен по программе, текст которой приведен в приложении В. Расчет выполнен при значении расстояния между ОПН и ОПВ 3 м, расстояния блока ДПИ от ОПН 1,8 м. Результаты расчета приведены в виде графиков границ областей погрешностей вычисления опорных углов крена и тангажа на рисунках 3.1 – 3.2. Результаты аналогичного расчета для системы с четырьмя ДМ отражены на рисунках 3.3 – 3.4. Как показали результаты расчета, наибольшее влияние на величину погрешности вычисления опорных углов крена и тангажа оказывают инструментальные погрешности углов выставки измерительных осей ДМ, т.е. факторы σi , µi , i ∈1, N .

87 Область возможных погрешностей вычисления оп =-30 ; o Область возможных погрешностей вычисления 2 а) оп б) 1.5 1.5 1 1 m in m in оп 0.5 оп max 0.5 max o оп 0 оп o оп оп o =-30 ; 2 0 -0.5 -0.5 -1 -1 -1.5 -1.5 -2 -30 -20 -10 10 0 20 -2 -30 30 -20 -10 10 0 20 30 ,o ,o Рисунок 3.1 – Границы области погрешности вычисления опорных углов крена (а) и тангажа (б) в зависимости от угла крена блока ДПИ при значении угла тангажа минус 30о для системы с тремя ДМ Область возможных погрешностей вычисления оп =30 ; o Область возможных погрешностей вычисления 2.5 2 а) оп o =30 ; 1.5 б) 1.5 1 m in оп max 1 m in оп 0.5 оп 0.5 max o 0 оп оп o оп 0 -0.5 -0.5 -1 -1.5 -1 -2 -1.5 -2.5 -30 -20 -10 10 0 20 -30 30 -20 -10 0 10 20 30 ,o ,o Рисунок 3.2 – Границы области погрешности вычисления опорных углов крена (а) и тангажа (б) в зависимости от угла крена блока ДПИ при значении угла тангажа 30о для системы с тремя ДМ Область возможных погрешностей вычисления оп =-30 ; o Область возможных погрешностей вычисления 1.5 а) оп =-30 ; o 2 2 б) 1 1.5 1 m in оп max o оп 0 -0.5 -0.5 -1 -1 -1.5 -1.5 -20 -10 0 ,o 10 20 оп max 0 -2 -30 m in 0.5 оп оп оп o 0.5 30 -2 -30 -20 -10 0 10 20 30 ,o Рисунок 3.3 – Границы области погрешности вычисления опорных углов крена (а) и тангажа (б) в зависимости от угла крена блока ДПИ при значении угла тангажа минус 30о для системы с четырьмя ДМ

88 Область возможных погрешностей вычисления оп ; =30 o Область возможных погрешностей вычисления 2 а) оп ; =30 o 2 1.5 б) 1 1.5 1 m in оп 0.5 m in 0.5 max оп оп 0 оп o оп o max 0 -0.5 -0.5 -1 -1 -1.5 -1.5 -2 -30 -20 -10 0 10 20 оп 30 -2 -30 -20 -10 ,o 0 10 20 30 ,o Рисунок 3.4 – Границы области погрешности вычисления опорных углов крена (а) и тангажа (б) в зависимости от угла крена блока ДПИ при значении угла тангажа 30о для системы с четырьмя ДМ Представляет интерес факторное исследование Simulink-модели дальномерной СУО, структурная схема которой для системы с тремя ДМ приведена на рисунке 3.5. Рисунок 3.5 – Структурная схема модели дальномерной СУО в режиме факторного исследования Модель дальномерной СУО входит в состав имитационной математической модели персональной ИИС, описание и структура которой приведены в приложении Г. Отличие приведенной схемы модели дальномерной СУО от описанной в приложении состоит в организации дополнительных блоков и связей, необходимых для выполнения ПФЭ. В основном это касается схемы блока «В.2.2.Инструментальные погрешности», приведенной на рисунке 3.6.

89 Рисунок 3.6 – Структурная схема подсистемы «Блок В.2.2.Инструментальные погрешности» Полный факторный эксперимент типа 2N позволяет получить уравнение регрессии, представляющее собой зависимость целевой функции от факторов в заданной области их изменения [64]. Целевыми функциями являются погрешности вычисления опорных углов крена и тангажа. В качестве факторов выберем наиболее значимые σi , i ∈1, N с единицами варьирования λ5=1 градус. Структура уравнений регрессии для N=3 с учетом z5i = ∆σi , i ∈1,3 имеет вид [64]: 3  ∆γ оп= b0γ + ∑ biγ ∆σi + b4γ ∆σ1∆σ2 + b5γ ∆σ1∆σ3 + b6γ ∆σ2 ∆σ3 + b7γ ∆σ1∆σ2 ∆σ3 ,   i =1  (3.61) 3 ∆ϑ оп= b0ϑ + ∑ biϑ∆σi + b4ϑ∆σ1∆σ2 + b5ϑ∆σ1∆σ3 + b6ϑ∆σ2 ∆σ3 + b7ϑ∆σ1∆σ2 ∆σ3 ,   i =1 где biγ , biϑ , i ∈ 0, 7 – искомые коэффициенты уравнений регрессии. Коэффициенты уравнений регрессии вычисляются по формуле [64] B = ( Xт X ) XтY , −1 где Х – полная матрица планирования ПФЭ 23: (3.62)

90 −1 −1 1 −1 −1 1 1 1 −1 −1 1 −1 −1 1 1 1 1 1  1  1 X= 1  1 1  1 −1 1 1 −1 −1 −1 −1 −1 1 −1 1 −1 1 1 −1 1 −1 1 1 −1 −1 1 1 1 1 −1 1 1  −1 1   −1 −1 ; −1 1   −1 −1 1 −1  1 1  В – матрица искомых коэффициентов уравнений регрессии: т  b0γ b1γ b2γ b3γ b4γ b5γ b6γ b7γ  ; B= ϑ ϑ ϑ ϑ ϑ ϑ ϑ ϑ b0 b1 b2 b3 b4 b5 b6 b7  Y – матрица значений целевых функций ∆γ оп и ∆ϑ оп , полученных при выполнении ПФЭ:  ( ∆γ оп )1 Y=   ∆ϑоп 1 ( ( ∆γ оп )2 ( ∆γ оп )3 ( ∆γ оп )4 ( ∆γ оп )5 ( ∆γ оп )6 ( ∆γ оп )7 ( ∆γ оп )8  т  . ) ( ∆ϑ ) ( ∆ϑ ) ( ∆ϑ ) ( ∆ϑ ) ( ∆ϑ ) ( ∆ϑ ) ( ∆ϑ )  оп оп 2 3 оп 4 оп оп 5 Так как единицы варьирования факторов оп 6 оп 7 z5i = ∆σi , i ∈1,3 8 равны 1, коэффициенты biγ , biϑ , i ∈1,3 есть оценки коэффициентов чувствительности σi γ σопi , ϑ оп , i ∈1,3 . В таблице 3.4 приведены значения коэффициентов уравнений регрессии и значения коэффициентов чувствительности σi γ σопi , ϑ оп , i ∈1,3 , полученные соответственно при реализации плана ПФЭ 23 на Simulink-модели дальномерной СУО при идеальных характеристиках ДМ и при расчете по программе приложения В при значениях γ=-30о, ϑ=-30о. По результатам расчета видно, что оценка коэффициентов чувствительности выполнена с высокой точностью. Кроме того, ПФЭ позволяет определить влияние на целевые функции взаимодействий факторов в виде их произведений. Например, для функции ∆ϑ оп влияние взаимодействия ∆σ1∆σ3 , оцениваемое коэффициентом b5γ =0,0037, оказалось выше, чем влияние фактора ∆σ3 , оцениваемое коэффициентом b3γ =0,02. Коэффициенты b0γ =8,75е-04 и b0ϑ = 0,0076 оценивают

91 величины целевых функций в центре ПФЭ, т.е. их теоретическое значение должно быть равно нулю. Если можно принять b0γ ≈0, то величина b0ϑ = 0,0076 говорит о том, что ощутимо влияние на целевую функцию ∆ϑ оп квадратов факторов, оценка которых смешана с коэффициентом b0ϑ . Таблица 3.4 – Результаты расчета коэффициентов уравнений регрессии и коэффициентов чувствительности при значениях γ=-30о, ϑ=-30о k ∆σ1ko ∆σ2ko ∆σ3ko ( ∆γ оп )k ( ∆ϑ ) 1 2 3 4 5 6 7 8 -1 1 -1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1 1 -1 -1 -1 -1 1 1 1 1 -0,9901 -0,6266 0,3294 0,6805 -0,6891 -0,3266 0,6399 0,9896 -0,7368 1,3810 -1,4020 0,7083 -0,7036 1,4290 -1,3700 0,7550 3.5 Разработка рекомендаций оп по k γ σопi , bkγ−1 bkϑ−1 8,75e-4 0,1784 0,659 0,1526 -0,0031 -0,0003 0,0023 -5e-5 0,0076 1,0607 -0,3348 0,0200 -0,0019 0,0037 -0,0003 -1,2e-5 дальнейшей i ∈1,3 0,1784 0,6589 0,1526 ϑ σопi , i ∈1,3 1,0606 -0,3347 0,0200 алгоритмической компенсации возможных инструментальных погрешностей системы При использовании в составе автономной персональной ИИС дальномерной СУО встает задача алгоритмической компенсации погрешностей параметров установки ДМ на блоке ДПИ, влияющих на точность определения опорных углов крена и тангажа последнего. Для решения поставленной задачи необходимо иметь инструмент воздействия на алгоритм вычисления опорных углов с целью коррекции результатов расчета при минимизации погрешности их определения. Формулы расчета опорных углов крена и тангажа для систем с тремя и четырьмя ДМ получены при номинальных значениях параметров установки ДМ и не имеют возможности коррекции результатов расчета при индивидуальном изменении величины каждого из конструктивных параметров. Ставится задача получения формул расчета опорных углов, на которые можно воздействовать указанным образом. Задача упрощается при допущениях: - корректирующие порции изменения конструктивных параметров имеют первый порядок малости, так что их произведениями можно пренебречь;

92 - имеет смысл учесть в формулах корректирующие добавки лишь к тем конструктивным параметрам, которые имеют наибольшее влияние на результат. К таким параметрам относятся углы ориентации измерительных осей ДМ σi , µi , i ∈1, N . В этом случае для системы с тремя ДМ получается шесть корректирующих факторов, для системы с четырьмя ДМ – восемь. В качестве примера рассматривается получение формул расчета опорных углов с возможностью их коррекции при калибровке дальномерной СУО для системы с тремя ДМ при наличии горизонтальной ОПВ. 3.5.1 Получение коррекционных формул определения опорных углов для системы с тремя ДМ при наличии горизонтальной ОПВ В соответствии с алгоритмом определения опорных углов блока ДПИ, приведенном на рисунке 2.3, исходными уравнениями для получения искомых формул являются уравнения (2.22), которые для случая N=3 имеют вид: ) ) sin γ ( f ( L ) − f ( L ) ) tgϑ,  ( f ( L ) − f ( L )) cos γ + ( f ( L ) − f ( L = ) ) sin γ ( f ( L ) − f ( L ) ) tgϑ,  ( f ( L ) − f ( L )) cos γ + ( f ( L ) − f ( L =  (3.63)       ) ) sin γ ( f ( L ) − f ( L ) ) tgϑ ( f ( L ) − f ( L )) cos γ + ( f ( L ) − f ( L = 21 1 22 2 31 1 32 2 12 2 11 1 21 1 23 3 31 1 33 3 13 3 11 1 22 2 23 3 32 −γ m < γ < γ m , 2 33 3 13 3 12 2 π π − ϑm < ϑ < ϑm , γ m < , ϑm < . 2 2 Выражения для конструктивных параметров, входящих в формулы для функций fij ( L j ) , i, j ∈1,3 , с учетом допущений записываются в виде:    σi = σ* + ∆σi , µi = µ*i + ∆µi ,  2 4  ν1* = µ1* = 0, ν*2 = µ*2 = π, ν*3 = µ*3 = π. 3 3  rdi =rd* , ydi =yd* , ν i =ν*i , (3.64) Выражения для функций fij ( L j ) , i, j ∈1,3 записываются в виде суммы номинальной и корректирующей составляющих:  f= fij* ( L j ) + ∆fij ( L j , ∆σ j , ∆µ j ) , i, j ∈1, 3 . ij ( L j ) (3.65)

93 Составляющие функций fij ( L j ) , i, j ∈1,3 записываются в виде:    * *   L2 cos ( σ ) + rd ,    L3 cos ( σ* ) + rd* ,   f11* (= L1 ) L1 cos ( σ* ) + rd* , f12* ( L2 ) =− 1 2 1 f13* ( L3 ) =− 2 ( ( ) (3.66) )     * * sin ( σ ) ∆σ2 − 3 cos ( σ ) ∆µ 2 ,    sin ( σ* ) ∆σ3 + 3 cos ( σ* ) ∆µ3 ,   (3.67) cos ( σ* ) L1∆σ1 ,  ∆f 21 ( L1 , ∆σ= 1)   *   ∆f 22 ( L2 , ∆σ= cos ( σ ) L2 ∆σ2 ,  2)  ∆f 23 ( L3 , ∆σ= )3 cos ( σ* ) L3∆σ3 ,  (3.68) ∆f11 ( L1 , ∆σ1 ) = − L1 sin ( σ* ) ∆σ1 , 1 L2 ∆f12 ( L2 , ∆σ = 2 , ∆µ 2 ) 2 1 L3 ∆f13 ( L3 , ∆σ = 3 , ∆µ 3 ) 2  ) sin ( σ* ) L + y* , f 21* ( L= 1 1 d f 22* ( L= sin ( σ* ) L2 + yd* , 2) sin ( σ* ) L3 + yd* , f 23* ( L= 3) ( ( ) )    3   *  * * = f32 ( L2 ) L2 cos ( σ ) + rd ,  2   3  f33* ( L3 ) =− L3 cos ( σ* ) + rd* ,  2  f31* ( L1 ) = 0, ( ) ( (3.69) )     * * cos ( σ ) ∆µ 2 + 3 sin ( σ ) ∆σ2 ,    cos ( σ* ) ∆µ3 − 3 sin ( σ* ) ∆σ3 .   ∆f31 ( L1 ,= ∆µ1 ) L1 cos ( σ* ) ∆µ1 , 1 ∆f32 ( L2 , ∆σ2 , ∆µ 2 ) = − L2 2 1 ∆f33 ( L3 , ∆σ3 , ∆µ3 ) = − L3 2 ( ( ) ) (3.70) Вводятся обозначения коэффициентов уравнений (3.63): A1= f 21 ( L1 ) − f 22 ( L2= ) A1*c + ∆u1c , c A1= f31 ( L1 ) − f32 ( L2= ) A1*s + ∆u1s ,  s  t *  ) − f ( L = + ∆ A1= f L A u , ( ) t 12 2 11 1 1t 1   (3.71)

94 A= f 21 ( L1 ) − f 23 ( L3= ) A2*c + ∆u2c , 2c A= f31 ( L1 ) − f33 ( L3= ) A2*s + ∆u2s ,  2s  t *  ) − f ( L= + ∆ A= f L A u , ) ( 2t 13 3 11 1 2t 2   ( ) ( ) ( ) (3.72) ( ) ( ) ( ) A= f 22 L2 − f 23 L3= A3*c + ∆u3c ,  3c  s  *   A= f32 L2 − f33 L3= A3 s + ∆u3 ,  3s   − f L = A* + ∆u t .  A= f L 3t 13 3 12 2 3t 3  (3.73) Компоненты выражений (3.71) – (3.73):    3   * * * A1s =− L2 cos ( σ ) + rd ,  2  1  A1*t =− ( L2 + 2 L1 ) cos ( σ* ) + 3rd*  ,  2  A1*c =− ( L1 L2 ) sin ( σ* ) , ( ) (3.74)     s * * * = ∆u1 1 1 2 2 + 3 sin ( σ ) ∆σ 2 ,   1  = ∆u1t L2 sin ( σ* ) ∆σ2 − 3 cos ( σ* ) ∆µ 2 + L1 sin ( σ* ) ∆σ1 ,   2 c ∆u= 1 ( L ∆σ − L ∆σ ) cos ( σ ) , 1 L cos ( σ ) ∆µ + L ( cos ( σ ) ∆µ 2 * 1 1 2 2 ( ) (3.75) )    3   * * * = A2 s L3 cos ( σ ) + rd ,  2  1   * * * A2t =− ( L3 + 2 L1 ) cos ( σ ) + 3rd  ,   2 A2*c =sin ( σ* )( L1 − L3 ) , ( ) (3.76)    1  s * * *  = ∆u2 L1 cos ( σ ) ∆µ1 + L3 cos ( σ ) ∆µ3 − 3 sin ( σ ) ∆σ3 ,  2  1  = ∆u2t L3 sin ( σ* ) ∆σ3 + 3 cos ( σ* ) ∆µ3 + L1 sin ( σ* ) ∆σ1.   2 ∆= u2c cos ( σ* )( L1∆σ1 − L3∆σ3 ) , ( ( ) ) (3.77)

95 ( )    3  * *   = L2 + L3 cos ( σ ) + 2rd ,  2  1   = L2 − L3 cos ( σ* ) ,  2 A3*c = L2 − L3 sin ( σ* ) , ( ) ( ) ( ) ( ) A3*s = f32 L2 − f33 L3 A3*t =f13 L3 − f12 L2 (( ) ) ( (3.78) ) ∆u3c= cos ( σ* ) L2 ∆σ 2 − cos ( σ* ) L3∆σ3 , (( (( ) ( ) ) )) 1  L3∆μ 3 − L2 ∆μ 2 cos ( σ* ) − 3 L3∆σ3 + L2 ∆σ2 sin ( σ* ) 2 1  L3∆σ3 − L2 ∆σ2 sin ( σ* ) + 3 cos ( σ* ) L3∆μ 3 + L2 ∆μ 2 = ∆u3t 2 = ∆u3s ( )     ,   .  (3.79) Уравнения (3.63) с учетом обозначений (3.71) – (3.73) запишутся в виде: A1c cos γ + A1s sin= γ A1t tgϑ,   A2 c cos γ + A2 s sin = γ A2t tgϑ,  A3c cos γ + A3 s sin = γ A3t tgϑ,  −γ m < γ < γ m , − ϑm < ϑ < ϑm , (3.80) π π γ m < , ϑm < . 2 2 При матричной форме записи уравнения (3.80) запишутся в виде: AF Qtg ( ϑ) . = (3.81) Матрицы в уравнении (3.81) определяются выражениями:  A1c A1s   A1t  cos ( γ )    A = Q  A2t  . = A2 c A2 s ; F  = ;       sin ( γ )   A3c A3 s   A3t  (3.82) Относительно матрицы F уравнение (3.82) решается методом наименьших квадратов [61]: = F (A A) т −1 A тQtg ( ϑ ) . (3.83) Вводится обозначение: V = ( A т A ) A тQ . −1 (3.84) Выражения для искомых переменных cos ( γ ) , sin ( γ ) записываются в виде:

96 cos = ( γ ) V (1) tg ( ϑ) ,  sin = ( γ ) V ( 2 ) tg ( ϑ). (3.85) Из системы (3.83) находится решение для опорного угла крена: V ( 2) tg ( γ ) = , V (1)      V ( 2)   arctg  γ оп =  . 1 V ( )   (3.86) Решение для тангенса угла тангажа: 1 1 cos ( γ ) = V (1) V (1) tg ( ϑ) = 1 = 2  V ( 2)  1+    V (1)  sign ( V (1) ) ( V (1) ) + ( V ( 2 ) ) 2 2 . (3.87) В соответствии с формулой (3.87) определяется опорный угол тангажа:   = ϑ arctg  оп    3.5.2 Алгоритм sign ( V (1) ) ( V (1) ) + ( V ( 2 ) ) 2 компенсации 2  .    инструментальных (3.88) погрешностей конструктивных параметров установки ДМ в лабораторных условиях Для выполнения эксперимента требуется стенд с поворотным устройством, на котором устанавливается блок ДПИ с закрепленными на нем ДМ, и горизонтальная ОПВ. Эксперимент выполняется для нескольких стационарных угловых положений блока ДПИ с точным определением значений его углов крена и тангажа по измерительным устройствам стенда. В каждой позиции блока ДПИ выполняются операции: 1) При нулевых значениях вариаций ∆σi , ∆µi , i ∈1, N определяются опорные углы крена и тангажа блока ДПИ и сравниваются с показаниями измерительных устройств стенда. Если погрешности вычисления опорных углов находятся в допустимых пределах, то необходимость в дальнейшем испытании дальномерной СУО отпадает. В противном случае такие испытания проводятся по следующей схеме.

97 2) В пространстве факторов ∆σi , ∆µi , i ∈1, N определяются координаты (N+1) вершин исходного выпуклого многоугольника, именуемого в теории математического планирования эксперимента «симплекс» [65]. 3) Для текущего углового положения блока ДПИ выполняется расчет значений опорных углов крена и тангажа при реализации в алгоритме расчета координат всех вершин симплекса в соответствии с формулами, выведенными в разделе 3.5.1. Полученные значения сравниваются с показаниями измерительных средств стенда и определяются погрешности определения опорных углов при реализации координат каждой из вершин симплекса. 4) В соответствии с симплексным методом поиска оптимума выполняется расчет координат новой вершины симплекса, отраженной по отношению к вершине, в которой погрешность вычисления опорных углов максимальна, и выполняется эксперимент по вычислению опорных углов при реализации в алгоритме расчета координат новой вершины симплекса. Процесс поиска корректирующих значений конструктивных параметров ∆σi , ∆µi , i ∈1, N продолжается до достижения заданной точности вычисления опорных углов блока ДПИ. 3.5.3 Рекомендации по определению параметров амортизирующего подвеса блока ДПИ на ПО методами факторного эксперимента При установке персональной ИИС на робота, управляемого оператором по каналу телесвязи, большое влияние на точность работы навигационной системы имеет выбор параметров схемы защиты блока ДПИ от вибраций объекта. Задача оптимизации параметров амортизирующего подвеса блока аппаратуры на подвижном основании методами факторного эксперимента рассмотрена в статье [66]. Блок ДПИ подвешивается на подвижном основании с помощью резиновых шнуров, которые на модели подвеса располагаются в одной плоскости, полагая, что основное возмущение со стороны подвижного объекта на блок аппаратуры передается вдоль вертикальной оси. Для факторного исследования схемы амортизирующего подвеса выбираются параметры опорной схемы подвеса: масса

98 объекта, длина, тип и количество шнуров, жесткость шнуров, углы их установки, статическое удлинение и др.). Задача обеспечения значения собственной частоты продольных колебаний объекта, удовлетворяющей условию f y ≤ f1 / 2 , где f y - собственная частота продольных колебаний объекта, f1 - частота возмущения, решена в работе [66]. Реализация достигнутых результатов позволит существенно уменьшить амплитуду вибраций блока ДПИ на основной частоте возмущения, что должно снизить погрешность построения телеуправления им. траектории ПО и повысить точность

99 ВЫВОДЫ 1. Получены уравнения чувствительности погрешностей вычисления опорных углов крена и тангажа дальномерной СУО к погрешностям установки системы ДМ на блоке ДПИ для схемы с 3-мя ДМ и с 4-мя ДМ. 2. Оценка погрешностей определения углов наклона блока датчиков первичной информации относительно опорной поверхности показала, что наибольшее влияние на величину погрешности вычисления опорных углов крена и тангажа блока ДПИ оказывают инструментальные погрешности углов σi , µi , i ∈1, N выставки направлений измерительных осей ДМ относительно связанной системы координат. 3. N На основе проведенного полного факторного эксперимента типа 2 , где N – число ДМ, в пространстве конструктивных параметров установки ДМ на блоке о ДПИ при их изменении в пределах ±1 мм и ±1 относительно номинала определены предельные области погрешностей вычисления опорных углов крена и тангажа, которые составляют ± (1,6…1,8)о для системы с четырьмя ДМ и ± (1,5…2,4)о для системы с тремя ДМ. 4. Реализация ПФЭ на Simulink-модели дальномерной СУО позволяет при детерминированной постановке ПФЭ получить оценки коэффициентов чувствительности моделей погрешностей вычисления опорных углов крена и тангажа, неточность определения которых по сравнению с расчетом по аналитическим зависимостям не превышает сотых долей процента. 5. С целью повышения точности определения координат ПО разработаны рекомендации по калибровке устройства вычисления опорных углов для конкретной конструкции дальномерной СУО, которая заключается в отладке параметров алгоритма в лабораторных условиях методами математического планирования эксперимента с применением соответствующего контрольно– измерительного оборудования.

100 Глава 4. РАЗРАБОТКА МЕТОДОВ РАБОТЫ ПОВЫШЕНИЯ АВТОНОМНОЙ ТОЧНОСТИ ПЕРСОНАЛЬНОЙ ИНФОРМАЦИОННО-ИЗМЕРИТЕЛЬНОЙ СИСТЕМЫ НАЗЕМНОГО ПОЗИЦИОНИРОВАНИЯ 4.1 Постановка задачи Для системного объединения разработанных алгоритмов определения углов наклона блока ДПИ в данной главе решается задача их схемной реализации и построения комплексной системы на основе комплексирования инерциальной и дальномерной СУО с возможностью дополнительного повышения точности на базе эвристических методов, в качестве которых за основу взяты методы коррекции по нулевой скорости (ZUPT) [22, 31, 41-43] и методы эвристического снижения дрейфа (HDR и iHDE) [39, 40]. Основные подзадачи, которые требуется при этом решить: - разработка схемы комплексной СУО; - разработка алгоритма обнуления выходов интеграторов; - разработка алгоритма коррекции путевой скорости ПО на приоритетных направлениях. 4.2 Разработка схемы комплексной системы угловой ориентации Схема персональной ИИС определения местоположения наземного ПО (рисунок 4.1) включает в себя комплексную СУО и вычислительное устройство (ВУ2), формирующее на выходе координаты местоположения ПО. Комплексная СУО (рисунок 4.2) содержит инерциальную СУО, дальномерную СУО и блок компенсации, состоящий из двух структурно одинаковых схем компенсации, каждая из которых содержит два сумматора и низкочастотный фильтр [24]. Принцип работы дальномерной СУО изложен в главе 2 (рисунок 2.1). На устройство определения углов наклона блока ДПИ, суть которого заключается в применении схемы комплексирования инерциальной и дальномерной СУО, получен патент РФ на изобретение №2649026 [67].

101 Инерциальная СУО Блок инерциальных ДПИ Дальномерная СУО ВУ1 ВУ2 Блок компенсации Комплексная СУО Рисунок 4.1 – Блок-схема автономной персональной ИИС наземного позиционирования Блок ДМ Блок инерциальных ДПИ ВУ1 ВОУ U0 ВУУ К1 Блок компенсации Рисунок 4.2 – Блок-схема комплексной СУО ВУ3

102 Низкочастотные фильтры блока компенсации построены как апериодические звенья с передаточными функциями W1 ( s ) = 1 1 , W2 ( s ) = , постоянные T1s + 1 T2 s + 1 времени Т1, Т2 которых определены из условия минимума установившейся дисперсии погрешности оценки полезного сигнала [24].  и γ комплексной СУО вычисляются по формулам Выходные сигналы ϑ = ϑ γ= (1 − W1 (s) ) ϑ ин + W1 (s)ϑ дм , (1 − W2 (s) ) γ ин + W2 (s)γ дм . (4.1)  , γ инерциальной СУО содержат погрешности Выходные сигналы ϑ ин ин ин εин ϑ , εγ :  = ϑ + εин , ϑ ин ϑ γ ин = γ + εин γ . (4.2)  , γ дальномерной СУО содержат погрешности Выходные сигналы ϑ дм дм ε ϑдм , ε дм γ :  = ϑ ϑ+ε ϑдм , γ дм = γ +ε дм дм γ . (4.3) Скорректированные сигналы на выходе комплексной СУО имеют вид:  = ϑ + W ( s ) ε дм + (1 − W ( s ) ) εин = ϑ + εΣ , ϑ ϑ ϑ ϑ 1 1 Σ ин γ = γ + W2 ( s ) ε дм γ + (1 − W2 ( s ) ) ε γ = γ + ε γ . (4.4) Результирующие погрешности вычисления тангажа εΣϑ и крена εΣγ равны: = εΣϑ W1 ( s ) ε ϑдм + (1 − W1 (= s ) ) εин ϑ 1 W1s ин εϑ , ε ϑдм + W1s + 1 W1s + 1 1 W2 s ин = εΣγ W2 ( s ) ε дм εγ . ε дм s ) ) εин γ + (1 − W2 (= γ γ + W2 s + 1 W2 s + 1 (4.5) Таким образом, благодаря комплексированию выходные сигналы (4.1) содержат значения измеряемых углов ϑ и γ, неискаженные фильтрами, и погрешности (4.5), существенно ослабленные по сравнению с исходными ин дм дм погрешностями εин ϑ , ε γ , ε ϑ , ε γ . Как видно из выражения (4.5), существенное ослабление помех возможно при условии, когда частотный спектр шумовых помех

103 дальномеров находится правее граничных частот 1/Тi, i=1, 2, а частотный спектр шумовых помех инерциальных датчиков – левее этих частот [68]. 4.3 Разработка алгоритма обнуления выходов интеграторов Учитывая, что наземный объект (человек) имеет возможность эпизодически останавливаться, для уменьшения погрешностей интегрирования предлагается алгоритм обнуления выходов интеграторов первого каскада, вычисляющих проекции скорости, при фиксации кратких (почти мгновенных) остановок ПО по показаниям акселерометров. Блок-схема, реализующая этот алгоритм, приведена на рисунке 4.3. g Блок ДПИ ВУ1 ≤ ВУ2 Uc Детектор остановок Рисунок 4.3 – Блок-схема, реализующая алгоритм обнуления выходов интеграторов Обозначения на рисунке 4.3:     a =i ⋅ ax + j ⋅ a y + k ⋅ az - вектор кажущегося ускорения блока ДПИ; ax , ay , az - сигналы на выходе акселерометров;  , γ – сигналы комплексной СУО, пропорциональные углам рысканья, , ϑ ψ тангажа и крена блока ДПИ;

104  xg , w  yg , w  zg - проекции абсолютного ускорения на оси нормальной системы w координат; { i i= w ( tk ) = M w k k −m } - математическое ожидание модуля абсолютного ускорения по выборке из (m+1) дискрет; g=9,81 м/с2 – ускорение свободного падения; min – пороговое значение ускорения; w Uc – сигнал сброса интеграторов;  xg ∆t , w  yg ∆t , w  zg ∆t – произведения проекций абсолютного ускорения на w период дискретности ∆t (основные сигналы на входах интеграторов); Vxg ( tk ) , Vyg ( tk ) , Vzg ( tk ) – сигналы проекции скорости движения блока ДПИ на оси нормальной системы координат; Vxg ( 0 ) , Vyg ( 0 ) , Vzg ( 0 ) – сигналы, задающие начальные значения проекций скорости блока ДПИ. С блока ДПИ на ВУ1 поступают сигналы проекций кажущегося ускорения, которые преобразуются из связанной системы координат в проекции axg , ayg , azg на оси нормальной системы координат. Затем эти сигналы преобразуются в проекции вектора абсолютного ускорения [69], которые поступают на вход вычислителя ВУ2, определяющего среднее значение модуля абсолютного ускорения w ( tk ) по выборке из m+1 дискрет. Детектор остановок сравнивает модуль w ( tk ) с пороговым min . Если среднее значение модуля абсолютного ускорения меньше или значением w min , то детектируется неподвижное состояние и сигнал Uc на выходе равно w детектора остановок становится равным единице. Поступая на входы сброса интеграторов, сигнал Uc обнуляет их выходы.

105 4.4 Разработка алгоритма коррекции путевой скорости подвижного объекта на приоритетных направлениях Если известна карта коридорной сети здания, в котором должны выполняться оперативные работы с применением персональной ИИС, то эта информация может быть использована для коррекции движения ПО по приоритетным направлениям, согласованным с картой коридорной сети. Блок-схема, реализующая подобный алгоритм, приведена на рисунке 4.4. Обозначения на рисунке 4.4:  ω(k ) - вектор угловой скорости блока ДПИ;  (k) и γ (k) - выходные сигналы комплексной СУО; ϑ  (k ) - текущее значение угла рысканья; ψ  ( k ) - текущее значение угловой скорости рыскания; ψ U1* - сигнал коррекции угла рысканья; U1 - сигнал коррекции угла рысканья, зависящий от характера движения ПО; ψ (0) - начальное значение угла рысканья; U br - сигнал прерывания коррекции путевой скорости при смене направления движения ПО; U Z* g - сигнал индикации движения ПО вдоль направления, параллельного оси Zg; U X* g - сигнал индикации движения ПО вдоль направления, параллельного оси Хg; UVXg - сигнал коррекции проекции путевой скорости на ось Хg; UVZg - сигнал коррекции проекции путевой скорости на ось Zg; VZ g (k ) , VX g (k ) - текущие значения проекций путевой скорости на ось Zg и Xg соответственно.

106 ВУ угловой скорости рысканья Углы приоритетных ВУ сигнала направлений коррекции угла рысканья ВУ сигнала прерывания коррекции путевой скорости Индикация движения вдоль оси Zg dt прерывание коррекции при смене направления + x0 Цифровой интегратор ВУ сигнала коррекции ходовой скорости Vzg ВУ сигнала коррекции боковой скорости Vхg ВУ сигнала коррекции боковой скорости Vzg Индикация движения вдоль оси Xg + ВУ сигнала коррекции ходовой скорости Vхg + Цифровой интегратор u x0 + + Цифровой интегратор u x0 Рисунок 4.4 – Блок-схема, реализующая алгоритм коррекции путевой скорости ПО на приоритетных направлениях

Коррекция выполняется в 2 этапа: коррекция угла рысканья и коррекция путевой скорости. 4.4.1 Коррекция угла рысканья  (k ), k = Текущее значение угла рысканья ψ 0,1,2,..., n (сигнал с выхода цифрового интегратора) сравнивается с заданными углами приоритетных направлений в блоке «ВУ сигнала коррекции угла рысканья». Выбирается одно  (k ) . значение приоритетного угла, ближайшее к текущему ψ Сигнал коррекции угла рысканья U1∗ формируется по формуле: ∗ U= kψ ( ψ пр − ψ ) , 1 где (4.6)  пр – угол приоритетного направления относительно первоначального ψ  пр =0; направления движения, соответствующего значению ψ  – текущее значение вычисленного угла рысканья; ψ kψ – передаточный коэффициент. Сигнал коррекции U1* подается на вход цифрового интегратора через блок «прерывание коррекции при смене направления». Смена направления при повороте на 90о фиксируется при превышении модулем угловой скорости рысканья ∆ψ ( k ) / ∆t порогового значения ψ m =0,3 (1/с) ≈ 17 о/с. Сигнал прерывает подачу на вход цифрового интегратора угла рысканья сигнала коррекции U1∗ .  * U1 ,  U1 =  0,   Сигнал коррекции U1 если ψ ( k ) − ψ ( k − 1) < ψ m , ∆t ψ ( k ) − ψ ( k − 1) если ≥ ψ m . ∆t (4.7) поступает на вход цифрового интегратора, вычисляющего угол рысканья, приближая текущее значение угла рысканья к приоритетному.

108 4.4.2 Коррекция путевой скорости Схема координированной коррекции вектора путевой скорости приведена на рисунке 4.5. Если ПО идет вдоль оси Xg (Zg), то проекцию путевой скорости на ось Xg (Zg) условно назовем ходовой, а проекцию на перпендикулярное направление – боковой скоростью. Zg D ϕ/2 A B Приоритетное направление ϕ Xg C Рисунок 4.5 – Схема координированной коррекции вектора путевой скорости при движении по приоритетному направлению На этапе коррекции путевой скорости формируются сигналы коррекции UVXg и UVZg , поступающие на входы цифровых интеграторов, вычисляющих соответствующие проекции скорости. Основные сигналы на входах цифровых интеграторов – произведения сигналов, пропорциональных проекциям абсолютного ускорения, на шаг интегрирования ∆t. Рассмотрим процесс формирования сигналов коррекции на примере сигнала UVZg . Сигнал UVZg состоит из двух составляющих: сигнала коррекции ходовой скорости VZ g и сигнала коррекции боковой скорости VZ g .

109 Сигнал U Vход формируется, когда ПО идет вдоль оси Zg, а сигнал U Vбок - когда Zg Zg ПО идет вдоль оси Xg. При уменьшении модуля вычисленного значения «боковой» скорости Vxg до нуля «ходовая» скорость Vzg должна возрасти на величину ∆Vzg , чтобы вектор путевой скорости развернулся по приоритетному направлению. Величина приращения определяется из треугольника ABD (рисунок 4.5): 1 Vxg  ∆Vzg = . 2 Vzg 2 (4.8) Активацию формирования той или иной составляющей выполняют блоки «Индикация движения вдоль оси Zg» и «Индикация движения вдоль оси Xg». Если ПО идет вдоль оси Zg, то сигнал U Z* g =1, иначе U Z* g =0. Если ПО идет вдоль оси Xg, то U X* g =1, иначе U X* g =0. Определение приоритетного движения ПО выполняется  ( k ) , скорректированному по приоритетному направлению. по сигналу ψ Сигнал U Vбок стремится уменьшить боковую скорость VZ g до нуля. При этом Zg сигнал U Vход несколько увеличивает проекцию VX g Xg таким образом, чтобы совместная коррекция соответствовала довороту вектора путевой скорости до приоритетного направления.

110 ВЫВОДЫ 1. Выполнена схемная реализация разработанных алгоритмов определения углов наклона блока ДПИ и построена комплексная система на основе комплексирования инерциальной и дальномерной СУО с возможностью дополнительного повышения точности на базе эвристических методов. 2. Разработано устройство определения углов наклона блока ДПИ, суть которого заключается в применении схемы комплексирования инерциальной и дальномерной систем угловой ориентации. На данное устройство получен патент РФ на изобретение №2649026 [67]. 3. Разработан алгоритм обнуления выходов интеграторов первого каскада, вычисляющих проекции скорости, при фиксации кратких остановок ПО по показаниям акселерометров, который позволяет сбрасывать погрешность определения скорости. 4. Разработан алгоритм коррекции вектора путевой скорости на приоритетных направлениях движения ПО внутри здания, который использует информацию о плане здания, в котором идет движение.

111 Глава 5. ОЦЕНКА РАЗРАБОТАННЫХ ТОЧНОСТИ ОПРЕДЕЛЕНИЯ ИМИТАЦИОННОЙ МЕТОДОВ ПОВЫШЕНИЯ МЕСТОПОЛОЖЕНИЯ МАТЕМАТИЧЕСКОЙ НА МОДЕЛИ ИНФОРМАЦИОННО-ИЗМЕРИТЕЛЬНОЙ СИСТЕМЫ 5.1 Постановка задачи С целью оценки разработанных способов и алгоритмов, представленных в предыдущих главах, требуется построить имитационную математическую модель автономной персональной ИИС, структура которой приведена на рисунке 5.1 Пульт управления режимами работы ИИС Модель движения наземного ПО с блоком ДПИ Uупр Модель персональной ИИС Блок анализа, диагностики и идентификации ИИС Рисунок 5.1 - Структурная схема имитационной математической модели Построение математической модели требует решения следующих подзадач: - построение модели движения наземного ПО с блоком ДПИ; - построение модели персональной ИИС; - представление результатов оценки точности определения координат местоположения ПО с помощью автономной персональной ИИС. Для решения поставленных подзадач выбран следующий вид модели: дискретная динамическая имитационная модель со случайными сигналами и матричным преобразованием координат [70]. 5.2 Построение модели движения наземного подвижного объекта с блоком датчиков первичной информации Модель движения наземного ПО строится как кинематическая модель движения элемента робота андроидного типа, имитирующего походку человека.

112 Создание кинематической модели движения робота андроидного типа производится при допущении, что данный ПО состоит из нескольких жестких элементов, соединенных между собой с помощью шарниров. С каждым из элементов связана местная прямоугольная система координат, полюс которой в процессе движения ПО движется по некоторой траектории относительно неподвижной системы отсчета, а элемент совершает периодические угловые колебания относительно полюса. При построении модели движения ПО с блоком ДПИ применяется схема преобразования координат проективного пространства, приведенная в приложении Б. Методика преобразования координат с применением формул (Б.1) – (Б.18), приведенных в приложении Б, используется для получения математических выражений при поэтапном решении поставленных задач в данном разделе главы, вначале для получения кинематических уравнений движения ПО, а затем – для кинематических уравнений движения блока ДПИ. 5.2.1 Кинематические уравнения движения ПО На рисунке 5.1 приведена кинематическая схема модели ПО с указанием возможных мест крепления блока ДПИ. Y0 Y1 Вертикаль 5 1 Горизонтальная плоскость О1 X0 ϑ1 X1 6 2 3 7 4 Рисунок 5.2 – Схема возможных мест крепления блока ДПИ на модели ПО

113 Обозначения на рисунке 5.1: 1 – верхняя часть тела; 2 – бедро; 3 – голень; 4 – стопа; 5 – блок ДПИ на шлеме; 6 – блок ДПИ на поясе; 7 – блок ДПИ на обуви; O1 X 1Y1Z1 – система координат, связанная с элементом 1 ПО; O1 X 0Y0 Z 0 –система координат, связанная с заданным положением вектора  путевой скорости Vп* , направленным по касательной к заданной линии пути;   VO′1 – проекция вектора скорости VO1 полюса О1 на плоскость X 1O1Y1 ; ϑ1 – угол тангажа элемента 1. Рассматривается задача построения кинематической модели движения элемента 1 ПО с блоком ДПИ, закрепленным на поясе или шлеме ПО. В качестве неподвижной навигационной системы координат, в которой будет определяться траекторию блока ДПИ, выбирается прямоугольная правая система осей O0XgYgZg, полюс которой O0 помещается в исходную точку движения ПО. Ось Yg направляется по вертикали места вверх, оси Xg, Zg образуют горизонтальную плоскость, по которой шагает ПО. С элементом 1 ПО связывается прямоугольная правая система осей O1 X 1Y1Z1 , полюс которой O1 помещается в точку элемента 1, расположенную примерно в центре масс ПО (на поясе). Задается следующая исходная ориентация осей системы координат O1 X 1Y1Z1 , связанной с элементом 1, определяющую угловое положение элемента 1 до начала движения ПО: ось Y1 направлена по вертикали места вверх, ось X1 указывает направление движения ПО, ось Z1 дополняет систему связанных осей по правилу правой системы координат.

114 Пространственное движение элемента 1 ПО разделяется на продольное и боковое движения. Угловой координатой продольного движения элемента 1 является угол тангажа ϑ1, поступательное продольное движение элемента 1  характеризуется изменением вектора скорости VO′1 по величине и направлению в плоскости X 0O1Y0 . В боковом угловом движении элемента 1 ПО координатами являются угол крена γ и угол рысканья ψ относительно заданного направления движения. Поступательное боковое движение элемента 1 ПО характеризуется  изменением путевого угла Ψ, измеряемого между проекцией VпO1 вектора скорости  VO1 на горизонтальную плоскость и осью Xg земной системы координат. Векторные кинематические траекторные уравнения движения точки О1 в проекции на горизонтальную плоскость имеют вид:  O 1 dRO O'  O dV  п 0 1 1 = V= aпO1 , , (5.1) п dt dt  где RO O' - радиус-вектор проекции О1′ ( xg , z g ) точки О1 на горизонтальную 0 1 плоскость;    = VпO1 igVXOg1 + k gVZOg1 - вектор путевой скорости точки О1;    = aпO1 ig a OX1g + k g aZOg1 - проекция вектора абсолютного ускорения точки О1 на горизонтальную плоскость (путевое ускорение). Уравнения (5.1) в проекциях на оси неподвижной системы координат: dx1g dz1g  = = −VпO1 sin ( Ψ ) ,  VXOg1 = VпO1 cos ( Ψ ) , VZOg1 = dt dt  O1 O1 dV  dVп Xg O1 O1 d Ψ a= = Ψ + V cos , ( )  Xg Zg dt dt dt  O1 O  dVZ g d Ψ dVп 1 aZOg1 = = −VXOg1 − sin ( Ψ ) .  dt dt dt  (5.2) В статье [71] выполнен анализ данных эксперимента, проведенного с участием человека в качестве ПО. Человек двигался по замкнутому прямоугольному маршруту с блоком ДПИ ADIS16405, закрепленным на поясе

115 пользователя. В процессе эксперимента выполнена запись показаний инерциальных датчиков, установленных в блоке ДПИ, в виде массивов данных с шагом дискретности по времени ∆t ≈ 0,025 с. Массивы показаний датчиков обработаны с применением библиотеки программ сглаживания случайных сигналов Curve Fitting Tool (MatLab) методом скользящего среднего (Moving Average) с размером окна (Span) 15 дискрет [72]. К сглаженным сигналам применено преобразование Фурье с построением частотного спектра распределения амплитуд гармоник ряда Фурье [73]. В результате анализа полученных данных с блока ДПИ и видеозаписи движения ПО сделаны выводы: - в установившемся режиме движения полюс O1 совершает вертикальные колебания от некоторой высоты Hmin до высоты Hmax. Высота Hmin соответствует моменту начала или окончания шага, а высота Hmax определяет положение полюса O1 в верхней точке траектории, когда линия опорной ноги вертикальна; - основная частота колебаний параметров продольного движения ϑ , ax , a y в проведенном эксперименте f1≈1,546 Гц (ω1=9,714 с-1), период этих колебаний равен 0,647 с и соответствует времени выполнения шага одной ногой при установившейся ходьбе;  az в  ψ, - основная частота колебаний параметров бокового движения γ, проведенном эксперименте f2≈f1/2≈0,773 Гц (ω2=ω1/2=4,857 с-1), период этих колебаний равен 1,294 с и соответствует времени выполнения двух шагов; - средняя скорость движения человека, выполняющего 5 шагов за 3,22 с на длине пути 3 м, равна 0,93 м/с или 3,35 км/ч. Средняя длина шага L1, измеренная между следами пяток разных ног, равна 0,6 м. Изменение вертикальной координаты полюса O1 = h(t ) H (t ) − H min , (5.3) где H(t) – текущая геометрическая высота полюса O1, моделируется уравнениями:

116  d 2 h dh dh∗ + =,  2 dt dt dt   dy  , если y ≥ 0, ∗   dh  dt =  dt  dy  − , если y < 0,   dt  dy = − ymω2 sin ( ω2t ) ,  y (t ) = ym cos ( ω2t ) ,  dt  H= (t ) h(t ) + H min ,  Th (5.4) ym H max − H min - диапазон вертикальных колебаний полюса O1. Начальные где =  условия для уравнений (5.4): = h(0) y= 0. m , h(0) Величина ym зависит от длины шага L1, длины ноги l и может быть приближенно оценена по формуле [8]: ym = l − l 2 − 0,25 L12 . (5.5) Для l=1 м, L1=0,6 м получим ym≈0,046 м. Пусть заданная линия пути движения ПО имеет вид прямоугольника, стороны которого сопряжены дугами радиуса R, а начальная и конечная точки маршрута совпадают с полюсом неподвижной системы координат О0 (рисунок 5.3). t3 Ψ* X0 Z0 E F t4 С3 С2 С1 О0 t0=0 Zg Xg D A t1 t2 C B Xg R Рисунок 5.3 - Заданная линия пути полюса О1 элемента 1 ПО

117 Принимается допущение, что в момент времени t=0 начальные значения углов ψ1, ϑ1, γ1 равны нулю. ПО начинает движение из точки O0 к точке А в течение времени t1 при нулевом начальном значении путевой скорости. Путевая скорость представляется в виде суммы двух векторов:    = VпO1 i0 Vп∗ + ∆Vп1 + k0 ∆Vп2 , ( где ) (5.6) Vп∗ – основная составляющая путевой скорости, определяющая процесс движения ПО по заданному маршруту; ∆Vп1 – составляющая путевой скорости, характеризующая колебания величины скорости относительно значения Vп∗ ; ∆Vп2 – составляющая путевой скорости, характеризующая боковые колебания полюса О1 относительно заданной плоскости движения. Установившееся значение скорости Vп∗ принимается равным Vпуст = 0,9м/с . Процесс разгона ПО от скорости Vп∗ = 0 до скорости Vп∗ = Vпуст моделируется передаточной функцией инерционного звена: Vп∗ = где p= 1 Vпуст , Tп p + 1 (5.7) d ; dt Тп – постоянная времени, которую примем равной 0,3 с. При продольных колебаниях профиль скорости на каждом шаге включает колебания около 0,3 м/с относительно средней скорости ходьбы Vп∗ [8]: = ∆Vп1 0,3sin ( 2ω2t + ϕ x 0 ) , (5.8) где ϕ x 0 = 0,2π – фазовый угол, согласующий продольные колебания профиля скорости полюса О1 с его вертикальными колебаниями (5.4). При боковых колебаниях полюса О1 его смещение ∆z0 относительно вертикальной плоскости будет наибольшим в момент окончания шага в сторону

118 ∆z0 имеет экстремум. шагающей ноги. При h=ym смещение ∆z0=0, а скорость ∆Vп 2 = Уравнение для скорости ∆Vп2 при установившемся движении записывается в виде: ∆Vп2 = −∆zm sin ( ω2t + ϕ z 0 ) , (5.9) где ϕ z 0 = 0,4π – фазовый угол, согласующий боковые колебания полюса О1 с его вертикальными колебаниями (5.4). По результатам, приведенным в статье [71], амплитуда боковых колебаний 0,15м/с . полюса О1 по скорости ∆zm = Двигаясь от точки А до точки B, ПО выполняет поворот влево на 90 градусов. При этом вектор путевой скорости поворачивается с угловой скоростью, определяемой решением уравнения ′ d 2 Ψ ∗ d Ψ ∗ VпO1 TΨ + = Ψ *зад − Ψ ∗ ) , ( 2 dt dt R где (5.10) Ψ *зад – заданное значение угла пути; TΨ – постоянная времени, характеризующая процесс изменения угловой d Ψ∗ ; скорости dt R – радиус разворота, равный в рассматриваемом примере одному метру; ′ VпO1 = (V ∗ п + ∆Vп1 ) + ( ∆Vп2 ) ≈ Vп∗ + ∆Vп1 – путевая скорость полюса О1. 2 2 От точки В до точки С ПО движется по прямой в отрицательную сторону оси Zg, достигая точки С в момент времени t2. От точки С до точки D выполняется второй поворот влево на 90 градусов, и до точки Е ПО движется вдоль отрицательного направления оси Xg, достигая точки Е в момент времени t3. От точки Е до точки F выполняется третий поворот на 90о , после чего ПО движется в положительном направлении оси Zg к точке O0. Если программа движения предусматривает двойной обход по замкнутому маршруту, то перед точкой выполняется четвертый поворот и картина движения повторяется еще раз. Программа выполнения поворотов при двойном обходе по замкнутому маршруту:

119 Ψ * зад 0, π  , 2 π,   3 π, 2 =  2π, 5  π, 2 3π, 7  π, 2 если t ≤ 90c, если 180с ≥ t > 90c , если 271,5с ≥ t > 180c, если 361,5с ≥ t > 271,5c, если 452,5с ≥ t > 361,5c, если 542,5с ≥ t > 452,5c, если 633с ≥ t > 542,5c, если 722с ≥ t > 633c (5.11) На рисунке 5.4 показан фрагмент структурной схемы модели движения ПО, реализующий программу поворотов при двойном обходе замкнутого маршрута. Рисунок 5.4 – Структурная схема модели выполнения поворотов ПО при двойном обходе замкнутого маршрута Относительно направления движения ПО, задаваемого углом пути Ψ*, вектор путевой скорости совершает колебания, отклоняясь от заданного направления на угол ΔΨ, определяемый формулой  ∆V  ∆Ψ = −arctg  Oп2 . 1  Vп  (5.12) Таким образом, угол пути складывается из двух составляющих: Ψ = Ψ ∗ + ∆Ψ . Для производной угла пути по времени получается выражение: (5.13)

120 d Ψ d Ψ ∗ d ∆Ψ . = + dt dt dt Формула для производной d ∆Ψ dt (5.14) d ∆Ψ : dt   dVп∗ d ∆Vп1   d ∆Vп2 ∗ + ∆ − + ∆ V V V ( п п1 )  (5.15)  п2 2 2  ∗ dt dt dt   (Vп + ∆Vп1 ) + ( ∆Vп2 )   1 Производные, входящие в уравнение (5.15), определяются формулами:     d ∆Vп1 = 0,6ω2 cos ( 2ω2t + ϕ x 0 ) ,   dt  dVп∗ p = Vпуст , dt Tп p + 1 (5.16) Принимается допущение, что при установившемся движении элемент 1 ПО совершает гармонические колебания по тангажу в диапазоне от 00 до минус 2ϑm . Значение ϑ =0 соответствует максимальной высоте полюса О1, а значение ϑ = −2ϑm соответствует минимальной высоте полюса О1, т.е. в начале каждого шага и в момент его завершения. Таким образом, при завершении каждого шага корпус робота наклоняется вперед. Уравнение гармонических колебаний по тангажу, согласованное по фазе с колебаниями h(t), имеет вид: ϑ1 (t ) = ϑm cos ( 2ω2t + ϕϑ ) − 1 , где (5.17) ϕϑ =−0,2π – фазовый угол, согласующий колебания элемента 1 ПО по тангажу с его вертикальными колебаниями (5.4). ϑm = 0,69о – амплитуда колебаний элемента 1 ПО по тангажу [71]. При симметричной ходьбе колебания элемента 1 ПО по крену должны быть симметричны относительно нуля, а по рысканью – симметричны относительного заданного направления движения. Принимается, что максимальный по модулю крен будет в момент завершения каждого шага в сторону шагающей ноги. Значение γ1=0 соответствует максимальной высоте полюса О1. Такая же схема колебаний

121 ∆ψ m , γ1 = − γ m соответствуют завершению будет и по рысканью. Значения ∆ψ1 = шага с левой ноги, а ∆ψ1 = −∆ψ m , γ1 = γ m - с правой. Уравнения изменения углов γ1(t) и ∆ѱ1(t), согласованные по фазе с уравнением h(t), имеют вид: γ1 (t ) = − γ m sin ( ω2t + ϕγ ) , где ∆ψ1 (t ) = ∆ψ m sin ( ω2t + ϕψ ) , (5.18) ϕγ =ϕψ =−0,15π – фазовые углы; γ= 1,13о , ∆ψ= 0,4о m m – амплитуды колебаний элемента 1 ПО по крену и рысканью [70]. Угол рысканья элемента 1 ПО и его угловая скорость рысканья складываются из двух составляющих: ψ1 = ∆ψ1 + Ψ ∗ ,   ∗ .  ψ 1 = ∆ψ 1 + Ψ  (5.19) На рисунках 5.5 – 5.9 приведены графики, иллюстрирующие согласованность колебаний параметров движения элемента 1 ПО при установившемся движении. h,м 0.2 V п2 , м/с , радиан 0.15 0.1 0.05 0 -0.05 -0.1 -0.15 -0.2 0 1 2 3 4 5 6 t, c Рисунок 5.5 – Графики установившихся колебаний параметров движения элемента 1 ПО h ( t ) , ∆Vп2 ( t ) , ∆Ψ ( t )

122 0.3 h,м V п1 , м/с 0.2 0.1 0 -0.1 -0.2 -0.3 1 0 2 3 4 5 6 t, c Рисунок 5.6 – Графики установившихся колебаний параметров движения элемента 1 ПО h ( t ) , ∆Vп1 ( t ) h,м 0.15 1 d ,радиан / dt , 1/c 1 0.1 0.05 0 -0.05 -0.1 -0.15 0 1 2 3 4 5 6 t, c Рисунок 5.7 – Графики установившихся колебаний параметров движения элемента 1 ПО h ( t ) , ϑ1 ( t ) , ϑ 1 ( t ) 0.05 h,м 1 0.04 d ,радиан 1 / dt , 1/c 0.03 0.02 0.01 0 -0.01 -0.02 -0.03 -0.04 0 1 2 3 4 5 6 t, c Рисунок 5.8 – Графики установившихся колебаний параметров движения элемента 1 ПО h ( t ) , ∆ψ1 ( t ) , ∆ψ 1 ( t )

123 0.1 0.08 0.06 0.04 0.02 0 -0.02 -0.04 -0.06 h,м -0.08 d 1 ,радиан 1 / dt , 1/c -0.1 0 1 2 3 4 5 6 t, c Рисунок 5.9 – Графики установившихся колебаний параметров движения элемента 1 ПО h ( t ) , γ1 ( t ) , γ 1 ( t ) Для возможности кратковременных остановок ПО в процессе движения по замкнутому маршруту строится программа семи остановок ПО в центрах прямолинейных участков маршрута путем сформирования управляющего сигнала U_stop*(t) при двойном обходе замкнутого маршрута, которая имеет вид: если 45c > t ≥ 0, 0,  +1( t − 45 ) 1, если 135c > t ≥ 45, U _ stop *= U _ stop * += 1( t − 135 ) 2, если 225c > t ≥ 135,  U _ stop * +1= ( t − 225) 3, если 315c > t ≥ 225, U _ stop* =  1( t − 315 ) 4, если 405c > t ≥ 315, U _ stop * += U _ stop * +1= ( t − 405) 5, если 495c > t ≥ 405,  U _ stop * +1( t − 495 ) = 6,если 585c > t ≥ 495,  1( t − 585 ) 7, если 722c ≥ t ≥ 585. U _ stop * += (5.20) На рисунке 5.10 показан фрагмент структурной схемы модели движения ПО, реализующий программу формирования управляющего сигнала U_stop*(t). Сигнал U_stop*(t) поступает на вход изодромного звена с передаточной функцией W ( s ) = s (рисунок 5.11). На выходе изодромного звена всякий раз при s +1 скачке сигнала U_stop*(t) возникает переходный процесс U _ stop ( τ ) =exp ( − τ ) ⋅ 1( τ ) , τ =t − t*, t* = 45 + ( k − 1) ⋅ 90, k = 1,2,...,7 (5.21)

124 Рисунок 5.10 – Структурная схема, формирующая сигнал U_stop*(t), управляющий остановками ПО (подсистема «7 остановок»). Рисунок 5.11 – Схема формирования сигнала U_stop (подсистема «Программа остановок»). Сигнал U_stop управляет входом «у» апериодического звена, формирующего сигнал-множитель с именем Q_stop, в соответствии с условиями (рисунок 5.12): 1, y= 0, если= x U _ stop − 0.3 ≤ 0, если= x U _ stop − 0.3 > 0. (5.22) Сигналы типа Q_stop входят сомножителями в выражения амплитуд всех кинематических параметров модели движения элемента ПО, обеспечивая их кратковременное обнуление во время остановок. Рисунок 5.12 – Схема формирования сигнала-множителя Q_stop (подсистема «Детектор остановок»)

125 Аналогично условиям (5.22) формируется заданная путевая скорость с учетом остановок ПО (рисунок 5.13): 0.9, Vзад =  0, если= x U _ stop − 0.3 ≤ 0, x U _ stop − 0.3 > 0. если= (5.23) Рисунок 5.13 – Схема формирования заданного значения путевой скорости (подсистема «Управление путевой скоростью»). 5.2.1 Кинематические уравнения движения блока ДПИ Основная задача вывода уравнений движения блока ДПИ – получить уравнения проекций вектора кажущегося ускорения блока ДПИ и проекций вектора его угловой скорости на оси системы координат Oc X cYc Z c . Вектор скорости движения полюса Ос системы координат Oc X cYc Z c , связанной с блоком ДПИ, определяется по формуле [69]:   dRO0Oc      VO= V = + ω   O1 1 × RO1Oc , c  dt    (5.24) g где  RO0Oc – радиус-вектор полюса Ос в земной системе координат;   dRO0Oc     – производная радиуса-вектора RO0Oc по времени относительно  dt  g земной системы координат;  RO1Oc – радиус-вектор полюса Ос в системе координат O1 X 1Y1Z1 , связанной с элементом 1 ПО;  ω1 – вектор угловой скорости элемента 1 ПО.

126 Вектор ускорения полюса Ос в земной системе координат определяется формулой [69]:            dV dR dV    dω  Oc O1Oc O1 . (5.25) = aOc  = +  1  × RO1Oc + ω1 ×      dt   dt   dt  g  dt  g  g   g   dVO1  Производная  есть вектор ускорения полюса О1 в земной системе  dt   g координат, определяемый выражением:     O  d 2h  O dV  O1 (5.26) aO1 = ig axg1 + jg 2 + k g azg1 .   = dt dt  g  Вектор кажущегося ускорения aOr1 полюса О1 в проекциях на оси земной системы координат выражается формулой:    d 2h    aOr1 = ig axOg1 + jg  2 + g  + k g azOg1 . (5.27) dt      d ω1   d ω1  Производная    выражается через локальную производную   dt 1  dt  g относительно системы координат O1 X 1Y1Z1 :     d ω1   d ω1     d ω1   d ωx1  d ω y1  d ωz1 . + j1 + k1 1 × ω1  =    + ω=  =  i1 dt dt dt  dt  g  dt 1  dt 1 (5.28) Проекции вектора угловой скорости элемента 1 ПО на оси системы координат O1 X 1Y1Z1 определяются выражениями: ωx1 = γ 1 + ψ 1 sin ( ϑ1 ) ,   ω y1 = ψ 1 cos ( ϑ1 ) cos ( γ1 ) + ϑ 1 sin ( γ1 ) ,   ωz1 = ϑ 1 cos ( γ1 ) − ψ 1 cos ( ϑ1 ) sin ( γ1 ) .  Продифференцировав выражения (5.29), получается: (5.29)

127 d ωx1   1 sin ( ϑ1 ) + ψ 1ϑ 1 cos ( ϑ1 ) , = γ1 + ψ  dt  d ω y1  sin ( γ ) + ϑ γ cos ( γ ) −   1 cos ( ϑ1 ) cos ( γ1 ) + ϑ = ψ 1 1 1 1 1  dt  −ψ 1ϑ 1 sin ( ϑ1 ) cos ( γ1 ) − ψ 1γ 1 cos ( ϑ1 ) sin ( γ1 ) ,   d ωz1    1 cos ( ϑ1 ) sin ( γ1 ) −  = ϑ1 cos ( γ1 ) − ϑ1γ 1 sin ( γ1 ) − ψ dt   −ψ 1γ 1 cos ( ϑ1 ) cos ( γ1 ) + ψ 1ϑ 1 sin ( ϑ1 ) sin ( γ1 ) .   (5.30) Выражения для производных углов ψ1 , ϑ1 , γ1 по времени. ϑ 1 =−2ϑmω2 sin ( 2ω2t + ϕϑ ) ,  =−4ϑ ω2 cos ( 2ω t + ϕ ) , ϑ    ϑ 1 2 m 2  ∗ + ∆ψ ω cos ( ω t + ϕ ) ,  ψ 1 = Ψ ψ  2 m 2 (5.31)  ∗ − ∆ψ ω2 sin ( ω t + ϕ ) ,  1 = Ψ ψ ψ 2 m 2   γ 1 = − γ mω2 cos ( ω2t + ϕγ ) ,   γ1 γ mω22 sin ( ω2t + ϕγ ) . =     d ω1  Векторное произведение  × R O1Oc :   dt  g       d ω1        × R = i ω z − ω y + j ω x − ω z + k 1 1 1 ω x1 yOc − ω y1 xOc . (5.32) O1Oc y1 Oc z1 Oc z1 Oc x1 Oc    dt  g   dRO1Oc  : Векторное выражение для производной   dt   g    dRO1Oc   dRO1Oc     =    + ω= 1 × RO1Oc dt dt (5.33)  g  1    = i1 ω y1 zOc − ωz1 yOc + j1 ωz1 xOc − ωx1 zOc + k1 ωx1 yOc − ω y1 xOc . ( ( ) ) ( ) (   dR  Векторное произведение ω1 ×  O1Oc   dt )   : g ( ( ) )

128     dR  O1Oc ω1 ×  = i1 ω y1 ωx1 yOc − ω y1 xOc − ωz1 ωz1 xOc − ωx1 zOc  +    dt   g  + j1 ωz1 ω y1 zOc − ωz1 yOc − ωx1 ωx1 yOc − ω y1 xOc  +    + k1 ωx1 ωz1 xOc − ωx1 zOc − ω y1 ω y1 zOc − ωz1 yOc  .   ( ( ( ) ) ) ( ( ( ) ) ) (5.34) Вектор кажущегося ускорения полюса Ос определяется по формуле:       r  O1   d 2 h aOc = ig axg + jg  2 + g  + k g azOg1 + i1axO1c + j1a Oy1c + k1azO1c . (5.35) dt    Проекции ускорения aOr c на оси системы координат O1 X 1Y1Z1 определяются выражениями:  y1 + ωz1ωx1 ) zOc + ( ω y1ωx1 − ω  z1 ) yOc − ( ω2y1 + ω2z1 ) xOc ,  axO1c = ( ω    z1 + ωx1ω y1 ) xOc + ( ωz1ω y1 − ω  x1 ) zOc − ( ω2x1 + ω2z1 ) yOc ,  a Oy1c = ( ω   x1 + ω y1ωz1 ) yOc + ( ωx1ωz1 − ω  y1 ) xOc − ( ω2x1 + ω2y1 ) zOc .  azO1c = ( ω  (5.36) Проекции вектора (5.35) на оси системы координат OcXcYcZc: a   r  =  a yc   r  a zc  r xc  axOg1   axO1c     Oc   d 2h  т Cтg1,c  2 + g  + C1',c  a y1  . dt  Oc    O1 a  az1    zg   (5.37) Выражение (5.37) записывается в скалярном виде:   d 2h  a = c a + c21  2 + g  + c31azOg1 + p11axO1c + p21a Oy1c + p31azO1c ,   dt   2  d h   a yrc= c12 axOg1 + c22  2 + g  + c32 azOg1 + p12 axO1c + p22 a Oy1c + p32 azO1c ,   dt    2 d h  azrc= c13axOg1 + c23  2 + g  + c33azOg1 + p13axO1c + p23a Oy1c + p33azO1c .   dt   r xc O1 11 xg (5.38) Проекции вектора угловой скорости блока ДПИ на связанные с ним оси вычисляются векторным уравнением: ωx ωy т т т ωx1 ω y1 ωz1  . ωz  = C1',c (5.39)

129 Уравнение (5.39) в скалярной форме: ωx = p11ωx1 + p21ω y1 + p31ωz1 ,   ω y = p12ωx1 + p22ω y1 + p32ωz1 ,   ωz = p13ωx1 + p23ω y1 + p33ωz1.  (5.40) Матрица преобразования однородных координат при переходе от связанной системы к нормальной земной определяется выражениями: C g ',ПО R O1Oc + R O0O1    , 1   C g ',c = C g ',ПОC1',c = ( uij ) ⋅ ( pij ) = ( qij ) ,  3×3 3×3 3×3  C g1,c R O0Oc   M g ,c M = =  . g , g 1М g 1,c  1   G0  C g ',c M g ,c M М M M = =  g, g ' g ',ПО ПО,1′ 1',c  G0 (5.41) При приравнивании выражений для матриц C g ',c и C g1,c получаются формулы для вычисления кинематических углов ψ, ϑ, γ ориентации блока ДПИ как функций углов ориентации элемента 1 ψ1, ϑ1, γ1, задаваемых в модели движения ПО в виде известных функций времени. = ϑ arcsin ( p11 sin ϑ1 + p21 cos γ1 cos ϑ1 − p31 cos ϑ1 sin γ1 ) ,    Sγ   Sψ   = γ arctg= ψ arctg  .   ,   C   Cγ   ψ  (5.42) Функции S γ , Cγ , Sψ , Cψ определяются выражениями: = S γ p33 cos ϑ1 sin γ1 − p13 sin ϑ1 − p23 cos γ1 cos ϑ1 ,   = Cγ p12 sin ϑ1 + p22 cos γ1 cos ϑ1 − p32 cos ϑ1 sin γ1 ,  = Sψ p11 cos ϑ1 sin ψ1 − p21 ( cos ψ1 sin γ1 + cos γ1 sin ψ1 sin ϑ1 ) −   − p31 ( cos γ1 cos ψ1 − sin γ1 sin ψ1 sin ϑ1 ) ,   = Cψ p11 cos ϑ1 cos ψ1 + p21 ( sin γ1 sin ψ1 − cos γ1 cos ψ1 sin ϑ1 ) +   + p31 ( cos γ1 sin ψ1 + cos ψ1 sin γ1 sin ϑ1 ) .  (5.43)  ψ при Выражения для кинематических угловых скоростей блока ДПИ ϑ , γ, условии = CПО,c времени: p ) (= ij 3×3 const получаются дифференцированием формул (5.42) по

130 1 ( p11 cos ϑ1 − ( p21 cos γ1 − p31 sin γ1 ) sin ϑ1 ) ϑ 1 −   cos ϑ  − ( p21 sin γ1 + p31 cos γ1 ) γ 1 cos ϑ1  ,   S γ Cγ − S γ C γ  = γ cos 2 γ, 2  Cγ     SψCψ − SψCψ 2  = ψ cos ψ . Cψ2  = ϑ ( ( ) (5.44) ) Производные функций S γ , Cγ , Sψ , Cψ определяются выражениями: S γ = ( p23 cos γ1 sin ϑ1 − p33 sin ϑ1 sin γ1 − p13 cos ϑ1 ) ϑ 1 +   + ( p23 sin γ1 cos ϑ1 + p33 cos ϑ1 cos γ1 ) γ 1 ,   C γ ( p12 cos ϑ1 − p22 cos γ1 sin ϑ1 + p32 sin ϑ1 sin γ1 ) ϑ 1 −  =  − ( p32 cos ϑ1 cos γ1 + p22 sin γ1 cos ϑ1 ) γ 1 ,  (5.45) Sψ = PϑS1 ϑ 1 + PψS1 ψ 1 + PγS1 γ 1 ,   S Pϑ= p31 sin γ1 sin ψ1 cos ϑ1 − p11 sin ϑ1 sin ψ1 − p21 cos γ1 sin ψ1 cos ϑ1 ,  1  = PψS1 p11 cos ϑ1 cos ψ1 − p21 ( cos γ1 cos ψ1 sin ϑ1 − sin ψ1 sin γ1 ) +   + p31 ( cos γ1 sin ψ1 + sin γ1 cos ψ1 sin ϑ1 ) ,   PγS1 p31 ( sin γ1 cos ψ1 + cos γ1 sin ψ1 sin ϑ1 ) − =   − p21 ( cos ψ1 cos γ1 − sin γ1 sin ψ1 sin ϑ1 ) ,  (5.46) C ψ= PϑC1 ϑ 1 + PψC1 ψ 1 + PγC1 γ 1 ,   C = Pϑ 1 p31 cos ψ1 sin γ1 cos ϑ1 − p11 cos ψ1 sin ϑ1 − p21 cos γ1 cos ψ1 cos ϑ1 ,   = PψC1 p31 ( cos γ1 cos ψ1 − sin γ1 sin ψ1 sin ϑ1 ) − p11 sin ψ1 cos ϑ1 +   + p21 ( sin γ1 cos ψ1 + cos γ1 sin ψ1 sin ϑ1 ) ,   = PγC1 p21 ( cos γ1 sin ψ1 + sin γ1 cos ψ1 sin ϑ1 ) +   + p31 ( cos ψ1 cos γ1 sin ϑ1 − sin ψ1 sin γ1 ) .  (5.47) Кинематические уравнения для вычисления проекций угловой скорости блока ДПИ на оси связанной системы координат имеют вид [69]:

131   ω y = ψ cos ϑ cos γ + ϑ sin γ,   ωz = ϑ cos γ − ψ cos ϑ sin γ.  ωx = γ + ψ sin ϑ, (5.48) Координаты полюса Ос в земной системе отсчета в соответствии с выражениями, приведенными в приложении Б, определяются формулой:  R O0Oc   R O1Oc   E R O0O1  C g ',ПО G 0т   R O1Oc  M M = =  =       g ,g ′ g ′,ПО  1  1  G 1 1 1      0   G0 C g ',ПО R O0O1   R O1Oc  C g ',ПО R O1Oc + R O0O1  = =    . G 1 1 1     0  (5.49) Из (5.49) следует: = R O0Oc C g ',ПО R O1Oc + R O0O1 . (5.50) Формула (5.50) записывается через элементы входящих в нее матриц.  xg    =  yg   zg     u11 u12 u13   xОc   x1g      u u22 u23   yОc  +  y1g  . 21   u31 u32 u33   zО   z1g   c (5.51) Ординаты полюсов О1 и Ос в земной системе координат есть геометрические высоты этих точек, для которых вводятся обозначения: = H1 y= H yOc . 1g , (5.52) Высота полюса Ос определяется формулой: H = H1 + u21 xОc + u22 yОc + u23 zОc = (5.53) = H1 + xОc sin ϑ1 + yОc cos γ1 cos ϑ1 − zОc cos ϑ1 sin γ1. Вектор скорости полюса Ос:   dRO0Oc      VO= = V + ω   O1 1 × RO1Oc c dt  g   Векторное произведение ω1 × RO1Oc : (5.54)      ω1 × RO1Oc= i1 ω y1 zOc − yOc ωz1 + j1 xOc ωz1 − ωx1 zOc + k1 ωx1 yOc − xOc ω y1 . ( ) ( ) ( Проекции вектора (5.24) на оси нормальной системы координат. ) (5.55)

132 VxOc   Og  c V= xg   Oc  Vxg  VxO1   u u  g   11 12 VxOg 1  + u21 u22  O Vxg 1  u31 u32 u13   ω y1 zOc − yOc ωz1    u23   xOc ωz1 − ωx1 zOc  .  u33  ωx1 yO − xO ω y1  c c   (5.56) Формула (5.56) позволяет сформировать начальные значения проекций скорости полюса Ос на оси нормальной системы координат, которые поступают на входы начальных значений цифровых интеграторов, вычисляющих текущие значения соответствующих проекций вектора путевой скорости (рисунок 4.3). 5.3 Построение модели персональной информационно-измерительной системы 5.3.1 Модели ДПИ Модели датчиков первичной информации построены в соответствии с уравнениями:     ωx ≥ ∆ ω ,      ωy ≥ ∆ω ,    ωz ≥ ∆ ω ,  (5.57)     axr ≥ ∆ a ,      a yr ≥ ∆ a ,     r  az ≥ ∆ a .  (5.58)  x = ω*x + εωx , ω  y = ω*y + εω y , ω  z = ω*z + εωz , ω 0, если ωx < ∆ ω , ω*x = если ( ωx − ∆ ω sign ( ωx ) ) , 0, если ω y < ∆ ω ,  ω = если  ω y − ∆ ω sign ( ω y ) , 0, если ωz < ∆ ω , ω*z = если ( ωz − ∆ ω sign ( ωz ) ) , * y ( ) r r r a= axr* + ε ar , a= a yr* + ε a r , a= azr* + ε a r , x y z x 0, если axr  axr* =  a r − ∆ a sign ( axr )  x 0, если axr  a yr* =  a r − ∆ a sign ( a yr )  y 0, если azr  r* az =  r r  az − ∆ a sign ( az ) ( ( ( y z < ∆a , ), если < ∆a , ), если < ∆a , ), если

133 Обозначения в уравнениях (5.57), (5.58): ωx , ω y , ωz – проекции вектора угловой скорости блока ДПИ на оси системы координат OcXcYcZc;  x,ω  y ,ω  z – нормированные сигналы на выходах датчиков угловой скорости; ω ω*x , ω*y , ω*z – полезные составляющие выходных сигналов датчиков угловой скорости с учетом зон нечувствительности последних; ∆ ω – ½ зоны нечувствительности датчиков угловой скорости; εωx , εωy , εωz – шумы датчиков угловой скорости; axr , a yr , azr – проекции вектора кажущегося ускорения блока ДПИ на оси системы координат OcXcYcZc; a xr , a yr , a zr – нормированные сигналы на выходах акселерометров; axr* , a yr* , azr* – полезные составляющие выходных сигналов акселерометров с учетом зон нечувствительности последних; ∆ a – ½ зоны нечувствительности акселерометров; ε a r , ε a r , ε a r – шумы акселерометров. x y z 5.3.2 Модель вычислителя углов Эйлера Модель вычислителя углов Эйлера выполняет интегрирование кинематических дифференциальных уравнений углового движения блока ДПИ [69]:  = ( ω y cos ( γ ) − ωz sin ( γ ) ) / cos ( ϑ) , ψ   ϑ = ω sin ( γ ) + ω cos ( γ ) ,  y z  γ = ωx − ω y cos ( γ ) tg ( ϑ ) + ωz sin ( γ ) tg ( ϑ ) .  (5.59) При реализации процесса вычисления углов Эйлера по уравнениям (5.59) учтем, что все переменные в этих уравнениях получены в результате обработки исходной информации, поступающей с выходов тех или иных приборных устройств. Тогда уравнения (5.62) записываются в виде:

134 ( )  ey cos ( γ ) − ω  ez sin ( γ ) ) / cos ϑ , ψ = ( ω      e  ez cos ( γ ) , ϑ = ω y sin ( γ ) + ω    ex − ω  ey cos ( γ ) tg ϑ + ω  ez sin ( γ ) tg ϑ ,  γ = ω  ( ) ( ) где (5.60) ψ , ϑ , γ – вычисленные значения производных углов Эйлера;  ex , ω  ey , ω  ez – сигналы с выхода экстраполятора 1; ω  , γ – вычисленные значения углов Эйлера, причем ϑ   , если U _ KS = 1, 1,  γ КС , если U _ KS = ϑ ϑКС = γ   , если U _ KS = 0. 0,  γ ИНС , если U _ KS = ϑ ИНС Уравнения (5.60) в матричном виде. ψ   ex  ω   e  y,  ϑ = Q ω      ez  ω   γ  где Q = ( qij ) 3×3 (5.61) ; ( ) q11 = q12 = 0, cos ( γ ) / cos ϑ , q21 = q22 = 0, sin ( γ ) , q31 = 1, ( ) − cos ( γ ) tg ϑ , q32 = ( ) − sin ( γ ) / cos ϑ ,  q13 =  q23 = cos ( γ ) ,  (5.62)  q33 = sin ( γ ) tg ϑ .  ( ) Уравнения (5.61) при переходе к разностным уравнениям примут вид:  ex ( k − 1)  ω ψ ( k ) − ψ ( k − 1)    )  Q ( k − 1) ω ey ( k − 1) ∆t ,  ϑ ( k ) − ϑ ( k − 1= e ω   γ ( k ) − γ ( k − 1)    z ( k − 1)  где (5.63) Δt – период дискретности (шаг интегрирования). При реализации уравнений (5.63) в пакете Simulink используется звено с передаточной функцией W ( z ) = z −1 , на выходе которого сигнал отстает на один шаг дискретного времени k по отношению к сигналу входа, т.е., например, ψ ( k − 1) = z −1ψ ( k ) . С учетом этого уравнения (5.63) записывается в виде:

135 ψ ( k ) (1 − z −1 )    ex ( k )   ω      −1 e  ϑ ( k ) (1 − z =  z −1  Q ( k ) ω  k ( ) ) y   ∆t  .   e  ω    γ ( k ) (1 − z −1 )    z ( k )     (5.64) Из (5.64) искомые переменные ψ ( k ) , ϑ ( k ) , γ ( k ) выражаются:   ex ( k )   ω ψ ( k )  1   e      = ϑ k Q k ω k ( ) ( ) ( ) y   ∆t  .   z −1 e    z ( k )   γ ( k )  ω    (5.65) 5.3.3 Модель вычислителя проекций вектора абсолютного ускорения блока ДПИ на оси нормальной системы координат Модель вычислителя проекций вектора абсолютного ускорения блока ДПИ на оси нормальной системы координат выполняет: - проектирование вектора кажущегося ускорения на оси нормальной системы координат по формуле  a xr  g где a yr g a zrg  = C g1,c ( a xr ) e  т ( a ) ( a )  r y e r z e т , (5.66) – вычисленные значения проекций вектора кажущегося a xrg , a yr g , a zrg ускорения на оси нормальной системы координат; ( a ) , ( a ) , ( a ) r x e r y e r z e – нормированные сигналы с выхода подсистемы «Блок В.1.1.4.Экстраполятор 2»; C g1,c = ( сij ) 3×3 – матрица направляющих косинусов (приложение Б); - вычисление проекций вектора абсолютного ускорения на оси нормальной системы координат по формуле  a x   g  a y=  g    a zg   a xr   0   g    a yr g  +  − g  ;  r     a zg   0  (5.67) - фиксацию кратковременных остановок ПО с помощью детектора остановок;

136 - включение режима «ZUPT» по сигналу Uс=1 и передачу этого сигнала, выполняющего сброс выходов интеграторов, вычисляющих проекции скорости блока ДПИ, на вход подсистемы «Блок В.1.4.Вычислитель координат и проекций скорости блока ДПИ на оси земной СК» (приложение Г). 5.3.4 Модель вычислителя координат и проекций скорости блока ДПИ на оси земной системы координат Модель вычислителя координат и проекций скорости блока ДПИ на оси земной системы координат реализует решение дифференциальных кинематических уравнений движения блока ДПИ  dVxg dVyg dVzg    a= azg ,  = a= xg , yg ,  dt dt dt  dx g  dy g  dzg   V= Vzg ,  = V= xg , yg , dt dt dt  (5.68) записанных по аналогии с (5.65) в виде разностных уравнений: Vx ( k )   a x ( k )   g   g  1   Vyg ( k )  =  a yg ( k )  ∆t ,   z −1   Vzg ( k )   a zg ( k )  Vx ( k )   x g ( k )  g  1      y g ( k )  = z − 1 Vyg ( k )  ∆t .    zg ( k )    Vzg ( k )  (5.69) 5.3.5 Модель дальномерной СУО Модель дальномерной СУО построена в соответствии с алгоритмами вычисления дальностей и опорных углов при условии, что ОПВ горизонтальна, приведенными в главе 2. 5.3.6 Модель схемы комплексирования Модель схемы комплексирования построена в соответствии с уравнениями, приведенными в разделе 4.2. 5.3.7 Модель обнуления выходов интеграторов Модель обнуления выходов интеграторов, вычисляющих проекции путевой скорости на оси нормальной системы координат, построена в соответствии с блоксхемой, приведенной в разделе 4.3.

137 5.3.8 Модель коррекции угла рысканья и вектора путевой скорости по приоритетным направлениям Модель коррекции угла рысканья и вектора путевой скорости по приоритетным направлениям построена в соответствии с блок-схемой, приведенной в разделе 4.4. Созданная математическая модель персональной ИИС, структура и описание которой приведены в приложении Г, позволяет использовать в качестве тестовых сигналов имитационную модель движения наземного ПО, а также массивы с датчиков, полученные экспериментально. Модель предусматривает возможности дальнейшего развития при совершенствовании модели движения наземного ПО. 5.4 Представление результатов оценки точности определения координат местоположения подвижного объекта с помощью автономной персональной информационно-измерительной системы Получена интервальная оценка разработанных способа и алгоритма определения углов наклона блока ДПИ, которая подтверждает их работоспособность и эффективность (рисунок 5.14). Так, СКО погрешности вычисления угла тангажа инерциальной СУО равно 0,208о, а СКО той же погрешности для комплексированной СУО равно 0,058о. При этом погрешность инерциальной СУО содержит постоянную составляющую с математическим ожиданием 0,257о, а математическое ожидание погрешности комплексной СУО близко к нулю. Выполнено моделирование движения ПО с блоком ДПИ при различных вариантах структуры автономной персональной ИИС: - инерциальная ИИС без комплексирования; - при комплексировании с дальномерной СУО; - при комплексировании с дальномерной СУО и с алгоритмом обнуления выходов интеграторов; - при комплексировании с дальномерной СУО и с алгоритмом коррекции по приоритетным направлениям;

138 - при комплексировании с дальномерной СУО и с алгоритмами обнуления выходов интеграторов и коррекции по приоритетным направлениям. а) Погрешности вычисления угла тангажа ИНС и КС 1 M(∆ϑ_ИНС)=0,257 градус σ(∆ϑ_ИНС)=0,208 градус 0.8 0.6 M(∆ϑ_КС)=0,00038 градус σ(∆ϑ_КС)=0,058 градус ,o 0.4 0.2 0 -0.2 -0.4 0 100 200 300 400 500 600 700 800 t, c б) Погрешности вычисления угла крена ИНС и КС 1.5 M(∆γ_ИНС)=0,757 градус σ(∆γ_ИНС)=0,279 градус ,o 1 0.5 M(∆γ_КС)=0,00087 градус σ(∆γ_КС)=0,106 градус 0 -0.5 0 100 200 300 400 500 600 700 800 t, c Рисунок 5.14 – Интервальная оценка способа и алгоритма определения углов наклона блока ДПИ: а – погрешности вычисления угла тангажа ИНС и КС; б - погрешности вычисления угла крена ИНС и КС Программная траектория представляет собой замкнутый прямоугольный маршрут размером (80×80) м. ПО начинает движение из угла А квадрата, дважды обходя его по периметру против часовой стрелки со средней скоростью 3,2 км/ч, и возвращаясь к месту старта (рисунок 5.15).

139 Xg 80 70 60 40 g x ,м 50 30 20 10 А 0 -90 -80 -70 -60 -50 -40 -30 -20 -10 Zg 0 10 z ,м g Рисунок 5.15 – Линия пути действительного движения блока ДПИ при двойном обходе ПО по замкнутому маршруту Дойдя до середины стороны квадрата, ПО останавливается на одну секунду, а затем продолжает движение. Последний отрезок маршрута после седьмого поворота ПО проходит без остановки. Таким образом, за время всего движения ПО останавливается на одну секунду семь раз. Время движения ПО от старта до финиша равно 12 минутам. На рисунках 5.16 – 5.20 приведены графики линий пути для некоторых вариантов моделирования. Точность определения координат местоположения оценивалась по двум параметрам: - погрешность определения пройденного расстояния в процентах по отношению к действительной длине линии пути; - погрешность от несовпадения координат точки старта и вычисленных координат точки финиша в процентах по отношению к действительной длине линии пути.

140 10 4 3.5 заданная траектория 3 вычисленная траектория 2.5 g x ,м 2 1.5 1 0.5 0 0 1 0.5 1.5 2 2.5 3 4 3.5 z ,м 10 g 4 Рисунок 5.16 – Линия пути, вычисленная инерциальной ИИС без комплексирования и применения разработанных алгоритмов Комплексирование ИНС с ДМ СУО 0 g x ,м 50 заданная траектория -50 вычисленная траектория -150 -100 -50 0 50 100 150 z ,м g Рисунок 5.17 – Линия пути, вычисленная при комплексировании с дальномерной СУО без применения алгоритмов

141 заданная траектория вычисленная траектория Комплексирование ИНС с ДМ СУО 80 60 g x ,м 40 20 0 -140 -120 -100 -80 -40 -60 -20 40 20 0 z ,м g Рисунок 5.18 – Линия пути, вычисленная при комплексировании с дальномерной СУО и с алгоритмом обнуления выходов интеграторов заданная траектория вычисленная траектория Комплексирование ИНС с ДМ СУО 80 40 g x ,м 60 20 0 -100 -80 -60 -40 -20 0 20 z ,м g Рисунок 5.19 – Линия пути, вычисленная при комплексировании с дальномерной СУО и с алгоритмом коррекции по приоритетным направлениям

142 заданная траектория вычисленная траектория Комплексирование ИНС с ДМ СУО 80 40 g x ,м 60 20 0 -100 -80 -60 -40 -20 20 0 z ,м g Рисунок 5.20 – Линия пути, вычисленная при комплексировании с дальномерной СУО и с алгоритмами обнуления выходов интеграторов и коррекции по приоритетным направлениям Результаты оценки точности определения координат местоположения ПО для всех вариантов приведены в таблице 5.1. Таблица 5.1. Результаты оценки точности определения координат местоположения ПО Вариант Инерциальная ИИС без комплексирования При комплексировании с дальномерной СУО При комплексировании и с алгоритмом обнуления При комплексировании и с алгоритмом коррекции по приоритетным направлениям При комплексировании и с алгоритмами обнуления и коррекции по приоритетным направлениям Длина линии пути, м Вычисленный путь, м Погрешность определения пройденного расстояния (σ) м % Расстояние между точкой старта и вычисленной точкой финиша (σ) м % 641,9 29870 29228 4553 18000 2804 641,9 661 19,1 2,98 103 16 641,9 628 -13,9 -2,17 23 3,58 641,9 619,1 -22,8 -3,55 1,0 0,16 641,9 629,5 -12,4 -1,93 3,2 0,5

143 ВЫВОДЫ 1. Разработана имитационная математическая модель персональной инерциальной ИИС, которая позволяет: - задавать программу движения наземного ПО и блока ДПИ; - формировать текущие значения угловых и линейных параметров движения объекта; - производить оценку точности работы ИИС при различных режимах. 2. Получена интервальная оценка разработанных способа и алгоритма определения углов наклона блока ДПИ, которая подтверждает их работоспособность и эффективность. 3. Произведена оценка точности определения координат местоположения ПО с помощью автономной персональной ИИС, которая показала, что: - удовлетворительная работа персональной ИИС без комплексирования с коррекцией углового положения блока ДПИ при его установке на шлеме (или поясе) практически невозможна; - комплексирование персональной ИИС с дальномерной СУО существенно повышает точность определения координат местоположения ПО по сравнению с ИИС без комплексирования, однако, полученные результаты не удовлетворяют предъявляемым требованиям; - алгоритм обнуления выходов интеграторов при обнаружении относительно неподвижных состояний блока ДПИ заметно повышает точность определения координат местоположения ПО, однако, при повторных обходах замкнутого маршрута картина линии пути имеет тенденцию к вращению в азимуте по часовой стрелке; - существенное повышение точности построения линии пути и определения координат местоположения ПО дает алгоритм коррекции вектора путевой скорости на приоритетных направлениях. В сочетании с алгоритмом обнуления выходов интеграторов этот алгоритм дает наилучший показатель определения длины пройденного пути и удовлетворительную точность определения координат местоположения ПО.

144 ЗАКЛЮЧЕНИЕ 1. Выполнена научно-обоснованная техническая разработка автономной персональной ИИС наземного позиционирования, в которой обеспечено улучшение технических характеристик системы посредством существенного уменьшения погрешности определения местоположения за счет введения дополнительной СУО, использующей неинерциальный принцип измерения, а также расширение эксплуатационных возможностей системы за счет ее универсальности, полной автономности и помехозащищенности. 2. Разработан новый способ и алгоритм определения углов наклона блока ДПИ без накопления погрешности с течением времени относительно как опорной плоскости, так и плоскости горизонта, что позволяет расширить эксплуатационные возможности инерциальной ИИС, увеличив временной диапазон ее использования. 3. Разработано новое устройство определения углов наклона блока ДПИ относительно как опорной плоскости, так и плоскости горизонта с применением комплексирования инерциальной и дальномерной СУО, позволяющее на порядок уменьшить погрешность вычисления углов наклона блока ДПИ относительно плоскости горизонта. 4. Разработана имитационная математическая модель персональной ИИС, позволяющая задавать программу движения ПО, формировать текущие значения угловых и линейных параметров движения объекта, формировать интервальные оценки точности работы ИИС при различных режимах. 5. Результаты диссертационной работы внедрены в ООО СКБ «Новые Технологии» (г. Казань) в виде способа и устройства определения углов наклона блока инерциальных измерителей комплексной системы угловой ориентации относительно плоскости горизонта. Результаты внедрения подтверждены соответствующим актом (приложение Д). Имитационная математическая модель навигационной системы прошла испытания и подтвердила свою работоспособность. Новизна и полезность технических решений подтверждены двумя патентами РФ на изобретение.

145 СПИСОК ЛИТЕРАТУРЫ 1. Каплин А.Ю., Степанов М.Г., Ярмолич А.Г. Оценка точности пешеходной навигационной системы методом имитационного моделирования // Радиопромышленность. – 2017. - №27 (4). - С. 6-12. 2. Фадеев C. Словарь сокращений современного русского языка. - С.-Пб.: Политехника, 1997. - 527 с. 3. Каплин навигационной А.Ю., системы Степанов М.Г. высокоточного Использование позиционирования автономной пешехода на местности // Информационно-измерительные системы. – 2015. - №6. – С.86-92. 4. Кирсанов А.В. Порядок использования навигационных систем в подготовке служебных собак: Учебно-практическое пособие. – Ростов-на-Дону: ФГКУ ДПО РШ СРС МВД России, 2016. – 16 с. 5. Ткачёв А. В., Шаныгин С. В. Обзор мобильных роботов, использующих бортовые системы навигации для автономного планирования пути к заданной цели // Молодой ученый. - 2015. - №19. - С. 215-219. 6. Матвеев В.В. Инерциальные навигационные системы: Учебное пособие. Изд-во ТулГУ, 2012.-199 с. 7. Солдаткин В.В. Построение и методы исследования информационно- измерительных систем: Учебное пособие / Под ред. проф. В.М. Солдаткина. Казань: Изд-во Казан. гос. техн. ун-та, 2008. - 198 с. 8. Дэвидсон П., Такала Я. Разработка алгоритмов инерциальной навигационной системы с учетом особенностей походки человека // Гироскопия и навигация. - 2013. - № 1(80). - С. 86-94. 9. Горенштейн И.А., Шульман И.А. Инерциальные навигационные системы. - М.: Машиностроение,1970. - 232 с. 10. Синютин С.А. Гибридные многоосевые датчики для бесплатформенных инерциальных навигационных систем // Ползуновский вестник. – 2015. - №1 (2). – С. 171-175.

146 11. Маринушкин П.С., Бахтина В.А., Подшивалов И.А., Стукач О.В. Вопросы разработки инерциальных пешеходных навигационных систем на основе МЭМС-датчиков // Наука и Образование. 2015. № 6. С. 157–173. 12. Шаймарданов И.Х., Дзуев А.А., Голиков В.П. Методы калибровки бесплатформенной навигационной системы (БИНС) различного класса точности // XXIII Санкт-Петербургская международная конференция по интегрированным навигационным системам. Сборник материалов. Под ред. Акад. РАН В.Г. Пешехонова. – СПб.: ГНЦ РФ АО «Концерн ЦНИИ «Электроприбор», 2016. С. 46-51. 13. Николаев С.Г. Калибровка бесплатформенных инерциальных навигационных систем // Изв. ВУЗов. Приборостроение. – 2009. – Т.52 (№7). – С. 50-55. 14. Syed Z. Design and implementation issues of a portable navigation system: PHD Thesis. – Calgary, Canada: The University of Calgary, 2009. – 230 p. В.В., Конаков А.С., микроэлектромеханических датчиков ускорений 15. Шаврин Тисленко и В.И. угловых Калибровка скоростей в бесплатформенных инерциальных навигационных системах // Доклады ТУСУРа. 2014. - №1 (25), часть 2. - С. 265-269. 16. Zhang X., Li Y., Mumford P., Rizos C. Allan Variance Analysis on Error Characters of MEMS Inertial Sensors for an FPGA-Based GPS/INS System // Proceedings of the International Symposium on GPS/GNSS: Tokyo, Japan. 11–14 November 2010. - Р. 127–133. 17. основных Кутовой Д. А., Ситников П. В., Федотов А. А., Якимов В. Л. Оценка характеристик бесплатформенного инерциального блока с использованием вариации Аллана // Вестник Самарского государственного аэрокосмического университета. - 2014. - №1 (43). - С. 201-209. 18. Кузовков Н.Т., Салычев О.С. Инерциальная навигация и оптимальная фильтрация. – М.: Машиностроение, 1982. – 216 с.

147 19. Сеславин А.И. Фильтры Калмана. Методические указания для студентов специальности «Управление и информатика в технических системах». М. - : МИИТ, 2011.- 16 с. 20. Грачев А.Н., Аль-Сабул Али Хусейн Хасан. Адаптивный расширенный фильтр Калмана для трассового сопровождения целей с использованием генетического алгоритма // Информатика и системы управления. – 2014. - №2 (40). – С. 102-112. 21. Bar-Shalom Y., Li X.R. and Kurubarajan T. Estimation with Application to Tracking and Navigation. – Wiley, 2001. 22. Jimenez А.R., Seco F., Prieto J.C., Guevara J. Indoor Pedestrian Navigation using an INS/EKF framework for Yaw Drift Reduction and a Foot-mounted IMU // in WPNC 2010: 7th Workshop on Positioning, Navigation and Communication. - 2010. 23. Семушин И.В., Цыгнаова Ю.В., Захаров К.В. Устойчивые алгоритмы фильтрации – обзор и новы результаты для систем судовождения // Информационные технологии и вычислительные сети. – 2013. - № 4. – С. 90-112. 24. Основы построения бесплатформенных инерциальных навигационных систем/ В.В. Матвеев, В.Я. Распопов / Под общ. ред. д.т.н. В. Я. Распопова. – СПб.: ГНЦ РФ ОАО «Концерн «ЦНИИ «Электроприбор», 2009. – 280 с. 25. Маринушкин П.С., Нестеренко Т.Г. Малогабаритная система персональной навигации на базе неортогонального инерциального измерительного блока с избыточной структурой. // Наука и Образование: Научное издание. – 2016. - №8. – С. 121-134. 26. Gupta A.K., Skog I., Handel P. Long-term performance evaluation of a foot- mounted pedestrian navigation device. IEEE Publ. - 2015. - P. 1-6. 27. Nilsson J-O., Skog I., Handel P. Aligning the forces – eliminating the misalignments in IMU arrays // IEEE Trans. Instrum. Meas. - Oct. 2014. - vol. 63, no. 10. - P. 2498-2500. 28. Никольский Б.А. Основы радиотехнических систем: учеб. . – Самара: Изд-во Самар. гос. аэрокосм. ун-та, 2013. – 315 с.

148 29. RTLS «Лабиринт» - система определения местоположения и перемещения в закрытых помещениях [Электронный ресурс] / Официальный сайт КБ «Навигационные Технологии». – Режим доступа: http://navi-tec.ru/rtls-labirint. 30. Kupervasser O., Rubinstein A. Correction of Inertial Navigation System’s Errors by the Help of Video-Based Navigator Based on Digital Terrarium Map. // Positioning. – February 2013. - vol. 4, no. 1. – P. 89-108. 31. Кесслер К., Ашер К., Флад М., Троммер Г.Ф. Многосенсорная индивидуальная система навигации с визуальными средствами коррекции для использования внутри помещения // Гироскопия и навигация. – 2012. - №1 (76). – С. 67-84. 32. Peter M., Schater B., Jo Agilia Bitsch Link. Versatile Geo-referenced Maps for Indoor Navigation of Pedestrians // International Conference on Indoor Positioning and Indoor Navigation. – November 2012. 33. Aggarwal P., Thomas D., Ojeda L., Borenstein J. Map Matching and Heuristic Elimination of Gyro Drift for Personal Navigation Systems in GPS-denied Conditions // Journal of Measurement Science and Technology. – 2011. - № 22. – Р. 1-21. 34. Спецификация на инерциальный измерительный блок ADIS16448 Compact, Precision Ten Degrees of Freedom Inertial Sensor [Электронный ресурс]. – Режим доступа: http://www.analog.com/media/en/technical-documentation/data- sheets/ADIS16448.pdf. 35. Лю Ю., Цай Т., Ян Х., Лю Ч., Сун Ц., Юй М. Пешеходная интегрированная навигационная система с микроИИМ / GPS / магнетометром / бароальтиметром // Гироскопия и навигация. – 2015. - № 4 (91). – С. 29-41. 36. Абдулрахим Х., Семан К., Отман М., Шуиб Ф. М. М., Мур Т., Хайд К., Хилл К. Коррекция курсовых показаний пешеходных ИНС по данным магнитометров // Гироскопия и навигация. – 2014. - № 1 (84). – С. 50-61. 37. Chung J., Donahoe M., Schmandt C., Kim I. J., Razavai P., Wiseman M. Indoor location sensing using geo-magnetism // In Proceedings of the 9th International

149 Conference on Mobile Systems, Applications, and Services. - Washington, USA. - 28 June – 1 July 2011. – P. 141-154. 38. Патент РФ 2459181. Шагомер / Душа Д. Заявл. 27.04.2012. Опубл. 20.08.2012. Бюл. № 23. 39. Borenstein J., Ojeda L., Kwanmuang S. Heuristic Reduction of Gyro Drift in IMU-based Personnel Tracking Systems // Journal of Navigation. – 2010. - № 62 (1). – Р. 41-58. 40. Jimenez A.R., Seco F., Zampella F., Prieto J.C., Guevara J. Improved Heuristic Drift Elimination with Magnetically-aided Dominant Directions (MiHDE) for Pedestrian Navigation in Complex Buildings // Journal of Location Based Services. – 2012. - №6 (3). – Р. 186-210. 41. для Лобусов Е.С., Фомичев А.В. Исследование режима ZUPT-коррекции бесплатформенной инерциальной навигационной системы наземного подвижного объекта // Вестник МГТУ им. Н.Э. Баумана. Сер. «Приборостроение». – 2014. - №6 (99). – С. 15-24. 42. Skog I., Nilsson J.-O., Handel P. Evaluation of Zero-Velocity Detectors for Foot-Mounted Inertial Navigation Systems // International conference on indoor positioning and indoor navigation (IPIN). - 15-17 September 2010. – P. 1-6. 43. Купоросова Е.С. Оценка эффективности алгоритмов обнуления скорости пешеходной инерциальной навигационной системы при разных способах крепления блока датчиков первичной информации // XXIII Туполевские чтения (школа молодых ученых): Международная молодежная научная конференция, 8-10 ноября 2017: Материалы конференции. Сборник докладов: в 4 т. – Казань: Изд-во Академии наук РТ, 2017. – Т. 2. – С. 62-67. 44. Park S. K., Suh Y. S. A Zero Velocity Detection Algorithm Using Inertial Sensors Pedestrian Navigation Systems // Sensors. – 2010. - № 10. – Р. 9163-9178. 45. Митрофанов С.С. Теоретические и физические основы устройства оптических приборов [Электронный ресурс] / Электронный учебник по дисциплине «Прикладная оптика». доступа: https://de.ifmo.ru/bk_netra/page.php?tutindex=33. – Режим

150 46. Олещук В.А., Верещагина А.С. Методы и средства измерений, испытаний и контроля: учеб. пособие. – Комсомольск-на-Амуре: ФГБОУ ВПО «КнАГТУ», 2015. – 92 с. 47. Крайнюк О.В. Использование оптических датчиков триангуляционного типа для диагностики качества литых автомобильных дисков на этапе производства // Автоматизация и управление в технических системах. – 2012. – № 1. 48. Датчики расстояния индуктивные [Электронный ресурс] / Официальный сайт ООО "АСИС ПРО". – Режим доступа: http://sensor365.ru. 49. Радж Балдеев, Раджендран В., Паланичами П. Применение ультразвука. – Москва: Техносфера, 2006. – 576 с. 50. Жмудь В.А., Кондратьев Н.О., Кузнецов К.А., Трубин В.Г., Димитров Л.В. Ультразвуковой датчик измерения расстояния HC-SR04 // Автоматика и программная инженерия. – 2017. - №4 (22). – С.18-26. 51. Бокшанский В.Б., Бондаренко Д.А., Вязовых М.В., Животовский И.В., Сахаров А.А., Семенков В.П. Лазерные приборы и методы измерения дальности. – М.: Изд-во МГТУ им. Н.Э. Баумана, 2012. – 92 с. 52. Ставров А.А., Поздняков М.Г. Импульсные лазерные дальномеры для оптико-локационных систем // Доклады БГУИР. - 2003. - Т.1. - №2. - С. 59–65. 53. Бокшанский В.Б. Вязовых М.В., Е Тэ Вун. Метод высокоточного измерения дальности путем использования цифровой обработки ЭХО-сигнала // Вестник МГТУ им. Н.Э. Баумана. Сер. «Приборостроение» – 2011. - № S2. – С. 177188. 54. Войновский В.А., Купцов А.В. Причины некорректных измерений дальностей с помощью лазерных альномеров, используемых в Вооруженных силах // Интерэскпо Гео-Сибирь. – 2013. 55. Вильнер В.Г., Ларюшин А. И., Мартынов В.Н., Рябокуль А.С. Усовершенствование импульсных полупроводниковых лазерных дальномеров для измерений в ближней зоне // Вестник МЭИ. – 2014. - № 3. – С. 83-88.

151 56. Берников Б.О., Бокшанский В.Б., Вязовых М.В., Перов А.Н. Исследование факторов, влияющих на погрешность измерения расстояния фазовым лазерным дальномером // Инженерный журнал: наука и инновации. – 2013. – вып. 9. – С. 1–8. 57. Миниатюрные лазерные дальномеры [Электронный ресурс] / Официальный сайт «FLIR system». Режим доступа: https://www.flir.com. 58. Зенкевич С.Л., Ющенко А.С. Основы управления манипуляционными роботами: Учебник для вузов. – 2-е изд., исправ. и доп. М.: Изд-во МГТУ им. Н.Э. Баумана, 2004. – 480 с. 59. Пантюшин С.В., Назаретов В.М., Тягунов О.А. Робототехника и гибкие автоматизированные производства: учеб. пособие / под ред. И.М. Макарова. - Том 5. Моделирование робототехнических систем и гибких автоматизированных производств. - М.: Высш. шк., 1986. — 175 с. 60. Потапов А.А. Параметры угловой ориентации подвижных объектов: прикладные задачи: Учеб. Пособие. – Казань: Изд-во Казан. гос. техн. ун-та, 2010. – 90 с. 61. Бронштейн И.Н., Семендяев К.А. Справочник по математике для инженеров и учащихся ВТУЗов. – М.: Наука, 1980. – 976 с. 62. Патент РФ 2646941. Способ определения углов наклона блока инерциальных измерителей комплексной системы угловой ориентации относительно плоскости горизонта / Потапов А.А., Купоросова Е.С. Заявл. 20.12.2016. Опубл. 12.03.2018. Бюл. № 8. 63. Жуков В.К. Теория погрешностей технических измерений: учебное пособие. – Томск: Изд-во Томского политехнического университета, 2009. – 180 с. 64. Адлер Ю.П., Маркова Е.В., Грановский Ю.В. Планирование эксперимента при поиске оптимальных условий / 2-е изд., перераб. и доп. – Москва: Наука, 1976. – 279 с.

152 65. Статистические методы в инженерных исследованиях. Лабораторный практикум/ Бородюк В.П., Вощинин А.П., Иванов А.З. Под редакцией Круга Г.К. М.: Высшая школа, 1983. 217 с. 66. Купоросова Е.С. Определение параметров амортизирующего подвеса инерциального модуля на подвижном объекте методами факторного эксперимента // Вестник КГТУ им. А.Н. Туполева. – 2015. - № 4 (80). – С. 143-149. 67. Патент РФ 2649026. Устройство определения углов наклона блока инерциальных измерителей комплексной системы угловой ориентации относительно плоскости горизонта / Потапов А.А., Купоросова Е.С. Заявл. 20.12.2016. Опубл. 29.03.2018. Бюл. № 10. 68. Бесекерский В.А., Попов Е.П. Теория систем автоматического управления / Изд. 4-е. перераб. и доп. – Спб.: Изд-во «Профессия», 2003 – 752 с. 69. Бутенин Н.В., Лунц Я.Л., Меркин Д.Р.. Курс теоретической механики. Статика и кинематика. Динамика: в 2 т. — СПб.: Лань, 2006.— 730 с. 70. Бочкин А. И. Методика преподавания информатики: учеб. пособие. - Минск: Выш.шк.,1998. - 431 с. 71. Купоросова, Е.С. Влияние гармонических колебаний блока инерциальных измерителей на погрешность работы алгоритма счисления пути пешеходной навигационной системы // Электронный журнал «Труды МАИ». – 2016. - № 90. – С. 1-18. 72. MATLAB & Simulink Release Notes for R2008a [Электронный ресурс] / Официальный сайт «MathWorks». – Режим доступа: http:www.mathworks.com. 73. Бендат Дж., Пирсол А. Прикладной анализ случайных данных: Пер. с англ. – М.:Мир, 1989. – 540 с. 74. Купоросова, Е.С. Состояние вопросов повышения точности работы блока датчиков первичной информации пешеходных навигационных систем // Гагаринские чтения – 2017: Сборник тезисов докладов XLIII Международной молодёжной научной конференции, г. Москва, 5-19 апреля 2017 г. – М.: Моск. авиационный (национальный исследовательский университет), 2017. – С. 860-861.

153 Приложение А. ОЦЕНКА ФУНКЦИОНИРОВАНИЯ ПЕШЕХОДНОЙ НАВИГАЦИОННОЙ СИСТЕМЫ OSMIUM MIMU22TР ПРИ ДВИЖЕНИИ ПО РАЗЛИЧНЫМ ПРЯМОУГОЛЬНЫМ МАРШРУТАМ 1. Постановка задачи Требуется оценить точность построения траектории движения пешехода по прямоугольному маршруту с помощью автономной навигационной системы Osmium MIMU22TР, блок инерциальных датчиков которой закреплен на обуви пешехода. Система Osmium MIMU22TР конструктивно представляет собой параллелепипед размером 13,5 мм × 23,5 мм × 31,0 мм (рисунок А.1), в котором находятся инерциальные датчики и сервисная электроника. Рисунок А.1 - Пешеходная навигационная система Osmium MIMU22TР Информация с выхода пешеходной навигационной системы (ПНС) в виде текущих значений координат ее местоположения в неподвижной системе отсчета Oxyz передается на компьютер (смартфон) по беспроводному каналу Bluetooth, также, как и команды управления передаются с компьютера на вход ПНС. Ориентация осей системы отсчета Oxyz сохраняется в памяти ПНС в момент включения ее в работу в исходной точке маршрута O при нулевых значениях координат x, y, z. Испытания ПНС выполнены при движении по прямоугольнику с возвратом в исходную точку маршрута для варианта крепления блока Osmium MIMU22TР на обуви, приведенного на рисунке А.2.

154 Рисунок А.2 - Вариант крепления блока Osmium MIMU22TР на обуви При креплении блока ПНС на левой ноге (рисунок А.2) ось Y системы координат, изображенной на блоке (рисунок А.1), направлена вперед, ось Z – вправо, ось Х – вниз. 2. Движение по периметру двора (вариант а крепления блока ПНС) На рисунке А.3 показана действительная траектория движения по периметру двора, длина пути равна 440 м. Движение выполнял человек, двигаясь со скоростью 4…5 км/час, не выдерживая паузу по завершении шага. Было выполнено три обхода по одному кругу и один троекратный обход. Результаты измерений координат в поворотных точках маршрута и по одной точке между ними приведены в таблицах А.1, А.2. Таблица А.1. Значения координат при обходах двора по одному кругу № точки 1 эксперимент X, м Y, м 1 0 0 2 -7,78 33,78 3 -36,79 93,13 4 -33,05 67,95 5 -6,53 27,01 6 28,61 -8,65 7 69,34 -39,95 8 50,7 -70,65 9 -8,61 -109,71 Z, м 0 1,7 6,01 14,03 24,48 24,4 28,14 28,76 37,35 2 эксперимент X, м Y, м Z, м 0 0 0 9,76 -27,36 -3,83 54,86 -4,36 0,15 78,18 -22,97 2,27 59,74 -69,33 12 10,33 -92,14 11,87 12,29 -61,66 10,84 0,31 -20,13 12,93 -3,54 19,22 13,56 3 эксперимент X, м Y, м 0 0 21,73 17,26 106,86 -132,77 89,73 -185,25 106,29 -225,34 80,98 -267,43 65,99 -275,23 138,73 -194,48 195,88 -199,13 Z, м 0 3,76 10,17 11,79 20,58 20,16 20,92 35,8 40,72

155 Рисунок А.3 – Действительная траектория движения по периметру двора Таблица А.2. Значения координат при троекратном обходе двора № точки 1 2 3 4 5 6 7 8 9 10 11 12 X, м 0 22,02 -60,11 -134,76 -244,4 -284,98 -329,49 -338,78 -367,55 -342,82 -295,4 -282,95 Y, м 0 23,8 -204,38 -219,66 -163,06 -184,48 -211,07 -180,69 -142,37 -123,57 -126,38 -154,44 Z, м № точки 0 13 2,59 14 -0,02 15 6,72 16 11,58 17 11,17 18 11,4 19 14,78 20 14,94 21 16,39 22 21,9 23 25,32 24 25 X, м Y, м -345,32 -140,16 -392,02 -144,68 -443,11 -153,31 -455,23 -109,17 -445,47 -99,64 -411,62 -101,15 -360,08 -122,91 -415,2 -113,39 -454,69 -117,32 -418,26 -97,83 -408,52 -78,15 -379,08 -73,86 -336,67 -98,23 Z, м 26,75 27,2 27,18 30,83 32,01 32,92 35,96 38,17 38,03 47,24 45,53 48,05 48,27 На рисунках А.4-А.7 по данным, приведенным в таблицах А.1, А.2, построены траектории движения.

156 Траектория движения по большому прямоугольному маршруту (1-й эксперимент) 80 60 40 20 y, м 0 -20 -40 -60 -80 -100 -100 -50 0 50 100 -x,м Рисунок А.4 – Траектория, построенная по результатам измерения координат при первом обходе двора Траектория движения по большому прямоугольному маршруту (2-й эксперимент) 10 0 -10 -20 y, м -30 -40 -50 -60 -70 -80 -90 -100 -80 -60 -40 -x,м -20 0 20 Рисунок А.5 – Траектория, построенная по результатам измерения координат при втором обходе двора

157 Траектория движения по большому прямоугольному маршруту (3-й эксперимент) 0 -50 y, м -100 -150 -200 -250 -250 -200 -150 -100 -x,м -50 0 50 Рисунок А.6 – Траектория, построенная по результатам измерения координат при третьем обходе двора Траектория движения по большому прямоугольному маршруту (три круга) 50 0 y, м -50 -100 -150 -200 -250 0 50 100 150 200 250 -x,м 300 350 400 450 Рисунок А.7 – Траектория, построенная по результатам измерения координат при троекратном обходе двора Как видно из рисунков, построенные схемы движения далеки от истинной траектории, что говорит о неудовлетворительной работе испытываемой ПНС Osmium MIMU 22BT при движении по улице.

158 3. Движение по коридору 3.1 Вариант (а) крепления блока ПНС В этом опыте выполнено четыре эксперимента при движении по прямоугольнику размером 35 м на 4 м. В первых трех экспериментах человек выполнил по одному обходу, двигаясь с разным темпом. В первом эксперименте (таблица А.3, рисунок А.8) человек двигался медленно, выдерживая паузу при завершении шага. Во втором эксперименте (таблица А.4, рисунок А.9) движение было нормальным, в третьем эксперименте ходьба была быстрой (таблица А.5, рисунок А.10). В четвертом эксперименте было выполнено три непрерывных обхода прямоугольного маршрута в замедленном темпе (таблица А.6, рисунок А.11). Таблица А.3. Эксперимент (офис) – 1 (один обход, медленная ходьба) № изм. 1 2 3 4 5 Реальное кол-во шагов 0 31 35 67 72 Расчетное кол-во шагов 0 10 13 22 24 Пройденная дистанция, м Рассчитанная дистанция, м V, км/ч 0 35 39 74 78 0 36,19 40,27 76,57 80,97 0 2,51 2,27 2,32 2,07 Координаты X, м Y, м Z, м 0 -26,17 -23,63 6,48 4,6 0 24,96 28,16 8,01 4,07 0 0,32 0,35 0,71 0,71 Таблица А.4. Эксперимент (офис) – 2 (один обход, нормальная ходьба) № изм. 1 2 3 4 5 Реальное кол-во шагов 0 26 30 57 61 Расчетное кол-во шагов 0 1 2 3 5 Пройденная дистанция, м Рассчитанная дистанция, м V, км/ч 0 35 39 74 78 0 36,31 40,37 76,97 81,22 0 3,73 3,16 3,38 3,14 Координаты X, м Y, м Z, м 0 0 -24,25 27,02 -21,65 30,14 6,81 7,13 4,61 3,54 0 0,27 0,33 0,63 0,68 Таблица А.5. Эксперимент (офис) – 3 (один обход, быстрая ходьба) № изм. 1 2 3 4 5 Реальное кол-во шагов 0 22 26 48 51 Расчетное кол-во шагов 0 2 3 5 7 Пройденная дистанция, м Рассчитанная дистанция, м V, км/ч 0 35 39 74 78 0 36,41 40,68 76,41 80,54 0 4,85 3,85 4,05 3,77 Координаты X, м Y, м Z, м 0 -31,7 -30,07 3,38 2,41 0 0 17,62 0,28 21,56 0,32 9,13 0,58 5,33 0,6

159 Таблица А.6. Эксперимент (офис) – 4 (три обхода, медленная ходьба) № изм. 1 2 3 4 5 6 7 8 9 10 11 12 13 Реальное кол-во шагов 0 31 35 64 69 99 104 134 138 168 172 202 205 Расчетное кол-во шагов 0 13 18 30 32 48 52 59 62 75 77 88 91 Пройденная дистанция, м 0 35 39 74 78 113 117 152 156 191 195 230 234 Рассчитанная дистанция, м V, км/ч 0 36,4 41,01 77,77 82,29 119,02 123,63 160,04 164,73 201,32 205,78 242,04 246,61 0 3,12 2,38 2,62 2,31 2,45 2,29 2,31 2,27 2,35 2,22 2,3 2,22 Координаты X, м Y, м Z, м 0 -35,35 -35,01 1,49 0,35 -35,44 -36,23 1,43 2,24 -32,99 -34,87 -3,74 -0,56 0 8,39 12,91 12,63 8,7 16,73 21,2 23,03 18,78 9,13 13,16 31,6 28,35 0 0,45 0,48 0,93 0,96 1,43 1,45 1,87 1,92 2,35 2,33 2,76 2,76 Траектория движения по большому прямоугольному маршруту (1-й эксперимент) 25 y, м 20 15 10 5 0 -5 0 5 10 -x,м 15 20 25 Рисунок А.8 – Траектория движения по прямоугольнику 35м × 4м при медленной ходьбе

160 Траектория движения по прямоугольному маршруту (2-й эксперимент) 30 25 y, м 20 15 10 5 0 -10 -5 0 5 10 -x,м 15 20 25 Рисунок А.9 – Траектория движения по прямоугольнику 35м × 4м при нормальной ходьбе Траектория движения по прямоугольному маршруту (3-й эксперимент) 20 y, м 15 10 5 0 0 5 10 15 -x,м 20 25 30 Рисунок А.10 – Траектория движения по прямоугольнику 35м × 4м при быстрой ходьбе

161 Траектория движения по прямоугольному маршруту (три круга) 30 25 y, м 20 15 10 5 0 0 5 10 15 20 25 30 35 -x,м Рисунок А.11 – Траектория троекратного обхода прямоугольника 35м × 4м при медленной ходьбе Как следует из таблиц А.3 – А.5 и рисунков А.8 – А.10, построенные траектории при однократном обходе близки по размерам и пройденной дистанции действительному маршруту, но последняя точка пути не совпала с исходной, координаты которой приняты за ноль. Ошибка этого несовпадения составила приблизительно шесть метров, что превышает размер малой стороны прямоугольника. В четвертом эксперименте (таблица А.6, рисунок А.11) кроме этой ошибки траектория при очередном обходе заметно смещается относительно предыдущей. Также можно отметить в качестве недостатка неопределенность привязки исходной системы координат к плану здания. Результаты оценки функционирования пешеходной навигационной системы Osmium MIMU22TР при движении по различным прямоугольным маршрутам приведены в таблице А.7.

162 Таблица А.7. Результаты оценки функционирования пешеходной навигационной системы Osmium MIMU22TР Вариант 1-й обход по квадрату 110×4 2-й обход по квадрату 110×4 3-й обход по квадрату 110×4 Трехкратный обход по квадрату 110×4 Один обход по офису, медленно Один обход по офису, нормально Один обход по офису, быстро Три обхода по офису, медленно Погрешность определения пройденного расстояния (σ) м % Расстояние между точкой старта и вычисленной точкой финиша (σ) м % Длина линии пути, м Вычисленный путь, м 440 383,3 -56,7 -12,9 110 25 440 327,1 -112,9 -25,7 19,5 4,4 440 530,9 90,9 20,7 279,3 63,5 1320 1310,7 -9,3 -0,7 350,7 26,6 78 80,8 2,8 3,65 6,1 7,9 78 81,2 3,2 4,07 5,8 7,5 78 80,1 2,1 2,74 5,85 7,5 234 246,4 12,4 5,3 28,4 12,1

163 Приложение Б. КИНЕМАТИЧЕСКИЕ СХЕМЫ И СИСТЕМЫ КООРДИНАТ Расположение систем координат для случая, когда опорная поверхность горизонтальна, показано на рисунке Б.1. При наличии уклона опорной поверхности относительно плоскости горизонта введены дополнительные системы координат, которые показаны на рисунке Б.2. Ki Yc1 Xdi Yc γ ϑ σi Ydi ДМi Yg1 Odi Zg1 ψ Zc1 Oc γ ψ Xg1 Zdi νi Xc Zc µi Xc1 ϑ Дуга радиуса rdi на плоскости XcOcZc Y0, Yg Zg yg Ψ Восток Z0 O0 Ψ Xg X0 Cевер xg zg Рисунок Б.1 - Кинематическая схема определения дальности до опорной поверхности i – тым ДМ (опорная поверхность горизонтальна) Описание систем координат, показанных на рисунке Б.1: - O0X0Y0Z0 – местная географическая система координат. Полюс O0 расположен в начальной точке маршрута на горизонтальной поверхности (плоскость X0Z0), по которой движется ПО. Ось X0 направлена на Север, ось Z0 – на Восток, ось Y0 – по вертикали места вверх.

164 Yоп Yg2 λx Zоп Zg2 Хоп λz Ki Плоскость опорной поверхности Xg2 Горизонтальная плоскость Xdi ДМi Yоп1 Y0, Yg h0 Odi Zg Oc Zоп1 O0 Xоп1 Xg yg xg zg Рисунок Б.2 - Кинематическая схема измерения дальности до опорной поверхности i-тым ДМ (опорная поверхность имеет уклон относительно плоскости горизонта) - O0XgYgZg – нормальная земная система координат, развернутая в азимуте относительно географической системы на угол Ψ, который может быть определен с помощью магнитометров, входящих в состав блока ДПИ. В системе координат O0XgYgZg строится траектория движения блока ДПИ. - OcXg1Yg1Zg1 – нормальная система координат, оси которой параллельны одноименным осям нормальной земной системы координат, а полюс Oc связан с блоком ДПИ. Положение полюса Oc в системе координат O0XgYgZg определяется радиусом-вектором R O O = [ xg 0 c yg z g ]т . - OcXcYcZc – связанная с корпусом блока ДПИ система координат. Начало координат Ос находится в центре масс блока ДПИ, а по осям Xc, Yc, Zc выставляются оси чувствительности инерциальных датчиков. При определении углов ориентации системы ОсXсYсZс относительно нормальной системы ОсXg1Yg1Zg1 примем

165 следующую схему поворотов: Oс X g1Yg1Z g1 (231) M g1,c ψ, ϑ, γ Oc X cYc Z c . Углы поворотов назовем соответственно – угол рысканья (ψ), угол тангажа (ϑ), угол крена (γ). Угол рысканья – угол между проекцией оси Xс на горизонтальную плоскость и осью Xg1; угол тангажа – угол между осью Xс и горизонтальной плоскостью; угол крена – угол между плоскостью XсОсYс и вертикальной плоскостью, проходящей через ось Xс. - OdiXс1Yс1Zс1 – вспомогательная система координат, оси которой параллельны одноименным осям связанной системы координат. Положение полюса Odi в системе координат ОсXcYcZc определяется радиусом-вектором (рисунок Б.3): = R OcOdi xdi ydi zdi ] [ rdi cosν i [= т Zc1 Xc Xc1 νi ydi − rdi sinν i ] . т (Б.1) Zc Oc zdi Odi xdi Рисунок Б.3 – Схема расположения i – того дальномера относительно связанной системы координат OcXcYcZc - OdiXdiYdiZdi – система координат, связанная с i – тым дальномером ДМi. Полюс Odi находится в точке установки ДМi на блоке ДПИ, которую примем за начало отсчета измеряемой дальности Li до опорной горизонтальной поверхности. Ось Xdi направлена по измерительной оси ДМi и составляет с плоскостью Xc1OdiZc1 угол σi. Плоскость XdiOdiYdi составляет с плоскостью Xc1OdiYc1 двугранный угол µi. Описание дополнительных систем координат, приведенных на рисунке Б.2: * - O0 X g 2Yg 2 Z g 2 – система координат, оси которой параллельны осям нормальной системы координат, а полюс O0* расположен на опорной поверхности.

166 * - O0 X опYоп Z оп – система координат, связанная с опорной поверхностью. В общем случае плоскость опорной поверхности имеет уклон относительно горизонтальной плоскости, который зададим двумя последовательными * поворотами системы координат O0 X опYоп Z оп относительно системы координат (31) M O0* X g 2Yg 2 Z g 2 : O0* X g 2Yg 2 Z g 2 ←  O0* X опYоп Z оп . Полюс O0* поместим на опорной λ ,λ g 2,оп z x поверхности над полюсом О0 земной системы координат, в которой положение полюса O0* определяется радиусом-вектором R O O* = [ 0 h0 0 0 0] , где h0 – т расстояние от пола до потолка, измеренное по вертикали. - Oс X оп1Yоп1Z оп1 – система координат, оси которой параллельны осям опорной системы координат, а полюс связан с блоком ДПИ. Системы координат, определяющие положение элемента ПО в пространстве и положение блока ДПИ относительно элемента ПО: - O1 X g′ Yg′Z g′ – нормальная система координат, оси которой параллельны одноименным осям системы O0 X gYg Z g , а полюс помещен в точку О1, принятую за центр элемента ПО, к которому крепится блок ДПИ. Положение точки О1 в земной системе координат определяется радиусом-вектором R O0O1 ; - O1 X 1Y1Z1 – система координат, связанная с элементом ПО, к которому крепится блок ДПИ. Относительно нормальной системы эта система координат занимает угловое положение, определяемое углами ψ1 (рыскание элемента), ϑ1 (тангаж элемента), γ1(крен элемента), которые в процессе моделирования движения ПО должны быть заданы соответствующими функциями времени; - Oс X 1′Y1′Z1′ – система координат, оси которой параллельны одноименным осям системы O1 X 1Y1Z1 , а полюс помещен в центр блока ДПИ – точку Ос. Положение точки Ос в системе координат O1 X 1Y1Z1 R O1Oc = [ xОc yОc определяется радиусом-вектором zОc ]т . Угловое положение блока ДПИ относительно элемента ПО

167 определяется углами δx, δy, δz поворота связанной системы относительно системы Oс X 1′Y1′Z1′ . Связь между введенными системами координат изобразим в виде графической схемы преобразования однородных координат в трехмерном пространстве (рисунок Б.4). Матрицы вращения переноса M 0, g , M g1,c , M c1, di , M g 2,оп , M оп, g 2 , M оп1,c , M1',c , M g ',ПО M g , g1 , M c , c1 , M g , g 2 , M g 2, g , M оп, оп1 , M ПО,1' , M g , g ' и имеют размер 4×4 и выражаются формулами:  cos ϕ 0 sin ϕ  C0, g G 0т   0 G0 = 1 0 ; М 0, g =   ; C0, g =  1 G  0   − sin ϕ 0 cos ϕ E M g , g1 =  G 0 М g1,c C =  g1,c  G0 R O0Oc  ; 1  1 0 0  E = 0 1 0  ;   0 0 1  G 0т  ; 1 [0 0 0]; (Б.2) (Б.3) (Б.4)   cos ψ 0 sin ψ   ; C g1,c = C ⋅ C ⋅ C = ( cij ) ; C = 0 1 0  3×3     − sin ψ 0 cos ψ   0 0  cos ϑ − sin ϑ 0  1   (Б.5) ( 3) (1)     C g1,c= sin ϑ cos ϑ 0 ; C g1,c= 0 cos γ − sin γ ;        0 0 sin γ cos γ  0 1    cos ϑ cos ψ sin γ sin ψ − cos γ cos ψ sin ϑ cos γ sin ψ + cos ψ sin γ sin ϑ   − cos ϑ sin γ C g1,c =  sin ϑ cos γ cos ϑ    − cos ϑ sin ψ cos ψ sin γ + cos γ sin ψ sin ϑ cos γ cos ψ − sin γ sin ψ sin ϑ   ( 2) g 1,c ( 3) g 1,c ( 2) g 1,c (1) g 1,c E M с , c1 =  G 0 M c1, di C =  c1,di  G0 R OcOdi  ; 1  (Б.6) G 0т  ; 1  (Б.7)

168 ( 2) c1,di ( 3) c1,di Cc1,di = С ⋅С ; С(c31,)di cos σi  sin σ = i   0 − sin σi cos σi 0  cos µi 0 sin µi   0 С = 1 0 ;    − sin µi 0 cos µi  0  cos σi cos µi − cos µi sin σi  0  ; Cc1,di = sin σi cos σi    − cos σi sin µi sin µi sin σi 1  ( 2) c1,di  E R O0O0*  M g , g 2 = =  , R O0O0* 1  G 0 М g 2,оп C g 2,оп G 0т  =  , G 1  0  Cоп, g 2 G 0т  М оп, g 2 = , 1 G  0  C g 2,оп cos λ z =  sin λ z   0 − sin λ z cos λ x sin λ z cos λ x cos λ z − sin λ x cos λ z  E −R O0O0*  = , 1  G 0  E R O0*С  M оп,оп1 =  , 1  G 0 0 с11оп1,с =cos ϑоп cos ψ оп , с12оп1,с = sin γ оп sin ψ оп − cos γ оп cos ψ оп sin ϑоп , с13оп1,с =cos γ оп sin ψ оп + cos ψ оп sin γ оп sin ϑоп , оп1,с с21 = sin ϑоп , оп1,с с22 =cos γ оп cos ϑоп , оп1,с с23 = − cos ϑоп sin γ оп , оп1,с с31 = − cos ϑоп sin ψ оп , оп1,с с32 = cos ψ оп sin γ оп + cos γ оп sin ψ оп sin ϑоп , оп1,с с33 = cos γ оп cos ψ оп − sin γ оп sin ψ оп sin ϑоп . 0 0 (c ) оп1,с ij 3×3  0] ,   (Б.9) 0  sin λ x  ,  cos λ x  (Б.10) (Б.11) (Б.12)    R= R − R , O0C O*С O O* Cоп1,с G 0т  = M оп1,c = Cоп1,с , G 1  0  h0 (Б.8) sin λ z sin λ x  − cos λ z sin λ x  ,   cos λ x cos λ z cos λ x sin λ x cos λ z   sin λ cos λ Cоп, g 2 =− z x   sin λ x sin λ z M g 2, g [0      sin µi   0  ,  cos µi   , (Б.13) (Б.14)

169 M g, g ' E = G 0 R O0O1  ; 1  1 0 0  E = 0 1 0  ;   0 0 1  М g ',ПО C =  g ',ПО  G0 R O0O1 G 0т  ; 1  x1g    =  y1g  ;  z1g    G 0 = [ 0 0 0]; (Б.15) C g ',ПО = ( uij ) × ; 3 3 (Б.16) u11 = cos ϑ1 cos ψ1; u12= sin γ1 sin ψ1 − cos γ1 cos ψ1 sin ϑ1; u13 = cos γ1 sin ψ1 + cos ψ1 sin γ1 sin ϑ1; sin ϑ1; u= 21 u22 =cos γ1 cos ϑ1; u23 = − cos ϑ1 sin γ1; u31 = − cos ϑ1 sin ψ1; u32= cos ψ1 sin γ1 + cos γ1 sin ψ1 sin ϑ1; u33= cos γ1 cos ψ1 − sin γ1 sin ψ1 sin ϑ1; E M ПО,1' =  G 0 M1',c C =  1',c  G0 p11 =cos δ z cos δ y ; p12= sin δ x sin δ y − cos δ x cos δ y sin δ z ; p13 = cos δ x sin δ y + cos δ y sin δ x sin δ z ; sin δ z ; p= 21 p22 =cos δ x cos δ z ; p23 = − cos δ z sin δ x ; p31 = − cos δ z sin δ y ; p32 =cos δ y sin δ x + cos δ x sin δ y sin δ z ; p33= cos δ x cos δ y − sin δ x sin δ y sin δ z . R O1Oc  ; 1  R O1Oc  xОc    =  yОc  ;    zОc  (Б.17) G 0т  ; 1  C1',c = ( pij )3×3 ; (Б.18)

Рисунок Б.4 – Кинематическая схема связи между системами координат (стрелками показаны направления преобразования координат)

Приложение В. ПРОГРАММА РАСЧЕТА ЧУВСТВИТЕЛЬНОСТИ ВЫЧИСЛЕНИЯ КОЭФФИЦИЕНТОВ ПОГРЕШНОСТЕЙ ОПОРНЫХ УГЛОВ КРЕНА И ТАНГАЖА Программа расчета коэффициентов чувствительности погрешностей вычисления опорных углов крена и тангажа дальномерной СУО с тремя ДМ к инструментальным погрешностям параметров установки ДМ на платформе блока ДПИ и определение границ областей погрешностей вычисления опорных углов крена и тангажа при изменении углов крена и тангажа в пределах (-30...+30) о. Листинг программы: clear close all % Исходные данные: mu=[0 2*pi/3 4*pi/3];% радиан, азимутальные углы ориентации ДМ на блоке ДПИ nu=[0 2*pi/3 4*pi/3];% радиан, азимутальные углы ориентации векторов rd yd=0.01;% м, ордината полюсов Od1,Od2,Od3 в связанной СК rd=0.01;% м, величина вектора rd (радиус окружности расположения полюсов % Od1,Od2,Od3 sigma=60*pi/180;% радиан, угол наклона осей ДМ к плоскости XcZc связанной СК h0=3;% м, расстояние от ОПН до ОПВ (от пола до потолка) yg=1.8;% м, расстояние от ОПН до полюса Ос связанной СК %Заготовки массивов: yg_Od_rd=zeros(1,3); yg_Od_yd=zeros(1,3); yg_Od_nu=zeros(1,3); c11_mu=zeros(1,3); c21_mu=zeros(1,3); c31_mu=zeros(1,3); c11_sig=zeros(1,3); c21_sig=zeros(1,3); c31_sig=zeros(1,3); yg_Od=zeros(1,3); c11=zeros(1,3); c21=zeros(1,3);

172 c31=zeros(1,3); L_yg_Od=zeros(1,3); L_c11=zeros(1,3); L_c21=zeros(1,3); L_c31=zeros(1,3); L_rd=zeros(1,3); L_yd=zeros(1,3); L_nu=zeros(1,3); L_mu=zeros(1,3); L_sig=zeros(1,3); L=zeros(1,3); g_op_rd=zeros(1,3); g_op_yd=zeros(1,3); g_op_nu=zeros(1,3); g_op_mu=zeros(1,3); g_op_sig=zeros(1,3); t_op_rd=zeros(1,3); t_op_yd=zeros(1,3); t_op_nu=zeros(1,3); t_op_mu=zeros(1,3); t_op_sig=zeros(1,3); err_g_rd=zeros(21,8); err_g_yd=zeros(21,8); err_g_nu=zeros(21,8); err_g_mu=zeros(21,8); err_g_sig=zeros(21,8); err_t_rd=zeros(21,8); err_t_yd=zeros(21,8); err_t_nu=zeros(21,8); err_t_mu=zeros(21,8); err_t_sig=zeros(21,8); dg_rd_min=zeros(1,7); dg_rd_max=zeros(1,7); dg_yd_min=zeros(1,7); dg_yd_max=zeros(1,7); dg_nu_min=zeros(1,7); dg_nu_max=zeros(1,7); dg_mu_min=zeros(1,7);

173 dg_mu_max=zeros(1,7); dg_sig_min=zeros(1,7); dg_sig_max=zeros(1,7); dt_rd_min=zeros(1,7); dt_rd_max=zeros(1,7); dt_yd_min=zeros(1,7); dt_yd_max=zeros(1,7); dt_nu_min=zeros(1,7); dt_nu_max=zeros(1,7); dt_mu_min=zeros(1,7); dt_mu_max=zeros(1,7); dt_sig_min=zeros(1,7); dt_sig_max=zeros(1,7); %Диапазон изменения углов ориентации блока ДПИ: Up=[0 45 90]*pi/180;% радиан, угол рысканья n1=length(Up); Ut=[-30 -20 -10 0 10 20 30]*pi/180;% радиан, угол тангажа n2=length(Ut); Ug=[-30 -20 -10 0 10 20 30]*pi/180;% радиан, угол крена n3=length(Ug); % Заготовки таблиц: nt=n2*n3; tab_g=zeros(nt,18); tab_t=zeros(nt,18); tab_L=zeros(nt,6); it=0; psi=Up(1); for j=1:n2 theta=Ut(j); for k=1:n3 it=it+1; gamma=Ug(k); for m=1:3 yg_Od_rd(m)=cos(nu(m))*sin(theta)+sin(nu(m))*cos(theta)*sin(gamma); yg_Od_yd(m)=cos(gamma)*cos(theta); yg_Od_nu(m)=rd*(cos(nu(m))*cos(theta)*sin(gamma)-sin(theta)*sin(nu(m))); c11_mu(m)=-cos(sigma)*(cos(psi)*cos(theta)*sin(mu(m))+... (cos(gamma)*sin(psi)+cos(psi)*sin(gamma)*sin(theta))*cos(mu(m)));

174 c21_mu(m)=cos(sigma)*(cos(theta)*sin(gamma)*cos(mu(m))-... sin(theta)*sin(mu(m))); c31_mu(m)=cos(sigma)*(sin(mu(m))*cos(theta)*sin(psi)-... cos(mu(m))*(cos(gamma)*cos(psi)-sin(gamma)*sin(psi)*sin(theta))); c11_sig(m)=cos(sigma)*(sin(gamma)*sin(psi)-cos(gamma)*cos(psi)*sin(theta))-... cos(psi)*sin(sigma)*cos(mu(m))*cos(theta)+... sin(sigma)*sin(mu(m))*(cos(gamma)*sin(psi)+cos(psi)*sin(gamma)*sin(theta)); c21_sig(m)=cos(gamma)*cos(theta)*cos(sigma)-(sin(theta)*cos(mu(m))+... cos(theta)*sin(gamma)*sin(mu(m)))*sin(sigma); c31_sig(m)=cos(sigma)*(cos(psi)*sin(gamma)+cos(gamma)*sin(psi)*sin(theta))+... sin(sigma)*(cos(mu(m))*cos(theta)*sin(psi)+... sin(mu(m))*(cos(gamma)*cos(psi)-sin(gamma)*sin(psi)*sin(theta))); yg_Od(m)=yg+rd*cos(nu(m))*sin(theta)+yd*cos(gamma)*cos(theta)+... rd*sin(nu(m))*cos(theta)*sin(gamma); c11(m)=sin(sigma)*(sin(gamma)*sin(psi)-cos(gamma)*cos(psi)*sin(theta))+... cos(sigma)*(cos(psi)*cos(mu(m))*cos(theta)-... sin(mu(m))*(cos(gamma)*sin(psi)+cos(psi)*sin(gamma)*sin(theta))); c21(m)=(sin(theta)*cos(mu(m))+cos(theta)*sin(gamma)*sin(mu(m)))*cos(sigma)+... cos(gamma)*cos(theta)*sin(sigma); c31(m)=sin(sigma)*(cos(psi)*sin(gamma)+cos(gamma)*sin(psi)*sin(theta))-... cos(sigma)*(cos(mu(m))*cos(theta)*sin(psi)+... sin(mu(m))*(cos(gamma)*cos(psi)-sin(gamma)*sin(psi)*sin(theta))); L(m)=(h0-yg_Od(m))*sqrt(c11(m)^2+c21(m)^2+c31(m)^2)/c21(m); L_yg_Od(m)=-sqrt(c11(m)^2+c21(m)^2+c31(m)^2)/c21(m); L_c11(m)=c11(m)*(h0-yg_Od(m))/(c21(m)*sqrt(c11(m)^2+c21(m)^2+c31(m)^2)); L_c21(m)=(h0-yg_Od(m))/sqrt(c11(m)^2+c21(m)^2+c31(m)^2)-... (h0-yg_Od(m))*sqrt(c11(m)^2+c21(m)^2+c31(m)^2)/c21(m)^2; L_c31(m)=c31(m)*(h0-yg_Od(m))/(c21(m)*sqrt(c11(m)^2+c21(m)^2+c31(m)^2)); L_rd(m)=L_yg_Od(m)*yg_Od_rd(m); L_yd(m)=L_yg_Od(m)*yg_Od_yd(m); L_nu(m)=L_yg_Od(m)*yg_Od_nu(m); L_mu(m)=L_c11(m)*c11_mu(m)+L_c21(m)*c21_mu(m)+L_c31(m)*c31_mu(m); L_sig(m)=L_c11(m)*c11_sig(m)+L_c21(m)*c21_sig(m)+L_c31(m)*c31_sig(m); end %-----------------------------------------------------------------------Ag_L(1)=0.5*3^0.5*sin(2*sigma)*(L(3)-L(2)); Ag_L(2)=-3^0.5*sin(sigma)*(L(1)*cos(sigma)+rd); Ag_L(3)=3^0.5*sin(sigma)*(L(1)*cos(sigma)+rd);

175 Bg_L(1)=(L(2)+L(3))*(cos(sigma))^2+2*rd*cos(sigma); Bg_L(2)=(L(1)+L(3))*(cos(sigma))^2+2*rd*cos(sigma); Bg_L(3)=(L(1)+L(2))*(cos(sigma))^2+2*rd*cos(sigma); Ag=3^0.5*(L(3)-L(2))*(L(1)*cos(sigma)+rd)*sin(sigma); Bg=(L(1)*L(2)+L(1)*L(3)+L(2)*L(3))*(cos(sigma))^2+... 2*rd*(L(1)+L(2)+L(3))*cos(sigma)+3*rd^2; Qt=Bg; Ft=(L(3)-L(2))*(L(1)*cos(sigma)+rd)*sin(sigma); V=Qt^2+3*Ft^2; At=sin(sigma)*((2*L(2)*L(3)-L(1)*(L(2)+L(3)))*cos(sigma)-... (2*L(1)-L(3)-L(2))*rd); Bt=sqrt(V); At_L(1)=-sin(sigma)*(cos(sigma)*(L(2)+L(3))+2*rd); At_L(2)=sin(sigma)*((2*L(3)-L(1))*cos(sigma)+rd); At_L(3)=sin(sigma)*((2*L(2)-L(1))*cos(sigma)+rd); Bt_L(1)=(2*Qt*((L(2)+L(3))*(cos(sigma))^2+2*rd*cos(sigma))+... 3*Ft*(L(3)-L(2))*sin(2*sigma))/(2*Bt); Bt_L(2)=(2*Qt*((L(1)+L(3))*(cos(sigma))^2+2*rd*cos(sigma))-... 6*Ft*(L(1)*cos(sigma)+rd)*sin(sigma))/(2*Bt); Bt_L(3)=(2*Qt*((L(1)+L(2))*(cos(sigma))^2+2*rd*cos(sigma))+... 6*Ft*(L(1)*cos(sigma)+rd)*sin(sigma))/(2*Bt); for m=1:3 % Коэффициенты g_op_rd,g_op_yd,t_op_rd,t_op_yd делим на 1000 % и умножаем на 180/pi для перевода в размерность градус/мм g_op_rd(m)=(Ag_L(m)*Bg-Bg_L(m)*Ag)*L_rd(m)/(Ag^2+Bg^2)*180/pi/1000; g_op_yd(m)=(Ag_L(m)*Bg-Bg_L(m)*Ag)*L_yd(m)/(Ag^2+Bg^2)*180/pi/1000; g_op_nu(m)=(Ag_L(m)*Bg-Bg_L(m)*Ag)*L_nu(m)/(Ag^2+Bg^2); g_op_mu(m)=(Ag_L(m)*Bg-Bg_L(m)*Ag)*L_mu(m)/(Ag^2+Bg^2); g_op_sig(m)=(Ag_L(m)*Bg-Bg_L(m)*Ag)*L_sig(m)/(Ag^2+Bg^2); t_op_rd(m)=(At_L(m)*Bt-Bt_L(m)*At)*L_rd(m)/(At^2+Bt^2)*180/pi/1000; t_op_yd(m)=(At_L(m)*Bt-Bt_L(m)*At)*L_yd(m)/(At^2+Bt^2)*180/pi/1000; t_op_nu(m)=(At_L(m)*Bt-Bt_L(m)*At)*L_nu(m)/(At^2+Bt^2); t_op_mu(m)=(At_L(m)*Bt-Bt_L(m)*At)*L_mu(m)/(At^2+Bt^2); t_op_sig(m)=(At_L(m)*Bt-Bt_L(m)*At)*L_sig(m)/(At^2+Bt^2); end

176 % Таблица коэффициентов чувствительности погрешности вычисления опорного % угла крена tab_g(it,1:18)=[psi*180/pi,theta*180/pi,gamma*180/pi,g_op_rd(1),... g_op_rd(2),g_op_rd(3),g_op_yd(1),g_op_yd(2),g_op_yd(3),g_op_nu(1),... g_op_nu(2),g_op_nu(3),g_op_mu(1),g_op_mu(2),g_op_mu(3),... g_op_sig(1),g_op_sig(2),g_op_sig(3)]; % Таблица коэффициентов чувствительности погрешности вычисления опорного % угла тангажа tab_t(it,1:18)=[psi*180/pi,theta*180/pi,gamma*180/pi,t_op_rd(1),... t_op_rd(2),t_op_rd(3),t_op_yd(1),t_op_yd(2),t_op_yd(3),t_op_nu(1),... t_op_nu(2),t_op_nu(3),t_op_mu(1),t_op_mu(2),t_op_mu(3),... t_op_sig(1),t_op_sig(2),t_op_sig(3)]; % Таблица значений дальностей tab_L(it,1:6)=[psi*180/pi,theta*180/pi,gamma*180/pi,L(1),L(2),L(3)]; end end %-------------------------------------------------------------------------------% Определение границ областей погрешностей вычисления опорных углов крена и тангажа при изменении угла крена от -30 градусов до +30 градусов при значениях угла тангажа -30 градусов, 0, +30 градусов по группам конструктивных параметров: 1-я группа-радиусы rd(1),rd(2),rd(3) при отклонении их от номинала на величину плюс-минус 1 мм; 2-я группа-координаты yd(1),yd(2),yd(3) при отклонении их от номинала на величину плюс-минус 1 мм; 3-я группа-углы nu(1),nu(2),nu(3) при отклонении их от номинала на величину плюс-минус 1 градус; 4-я группа-углы mu(1),mu(2),mu(3) при отклонении их от номинала на величину плюс-минус 1 градус; 5-я группа-углы sig(1),sig(2),sig(3) при отклонении их от номинала на величину плюс-минус 1 градус. %-------------------------------------------------------------------------------% Матрица инструментальных погрешностей для всех конструктивных параметров: x=[-1 -1 -1 1 -1 -1 -1 1 -1 1 1 -1 -1 -1 1 1 -1 1 -1 1 1 1 1 1];

177 % Угол тангажа минус 30 градусов for i=1:7 for j=1:8 err_g_rd(i,j)=tab_g(i,4)*x(j,1)+tab_g(i,5)*x(j,2)+tab_g(i,6)*x(j,3); err_g_yd(i,j)=tab_g(i,7)*x(j,1)+tab_g(i,8)*x(j,2)+tab_g(i,9)*x(j,3); err_g_nu(i,j)=tab_g(i,10)*x(j,1)+tab_g(i,11)*x(j,2)+tab_g(i,12)*x(j,3); err_g_mu(i,j)=tab_g(i,13)*x(j,1)+tab_g(i,14)*x(j,2)+tab_g(i,15)*x(j,3); err_g_sig(i,j)=tab_g(i,16)*x(j,1)+tab_g(i,17)*x(j,2)+tab_g(i,18)*x(j,3); err_t_rd(i,j)=tab_t(i,4)*x(j,1)+tab_t(i,5)*x(j,2)+tab_t(i,6)*x(j,3); err_t_yd(i,j)=tab_t(i,7)*x(j,1)+tab_t(i,8)*x(j,2)+tab_t(i,9)*x(j,3); err_t_nu(i,j)=tab_t(i,10)*x(j,1)+tab_t(i,11)*x(j,2)+tab_t(i,12)*x(j,3); err_t_mu(i,j)=tab_t(i,13)*x(j,1)+tab_t(i,14)*x(j,2)+tab_t(i,15)*x(j,3); err_t_sig(i,j)=tab_t(i,16)*x(j,1)+tab_t(i,17)*x(j,2)+tab_t(i,18)*x(j,3); end dg_rd_min(i)=min(err_g_rd(i,:)); dg_rd_max(i)=max(err_g_rd(i,:)); dg_yd_min(i)=min(err_g_yd(i,:)); dg_yd_max(i)=max(err_g_yd(i,:)); dg_nu_min(i)=min(err_g_nu(i,:)); dg_nu_max(i)=max(err_g_nu(i,:)); dg_mu_min(i)=min(err_g_mu(i,:)); dg_mu_max(i)=max(err_g_mu(i,:)); dg_sig_min(i)=min(err_g_sig(i,:)); dg_sig_max(i)=max(err_g_sig(i,:)); dt_rd_min(i)=min(err_t_rd(i,:)); dt_rd_max(i)=max(err_t_rd(i,:)); dt_yd_min(i)=min(err_t_yd(i,:)); dt_yd_max(i)=max(err_t_yd(i,:)); dt_nu_min(i)=min(err_t_nu(i,:)); dt_nu_max(i)=max(err_t_nu(i,:)); dt_mu_min(i)=min(err_t_mu(i,:)); dt_mu_max(i)=max(err_t_mu(i,:)); dt_sig_min(i)=min(err_t_sig(i,:)); dt_sig_max(i)=max(err_t_sig(i,:)); end dg_sum_min=dg_rd_min+dg_yd_min+dg_nu_min+dg_mu_min+dg_sig_min; dg_sum_max=dg_rd_max+dg_yd_max+dg_nu_max+dg_mu_max+dg_sig_max;

178 dt_sum_min=dt_rd_min+dt_yd_min+dt_nu_min+dt_mu_min+dt_sig_min; dt_sum_max=dt_rd_max+dt_yd_max+dt_nu_max+dt_mu_max+dt_sig_max; figure(40) plot(tab_g(1:7,3),dg_sum_min,'-o',tab_g(1:7,3),dg_sum_max,'-s','LineWidth',2);grid xlabel('\gamma,^o') ylabel('\Delta\gamma_о_п^o') legend('\Delta\gamma_о_п^m^i^n','\Delta\gamma_о_п^m^a^x') title('Область возможных погрешностей вычисления \gamma_о_п; \vartheta=30^o') figure(50) plot(tab_t(1:7,3),dt_sum_min,'-o',tab_t(1:7,3),dt_sum_max,'-s','LineWidth',2);grid xlabel('\gamma,^o') ylabel('\Delta\vartheta_о_п^o') legend('\Delta\vartheta_о_п^m^i^n','\Delta\vartheta_о_п^m^a^x') title('Область возможных погрешностей вычисления \vartheta_о_п; \vartheta=30^o') %-----------------------------------------------------------------% Угол тангажа 0 градусов for i=22:28 k=i-14; n=i-21; for j=1:8 err_g_rd(k,j)=tab_g(i,4)*x(j,1)+tab_g(i,5)*x(j,2)+tab_g(i,6)*x(j,3); err_g_yd(k,j)=tab_g(i,7)*x(j,1)+tab_g(i,8)*x(j,2)+tab_g(i,9)*x(j,3); err_g_nu(k,j)=tab_g(i,10)*x(j,1)+tab_g(i,11)*x(j,2)+tab_g(i,12)*x(j,3); err_g_mu(k,j)=tab_g(i,13)*x(j,1)+tab_g(i,14)*x(j,2)+tab_g(i,15)*x(j,3); err_g_sig(k,j)=tab_g(i,16)*x(j,1)+tab_g(i,17)*x(j,2)+tab_g(i,18)*x(j,3); err_t_rd(k,j)=tab_t(i,4)*x(j,1)+tab_t(i,5)*x(j,2)+tab_t(i,6)*x(j,3); err_t_yd(k,j)=tab_t(i,7)*x(j,1)+tab_t(i,8)*x(j,2)+tab_t(i,9)*x(j,3); err_t_nu(k,j)=tab_t(i,10)*x(j,1)+tab_t(i,11)*x(j,2)+tab_t(i,12)*x(j,3); err_t_mu(k,j)=tab_t(i,13)*x(j,1)+tab_t(i,14)*x(j,2)+tab_t(i,15)*x(j,3); err_t_sig(k,j)=tab_t(i,16)*x(j,1)+tab_t(i,17)*x(j,2)+tab_t(i,18)*x(j,3); end dg_rd_min(n)=min(err_g_rd(k,:)); dg_rd_max(n)=max(err_g_rd(k,:)); dg_yd_min(n)=min(err_g_yd(k,:)); dg_yd_max(n)=max(err_g_yd(k,:)); dg_nu_min(n)=min(err_g_nu(k,:));

179 dg_nu_max(n)=max(err_g_nu(k,:)); dg_mu_min(n)=min(err_g_mu(k,:)); dg_mu_max(n)=max(err_g_mu(k,:)); dg_sig_min(n)=min(err_g_sig(k,:)); dg_sig_max(n)=max(err_g_sig(k,:)); dt_rd_min(n)=min(err_t_rd(k,:)); dt_rd_max(n)=max(err_t_rd(k,:)); dt_yd_min(n)=min(err_t_yd(k,:)); dt_yd_max(n)=max(err_t_yd(k,:)); dt_nu_min(n)=min(err_t_nu(k,:)); dt_nu_max(n)=max(err_t_nu(k,:)); dt_mu_min(n)=min(err_t_mu(k,:)); dt_mu_max(n)=max(err_t_mu(k,:)); dt_sig_min(n)=min(err_t_sig(k,:)); dt_sig_max(n)=max(err_t_sig(k,:)); end dg_sum_min=dg_rd_min+dg_yd_min+dg_nu_min+dg_mu_min+dg_sig_min; dg_sum_max=dg_rd_max+dg_yd_max+dg_nu_max+dg_mu_max+dg_sig_max; dt_sum_min=dt_rd_min+dt_yd_min+dt_nu_min+dt_mu_min+dt_sig_min; dt_sum_max=dt_rd_max+dt_yd_max+dt_nu_max+dt_mu_max+dt_sig_max; figure(60) plot(tab_g(22:28,3),dg_sum_min,'-o',tab_g(22:28,3),dg_sum_max,'-s','LineWidth',2); grid xlabel('\gamma,^o') ylabel('\Delta\gamma_о_п^o') legend('\Delta\gamma_о_п^m^i^n','\Delta\gamma_о_п^m^a^x') title('Область возможных погрешностей вычисления \gamma_о_п; \vartheta=0^o') figure(70) plot(tab_t(22:28,3),dt_sum_min,'-o',tab_t(22:28,3),dt_sum_max,'-s','LineWidth',2);grid xlabel('\gamma,^o') ylabel('\Delta\vartheta_о_п^o') legend('\Delta\vartheta_о_п^m^i^n','\Delta\vartheta_о_п^m^a^x') title('Область возможных погрешностей вычисления \vartheta_о_п; \vartheta=0^o') % Угол тангажа плюс 30 градусов for i=43:49 k=i-28; n=i-42;

180 for j=1:8 err_g_rd(k,j)=tab_g(i,4)*x(j,1)+tab_g(i,5)*x(j,2)+tab_g(i,6)*x(j,3); err_g_yd(k,j)=tab_g(i,7)*x(j,1)+tab_g(i,8)*x(j,2)+tab_g(i,9)*x(j,3); err_g_nu(k,j)=tab_g(i,10)*x(j,1)+tab_g(i,11)*x(j,2)+tab_g(i,12)*x(j,3); err_g_mu(k,j)=tab_g(i,13)*x(j,1)+tab_g(i,14)*x(j,2)+tab_g(i,15)*x(j,3); err_g_sig(k,j)=tab_g(i,16)*x(j,1)+tab_g(i,17)*x(j,2)+tab_g(i,18)*x(j,3); err_t_rd(k,j)=tab_t(i,4)*x(j,1)+tab_t(i,5)*x(j,2)+tab_t(i,6)*x(j,3); err_t_yd(k,j)=tab_t(i,7)*x(j,1)+tab_t(i,8)*x(j,2)+tab_t(i,9)*x(j,3); err_t_nu(k,j)=tab_t(i,10)*x(j,1)+tab_t(i,11)*x(j,2)+tab_t(i,12)*x(j,3); err_t_mu(k,j)=tab_t(i,13)*x(j,1)+tab_t(i,14)*x(j,2)+tab_t(i,15)*x(j,3); err_t_sig(k,j)=tab_t(i,16)*x(j,1)+tab_t(i,17)*x(j,2)+tab_t(i,18)*x(j,3); end dg_rd_min(n)=min(err_g_rd(k,:)); dg_rd_max(n)=max(err_g_rd(k,:)); dg_yd_min(n)=min(err_g_yd(k,:)); dg_yd_max(n)=max(err_g_yd(k,:)); dg_nu_min(n)=min(err_g_nu(k,:)); dg_nu_max(n)=max(err_g_nu(k,:)); dg_mu_min(n)=min(err_g_mu(k,:)); dg_mu_max(n)=max(err_g_mu(k,:)); dg_sig_min(n)=min(err_g_sig(k,:)); dg_sig_max(n)=max(err_g_sig(k,:)); dt_rd_min(n)=min(err_t_rd(k,:)); dt_rd_max(n)=max(err_t_rd(k,:)); dt_yd_min(n)=min(err_t_yd(k,:)); dt_yd_max(n)=max(err_t_yd(k,:)); dt_nu_min(n)=min(err_t_nu(k,:)); dt_nu_max(n)=max(err_t_nu(k,:)); dt_mu_min(n)=min(err_t_mu(k,:)); dt_mu_max(n)=max(err_t_mu(k,:)); dt_sig_min(n)=min(err_t_sig(k,:)); dt_sig_max(n)=max(err_t_sig(k,:)); end dg_sum_min=dg_rd_min+dg_yd_min+dg_nu_min+dg_mu_min+dg_sig_min; dg_sum_max=dg_rd_max+dg_yd_max+dg_nu_max+dg_mu_max+dg_sig_max; dt_sum_min=dt_rd_min+dt_yd_min+dt_nu_min+dt_mu_min+dt_sig_min; dt_sum_max=dt_rd_max+dt_yd_max+dt_nu_max+dt_mu_max+dt_sig_max;

181 figure(80) plot(tab_g(43:49,3),dg_sum_min,'-o',tab_g(43:49,3),dg_sum_max,'-s','LineWidth',2); grid xlabel('\gamma,^o') ylabel('\Delta\gamma_о_п^o') legend('\Delta\gamma_о_п^m^i^n','\Delta\gamma_о_п^m^a^x') title('Область возможных погрешностей вычисления \gamma_о_п;\vartheta=30^o') figure(90) plot(tab_t(43:49,3),dt_sum_min,'-o',tab_t(43:49,3),dt_sum_max,'-s','LineWidth',2);grid xlabel('\gamma,^o') ylabel('\Delta\vartheta_о_п^o') legend('\Delta\vartheta_о_п^m^i^n','\Delta\vartheta_о_п^m^a^x') title('Область возможных погрешностей вычисления \vartheta_о_п;\vartheta=30^o')

182 Приложение Г. ОПИСАНИЕ И СТРУКТУРА МАТЕМАТИЧЕСКОЙ ИМИТАЦИОННОЙ МОДЕЛИ ПЕРСОНАЛЬНОЙ АВТОНОМНОЙ ИНФОРМАЦИОННО- ИЗМЕРИТЕЛЬНОЙ СИСТЕМЫ 1 Общий вид структурной схемы имитационной модели персональной ИИС определения местоположения ПО На рисунке Г.1 приведена структурная схема имитационной модели персональной ИИС определения местоположения ПО, состоящая из четырех подсистем с именами: «Блок А. Модель движения наземного ПО», «Блок В. Персональная ИИС», «Блок С. Управление структурой ИИС», «Блок D. Индикация параметров движения». Рисунок Г.1– Общий вид структурной схемы имитационной модели персональной ИИС определения местоположения ПО

183 2 Структурная схема модели движения наземного ПО с блоком ДПИ Общий вид схемы модели движения наземного ПО показан на рисунке Г.2. Рисунок Г.2 – Общий вид схемы модели движения наземного ПО Список выходных параметров модели движения наземного ПО. 1) wx,wy,wz – проекции вектора угловой скорости блока ДПИ на связанные с ним оси, 1/с; 2) axr,ayr,azr – проекции вектора кажущегося ускорения блока ДПИ на связанные с ним оси, м/с2; 3) p,t,g – углы рысканья, тангажа, крена блока ДПИ, радиан (на схему модели персональной ИИС эта информация поступает как начальные значения перечисленных углов); 4) Н – высота центра блока ДПИ относительно нижней опорной поверхности, м; 5) dxg/dt,dzg/dt – проекции путевой скорости блока ДПИ на оси нормальной системы координат, м/с. На рисунке Г.3 показана структурная схема блока А «Модель движения наземного ПО», включающая в себя четыре подсистемы, оформленные в виде блоков с именами: блок А.1 - «Поступательное движение элемента ПО», блок А.2 - «Угловые колебания элемента ПО и блока ДПИ», блок А.3 - «Поступательное движение блока ДПИ», блок А.4 относительно ПО». «Координаты положения блока ДПИ

184 2.1 Блок А.1 «Поступательное движение элемента ПО». На вход блока А.1 поступает информация об углах ориентации блока ДПИ относительно нормальной системы координат p=ψ, t=ϑ, g=γ в размерности «радиан». Рисунок Г.3 – Структурная схема блока А «Модель движения наземного ПО» Список выходных параметров блока А.1. 1) axr1,ayr1,azr1 – проекции вектора кажущегося ускорения полюса О1 элемента 1 ПО, м/с2; 2) dPSI*/dt –составляющая угловой скорости программного изменения путевого угла элемента ПО, 1/с; 3) PSI* – программная составляющая угла пути элемента ПО, радиан; 4) d2PSI*/dt2 –составляющая углового ускорения программного изменения путевого угла элемента ПО, 1/с2; 5) U_stop – сигнал управления остановками ПО; 6) x_g1,H1,z_g1 – координаты полюса О1 в неподвижной системе отсчета, м; 7) dxg1/dt,dzg1/dt – проекции путевой скорости полюса О1 на оси нормальной системы координат, м/с.

185 Структурная схема блока А.1 приведена на рисунке Г.4. Она включает в себя шесть подсистем, оформленных в виде блоков с именами: блок А.1.1 – «Путевое движение полюса О1», блок А.1.2 – «Повороты», блок А.1.3 – «Боковые колебания центра О1 относительно заданной линии пути», блок А.1.4 – «Движение полюса О1 по вертикали», блок А.1.5 – «Проекции кажущегося ускорения и радиуса-вектора центра О1 на оси земной СК», блок А.1.6 – «Матрица С_с_g». Рисунок Г.4 – Структурная схема блока А.1 «Поступательное движение элемента ПО» Структурная схема блока А.1.1 «Путевое движение полюса О1» приведена на рисунке Г.5. В схеме блока А.1.1 имеются подсистемы с именами «Программа остановок», «Детектор остановок», «Управление путевой скоростью. Рисунок Г.5 – Структурная схема блока А.1.1«Путевое движение полюса О1»

186 Структурные схемы блоков А.1.2 – А.1.6 приведены на рисунках Г.6 – Г.14. Рисунок Г.6 – Структурная схема блока А.1.2 «Повороты» Рисунок Г.7 – Структурная схема блока А.1.3 «Боковые колебания центра О1 относительно заданной линии пути» Рисунок Г.8 – Структурная схема блока А.1.4 «Движение полюса О1 по вертикали»

187 Рисунок Г.9 – Структурная схема блока А.1.5 «Проекции кажущегося ускорения и радиуса-вектора центра О1 на оси земной СК» Рисунок Г.10 – Структурная схема блока А.1.6 «Матрица С_с_g» Рисунок Г.11 – Структурная схема подсистемы «Тригонометрические функции» блока А.1.6 «Матрица С_с_g»

188 Рисунок Г.12 – Структурная схема подсистемы «Строка с11_с21_с31» блока А.1.6 «Матрица С_с_g» Рисунок Г.13 – Структурная схема подсистемы «Строка с12_с22_с32» блока А.1.6 «Матрица С_с_g» Рисунок Г.14 – Структурная схема подсистемы «Строка с13_с23_с33» блока А.1.6 «Матрица С_с_g»

189 2.2 Блок А.2 «Угловые колебания элемента ПО и блока ДПИ». Список входных параметров блока А.2. 1) dPSI*/dt –составляющая угловой скорости программного изменения путевого угла элемента ПО, 1/с; 2) PSI* – программная составляющая угла пути элемента ПО, радиан; 3) d2PSI*/dt2 –составляющая углового ускорения программного изменения путевого угла элемента ПО, 1/с2; 4) U_stop – сигнал управления остановками ПО; 5) a_dy,a_dz,a_dx – углы ориентации блока ДПИ относительно системы координат O1X1Y1Z1, связанной с элементом ПО, полученные тремя последовательными поворотами системы координат OcXcYcZc относительно осей Y, Z и X соответственно, радиан. Список выходных параметров блока А.2. 1) dwx1/dt,dwy1/dt,dwz1/dt – проекции вектора углового ускорения элемента ПО на оси системы координат O1X1Y1Z1, 1/с2; 2) wx1,wy1,wz1 – проекции вектора угловой скорости элемента ПО на оси системы координат O1X1Y1Z1, 1/с; 3) p1,t1,g1 – углы рысканья, тангажа и крена элемента ПО соответственно, радиан; 4) wx,wy,wz – проекции вектора угловой скорости блока ДПИ на оси системы координат OcXcYcZc, 1/с; 5) p,t,g – углы рысканья, тангажа и крена блока ДПИ соответственно, радиан. На рисунке Г.15 приведена структурная схема блока А.2, включающая в себя три подсистемы, оформленные в виде блоков: блок А.2.1 – «Углы ориентации элемента ПО и их производные по времени», блок А.2.2 – «Угловые скорости и ускорения элемента ПО в проекциях на оси СК O1X1Y1Z1», блок А.2.3 – «Угловое движение блока ДПИ». Структурные схемы блоков А.2.1 и А.2.2 показаны на рисунках Г.16, Г.17. Многоуровневая структура блока А.2.3 раскрыта на рисунках Г.18 – Г.25.

190 Рисунок Г.15 – Структурная схема блока А.2 «Угловые колебания элемента ПО и блока ДПИ» Рисунок Г.16 – Структурная схема блока А.2.1 «Углы ориентации элемента ПО и их производные по времени» Рисунок Г.18 – Структурная схема блока А.2.3 «Угловое движение блока ДПИ»

191 Рисунок Г.17 – Структурная схема блока А.2.2 «Угловые скорости и ускорения элемента ПО в проекциях на оси СК O1X1Y1Z1» Рисунок Г.19 – Структурная схема подсистемы «Проекции угловой скорости блока ДПИ на оси СК OcXcYcZc» блока А.2.3 «Угловое движение блока ДПИ»

192 Рисунок Г.20 – Структурная схема подсистемы «Углы ориентации блока ДПИ» блока А.2.3 «Угловое движение блока ДПИ» Рисунок Г.21 – Структурная схема подсистемы «Матрица С_ДПИ_ПО» блока А.2.3. «Угловое движение блока ДПИ» (рисунок 5.31)

193 Рисунок Г.22 – Структурная схема подсистемы «тригонометрические функции» блока А.2.3 «Угловое движение блока ДПИ» (рисунок Г.21) Рисунок Г.23 – Структурная схема подсистемы «строка р11_р12_р13» блока А.2.3. «Угловое движение блока ДПИ» (рисунок Г.21) Рисунок Г.24 – Структурная схема подсистемы «строка р21_р22_р23» блока А.2.3. «Угловое движение блока ДПИ» (рисунок Г.21)

194 Рисунок Г.25 – Структурная схема подсистемы «строка р31_р32_р33» блока А.2.3. «Угловое движение блока ДПИ» (рисунок 5.32) 2.3 Блок А.3 «поступательное движение блока ДПИ». Список входных параметров блока А.3. 1) axr1,ayr1,azr1 – проекции вектора кажущегося ускорения полюса О1 элемента ПО, м/с2; 2) dwx1/dt,dwy1/dt,dwz1/dt – проекции вектора углового ускорения элемента ПО на оси системы координат O1X1Y1Z1, 1/с2; 3) wx1,wy1,wz1 – проекции вектора угловой скорости элемента ПО на оси системы координат O1X1Y1Z1, 1/с; 4) p1,t1,g1 – углы рысканья, тангажа и крена элемента ПО соответственно, радиан; 5) x_g1,H1,z_g1 – координаты полюса О1 в неподвижной системе отсчета, м; 6) x_Oc,y_Oc,z_Oc – координаты полюса Ос в системе координат O1X1Y1Z1, м. Список выходных параметров блока А.3. 1) axr,ayr,azr – проекции вектора кажущегося ускорения блока ДПИ на связанные с ним оси, м/с2; 2) Н – высота центра блока ДПИ относительно нижней опорной поверхности, м;

195 3)   dRO1Oc dVxg,dVzg – проекции вектора   dt    на оси Xg, Zg, м/с. g На рисунке Г.26 приведена структурная схема блока А.3, включающая в себя четыре подсистемы, оформленные в виде блоков: блок А.3.1 – «Проекции вектора кажущегося ускорения блока ДПИ на оси СК O1X1Y1Z1», блок А.3.2 – «Проекции вектора кажущегося ускорения блока ДПИ на оси СК OcXcYcZc», блок А.3.3 – «Вычисление проекций вектора R_O1_Oc на оси земной СК», блок А.3.4 – «Вычисление проекций производной вектора R_O1_Oc на оси земной СК». Рисунок Г.26 – Структурная схема блока А.3 «Поступательное движение блока ДПИ» Структурные схемы блоков А.3.1 – А.3.4 приведены на рисунках Г.27 – Г.35.

196 Рисунок Г.27 – Структурная схема блока А.3.1 «Проекции вектора кажущегося ускорения блока ДПИ на оси СК O1X1Y1Z1» Рисунок Г.28 – Структурная схема блока А.3.3 «Вычисление проекций вектора R_O1_Oc на оси земной СК»

197 Рисунок Г.29 – Структурная схема блока А.3.2 «Проекции вектора кажущегося ускорения блока ДПИ на оси СК OcXcYcZc»

198 Рисунок Г.30 – Структурная схема блока А.3.4 «Вычисление проекций производной вектора R_O1_Oc на оси земной СК» Рисунок Г.31 – Структурная схема подсистемы «Матрица С_g′,ПО», входящей в состав блоков А.3.3 и А.3.4 (рисунки 5.40, 5.41) Рисунок Г.32 – Структурная схема подсистемы «тригонометрические функции» блока «Матрица С_g′,ПО» (рисунок 5.42)

199 Рисунок Г.33 – Структурная схема подсистемы «строка u11_u12_u13» блока «Матрица С_g′,ПО» (рисунок 5.42) Рисунок Г.34 – Структурная схема подсистемы «строка u21_u22_u23» блока «Матрица С_g′,ПО» (рисунок 5.42) Рисунок Г.35 – Структурная схема подсистемы «строка u31_u32_u33» блока «Матрица С_g′,ПО» (рисунок 5.42)

200 2.4 Блок А.4 «Координаты положения блока ДПИ относительно ПО» Блок А.4 содержит два звена типа «Constant» с угловыми и линейными координатами, определяющими положение блока ДПИ в системе координат O1 X 1Y1Z1 , связанной с элементом ПО (рисунок Г.36). Список выходных параметров блока А.4. 1) a_dy,a_dz,a_dx – углы δ y , δ z , δ x ориентации блока ДПИ относительно элемента ПО, радиан; 2) x_Oc,y_Oc,z_Oc – координаты полюса Ос в системе координат O1 X 1Y1Z1 , м. Рисунок Г.36 – Структурная схема блока А.4 «Координаты положения блока ДПИ относительно ПО» 3 Структурная схема модели комплексной пешеходной ИИС определения местоположения ПО На рисунке Г.37 показан общий вид схемы модели персональной ИИС «Блок В. Персональная ИИС». Список входных параметров блока В. 1) U_KS – сигнал, управляющий структурой ИИС: U_KS=1 – инерциальная СУО работает в комплексе с дальномерной СУО; U_KS=0 – автономная работа инерциальной СУО; 2) U_PN – сигнал, управляющий режимом коррекции путевой скорости на приоритетных направлениях: U_PN=1 – коррекция включена; U_PN=0 – коррекция выключена;

201 3) U_RR – сигнал, управляющей сбросом выходов интеграторов, вычисляющих проекции вектора скорости блока ДПИ, при кратковременных остановках ПО (режим «ZUPT»): U_RR=1 – режим «ZUPT» включен; U_RR=0 – режим «ZUPT» выключен; 4) wx,wy,wz – проекции вектора угловой скорости блока ДПИ на оси системы координат OcXcYcZc, 1/с; 5) axr,ayr,azr – проекции вектора кажущегося ускорения блока ДПИ на оси системы координат OcXcYcZc, м/с2; 6) p(0),t(0),g(0) – начальные значения углов ориентации блока ДПИ относительно нормальной системы координат (p=ψ, t=ϑ, g=γ), радиан; 7) Н – высота полюса Oc относительно нижней опорной поверхности, м; 8) dxg/dt(0),dzg/dt(0) – начальные значения проекций вектора путевой скорости блока ДПИ на оси нормальной системы координат, м/с. Рисунок Г.37 – Общий вид схемы модели персональной ИИС Список выходных параметров блока В. 1) theta_KS – нормированный сигнал по углу тангажа блока ДПИ на выходе комплексной СУО, радиан; 2) gamma_KS – нормированный сигнал по крену блока ДПИ на выходе комплексной СУО, радиан;

202 3) psi_INS – нормированный сигнал по углу рысканья блока ДПИ на выходе инерциальной СУО, радиан; 4) theta_INS – нормированный сигнал по углу тангажа блока ДПИ на выходе инерциальной СУО, радиан; 5) gamma _INS – нормированный сигнал по углу крена блока ДПИ на выходе инерциальной СУО, радиан; 6) VECTOR – массив выходных параметров, включающий в себя: - Uwx,Uwy,Uwz – нормированные сигналы с выходов датчиков угловых скоростей, 1/с; - Uaxr,Uayr,Uazr – нормированные сигналы с выходов акселерометров, м/с2; - Uaxg,Uayg,Uazg – нормированные сигналы с выхода вычислителя, формирующего проекции абсолютного ускорения блока ДПИ на оси нормальной системы координат, м/с2; - xg,yg,zg – вычисленные значения координат блока ДПИ в земной системе координат, м; - Vxg,Vyg,Vzg – вычисленные значения проекций вектора земной скорости блока ДПИ на оси нормальной системы координат, м/с; - reset Rising – сигнал сброса выходов интеграторов, вычисляющих проекции вектора скорости блока ДПИ, в режиме «ZUPT»; 7) theta_DM – нормированный сигнал опорного угла тангажа блока ДПИ на выходе дальномерной СУО, радиан; 8) gamma _DM – нормированный сигнал опорного угла крена блока ДПИ на выходе дальномерной СУО, радиан. На рисунке Г.38 приведена структурная схема модели персональной ИИС, включающая в себя три подсистемы: «Блок В.1.ИНС», «Блок В.2.Дальномерная СУО», «Блок В.3.Схема комплексирования».

203 Рисунок Г.38 – Структурная схема модели «Блок В. Персональная ИИС» 3. 1 Подсистема «Блок В.1. ИНС» Список входных параметров блока В.1. 1) psi(0),theta(0),gamma(0) – начальные значения углов ориентации блока ДПИ относительно нормальной системы координат (p=ψ, t=ϑ, g=γ), радиан; 2) theta – сигнал по углу тангажа блока ДПИ с выхода схемы комплексирования (theta_KS) при U_KS=1 или с выхода вычислителя ИНС (theta_INS) при U_KS=0, радиан; 3) gamma – сигнал по углу крена блока ДПИ с выхода схемы комплексирования (gamma _KS) при U_KS=1 или с выхода вычислителя ИНС (gamma _INS) при U_KS=0, радиан; 4) yg(0) – начальное значение высоты центра Ос блока ДПИ относительно нижней опорной поверхности, м; 5) wx,wy,wz – проекции вектора угловой скорости блока ДПИ на оси системы координат OcXcYcZc, 1/с; 6) axr,ayr,azr – проекции вектора кажущегося ускорения блока ДПИ на оси системы координат OcXcYcZc, м/с2; 7) xg1(0) – начальное значение проекции путевой скорости блока ДПИ на ось Xg, м/с; 8) ось Zg, м/с; zg1(0) – начальное значение проекции путевой скорости блока ДПИ на

204 9) U_PN – сигнал, управляющий режимом коррекции путевой скорости на приоритетных направлениях: U_PN=1 – коррекция включена; U_PN=0 – коррекция выключена; 10) U_RR – сигнал, управляющей сбросом выходов интеграторов, вычисляющих проекции вектора скорости блока ДПИ, при кратковременных остановках ПО (режим «ZUPT»): U_RR=1 – режим «ZUPT» включен; U_RR=0 – режим «ZUPT» выключен. Список выходных параметров блока В.1. 1) psi_INS – нормированный сигнал по углу рысканья с выхода инерциальной СУО, радиан; 2) theta_INS – нормированный сигнал по углу тангажа блока ДПИ на выходе инерциальной СУО, радиан; 3) gamma_INS – нормированный сигнал по углу крена блока ДПИ на выходе инерциальной СУО, радиан; 4) Uwx,Uwy,Uwz – нормированные сигналы с выходов гироскопических датчиков угловой скорости, 1/с; 5) Uaxr,Uayr,Uazr – нормированные сигналы с выходов акселерометров, измеряющих проекции вектора кажущегося ускорения блока ДПИ на оси системы координат OcXcYcZc, м/с2; 6) Uaxg,Uayg,Uazg – нормированные сигналы с выходов вычислителя проекций абсолютного ускорения блока ДПИ на оси нормальной системы координат, м/с2; 7) xg,yg,zg – вычисленные значения координат блока ДПИ в земной системе координат, м; 8) Vxg,Vyg,Vzg – вычисленные значения проекций вектора земной скорости блока ДПИ на оси нормальной системы координат, м/с; 9) reset Rising – сигнал сброса выходов интеграторов, вычисляющих проекции вектора путевой скорости блока ДПИ, в режиме «ZUPT». На рисунке Г.39 приведена структурная схема подсистемы «Блок В.1. ИНС», включающая в себя четыре подсистемы: «Блок В.1.1.Датчики первичной

205 информации», «Блок В.1.2.Вычислитель углов Эйлера», «Блок В.1.3.Вычислитель проекций абсолютного ускорения блока ДПИ на оси нормальной СК», «Блок В.1.4.Вычислитель координат и проекций скорости блока ДПИ на оси земной СК». Рисунок Г.39 – Структурная схема подсистемы «Блок В.1.ИНС» 3.1.1 Модели датчиков первичной информации На рисунке Г. приведена структурная схема подсистемы «Блок В.1.1.Датчики первичной информации», в состав которой входят четыре подсистемы: «Блок В.1.1.1.Датчики угловой скорости», «Блок В.1.1.2. Акселерометры», «Блок В.1.1.3. Экстраполятор 1», «Блок В.1.1.4. Экстраполятор 2». Рисунок Г.40 – Структурная схема подсистемы «Блок В.1.1.Датчики первичной информации»

206 Структурные схемы моделей ДПИ приведены на рисунках Г.41, Г.42. Рисунок Г.41 – Структурная схема подсистемы «Блок В.1.1.1.Датчики угловой скорости» Рисунок Г.42 – Структурная схема подсистемы «Блок В.1.1.2.Акселерометры» Структурная схема подсистем «Блок В.1.1.3. Экстраполятор 1», «Блок В.1.1.4. Экстраполятор 2» приведена на рисунке Г.43 и соответствует передаточной функции экстраполятора нулевого порядка [68] WЭ ( s ) = 1 − exp ( −τs ) . s Рисунок Г.43 – Структурная схема подсистем «Блок В.1.1.3.Экстраполятор1», «Блок В.1.1.4.Экстраполятор 2»

207 Входной сигнал интегратора U1* равен произведению сигнала U1, поступающего с ДПИ, на импульсный сигнал U_delta, моделирующий функцию δ ( t − k τ ) , k =0,1,2,... . Экстраполятор нулевого порядка удерживает на выходе сигнал U 2 ( t ) =U1 ( k τ ) , k = 0,1,2,... в течение всего периода дискретности Td=τ. Основными выходными параметрами подсистемы «Блок В.1.1.Датчики первичной информации» являются нормированные сигналы на выходах экстраполяторов, обозначенные символами Ewx,Ewy,Ewz (проекции угловой скорости) и Eaxr,Eayr,Eazr (проекции кажущегося ускорения), которые поступают на входы вычислителей ИНС. Нормированные сигналы с выходов датчиков угловой скорости (Uwx,Uwy,Uwz) и с выходов акселерометров (Uaxr,Uayr,Uazr) поступают в блок индикации параметров движения с целью контроля этих сигналов. 3.1.2 Модель вычислителя углов Эйлера Список входных параметров подсистемы «Блок В.1.2.Вычислитель углов Эйлера». 1) Ewx,Ewy,Ewz – нормированные сигналы с выхода подсистемы «Блок В.1.1.3.Экстраполятор1», несущие информацию о проекциях вектора угловой скорости блока ДПИ на оси системы координат OcXcYcZc, 1/с; 2) psi(0),theta(0),gamma(0) – начальные значения углов рысканья, тангажа и крена блока ДПИ, радиан; 3) theta – сигнал по углу тангажа блока ДПИ с выхода схемы комплексирования (theta_KS) при U_KS=1 или с выхода вычислителя ИНС (theta_INS) при U_KS=0, радиан; 4) gamma – сигнал по углу крена блока ДПИ с выхода схемы комплексирования (gamma _KS) при U_KS=1 или с выхода вычислителя ИНС (gamma _INS) при U_KS=0, радиан; 5) U_PN – сигнал, управляющий режимом коррекции путевой скорости на приоритетных направлениях: U_PN=1 – коррекция включена; U_PN=0 – коррекция выключена.

208 Список выходных параметров подсистемы «Блок В.1.2.Вычислитель углов Эйлера». 1) theta_INS – нормированный сигнал по углу тангажа блока ДПИ на выходе инерциальной СУО, радиан; 2) gamma_INS – нормированный сигнал по углу крена блока ДПИ на выходе инерциальной СУО, радиан; 3) psi_INS – нормированный сигнал по углу рысканья с выхода инерциальной СУО, радиан; 4) На dpsi(k)/dt – угловая скорость рысканья ∆ψ ( k ) / ∆t , 1/с. рисунке Г.44 приведена структурная схема подсистемы «Блок В.1.2.Вычислитель углов Эйлера», содержащая в своем составе две подсистемы: «Блок В.1.2.1.Матрица Q» и «Блок В.1.2.2.Коррекция угла psi на приоритетных направлениях». Рисунок Г.44 – Структурная схема подсистемы «Блок В.1.2.Вычислитель углов Эйлера» Структурная схема подсистемы «Блок В.1.2.1.Матрица Q» приведена на рисунке Г.45.

209 Рисунок Г.45 – Структурная схема подсистемы «Блок В.1.2.1.Матрица Q» На рисунке Г.46 приведена структурная схема подсистемы «Блок В.1.2.2. Коррекция угла psi на приоритетных направлениях», состоящая из двух подсистем: «Формирование сигнала коррекции угла psi» и «Прерывание коррекции при смене направления». Рисунок Г.46 – Структурная схема подсистемы «Блок В.1.2.2. Коррекция угла psi на приоритетных направлениях» Структурная схема подсистемы «Формирование сигнала коррекции угла psi», приведенная на рисунке Г.47, построена при условии, что очередная смена направления движения ПО происходит при повороте на угол ±90о по отношению к предыдущему направлению движения. Рисунок Г.47 – Структурная схема подсистемы «Формирование сигнала коррекции угла psi»

210 Звено «1-D Lookup Table» (рисунок Г.47) реализует методом квантования конкретную программу зависимости угла приоритетного направления движения  пр (Table data) от текущего значения угла рысканья ψ  (Breakpoints 1) при условии ψ выполнения семи поворотов на 90о каждый при двойном обходе замкнутого маршрута против часовой стрелки. Структурная схема подсистемы «Прерывание коррекции при смене направления» приведена на рисунке Г.48. Рисунок Г.48 – Структурная схема подсистемы «Прерывание коррекции при смене направления» 3.1.3 Модель вычислителя проекций вектора абсолютного ускорения блока ДПИ на оси нормальной системы координат Список входных параметров подсистемы «Блок В.1.3.Вычислитель проекций абсолютного ускорения блока ДПИ на оси нормальной СК». 1) рsi_INS,theta,gamma – нормированные сигналы по углам рысканья, тангажа и крена блока ДПИ, сформированные на выходе инерциальной СУО (рsi_INS, а также theta,gamma при U_KS=0) или на выходе комплексной СУО (theta,gamma при U_KS=1), радиан; 2) U_RR – сигнал, управляющей сбросом выходов интеграторов, вычисляющих проекции вектора скорости блока ДПИ, при кратковременных остановках ПО (режим «ZUPT»): U_RR=1 – режим «ZUPT» включен; U_RR=0 – режим «ZUPT» выключен; 3) Eaxr,Eayr,Eazr – нормированные сигналы с выхода подсистемы «Блок В.1.1.4.Экстраполятор2», несущие информацию о проекциях вектора кажущегося ускорения блока ДПИ на оси системы координат OcXcYcZc, м/с2.

211 Список выходных параметров подсистемы «Блок В.1.3.Вычислитель проекций абсолютного ускорения блока ДПИ на оси нормальной СК». 1) reset Rising – сигнал сброса выходов интеграторов, вычисляющих проекции вектора скорости блока ДПИ, в режиме «ZUPT»; 2) Uaxg,Uayg,Uazg – вычисленные значения проекций вектора абсолютного ускорения блока ДПИ на оси нормальной системы координат, м/с2. На рисунке Г.49 приведена структурная схема подсистемы «Блок В.1.3. Вычислитель проекций абсолютного ускорения блока ДПИ на оси нормальной СК», включающая в себя две подсистемы: «Блок В.1.3.1.Матрица С_g1,c» и «Блок В.1.3.2.Детектор остановок». Рисунок Г.49 – Структурная схема подсистемы «Блок В.1.3. Вычислитель проекций абсолютного ускорения блока ДПИ на оси нормальной СК» Структура подсистемы «Блок В.1.3.1.Матрица С_g1,c» точно такая же, как и структура подсистемы «Матрица С_g′,ПО», показанная на рисунках Г.31 – Г.35. Структурная схема подсистемы «Блок В.1.3.2.Детектор остановок» приведена на рисунке Г.50. Рисунок Г.50 – Структурная схема подсистемы «Блок В.1.3.2.Детектор остановок»

212 Подсистема «Блок В.1.3.2.Детектор остановок» определяет среднее значение модуля абсолютного ускорения по выборке из 46 дискрет (а_46_ср) и сравнивает его с пороговым значением a_min. Если среднее значение модуля абсолютного ускорения меньше или равно a_min, сигнал reset Rising на выходе подсистемы становится равным единице. Среднее значение модуля ускорения вычисляется с помощью подсистем с именами «Окно 1», «Окно 2», «Окно 3», «Окно 4», «Окно 5», структурные схемы которых приведены на рисунках Г.51 – Г.55. Рисунок Г.51 – Структурная схема подсистемы «Окно 1» Рисунок Г.52 – Структурная схема подсистемы «Окно 2» Рисунок Г.53 – Структурная схема подсистемы «Окно 3»

213 Рисунок Г.54 – Структурная схема подсистемы «Окно 4» Рисунок Г.55 – Структурная схема подсистемы «Окно 5» 3.1.4 Модель вычислителя координат и проекций скорости блока ДПИ на оси земной системы координат Список входных параметров подсистемы «Блок В.1.4.Вычислитель координат и проекций скорости блока ДПИ на оси земной СК». 1) reset Rising – сигнал сброса выходов интеграторов, вычисляющих проекции вектора скорости блока ДПИ, в режиме «ZUPT»; 2) Uaxg,Uayg,Uazg – вычисленные значения проекций вектора абсолютного ускорения блока ДПИ на оси нормальной системы координат, м/с2; 3) yg(0) – начальное значение расстояния блока ДПИ относительно нижней опорной поверхности, м; 4) xg1(0) – начальное значение проекции путевой скорости на ось Xg, м/с; 5) zg1(0) – начальное значение проекции путевой скорости на ось Zg, м/с; 6) psi_INS – нормированный сигнал по углу рысканья с выхода инерциальной СУО, радиан; 7) dpsi/dt – нормированный сигнал по угловой скорости рысканья с выхода инерциальной СУО, 1/с;

214 8) U_PN – сигнал, управляющий режимом коррекции путевой скорости на приоритетных направлениях: U_PN=1 – коррекция включена; U_PN=0 – коррекция выключена. Список выходных параметров подсистемы «Блок В.1.4.Вычислитель координат и проекций скорости блока ДПИ на оси земной СК». 1) xg,yg,zg – координаты местоположения блока ДПИ в земной системе координат, м; 2) Vxg,Vyg,Vzg – проекции вектора скорости блока ДПИ на оси нормальной системы координат, м/с. На рисунке Г.56 приведена структурная схема подсистемы «Блок В.1.4. Вычислитель координат и проекций скорости блока ДПИ на оси земной СК», содержащая одну подсистему «Блок В.1.4.1.Коррекция путевой скорости на приоритетных направлениях». Рисунок Г.56 – Структурная схема подсистемы «Блок В.1.4. Вычислитель координат и проекций скорости блока ДПИ на оси земной СК» Начальные значения проекций путевой скорости xg1(0) и yg1(0) подаются на входы «х0» цифровых интеграторов через ключи, управляемые сигналом сброса «reset Rising», который поступает также на входы сброса интеграторов. При значении reset Rising=1 на входы интеграторов «х0» будут поданы нули, которые и будут присвоены сигналам на выходах интеграторов. Структурная схема подсистемы «Блок В.1.4.1.Коррекция путевой скорости на приоритетных направлениях» приведена на рисунке Г.57. Эта схема содержит

215 шесть подсистем с именами: «Индикация движения вдоль оси Zg» (рисунок Г.58), «Индикация движения вдоль оси Хg» (рисунок Г.59), «Коррекция «ходовой» скорости Vzg» (рисунок Г.60), «Коррекция «боковой» скорости Vxg» (рисунок Г.61), «Коррекция «боковой» скорости Vzg» (рисунок Г.62), «Коррекция «ходовой» скорости Vxg» (рисунок Г.63). Рисунок Г.57 – Структурная схема подсистемы «Блок В.1.4.1.Коррекция путевой скорости на приоритетных направлениях» Рисунок Г.58 – Структурная схема подсистемы «Индикация движения вдоль оси Zg»

216 Рисунок Г.59 – Структурная схема подсистемы «Индикация движения вдоль оси Хg» Рисунок Г.60 – Структурная схема подсистемы «Коррекция «ходовой» скорости Vzg» Рисунок Г.61 – Структурная схема подсистемы «Коррекция «боковой» скорости Vzg»

217 Рисунок Г.62 – Структурная схема подсистемы «Коррекция «ходовой» скорости Vхg» Рисунок Г.63 – Структурная схема подсистемы «Коррекция «боковой» скорости Vхg» Характеристика каждого из звеньев «1-D T(u)» представляет собой почти прямоугольную трапецию с нижним основанием в границах от (psi*-pi/4) до (psi*+pi/4) и с верхним основанием в границах от (psi*-pi/4+0.001) до (psi*+pi/40.001), где psi* – значение приоритетного угла рысканья. Если текущее значение угла рысканья psi_INS находится в границах верхнего основания трапеции, то выходной сигнал звена «1-D T(u)» равен единице, если же значение psi_INS находится за пределами границ нижнего основания трапеции, то выходной сигнал звена «1-D T(u)» равен нулю. Т.к. при многократных поворотах ПО угол рысканья может меняться неограниченно, то движению ПО вдоль одного и того же приоритетного направления соответствуют значения psi*, кратные углу π/2, т.е. psi*=kπ/2. При движении ПО вдоль оси Xg k=0, 2, 4,…, а при движении ПО вдоль

218 оси Zg k=1, 3, 5, 7,… На рисунке Г.64 приведены графики изменения сигналов y_Xg и y_Zg при двойном обходе по замкнутому прямоугольному маршруту. а) Индикатор движения ПО вдоль оси X б) g 2 Индикатор движения ПО вдоль оси Z g 2 1.8 1.8 1.6 1.6 1.4 1.4 1.2 1.2 1 1 0.8 0.8 0.6 0.6 0.4 0.4 0.2 0.2 0 0 0 100 200 300 400 500 600 700 800 0 100 200 300 t, c 400 500 600 700 800 t, c Рисунок Г.64 – Графики изменения выходных сигналов индикаторов движения ПО по приоритетным направлениям вдоль оси Xg (а) и вдоль оси Zg (б) при двойном обходе по замкнутому прямоугольному маршруту Структурные схемы, приведенные на рисунках Г.60 и Г.62 содержат подсистему «Запрет деления на ноль», структура которой показана на рисунке Г.65. Рисунок Г.65 – Структурная схема подсистемы «Запрет деления на ноль» 3.2 Подсистема «Блок В.2. Дальномерная СУО» Список входных параметров блока В.2. 1) p,t,g – кинематические углы рысканья, тангажа и крена блока ДПИ, радиан; 2) Н – высота блока ДПИ относительно ОПН, м. Список выходных параметров блока В.2. 1) theta_DM – вычисленное значение опорного угла тангажа блока ДПИ, радиан; 2) радиан. gamma_DM – вычисленное значение опорного угла крена блока ДПИ,

219 На рисунке Г.66 «Блок В.2.Дальномерная приведена СУО», «Блок В.2.1.Кинематические «Блок В.2.2.Инструментальные уравнения структурная содержащая дальностей, погрешности», схема подсистемы три подсистемы: измеряемых ДМ», «Блок В.2.3.Дальномеры и вычислитель опорных углов тангажа и крена». Рисунок Г.66 – Структурная схема подсистемы «Блок В.2.Дальномерная СУО» 3.2.1 Подсистема «Блок В.2.1. Кинематические уравнения дальностей, измеряемых ДМ» Список входных параметров блока В.2.1. 1) p,t,g – кинематические углы рысканья, тангажа и крена блока ДПИ, радиан; 2) d(sig1), d(mi1) – отклонения от номинальных значений углов σ1, µ1, радиан; 3) d(sig2), d(mi2) – отклонения от номинальных значений углов σ2, µ2, радиан; 4) d(sig3), d(mi3) – отклонения от номинальных значений углов σ3, µ3, радиан; 5) Н – высота блока ДПИ относительно ОПН, м. Список выходных параметров блока В.2.1. 1) L1,L2,L3 – кинематические значения расстояний, измеряемых ДМ, м. На рисунке Г.67 приведена структурная схема подсистемы «Блок В.2.1. Кинематические уравнения дальностей, измеряемых ДМ», содержащая шесть

220 подсистем: «Блок В.2.1.1.Элементы матрицы C_g_d1», «Блок В.2.1.2.Элементы матрицы C_g_d2», «Блок В.2.1.3.Элементы матрицы C_g_d3», «Блок В.2.1.4.Высота полюса Od1», «Блок В.2.1.5.Высота полюса Od2», «Блок В.2.1.6.Высота полюса Od3». Рисунок Г.67 – Структурная схема подсистемы «Блок В.2.1. Кинематические уравнения дальностей, измеряемых ДМ» Подсистема «Блок В.2.1» реализует вычисление действительных расстояний Li . Вычисление коэффициентов матриц Cg,di , i=1, 2, 3 выполняют подсистемы «Блок В.2.1.1.Элементы матрицы C_g_d1», «Блок В.2.1.2.Элементы матрицы C_g_d2», «Блок В.2.1.3.Элементы матрицы C_g_d3», структура которых одинакова, поэтому на рисунках Г.68 – Г.72 приведена структурная схема и ее детализация лишь для подсистемы «Блок В.2.1.1.Элементы матрицы C_g_d1». Рисунок Г.68 – Структурная схема подсистемы «Блок В.2.1.1.Элементы матрицы C_g_d1»

221 Рисунок Г.69 – Структурная схема подсистемы «тригонометрические функции» (рисунок 5.80) Рисунок Г.70 – Структурная схема подсистемы «элемент с11» (рисунок 5.80) Рисунок Г.71 – Структурная схема подсистемы «элемент с21» (рисунок 5.80)

222 Рисунок Г.72 – Структурная схема подсистемы «элемент с31» (рисунок 5.80) Вычисление высот y gOdi выполняют подсистемы «Блок В.2.1.4.Высота полюса Od1», «Блок В.2.1.5.Высота полюса Od2», «Блок В.2.1.6.Высота полюса Od3», структура которых одинакова, поэтому на рисунке Г.73 приведена структурная схема лишь для подсистемы «Блок В.2.1.4.Высота полюса Od1». Рисунок Г.73 – Структурная схема подсистемы «Блок В.2.1.4.Высота полюса Od1»

223 3.2.2 Подсистема «Блок В.2.2.Инструментальные погрешности» Вычисление дальностей инструментальных установки ДМ выполняется погрешностей на блоке основных ДПИ, с учетом конструктивных задаваемых в возможных параметров подсистеме «Блок В.2.2.Инструментальные погрешности» (рисунок Г.74). Рисунок Г.74 – Структурная схема подсистемы «Блок В.2.2.Инструментальные погрешности» В схеме на рисунке Г.74 можно задать отклонения от номинальных значений следующих конструктивных параметров, существенно влияющих на точность определения опорных углов: - угловых координат установки ДМ σi, µi, i=1, 2, 3 (d(sig1),d(sig2),d(sig3), d(mi1),d(mi2),d(mi3)); - ординат yd i центров Odi , i = 1,2,3 в системе координат OcXcYcZc (d(yd1),d(yd2),d(yd3)). 3.2.3 Подсистема «Блок В.2.3.Дальномеры и вычислитель опорных углов тангажа и крена» Список входных параметров блока В.2.3. 1) L1,L2,L3 – кинематические значения расстояний, измеряемых ДМ, м. Список выходных параметров блока В.2.3. 1) theta_DM – вычисленное значение опорного угла тангажа блока ДПИ, радиан; 2) радиан. gamma_DM – вычисленное значение опорного угла крена блока ДПИ,

224 На рисунке Г.75 приведена структурная схема подсистемы «Блок В.2.3.Дальномеры и вычислитель опорных углов тангажа и крена», содержащая три подсистемы: «Блок В.2.3.1.Дальномеры», «Блок В.2.3.2.Экстраполятор-3», «Блок В.2.3.3. Вычислитель опорных углов тангажа и крена». Рисунок Г.75 – Структурная схема подсистемы «Блок В.2.3.Дальномеры и вычислитель опорных углов тангажа и крена» Структурная схема подсистемы «Блок В.2.3.1.Дальномеры» приведена на рисунке Г.76. Рисунок Г.76 – Структурная схема подсистемы «Блок В.2.3.1.Дальномеры» Подсистема «Блок В.2.3.2.Экстраполятор-3» имеет такую же структуру, что и подсистемы «Блок В.1.1.3. Экстраполятор1», «Блок В.1.1.4. Экстраполятор 2» (рисунок Г.43). Вычисление опорных углов тангажа и крена выполняет подсистема «Блок В.2.3.3. Вычислитель опорных углов тангажа и крена» (рисунок Г.77).

225 Рисунок Г.77 – Структурная схема подсистемы «Блок В.2.3.3. Вычислитель опорных углов тангажа и крена» 3.3 Подсистема «Блок В.3. Схема комплексирования» Список входных параметров блока В.3. 1) theta_INS – нормированный сигнал с выхода инерциальной СУО по углу тангажа блока ДПИ, радиан; 2) gamma_INS – нормированный сигнал с выхода инерциальной СУО по углу крена блока ДПИ, радиан; 3) theta_DM – нормированное значение опорного угла тангажа блока ДПИ, вычисленное дальномерной СУО, радиан; 4) gamma_DM – нормированное значение опорного угла крена блока ДПИ, вычисленное дальномерной СУО, радиан. Список выходных параметров блока В.3. 1) theta_KS – нормированный сигнал на выходе комплексной системы по углу тангажа блока ДПИ, радиан; 2) gamma_KS – нормированный сигнал на выходе комплексной системы по углу крена блока ДПИ, радиан. Структурная схема подсистемы «Блок В.3.Схема комплексирования» приведена на рисунке Г.78.

226 Рисунок Г.78 – Структурная схема подсистемы «Блок В.3.Схема комплексирования» Блок В.3 содержит два одинаковых по структуре фильтра, которые существенно подавляют шумовые помехи поступающих на входы фильтров сигналов с выходов инерциальной и дальномерной СУО. В структурной схеме на рисунке Г.79 реализован цифровой вариант фильтра, при переходе к которому передаточная функция преобразуется к виду: W1 ( z ) = ∆t , где ∆t – период дискретности. Tф1 z − (Tф1 − ∆t )

227

Рецензии:

Авторизуйтесь, чтобы добавить рецензию

Отзывы:

Авторизуйтесь, чтобы оставить отзыв

АВТОНОМНАЯ ПЕРСОНАЛЬНАЯ ИНФОРМАЦИОННО-ИЗМЕРИТЕЛЬНАЯ СИСТЕМА НАЗЕМНОГО ПОЗИЦИОНИРОВАНИЯ С КОРРЕКЦИЕЙ УГЛОВ НАКЛОНА ПО ОПОРНОЙ ПОВЕРХНОСТИ

Елена Купоросова

Поделиться работой

Рецензии:

Рецензия от Елена Купоросова

Рецензия от Елена Купоросова

Рецензия от Елена Купоросова

Рецензия от Елена Купоросова

Рецензия от Елена Купоросова

Рецензия от Елена Купоросова

Рецензия от Елена Купоросова

Рецензия от Елена Купоросова

Рецензия от Елена Купоросова

Рецензия от Елена Купоросова

Рецензия от Елена Купоросова

Рецензия от Елена Купоросова

Отзывы: