Робот-балансир на ESP32: три контура управления

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

Это двухколёсный робот-балансир: напечатанная на 3D-принтере трёхэтажная рама на двух моторах-редукторах постоянного тока, плата ESP32 и датчик движения. Мы собрали его, чтобы сравнить разные способы удерживать робота в вертикальном положении на одном и том же железе, и начали с классического — ПИД-регулятора. Статья рассказывает о десяти днях от первого теста до прыжка в том порядке, в каком всё происходило: что внутри, откуда робот знает, где верх, три контура один за другим и чему нас научило управление по Wi-Fi. Числа взяты из журналов самого робота.

Робот

Рама — это три напечатанных этажа, поставленных друг на друга, и каждый из них — отдельная деталь. Мы спроектировали её во Fusion 360 вместе с покупными компонентами, так что у моторов и драйверов были свои места ещё до начала печати.

Снизу вверх: моторы висят под нижним этажом на кронштейнах из комплекта. На первом этаже стоят два драйвера моторов, радиаторами вниз. Второй этаж — мозг: плата ESP32 со своим маленьким экраном на макетной плате, датчик движения и зуммер. На верхнем этаже — трёхбаночный аккумулятор с платой защиты, выключатель и понижающий модуль на 5 В. Корпус — 164 × 128 × 60 мм, ширина с колёсами — 235 мм, а весь робот весит 1025 г.

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

И ещё: два мотора — не близнецы. На съёмке с уровня пола одна и та же команда ШИМ качнула дальнее колесо на 18 мм, а ближнее — на 8 мм. Команда была одинаковой; почему отклик разный, мы не выяснили. Если ничего не делать, из-за этой разницы робот, балансируя, медленно поворачивается на месте, и с этим справляется небольшая поправка по курсу, о которой рассказано в разделе про управление по Wi-Fi.

Где верх

Всё, что делает балансирующий робот, зависит от одного числа — его наклона. MPU-6500 даёт два способа его узнать, и ни один не работает сам по себе.

Гироскоп измеряет, как быстро поворачивается корпус. Суммируйте его показания каждые 2 мс — и получите угол, гладкий и быстрый, но он дрейфует: без поправки на собственное смещение нуля гироскопа он, по нашим замерам, уходил примерно на 15° в минуту. Акселерометр знает, куда направлена сила тяжести, поэтому его угол дрейфовать не может, но он чувствует и каждый толчок. При работающих моторах его угол скакал в диапазоне шириной 18,7°.

Комплементарный фильтр берёт у каждого лучшую половину. На каждом шаге гироскоп продвигает угол дальше, а затем угол подтягивается к показанию акселерометра на крошечную долю разницы — одну тысячную. Иначе говоря: 99,9 % гироскопа и 0,1 % акселерометра, что соответствует постоянной времени в две секунды. На стенде отфильтрованный угол держался в пределах шести сотых градуса.

В прошивке всё сводится к нескольким строкам. Плата датчика стоит на ребре с креном 1,4°, поэтому первым делом код разделяет скорость по тангажу, которая нужна фильтру, и скорость по рысканию, которой позже пользуется код поворота:

// Leaning forward, the forward axis dips below the horizontal:
// its reading is -sin(theta).
att.thetaAcc = atan2f(-(float)s.accel[1], (float)s.accel[0])
               * DEG_PER_RAD;

// The board is rolled by 1.4 deg: take pitch and yaw rates apart.
const float gx = s.gyro[0] / GYRO_LSB_PER_DPS - att.gyroBiasDps[0];
const float gz = s.gyro[2] / GYRO_LSB_PER_DPS - att.gyroBiasDps[2];
att.omegaPitch = gz * rollCos - gx * rollSin;
att.omegaYaw = gx * rollCos + gz * rollSin;

// Complementary filter: the gyroscope carries the angle from
// sample to sample, the accelerometer pulls it back to gravity.
// FILTER_K = dt / (tau + dt) = 0.002 / 2.002, about 0.1 %.
att.theta = wrapDeg(att.theta + att.omegaPitch * IMU_DT);
att.theta = wrapDeg(att.theta
                    + FILTER_K * wrapDeg(att.thetaAcc - att.theta));

