Многоканальная распределённая система управления пространственным положением объекта на базе безинерционной системы навигации

Размещено на http://www.allbest.ru/

Размещено на http://www.allbest.ru/

ФЕДЕРАЛЬНОЕ ГОСУДАРСТВЕННОЕ АВТОНОМНОЕ

ОБРАЗОВАТЕЛЬНОЕ УЧРЕЖДЕНИЕ ВЫСШЕГО ПРОФЕССИОНАЛЬНОГО ОБРАЗОВАНИЯ «НАЦИОНАЛЬНЫЙ ИССЛЕДОВАТЕЛЬСКИЙ УНИВЕРСИТЕТ «ВЫСШАЯ ШКОЛА ЭКОНОМИКИ»

Московский институт электроники и математики им. А.Н. Тихонова

Выпускная квалификационная работа

Многоканальная распределённая система управления пространственным положением объекта на базе безинерционной системы навигации

по направлению 11.03.02 Инфокоммуникационные технологии и системы связи

студента образовательной программы бакалавриата «Инфокоммуникационные технологии и системы связи»

Кожевникова Оксана Владимировна

Москва, 2018 г.

• Оглавление
• Введение
• Глава 1. Теоретические основы инерциальной навигации
• 1.1 Спутниковая навигация
• 1.2 Инерциальная навигация
• 1.2.1 Акселерометр
• 1.2.2 Гироскоп
• 1.3 Расчет местоположения
• 1.4 Ориентация
• 1.4.1 Углы Эйлера
• 1.4.2 Кватернионы
• 1.4.3 Перевод
• 1.5 Задача подавления помех и ошибок измерения
• Глава 2. Разработка программного обеспечения навигационной системы
• 2.1 Требования к приложению
• 2.2 Используемые устройства
• 2.3 Описание программного обеспечения
• 2.4 Обработка сигналов
• 2.5 Описание работы программы
• 2.6 Вывод информации пользователю
• Глава 3. Экспериментальные данные
• Выводы
• Заключение
• Список литературы
• Введение
• Определение пространственных координат объекта является самостоятельной и вспомогательной задачей для многих приложений. В самом общем смысле задача навигации сводится к определению координат местоположения движущегося тела, другой задачей является определение ориентации объекта. Решение данных задач применяется для стабилизации движения, управления механизмами, ведения различных объектов по заданному маршруту. Для управления и навигации морскими и воздушными судами, автомобилями, беспилотными летательными аппаратами необходимо использование различных навигационных систем.
• Целью данной работы является создание навигационной системы, которая будет определять ориентацию и положение тела. Система будет построена на инерциальном измерительном блоке, содержащем простые датчики: акселерометр, гироскоп и магнитометр, позволяющие непосредственно измерять линейное ускорение и угловую скорость устройства. Использование данной информации позволит рассчитать параметры движения, ориентацию и местоположение объекта.
• Используемый комбинированный датчик Trema-модуль на базе чипа BMX055 будет управляться контроллером ESP32 на плате NodeMSU. Основной задачей диплома является написание программного обеспечения для контроллера, которое обеспечит чтение и фильтрацию данных с датчика, перевод и отображение информации на Web-сервере, созданном на контроллере. Окончательным результатом работы является готовая завершенная программа для контроллера, которая позволит рассчитывать ориентацию тела в пространстве и может быть использована в различных приложениях.
• Глава 1. Теоретические основы инерциальной навигации
• Навигация - определение местоположения, скорости, ориентации и направления движения объектов посредством датчиков или внешних источников, процесс управления некоторым объектом в пространстве. Точное определение ориентации в пространстве играет важную роль во многих областях, в числе которых: аэрокосмическая, робототехника, навигация, анализ движений человека, взаимодействие человеческих и машинных движений. Навигационные системы являют собой комплекс приборов, алгоритмов и программ, позволяющих произвести ориентирование объекта в пространстве.
• Различные технологии и системы используются для определения ориентации и положения, в частности, GPS и ГЛОНАСС. Такие системы не являются автономными и требуют обеспечения прямой видимости тела спутником. Другим средством измерения ориентации являются инерциальные измерительные системы, основанные на инерционных измерительных блоках (IMU), которые не требуют ввода информации извне. Сегодня оба типа систем используются, например, для управления автомобилем по запланированному маршруту. Хотя спутник определяет положение на открытых участках местности, IMU подхватывает и продолжает управление автомобилем в туннелях или при высокой облачности.
• Рисунок 1. Спутниковая и инерциальная системы навигации
• 1.1 Спутниковая навигация
• Спутниковые навигационные системы - это сложный комплекс устройств, включающий космическое оборудование и наземный сегмент, как и все навигационные системы, предназначенный для вычисления местоположения. На основе данных о координатах двух точек и времени перемещения между ними можно вычислить другие параметры движения: скорость, курс, пройденный путь.
• Спутниковый приемник определяет свое местоположение, используя информацию о расположении спутников, которое известно с большой точностью, и расстоянии до них. Полагая скорость распространения радиоволн известной, вычисляется время распространения сигнала, а исходя из полученных данных и расстояние до спутника. Предварительно часы приемника синхронизируются с точными часами спутника, что и дает в дальнейшем возможность вычислить задержку между временем приема сигнала и временем излучения со спутника, содержащемся в самом сигнале. Для того чтобы вычислить положение объекта в пространстве, достаточно иметь информацию о расстояниях до трех спутников, если высота над поверхностью земли известна, иначе требуются сигналы от четырех спутников. (Рис. 2)
• Рисунок 2. Определение координат по спутникам.
• Достоинствами данных навигационных систем являются неограниченная дальность действия, высокая точность определения положения неограниченного количества клиентов, моментальная готовность к работе.

