Министерство образования Российской Федерации
Воронежский Государственный Университет
РЕШЕНИЕ КИНЕМАТИЧЕСКОЙ ЗАДАЧИ
ОРИЕНТАЦИИ ТВЕРДОГО ТЕЛА В ПРОСТРАНСТВЕ ДЛЯ
ПОСТРОЕНИЯ СИСТЕМЫ ИНЕРЦИАЛЬНОЙ НАВИГАЦИИ.
ЧАСТЬ 1
Учебно-методическое пособие для студентов 3 – 4х курсов очной
формы обучения по направлениям: «Механика и математическое
моделирование» и «Прикладная математика и информатика»
Воронеж
2017
Стр.1
Содержание
Введение……………...…………………………………………………………...4
Глава 1. Инерциальные датчики. .................................................................... 4
1.1. Трехосевой микромеханический акселерометр. ....................................... 4
1.2 Трехосевой микромеханический гироскоп ................................................ 7
Глава 2. Определение ориентации твердого тела в пространстве
кватернионным и векторным способами. .................................................... 11
2.1 Углы Крылова. ........................................................................................... 11
2.2 Кинематические уравнения Эйлера. ........................................................ 12
2.3 Уравнения Пуассона ................................................................................. 12
2.4 Кинематические уравнения, записанные в кватернионах....................... 13
2.4.1 Краткие сведения из математики кватернионов ............................... 13
2.4.2 Операции над кватернионами............................................................ 14
2.5 Ориентация твердого тела в трехмерном пространстве с
использованием кватернионов ....................................................................... 15
2.6 Фильтрация данных. Комплементарный фильтр. ................................... 18
2.7 Использования векторного способа для описания поворота твердого
тела в пространстве. ........................................................................................ 19
Глава 3. Алгоритм Маджвика ........................................................................ 21
3.1 Основные аспекты метода градиентного спуска. .................................... 21
3.2 Использование угловой скорости. ............................................................ 22
3.3 Использование линейных ускорений ....................................................... 22
3.4 Алгоритм объединения данных. ............................................................... 24
Приложение. ...................................................................................................... 28
Список литературы…………………………………………………………… 39
3
Стр.3
Рисунок 2. Блок-схема ММА
На неподвижные пластины конденсатора ЧЭ от генератора подаются
противофазные прямоугольные импульсы с частотой следования 1 МГц,
амплитуды которых равны, но сдвинуты по фазе на 180°. В начальном
(спокойном) состоянии емкости конденсаторов С1 и С2 одинаковы, поэтому
выходное напряжение на их электрическом центре (или на подвижной
пластине, присоединенной к ИМ) равно 0 (рисунок 2).
Когда ИМ начинает двигаться, разность емкостей приводит к
появлению выходного сигнала на центральной пластине, амплитуда которого
будет расти по мере увеличения ускорения, приложенного к ИМ. Усилитель
У1 служит для связи центральной пластины с синхронным детектором. Так
как направление движения ИМ влияет на фазу сигнала, синхронное
детектирование необходимо для отслеживания информации об амплитуде.
Выход синхронного детектора через усилитель У2 обеспечивает выходное
напряжение ускорения V.
Разработкой и серийным производством ММА занимаются фирмы:
Motorola, Hitachi, Analog Devices и другие. Наиболее широким спектром
моделей и характеристик обладают ММА серии ADXL (Analog Devices).
Ниже для примера представлена модель ADXL 335 и ее характеристики
(рисунок 3).
6
Стр.6
Рисунок 3. ММА ADXL 335
Технические характеристики ММА ADXL 335:
• Измерение по трем осям,
• Миниатюрный, низкопрофильный корпус LFCSP,
• Размер 4 мм Ч 4 мм Ч 1.45 мм,
• Малый потребляемый ток: 350 мкА,
• Однополярное питание от 1.8 В до 3.6 В,
• Выдерживает ударные нагрузки до 10000 g,
• Регулировка ширины полосы при помощи одного конденсатора на ось.
1.2 Трехосевой микромеханический гироскоп
Все гироскопы делятся на два основных класса: свободные гироскопы
(астатические), задача которых заключается в хранении заданного
направления или системы координат, и датчики измерения угловой скорости
(ДУС) объекта, на котором они установлены. Для астатических гироскопов
основным показателем точности является остаточная скорость ухода (дрейф)
гироскопа, а для ДУС – погрешности измерения угловой скорости,
обусловленные различными причинами ([5], [7]).
Микромеханические гироскопы (ММГ) относятся к классу
астатических гироскопов и являются абсолютными лидерами по минимуму
массы, габаритов, потребления ресурсов и стоимости. ММГ, так же как и
ММА, представляет из себя конструкцию, в которой механически
7
Стр.7
подвижные узлы, реализующие ЧЭ ММГ, а также сервисная электроника
образуют единый чип, созданный с помощью технологий МЭМС.
ММГ – электромеханическая система, в которой энергия вынужденных
(первичных) колебаний инерционной массы (ИМ) на упругом подвесе
(резонатор) при появлении переносной угловой скорости преобразуется в
энергию вторичных колебаний, содержащую информацию об искомой
угловой скорости. Это преобразование осуществляется вследствие
воздействия на резонатор сил (моментов) инерции Кориолиса при вращении
резонатора с переносной угловой скоростью, вектор которой
перпендикулярен вектору количества движения, или момента количества
движения (кинетического момента), соответственно для поступательных и
вращательных первичных колебаний ИМ.
Первичные колебания называют также режимом движения (РД), или
движением по координате возбуждения, а вторичные – режимом
чувствительности (РЧ), или движением по координате выходного сигнала.
По виду движения ИМ в РД и РЧ различают гироскопы:
• LL-гироскопы (linear-linear),
• RR-гироскопы (rotate-rotate),
• LR(RL)-гироскопы.
Рассмотрим подробнее устройство ММГ (рисунок 4).
Рисунок 4. Устройство ММГ
Конструкция ММГ образована соединением двух рамок: 1 – внешний
8
плоский элемент, 2 – внутренний. Они соединены между собой и с
основанием 6 с помощью торсионов 3 и 4, оси которых перпендикулярны
друг к другу. Для увеличения инерционности на внутреннем элементе
Стр.8
расположена дополнительная масса 5. С помощью электростатических
датчиков силы наружному элементу 1 сообщаются первичные угловые
колебания (РД) относительно оси 𝑌. Эти колебания передаются внутреннему
элементу через торсионы 3, сообщая ему колебательный кинетический
момент Н. При вращении прибора со скоростью ω𝑧 относительно оси 𝑍
возникают моменты сил Кориолиса 𝐻ω𝑧, которые заставляют колебаться
внутренний элемент (РЧ) относительно оси 𝑋. Амплитуда этих колебаний,
измеряемая расположенным под внутренним элементом емкостным
преобразователем, будет пропорциональна измеряемой угловой скорости.
Движение ЧЭ в простейшем случае описывается следующими
соотношениями
�𝐽𝛾𝛾̈ + 𝑏𝛾𝛾̇ + 𝐺𝛾𝛾 = 𝑀0 sin 𝑝𝑡,
𝐽𝛼𝛼̈ + 𝑏𝛼𝛼̇ + 𝐺𝛼𝛼 = 𝐻𝜔,
(1.2)
где 𝛾 – угол колебания рамок 1 и 2 относительно оси 𝑌, 𝛼 – угол колебаний
рамки 2 относительно оси 𝑋, 𝐽𝛾 – суммарный момент инерции тел 1, 2, 5
относительно оси 𝑌, 𝐽𝛼 – суммарный момент инерции тел 2, 5 относительно
оси 𝑋, 𝑏𝛾 – коэффициент демпфирования рамок 1, 2 при колебании
относительно оси 𝑌, 𝑏𝛼 – коэффициент демпфирования рамки 2 при
колебании относительно оси 𝑋, 𝐺𝛾, 𝐺𝛼 – жесткости на кручение торсионов 4
и 3 соответственно. Величина 𝑀0 sin 𝑝𝑡 – момент привода РД, где 𝑀0 – его
амплитуда, а 𝑝 – частота.
Одним из наиболее популярных и удачных решений в области
датчиков МЭМС является совмещение нескольких приборов на одной
микросхеме, так как это позволяет снизить погрешность приборов, а так же
упростить общение между схемой и командной машиной, что позволяет
снизить затраты времени. Ярким примером такой микросхемы является
MPU6050 ([8], [9]), которая содержит в своем составе не только гироскоп и
акселерометр, но и температурный датчик.
9
Стр.9
Рисунок 5. Модуль МЭМС MPU6050
Технические характеристики МЭМС MPU6050:
• Компактный размер 21×16×3 мм,
• Трех осевой MEMS гироскоп с 16 битным АЦП,
• Трех осевой MEMS акселерометр с 16 битным АЦП,
• Цифровая обработка данных Digital Motion Processor (DMP),
• Интерфейс slave I2C для подключения к микроконтроллеру,
• Интерфейс master
I2C
дополнительного датчика,
• Температурный сенсор,
• Самопроверка гироскопа и акселерометра.
для
подключения
к
микросхеме
10
Стр.10