Шаг управления идёт в такт самому датчику. Аппаратный таймер будит задачу управления прямо перед каждым ожидаемым измерением, задача читает его и прогоняет весь регулятор, а экран и журнал запуска ждут своей очереди с более низким приоритетом. Wi-Fi живёт на другом ядре. Эта схема пришла из нашего прежнего балансира на пропеллере, собранного на той же плате и с тем же датчиком.

Первый контур: угол

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

• P — пропорциональная составляющая: kp = 30, то есть один градус ошибки требует 30 % полной мощности.
• I — интегральная составляющая: ki = 30 накапливает ошибку во времени и продолжает давить, пока ошибка держится.
• D — дифференциальная составляющая: kd = 0,3 гасит раскачку. Скорость она берёт прямо с гироскопа, а не дифференцирует шумный угол.

Уставка — не ровно ноль. Центр масс находится не точно над осью колёс, поэтому робот балансирует при θ_ref = 0,3°.

float pidStep(float theta, float omegaPitch, float thetaRef,
              const Params &p, float dt, float *raw) {
    const float error = theta - thetaRef;
    const float summed = integral + p.ki * error * dt;
    integral = constrain(summed, -p.uMax, p.uMax);
    const float u = p.kp * error + integral + p.kd * omegaPitch;
    *raw = u;
    return constrain(u, -p.uMax, p.uMax);
}

Первый запуск на полу провалился так, как на стенде увидеть было нельзя. В воздухе все проверки проходили: колёса крутились, энкодеры считали, датчик видел наклон. На полу же колёса уезжали от наклона, а не подъезжали под него, и робот упал. Угол был верный, а вот «вперёд» у моторов и у энкодеров оказалось перевёрнутым. Когда в конфигурации инвертировали и то и другое, робот встал, а позже с этими коэффициентами простоял больше двух минут со стандартным отклонением угла 0,2°.

Здесь возможности одного контура заканчиваются. Во время того толчка корпус ни разу не наклонился больше чем на полградуса, а робот откатился на 27 см и там и остался. Для контура угла это успех: с углом всё в порядке. Для него нет разницы, стоит робот на месте или катится с постоянной скоростью: на нашем полу, чтобы держать скорость v, нужно около 4,5 % мощности плюс 78 % на каждый метр в секунду, и интеграл просто сохраняет ту мощность, которую ему оставил толчок. Любая скорость для него неотличима от равновесия.

Второй контур: скорость

Второй контур следит за колёсами. Он срабатывает 100 раз в секунду, читает оба энкодера и сравнивает скорость робота с той, какой она должна быть, — пока это ноль. Моторов он не касается вовсе: на выходе у него угол, который должен держать первый контур. Робот катится вперёд? Значит, нужно чуть наклониться назад — и контур угла, догоняя новый угол, затормозит робота. Это ПИ-регулятор, и его интеграл ограничен теми же несколькими градусами, что и выход:

float pidSpeed(float vRef, float v, const Params &p, float dt,
               bool hold) {
    const float error = vRef - v;
    const float summed = hold ? speedIntegral
                              : speedIntegral + p.kvi * error * dt;
    speedIntegral = constrain(summed, -p.thetaMax, p.thetaMax);
    return constrain(p.kv * error + speedIntegral,
                     -p.thetaMax, p.thetaMax);
}

На первой попытке робот дёргался пять раз в секунду. Коэффициент взяли из модели, в которой контур угла был втрое слабее настоящего: kv = 10 рассчитывали под kp = 10, а робот работает с kp = 30. Кроме того, модель не учитывала десятую долю секунды, которая нужна колёсам, чтобы отреагировать. Помог не меньший коэффициент, а более спокойное измерение: скорость теперь усредняется за 200 мс вместо 40 мс, а усреднение за 200 мс как раз полностью гасит колебания с частотой 5 Гц.

