Dato: 16/9 2010
Varighed: 11-16
Deltagere: Daniel, Leni, Martin A og Martin N
Varighed: 11-16
Deltagere: Daniel, Leni, Martin A og Martin N
1. Formål
Formålet med denne øvelse var at bygge en LEGO SegWay, dvs. en selv balancerende robot. Der findes tre sensor opsætninger som kan anvendes til en sådan robot:
Formålet med denne øvelse var at bygge en LEGO SegWay, dvs. en selv balancerende robot. Der findes tre sensor opsætninger som kan anvendes til en sådan robot:
- En traditionel lyssensor
- Et accelerometer
- Et gyrometer
Ved at anvende enten et accelerometer eller et gyrometer er det muligt at få det mest stabile respons og derigennem en robot hvis evne til at opretholde sin egen balance er forøget i forhold til kun at benytte en lyssensor.
Denne øvelse har imidlertid til opgave at give et indblik i hvorledes et selvregulerende system kan fungerer, hvortil en lyssensor vælges for at simplificerer opgaven.
2. Øvelsesgennemgang
Den selvbalancerende robot blev opbygget ud fra et design af Philippe Hurbain's NXTway, som er en portering af Steve Hassenplug's Legway fra RCX til NXT.
Et billede af den færdige robot kan ses herunder:
Et billede af den færdige robot kan ses herunder:
Selve reguleringen af robotten foregår igennem en PID regulator hvor kildekoden bygger på elementer fra Brian Bagnall's bog "Maximum Lego NXTBuilding Robots with Java Brains".
Den færdige robot fungerer ved at programmet startes, hvorefter robottens balance position findes. Når dette punkt er fundet, trykkes ENTER knappen ned for at definerer et offset.
Proportional regulatoren måler herefter de konstante ændringer som opstår ved at robottens motor med små ryk får den til at bevæge sig frem og tilbage. Den aktuelle værdi trækkes fra balance offsettet, og udgører sammen med Kp (proportional konstanten) P vægten i PID regulatoren.
De øvrige regulerings elementer vil ikke blive forklaret i dette afsnit, istedet er det muligt at se en video af hvordan den ovenfor byggede robot holder balancen:
Det er tydeligt at robotten har svært ved at holde balancen, hvor det faktum at robottens balance punkt ikke befinder sig ved 90grader har en stor del af skylden. Ligeledes blev det observeret at når balance punktet skulle indstilles, så var det svært at justerer det 100% korrekt da trykket på ENTER fik robotten til at rykke sig en smule.
Løsning af balance problem - SegWay Rider
Der var i gruppen enighed om at såfremt man justerede Kp, Ki og Kd så burde det være muligt at opnå et mere stabilt respons. Men istedet for at spilde en masse ressourcer på en LEGO model som efter vores opfattelse var "dårlig", valgte vi at opbygge en ny model. Grundlaget herfor var at vi ønskede en robot hvis naturlige balance punkt var placeret ved 90grader for at opnå en mere stabil robot.
En LEGO model af en reel Segway PT designet af Dave Parker blev brugt som grundmodel til vores SegWay Rider. Informationer samt bygge instruktioner kan findes på følgende side:
http://nxtprograms.com/segway/index.html
Selve koden til at få denne robot til at balancerer var bygget til LEGO's egen udviklingsplatform, men vi vurderede at den eksisterende PID regulator ville kunne justeres til at fungerer med denne LEGO model istedet.
Et billede af SegWay Rider kan ses herunder:
Fordelen ved denne model er at dens naturlige balance punkt befinder sig i en vinkel på 90grader, samt at den har sin vægt placeret "højt" over hjulene, hvorved den får et meget mere stabilt tyngdepunkt.
For at undgå at skulle benytte ENTER til at kalibrerer et balance punkt, blev denne robot designet således at man har 4 sekunder fra programmet startes til at finde balance punktet. Herefter samples den aktuelle værdi fra lyssensoren og benyttes som offset for alle fremtidige målinger. Dette viste sig at gøre det meget nemmere at gentage balance indstillingen med større præcision end før, da man kun skulle fokuserer på at opretholde robotten i dens naturlige balance punkt.
Koden til at foretage denne kalibrering kan ses herunder:
public void getBalancePos() throws Exception {
// turn up system volume to ensure user hears the beeps
Sound.setVolume(75);
// wait for 4 seconds before determining balance position (offset)
int timeOut = 3;
while (timeOut != 0) {
Sound.beep();
Thread.sleep(1000);
timeOut--;
}
// SegWay Rider must be balanced when we reach 0!!
offset = ls.readRawValue();
Sound.beepSequenceUp();
}
Selve reguleringen af SegWay Rider bygger på den tidligere omtalte PID regulator, hvor præcisionen af de forskellige målinger og del resultater blev forøget ved at anvende float istedet for int.
Herunder vil P, I og D leddene blive gennemgået for at forklare hvorledes disse udregnes:
float pError = avgValue - offset;
if (pError < 0)
pError = pError * 1.8F;
integralError = ((integralError + pError) * 2)/3;
float derivError = pError - prevError;
prevError = pError;
float PIDValue = KP * pError + KI * integralError + KD * derivError;
Først findes proportional fejlen som den aktuelle måling minus det fastsatte balance offset. Derefter afgøres det hvorvidt fejlen er negativ (svarende til at robotten tilter bagud). Er det tilfældet så justeres proportional fejlen med en faktor 1,8 for at kompenserer for den mindre mængde lys som sensoren kan se i forhold til et fremad tilt.
Integrations fejlen findes som summationen af alle tidligere fejl vægtet med 2/3, for at sikrer at de første målinger vil påvirke denne summation mindre og mindre som tiden går. Hvis denne skalering ikke anvendes vil værdierne løbe løbsk og få robotten til at oscillerer ud af kontrol.
Differential fejlen findes som den aktuelle proportional fejl minus den forrige fejl.
Disse tre led som samlet udgører PID regulatoren summeres efter deres individuelle konstanter er blevet ganget på, som det også fremgår af den sidste kodelinie ovenfor.
Forbedringer af stabiliteten
For at forbedrer stabiliteten af robotten var det nødvendigt at se på udsvingene i input værdierne til lyssensoren. Et plot af sensor målinger kan ses herunder for en NXT 2.0 farve lyssensor:
Målingerne er foretaget vha. den tidligere benyttede datalogger fra øvelse 3 på proportional fejlen. Plottet illusterer et udsving mellem +/- 20grader i forhold til robottens balance punkt. Det ses tydeligt at der er mange små skift i de målte værdier, men at kurve formen eller passer overens med det forventede. Endvidere er det værd at observerer at sample værdierne mellem 500-2500 begynder at bevæge sig mod nul selvom robotten stadig bevæger sig fremad.
Altså kan det konkluderes at såfremt robotten får et udsving på mere end ca. 10grader i fremadgåede retning, så vil målingerne ikke afspejle den reele retning. Det er derfor vigtigt at PID regulatoren fungerer hurtigt nok til at kunne udgå udsving større end +/- 10grader for at opretholde et stabilt respons.
De små udsving kan fjernes ved at lave en midling af de foregående samples, et såkaldt moving average filtrering. Efter nærmere test, kunne det konkluderes at det kun var muligt at midle over de 2 sidst målte samples for at robotten ikke kom ud over den ovenfor beskrevede +/- 10graders begræsning (grundet den højere sampletid).
Et plot af proportional fejlen for et 2. ordens moving average filtreret sensor input kan ses herunder:
Det ses udfra plottet at de små udsving tydeligt blevet fjernet, hvorfor det blev valgt at benytte denne midlede sample værdi. Koden til realisering af MA(2) filteret kan ses herunder:
public float MAFilter(float value) {
// update the delayline top->bottom and add new sensor value
delayline[0] = delayline[1];
delayline[1] = value;
// return the calculated average
return (delayline[0]+delayline[1])/2;
}
Hvor delayline er et array indeholdende 2 pladser som begge er initialiseret til 0.
Plot af henholdsvis: proportional, integration og differential fejlene for robotten under balance kan ses herunder, sammen med et plot af den samlede PID regulator værdi:
De indivudelle konstanter Kp, Ki og Kd blev justeret, således at proportional og diffential fejlene ville få mest indflydelse på PID regulator værdien.
Til at styre motorens retning blev følgende kode benyttet:
if (PIDValue > 100)
PIDValue = 100;
if (PIDValue < -100)
PIDValue = -100;
int normPower = (int)(45 + (Math.abs(PIDValue) * 55) / 100);
Motor.B.setPower(normPower);
Motor.C.setPower(normPower);
if (PIDValue > 0) {
Motor.B.forward();
Motor.C.forward();
} else {
Motor.B.backward();
Motor.C.backward();
}
Variablen normPower styrer kraften til de to motorer B og C (disse blev valgt da de sad på samme H bro hvorved de udsættes for de samme delays og derfor skulle reagerer på samme tid). normPower er normalizeret således at PID regulator værdien (som er begrænset til værdier mellem +/- 100) højst vil bedrage med 55% af motor kraften, mens de resterende 45% svarer til den kraft som motorerne konstant udsættes for selvom robotten er i perfekt balance. Det var nødvendigt at sætte den minimale motor kraft så højt for at få motorerne til at reagerer kraftigt nok til at kunne opretholde sit balance punkt.
Herunder kan en video af hvor effektivt den nye SegWay Rider kørte:
Det var muligt at få robotten til at balancerer i 2 minutter inden videoen blev startet, samt 3 minutter efter. Den væltede først da Martin gik ind foran lampen som oplyste gulvet hvorved lysniveauet ændrer sig så meget at det fastsatte offset ikke længere var aktuelt.
3. Konklusion
Det lykkedes at få lavet en LEGO model (SegWay Rider) med et stabilt respons ved at benytte en lyssensor.
Konklusionen på effektiviteten af denne type balancerende robot er at det er rigtig svært at få indstillet balance punktet korrekt, hvis det blot er en smule skævt eller hvis sampleværdien springer i det øjeblik offsettet udmåles vil robotten bevæge sig ud af balance.
Det er ligeledes meget svært at lave en robot som vil virke på alle typer overflader, samt er det ikke muligt (som beskrevet ovenfor) at ændrer alt for meget på miljøet omkring robotten mens den balancerer (dvs. lys, underlag etc.) uden at den ikke vælter.
Såfremt man ønskede at lave en mere effektiv balancerende robot, vil det være mere praktisk at anvende et gyro meter, og så evt. en lyssensor hvis man ønsker at få robotten til at følge en linie.
4. Kildekode
Link til kildekoden for SegWay Rider kan hentes her!
Det lykkedes at få lavet en LEGO model (SegWay Rider) med et stabilt respons ved at benytte en lyssensor.
Konklusionen på effektiviteten af denne type balancerende robot er at det er rigtig svært at få indstillet balance punktet korrekt, hvis det blot er en smule skævt eller hvis sampleværdien springer i det øjeblik offsettet udmåles vil robotten bevæge sig ud af balance.
Det er ligeledes meget svært at lave en robot som vil virke på alle typer overflader, samt er det ikke muligt (som beskrevet ovenfor) at ændrer alt for meget på miljøet omkring robotten mens den balancerer (dvs. lys, underlag etc.) uden at den ikke vælter.
Såfremt man ønskede at lave en mere effektiv balancerende robot, vil det være mere praktisk at anvende et gyro meter, og så evt. en lyssensor hvis man ønsker at få robotten til at følge en linie.
4. Kildekode
Link til kildekoden for SegWay Rider kan hentes her!
No comments:
Post a Comment