Balancerende robot

Uit RoboWiki
Ga naar: navigatie, zoeken

Balanceren

Het principe van balanceren is zeer eenvoudig : zorg ervoor dat het steunpunt zich loodrecht onder het zwaartepunt bevindt. Het steunpunt bevindt zich bij een tweewielige robot op de plaats waar de wielen de grond raken. Het zwaartepunt ligt uiteraard een stuk hoger. Om de balans te bewaren, moeten we dus zorgen dat de wielen zodanig aangedreven worden, dat het zwaartepunt zich "gemiddeld" loodrecht boven het steunpunt bevindt. Hiervoor moeten we vertikale positie van de robot kunnen meten. Dit gaat uitstekend met de volgende sensoren :

Gyro sensor

Een gyro sensor geeft de hoeksnelheid weer die hij ondervind. Elk lichaam kan zich natuurlijk draaien over drie verschillende assen, die loodrecht tov mekaar staan (X,Y,Z). Voor een balansrobot heb je aan één as voldoende. Natuurlijk moet de sensor zodanig gemonteerd worden, dat de gevoelige as van de sensor evenwijdig loopt aan de as van de wielen. Dikwijls zijn het sensoren met een analoge uitgang. Deze uitgang is meestal niet "ratiometrisch". Dat betekent dat de analoge uitgang niet direct evenredig is met de voedingspanning. Daarom is het nuttig om als referentie van je ADC-converter een precisie-referentie te gebruiken. Zo vermijd je dat schommelingen van de voeding een invloed hebben op de meetwaarde van de gyro. Je moet immers de gemeten waarde gaan integreren in de tijd (= optellen). Als je robot niet beweegt, moet de gemeten hoeksnelheid 0°/s zijn. Na optellen van de vorige meting blijft dit natuurlijk 0°. Op deze manier kan je de hoekverandering meten tov de vorige meting. Helaas, er is steeds een kleine afwijking die ervoor zorgt dat sommatie van al je metingen niet 0 is, niettegenstaande je robot stilstaat. Dit is de zogenaamde drift. Bij mijn gyro bedraagt deze drift ca 0.1°/sek. Na 1 minuut is er reeds een fout van 6° in de sommatie !! Deze drift is dikwijls ook nog eens afhankelijk van de temperatuur. Daarom dat er dikwijls een temperatuursensor is geintegreerd in de gyro. Zo kan de temperatuur van de gyro meten, en de drift enigzins kompenseren. Omdat de gyro drift heeft, kan je de stand van je robot hier niet mee bepalen. Hiervoor heb je een versnellingsensor nodig.

Versnelling sensor

Deze sensor geeft de linieaire versnelling weer die hij ondervindt. Ook hier heb je weer drie mogelijke richtingen van de versnelling. Daar onze aarde gezegend is met de zwaartekracht, zal deze sensor de versnelling "g" kunnen meten. Voor een balans robot is een 2-assige versnelling sensor voldoende. Deze moet ook weer juist uitgericht worden tov de wielassen. Beide assen van de sensor moeten loodrecht staan op de wielas. Uit de combinatie van de beide versnellingen kan je de richting van de zwaartekracht afleiden. Dat is wat we nodig hebben !! Helaas meten deze sensoren ook de versnelling van onze robot zelf !! Bij stilstand heb je dus een perfecte meting, maar als de robot beweegt, is er een flinke afwijking !! En balancerende robots staan nooit stil... De oplossing voor dit probleem is even eenvoudig als geniaal : over een langere tijd gezien, staat onze robot gemiddeld wel loodrecht. We moeten dus de gemiddelde richting van de versnelling meten over een langere periode.

C-code inertiaalsensoren

void task_initial(void)
{	
	static uint8_t i=0;
	static uint8_t j=0;	
	int16_t offset_temp=6140;
	
	if(getStopwatch1() > 3) {		//elke 4 mS worden de gyro + ADXL345 uitgelezen + opgeteld
		setStopwatch1(0);					//sample rate is zeer belangrijk voor integraal gyro !!!
		adc0=adc0+readADC(ADC_0);	//oversamplen gyro signaal
		adc5= adc5+readADC(ADC_5);	//oversamplen acc X signaal
		adc6= adc6+readADC(ADC_6);	//oversamplen acc Y signaal
		i++;		
		if(i>9){					//na 40 ms worden de gemiddelde waarde gebruikt
			aXX=(adc5-4200);adc5=0;	// 0 g = 4200 voor de X-as
			aYY=(adc6-4300);adc6=0;	// 0 g = 4300 voor de Y-as
			hoek=atan2(aYY,-aXX)/M_PI*1800;//hieruit volgt dan de hoek van de versnellingsvector in 0,1°
			i=0;				
			gyro_d=(adc0-6323);adc0=0;	//turnrate gyro over 10 samples-offset, 6323 bij stilstand als i>9
			gyro_hoek=gyro/35;//dit geeft dan de hoek weer in 0.1 graden
			gyro=gyro-gyro_d+0.25*(hoek-gyro_hoek);	//sommatie = actuele hoek van gyro+ compensatie drift van ACC
			drift=drift+hoek-gyro_hoek;
			oldgyro=oldgyro-gyro_d;			//dummy variable om de drift te kunnen zien
			oldgyro_hoek=oldgyro/35;
			if(program==1) task_balance();else setMotorPower(0,0);
			j++;
			if (j>9){
				adc1=0.8*adc1+0.2*readADC(ADC_1); //uitlezen temp gyro voor driftcorrectie	
				offset_temp=(adc1-640);	//temp compensatie gyrodrift, 640 is ca 20°C 
				gyro=gyro-offset_temp;	//elke 400 ms wordt de temperatuur correctie uitgevoerd
				j=0;
				}	
		}	
	}
}

