Freestyle Robot mobilny [fuzzy logic]

[ahmed10]

Witam!
Chciałbym przedstawić wam moją konstrukcję, która stała się przedmiotem mojej pracy inżynierskiej.

Mechanika:

Konstrukcja mechaniczna w zasadzie oparta jest na 2 płytkach PCB. Do płytki głównej zamontowane są dwie kulki podporowe, a także silniki pololu o przekładni 1:10 co stanowi napęd robota. Z przodu zamontowana jest płytka z podstawą zawierająca uchwyty do czujników ultradźwiękowych. Taki sam uchwyt pod czujnik znajduje się również na 'ogonie' robota - jest on nie używany i został zamontowany eksperymentalnie. Na przedłużonej osi silników zostały zamontowane magnesy enkoderów magnetycznych co ilustruje poniższa fotografia.

Elektronika:

Sercem robota jest mikrokontroler AVR ATmega162, którego zadaniem jest sterowanie robotem w tym: obsługa enkoderów, sterowanie silnikami, odbiór informacji z mikrokontrolera slave nt. odległości mierzonych przez czujniki, realizację algorytmów sterowania, realizacja połączenia bluetooth, obsługa wyświetlacza LCD hd44780. Drugi mikrokontroler również AVR, ATmega328 realizuje tylko i wyłącznie obsługę czujników odległości HC-SR04. Pomiary normalizuje i wysyła interfejsem SPI do jednostki głównej ATmega162. Na pokładzie robota znajdują się również dwa enkodery magnetyczne AS5040 pracujące w pętli sterowania regulatora PI silników. Enkodery te pracują w trybie defaultowym czyli wyjście kwadraturowe, rozdzielczość 2x256. Silniki sterowane są poprzez mostek H L293. Komunikacją z robotem zajmuje się moduł bluetooth bt222 - czyli komunikacja jest przez UART mikrokontrolera. Główny mikrokontroler jest taktowany zewnętrznym kwarcem 16Mhz co przy takiej ilości obsługiwanych urządzeń i wykorzystywanych interfejsów było po prostu konieczne, z kolei mikrokontroler atmega328 jest taktowany zewnętrznym kwarcem jednak z powodu lepszej podstawy czasowej, która jest konieczna do dokładnych odczytów z czujników odległości. Na wyświetlaczu LCD są wyświetlane aktualnie mierzone wartości odległości czujników.

Automatyka:

Zostało wspomniane, że silnikami steruje algorytm regulatora PI. Aby obliczyć jego nastawy została przeprowadzona identyfikacja transmitancji. W tym celu wykorzystałem UART do przesyłania aktualnej prędkości silników oraz środowisko Matlab do wizualizacji danych na wykresie. Z wyliczeń wyszło, że silniki z zastosowanymi kołami mają transmitancję:

  G(s)=107/(0.19s+1)	

Po obliczeniu nastaw regulatora i zastosowaniu w praktyce okazało się, że można jeszcze troszkę dopieścić nastawy i ostatecznie wykresy odpowiedzi skokowej bez i z regulatorem wyglądają następująco:

Software:

Napisane w C w środowisku Eclipse. Umożliwia robotowi 3 tryby.

Tryb avoid. W trybie tym robot ma za zadanie omijać przeszkody wykorzystując regulator rozmyty Takagi-Sugeno. Wykorzystuje w tym celu odczyty z czujników odległości.

Tryb goal seeking. Robot podąża do celu czyli do odebranych przez bluetooth współrzędnych kartezjańskich podanych w [mm], zakładając, że miejsce od którego zaczyna lub dostał nowy zestaw współrzędnych, ma współrzędne 0,0. W trybie tym regulator Takagi-Sugeno ma za zadanie na podstawie zaimplementowanej odometrii oraz współrzędnych końcowych sterować robotem w celu osiągnięcia zadanego położenia. Problem algorytmu przedstawia ilustracja:

Tryb avoid + goal seeking. Jest to połączenie dwóch wcześniej opisanych trybów. W trybie tym działają równocześnie obydwa wcześniej opisane algorytmy, jednak na wejście regulatorów PI sterujących silnikami jest podawany zbalansowany sygnał z obu algorytmów. Tzn w zależności od najmniejszej odległości od przeszkody mierzonej przez któryś z czujników jest obliczany procentowy udział wartości sterowania od dwóch algorytmów. Brzmi skomplikowanie ale takie nie jest. Najlepiej zobrazuje to wzór:                                                                     

V=K*Vg+(1-K)*Va

gdzie V jest prędkością zadaną na regulator PI silnika, Vg jest prędkością wynikającą z algorytmu goal seeking, Va jest prędkością wynikającą z algorytmu avoid, K jest parametrem, który przyjmuje wartość z przedziału 0-1 w zależności od mierzonej minimalnej odległości do przeszkody.

Robota wprowadza się w odpowiedni tryb za pomocą dwóch przycisków lub poprzez interfejs bluetooth. Odpowiednie tryby sygnalizują diody LED. W celu przesłania komendy zawierającej współrzędne celu najpierw należy wprowadzić robota w odpowiedni tryb, a następnie wysłać do niego wg stworzonego prze zemnie protokołu dane. Protokół wysyłania danych jest następujący:

 X±⌴⌴⌴Y±⌴⌴⌴&