1.2 Инерциальная навигация

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

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

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

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

1.2.1 Акселерометр

Акселерометр - прибор, измеряющий параметр линейного ускорения, представляющий собой чувствительный элемент (Рис. 3), состоящий из массы, закрепленной в упругом подвесе, отклоняющейся при приложении к объекту внешних сил и возникновении ускорения. Акселерометр измеряет проекцию на ось чувствительности кажущегося, то есть без учета постоянно действующей гравитации, ускорения своего основания. Чувствительный элемент отклоняется от своего положения равновесия при приложении к нему внешней силы, которая в свою очередь, зависит от ускорения объекта. Таким образом, чем больше перемещение массы, тем более ускорение движущегося тела.

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

Рисунок 3. Физический принцип работы акселерометра

1.2.2 Гироскоп

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

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

Рисунок 4. Конструкция механического гироскопа

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

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

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

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

1.3 Расчет местоположения

Одной из задач навигации является определение параметров движения тела как материальной точки. Положение объекта чаще всего представляется географическими координатами: ц - широта, л - долгота, h - высота над уровнем моря. Географическая система координат OX_gY_gZ_g задается следующим образом: OX_g сориентирована по меридиану на север, OY_g вертикально вверх, OZ_g по параллели на восток. Точка O совпадает с центром масс движущегося объекта.

При движении точки O со скоростью V относительно поверхности Земли проекции V на оси географической системы координат будут равны:

,

Где V_r - горизонтальная составляющая скорости, К - курс.

Угловые скорости при передвижении центра координат O можно представить как приращение координат:

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

(1)

Текущие координаты объекта получим интегрированием скоростей:

(2)

Для любой ИНС необходимо учитывать начальные проекции скоростей и координаты.

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

1.4 Ориентация

Следующей задачей навигации является определение ориентации как параметров углового движения вокруг его центра масс.

Для измерения ориентации относительно географической системы координат вводится связанная система координат OXYZ, ось OX которой направлена вдоль летательного аппарата к носу, OY направлена перпендикулярно плоскости крыльев вверх, OZ направлена вдоль поперечной оси в сторону правого крыла.

1.4.1 Углы Эйлера

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

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

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

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

1.4.2 Кватернионы

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

Кватернионы имеют вид Л = л₀ + iл₁ + jл₂ + kл₃, где i, j, k - мнимые единицы, которые вместе с одной действительной единицей задают четырехмерный базис, л₀, л₁, л₂, л₃ - параметры Родрига-Гамильтона, которые в свою очередь задаются соотношениями:

Пусть некоторая система координат поворачивается относительно некоторой неподвижной системы Oxyz вокруг оси u, - направляющая ось, на угол ц, тогда l, m, n - косинусы углов между направляющей осью и осями Ox, Oy, Oz. Причем выполняется тождество

,откуда следует, что

Рассмотрим пример, каким образом из последовательности поворотов вычисляется конечный поворот.

Пример.

Пусть имеется географическая система координат OX_gY_gZ_g и связанная система координат OXYZ, совпадающие в начальный момент..