Motor regeling

Als we de hoek van de robot betrouwbaar kunnen meten, komt de volgende stap : de motoren zodanig regelen dat de robot rechtop blijft. Hiertoe worden de motoren aangestuurd via een H-brug. Zo is het mogelijk dat we een traploze regeling hebben van de wielsnelheid/koppel, en dat in twee richtingen. In de onderstaande figuur zie het verband tussen PWM en motorsnelheid. De PWM waarde gaat van -500 tot 500. Pas bij een PWM waarde van -120/+120 starten de motoren. HIermee moet rekening gehouden worden met de aansturing. PWM balance.gif

Regelkring

Op zich is de regeling vrij eenvoudig. De basis van de regeling bestaat uit de som van 2 factoren :

1. Hoekregeling : Het verschil tussen de werkelijke hoek en de hoek waarbij de robot perfect in evenwicht staat is de eerste factor. Deze hoek wordt vermenigvuldigt met een regelparameter en dat is dan een deel van de PWM aansturing. Het is duidelijk dat deze factor alleen nooit een stabiele robot kan geven : er wordt hier geen rekening gehouden met hoe snel de robot naar zijn evenwichtspositie terugkeert.

2. Hoeksnelheid regeling : De werkelijke hoeksnelheid wordt weer met een regelparameter vermenigvuldigt en is dan een ander deel van de PWM aansturing. Deze factor gaat dus vooral de snelle bewegingen tegengaan.

Daarnaast heb ik ook nog 2 extra factoren, maar hiervoor zijn odometrie sensoren noodzakelijk :

3. Snelheidsregeling : De snelheid van de motoren wordt gemeten en dit wordt "meegekoppeld" met de PWM. Dit wil zeggen dat als de robot snelheid opbouwt vooruit, de hoek automatisch wordt teruggeregeld, de robot helt in de tegengestelde richting van de snelheid. Dit gebeurt door de snelheid van de motoren te vergroten !!! Het lijkt tegenstrijdig, maar hierdoor wordt effectief de gemiddelde snelheid naar omlaag geregeld ! Ook hier moet je weer een regelparameter instellen.

4. Positieregeling : De positie van de robot wordt gemeten met de kwadratuursensoren, en deze wordt tegengekoppeld met de PWM. Hierdoor zal de robot steeds terugkeren naar zijn startpositie. Hij pendelt heen en weer rondom de evenwichtspositie. Wederom hoort hier een regelparameter bij.

C-code Regelkring

void task_balance(void)
{	
	soll_distance=soll_distance-2*des_speed;		// bij vaste snelheid rijden : soll_distance aanpassen !!!
	int16_t distance= (mleft_dist+mright_dist+soll_distance)/8;	//sleepfout door soll_distance geeft offset aan hoek
			if (distance>300) distance=300;			//maximale correctie distance = 300*10/8=375 PWM
			if (distance<-300) distance=-300; 
	speed= mleft_speed+mright_speed-des_speed;	//mleft_speed = gemeten snelheid motor links
												//mright_speed = gemeten snelheid   motor rechts 	 										//des_speed = gewenste snelheid
	ist_speed=mleft_speed+mright_speed;
	if(ist_speed>speed_limit) over_speed=ist_speed-speed_limit;	//afremmen als snelheid groter dan 35 via integraal term
	if(ist_speed<-speed_limit)over_speed=ist_speed+speed_limit;
	if((ist_speed<=speed_limit)&(ist_speed>=-speed_limit))over_speed=0;
	if(over_speed) max_speed=max_speed+over_speed;
	else max_speed=max_speed*0.8;	// als snelheid terug kleiner dan limiet, integral term afbouwen
	pwm=(sollwert-gyro_hoek)*P_balance/4+gyro_d*D_balance/64+distance*P_distance/8+speed*P_speed/4+max_speed/4;  	
        offset=e_compass/4;				//richting korrectie volgens e_compass = verschil tussen afstand rechts/links
	if(offset>max_offset) offset=max_offset;		// begrenzing draaisnelheid
	if(offset<-max_offset)offset=-max_offset;
	pwm_L=pwm-offset;			//offset voor draaien 
	pwm_R=pwm+offset;
	
	if (pwm_L>10) pwm_L=pwm_L+100;	//correctie aanloopkoppel min PWM = 120
	if (pwm_L<-10) pwm_L=pwm_L-100;
	if (pwm_R>10) pwm_R=pwm_R+100;	//correctie aanloopkoppel min PWM = 120
	if (pwm_R<-10) pwm_R=pwm_R-100;
	setMotorPower(pwm_L,pwm_R);		//offset voor draaien robot PWM-L -offset, R+offset
	if((sollwert-gyro_hoek)>450)  {setMotorPower(0,0);program=0;}		//noodstop, robot is omgevallen
	if((sollwert-gyro_hoek)<-450) {setMotorPower(0,0); program=0;}		
}

Mijn roborama robot "Olimex3" is omgebouwd naar een balans robot... Hier twee filmpjes op youtube :