Гироскоп для TX/TXT

Обсуждаем модели и конструкции, построенные своими руками
Аватара пользователя
Mr.Kubikus
Сотрудник ПАКПАК
Сообщения: 1018
Зарегистрирован: 22 окт 2010, 23:57

Re: Гироскоп для TX/TXT

Сообщение Mr.Kubikus »

Привет! Спасибо и с Новым годом!

Внушительный у вас стенд получился. Мне нравится.

Давайте вместе попробуем разобраться с этим прибором.

Цифровой твердотельный гироскоп, который вы используете, является по сути аналого-цифровым преобразователем - он преобразует угловую скорость собственного вращательного движения в цифровой код. Таким образом на выходе у него будет код - пропорциональный угловой скорости собственного вращения. Выглядит это примерно так: вращаем корпус гироскопа - на выходе код отличный от нуля, прекратили вращение - на выходе ноль. Тут, конечно, я все сильно упростил т.к не учел ошибки измерения, дрейф нуля и т.д. Самое важное для нас из вышенаписанного - гироскоп измеряет угловую скорость (скорость вращения), но не угол поворота. Чтобы узнать угол поворота придется немного потрудиться.

Теперь немного математики. Давайте вспомним, что такое угловая скорость? Угловая скорость (omega) - это величина, которая показывает на какой угол повернулось тело за единицу времени:

omega = delta-alpha/delta-t,

где delta-alpha - угол поворота [град], на который повернулось тело за время delta-t [c]. Как же тогда посчитать угол поворота тела, зная как изменялась его угловая скорость? Да очень просто. Надо проинтегрировать угловую скорость omega по времени t:

alpha = integral (omega(t)*delta-t)
;

Выглядит, конечно, пугающе, но на самом деле все не так страшно. Так как наша система дискретная (измерения угловой скорости выполняются периодически через равные промежутки времени Ts), то операция интегрирования превращается в суммирование и в результате мы получим:

alpha = sum (omega(t)*Ts).

Таким образом чтобы получить угол поворота гироскопа надо просуммировать все результаты измерений угловой скорости, умноженные на длительность периода между измерениями Ts. Здесь надо обратить внимание на выбор периода между измерениями Ts. Если измерения делать редко, то, скорее всего, наша система будет измерять угол поворота с ошибками т.к. за время между измерениями гироскоп успеет повернуться на некоторый угол и мы этого не заметим. Делать измерения бесконечно быстро мы не можем, т.к. наша цифровая система имеет ограниченное быстродействие. Для механических систем рекомендуется выполнять 100-200 измерений в секунду. Этого будет достаточно. Таким образом рекомендуемые значения Ts будут находиться в диапазоне от 0.005 до 0.01 с. Длительность цикла исполнения сканирования входов/выходов контроллера TXT составляет 10 мс. Следовательно Ts будет 0.01 c.

Алгоритм для измерения угла поворота будет выглядеть так:
Алгоритм измерения угла поворота
Алгоритм измерения угла поворота
gyro-1.png (15.69 КБ) 26296 просмотров
Подпрограмму "Читать гироскоп" надо реализовать самостоятельно в соответствии с особенностями вашего датчика.

Для тестирования я создал программный "симулятор" гироскопа. Его выходные значения можно задавать в реальном времени вручную с помощью бегунка:
Имитатор для проверки алгоритма
Имитатор для проверки алгоритма
gyro-2.png (50.03 КБ) 26296 просмотров
Обратите внимание на ригистрограмму сигналов. Угол поворота изменяется только когда угловая скорость omega отлчается от нуля.

Теперь пару слов по поводу разрешения и масштабных коэффициентов. Судя по документации, разрешение вашего прибора для выбранного диапазона (+/- 500 град/с) составляет 0.0153 град/с на одно дискретное значение выходного цифрового кода. Таким образом чтобы получить в программе значение в размерности град/с надо умножить цифровой код, полученный от датчика на 0.0153 или разделить на 65.359.

Исходные проекты для ROBO Pro:
gyro-1.rpp
(62.62 КБ) 801 скачивание
gyro-2.rpp
(95.46 КБ) 783 скачивания
Хороших праздников!
С уважением, Григорий
GitHub FB ВК
vladmed
Сообщения: 19
Зарегистрирован: 09 сен 2015, 18:25

Re: Гироскоп для TX/TXT

Сообщение vladmed »

