Pull to refresh

Создание робота балансера на arduino

Mathematics *Matlab *
Мне давно не давало покоя желание рассчитать какой-нибудь достаточно сложный механизм и воплотить его жизнь.
Выбор пал на задачу об обратном маятнике. Итог на видео:



Математическая модель


Я не буду приводить вывод уравнений движения, все таки это третий курс института. Для тех же, кому интересен вывод, в конце статьи ссылка, где он описан подробнее.
Систему представим в следующем виде:


Маятник это масса 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;
	}


Заключение


Самое приятное во всей постройке это то, что математическая модель сошлась с физической реализацией. Сама постройка железки не представляет из себя какой-то сложности, однако подбор правильных моторов, высоты робота, массы груза сверху и синтезирование управления довольно интересная задача.

Как и обещал вывод уравнений движения перевернутого маятника на колесе: Вывод уравнений и немного о постройке
репозитарий на GitHub
Tags:
Hubs:
Total votes 88: ↑86 and ↓2 +84
Views 75K
Comments Comments 19