Создание робота балансера на arduino
Мне давно не давало покоя желание рассчитать какой-нибудь достаточно сложный механизм и воплотить его жизнь.Выбор пал на задачу об обратном маятнике. Итог на видео:[embedded content]Математическая модель
Я не буду приводить вывод уравнений движения, все таки это третий курс института. Для тех же, кому интересен вывод, в конце статьи ссылка, где он описан подробнее.Систему представим в следующем виде: 
Маятник это масса mp прикрепленная на конце невесомого стержня длины l. На другой конец стержня прикреплен двигатель, развивающий максимальный момент Mk и передающий его на колесо массой mw и радиусом r.Задача управления — стабилизировать маятник в вертикальном положении и возвращать в начальное положение колесо.
Уравнения движения, описывающие обратный маятник, представимы в следующем виде: 
Они кажутся довольно неприятными, но сам робот о них ничего не знает, а управление использует линеаризованную модель, то есть такую: 
Синтезирование управления Завидую людям, у которых работает PID-регулятор. Я потратил несколько часов на подгонку его коэффициентов, но так и не сумел добиться стоящего результата. Научный руководитель посоветовал воспользоваться линейно-квадратичным регулятором (вики). Этот регулятор, в отличие от PID-регулятора, представляет собой просто произведение своих коэффициентов на ошибки по каждой координате. Никаких дискретных аналогов производной и интеграла. Однако для его вычисления нужна модель системы и умение решать уравнение Риккати, ну или Matlab.В матлабе расчет регулятора представляет собой такой набор комманд:
A=[0 1.0 0 0;0 0 -140 0;0 0 0 1.0;0 0 28 0] B=[0;212.85;0;-19.15] Q=[5 0 0 0;0 5 0 0;0 0 1 0;0 0 0 1] R=1500 [K, S, e]=lqr (A, B, Q, R) Здесь матрицы A и B — соответствующие матрицы из линеаризованной модели с подставленными значениями реального робота.Матрица Q определяет на сколько нужно штрафовать систему за отклонение от начала координат, заметьте, в нашем случае в координаты входят скорости.Матрица R определяет на сколько нужно штрафовать систему за растрату энергии управлением.В переменной K будут лежать коэффициенты регулятора.
Симуляция
В Matlab simulink можно легко эмулировать систему, если кому-нибудь необходимо я могу поделиться моделью. Здесь же я только приведу графики.Угол отклонения маятника: 
Угол отклонения колеса: 
Момент двигателя: 
Реализация в железе
Сам каркас робота это алюминиевые профиля 12 мм и 14 мм, они входят друг в друга. Соединены заклепками. Электроника прикреплена на кусок стеклотекстолита в форме буквы T. Моторы так же прикреплены через стеклотекстолитовый переходник.
Изначально я пытался использовать такие моторы: 
Их крутящий момент 2,2 кг*см или 0.2Нм. Исходя из симуляции нам нужно гораздо больше, поэтому были выбраны другие моторы: 
ссылка на производителяМаксимальный крутящий момент 14 кг*см или 1.4Нм. Тока они потребляют до 5A, поэтому популярный у ардуинщиков L293D тут не подойдет.
Для определения угла и угловой скорости используется IMU — гироскоп и акселерометр. У меня завалялась плата с гироскопом L3G и акселерометром с магнетометром LSM303. Подобных плат очень много и я не стану приводить код получения значений сенсоров. Однако показания датчиков нужно отфильтровать, так как гироскоп постоянно уходит, а акселерометр шумит и сильно врет, если робот начинает двигаться не меняя угла.Используют разные фильтры, но наиболее популярны фильтр Калмана и RC-фильтр (complementary filter). Я использую такой код:
float lastCompTime=0; float filterAngle=1.50; float dt=0.005;
float comp_filter (float newAngle, float newRate) {
dt=(millis ()-lastCompTime)/1000.0; float filterTerm0; float filterTerm1; float filterTerm2; float timeConstant;
timeConstant=0.5;
filterTerm0 = (newAngle — filterAngle) * timeConstant * timeConstant;
filterTerm2 += filterTerm0 * dt;
filterTerm1 = filterTerm2 + ((newAngle — filterAngle) * 2 * timeConstant) + newRate;
filterAngle = (filterTerm1 * dt) + filterAngle;
lastCompTime=millis ();
return filterAngle;
}
Работает не идеально, но достаточно хорошо для данной задачи.Следующий сенсор — квадратурный энкодер на моторе. Он генерирует прямоугольные импульсы на 2х своих выводах: 
Считать их можно либо прерываниями, либо считыванием значений в цикле. На arduino playground есть замечательная статья с примерами кода.
Осталось получить угловую скорость колеса. Тут на помощь приходит школьная формула пройденное расстояние/затраченное время.
#define ToPhiRad (x) ((x)*0.00280357142)
timer_old = timer; timer=millis (); G_Dt = (timer-timer_old)/1000.0; dPhi=(ToPhiRad (encoder0Pos)-lastPhi)/G_Dt; ToPhiRad переводит количество тиков энкодера в угол колеса, мой энкодер выдает около 2240 тиков на оборот. Чтобы получить угол нужно умножить тики на 2 Пи и разделить на их количество при полном обороте колеса.Показания сенсоров поступают в LQR регулятор:
float K1=0.1, K2=0.29, K3=6.5, K4=1.12;
long getLQRSpeed (float phi, float dphi, float angle, float dangle){
return constrain ((phi*K1+dphi*K2+K3*angle+dangle*K4)*285,-400,400);
}
Коэффициенты взяты из Matlab, правда для большей стабильности я подправил 2 первых коэффициента.Мой драйвер, вернее его библиотека, принимает значения от -400 до 400. Я предположил, что на 400 он выдает на мотор 12В, т.е. мотор развивает максимальный момент (1.4Нм). Разделив 400 на 1.4 получаем коэффициэнт перевода из Нм, которые выдает LQR, в значения, понятные драйверу.Просто стабилизировать робота в одной точке не очень интересно, поэтому к нему добавился BT-модуль HC-05. Модуль подключается к серийному порту микроконтроллера. Он работает на 3.3В, а arduino на 5В, поэтому подключать принимающий вход модуля надо через делитель напряжения. Вот схема подключения: 
Во время цикла микроконтроллер опрашивает модуль на предмет символов:
float phiDif=0.f; float factorDif=0.f;
float getPhiAdding (float dif){ //сколько прибавлять к углу колес для движения вперед-назад
if (dif<200 && dif>-200){return 0.f;} float ret = dif*0.08;
return ret; }
float getFactorAdding (float dif){//сколько добавлять к управлению для поворота
if (dif<200 && dif>-200){return 0.f;}
float ret = dif/500×20;
return ret;
}
//========
if (Serial.available ()){
BluetoothData=Serial.read ();
if (BluetoothData=='w'){
phiDif=200;
} else if (BluetoothData=='s'){
phiDif=-200;
} else if (BluetoothData=='a'){
factorDif=200;
} else if (BluetoothData=='d'){
factorDif=-200;
} else if (BluetoothData=='c'){
factorDif=0;
phiDif=0;
}
}
В конечном итоге показания сенсоров поступают в регулятор, а его управление и воздействие пользователя поступают на моторы:
encoder0Pos+=getPhiAdding (phiDif);
lastPhi=ToPhiRad (encoder0Pos);
spd=getLQRSpeed (ToPhiRad (encoder0Pos), dPhi, balanceAt-angle, gyroRate[coordY]);
float factorL=getFactorAdding (factorDif);
md.setSpeeds (spd-factorL, spd+factorL);
Раз в 50 миллисекунд посылается телеметрия-угол робота:
if (millis ()%50==0){
Serial.println (angle);
}
Добавляем радиоуправление
Управлять будем с телефона под android.
При запуске приложения попросим пользователя выбрать к кому подключаться, bt-модуль должен быть уже сопряжен с телефоном (стандартный код 1234).
BluetoothAdapter bluetooth;
String []boundedItems;
protected static final int RECIEVE_MESSAGE = 1;
@Override
protected void onCreate (Bundle savedInstanceState) {
//…
bluetooth = BluetoothAdapter.getDefaultAdapter ();
if (bluetooth!= null){
if (! bluetooth.isEnabled ()) {
bluetooth.enable ();
}
}
Set
public void showListDialog (){ AlertDialog.Builder builder = new AlertDialog.Builder (this);
builder.setTitle («Pick a device»); builder.setItems (boundedItems, new DialogInterface.OnClickListener () {
public void onClick (DialogInterface dialog, int item) { connectTo (item); }
});
AlertDialog alert = builder.create ();
alert.show (); } После выбора устройства подключимся к нему: private static final UUID MY_UUID = UUID.fromString (»00001101–0000–1000–8000–00805F9B34FB»); BluetoothSocket btSocket;
public void connectTo (int id){
Set
public void draw (Canvas canvas) { Paint paint=new Paint (); paint.setStrokeWidth (3); canvas.save (); canvas.rotate ((float) (angle*180.f/Math.PI), cx, cy); paint.setColor (Color.BLACK); canvas.drawRect (cx-15, cy-150, cx+15, cy, paint); paint.setColor (Color.WHITE); canvas.drawRect (cx-12, cy-147, cx+12, cy-3, paint); paint.setColor (Color.BLACK); canvas.drawCircle (cx, cy, 30, paint); paint.setColor (Color.WHITE); canvas.drawCircle (cx, cy, 25, paint); canvas.restore (); } Комманды роботу посылаются при появлении событий onTouch, чтобы можно было управлять роботом удерживая кнопку. @Override public boolean onTouch (View v, MotionEvent me) { if (me.getAction ()==MotionEvent.ACTION_UP){ ct.sendCmd ('c'); return false; } if (v==wB){ ct.sendCmd ('w'); }else if (v==aB){ ct.sendCmd ('a'); }else if (v==sB){ ct.sendCmd ('s'); }else if (v==dB){ ct.sendCmd ('d'); } return false; } Заключение Самое приятное во всей постройке это то, что математичесская модель сошлась с физической реализацией. Сама постройка железки не представляет из себя какой-то сложности, однако подбор правильных моторов, высоты робота, массы груза сверху и синтезирование управления довольно интересная задача.Как и обещал вывод уравнений движения перевернутого маятника на колесе: Вывод уравнений и немного о постройке
