Инклинометр на базе гироскопа

Сегодня речь пойдет об отладочной плате IMU, которую я сделал для проверки своих идей перед тем, как включить эту схему в плату контроллера балансирующего робота. IMU будет его вестибулярным аппаратом, отвечающим за его угол относительно горизонта, скорость сваливания, горизонтальную скорость и ускорение. Для этого обычно используют пару акселерометров, гироскоп, подключают все хозяйство к микроконтроллеру, оцифровывают значения с помощью АЦП и используют фильтр Калмана для фильтрации результатов.

Для определения угла относительно горизонта используется интегрирование сигнала от гироскопа. Для коррекции угла используются показания акселерометров, которые в стабильном вертикальном положении показывают один 0, а другой -g (в такие моменты угол можно сбросить в 0 и продолжить интегрировать дальше).

Но для интегрирования показаний гироскопа его выход оцифровывают 10-12 битным АЦП, делать это можно раз 100-500 в секунду, и при этом каждый раз получается погрешность преобразования, которая аккумулируется при интегрировании. Тут недолго и потерять равновесие.

Вспомним, зачем это нужно. Вообще, для того, чтобы поддерживать тело робота в вертикальном положении, можно использовать обычный PID-регулятор. Здесь P (Proportional) – это угол отклонения. I (Integral)– это сумма ошибок угла за предыдущий период, а D (Derivative) – это угловая скорость, то есть само показание гироскопа.

Поэтому моей idee fixe было установить аналоговый интегратор показаний гироскопа, чтобы получился настоящий инклинометр. Ведь здорово было бы в каждый конкретный момент времени оцифровывать и угол и скорость — тогда бы PID регулятор был очень четок.

В Терраэлектронике как раз появился относительно новый чип от ST Semiconductor, LSM320HAY30. Этот чип представляет собой 3-осевой акселерометр с цифровым выходом, совмещенный на одном чипе с 2-осевым гироскопом, имеющим аналоговый выход — то, что надо. Несколько волнительно было паять его феном на изготовленную методом ЛУТ плату (корпус LGA-28L с шагом площадок 0.6мм), но обошлось.

Вся схема питается от 3.3В.

Аналоговый интегратор собран по классической схеме на операционно усилителе. Реальный интегратор, помимо полезного сигнала, будет интегрировать суммарную ошибку, состоящую из разнообразных погрешностей, токов утечки и напряжений смещения. Моделируя работу интегратора в LTSpice IV, выяснил несколько тонких мест, на которые нужно было обратить внимание при подборе компонентов.

Во-первых, чтобы интегратор был достаточно точным, пришлось подыскать операционник с входным током смещения порядка пикоампер и малым напряжением смещения (сотни микровольт) — иначе бы интегрировал только свои собственные ошибки. Например, напряжение смещения MC33072 – 3мВ, он нам не подойдет. Мои фавориты: MAX4236, OPA376, AD8603. Легко и недорого удалось купить MAX4236.

Для сброса показаний нужен ключ, и тут CMOS ключи старых серий, типа 4016, тоже не подойдут. У них в разомкнутом состоянии ток утечки достаточно велик, чтобы испортить всю картину. У более продвинутых и современных микросхем этот ток порядка на три меньше. Из предложений терраэлектроники я выбрал MAX4544.

AppNote разных произвоителей сообщают нам, что и конденсаторы для точных интеграторов должны быть тоже особенные: желательно, с майларовым или полиэстеровым диэлектриком. Не знаю, насколько это существенно в моем случае, но сейчас полно в продаже метал-полиэстеровых конденсаторов, которые как раз очень хороши для интеграторов. Я купил MKT B32521 0.22 мкФ производства EPCOS.

Также установил на плате буферные усилители для выходов гироскопа. Где-то на просторах интернета я прочитал топик одного товарища, имевшего проблемы с показаниями гироскопа из-за того, что АЦП семплировал показания гироскопа слишком часто. Схема sample-and-hold оттягивала слишком много тока от слабенького выхода гироскопа. Я промоделировал в LTSpice IV эту схему с моделью входа АЦП микроконтроллеров STM32, и пришел к выводу, что в данном случае такого не произойдет. Но так как к выходу гироскопа напрямую подключен интегратор, я все-таки решил добавить буферный усилитель. Но сделал это не совсем удачно. Об этом потом.

Вот что получилось.

Схема:

Схема и BOM в pdf:

inclinometer-sch-v1.pdf

inclinometer-bom-v1.pdf

Плата:

Фото сверху:

Фото снизу:

Расчеты

Граничная частота low-pass фильтра обоих гироскопов:

