Подвес для камеры на двух сервах я собрал за выходные, а угадать, куда он смотрит, не смог ни за один вечер. Угол я брал из акселерометра: платформа качнулась — рука дрогнула — угол прыгнул на десять градусов, серва дёрнулась следом, платформа качнулась ещё. Классическая петля: датчик путает наклон с тряской, привод отрабатывает призрака, конструкция раскачивается сама себя.
Причина в физике: акселерометр видит сумму гравитации и всякого ускорения, и разобрать их по трём числам нельзя — поле одно. Выход старый: добавить второй датчик, который тряску в принципе не видит. Гироскоп измеряет скорость вращения: платформа стоит — он выдаёт около нуля, наклоняется — показывает, сколько градусов в секунду.
В уроке про сырые данные мы научились читать регистры — гироскоп живёт дальше по карте, с 0x43: по два байта на ось, старший первым. Умолчательный диапазон ±250°/с, и один градус в секунду — это 131 единица. Перевод знакомый: то же деление, только получаются не g, а градусы в секунду.
Диапазон расширяется регистром GYRO_CONFIG по адресу 0x1B: два бита FS_SEL дают лестницу 250, 500, 1000 и 2000 градусов в секунду, и вместе с ней меняется делитель — 131, 65.5, 32.8 и 16.4 единицы на °/с. Для подвеса и уровня я оставляю 250: вращения там медленные, мелкая ступенька в 131 единицу даёт гладкую оценку. 2000 — для платформ, которые крутятся по-честному: авиамодель или вращающийся стенд, где полные 360 градусов в секунду на потолке 250 обрубались бы уже на одном обороте. Расширять диапазон на уровне «на всякий случай» не советую: те же качания руки в единицах широкого диапазона выглядят втрое мельче, и оценка грубеет.
Интегрирование: скорость в угол через dt
Гироскоп не знает угла — только скорость его изменения. Угол добывается интегрированием: каждый проход loop прибавляет скорость, умноженную на время шага. Тут и живёт главная грабля темы. В моём первом скетче стояло ugol += gys * 0.01 — цифру я взял из примера, а loop на деле ходил за четыре миллисекунды. Каждое прибавление завышалось в два с половиной раза, и за минуту покоя угол улетел на несколько сотен градусов. Час я ловил дрейф, которого не было: это была обычная арифметика с чужой константой.
Лечение: время шага не угадывают, а замеряют. Перед каждым интегрированием снимаю millis(), вычитаю прошлое значение, делю на тысячу — честная доля секунды готова. Поменяли delay, добавили печать, повесили ещё датчик — dt подстроится сам, потому что он измеряется, а не вбит. Ритм без задержек разобран в уроке про millis вместо delay.
Почему именно секунды: скорость приходит в градусах в секунду, значит, и множитель времени должен быть в секундах. millis() отдаёт миллисекунды, отсюда деление на 1000.0 — выходит честная доля секунды. Тут же кроется второй грабёж: напишете 1000 без дробной части — целочисленное деление съест остаток, шаг в двадцать миллисекунд превратится в нулевой dt, и угол замрёт. Я гонял такого призрака однажды полвечера: скетч выглядел живым, но печатал одно и то же число, пока dt не уехал в Serial. С тех пор делитель в моих скетчах всегда пишется с точкой.
Второй нюанс — ось и знак. Наклон в плоскости X-Z, который мы считали из ax и az, — это вращение вокруг оси Y, и скорость его даёт канал gy. Оси на GY-521 подписаны, но монтаж у всех свой: на живой плате наклоните модуль в одну сторону и посмотрите, какой канал растёт, — так вы свяжете угол и скорость между собой за минуту.
// MPU6050: угол через гироскоп + комплементарный фильтр #include <Wire.h>#include <math.h> const byte MPU = 0x68;const float GY_LSB = 131.0; // единиц на градус в секунду при ±250 °/сconst float K_GYR = 0.98; // доля гироскопа в фильтре float ugol = 0; // оценённый угол, градусыfloat smesGy = 0; // смещение нуля гироскопаunsigned long tPosled = 0; int16_t syrye[7]; // ax ay az t gx gy gz void chitatVse() { Wire.beginTransmission(MPU); Wire.write(0x3B); Wire.endTransmission(false); Wire.requestFrom(MPU, 14); for (int i = 0; i < 7; i++) { int16_t hi = Wire.read(); int16_t lo = Wire.read(); syrye[i] = (hi << 8) | lo; }} float ugolAks() { // угол из акселерометра: плоскость X-Z chitatVse(); return atan2(syrye[0], syrye[2]) * 57.2958;} void setup() { Serial.begin(9600); Wire.begin(); Wire.beginTransmission(MPU); Wire.write(0x6B); Wire.write(0x00); // будим чип Wire.endTransmission(); // Калибровка: плату не трогать! среднее из 200 замеров gy long summa = 0; for (int i = 0; i < 200; i++) { chitatVse(); summa += syrye[5]; delay(10); // 200 замеров — две секунды } smesGy = summa / 200.0; ugol = ugolAks(); // стартовое значение — из акселерометра tPosled = millis();} void loop() { unsigned long now = millis(); float dt = (now - tPosled) / 1000.0; // честная доля секунды tPosled = now; chitatVse(); float gys = (syrye[5] - smesGy) / GY_LSB; // °/с вокруг оси Y float ua = atan2(syrye[0], syrye[2]) * 57.2958; ugol = K_GYR * (ugol + gys * dt) + (1.0 - K_GYR) * ua; Serial.print("syroj="); Serial.print(ua, 1); Serial.print(" filtr="); Serial.println(ugol, 1); delay(20);}| Источник угла | Скорость | Слабое место | Где ставлю |
|---|---|---|---|
| Акселерометр (atan2) | реагирует сразу | шумит и врёт при тряске | уровни, неподвижные платформы |
| Гироскоп + интегрирование | гладкий, быстрый | плывёт: дрейф копится | короткие движения, замеры поворота |
| Комплементарный фильтр | быстрый и гладкий | нужна калибровка нуля | подвесы, балансиры |
| DMP внутри чипа | готовые кватернионы | тяжёлая библиотека, медленный I2C | когда лень писать фильтр |
Дрейф: один градус в секунду — это шестьдесят за минуту
Даже исправное интегрирование ползёт. Ноль гироскопа — не ноль: паспортный разброс смещения у MPU6050 достигает ±20°/с сразу после включения, а после прогрева остаётся хвост в десятые доли градуса в секунду. Звучит безобидно, но интегрирование копит и его: остаток 0,5°/с — это 30 градусов за минуту. Мой подвес через четверть часа ровно висел с показанием 23 градуса, и я ждал, пока он выровняется сам. Не выровнялся: смещение — константа конкретного кристалла, а не переходный процесс.
Лечение — замер при старте и вычитание навсегда: плату не трогают две-три секунды, скетч берёт среднее из 200–300 замеров gy и запоминает его как ноль. Это то же усреднение, что и калибровка акселерометра из прошлого урока. После калибровки на моих платах остаётся 0,1–0,3°/с — и этот остаток уже добивает комплементарный фильтр.
Вот как дрейф выглядит вживую. На самом спокойном экземпляре моих плат остаток после калибровки вышел около 0.06 градуса в секунду: я оставил платформу отдыхать на минуту — угол уплыл на 3-4 градуса. Тихо, без рывков: доли градуса за шаг, глаз не замечает, а оценка уже ушла. На типичных платах хвост вдвое-втрое хуже — числа выше по тексту. Если платформе предстоит держать угол час, даже лучший из остатков натечёт на десятки градусов, и тут никакая калибровка нуля не спасёт: нужна вторая опора, которая дрейфовать не умеет, — акселерометр.
Фильтр сводит обоих датчиков: ugol = 0.98 * (ugol + gys * dt) + 0.02 * ugolAks. Гироскоп ведёт угол от шага к шагу — быстро и гладко. Акселерометр раз в шаг подтягивает оценку к настоящей гравитации — медленно, зато навсегда: дрейфовать он не умеет в принципе. Коэффициент 0.98 при шаге 20 мс даёт характерное время подтягивания около секунды: медленное расхождение чинится, а быстрая тряска за секунду не успевает пролезть в оценку.
Цифры подбираются под задачу. Для уровня из урока про угол с фильтром хватало 0.8 на 0.2 — там нужна неподвижная правда. Для подвеса камеры беру 0.98 — дрожь рук не должна попадать в угол сервы. Подвинете коэффициент к единице — фильтр оглохнет для акселерометра, и дрейф вернётся. Картинку удобно смотреть в Serial Plotter: печатайте пары syroj= и filtr=, как устроены такие кадры — в уроке про график.
Где я не связываюсь с фильтрами: когда хватает DMP. Чип умеет считать ориентацию кватернионами сам, библиотека i2cdevlib умеет это доставать — но тянет приличный код и небыстрый обмен по I2C. Алгоритмы Мадгвика и Махони живут на Uno, однако съедают миллисекунды на проход; для Pro Mini на 8 МГц, по моему опыту, уже тесно. Десять строк комплементарного фильтра выигрывают в девяти проектах из десяти.
Что в симуляторе честно, а что упрощено. Честно: сырые значения, dt из millis(), сама формула — скетч переносится на живую плату без правок, оживут только числа gy. Упрощено: вращение. Движок умеет наклонять платку, но не вращать её, поэтому gx/gy/gz остаются нулями и слагаемое с гироскопом вырождается. Фильтр в симе работает сглаживателем, и это видно: syroj дёргается за мышью, filtr идёт по пяткам с отставанием.
Куда двигаться дальше: повесить оценку угла на серву и получить подвес — камера на пан-тилте из двух серво просится именно сюда, комплементарный фильтр гасит дрожь рамы раньше, чем она доберётся до сервомашинки. Или собрать балансир, где фильтр кормит моторный мост. Компромисс скорость-шум и подбор коэффициента — та же арифметика, что в уроке про фильтры АЦП, только на входе не аналоговый вход, а atan2.
- 1. Что выдаёт канал gy при умолчательных настройках?
- 2. Платформа вращается со скоростью 90°/с при диапазоне ±250°/с. Что вернёт gy?
- 3. Как правильнее всего получить dt для интегрирования?
- 4. Откуда берётся дрейф угла гироскопа?
- 5. Зачем в комплементарном фильтре слагаемое от акселерометра?
- Гироскоп меряет скорость: 131 единица на °/с при ±250°/с, регистры с 0x43, старший байт первым.
- Угол — это сумма скоростей, умноженных на dt; dt берите из millis(), а не из константы примера.
- Смещение нуля замеряйте при старте: 200–300 замеров за две секунды, среднее — и вычитайте всегда.
- Комплементарный фильтр 0.98/0.02: гироскоп ведёт шаг, акселерометр не даёт оценке сползать; подвинули коэффициент к единице — вернули дрейф.
- Контроль качества за две минуты: плата на столе, угол не ползёт — dt и калибровка в порядке.
Частые вопросы по теме урока
- Что выдаёт канал gy при умолчательных настройках?
- Гироскоп меряет скорость вращения. При диапазоне ±250°/с масштаб — 131 единица на °/с; угол из него ещё предстоит интегрировать. Угловую скорость: 131 единица на один градус в секунду
- Платформа вращается со скоростью 90°/с при диапазоне ±250°/с. Что вернёт gy?
- 90 °/с умножаются на 131 единицу на °/с — получается 11790. Обратно делим на 131.0 и получаем градусы в секунду. 11790
- Как правильнее всего получить dt для интегрирования?
- Время шага замеряется, а не угадывается: любая константа рассыпется, как только изменится скетч — появится печать или лишний датчик. dt = (millis() − tPosled) / 1000.0 перед каждым шагом
- Откуда берётся дрейф угла гироскопа?
- Интегрирование копит всё, что осталось после вычитания нуля. Остаток 0,5°/с превращается в 30 градусов за минуту. Смещение нуля и шум: маленькая ложная скорость прибавляется каждый шаг интегрирования
- Зачем в комплементарном фильтре слагаемое от акселерометра?
- Гироскоп быстрый, но плывёт; акселерометр шумит, но не дрейфует. Два процента за шаг привязывают угол к гравитации навсегда. Оно не даёт оценке сползать: акселерометр не дрейфует на длинной дистанции