Евразийское мышление. Вопросы науки и техники. Сборник 1

Таблица 3—1 Настройки параметров интерфейса

Структура протокола связи блока измерения принимает унифицированный формат сообщения, который предусматривает, что эффективная длина данных для передачи составляет 0—255 байт, а общее количество байтов сообщения составляет 5—260 байт. Структура сообщения протокола связи инерциального измерительного блока показана в Таблице 3—2.

Таблица 3—2 Протокол связи с инерционным измерительным блоком

Первый байт (PRE): В качестве стартового байта первый байт представляет собой определенный байт. Первый байт единицы измерения в этом эксперименте – 0xFA, а первый байт блока контроллера – 0x23, 0x73, 0x79.

Адрес устройства (BID): определение адреса устройства, по умолчанию – 0x01.

Тип сообщения (MID): эта строка сегментов байтов в основном показывает, какой тип сообщения используется для отправки командных команд.

Длина данных (LEN): охватывает значение длины всех данных, общее значение не превышает 255. Если он равен 0, символы или нулевые символы не передаются.

Данные (DATA): эта строка сообщений содержит действительные данные соответствующей команды, а диапазон ее данных ограничен длиной данных.

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

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

Команда AHRS0 представляет собой полный вывод исходных данных. MID сообщения непрерывного вывода равен 0x80, а MID сообщения одношагового вывода – 0x81;

Команда AHRS1 представляет собой полный вывод обработанных данных. MID сообщения непрерывного вывода равен 0x82, а MID сообщения одношагового вывода – 0x83;

После получения сообщения измерительный элемент немедленно реагирует, выполняет соответствующую команду и непрерывно (пошагово) выводит соответствующие данные сообщения. Формат вывода сообщения показан в Таблице 3—3.

Таблица 3—3 Формат выходного сообщения

Основная команда, используемая в этом эксперименте, – это непрерывный вывод необработанных данных. Она использует команду AHRS0 и использует MID=0x80 сообщения непрерывного вывода. Структура отправленного сообщения протокола показана в Таблице 3—4.

Таблица 3—4 Команда AHRS0 – структура сообщения режима вывода полных данных (исходные данные)

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

Формат входного сообщения показан в Таблице 3—5.

Таблица 3—5 Формат входного сообщения контроллера

Протокол связи, использованный в этом эксперименте, использует сообщение о настройке поворота и сообщение о настройке скорости двухосного контроллера электрического проигрывателя. Структура протокола связи показана в Таблицах 3—6 и Таблице 3—7 соответственно.

Таблица 3—6 Формат входного сообщения установки скорости

Таблица 3—7 Формат входного сообщения настройки поворота

4. Расчет и фильтрация отношения.

4.1. Метод решения угла наклона. Угол ориентации также называется углом Эйлера, который отражает угол поворота между координатами носителя и географическими координатами. Путем расчета угла ориентации можно описать ориентацию системы. Данные, собранные в этом исследовании, представляли собой чип MPU6050 инерциальной навигационной платформы, поэтому он использовался в качестве носителя, а магнитное поле Земли использовалось в качестве опорной координаты. Система координат, определенная MPU6050, показана на Рисунке 4—1, а географическая система координат показана на Рисунке 4—2.

4.1. Метод решения угла наклона. Угол ориентации также называется углом Эйлера, который отражает угол поворота между координатами носителя и географическими координатами. Путем расчета угла ориентации можно описать ориентацию системы. Данные, собранные в этом исследовании, представляли собой чип MPU6050 инерциальной навигационной платформы, поэтому он использовался в качестве носителя, а магнитное поле Земли использовалось в качестве опорной координаты. Система координат, определенная MPU6050, показана на Рисунке 4—1, а географическая система координат показана на Рисунке 4—2.

Рисунок 4—1 Географическая справочная система координат

Рисунок 4—2 Система координат чипа MPU6050

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

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

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

1. Объект вращается вокруг оси Z. Поскольку ось Z не перемещается, соотношение преобразований x и y показано на рисунке 4—3.

Рисунок 4—3 Координатное соотношение вокруг осей Z, X и Y

Из рисунка 4—3 видно, что

Видно, что соотношение координат в матричной форме, полученное вращением угла вокруг оси z, имеет вид:

2. Угол поворота объекта вокруг оси Y. Поскольку ось Y не перемещается, соотношение трансформаций x и z показано на рисунке 4—4.

Рисунок 4—4 Координатное соотношение вокруг осей Z, X и Y

Из рисунка 4—4 видно, что

3. Объект вращается вокруг оси X. Поскольку ось X не перемещается, соотношения преобразования Y и Z показаны на рисунке 4—5.

Рисунок 4—5 Координатное соотношение вокруг осей Z, X и Y

Из рисунка 4—5 видно, что

Угол ориентации объекта можно рассматривать как совокупность трех углов поворота, где угол поворота вокруг оси z записывается как угол курса, угол поворота вокруг оси y записывается как угол тангажа y, и угол поворота вокруг оси x называется углом крена 0.

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

Сопоставьте параметры уравнений (15) и (16) один за другим, возьмем три элемента в третьей строке уравнения (16), приравняем их g1, g2 и g3 соответственно и возьмем первый элемент уравнения ( 16). Первые два элемента столбца – это g5 и g4 соответственно.

Его значения следующие:

Затем я подставил кватернион в последний момент в формулу угла ориентации (19), чтобы найти эти три угла ориентации. Формула для окончательного получения угла ориентации выглядит следующим образом:

4.2. Решение параметров кватернионов.

Затем преобразуйте его в матричную форму и получим уравнение (24).

4.3. Процесс фильтрации EKF. Расширенный фильтр Калмана (EKF) – это прикладное расширение фильтра Калмана, которое использует данные наблюдений системы для оптимальной оценки состояния нелинейной системы и фильтрации помех, таких как шум и дрейф. Этот алгоритм использует локальную линеаризацию для решения нелинейных задач путем вывода нелинейных уравнений.

Затем мы инициализируем фильтр и устанавливаем матрицу шума процесса Q, матрицу ошибок измерения R, матрицу состояний X и ковариационную матрицу P.

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