Два акселерометра, губка для посуды и четыре гайки

в 15:30, , рубрики: stm32, Алгоритмы, геометрия на пальцах, математика, перевёрнутый маятник, Программирование, программирование микроконтроллеров, Разработка робототехники, фильтры

Вводная: измерение угла маятника

Итак, разобравшись с датчиками Холла для бесколлекторных двигателей, покажу для чего это было нужно. Я сделал стенд с простейшим обратным маятником, который стабилизируется при помощи маховика, вращаемого моторчиком на свободном конце маятника:


Я учусь теории управления и построил таких игрушек некоторое количество, вот, например, мой обратный маятник на тележке:

Разбор управления подобными штуками оставим на следующую статью, а темой сегодняшней статьи является измерение угла маятника. Самый простой вариант — это поставить инкрементальный энкодер, это я уже подробно разобрал.

Но в данной статье я хочу измерять положение маятника при помощи акселерометров, к нему прикрепленных. Пользоваться я буду самыми копеечными adxl345. Итак, если мы прицепим акселерометр на маятник на расстоянии r от шарнира, как определить угол? Я прицепил акселерометр так, чтобы ось Y была вдоль маятника, а ось X была бы маятнику ортогональна:

Два акселерометра, губка для посуды и четыре гайки - 1

Давайте предположим для начала, что маятник не движется. Тогда акселерометр будет выдавать проекцию вектора силы тяжести на свои оси. Поскольку всё происходит в одной плоскости, то в идеале по оси Z акселерометр будет выдавать нулевые значения. То есть, измерение акселерометра будет выглядеть как (x,y,z) = (g cos θ, -g sin θ, 0). Ну а угол можно получить при помощи арктангенса от y/x.

Если же акселерометр начинает двигаться, то всё становится чуточку сложнее: помимо ускорения свободного падения он будет ещё измерять угловое ускорение маятника и соответствующее центростремительное ускорение, они показаны красными стрелочками на схеме.

Я повесил акселерометр на маятник, и заодно для контроля завёл инкрементальный энкодер, который мне даёт настоящий угол, с которым можно сравнивать измерения по акселерометру.

Итак, если я возьму простейший арктангенс по измерениям акселерометра, то получу следующую вещь:

Два акселерометра, губка для посуды и четыре гайки - 2

В этой картинке я у меня качается маятник, изменение его настоящего угла во времени — это красная кривая, а синяя кривая — арктангенс от y/x, они же измерения акселерометра. Хорошо видно, что кривые не очень совпадают.

А теперь давайте добавим второй акселерометр, на сей раз на расстоянии r/2 от центра. Два акселерометра будут нам давать измерения (xa,ya) и (xb,yb), которые, в идеальном случае должны зависеть следующим образом от угла θ и от угловой скорости/ускорения:

Два акселерометра, губка для посуды и четыре гайки - 3

В принципе, всё двоольно прозаично, но зачем я так сделал? А теперь магия, следите за руками! Давайте посчитаем, чему будет равняться (xa,ya) — 2*(xb, yb):

Два акселерометра, губка для посуды и четыре гайки - 4

Вся динамика самого маятника ушла, осталась только проекция вектора ускорения свободного падения! То есть, при помощи двух акселерометров мы можем оценивать угол маятника, не оглядываясь на его динамику:

Два акселерометра, губка для посуды и четыре гайки - 5

А вот так выглядит оценка в реальном времени:

Два акселерометра, губка для посуды и четыре гайки - 6

Здесь синяя и зелёная кривая — это «сырые» данные с акселерометров, а чёрная кривая — это посчитанный угол. Он весьма недурно совпадает с эталонной кривой, полученной с энкодера. Я не занимался юстировкой датчиков, всё прикручено «на глаз», откуда небольшие расхождения между данными с акселерометров и с энкодера.

На этом историю можно было бы закончить, но я обещал губку для посуды (и четыре гайки!), где же они? Сейчас будут. Этот график был получен совсем не с первого подхода. Давайте теперь начнём с самого начала.

Как крепить акселерометры

Поначалу я просто прикрутил акселерометры к моему маятнику и был неприятно удивлён мусором, который шёл в качестве данных. Давайте я покажу масштаб катастрофы, который я изначально получил:

Два акселерометра, губка для посуды и четыре гайки - 7

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

Сначала я грешил на электронаводки, но оказалось, что виновником являются высокочастотные вибрации при работе бесколлекторного движка. Тогда решил делать виброразвязку. Никогда ничем подобным не занимался, под рукой был только соплемёт. Вот так выглядела моя первая виброразвязка.

Два акселерометра, губка для посуды и четыре гайки - 8

Гайки служат в качестве инерционного гасителя колебаний. Первый тест, и ситуация улучшилась кардинально! Я возрадовался и сколхозил вторую версию, где гайки были заменены на стальную пластинку:

Два акселерометра, губка для посуды и четыре гайки - 9

Вот так выглядят графики:

Два акселерометра, губка для посуды и четыре гайки - 10

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

Два акселерометра, губка для посуды и четыре гайки - 11

Вот её графики, отличия видны в районе восьмой-десятой секунды на синем графике, который должен быть более-менее треугольным.

Два акселерометра, губка для посуды и четыре гайки - 12

На этом я было успокоился, но оказалось, что я взял новую губку, которая через пару дней высохла так, что стала недостаточно эластичной. Можно было бы попробовать капнуть глицерина, но это не наш метод! Поэтому заменил кухонную губку на антистатик-поролон:

Два акселерометра, губка для посуды и четыре гайки - 13

Вот соответствующие графики:

Два акселерометра, губка для посуды и четыре гайки - 14

И тут меня посетила мысль: а зачем я вообще парюсь с массой, нельзя ли платку акселя непосредственно на поролон приклеить? Попробовал следующую конфигурацию:

Два акселерометра, губка для посуды и четыре гайки - 15

Ответ: нет, нельзя!

Два акселерометра, губка для посуды и четыре гайки - 16

В общем всё, что вы хотели знать об НЧ-фильтрах, но боялись спросить. Вывод: виброразвязка это хорошо, но надо грамотно подбирать частоту отсечки. Если кто-нибудь сможет преложить менее экспериментальный способ подбора материалов, их толщины и соответствующих грузов, буду очень благодарен!

Автор: haqreu

Источник


* - обязательные к заполнению поля


https://ajax.googleapis.com/ajax/libs/jquery/3.4.1/jquery.min.js