Знакомство с робототехническим конструктором ТРИК: обратный маятник
Что общего между женской грудью и игрушечной железной дорогой? Правильно, и то, и то предназначено для детей, а играют с ними папы. Несколько дней назад я обзавёлся роботехническим конструктором ТРИК. Комплект довольно суровый, разработчики утверждают, что он хорош для быстрого прототипирования и для обучения, а именно (само-)обучение меня в данный момент и интересует.
Что сейчас широко доступно на рынке для робототехнических игр? Самодельное изготовление плат под каждый проект не рассматриваем. Лего, распи, ардуино. Лего прекрасен, но, к сожалению, очень и очень сильно ограничен. Распи и ардуины неплохо расширяются, но довольно неудобны и быстро превращаются в рассыпуху разных карточек-шильдиков-макеток. Вот тут и выходят на рынок питерские ребята со своим конструктором ТРИК.
Итак, моя задача понять, насколько это доступно широкой публике (мне). Я никогда не посещал лекций ни по теоретической кибернетике, ни по теории управления. Закон Ома я выучил ровно настолько, чтобы понять, что розетку лизать не стоит, и паяльник не является моим другом. Но как всякий нормальный (великовозрастный) ребёнок играть я люблю, и поэтому заинтересовался этой темой.
Я получил вот такой набор: 
Вообще цена их наборов варьируется от примерно двадцати до семидесяти тысяч рублей. Дороже ли это, чем лего? Нет. Месяц назад я купил 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):
У меня довольно быстро (за один вечер) получилась самобалансирующаяся тележка. Учитывая, что программа у меня получилась аж в двадцать пять строк, то теперь на досуге осталось добавить управление тележкой, повороты и прохождение полосы препятствий, это не должно составить больших сложностей.
Ни электротехнического, ни кибернетического бэкграунда у меня нет, то есть, это вполне доступно рядовым пользователям, что, собственно, мне и очень интересно. Буду продолжать изучение!
По сравнению с тем же леговским набором оно всё, конечно, выглядит несколько топорнее, китайские датчики (как и у лего), но не закатанные в толстенный качественный леговский пластик. Менее вылизанный софт, который находится в активной разработке. Существенно меньше сообщество людей, программирующих на этом контроллере, но учитывая, что проекту без году неделя, да ещё видя недавний успех ардуино, это меня мало пугает. Зато меня восхищает гибкость контроллера, которая получилась из потрясающго энтузиазма, с которым в Питере взялись за разработку.
