Как скомпенсировать отклонение угла при вращении гироскопа?

Question

[Het_Masteen]

При использовании фильтра Маджвика для гироскопа-акселерометра наблюдается проблема – происходит плавное изменение угла yaw(вокруг оси Z) на десятки градусов при вращении, если ось вращения расположена между осями X и Y. С чем может быть связана данная проблема и как можно скомпенсировать отклонение угла yaw?

Answer 1

[hint000]

Очень интересно, но непонятно. Девушка, если вы сократите вопрос с двух предложений до одного, то уровень непонятности останется тем же самым. :)
Не стесняйтесь добавлять подробности, если действительно хотите получить полезный ответ.
Какого рода проблема?:

• С конкретным устройством? Так оно не названо. Характерна ли проблема только для этого устройства?
  
• С условиями использования устройства? Про них ни слова.
  
• С алгоритмом обработки данных, получаемых с устройства? Так надо проанализировать исходные данныые; на алгоритм можно пенять только при уверенности, что с исходными данными всё в порядке.
  

Если вы имеете в виду, что проблема алгоритмическая\математическая, то посмотрите вот это:
https://ru.wikipedia.org/wiki/Складывание_рамок

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

Я не вникал в конкретный алгоритм, но, судя по тому, что в комментариях обсуждаются ошибки в алгоритме https://diydrones.com/forum/topics/madgwick-imu-ah...
...а на Хабре переводчик пишет: "Мною найдено более 10 опечаток в оригинальном тексте." https://habr.com/ru/articles/255661/
.., то оригинал был далёк от совершенства, и количество ошибок может зависеть от источника, из которого вы брали алгоритм или готовый код. Исправлены ли ошибки на https://github.com/xioTechnologies/Fusion - это ещё надо выяснять. Также учтите, что некоторые авторы сознательно публикуют исходный код с ошибками, чтобы повысить порог входа, т.е. чтобы код не использовали те, кто не способен его исправить.

Comments

[Het_Masteen]

Это первый подобный проект, поэтому сравнивать не с чем. Специалистов, к которым можно обратиться нет.

Задача.
Прежде всего нужен немагнитный компас, который инициализируется вначале и держит определенное направление на протяжении некоторого времени практически в любых положениях (наклон в 90 +-5 градусов не рассматривается). Пытался использовать алгоритмы Махони и Маджвика, Калман оказался слишком тяжел в реализации и настройке.
Проблема.
Алгоритмы Маджвика и Махони ведут себя примерно одинаково, а именно, при вертикальном повороте устройства происходит смещение курса, в зависимости от оси поворота.
График зависимости отклонения по оси Z в зависимости от угла наклона

1. Зеленый – поворот вокруг оси Y
2. Желтый – поворот вокруг оси Y + 20 градусов
3. Серый – поворот вокруг оси Y + 40 градусов
…
4. Голубой – поворот вокруг оси X

Используемый вариант кода алгоритма Маджвика.
Сейчас код обновления кватерниона такой:
class TMadgwickFilter …
void __fastcall Update(TVector3_t G, TVector3_t A)
{
A.Norm();
TQuaternion_t S(
-2.0f * (FQ.Y * A.X - FQ.X * A.Y - FQ.W * A.Z),
2.0f * (FQ.Z * A.X + FQ.W * A.Y - FQ.X * A.Z),
-2.0f * (FQ.W * A.X - FQ.Z * A.Y + FQ.Y * A.Z),
2.0f * (FQ.X * A.X + FQ.Y * A.Y + FQ.Z * A.Z));
S.Norm();
S = S * FBeta;

TQuaternion_t QDot(
-FQ.X * G.X - FQ.Y * G.Y - FQ.Z * G.Z,
FQ.W * G.X + FQ.Y * G.Z - FQ.Z * G.Y,
FQ.W * G.Y - FQ.X * G.Z + FQ.Z * G.X,
FQ.W * G.Z + FQ.X * G.Y - FQ.Y * G.X);
QDot = QDot * 0.5f - S;
FQ = FQ + QDot * Fdt;
FQ.Norm();
}
Углы Эйлера извлекаю так:
TVector3_t __fastcall GetEiler()
{
return TVector3_t(
atan2(2. * (W * X + Y * Z), 1. - 2. * (X * X + Y * Y)),
asin(2. * (W * Y - Z * X)),
atan2(2. * (W * Z + X * Y), 1. - 2. * (Y * Y + Z * Z)));
}
Это основные фрагменты кода.
В остальном фильтр устраивает, т.к. в вертикальных положениях данные возвращаются в исходное состояние стабильно при любых поворотах.
Я так понял, что отклонение связано с физикой или с особенностями алгоритма Маджвика. Отсюда и возник вопрос, возможно ли как-то, относительно точно, скомпенсировать изменение курса при наклонах или я хочу невозможного. Возможно есть другие пути решения задачи.
Датчик.
Используемый датчик DMU11.

