Внушительный у вас стенд получился. Мне нравится.
Давайте вместе попробуем разобраться с этим прибором.
Цифровой твердотельный гироскоп, который вы используете, является по сути аналого-цифровым преобразователем - он преобразует угловую скорость собственного вращательного движения в цифровой код. Таким образом на выходе у него будет код - пропорциональный угловой скорости собственного вращения. Выглядит это примерно так: вращаем корпус гироскопа - на выходе код отличный от нуля, прекратили вращение - на выходе ноль. Тут, конечно, я все сильно упростил т.к не учел ошибки измерения, дрейф нуля и т.д. Самое важное для нас из вышенаписанного - гироскоп измеряет угловую скорость (скорость вращения), но не угол поворота. Чтобы узнать угол поворота придется немного потрудиться.
Теперь немного математики. Давайте вспомним, что такое угловая скорость? Угловая скорость (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 КБ) 31961 просмотр
Для тестирования я создал программный "симулятор" гироскопа. Его выходные значения можно задавать в реальном времени вручную с помощью бегунка:
- Имитатор для проверки алгоритма
- gyro-2.png (50.03 КБ) 31961 просмотр
Теперь пару слов по поводу разрешения и масштабных коэффициентов. Судя по документации, разрешение вашего прибора для выбранного диапазона (+/- 500 град/с) составляет 0.0153 град/с на одно дискретное значение выходного цифрового кода. Таким образом чтобы получить в программе значение в размерности град/с надо умножить цифровой код, полученный от датчика на 0.0153 или разделить на 65.359.
Исходные проекты для ROBO Pro: Хороших праздников!
. Однако накопление ошибки происходит постепенно, не так быстро как изменяется полезный сигнал (угловое перемещение). Если рассматривать сигнал на выходе нашего алгоритма в частотной области, то можно обнаружить, что ошибки связанные с дрейфом находятся в области низких частот. Это наблюдение нам поможет в дальнейшем при решении проблемы.