Делаю OSD, автопилот и инерциалку...
На основе компаса и гироскопа не получится.
На одном отсчете не получится.
А если отсчетов несколько и компас (3-х осевой магнитометр) меняется ?
(самоль покачивается)
На основе компаса и гироскопа не получится. Это еще Артем Силкин (ученик упомянутого выше дедушки Чистякова) в своем диссере доказал 11 лет назад примерно.
Скорее всего там речь идет о моментальном определении положения. Вы не правильно интерпритируете выводы.
На одном отсчете не получится. А если отсчетов несколько и компас (3-х осевой магнитометр) меняется ?
Да. Так и есть.
Dir’ = Mxyz*Dir; Dit’ и Dir - известны. М надо найти.
Mxyz - это матрица поворота по углам Эйлера. Т.е. это просто функция трех переменных.
В итоге имеем три уравнения с тремя неизвестными. Да, там неудобно то, что придется выражать косинус через корень и квадрат синуса. И ограничения будут при углах близких к углам осей. Но в принципе задачка решаема.
Сейчас есть два пути решения задачи коррекции гироскопа.
- Я вычисляю оффсет АЦП и вычитаю его из каждой из коорданит гироскопа перед интегрированием. Этот метод аналогичен методу авто подстройци частоты по частоте. Недостатот в том, что ошибка будет накапливаться из-за скорости реакции. Теоретически ошибка должна быть нулевой. Но если количество левых поворотов будет больше чем правых, то гироскоп будет врать немного в право. 😃
- Вычитать из сигнала гироскопа полученный сигнал от компаса. Недостаток в том, что метод будет работать в определенном диапазоне углов. Тут ошибка будет кратковременной, или ошибка будет при определенных углах.
Вот так, пока короче. 😃
Сейчас вот сижу и икса гоняю по бумаге. 😃
На основе компаса и гироскопа не получится.
Еще забыл сказать.
Мы ищем и компенсируем ошибку гироскопа, а не пытаемся заменить гироскоп компасом. Заменить гироскоп компасом действительно невозможно, поэтому мои выкладки никак не опровергают дисер товарища Силкина, а наоборот его только подтверждают. 😃
Всем привет!
Сделал я короче коррекцию по компасу! А за одно и с гироскопом разобрался. 😃
Получилось очень интересно и красиво. Есть конечно ограничения, но отслеживает достаточно хорошо. 😃
Как это ни странно, но получился аналог фазовой коррекции, что означает что ошибка не будет накапливаться.
Осталось только оптимизировать и улучшить алгоритм слежения. 😃
PS. Пока реализовывал алгоритм, выяснил, что значения гироскопа надо интегрировать как матрицу поворота, что уже само по себе является достаточно энергоемкой задачей. Я не знаю как народ может что-то подобное без плавучки реализовать. 😃
Сделал я короче коррекцию по компасу! А за одно и с гироскопом разобрался.
А если- чуть подробнее?
Алгоритм, хотя бы “на пальцах”?
А если- чуть подробнее? Алгоритм, хотя бы “на пальцах”?
Там все просто.
Есть с[n] это значение компаса в n моменты времени.
Есть угол поворота в момент времени n - f[n], M(f[n]) - это матрица поворота.
Имеем:
C[n] = M(f[n])*С[n-1]
Дальше все просто. Методом подбора из комбинации 81 возможных 3Д углов (0, +a, -a для каждой оси) по трем осям определяем в какую сторону надо сдвигать C[n-1] чтобы в итоге приблизиться к С[n].
Т.е. получаем некий l[n]. Сдвигаем C[n-1] на этот угол и повторяем все снова (тут у меня еще ФНЧ стоит для этого угла). В итоге за определенное время мы приблизимся к необходимому нам f[n].
Сдвигаем вектор в эту сторону. Потом повторяем операцию. Вместе с компасом сдвигаем начальную матрицу [1,1,1] что есть наша система координат.
В итоге после всех этих опираций мы имеем реальное положение самолета определенное по компасу.
А дальше начинаются манимуляции с оценкой ошибки гироскопа и ее коррекцией. 😃
По идее, в данном случае мне гироскоп вообще не нужен, так как я уже знаю матрицу поворота и могу определить положение самолета. 😃
Но в связи с тем что значение компаса требует значительного усреднения и фильтрации, то оно может использоваться с очень низкой частотой и использовать его как показатель крена, тангажа и рысканья будет достаточно проблематично. Поэтому надо использовать гироскоп.
Недостаток данного метода один: он не корректирует постоянную составляющую гироскопа. Исходя из данного метода я могу корректировать результать интегрирования гироскопа, но никак не сам гироскоп.
Достоинство одно - ошибка оценки положения стремиться к нулю и не может быть больше чем заданное значение (у меня в модели это 0.01 градуса, в реале такое будет не нужно).
Сейчас я пытаюсь доделать алгоритм коррекции именно значения гироскопа, т.е. убрать ADC офсет. 😃
Как-то так. 😃
Еще забыл написать.
Коррекция матрицы поворота производится только когда вектор меняется.
Еще забыл написать.
Всё это хорошо, бумага- написана правильно 😃
Но может, чтобы не изобретать велосипед, поглядеть как это сделано у тех, у кого уже летает: diydrones.com/…/getting-the-most-out-of-your
Проект уже несколько лет существует, участвуют в нем множество народу со всего мира…
А я, не математик, ничего не понял… Ну узнаем мы поворот вектора магнитного поля в системе координат датчика (модели) за одну итерацию (по сути - скорость поворота этого вектора)… Но по нему все равно невозможно однозначно определить положение самой системы координат. Ибо вокруг одного вектора можно построить множество этих систем. И тут никакие математические уловки не помогут. Ну не изменится значения проекций вектора при повороте системы координат вокруг оси вектора, продифференцируй их хоть 100 раз…
А я, не математик, ничего не понял
Я- тоже.
Но поскольку я практический разработчик и всё- довожу “до железа”, то всегда стараюсь изучить наибольший объём информации по предмету и затем, отсеяв ненужное- получить продукт.
По моему убеждению, полноценно имитировать работу ИМУ “на столе”- нельзя.
Слишком много реальных возмущений не поддаётся моделированию. Либо-стоимость стенда будет=цене Шаттла 😃
И поэтому, сделать работающее устройство можно только в режиме : плата-программа-лётные испытания.
А поскольку АрдуПилот именно такое изделие, многократно испытанное многими людьми, я бы- работал именно в режиме модификации готового продукта.
Если Дима действительно толковый математик- нет причины, почему бы он не смог улучшить проект.
Ну не изменится значения проекций вектора при повороте системы координат вокруг оси вектора, продифференцируй их хоть 100 раз…
Все правильно. Так и есть. 😃
Поэтому это и является лишь коррекцией позволяющей убрать постоянную составляющую поворота.
Попытаюсь пересказать другими словами на простом примере.
Вот есть у нас ошибка АЦП в виде постоянной составляющей а на все три оси. В итоге, если мы находимся в покое, то получится что после интеграции все три оси будут вращаться вокруг вектора, а сам вектор будет находиться на месте (это как вращать зонтик вокруг ручки с определенной скоростью). И ту действительно ничего не сделаешь. 😃
Но…
Если отбросить аргумент что самолеты так не летают (это мы будем потом корректировать), то нам надо скорректировать постоянную А в моменты, когда вектор меняет положение и минимизировать вращение в моменты когда вектор постоянен.
Чтобы понять принцип коррекции нужно просто представить, что если вектор совпадает с осью Х и у нас есть его вращение по окружности в плоскости ХУ на данных гироскопа, а данные вектора компаса постоянны, то нам надо корректировать гироскоп по оси Х, а все остальные оси нас не интересуют. То-же самое делаем по другим осям. 😃
А использование матрицы и т.д. позволяет нам это делать не только в моменты когда Z=0, но и в других случаях.
Ибо вокруг одного вектора можно построить множество этих систем. И тут никакие математические уловки не помогут.
А если вдруг известно правило как изменяется угловая скорость в зависимости от ориентации ?
Например: при правом крене - угловая скорость по курсу увеличивается вправо, при левом - налево;
если нос вверх - то появляется(увеличивается) угловая скорость по тангажу, чтобы клюнул носом;
если нос смотрел вниз - то начинает задирать вверх
(нормальные самолетики, которые сами летают, вроде должны так себя вести).
Тогда штурман, видя: 1)магнитометр в пол(например) и 2)гира показывает(в следующем отсчете) увеличение угловой скорости вправо -
догадается, что: 1)правый крен и 2) летим на восток.
А если вдруг известно правило как изменяется угловая скорость в зависимости от ориентации ?
К сожалению не известно.
Нам нужна ориентация для управления, а не управление для ориентации. 😃
Хотя с другой стороны про штурмана все правильно написано. Но там задача другая.
Сейчас у меня задача - коррекция ошибки гироскопа с помощью показаний компаса. Если формулировать еще точнее, то - автоподстройка постоянной ошибки угловой скорости (ADC offset).
Нее… бесполезно мне объяснять, или вы неважный учитель или я бестолковый ученик… 😃
Если отбросить аргумент что самолеты так не летают…
Для FPV характерны длительные полеты по одному неизменяемому курсу. Летим вдоль вектора магнитного поля- никак не сможем определить и даже заметить компасом изменения крена. Полетели перпендикулярно- не видим тангаж.
ЗЫ Статическая ошибка нуля угловой скорости гироскопа, (как понял то- что вы почему-то называете ADC-offset), всего лишь одна из причин вызывающая накапливаемую ошибку при интегрировании. Не меньший вклад в эту ошибку, вносит общая неточность и неидеальная линейность показания гироскопа и дискретный характер ее получения и обработки. Хотя общего характера проблемы это не меняет- необходимость коррекции постоянной составляющей, которая неизбежно “уплывает” при интегрировании значений угловых скоростей, читаемых из гироскопа.
Для FPV характерны длительные полеты по одному неизменяемому курсу. Летим вдоль вектора магнитного поля- никак не сможем определить и даже заметить компасом изменения крена. Полетели перпендикулярно- не видим тангаж.
Вы сами ответили на вопрос!
Летим по курсу, не можем измерить крен, и не надо! Корректируйте остальных два угла, точнее скорости изменения. 😃
Летите перпендикулярно - корректируйте другие два. 😃
И т.д.
Я доделаю алгоритм - отпишу поподробнее.
Так как задача алгоритма- устранить дрейф гироскопов (случайные ошибки датчиков, шумы, погрешности измерений, пр.) то корректировать надо ВСЕ гироскопы. Т.к самолет никогда не летит прямо. И при использование ОДНОГО вектора коррекции, вся матрици начнет крутиться вокруг оси этого вектора, как уже сказал msv.
Это если мы хотим построить “инерциалку” , как вы пишете,
а не летающий аппарат использующий комплексные особенности ЛА, как предполагает Frr.
Для этого вполне достаточно ОДНОГО датчика угловой скорости.
И никаких компасов и акселерометров. Все “элероны” до недавних времен так летали.
Все! Ура!
Можете меня поздравить! После недели траха, я сделал коррекцию гироскопа по компасу! Точнее я написал алгоритм. 😃
Работает всегда и при любых условиях.
Коррекция не зависит ни от чего! Исправляет как оффсет, так и любые другие ошибки гироскопа! Точность коррекции определяется точностью компаса и качеством фильтрации. 😃
Теперь уже будем корректировать компас.
Те кто говорил что нельзя корректировать гироскоп компасом, мягко скажем - неправы! 😃
С другой стороны, то что я писал до этого, тоже немного неправильно, точнее не совсем правильно. 😃
Сейчас буду это дело вставлять в железо. Там математики дофига. Хорошо что ресурсы под это дело есть. 😃
Артему Силкину - привет!
Так как задача алгоритма- устранить дрейф гироскопов (случайные ошибки датчиков, шумы, погрешности измерений, пр.) то корректировать надо ВСЕ гироскопы. Т.к самолет никогда не летит прямо.
В ходе разработки задача поменялась. 😃
Теперь компас корректирует не только дрейф но и вообще ВСЁ. Все ТРИ гироскопа теперь корректируются автоматом независимо не от чего. 😃
Теперь компас корректирует не только дрейф но и вообще ВСЁ. Все ТРИ гироскопа теперь корректируются автоматом независимо не от чего.
- при наклоне компаса ничего не плывет? (у меня при наклоне немного уплывает)
- если проекция вектора маг.поля на плоскость ХУ равна 0?
- в условиях полета (силовые провода под боком, платы с металлизацией вокруг, болты, металлическая рама) коррекция работает?
На столе часто все красиво, а потом дрова в пакете…
- при наклоне компаса ничего не плывет? (у меня при наклоне немного уплывает)
Нет. Алгоритм коррекции устроен таким образом, что автоматом исправляет любой уплыв.
- если проекция вектора маг.поля на плоскость ХУ равна 0?
В этом случае коррекция по оси Z отключается, а корректируется только по XY.
- в условиях полета (силовые провода под боком, платы с металлизацией вокруг, болты, металлическая рама) коррекция работает?
Работает. Во первых, сейчас важно что работает алгоритм коррекции. И второе: компас сам по себе еще будет корректироваться, да и коррекция положения по компасу не единственная коррекция которая будет реализована.
Сейчас главное что найдено общее решение проблемы коррекции положения по компасу. Это решение основанное на нескольких формулах, решение которых дает значение абсолютных углов коррекции положения самолета в любой момент времени. 😃
Фактически я просто вычисляю формулу:
E(n) = D(Gyro(n),Comp(n)), где E[n] - это ошибка положения самолета в момент времени n, т.е. углы е1[n], e2[n], e3[n] по осям x,y,z.
PS. У меня друг один есть, так он как-то сформулировал такую вещь, что если узнаешь красивое решение, то потом можно ходить и улыбаться весь день. Так вот, алгоритм решения коррекции по компасу одно из таких решений. 😃
Очень рад, что Вы добились результатов и весь день улыбаетесь. 😃
Сутью вашего алгоритма не поделитесь? Уверен, что многим интересно.
Сутью вашего алгоритма не поделитесь? Уверен, что многим интересно.
В принципе все просто! 😃
Мы знаем формулу расчета для матрицы поворота: D(a[n],b[n],c[n]). abc - это углы поворота которые мы получаем с гироскопа.
C[n] - это значение компаса в момент времени n, C’[n] - оценка компаса в момент времени n будет D(a,b,c)*C[n-1];
Но мы знаем, что a,b,c содержат ошибки e1,e2,e3.
В итоге, мы можем записать следующее уравнение:
C[n] = D(a[n]-e1,b[n]-e2,c[n]-e3)*C[n-1];
С[n],С[n-1],…С[n-бесконечность] - известно
abc[n],abc[n-1],…,abc[n-бесконечность] - известно
e1,e2,e3 - константы и их надо найти. 😃
Дальше просто составляем систему уравнений и решаем ее. 😃
В случае если C[n]=C[n-1] решения не нет, так как определитель равен 0 (это как раз тот случай, когда оси вертятся как зонтик вокруг вектора). Так-же не имеем решения когда самолет находится на месте и вектор C[n] - константа, и система имеет бесконечное множество решений. 😃
Но как только самолет начинает двигаться, т.е. С[n] != С[n-1] != С[n-2] , сразу получаем одно единственное решение в виде E[n]. 😃
Коррекцию проводим на разных интервалах времени с различной фильтрацией и т.д… 😃
В итоге имеем, что меняя положение относительно неподвижного вектора, можно определить постоянную ошибку поворота по всем осям. 😃
Т.е. если гироскоп откорректирован и самолет летит ровно, то мы не можем вычислить коррекцию, но она нам и не нужно. А вот как только самолет начинает немного изменять свое положение (даже от порыва ветра), так сразу мы можем эту коррекцию вычислить. 😃
По идее, нам достаточно даже просто компаса с ГПС, чтобы вычислить все три ошибки. 😉
Восторженные оценки алгиритма приветствуются! 😃
Интересное решение. Но все эти размышления верны, если мы считаем, что e1,e2,e3 остаются постоянными для моментов времён n, n-1, n-2. Верно? Только в этом случае мы можем составить три уравнения с 3-мя неизвестными (в нашем случае e1,e2,e3 - неизвестные).