gdzie +- – znak współrzędnej,  _ – wartość współrzędnej.

Testy.

Konstrukcja została przetestowana pod względem dokładności odometrii. Robot podczas licznych testów na bieżąco wysyłał swoje współrzędne, które w dalszej kolejności były obrabiane w środowisku Octave do przyjemnych w analizie wykresów.

Przykładowy wykres drogi, którą pokonał robot na 'placu manewrowym' w trybie avoid:

... oraz rzut z góry na 'plac manewrowy':

U góry pozycja początkowa, na dole końcowa robota. Cały ten obszar był dookoła ogrodzony ścianką.

Słowa samokrytyki

Co bym zmienił ? Czujniki. Ze względu na problematyczność wykrywania małych przeszkód lub gdy płaszczyzna przedmiotu uniemożliwia odbicie się fali ultradźwiękowej w stronę odbiornika.
Zdarza się, że z powodu gubienia przeszkód robot po prostu w nie wpada.

Najciekawsze fragmenty programu:

Regulatory PI:

//######################################## REGULATOR PRĘDKOŚCI ############################################

	erra = vl-Va;//liczenie błedu prędkości
	errb = vp-Vb;

	if(ua<1023&&ua>-1023)Ca = Ca+(erra*dt);//całka błędu
	ua = kp*(erra  +  Ca/Ti);//regulator PI
			//P			I
	if(ub<1023&&ub>-1023)Cb = Cb+(errb*dt);//całka błędu
	ub = kp*(errb  +  Cb/Ti);//regulator PI
			//P			I
	if((ua<10)&&(ua>-10))ua=0;
	if((ub<10)&&(ub>-10))ub=0;
	motor_set(ua,ub);//ustawia sterowanie silników

Regulator rozmyty Takagi-Sugeno trybu goal seeking:

inline void PsiZ2Vlr(float *Psi, float *z, int16_t *vl,int16_t *vp)
{
	float PSI[3];
	float XL[6],XR[6];
	float NR,FR,L,S,P,VL=0,VP=0,m=0;
	uint8_t i,k;
///////////////////////////////LICZENIE PRZYNALEŻNOŚCI//////////////////////////////////
	if(*z<=100)NR=-0.01*(*z)+1;else NR=0;//jak bardzo blisko jest cel
	if(*z<=100)FR=0.01*(*z);else FR=1;//jak bardzo daleko jest cel
	if(*Psi<=0)L=-0.3183*(*Psi);else if(*Psi<-M_PI)L=1;else L=0;
	if(*Psi<=0)S=0.3183*(*Psi)+1;else if(*Psi<-M_PI)S=0; else if(*Psi>M_PI)S=0;else S=-0.3183*(*Psi)+1;
	if(*Psi>=0)P=0.3183*(*Psi);else if(*Psi>M_PI)P=1;else P=0;
/////////////////////////////////////TABLICA REGUŁ//////////////////////////////////////
				PSI[0]=L;					PSI[1]=S;						PSI[2]=P;
//NR;
		XL[0]=-4000;XR[0]=4000;			XL[1]=0;XR[1]=0;			XL[2]=4000;XR[2]=-4000;

//FR;
		XL[3]=-4000;XR[3]=4000;		 XL[4]=4000;XR[4]=4000;			XL[5]=4000;XR[5]=-4000;
/////////////////////////////obliczanie prędkości silników//////////////////////////////
	for(k=0,i=0;k<3;k++,i++)//pierwszy wiersz tabeli
	{
		VL+=NR*PSI[k]*XL[i];
		VP+=NR*PSI[k]*XR[i];
		m+=NR*PSI[k];
	}
	for(k=0;i<6;k++,i++)//drugi wiersz tabeli
	{
		VL+=FR*PSI[k]*XL[i];
		VP+=FR*PSI[k]*XR[i];
		m+=FR*PSI[k];
	}
	*vl=(int16_t)VL/m;
	*vp=(int16_t)VP/m;
}

Obsługa czujników odległości:

//PRZERWANIE OD CAPTURE PIN
ISR(TIMER1_CAPT_vect)
{
        if( (TCCR1B & (1<<ICES1)) )
        {
                LastCapture = ICR1;//jesli zbocze narastajace, zlap ICR1

        } else {
                PulseWidth = ICR1 - LastCapture;//Jeśli zbocze opadajace oblicz PW
                pomiar=0;//wyzeruj flagę pomiaru
        }
        TCCR1B ^= (1<<ICES1);//Zmiana zbocza wyw. przerwanie
}

Mam nadzieję że robocik się podoba. W razie pytań proszę śmiało. Całości kodu nie udostępniam ze względu na prawa autorskie niektórych funkcji bibliotecznych. Co mogłem udostępniłem.
Pozdrawiam was wszystkich !

[Treker (Damian Szymański)]

@ahmed10, właśnie zaakceptowałem Twój opis, możesz go teraz zgłosić do akcji rabatowej umieszczając link w temacie zbiorczym. Dziękuję za przedstawienie ciekawego projektu, zachęcam do prezentowania kolejnych DIY oraz aktywności na naszym forum 🙂