Потом робот начал уползать. С чисто пропорциональным контуром скорости, kvi = 0, восемь запусков из десяти закончились дальше чем в 10 см от места старта. Небольшой интеграл, kvi = 1, это исправил: из следующих одиннадцати запусков ни один не уполз дальше чем на 2,7 см.

А потом он сделал то, о чём его никто не просил: после толчка он вернулся.

Всё дело в интеграле. Интеграл скорости — это просто пройденный путь, поэтому, чтобы удержать смещённого робота на нулевой скорости, интеграл скорости в итоге держит небольшой наклон — около одного градуса на каждый метр от точки старта. Это слабая скрытая пружина, которая тянет робота домой. Именно слабая: робот возвращается не в точку, а в полосу шириной около 30 см, и в этом дубле две точки, где он останавливался, оказались в 34 см друг от друга. Наша догадка: внутри полосы наклон слишком мал, чтобы выбрать люфт и провернуть редуктор, но это догадка, а не измерение. Дубль выше снят с kv = 5, а закрыли этап с kv = 7.

Третий контур: положение

Третий контур помнит, откуда робот стартовал, а это место мы отметили маленьким напечатанным корабликом. Двадцать раз в секунду контур измеряет расстояние от дома и просит у контура скорости скорость возврата. Его коэффициент kx = 1,5, так что в десяти сантиметрах от дома он попросил бы 15 см/с, но больше 10 см/с не просит никогда. Положение задаёт скорость, скорость задаёт угол, а угол управляет моторами.

Расстояние берётся с энкодеров с поправкой на наклон: когда корпус наклоняется, колёса подкатываются под него, даже если робот в целом стоит на месте. Вот каскад в том виде, в каком он работает внутри шага на 500 Гц, немного сокращённый: ветка, через которую роботом управляет ПК, опущена.

// Every step runs at 500 Hz: position every 25th, speed every 5th.
if (positionPhase == 0) {
    vRefNow = pidPosition(xHold + params.xRef, xNow, params);
}
positionPhase = (positionPhase + 1) % POSITION_EVERY;

if (speedPhase == 0) {
    // Hold the speed integral while a hand carries the robot away.
    const bool limited = fabsf(vRefNow) >= params.vMax;
    const bool carried = (vRefNow > 0) ? (vNow < -AWAY_M_S)
                                       : (vNow > AWAY_M_S);
    thetaOffset = pidSpeed(vRefNow, vNow, params,
                           SPEED_EVERY * IMU_DT, limited && carried);
}
speedPhase = (speedPhase + 1) % SPEED_EVERY;

thetaRefNow = params.thetaRef + thetaOffset;
percent = pidStep(a.theta, a.omegaPitch, thetaRefNow, params,
                  IMU_DT, &uCtl);

У строки с «carried» есть своя история. В первой версии робот после толчка возвращался со скоростью от 20 до 46 см/с и проскакивал свою точку на 10–24 см. Интеграл скорости копился всё время, пока толкала рука, и запоминал эту руку. Заморозка интеграла всякий раз, когда контур положения просил полную скорость, сократила перерегулирование примерно до 5 см, но тогда после сильных толчков робот не доезжал до дома 12–27 см. Прижилось правило, которое замораживает интеграл, только пока контур просит полную скорость обратно, а робот всё ещё удаляется от дома быстрее 1 см/с, — то есть пока его несёт рука. С этим правилом после толчков на 68 и 80 см робот возвращался с точностью до 3 см и без перерегулирования.

Сам kx подбирали, двигая целевую точку по Wi-Fi и глядя, как робот за ней следует, — по 36 перемещений на каждое значение:

Точно к кораблику он не возвращается: останавливается, не доехав до него или проехав его на два-три сантиметра. Там робота держит то же трение, из-за которого с двумя контурами он мог осесть где угодно в своей полосе. Толчок на 50 см в другую сторону он отработал за 8,6 секунды. Поставьте три этапа рядом — и прогресс очевиден:

Управление по Wi-Fi

Раз робот умеет держать точку, то управлять им — значит двигать эту точку. ESP32 подключается к домашней сети Wi-Fi, а на ПК небольшой мост на Python раздаёт веб-страницу со стрелками: клавиатура, геймпад или экранный джойстик. Мост двадцать раз в секунду пересылает роботу по UDP самую свежую команду, и робот сразу отвечает на каждую своей телеметрией: угол, скорость, положение, курс, мощность, напряжение аккумулятора и уровень сигнала. Пока кнопка управления нажата, место выхода контура положения занимает скорость движения с плавным разгоном; когда её отпускают, контур положения держит то место, где робот остановился.

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

// Balance first: the turn gets what is left of the limit, so the
// mean of the two wheels stays the angle loop's command.
static void driveApart(float percent, float turn, float limit) {
    drivePercent = percent;
    const float room = max(limit - fabsf(percent), 0.0f);
    uTurnNow = constrain(turn, -room, room);
    motorDrive(Wheel::Left, percent - uTurnNow);
    motorDrive(Wheel::Right, percent + uTurnNow);
}

Рабочие пределы — 0,35 м/с и 75° в секунду. Последний запуск 8 ноября длился 150 секунд: 28 метров езды с пиками до 0,57 м/с, два прыжка с весов и ни одного падения. При первом приземлении корпус на мгновение наклонился до +7°, и робот устоял.

Не каждый прыжок проходил так гладко. Раньше в тот же день после приземления с небольшого трамплина робот наклонился вперёд на 9°. Чтобы удержаться, он разогнался примерно до метра в секунду и въехал прямо в кровать. Чтобы робот откинулся назад и затормозил, колёса сначала должны обогнать корпус, а команда мощности уже упиралась в свой предел 85 %: на такой скорости вывести их вперёд было уже нечем. С тех пор предел подняли до 100 %, но на полу мы это ещё не пробовали.

Что не даёт ему навредить себе

• Запускается только намеренно. Нажатие кнопки взводит его, о чём сообщают три писка, а включается он, когда наклон в течение 0,2 с остаётся в пределах 5° от вертикали.
• Сдаётся при 25°. При большем наклоне ему уже не выправиться, поэтому моторы отключаются, а не прокручивают колёса по полу.
• Свой сторожевой таймер. Отдельный аппаратный таймер снимает сигнал с линии разрешения (enable) драйверов, если от датчика 10 мс не приходило ни одного измерения. Зависшая шина I²C может заблокировать задачу управления примерно на секунду, а робот, который секунду едет вслепую, — это робот в стене.
• Проверки аккумулятора и датчика. Робот останавливается при разряженном аккумуляторе и при отказе датчика. Потеря связи по Wi-Fi останавливает только езду: робот замирает на месте и продолжает балансировать.

Весь проект в ролике

Ролик рассказывает ту же историю за восемь с половиной минут, с данными из журналов поверх кадров. Плеер загружается с YouTube, только когда вы на него нажмёте.

Воспроизвести видео: Я собрал робота-балансира: три контура управления 8:40

Я собрал робота-балансира: три контура управления

• Смотреть на YouTube

Видео загружается с YouTube после нажатия кнопки воспроизведения. Политика конфиденциальности

Что дальше

Десять дней, три контура, один робот, который стоит. Каскад ПИД работает, но это набор частей, настроенных вручную, и каждую добавляли, чтобы исправить то, чего не умела предыдущая.

Дальше мы хотим заменить все три контура одним LQR-регулятором, коэффициенты которого рассчитываются по модели робота, а не подбираются человеком по одному. После этого хотим попробовать небольшую нейросеть, работающую прямо на ESP32. Робот уже записывает журналы своих запусков — отчасти для того, чтобы такой сети было на чём учиться. Ни того, ни другого пока нет. Когда они появятся, их ждут те же толчки, что и ПИД здесь, и мы опубликуем числа.