Робот-балансир на 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. Робот уже записує журнали своїх запусків, зокрема для того, щоб такій мережі було на чому вчитися. Поки що немає ні того, ні іншого. Коли вони з’являться, їм дістануться ті самі поштовхи, що й ПІД-регулятору тут, і ми опублікуємо числа.