Огромное спасибо за столь подробное описание!
Я правильно понял, что в подпрограмму "Читать гироскоп" нужно ввести то, что у нас есть для получения сырых значений гироскопа, это и есть угловая скорость omega? Сейчас попробовали, не получается, не удается вывести тот самый угол поворота тела alpha.
И еще, мы запутались с целыми и плавающими, на выходе подпрограммы удается получить только целое, иначе возникает ошибка "Отсутствует вход для обработки сообщения =". Но если получать целое, та же ошибка в основной программе...
Вложения
Снимок2.PNG
Снимок2.PNG (12.67 КБ) 26295 просмотров
Аватара пользователя
Mr.Kubikus
Сотрудник ПАКПАК
Сообщения: 1018
Зарегистрирован: 22 окт 2010, 23:57

Re: Гироскоп для TX/TXT

Сообщение Mr.Kubikus »

1. Все верно. В подпрограмме "Читать гироскоп" выполняется чтение сырых данных от гироскопа и приведение к размерности [град/с].

2. Данные от гироскопа поступают с типом "Целое число со знаком". Перед вычислениями нам надо преобразовать эти данные к типу "Число с плавающей точкой". Для это есть специальный элемент, - смотрите на картинке ниже. Кроме того необходимо изменить тип обрабатываемых данных во всех программных элементах, которые как-либо взаимодействуют с потоком данных от гироскопа в подпрограмме и в основной программе.

3. Перед выводом на экран контроллера данные следует преобразовать обратно к типу "Целое число со знаком".
gyro-3.png
gyro-3.png (45.72 КБ) 26295 просмотров
С уважением, Григорий
GitHub FB ВК
vladmed
Сообщения: 19
Зарегистрирован: 09 сен 2015, 18:25

Re: Гироскоп для TX/TXT

Сообщение vladmed »

Ура, все получилось! Видим значение угла по X. Но это все только благодаря вам, сами бы мы не додумались... Теперь будем копать дальше, пока при повороте на 90 показывает около 60. Будем бороться за точность. Еще раз огромное спасибо и извините, что пришлось потревожить вас в праздники!
vladmed
Сообщения: 19
Зарегистрирован: 09 сен 2015, 18:25

Re: Гироскоп для TX/TXT

Сообщение vladmed »

Добрый день!
Гироскоп наш в целом работает, но точность оставляет желать... Главная проблема - дрифт, справиться с которым вроде бы можно с помощью данных с акселерометра. Но, начав разбираться с акселерометром, я запутался окончательно. Для начала я так и не понял, по какой формуле вычисляются углы, пробовал формулу от драйвера к ADXL355 из Robopro, изучил эту статью http://bitaks.com/resources/inclinometer/content.html, все равно, ерунда какая-то. Кроме того, допустим мы хотим ехать по гироскопу, некоторые источники утверждают, что в данной ситуации акселерометр просто не может работать на компенсацию отклонений гироскопа.
Подскажите, стоит ли пытаться использовать акселерометр, или проще компенсировать дрифт, используя что-то вроде фильтра Калмана? А может быть есть фильтр попроще?
Изначально вся эта затея была для того, чтобы повторить этот эксперимент http://reivilofischertechnik.weebly.com ... evice.html, т.к. ребенок изучает PID и даже прикрутил его к камере, чтобы плавно ехать по линии, но теперь он хочет и фильтр Калмана изучать, а у меня от одного вида программы :shock:

Еще, если можно, вопрос из другой оперы: можно ли подключить ультразвуковой датчик fischer к arduino?
vladmed
Сообщения: 19
Зарегистрирован: 09 сен 2015, 18:25

Re: Гироскоп для TX/TXT

Сообщение vladmed »

Случилось озарение, мы немного продвинулись. Приложил программу, хотелось бы услышать ваше мнение о применении комплиментарного фильтра.
Вложения
bmi160_acc_gyro_anton.rpp
(368.45 КБ) 792 скачивания
Аватара пользователя
Mr.Kubikus
Сотрудник ПАКПАК
Сообщения: 1018
Зарегистрирован: 22 окт 2010, 23:57

Re: Гироскоп для TX/TXT

Сообщение Mr.Kubikus »

Привет!

Вот это класс - такого качественного оформления проекта я давно не видел. Все в порядке - и структура и комментарии со ссылками на техническую документацию. Коллеги, берите пример.

