MPU6050: вычисляем угол наклона

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

Обе беды лечатся одной формулой из двух строк — комплементарным фильтром. Акселерометр медленный, но в среднем не врёт; гироскоп быстрый, но плывёт. В этом уроке соберём угол из обоих своими руками: арктангенс из проекций веса, интегрирование скорости с настоящим шагом времени, калибровка нуля и правильная доля каждого датчика. Фильтр целиком работает в симуляторе — коэффициенты можно крутить прямо в браузере.

Отправная точка — сырые оси из урока про акселерометр и гироскоп MPU6050: будим чип нулём в регистр 0x6B, читаем байты со сдвигом, делим на цену отсчёта. Если сырые числа у вас уже приходят правильными, всё дальнейшее — чистая математика над ними, никакой новой периферии не понадобится.

Акселерометр в покое честно показывает, куда смотрит земля: на ровном столе весь 1 g достаётся оси Z, а X и Y сидят на нуле. Наклоните плату — вес перераспределится, и угол вылезает из двух проекций: atan2(ax, az) возвращает наклон в радианах, умножаем на 57,2958 и получаем градусы. Диапазон такой схемы — плюс-минус 90 градусов: когда ось X встанет вертикально, арктангенс выдать больше не сможет.

Ловушка прячется в слове «покой». Акселерометр измеряет всё, что давит на пластину: вибрацию моторов и собственное ускорение робота тоже. На разгоне проекция Z проседает, и угол уезжает на десятки градусов — я видел, как при старте моторов «горизонт» улетал на сорок. Поэтому из акселерометра берём угол только в среднем: это долгосрочная правда, по которой сверяются, а не мгновенная стрелка.

Почему atan2, а не деление и обычный atan: при вертикальной плате знаменатель обращается в ноль, и деление в лоб рвёт угол. atan2(ax, az) спокойно съедает нулевой знаменатель и выдаёт законные 90 градусов. Сырые отсчёты можно подавать прямо в него — делить на 16384 не обязательно, отношение двух осей от масштаба не меняется.

И проверка здравым смыслом, которую я ставлю в каждый скетч с углом: если суммарная длина вектора ускорения в разы отличается от 1 g, значит плата ускоряется или трясётся — верить такому углу нельзя. Флаг «покой» из этого сравнения поднимается одной строкой sqrt(ax*ax + ay*ay + az*az) и дальше решает, подпитывать фильтр от акселя или пережидать: без этого флага робот на разгоне показывает горизонт, которого нет.

Гироскоп отвечает на другой вопрос: не «под каким углом я стою», а «с какой скоростью вращаюсь». Скорость в градусах за секунду умножаем на шаг времени dt и прибавляем к углу: angle = angle + gz * dt. Шаг берём настоящий, из millis: dt = (millis() - tPrev) / 1000.0, и tPrev обновляем в каждом кадре. Я однажды написал dt константой при loop() с задержками разной длины — угол стал отставать от реальности так, что робот «видел прошлое». Про таймер без блокировок я подробно писал в уроке про millis вместо delay. Кадр в 10-20 мс здесь не украшение: чем мельче шаг, тем меньше ошибка от того, что за время между замерами реальная скорость успела поменяться.

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

Сам фильтр — одна строка: angle = 0.98 * (angle + gz * dt) + 0.02 * accAngle. Девяносто восемь процентов — быстрый, но плывущий гироскоп, два процента — медленный, но честный акселерометр. Между опросами угол крутит гироскоп, а каждые несколько кадров аксель мягко тянет его к реальному наклону, не давая уплыть. Коэффициент — не магия, а торг: мотор трясёт плату — урежьте долю акселя до 0.01, иначе вибрация пролезет в угол; робот медленный и аккуратный — смело 0.05, правда подтянется быстрее. Мой рабочий коридор — 0.95-0.99: за его пределами либо фильтр сползает за акселем, либо дрейфует на гироскопе. Формула — упрощённый фильтр Калмана, и для девяти самодельных балансиров из десяти её хватает с запасом.

Коэффициент я подбирал на трёх проектах, и все три выбрали себе разные числа. Балансирующий робот живёт на 0.985: корпус трясёт мотором, а падение стоит дорого — значит, верим почти только гироскопу, и аксель подтягивает накопленный дрейф раз в несколько секунд. Наклономер для тепличной шторы — 0.95: моторов рядом нет, зато важно, чтобы показания честно догоняли медленный наклон, и тридцать секунд на полное обновление угла там никого не смущают. Указатель антенны на 0.99 дрейфовал два градуса за минуту — пришлось возвращать акселю долю побольше.

Не подбирайте число наугад — логируйте разрыв источников. Я печатаю в Serial пару acc= и compl= и смотрю, как далеко фильтр отстаёт от акселя после резкого наклона: если разрыв копится — доля акселя мала; если угол дрожит вместе с акселем — велика. Замеры удобно вести приёмом из урока про таблицу данных в Serial, а график — в Serial Plotter.

И про честность на упаковке: комплементарный фильтр — не фильтр Калмана, и на сложных траекториях с несколькими осями сразу он даёт рывки при проходе через 180 градусов. Для двухосевых наклонов и балансиров этого не видно; когда понадобится полный ориентационный расчёт — берите готовую библиотеку с Махони или Мадгвиком, но её формулы перестанут выглядеть заклинанием ровно после этого урока.

В симуляторе калибровка честно отработает: заглушка шины отвечает 42, среднее двухсот отсчётов — 10794, и после вычитания гироскоп стоит ровно в нуле. Угол же ползёт от нуля к сорока пяти и замирает: это atan2 двух одинаковых осей-заглушек тянет фильтр к себе. На живой плате числа будут настоящими: наклоните плату — угол поедет за ней, и Serial Plotter покажет, как гиро-часть мгновенно ловит поворот, а аксель-часть чистит накопившуюся мелочь.

Подкрутите коэффициент в симуляторе: 0.90 — угол дотянет до сорока пяти заметно быстрее, 0.99 — будет ползти неспешно. Это и есть торг между скоростью реакции и устойчивостью к шуму. Мой протокол проверки на столе занимает три минуты: тридцать секунд покоя — дрейф меньше градуса; наклон до упора и обратно — угол обязан вернуть исходное число; тридцать секунд с гудящим мотором рядом — дрожь не больше градуса. Все три пункта прошли — фильтр можно нести в робота, а угол выводить на LCD без мерцания.

Дальше — соседняя задача: курс, куда смотрит плата по сторонам света. Магнитометр QMC5883L из урока про компас по I2C без наклонной поправки врёт на десятки градусов, и именно ваш свежесобранный угол станет той поправкой. Так две строки из этого урока превращаются в навигационную систему всей самоделки.