1) Первый поворот OXYZ происходит на угол курса ш вокруг оси OY_g.



Рисунок 5. Первый поворот

Кватернион, характеризующий поворот, выглядит так:

2) Поворот на угол тангажа и вокруг оси OZ_g:

Кватернион:



Рисунок 6. Второй поворот

3) Поворот на угол крена г вокруг оси OX_g:

Рисунок 7. Последний поворот

Направляющие косинусы и кватернион:

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

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

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

1.4.3 Перевод

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

Из данной матрицы легко найти углы ориентации:

С другой стороны, можно выразить направляющие косинусы осей связанной системы координат через направляющие косинусы осей конечного поворота l, m, n и угол поворота. В итоге матрица косинусов углов между осями координат будет иметь вид:

,

который путем несложных преобразований можно представить через параметры Родрига-Гамильтона:

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

(3)

1.5 Задача подавления помех и ошибок измерения

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

1. Погрешности ввода начальных условий;

2. Погрешности чувствительных элементов;

3. Помехи канала связи.

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

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

Рисунок 8. Ошибки, вызванные неточным заданием начального положения: ошибка построения вертикали, определения скорости, определения пройденного расстоянии 1 - в₀ = 1 угл. мин.; 2 - в₀ = 2 угл. мин.; 3 - в₀ = 3 угл. мин.

Рисунок 9. Ошибки, вызванные смещением нуля акселерометра: ошибка построения вертикали, определения скорости, определения пройденного расстоянии 1 - дax = 10-4 g.; 2 - дax = 5*10-4 g.; 3 - дax = 10-3 g.

Рисунок 10. Ошибки, вызванные дрейфом гироскопа: ошибка построения вертикали, определения скорости, определения пройденного расстоянии 1 - е = 0.01 град/ч.; 2 - е = 0.05 град/ч.; 3 - е = 0.1 град/ч.

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

Погрешности чувствительных элементов и канала представляют собой случайный процесс. Универсальным способом борьбы с такого рода шумами является цифровая фильтрация сигналов. Реализация фильтров может быть не только программная, но и аппаратная, что менее удобно с точки зрения перестройки параметров фильтра и универсальности. Задача фильтрации сигналов в ИНС рассмотрена в ряде статей и монографий [1], [6], [7].

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

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

Глава 2. Разработка программного обеспечения навигационной системы

2.1 Требования к приложению

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

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

1. Инициализировать, протестировать, откалибровать и запустить работу датчиков;

2. Получить сигналы с датчиков;

3. Обработать данные и перевести (представить) их в удобный для пользователя вид (углы Эйлера, истинное ускорение);

4. Подключиться к локальной сети и создать в ней web-сервер на базе микроконтроллера;

5. По запросу клиента предоставлять измеренные значения на HTTP-странице.

2.2 Используемые устройства

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

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

Рисунок 11. Принципиальная схема MEMS-акселерометра

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

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

Рисунок 12. Схема магнитно-резистивного преобразователя

Через интерфейс I2C датчик подключается к контроллеру ESP32 (Рис. 13), имеющему мощные вычислительные способности. Он создан для решения широкого спектра задач, которые могут возникнуть у пользователя. Процессор с частотой 240 MHz и объемом оперативной памяти 520 KByte, позволяет реализовывать даже большие и громоздкие проекты. В нем присутствует собственная энергонезависимая память 448KByte.

Рисунок 13. Комбинированный датчик и микроконтроллер

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

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

2.3 Описание программного обеспечения

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

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

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

2.4 Обработка сигналов

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

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

Рисунок 14. Объект в гравитационном поле

Выходными данными датчиков являются:

· акселерометра -- кажущееся ускорение в количестве ускорений свободного падения g;

· гироскопа -- угловая скорость в градусах в секунду;

· магнитометра -- магнитная индукция в миллигауссах.

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

2.5 Описание работы программы

Результатом выполнения предыдущей части программы стали проекции истинного ускорения на три оси географической системы координат. В разделе «Определение местоположение» были выведены формулы (1) и (2) для расчета скорости и пути, пройденного объектом.

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

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

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

Рисунок 15. Метод прямоугольников

Значение интеграла на элементарном отрезке [a,b] можно найти по формуле:

Составная формула прямоугольников имеет вид:

(4)

Рассмотрим, как можно реализовать этот расчет программно.

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

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

2.6 Вывод информации пользователю

