Знайомство з робототехническим конструктором ТРИК: зворотний маятник

Вступ і постановка завдання


Що спільного між жіночими грудьми і іграшковою залізницею? Правильно, і те, і те призначено для дітей, а грають з ними папи. Кілька днів тому я обзавівся роботехническим конструктором ТРИК. Комплект досить суворий, розробники стверджують, що він хороший для швидкого прототипування і для навчання, а саме (само-)навчання мене в даний момент і цікавить.

Що зараз широко доступно на ринку для робототехнічних ігор? Саморобний виготовлення плат під кожен проект не розглядаємо. Лего, розпи, ардуїнов. Лего прекрасний, але, на жаль, дуже і дуже сильно обмежений. Розпи і ардуины непогано розширюються, але досить незручні і швидко перетворюються в рассыпуху різних карток-шильдиків-макеток. Ось тут і виходять на ринок пітерські хлопці зі своїм конструктором ТРИК.

Отже, моє завдання зрозуміти, наскільки це доступно широкому загалу (мені). Я ніколи не відвідував лекцій ні з теоретичної кібернетики, ні з теорії управління. Закон Ома я вивчив рівно настільки, щоб зрозуміти, що розетку лизати не варто, і паяльник не є моїм другом. Але як кожен нормальний (дорослий) дитина грати я люблю, і тому зацікавився цією темою.



Я отримав ось такий набір:


Взагалі ціна їх наборів варіюється від приблизно двадцяти до сімдесяти тисяч рублів. Дорожче, ніж лего? Немає. Місяць тому я купив Lego EV3. Ціна питання 370€ базовий набір + 100€ акумулятор (вони там зовсім офігіли?!) І це ще я не вважав зарядника за тридцять євро. Плюс до всього в базовий набір не входять ні сонар (+35€), ні гіроскоп (+35€). А вже про камеру з мікрофоном і взагалі можна забути, не згадуючи взагалі в принципі отустствия доступу всередину леговского лінукса.

В мій набір (він і розкладений на попередній фотографії) входять два контролера, дві камери, два мікрофони, сонари, два типи інфрачервоних датчиків, кнопки, шість електродвигунів з оптоэнкодерами, три серви, два механічних захоплення, купа коліс, у тому числі голономных, зарядки-акумулятори-шнурки, зубчасті колеса, рейки і купа металевих пластин і куточків (привіт дитинство!). Конструктор чисто для початку, взагалі до контролера можна підключити практично все, на що вистачить уяви. Центральний процесор ARM9, під відео окремий процесор, щоб не вантажити центральний. Програмувати можна на чому завгодно від асемблера до C#, вам дають рутовую консоль, плюс весь код прошивки опенсорсний.

Ось так виглядає моя диво-коробка:




В якості першого проекту я вирішив зібрати зворотний маятник, він і представлений на головній картинці. Ось так він виглядає ззаду:

З набору мені знадобилося два двигуна, два колеса, трохи кріплення і безпосередньо контролер з батареєю. Моєю метою не було скопіювати туторіал з навчального курсу ТРИК, мені цікаво обламати зуби про всі проблеми самостійно, тому я буду винаходити велосипед.

Отже, у моєму розпорядженні одна ступінь свободи, акселерометр і гіроскоп, енкодери від двигунів я не використовував. Писати буду на Qt Script.

Читання датчиків
Спочатку я хотів обійтися одним акселерометром. Типу, читаю знак проекції на вісь Z, якщо він позитивний, то кручу колеса в один бік, якщо негативний, то в іншу. В теорії все добре, але зробивши це, домігся лише дикого дрыганья мого візка. Зітхнув і сів студіювати літературу, благо, що вона виявилася не дуже довгою. Отже, мене цікавить тільки кут відхилення візки від вертикалі.

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

Ось мій код роботи з гіроскопом, тут while(true) — основний цикл програми.
var gyr_x_angle = 0;
var lasttime = Date.now();
while (true) {
var G = brick.gyroscope().read();
G[0] = G[0] + 69; // drift correction
var curtime = Date.now();
var dt = (curtime - lasttime)/1000.0;
lasttime = curtime;
gyr_x_rate = G[0] * 0.07;
gyr_x_angle = gyr_x_angle + gyr_x_rate * dt;
}


В масив G я читаю значення датчика, в наступній сходинці виробляю корекцію цікавить мене осі. З'ясовується, що мій конкретно датчик в повному спокої показує в середньому швидкість в 69 одиниць, тому я їх вычитаю, щоб отримати цікаву для мене швидкість.
Датчик видає ціле число, яке потрібно перевести в кути. У штатному режимі він працює на 2000 градусів/с (dps). Даташит каже, що цьому відповідає константа в 70mdps/digit. Таким чином, G(digits) * 0.07 (dps/digit) дає нам кутову швидкість. Залишилося її проінтегрувати, помноживши на час вимірювання dt.

Акселерометр
З акселерометра кут отримати ще простіше, однак проблема в тому, що аж надто він галасливий, а вже коли візок починає сіпатися туди-сюди, взагалі туші світло. Ось так виглядає код:
[...]
while (true) {
[...]
var A = brick.accelerometer().read();
var a_x_angle = Math.atan(A[2] / A[0]) * 180.0 / pi;
}