По поводу дрейфа MEMS-гироскопа могу сказать следующее: это действительно стандартное поведение приборов такого класса. Также важно понимать, что алгоритм преобразования угловой скорости в угловое перемещение, который мы используем, содержит функцию интегрирования, которая интегрирует и полезный сигнал и ошибку измерения. В результате ошибка измерения постоянно накапливается и измерение становится полностью недостоверным :(. Однако накопление ошибки происходит постепенно, не так быстро как изменяется полезный сигнал (угловое перемещение). Если рассматривать сигнал на выходе нашего алгоритма в частотной области, то можно обнаружить, что ошибки связанные с дрейфом находятся в области низких частот. Это наблюдение нам поможет в дальнейшем при решении проблемы.

Получается, что одного MEMS-гироскопа недостаточно для измерения углового перемещения в долгосрочной перспективе. Нужен еще какой-то источник информации об угловом положении, по которому мы сможем скорректировать результат. И такой источник есть! Причем находится он в том же самом чипе - это MEMS-акселлерометр. Выходные сигналы этого прибора передают информацию о линейных ускорениях по трем осям - X, Y и Z. Путем некоторых математических преобразований из этой информации можно получить угловое положение. Однако и с этим прибором не все идеально - при воздействии на прибор внешней силы в измерениях будет появляться ошибка. Причем в отличие от гироскопа у этого прибора ошибки находятся в области высоких частот.


В итоге, каждый из датчиков по отдельности задачу решить не может. Почему бы им не заставить их работать сообща?

Для этого, как вы верно отметили, можно использовать так-называемый "комплементарный фильтр". Основная идея состоит в том, чтобы дрейф нуля гироскопа компенсировать показаниями акселерометра. Если отбросить все сложные математические выкладки, то в результате получается вот такая нехитрая формула:

a[i+1] = (1 – w) * (a + gyro_omega*Ts) + w * acc_a,

где
w - коэффициент "недоверия" к показаниям гироскопа в диапазоне от 0.0 до 1.0
a[i+1] - расчетное угловое положение на текущем шаге
a - расчетное угловое положение, полученное на предыдущем шаге
gyro_omega - угловая скорость от гироскопа (именно скорость, а не интеграл!)
Ts - период между измерениями
acc_a - угловое положение по акселерометру после математического преобразования


Правда есть еще одна проблема - акселерометр даст нам компенсацию только для двух углов. Вращение вокруг вертикальной оси он скомпенсировать не может. Для этого нам нужно устройство, которое сможет измерять угловое положение в горизонтальной плоскости. Это можно сделать с помощью цифрового компаса. При этом алгоритм фильтра будет аналогичным.
С уважением, Григорий
GitHub FB ВК
vladmed
Сообщения: 19
Зарегистрирован: 09 сен 2015, 18:25

Re: Гироскоп для TX/TXT

Сообщение vladmed »

Большое спасибо за столь высокую оценку! Мы старались, мне до сих пор не верится, что у нас что-то получилось. Осталось победить фильтр, беремся за работу...
vladmed
Сообщения: 19
Зарегистрирован: 09 сен 2015, 18:25

Re: Гироскоп для TX/TXT

Сообщение vladmed »

Григорий, извините, наверно я вас совсем замучил, но с фильтром непонятно. Мы написали программу фильтра, но не уверен, что правильно.
Во-первых, неясно, как получить первоначальное значение a(i)?
Во-вторых, если фильтр - это конечная стадия обработки, получается, что проинтегрированное значение с гироскопа AlphaX никак не используется. Или следует сначала применить фильтр, а затем интегрировать показания гироскопа?
Вложения
bmi160_acc_gyro_anton.rpp
(381.46 КБ) 790 скачиваний
vladmed
Сообщения: 19
Зарегистрирован: 09 сен 2015, 18:25

Re: Гироскоп для TX/TXT

Сообщение vladmed »

Здравствуйте, Григорий!

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

Подскажите, пожалуйста, как я понял, в отличие от BMI160, этот датчик выдает значения в двухбайтовых числах (рис.1), т.е. для получения Heading необходимо прочитать регистры 1B и 1A, затем как-то совместить полученные значения. Как это можно сделать средствами Robopro? Подходит ли такая подпрограмма, как на рисунке 2?
Вложения
2.PNG
2.PNG (4.88 КБ) 19069 просмотров
1.PNG
1.PNG (30.19 КБ) 19069 просмотров
Ответить