[hint000]

Het_Masteen, по ссылке на Википедию читали? Там хорошо объясняется голубая линия на вашем графике. Объясняется, почему чисто математически неизбежны подобные скачки на 180 градусов для одной из осей. Я же специально дал цитату оттуда.

Т.е. на этом алгоритмическом уровне вам нужно смитриться с этим эффектом, достаточно помнить, что такой эффект существует. Бороться с этим можно на более высоком уровне, там где понятна цель использования этих данных, где не просто абстрактые циферки для трёх осей, а за ними стоит какой-то прикладной смысл. Вот там уже отталкиваясь от смысла, от того, как эти циферки используются, можно придумать тот или иной способ обойти этот эффект. Ну вот возьмём простой компас с одной осью; вот стрелка приближается к 360 градусам - прыг - и она на нуле градусов: и вас же не напрягает этот скачок между 0 и 360. Потому что вы понимаете, что по-другому и не бывает. В случае с тремя осями это не так интуитивно, труднее для понимания, но суть похожая - чисто математечиски "по-другому и не бывает".

И нет, в алгоритм Маджвика я не вникал (потребовалось бы время, чтобы погрузиться в это), мои выводы сейчас основаны на вашем графике, он выглядит довольно естественно. Т.е., скорее всего, полученные вами результаты правильные, просто они неожиданные для вас.

[Het_Masteen]

hint000, большое спасибо. Мне нужно было знать что я иду в правильном направлении

Answer 2

[Николай Китаев]

Присоединюсь к предыдущему оратору - слишком мало условий.

Если речь идет об уходе гироскопа на 15°/час, то это нормально - вы наблюдаете вращение планеты. Если еще по каким то условиям, то озвучьте их. Или ссылаясь на "если ось вращения расположена между осями X и Y." имеете ввиду что то совсем другое?

С суточным уходом гироскопа борются путем введения коррекции разными способами, и опять же в зависимости от условий эксплуатации, назначения и места применения. Если речь идет о дроне, то используют достаточно банальные методы - курсовой угол исчисляемый по ГНСС, или по магнитометру, так как там время работы исчисляется несколькими 10 минут. Если требуется до нескольких часов, то все равно точность работы ИНС как источника навигации очень низка и используют другие методы навигации. Если в ваших условиях "другие" методы не приемлимы, то тогда опять же - коррекция по имеющимся в доступе источникам, а ими может выступать что угодно. От радиостанции "Маяк" и базовых станций вышек сотовой связи до звезд и Луны.

Comments

[hint000]

Николай Китаев, в комментариях тема раскрывается лучше, чем в вопросе. Речь не о суточном дрейфе гироскопа, а об эффекте складывания рамок (который работает и при отсутствии рамок как таковых). Представьте себе, что один самолёт выполнил полупетлю, а другой выполнил полубочку. После этих фигур они в одинаковом положении. Гироскоп может показать поворот на 180 градусов вокруг оси X или такой же поворот вокруг двух других осей - Y и Z, т.е. одинаковому положению соответствуют два теоретически возможных показания гироскопа - и оба правильные.

[Николай Китаев]

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

[hint000]

Николай Китаев, на самом деле нет, на уровне общей эрудиции, немного математики, немного физики, немного гугления. :) Сам я такими устройствами не занимался.