f = 1/(2*PI*R16*C28) ≈ 103 Гц

Разрешение гироскопа

a = 3.33 мВ/(°/c)

Рассчитаем разрешение интегратора. При угловой скорости 1°/c ток заряда конденсатора будет:

I=a/R19 (А)

За 1 секунду заряд конденсатора увеличится на

Q=1c*a/R19 (Кл)

Напряжение изменится на

b=Q/C32.

При этом за секунду на 1° изменится наклон. Получится, что разрешение интегратора это:

b=1c*a/(R19*C32) (мВ/°)

b = 45.87 мВ/°

Опорное напряжение гироскопа составляет 1.5В. Поэтому максимальный измеряемый угол, при учете использования rail-to-rail ОУ составит A = 1500/45.87 = 32.7°. Для целей поддержания вертикального положения этого более, чем достаточно.

Расчеты погрешности интегратора, связанной с входными током и напряжением смещения, приводить не буду. Для данной схемы расчет показал, что из-за погрешностей операционника уход выходного значения интегратора будет от 75мкВ/c (типичные значения из datasheet) до 10мВ/c (максимальные отклонения из datasheet). То есть, по максимуму, будем иметь ошибку в один градус за 4.5 секунды. Неплохо.

Испытания

После подачи питания видим на выходе Vref гироскопа 1.49В — близко к номинальному 1.5В. Без движения выходы X и Z показывают в один на 0.02В меньше, другой на 0.02В больше опорного напряжения. Даташиты говорят об этом так: есть некий Zero-rate level, который меняется при пайке и сильных стрессах. Это нормально. И хотя в документации и не сказано, каково максимальное допустимое отклонение от Vref, но я его феном паял, так что могло быть и хуже.

Ко цепи RST_GYRO подцепил переменный резистор, выполняющий роль источника логического сигнала. 3.3В на RST_GYRO – интегратор сброшен, 0 — начинает интегрировать.

Тут же заметил, что выход интегратора очень быстро падает до 0. То есть ему далеко до идеального интегратора. После некоторых поисков стало ясно, что интегрируется разница между выходом гироскопа и Vref, то есть тот самый Zero-rate level. Он и является основным источником погрешности в схеме. Если его не устранить, то все равно — какие брать операционники и т.п.

К счастью, производитель обещает, что в процессе работы Zero-rate level почти не меняется, а это значит, что я могу компенсировать его просто подав слабенький ток на вход операционного усилителя. Для этого между 0 и 3.3В повешен второй переменный резистор, движок которого через резистор 2.7МОм подключен к инвертирующему входу интегратора. Подстроил — voila, все работает.

Результат

На видео видно, как я качаю плату. Я быстро поднимаю один ее конец, жду, отпускаю, жду. На осциллографе видны две дорожки. Одна — угловая скорость, показания гироскопа. Вторая — угол, показания интегратора. Как видно, выход интегратора выписывает довольно аккуратный меандр.

При не слишком резких толчках выход гироскоп не достигает ни 0В, ни 3.3В, хотя и подходит к ним близко. При совсем резких он все таки выходит из диапазона и выходные показания инклинометра несколько сбиваются. Возможно, мне надо было использовать выход x1 для интегратора, а встроенные в чип усилители x4 – только для измерения угловой скорости.

Диапазона выходных значений интегратора хватает на расчетные отклонения примерно ±30°. Впрочем, все можно изменить, подобрав номиналы резистора и конденсатора в интеграторе.

Lessons learned

Во-первых, в следующей реализации обязательно надо ставить схему коррекции Zero-rate level. Причем, если надо интегрировать несколько значений — ставить несколько таких схем.

Во-вторых, я неправильно сделал буфера для гироскопа. Это должны были быть инвертирующие усилители с коэффициентом усиления -1, считая за опорную точку Vref. Но даже с использованием 1% резисторов они будут иметь относительную погрешность больше 1%, что я и наблюдал. В следующий раз буду использовать схему неинвертирующего повторителя.

В-третьих, диапазона выходных значений гироскопа в режиме x4 (±300°) может не хватить при резких толчках.

Заключение

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

Я перерисовал схему, чтобы исправить найденные недочеты:

inclinometer-sch-v2.pdf

inclinometer-bom-v2.pdf

Не знаю, буду ли я использовать интегратор в плате IMU, но уж точно заложу его в реальную схему. Теперь в планах подключить все это к микроконтроллеру и попробовать, как работают акселерометры.

Tags:

Добавить комментарий

Ваш e-mail не будет опубликован. Обязательные поля помечены *

*


4 - = три

Можно использовать следующие HTML-теги и атрибуты: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>