Среди поставленных в работе задач был вывод рассчитанной ориентации пользователю по локальной сети средствами встроенного в контроллер Wi-Fi-модуля.

Чтобы наладить такой вывод информации, для начала нужно настроить на контроллере web-сервер. В Arduino IDE имеется библиотека для работы с в Wi-Fi-модулями, в которой предусмотрен класс WiFiServer() для создания web-серверов. Создается экземпляр класса, которому компилятор присваивает IP-адрес. При обращении клиента по IP-адресу, выводится для просмотра HTML-страничка, на которой отображается требуемая информация.

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

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

инерциальный навигация программный

Глава 3. Экспериментальные данные

В соответствии с заданием в предыдущей главе было написано и отлажено программное обеспечение, текст которого приведен на страницах 31-32. В нем реализованы процессы чтения и обработки данных, расчета параметров, предоставления информации человеку.

Типовая структура скетча в среде Arduino содержит два обязательных блока: setup() и loop(). Функция setup() вызывается в начале работы программы, используется для инициализации переменных, запуска библиотек и для любых других операций, которые требуется выполнить один раз. В моей программе эта функция устанавливает соединение с компьютером для обмена отладочной информацией, подключает плату к Wi-Fi-сети. Далее запускается созданный сервер, включается и калибруется датчик и фиксируется текущее время.

Вторая функция loop() выполняется в цикле все время, пока работает процессор. В нее помещают команды, которые должны выполняться постоянно. Поэтому включенное устройство автономно и не требует от пользователя активных действий. При его запуске с датчика снимаются показания в виде напряжений на пинах, информация о величине которых передается по шине I2C в процессор. Далее все сигналы подаются на вход цифрового фильтра Маджвика. Данные из фильтра запрашиваются об ориентации в градусах и сразу же, чтобы не пересчитывать, об ускорении объекта в м/с². Алгоритм фильтра составляют следующие операции: нормализация всех входных параметров, вычисление поправочных коэффициентов и коррекция измерений, нахождение скорости изменения кватернионов и определение значения в точке.

Проекции ускорения используются для расчета скорости и координаты по формуле (4), где f - ускорение или скорость, а Дx - промежуток времени dt. При начальном выполнении программы рассчитываются значения в нулевой момент времени, для дальнейшего расчета dt будет зафиксирован после выполнения всей программы перед следующим снятием показаний.

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

Считывается запрос удаленного клиента, по его окончании посылается ответ. Интерфейс при работе с устройством выглядит как HTML-страница в окне браузера (Рис. 16), которая содержит результаты измерений в виде таблицы с текущими параметрами движения устройства. В строки таблицы вписаны измеренные параметры ориентации, ускорения, скорости и координаты, колонки показывают ось, по которой сделано измерение.

Рисунок 16. Интерфейс пользователя

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

#include <Wire.h>

#include <WiFi.h>

#include <iarduino_Position_BMX055.h>

iarduino_Position_BMX055 sensor(BMX);

double timing = 0; double dt = 0;

float pitch = 0;float roll = 0;float yaw = 0;// углы Эйлера

double vx = 0;double vy = 0;double vz = 0;// проекции скоростей

double sx = 0;double sy = 0;double sz = 0;// координаты

char ssid[] = "Login";

char password[] = "Password";

WiFiServer server(80);s

char linebuf[80]; int charcount = 0;

void setup() {

Serial.begin(115200);

while (!Serial){}// Ожидание подключения последовательного порта

Serial.print("Connecting to ");

Serial.println(ssid);

WiFi.begin(ssid, password);

while (WiFi.status() != WL_CONNECTED) {// Попытаться подключиться в Wi-Fi-сети

delay(500);// Соединение с WPA/WPA2-сетью

Serial.print(".");

}

Serial.println("WiFi connected");

Serial.println("IP address: ");

Serial.println(WiFi.localIP());

server.begin();

sensor.begin();

sensor.setFastOffset();

timing = micros();

}

