Выбор пал на задачу об обратном маятнике. Итог на видео:
Математическая модель
Я не буду приводить вывод уравнений движения, все таки это третий курс института. Для тех же кому интересен вывод, в конце статьи ссылка, где он описан подробнее.
Систему представим в следующем виде:
Маятник это масса 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<BluetoothDevice> bounded=bluetooth.getBondedDevices(); boundedItems=new String[bounded.size()]; int i=0; for (BluetoothDevice bluetoothDevice : bounded) { boundedItems[i++]=bluetoothDevice.getName(); } showListDialog(); //... } 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<BluetoothDevice> bounded=bluetooth.getBondedDevices(); for (BluetoothDevice bluetoothDevice : bounded) { if(bluetoothDevice.getName().equalsIgnoreCase(boundedItems[id])){ try { btSocket=bluetoothDevice.createRfcommSocketToServiceRecord(MY_UUID); btSocket.connect(); ct=new ConnectionThread(btSocket); ct.start(); } catch (IOException e) { e.printStackTrace(); try { btSocket.close(); } catch (IOException e1) { e1.printStackTrace(); } showListDialog(); } return; } } } После подключения стартует поток, который занимается коммуникацией между приложением и роботом:
private class ConnectionThread extends Thread{ private final InputStream mmInStream; private final BufferedReader br; private final OutputStream mmOutStream; public ConnectionThread(BluetoothSocket socket) throws IOException { mmInStream = socket.getInputStream(); br=new BufferedReader(new InputStreamReader(mmInStream)); mmOutStream = socket.getOutputStream(); } public void run() { while (true) { try { String line=br.readLine(); h.obtainMessage(RECIEVE_MESSAGE, line).sendToTarget(); } catch (IOException e) { e.printStackTrace(); } } } public void sendCmd(char cmd){ try{ mmOutStream.write(cmd); }catch (IOException e) { e.printStackTrace(); } } } Поток посылает сообщение основному потоку приложения через Handler, который определяется так:
h = new Handler() { public void handleMessage(android.os.Message msg) { switch (msg.what) { case RECIEVE_MESSAGE: String line=(String)msg.obj; try{ float a=Float.parseFloat(line.trim()); balancerView.setAngle((float) (a-Math.PI/2.f)); }catch (Exception e) { } break; } }; }; balancerView это потомок класса SurfaceView, он занимается выводом на экран текущего положения робота.
Вот его метод перерисовки:
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; } Заключение
Самое приятное во всей постройке это то, что математичесская модель сошлась с физической реализацией. Сама постройка железки не представляет из себя какой-то сложности, однако подбор правильных моторов, высоты робота, массы груза сверху и синтезирование управления довольно интересная задача.
Как и обещал вывод уравнений движения перевернутого маятника на колесе: Вывод уравнений и немного о постройке
ссылка на оригинал статьи http://habrahabr.ru/post/220989/
Добавить комментарий