Делаю OSD, автопилот и инерциалку...
А я, не математик, ничего не понял… Ну узнаем мы поворот вектора магнитного поля в системе координат датчика (модели) за одну итерацию (по сути - скорость поворота этого вектора)… Но по нему все равно невозможно однозначно определить положение самой системы координат. Ибо вокруг одного вектора можно построить множество этих систем. И тут никакие математические уловки не помогут. Ну не изменится значения проекций вектора при повороте системы координат вокруг оси вектора, продифференцируй их хоть 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 - неизвестные).
e1,e2,e3 остаются постоянными для моментов времён n, n-1, n-2. Верно?
Верно!
Если Е и меняется, то медленнее чем скорость с которой мы измеряем значение компаса и гироскопов.
К тому-же нам все равно, меняется е или нет. Нам надо найти среднюю ошибку е за три иттерации. Т.е. скорость изменения ошибки должна быть в три раза меньше чем скорость изменения вектора. 😃
И вообще, нам все равно что там будет. Главное угадать знак ошибки. Или даже скажем так: главное чтобы знак ошибки выдавался чаще правильно чем не правильно. В этом случае коррекция будет работать. 😃
И еще.
Нам даже не надо брать три значения подряд. Нам просто надо три изменения значения. 😃
PS. Возражения не принимаются, так как алгоритм проверен и реально работает. 😃
На чем проверен? на самолете в полете?
Восторженные оценки алгиритма приветствуются!
Возражения не принимаются, так как алгоритм проверен и реально работает.
Выражаю свой восторг, от возражений не удержусь… 😃
На чем проверен? на самолете в полете?
Полет не обязателен… Можно и на столе убедиться, что при некоторых углах ориентации (вполне реальных для полета), гироскопы неизбежно поплывут. Одна надежда, что пилот или ветер будет все время менять курс.
Алгоритм конечно красивый и похоже правильный, но мне кажется тут изобретен велосипед. Неужели этот алгоритм никто еще не реализовывал?
Как бы то ни было, до готового устройства, способного достойно управлять самолетом, еще очень далеко.
Буду рад если ошибаюсь…
Алгоритм конечно красивый и похоже правильный, но мне кажется тут изобретен велосипед.
Это нормальная реакция. Когда рассказали как, все стало очивидно, и возникает вопрос, а почему раньше этого не сделали, это-же очивидно? 😃
Неужели этот алгоритм никто еще не реализовывал?
Именно этот, конкретно для коррекции гироскопов я нигде не видел. Все что я видел в интернете про коррекцию, относилось к загадочному фильтру Калмана, который якобы должен где-то работать. 😃
А вообще, подобным образом сделан алгоритм пеленгации. Точнее, алгоритм определения координат объекта с помощью одного пеленгатора (это то-же возможно). 😃
Одна надежда, что пилот или ветер будет все время менять курс.
Тут дело вот в чем. Нам нужна ориентация, чтобы управлять самолетом. При прямолинейном полете мы самолетом не рулим (теоритически). Но как только гироскоп начинает плыть, так мы сразу детектируем изменение положения и начинаем корректировать курс. Корректируем положение самолета, компас изменяется. Компас изменяется, гироскоп корректируется. Положение по откорректированному гироскопу снова изменяется. Мы снова приводим все в норму. И т.д.
Т.е. в случае автопилота, система будет сама себя корректировать независимо ни от чего. Тут просто отрицательная обратная связь. 😃
дело в том что каждый из сенсоров - по сути источников информации может давать ложные данные
например у гирика и акселя может быть ошибка изза дескретности измерения по времени в условиях вибрации,
то есть пошел микроудар - фильтрующий кондер подзарядился и ацп считал ошибку через микросекунду уровень восстановился но ацп будет следующее значение считывать скажем через 10 микросекунд (к примеру) к этому времени может так сложиться что будет второй удар и ошибка накопится, не будет учтено что между замерами были провалы, с акселем тоже самое.
3д Компас я полагаю был бы идеальным сенсором, который бы заменил гирик еслиб не одно “но” магнитные и электромагнитные поля, если воспринимать его данные как чистую монету, можно словить глюк в реальных полетах вблизи ЛЭП и местах залегания магнитных руд
ЖПС неплохой сенсор который дает достаточно точный 3d вектор курса на скорости, при большом количестве спутников.
собственно о чем я хотел сказать, что какие бы небыли сенсоры у них есть параметр точность, и он не статичен и было бы логично учитывать точность того или иного сенсора в зависимости от совокупности условий, в качестве весовых коэффициентов при получении вычисляемого положения модели.
к примеру если гпс на скорости 30км/ч при 11 спутниках говорит что курс север, а компас показывает восток, то вероятна трабла с локальными магнитными полями, если гирик не отклоняется а компас резко уходит в сторону - вероятно рядом точечный магнитный полюс. Инаоборот если спутников к примеру 3 - координаты могут сильно прыгать, а аксель гирик и компас дают показания которые согласуются - тут логичнее не поверить гпс.
зы. а вообще молодец автор, такую махину задумал в одиночку сделать…
Т.е. в случае автопилота, система будет сама себя корректировать независимо ни от чего.
При полете на север-юг (и наоборот) поплывет крен, действительно изменится курс, полетит хоть переменными галсами, но еще терпимо… При полете запад-восток (и наоборот) поплывет тангаж… курс может изменится, когда будет слишком поздно…
дело в том что каждый из сенсоров - по сути источников информации может давать ложные данные
Основная проблема коррекции по компасу- вектор почти все время находится очень близко к плоскости горизонтального полета (в отличии от акселя…). Поэтому ситуации когда он будет давать 0-вые проекции на две оси совсем ненадуманные…