void loop() {

sensor.read(BMX_DEG);

pitch = sensor.axisX;roll = sensor.axisY;yaw = sensor.axisZ;

sensor.read(BMX_M_S);

vx += sensor.axisX*dt;vy += sensor.axisY*dt;vz += sensor.axisZ*dt;

sx += vx*dt;sy += vy*dt;sz += vz*dt;

WiFiClient client = server.available();

if (client) {

Serial.println("New client");

memset(linebuf, 0, sizeof(linebuf));

charcount = 0;

boolean currentLineIsBlank = true;// http-запрос оканчивается пустой строкой

while (client.connected()) {

if (client.available()) {

char c = client.read();

Serial.write(c);

linebuf[charcount] = c;// Чтение http-запроса от клиента

if (charcount<sizeof(linebuf) - 1) charcount++;

if (c == '\n' && currentLineIsBlank) {// В конце запроса пустая строка

client.println("HTTP/1.1 200 OK");// Отправка заголовка

client.println("Content-Type: text/html");// http-ответа

client.println("Connection: close");// Соединение будет

client.println();// закрыто после отправки ответа

client.println("<!DOCTYPE HTML><html><head>");

client.println("<meta name=\"viewport\" content=\"width=device-width, initial-scale=1\"> </head>");

client.println("<h1>IMU - Web Server</h1>");

client.println("<table>");

client.println("<tr><td>Parameter</td><td>| X</td>");

client.println("<td>| Y</td><td>| Z</td></tr>")

client.println("<tr><td>Orientation (deg)</td><td>| ");

client.println(pitch);

client.println("</td><td>| ");

client.println(roll);

client.println("</td><td>| ");

client.println(yaw);

client.println("</td></tr>");

client.println("<tr><td>Acceleration (m/s^2)</td><td>| ");

client.println(sensor.axisX);

client.println("</td><td>| ");

client.println(sensor.axisY);

client.println("</td><td>| ");

client.println(sensor.axisZ);

client.println("</td></tr>");

client.println("<tr><td>Velosity (m/s)</td><td>| ");

client.println(vx);

client.println("</td><td>| ");

client.println(vy);

client.println("</td><td>| ");

client.println(vz);

client.println("</td></tr>");

client.println("<tr><td>Coordinate (m)</td><td>| ");

client.println(sx);

client.println("</td><td>| ");

client.println(sy);

client.println("</td><td>| ");

client.println(sz);

client.println("</td></tr></table></html>");

break;

}

if (c == '\n') {

currentLineIsBlank = true;// Начало новой строки

memset(linebuf, 0, sizeof(linebuf));

charcount = 0;

}

else if (c != '\r') {

currentLineIsBlank = false;// Получен символ

}// в текущей строке

}

}

delay(1);

client.stop();// Закрытие соединения

Serial.println("client disconnected");

dt = (micros() - timing) / 1000000;

timing = micros();

}

}

Выводы

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

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

3. Описаны используемые устройства, продуман алгоритм программы, обеспечивающей работу с ними.

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

Заключение

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

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

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

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

Список литературы

1. Матвеев В.В., Распопов В.Я. Основы построения бесплатформенных инерциальных навигационных систем. - СПб.: ГНЦ РФ ОАО «Концерн «ЦНИИ «Электроприбор», 2009. - 280 с.

2. Анучин О.Н., Емельянцев Г.И. Интегрированные системы ориентации и навигации для морских подвижных объектов.

3. Селиванова Л.М., Шевцова Е.В. Инерциальные навигационные системы: учебное пособие. - М.:Изд-во МГТУ им. Н.Э. Баумана, 2012. - 46, [2] с.

4. Распопов В.Я. Микромеханические приборы. - М.: Приборостроение, 2007. - 400 с.

5. Ишлинский А.Ю. Ориентация, гироскопы и инерциальная навигация. - М.: Наука, 1976. - 672 с.

6. Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan. “Estimation of IMU and MARG orientation using a gradient descent algorithm”, 2011 IEEE International Conference on Rehabilitation Robotics Rehab Week Zurich, ETH Zurich Science City, Switzerland, June 29 - July 1, 2011.

7. Z. F. Syed, P. Aggarwal, C. Goodall, X. Niu and N. El-Sheimy, “A new multi-position calibration method for MEMS inertial navigation systems”, J. Meas. Sci. Technol. no. 18, pp. 1897-1907, May 2007.

8. Jack C. K. Chou, M. Kamel, “Finding the Position and Orientation of a sensor on a Robot Manipulator Using Quaternions”, The International Journal of Robotics Research, pp. 240-254.

9. 3DNews Daily Digital Digest / Алексей Дрожжин. MEMS: микроэлектромеханические системы, часть 1. URL:https://3dnews.ru/600098

10. IMU-сенсор на 9 степеней свободы (Trema-модуль V2.0).

Размещено на Allbest.ru