Гасимо шум: суміщення показань акселерометра і гіроскопа
[...]
var CF_x_angle = 0;
while (true) {
[...]
CF_x_angle = 0.98*(CF_x_angle+ gyr_x_rate*dt) + 0.2*a_x_angle;
}

Це просто говорить, що значення поточного кута це на 98% значення попереднього кута з поправкою від гіроскопа, а на 2% — це пряме читання кута від акселерометра. Таке поєднання дозволяє боротися з уплыванием гіроскопа, зверніть увагу, що змінну gyr_x_angle ми тут взагалі не використовували.

ПІД-регулятор
Як я вже говорив, я не відвідував розумних лекцій з теорії управління, LQR-регулятори мені не по зубах за розумне (пара годин) час. А ось ПІД цілком підійде.

Код

В попередньому параграфі ми отримали пристойну (я сподіваюся) оцінку кута відхилення візки від вертикалі. Поліпшити її можна, використовуючи фільтрації Калмана, але це стрілянина з гармати по горобцях. Тепер прийшов час крутити колеса — чим більше кут відхилення, тим швидше потрібно крутити колесо.

Швидкість обертання колеса, яку нам видасть ПІД, складається з трьох (зважених) доданків: пропорційною, интегирующей і диференціюючою. Пропорційна просто дорівнює куту відхилення від вертикалі, інтегруюча — це сума всіх помилок регулювання, а дифференцирующая пропорційна швидкості зміни відхилення від вертикалі.

Звучить це страшно, а на ділі код вкрай простий:
повний код програми
var gyr_x_angle = 0;
var lasttime = Date.now();
var CF_x_angle = 0;
var iTerm = 0;
var CF_x_angle_prev = 0;

var KP = 0;
var KI = 0;
var KD = 0;

while (true) {
var G = brick.gyroscope().read();
G[0] = G[0] + 69; // drift correction
var curtime = Date.now();
var dt = (curtime - lasttime)/1000.0;
lasttime = curtime;
gyr_x_rate = G[0] * 0.07;

var A = brick.accelerometer().read();
var a_x_angle = Math.atan(A[2] / A[0]) * 180.0 / pi;

CF_x_angle = 0.98*(CF_x_angle+ gyr_x_rate*dt) + 0.2*a_x_angle;

// крутимо колеса!

var pTerm = KP*CF_x_angle; // пропорційна складова

iTerm = iTerm + KI*CF_x_angle; // интегирующая складова

var dTerm = KD * (CF_x_angle - CF_x_angle_prev); // диференціальна складова
CF_x_angle_prev = CF_x_angle;

power = pTerm + iTerm + dTerm;
brick.motor(M3).setPower(power);
brick.motor(M4).setPower(power);
}



Вибір констант KP, KI, KD
Залишилася сама складна частина: знайти значення ваг в сумі, на жаль, це можна тільки робити емпірично.

Для початку знайдемо коефіцієнт KP. Покладемо KI і KD рівними нулю і збільшуємо KP починаючи з нуля до того моменту, коли наша візок почне здійснювати (приблизно) постійні коливання, приблизно ось так (KP=8, KI=0, KD=0):


Очевидно, що це перебір, візок отримує занадто сильний сигнал від пропорційної складової, тому зменшимо її приблизно наполовину, отримаємо ось це (KP=5, KI=0, KD=0):


Тепер візку не вистачає чисто пропорційного сигналу, збільшимо її швидкість, додавши інтегруючої компоненти. Плавно збільшуємо KI з нуля, намагаючись досягти моменту, коли знову отримаємо коливання візки навколо бажаного положення (KP=5, KI=0.5, KD=0):


Тепер додаємо диференціюючу компоненту, яка буде грати роль демпфера, гасить коливання, ось що у мене виходить (KP=5, KI=0.5, KD=5):


Висновок
У мене досить швидко (за один вечір) вийшла самобалансирующаяся візок. Враховуючи, що програма у мене вийшла аж у двадцять і п'ять рядків, то тепер на дозвіллі залишилося додати керування візком, повороти і проходження смуги перешкод, це не повинно скласти великих труднощів.

Ні електротехнічного, ні кібернетичного бекграунду у мене немає, тобто, це цілком доступна пересічним користувачам, що, власне, мені і дуже цікаво. Продовжуватиму вивчення!

У порівнянні з тим же леговским набором воно все, звичайно, виглядає дещо топорнее, китайські датчики (як і в лего), але не закочені в товстелезний якісний леговский пластик. Менш вилизаний софт, який знаходиться в активній розробці. Істотно менше спільнота людей, програмуючих на цьому контролері, але враховуючи, що проектом без року тиждень, та ще бачачи недавній успіх ардуїнов, це мене мало лякає. Зате мене захоплює гнучкість контролера, яка вийшла з потрясающго ентузіазму, з яким в Пітері взялися за розробку.

Джерело: Хабрахабр

0 коментарів

Тільки зареєстровані та авторизовані користувачі можуть залишати коментарі.