Balanserande robot

(1)

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Balanserande robot

Examensarbete utfört i Datorteknik

av

Fredrik Nilsson

Rapport LiTH-ISY-EX-ET--08/0344--SE

Linköping 2010

TEKNISKA HÖGSKOLAN

LINKÖPINGS UNIVERSITET

Department of Electrical Engineering Linköping University

S-581 83 Linköping, Sweden

Linköpings tekniska högskola Institutionen för systemteknik 581 83 Linköping

(2)

(3)

Balanserande robot

Examensarbete utfört i ämne Datorteknik

vid Linköpings tekniska högskola

av

Fredrik Nilsson

LITH-ISY-EX-ET--08/0344--SE

Handledare: Johan Stenberg vid SAAB communications, Växjö Examinator: Michael Josefsson

(4)

(5)

Presentationsdatum

2010-02-18

Publiceringsdatum (elektronisk version)

2010-03

Institution och avdelning Institutionen för systemteknik Department of Electrical Engineering

URL för elektronisk version

http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-19742 Publikationens titel Balanserande robot Författare Fredrik Nilsson Sammanfattning

Denna rapport behandlar utveckling av en tvåhjulig balanserande robot. En PID-regulator är implementerad i en

mikrokontroller, som även AD-omvandlar en sensorsignal, samt styr motorer via H-bryggor med pulsbreddsmodulering. I rapporten förklaras framtagning av diskret PID-regulator, processorkort, motorkort, val av komponenter och sensor för att mäta robotens vinkel mot ett vertikalplan.

Roboten kan balansera stillastående, men behöver kompletteras ytterligare med hjulåterkoppling för att kunna balansera medan den kör. Roboten balanserar med mätdata från en mekanisk golvavkännare (potentiometer). Andra sorters sensorer diskuteras också, t ex accelerometer, gyro och optisk sensor.

Abstract

This report explain the development of a two wheel balancing robot. A PID-regulator is implemented in a microcontroller, which also AD-converts a sensor signal, and generate pulse width modulated signals to drive H-bridges. It is discussed how to develop a discretisized PID regulator, microcontroller and H-bridge circuit boards, as well as how to chose components and suitable sensors for measuring the robots angle against a vertical plane.

The robot is able to balance on its own when standing still, but it need to have feedback from its wheel position if it should be able to maintain balancing when moving. The robot use a mechanical floor feeler (potentiometer). Other sensors are also discussed, e g accelerometer, rate gyro and optical sensor.

Nyckelord

Balanserande robot, digital reglering, PID, H-brygga, PWM, gyro, accelerometer, mikrokontroller.

Språk

X Svenska

Annat (ange nedan)

Antal sidor 52 Typ av publikation Licentiatavhandling X Examensarbete C-uppsats D-uppsats Rapport

Annat (ange nedan)

ISBN (licentiatavhandling) ISRN

Serietitel (licentiatavhandling)

(6)

(7)

Förord

Arbetet kom till som en egen ide på ett lämpligt examensjobb som både var utmanande och roligt. En genomarbetad designspecifikation skickades till min examinator, Michael Josefsson vid ISY/datorteknik, som visade intresse. Från början var det tänkt att göra arbetet i lokaler på skolan, men efter att jag varit i kontakt med Saab communications (senare Combitech) i Växjö, beslöts det att istället utföra arbetet på plats hos dem, under samma förutsättningar som tidigare.

Både Linköpings Universitet och Saab Communications i Växjö har sponsrat med komponenter till robotbygget.

Under resans gång har jag dragit nytta av det jag lärt mig under de tre år på elektroteknikutbildningen för högskoleingenjörer på Linköpings universitet. Utbildningen håller enligt mig hög kvalitet och har en bredd som gör det möjligt att sätta sig in i problem av skiljda slag. Examensarbetet i sig spänner över flera ämnesområden, och jag har kunnat använda och fördjupa mina kunskaper inom elektronikkonstruktion, reglerteknik, datorteknik m.m. Projektet består av en enligt mig lagom blandning av teoretiskt och praktiskt arbete.

Jag skulle vilja passa på och tacka Saab Communications i Växjö som ville ha mig hos sig i tio veckor, speciellt Janne Jöhag som sett till att jag kom dit, och Johan Stenberg som agerat handledare och bollplank. Tack också till examinatorn på skolan, Michael Josefsson, som trodde på mig och idén.

___________________________________________ Fredrik Nilsson, 2009

(8)

Förklaringar

Notationer

e(t) Reglerfel som funktion av tid |H(jω)| Beloppsfunktion

H(z) Överföringsfunktion i z-planet x[t] Tidskontinuerlig signal

x[n] Tidsdiskret signal

z-m _{Tidsfördröjning, y[n]}⋅_z-m _{= y[n-m]} ω Vinkelfrekvens, ω=2πf

ωc Brytvinkelfrekvens vid -3dB ϕ Vinkel mot ett vertikalplan

m massa

T (Samplings-)periodtid

s Laplaceoperator

g Gravitetsacceleration, ca 9.82 m/s2

Förkortningar och vokabulär

ADC Analog-to-Digital Converter

BGA Ball Grid Array. En IC-kapsling där anslutningarna är rundade och sitter i rektangulära fält

Butterworth Standard filterapproximation, andra är t ex Chebychev, Bessel, Cauer etc.

H-brygga Transistorkoppling med fyra transistorer som styr ut en stor ström med möjlighet att enkelt byta polaritet. LSB Lägst (Least) Signifikanta Biten, se även MSB

MATLAB Avancerat beräkningsprogram från The MathWorks MEMS MicroElectroMechanicalSystem

MIL “milli-inch” 1 MIL = 1/1000 tum

MSB Mest (Most) Signifikanta Biten, se även LSB PSpice Välkänt simuleringsprogram för elektronik. PWM PulseWidthModulation, pulsbreddsmodulering RPM Varvtal, Revolutions Per Minute

UART Universal asynchronous reciever transmitter VHP Vänster HalvPlan, avser enhetscirkeln

(9)

Innehållsförteckning

FÖRORD ... 1 INNEHÅLLSFÖRTECKNING ... 3 1 INLEDNING ... 5 1.1SYFTE OCH MÅL... 6 1.2KÄLLOR... 6 2 PROBLEMBESKRIVNING ... 7 3 LÖSNINGAR ... 9 3.1REGULATOR... 9 3.2SENSORER... 10 3.2.1 MEMS ... 10 3.2.2 Gyro... 11 3.2.3 Accelerometer... 14

3.2.4 Balansering med gyro och accelerometer som vinkelsensor ... 14

3.2.5 Mekanisk sensor ... 15 3.2.6 Optisk sensor ... 15 3.2.7 Sensorfel ... 15 4 BESKRIVNING AV SYSTEMET ... 17 4.1SYSTEMÖVERSIKT... 17 4.2PID-REGULATOR... 18 4.3SPECIFIKATIONER... 25

4.4INSTÄLLNING AV PID-PARAMETRAR... 26

5 BESKRIVNING AV HÅRDVARA ... 27 5.1MIKROKONTROLLER... 27 5.1.1 ADC ... 27 5.1.2 PWM ... 28 5.2ROBOTCHASSI... 28 5.3MOTORER... 28 5.4HJUL... 29 5.5ENCODERS... 30 5.6MOTORKORT... 30 5.7BATTERIVAL... 32 5.8SIGNALBEHANDLING, MÄTSIGNAL... 33 5.9FRAMTAGNING AV LÅGPASSFILTER... 34

5.10DC-DC OMVANDLARE,TED0522+EMI-FILTRET... 38

5.11PROCESSORKORT... 39

5.12TILLVERKNING AV KRETSKORT... 40

6 BESKRIVNING AV MJUKVARA ... 43

6.1TILLVÄGAGÅNGSSÄTT VID PROGRAMMERINGEN... 43

(10)

6.2.2 Reglerfunktion ... 48

7 SLUTSATSER... 49

7.1BALANSERING... 49

7.2BALANSERING MED GYRO OCH ACCELEROMETER... 49

7.3FÖRBÄTTRINGAR... 50

7.4AVSLUTNING... 50

8 REFERENSER... 52

APPENDIX A Kretsscheman APPENDIX B Programlistning

(11)

1 Inledning

Under min utbildning på LiTH har en tanke länge grott att konstruera en balanserande robot. Problemet i sig liknar mycket regleringen av en inverterad pendel som är ett klassiskt reglerproblem. Istället för att fördjupa sig onödigt mycket i reglertekniken och modellera roboten, har ett annat angreppssätt valts: att implementera en vanlig form av regulator, och sedan ge sådana justeringsmöjligheter som direkt ger verkan. På detta sätt kan man få tillräckligt bra resultat på experimentell väg. Det finns en del inspirationskällor, bland andra en välkänd kommersiell tillämpning, Segway. Den innehåller fleraxliga sensorer, och har motor- och batterikraft nog för att hålla en fullvuxen människa balanserad. Allt detta

gör att en Segway blir dyr. Figur 1. Segway [www.segway.com]

Detta exjobb ska handla om elektronikkonstruktion av ett system som mäter sensordata, använder en mikrokontroller för behandling av data och generering av styrsignaler för att sedan styra två motorer. Projektet innefattar förutom elektronikkonstruktion, även mekanisk uppbyggnad av robotkroppen, samt programmering av en mikrodator. Denna rapport behandlar dock endast system- och elektronikkonstruktion. För att underlätta läsningen rekommenderas en titt i ”förklaringar” på sidan 2. Det är bra men inte nödvändigt om läsaren har viss grundläggande kännedom om Laplace- och z-transform, reglerteknik, allmän elektronik såsom programmering av en mikrodator, analoga filter etc.

(12)

1.1 Syfte och mål

Detta examensarbete handlar om att bygga en tvåhjulig robot som själv balanserar. Huvudmålet är att få robotchassiet att balansera stillastående. Balansering skall ske med mätdata från en noggrann sensor. Därifrån kan en del förbättringar göras i mån av tid, t ex hjulåterkoppling. Rapporten i sig tjänar som dokumentation av arbetet snarare än en byggbeskrivning. Tänkbara framtida förbättringar är körning framåt och åt sidorna, radiostyrning och detektion av hinder.

1.2 Källor

Internet är en rik inspirationskälla att ösa ur. Samtidigt som den förenar självbyggare världen över, gäller det att sålla ut och faktagranska. Mycket finns skrivet om den s k inverterade pendeln, som i stort liknar en balanserande robot. Ett rekorderligt material jag hittat om mitt ämne är [3], som beskriver just regleringen av en inverterad pendel. Det finns mycket böcker skrivna i ämnet reglerteknik, ibland kan det vara svårt att sålla. En av de källor jag använt som förtjänar lite extra uppmärksamhet än vad som framgår i mitt arbete är [2], Microcontroller based applied digital control. Den tar upp mycket om digital reglering, och kopplar även teorin till vad man kan göra praktiskt. Som med många bra böcker inom ett specialämne, krävs det grundkunskaper för att kunna ta till sig innehållet. Till min hjälp har [1] och [7] varit, som båda har varit läromedelslitteratur i kurser jag läst.

(13)

2 Problembeskrivning

För att få en tvåhjulig plattform (robot) att balansera krävs noggrann mätning av dess absoluta vinkel ϕ mot jordens gravitationsaxel. Faller robotkroppen framåt, resulterar detta i en ökad vinkel, vilket gör att hjulen måste köra ifatt i motsvarande grad, se figur 2. Hastigheten med vilken roboten faller har betydelse för hur mycket kraft som måste läggas ut på motorerna, därför måste dessa kunna varvtalsregleras. Efter det att motorerna har hämtat upp robotkroppen, måste dessa stanna vid jämviktsläget, ingen oscillation eller för stor överkompensering kan tillåtas. Här krävs någon form av regulator som alltså kalkylerar den kraft motorerna behöver för att räta upp roboten, då en sensor känner en vinkeländring.

ϕ

Sensor

Figur 2. Balanserande robot.

Robotens fysiska utformning har betydelse för dess balanserande egenskaper. För enklare (stabilare) balansering bör roboten ha en hög tyngdpunkt, jämför med om man skulle balansera t ex en slägga med järnklumpen i handflatan, respektive tvärtom. (Släggan skulle visserligen kunna stå av sig själv med klumpen nedåt, men då faller ju hela idén med att också kunna balansera den.) Det visar sig också att längden på skaftet har betydelse, en kort pensel är svårare att balansera än en sopborste med långt skaft, för att ta ett exempel. Matematiskt beskrivs detta med hjälp av

(14)

Huvudtanken är att välja en generell regulator som enkelt kan ställas in efter rådande omständigheter, hellre än att anpassa en regulator till ett känt system. Systemet är inte känt i detta fall, det skulle kräva en mycket omfattande systemidentifikation, med hänsyn tagen till saker som motorernas överföringsfunktion, dödlägen (glapp) i kraftöverföring som orsakar tidsfördröjningar, friktion i underlaget, robotkroppens längd och vikt, sensorplacering etc.

En PID-regulator är just en sådan generell regulator som kan användas till en mängd tillämpningar, och är till sin natur sådan att dess tre parametrar enkelt kan ändras. Roboten strävar efter att hålla sig upprätt kring ett smalt område, en arbetspunkt (ϕ = 0), och området kan approximeras som linjärt. Då passar en PID-regulator för ändamålet. PID-regulatorn är även förhållandevis enkel att implementera digitalt i en dator, eller mikrokontroller. En mikrokontroller är en smidig lösning för en balanserande robot, då den innehåller det mesta som behövs: AD-omvandling av sensorsignal(er), inbyggda räknare, avbrottshantering, samt möjlighet att pulsbreddsmodulera (PWM) utgångar, vilket gör det enkelt att varvtalsstyra motorer. I mikrokontrollern implementeras sedan en diskret PID-algoritm som ska balansera roboten. PID-parametrar kan vara svåra att bestämma, därför bör yttre inställningsmöjligheter finnas för detta (t. ex. potentiometrar vars utspänning AD-omvandlas). Regulatorn genererar PWM-signaler till två H-bryggor, som ger tillräckligt med ström till två motorer. På motorerna sitter encodrar som mäter pulser per varv. Detta återkopplas till mikrokontrollern, som då kan hålla reda på var roboten befinner sig, ett s k MIMO-system med flera olika sensoringångar. Utan hjulåterkoppling skulle roboten visserligen kunna balansera, men då utan att veta om den står stilla på stället, eller håller på att skena. Är motorerna redan fullt utstyrda finns ingen marginal kvar för balansering. Om man vill få roboten att köra framåt, kan detta enkelt ske genom att simulera ett litet vinkelfel, som måste köras ifatt. Här kan ytterligare en PID-regulator användas för att hålla rätt hastighet med hjälp av sensordata från hjulen. Utsignalen hos den ena regulatorn blir styrsignal till den andra, se Figur 4. Börvärdet ’A’ ska vara

(16)

PID

ENCODER

Vinkelsensor

PID

noll för balansering. Ett vinkelfel kan läggas på ’B’, för fram/back. Den första PID-regulatorn (PID 1) sköter balanseringen, medan PID 2 reglerar hastigheten hos roboten.

B

A

1

2

Figur 4. Kaskadkopplade PID-regulatorer

3.2 Sensorer

Hur kan mätning av robotens vinkel mot gravitationsaxeln ske? Nedan finns en del idéer, men endast en variant kommer att väljas i detta arbete.

3.2.1 MEMS

Mikroelektromekaniska sensorer (MEMS) är högaktuella, då de på senare tid har blivit mycket små, exakta och dessutom billiga. I denna tillämpning kan ett gyro eller en accelerometer användas för vinkelmätning.

(17)

3.2.2 Gyro

De MEMS-gyro som finns tillgängliga är s k rate gyro, som mäter vinkelhastighet. MEMS står ju för mikroelektromekaniska system, och är just små mekaniska delar integrerade på en chipyta. Gyrot känner den s k coriolisaccelerationen [8], betrakta figur 5 nedan.

Figur 5. Roterande skiva visande coriolisacceleration. Hämtad ur [8, www.analog.com]

För att personen ska hålla en rak kurs om hon går från centrum och ut, måste hon också gå åt vänster, med en acceleration motriktad coriolisaccelerationen. Ersätts personen med en oscillerande massa enligt figur 6, kommer coriolisaccelerationen att avkännas kapacitivt med hjälp av ”coriolis sense fingers”, här benämnt sensorgafflar.

Figur 6. Coriolisaccelerationen känns av genom att en oscillerande massa upphängd i fjädrar påverkar kapacitansen i sensorgafflar. Bild från [8, www.analog.com]

(18)

Figur 7a, massan rör sig från mitten och ut, och 7b där massan rör sig utifrån och in mot cirkelns mitt.

Från [8, www.analog.com].

Den vågräta pilen i figur 7 visar åt vilket håll den oscillerande massan upplever en kraft. Rör sig massan från mitten och ut (a), accelereras massan åt höger, och massan upplever en kraft riktad åt vänster. I (b) rör sig massan in mot mitten, den minskande coriolisaccelerationen (retardation) får massan att uppleva en kraft åt höger.

Väldigt små ändringar i kapacitansen mellan sensorgafflarna kan mätas, ned till 12x10-21_{Farad [8], vilket skulle motsvara en rörelse på 16}

femtometer. Figur 8 är ett fotografi på den integrerade rörliga (elektromekaniska) sensorytan. Likheten kan ses med figur 6; på chipytan används dock två likadana sensorkroppar kopplade differentiellt, för att undertrycka störningar.

(19)

Figur 8. Sensorkropparna, i storleksordningen 10-4_m.

[8, www.analog.com]

Förutom sensorkropparna finns på chipytan även förstärkare och signalkonditionering, utgångssteg m m. Chipet kapslas sedan in i en 7 x 7 mm s k BGA-kapsling, svår att löda för hand, därför säljs chipen monterade på en bit kretskort enligt figur 9, där gyrot är till vänster på kretskortet och en tvåaxlig accelerometer till höger.

Figur 9. Gyro och accelerometer. [www.sparkfun.com]

(20)

3.2.3 Accelerometer

En MEMS-accelerometer fungerar på ett liknande sätt som gyrot, med en massa som är upphängd i en fjäderanordning. Notera dock skillnaden mellan accelerometer och gyro. Ett gyro mäter vinkelhastigheten; Stannar rotationen, finns ej heller coriolisacceleration, och vinkelhastigheten blir noll. En accelerometer mäter absolut gravitation, vinklas sensorkroppen kommer massan få fjädern att ställa sig i ett läge, och massan hålls kvar där, så länge lutningen finns.

3.2.4 Balansering med gyro och accelerometer som vinkelsensor

Nu är det dock så att en absolut mätning av vinkeln kräver både ett gyro och en accelerometer. Dessa sensorer har var för sig en del nackdelar; en accelerometer reagerar alltför kraftigt på minsta lilla störning, och påverkas även av en horisontalacceleration, när roboten faller eller kör. Mätningen kompletteras därför av ett gyro, som mäter vinkelaccelerationen. Gyrot lider dock av lågfrekvent drift i utsignalen. Som vinkelmätare skulle ett gyro vara värdelöst då ett biasfel efter integrering av vinkelhastigheten betyder en stor felvinkel.

Genom att kombinera dessa två sensorers fördelar och filtrera bort allt annat kan en god uppskattning av ϕ ändå göras. Ett sätt som förtjänas att nämnas i sammanhanget är med ett s k Kalmanfilter, som är ett rekursivt filter som implementeras digitalt. Beräkningarna som krävs är i huvudsak matrisalgebra, varför de blir numeriskt svårhanterbara. Ett annat sätt är med s k komplementärfiltrering [6]. Lågfrekvensegenskaperna för accelerometern, och högfrekvensegenskaperna hos gyrot kombineras i en komplementär filterfunktion. I praktiken innebär detta ett högpassfilter och ett lågpassfilter med samma brytfrekvens (därav att de är komplementära), på detta sätt erhålls ändå en tillräcklig bandbredd.

Det finns även kommersiella sensorer som innehåller både accelerometrar och gyro samt någon utav ovan nämnda signalbehandling. Dessa benämns ofta IMU, Inertial Measurement Unit, eller inklinometer, se figur 10.

(21)

Figur 10. Inklinometer, från [www.microstrain.com]

3.2.5 Mekanisk sensor

En helt annan, inte lika sofistikerad metod, är att känna av robotens lodräta vinkel mot golvytan den står på. Detta kan göras med en golvavkännare, en stav, ena änden förbunden med en lättgående potentiometer, andra änden vilar mot golvet. Roboten får inget stöd av staven.

En fördel med golvavkännaren är att robotens reglering kan utvecklas för sig, när allt fungerar kan den ersättas av gyro och accelerometer.

3.2.6 Optisk sensor

Den optiska sensorn skickar ut en ljusstråle mot golvytan, och fångar upp det reflekterade ljuset i en fotosensor. Genom optisk triangulering kan avståndet till golvytan bestämmas, och därmed robotens vinkel. Ändras ljusförhållandena, ändras även sensorns utsignal, och balanseringen äventyras.

3.2.7 Sensorfel

Utsignalen från samtliga sensorer är behäftade med olika fel; Brus genereras elektriskt i alla sensorer, och bestämmer en undre gräns för hur små signaler som går att detektera innan signalen drunknar i brus. Biasfel

(22)

(nollfel) känns igen på att en konstant utsignal finns fast ingen storhet finns att mäta. Linjäritetsfel och skalfaktorfel beror på olika förstärkningar vid olika mätutslag eller frekvenser. Sensorer med två eller flera mätaxlar kan även ha ortogonalitetsfel, dvs att två axlar inte är monterade i rät vinkel mot varandra.

(23)

4 Beskrivning av systemet

4.1 Systemöversikt

Figur 11 visar en schematisk bild av robotens uppbyggnad, såsom nämndes i avsnitt 3.1. Detta avsnitt ska främst handla om hur man teoretiskt kan gå tillväga för att ta fram en diskret PID-regulator – själva hjärtat i roboten.

A

D

C

8x 10bit

Mikrokontroller

Vinkelsensor Förstärkning, Filtrering

PID

H-brygga

PWM PWM VÄ HÖ

M

Ställbara parametrar

P, I, D, offset Puls-formning

Encoder

(24)

2 System

PID 1

4.2 PID-regulator

För att ta fram en diskret PID-regulator kan man utgå från den tidskontinuerliga:

u ( t ) = K

_p

$ e (t ) C K

_I

$

⌠

⌡

0 t

e ( t ) dt C K

_D

$ de ( t )

dt

(1)

där e(t) är reglerfelet: e(t)=r(t)-y(t), se även figur 12 nedan:

r(t) e(t) u(t) y(t)

Figur 12. Återkopplad reglerloop.

Regulatorn skrivs ofta [17]:

u ( t ) = K

_p





e ( t ) C

1 T

_I

$

⌠

⌡

_t0 t

e ( t ) dt C T

_D

$ de ( t )

dt





(2)

där koefficienterna TI och TD ofta kan anges i sekunder [1, s.39]

Laplacetransformen av (2) ger överföringsfunktionen:

U ( s )

E ( s )

= K

p





1 C

1 T

_I

s

C T

D

s

(25)

Genom att multiplicera in Kp fås

U ( s )

E ( s )

= K

p

C

K

_p

T

_I

s

C T

D

K

p

s

(3)

och schematiskt visas ekvation 3 som en signalflödesgraf i figur 13.

2 s kp P 1 s ki I kd D 1

Figur 13. Signalflödesschema över ekvation (3).

För att kunna implementera (3) tidsdiskret, dvs. i en dator, måste en del approximationer göras. Integreringsdelen innebär ju matematiskt att felet summeras, och tidsdiskret innebär detta att endast felet vid varje samplingstidpunkt summeras. Därför förloras information mellan samplingarna, och detta måste beaktas vid val av samplingstid.

Tidsdiskret derivering går heller inte att göra exakt i praktiken, då det skulle krävas en oändlig förstärkning av insignalen [1, s.39]. Derivering kan därför göras med en differensapproximation, t ex ”Euler bakåt”, där man helt enkelt tar nuvarande värde minus det förra, dividerat med (samplings-)tiden mellan de båda värdena.

Derivering genom ”Euler bakåt”:

de ( t )

dt

z

e ( kt ) Ke ( kt KT )

T

(26)

⌠



⌡

0 t

e ( t ) dt

_{z T$}

>

k = 1 n

e ( kt )

Integrering, t=nT:

Dessutom bör man begränsa I-delens bidrag till styrsignalen genom att ange ett maximalt integratorvärde:

Om

In =

T $

>

k = 1 n

e ( kt )

så sätter man följande begränsningar: Imax, för In > Imax In = In , för Imin≤ In ≤ Imax Imin, för In < Imin

vilket är enkelt att realisera i ett datorprogram:

// Antiwindup

if(integration_sum > integration_max)

integration_sum = integration_max; if(integration_sum < (0-integration_max))

(27)

Detta kallas Antiwindup, eller motverkan av integratoruppvridning, och innebär att man vill förhindra att integratorsumman blir onormalt stor. Det skulle annars ta lång tid efter det att reglerfelet skiftat tecken att integrera ner felet, medan styrsignalen ligger kvar på en hög nivå [1, s.195].



C uo (4)

Vid varje samplingstidpunkt, T, görs en ny beräkning, och k räknas upp. Förutom (4) krävs enligt tidigare speciell behandling om integratorsumman blir för stor.

Z-transformen av PID-regulatorn i (2) ger H(z):

H ( z ) =

U ( z )

E ( z )

= K

p





Denna kan enkelt realiseras i ett signalflödesschema med s k parallell form [7, s.49] i figur 14. (KP är multiplicerad in i parentesen, och KI = KP*T/TI, KD =

(28)

2 kp P -1 Z -1 Z ki I kd D 1

Figur 14. Parallellform av PID-regulator

Ekvation (5) kan emellertid modifieras något, i detta arbete ska en PID-regulator av typen ”velocity form” användas, då den ska ge ett lugnare uppträdande vid små fel [2]. Dessutom har den ett inbyggt skydd mot integratoruppvridning. Ekvationen kan istället skrivas på formen

H ( z ) =

a C bz

K 1

_{C cz}

K2

1Kz

K1 (6)

(29)

2 a -1 Z -1 Z c b 1

b = KK

_P

K

2 K

r

1

r

3

v

1

u

r

2

r

4

v

2

r

5

Figur 15. Ekvation (6) på direkt kanonisk form

Signalflödesgrafen i figur 15 ska översättas till ett körbart program, och därför tilldelas noder med variablerna r1-r5, minneselementen v1 och v2,

insignalen e och utsignalen u.

Initialnod: N1 = {e, v1, v2} Beräkningsnoder: N2 = {r1, r4, r5}

N3 = {r3} N4 = {u}

(30)

Koppla samman noderna enligt figur 16:

z-1

Figur 16. Schematisk framställning av nodberäkningarna.

Översätt nodfiguren till differensekvationer i beräkningsbar ordning: r1 = e + v1

r4 = v1 * b r5 = v2 * c r3 = r1 * a u = r3 + r4 + r5

(31)

//Deklarationer const T=0.025, Kp=10, Ti=1, Td=0.5, N=101; float e, r1, r3, r4, r5, v1, v2, u; int n=0; //Tilldelningar a = kp+(kp*T/Ti)+(kp*Td/T); b = -(kp+(2*kp*Td/T)); c = kp*Td/T; v1=v2=0; for n=1 to N { //Beräkningar read e; r1=e*v1; r4=v1*b; r5=v2*c; r3=r1*a; u=r3+r4+r5; //Uppdatering av minneselement v2=v1; v1=r1; }end;

Ovanstående kod är helt framtagen genom att översätta ett matematiskt samband till körbar programkod. Dock lämnar den en del över, t ex AD-omvandling av mätsignal, utstyrning till aktuatorer etc. Pseudokoden kan kännas igen i C-kodlistningen i appendix B. Lägg märke till att samplingstiden T ligger inbakad i konstanterna.

4.3 Specifikationer

Ett reglersystems stegsvarsspecifikationer beskrivs i figur 17. Systemets stigtid. tr, definieras som tiden det tar för signalen att gå från 10 % till 90 %

av slutvärdet. Överslängen, Mp, anges i procent. Det är även intressant att

veta insvängningstiden, ts, där man först får bestämma ett största och

minsta värde som utsignalen måste hamna inom efter en viss tid, i figuren

(32)

Figur 17. Stegsvarsspecifikation

4.4 Inställning av PID-parametrar

Det finns en mängd olika tekniker att ta till för snabb inställning av en PID-regulator. Bäst resultat fås om man kan studera stegsvaret, och sedan utifrån specifikationerna ställa PID-parametrarna efter vissa tumregler. En variant är Ziegler-Nichols metod. Den innebär att man först kopplar bort I- och D-delarna. Förstärkningen P ökas från noll tills systemet börjar självsvänga med konstant amplitud. P-värdet och periodtiden hos svängningen. Därefter kan I- och D-delarna ställas efter tumregler [1].

Nedan beskrivs en förenklad variant av Ziegler-Nicholsmetoden, som är aktuell för inställning av roboten. Den ger endast ett grovt utgångsläge, och lämnar en del över för finjustering.

1. Sätt I- och D-delarna till 0

2. Öka P tills roboten börjar oscillera, minska då till hälften 3. Öka D tills en snabb dämpning av överslängar kan ses. 4. Öka I tills självsvängning, minska sedan till hälften.

(33)

5 Beskrivning av hårdvara

I appendix A finns kretsschema på processorkort och motorkort, dit hänvisningar i texten kommer att göras.

5.1 Mikrokontroller

Processordelen i mikrokontrollern ska sköta de beräkningar som krävs för PID-regleringen, men de perifera enheterna är de utslagsgivande vid val av mikrokontroller. De krav som ställs kan sammanfattas:

• 2 st oberoende PWM-utgångar.

• 2 st 10 bit AD.

• Minst 4 st 8bit AD för omvandling av potentiometervärden.

• Klockfrekvens, 4 MHz, ej kritisk.

• Stöd för ISP-programmering. (In-Socket-Programming).

• Minst en intern timer/räknare.

• Minst en extern avbrottsingång.

• 10st fria I/O för framtida utbyggnad.

Ovanstående kan lösas med t ex PIC16F874 alternativt en ATmega16. Den sistnämnda har dock en del extra fördelar, bla USART, samt att Atmel tillhandahåller en (gratis) utvecklingsmiljö, AVRstudio som kan köras med WinAVR för C-programmering. ATmega16 har 32 fria I/O vilket väl täcker behovet.

5.1.1 ADC

AD-omvandlingen i ATmega16 sker enligt successiv approximation, 10 bitars maximal upplösning vilket motsvarar ca 5mV då Vref = 5 V. Ingången är multiplexad så att 8 kanaler finns tillgängliga. En första omvandling tar 50 klockcykler, därefter 26 klockcykler. ADC-klockan är halva masterklockan, dvs. 2 MHz, varför signaler över Nyquistfrekvensen, 1MHz, ej kan konverteras utan vikningsdistorsion. Inimpedansen till ADC´n bör vara 10 kohm eller lägre, för att sample- och hållkondensatorn ska hinna laddas upp innan nästa omvandling sker.

(34)

5.1.2 PWM

ATmega16 erhåller fyra oberoende PWM-kanaler, där två används för drivning av robotens motorer, via motordrivkrets. Maximal PWM-frekvens vid 4 MHz klocka är 15625 kHz, vilket passar bra då eventuella störande resonansljud från motorerna inte hörs. Figur 18 visar hur en PWM-signal genereras. Övre kurvan är en räknare i processorn som hela tiden börjar om efter maxvärdet, samtidigt som PWM-utsignalen (undre kurvan) går hög. En variabel i det s k Output Compare-registret (OCR) bestämmer pulsförhållandet, duty-cycle, hos PWM-signalen. Då räknaren når upp till inställt Output Compare-värde går PWM-signalen låg.

Figur 18. Pulsbreddsmodulering i ATmega16.

5.2 Robotchassi

Robotchassiet är tillverkat av vinkelprofiler och plåtar i aluminium. Allt bildar en ram där kretskort och motorer enkelt skruvas fast.

5.3 Motorer

Motorerna är valda med omsorg då de dels måste vara tillräckligt snabba för att hämta upp en snabb vinkelstörning hos roboten, och dels måste de kunna leverera tillräckligt med vridmoment för att hantera den ganska tunga robotkroppen. Det bör inte förekomma något större glapp i växellådan. Två 12 V motorer med kuggväxel i metall från micromotors väljs, av typen RH158 med utväxling 30:1. Dessa har ett vridmoment på 200 mNm, ett varvtal vid maxlast på 140 RPM, och drar då en ström på 660 mA. Obelastat varvtal är 210 RPM. Se figur 19.

(35)

Figur 19. En av två motorer som används i roboten.

5.4 Hjul

Hjulen i figur 20 är luftgummihjul avsedda för modellflygplan, där plastnavet ersattes med svarvat nav i aluminium för enkel infästning mot motoraxeln med en stoppskruv. Diametern hos hjulen är 125 mm varför hjulet rullar 39 cm på ett varv och tillryggalägger 91 cm på en sekund vid maxlast. Hjulen är direkt fastsatta i motoraxeln, och från datablad läser man att motoraxeln kan belastas med 50 N radiellt (2 motorer = 100 N = 10 kg), och 10 N axiellt. Eftersom robotens egenvikt maximalt kommer upp i en tredjedel av 100 N, kommer detta inte innebära några

(36)

5.5 Encoders

Direkt på motoraxeln (dvs. inte den utväxlade axeln) sitter en encoderskiva, figur 21, med 3 vita fält, och 3 svarta. Vid maxvarv fås 18900 pulser per minut, eller uttryckt i sträcka som roboten tillryggalägger per puls, ca 4,5 mm per puls, vilket borde vara tillräckligt för att reglera dess

position.

Figur 21. Encoderskiva. Två reflexsensorer sitter monterade på ett kretskort ca fem millimeter från encoderskivan. De har monterats i 90 graders vinkel mot varandra. Kodskivan har udda antal vita och svarta fält, vilket innebär att om ett svart fält täcker mitt över den ena reflexsensorn, kommer alltid den andra reflexsensorn befinna sig på gränsen mellan svart och vitt fält. På så sätt kan en riktningsbestämning göras. Pulserna från de optiska reflexdetektorerna går genom schmitt-triggers, D1A-D1D i kretsschema motorkort (Appendix A), för att skärpa till pulserna något, innan de matas till mikrokontrollern.

5.6 Motorkort

Systemet är konstruerat med tanke på störundertryckning, och därför är all elektronik som hör till drivningen av motorerna separerade från övrig kontrollelektronik. Praktiskt innebär detta även att styrsignalerna från mikroprocessorn är galvaniskt åtskiljda från motordrivkretsens ingångsben via optokopplare. I kretsschema för motorkortet syns V1 och V5 som är den lämpliga MCT2. Denna har en kopplingsfaktor (CTR) på minst 20 % som beaktas vid beräkningar av förkopplingsmotstånden.

(37)

Figur 22. Bestyckat motorkort.

LMD18200 är en helhetslösning vad gäller motordrivkrets. Vid maximal last drar en motor 660 mA. I startögonblicket kan dock högre strömspikar uppkomma, varför en väl tilltagen motordrivkrets är att föredra. LMD18200 klarar 6 A strömspikar och 3 A kontinuerlig last. LMD18200 innehåller termiskt överhettningsskydd, överströmsskydd, ingångslogik för enkel anslutning till processor, samt en H-brygga med s k frihjulsdioder. Från motordrivkretsens felutgång (aktivt låg) ansluts en röd lysdiod. Se figur 22. Motordrivkretsen är av en s k multiwatt-kapsel och är avsedd att monteras på kylfläns. Kretsen klarar en maxbelastning på 25 W, och det kan vara bra att ta reda på kylflänsens storlek. Detta görs genom att räkna med termiska resistanser, Rth, vilket är ett mått på ett materials förmåga att leda värme.

Figur 23 visar Pf som är den förlusteffekt i form av värme som ska ledas bort

genom transistorkapseln, Rthjc, och ett mellanlägg, Rthc-h. Kylflänsen Rthh-a

skall leda bort båda motordrivkretsarnas förlusteffekter till omgivningsluften, Ta.

(38)

R

_{thh K a}

=

T

j

K T

a

K P

f

( R

thj K c

C R

thc K h

)

P

_f

Figur 23. Termiska resistanser för motordrivkrets och kylfläns.

I datablad för LM18200 hittas den termiska resistansen för kapseln Rthjc =

30°C/W, maximala kapseltemperaturen, Tjmax = 125 °C, och max

förlusteffekt Pfmax = 25 W. Termisk resistans för mellanlägget är omkring

1°C/W. Den termiska resistansen för erforderlig kylfläns beräknas då enligt:

2

Vilket skulle ge en kylfläns med Rth = 1,65 °C/W

I praktiken behövs inte en sådan stor kylfläns, motorerna kommer aldrig upp i maxström, som dessutom ligger långt under strömtåligheten hos drivkretsarna. Dessa är därför endast monterade på en aluminiumvinkel, och en liten fläkt blåser luft direkt på denna.

5.7 Batterival

Då motorerna behöver 12V, måste batterierna också lämna 12V, eller något högre då motorerna pulsbreddsmoduleras. Batterierna bör också lämna minst 2 Ah för rimlig driftstid. Valet föll på två stycken seriekopplade 7,2 V RC-ackumulatorer, för 14,4 V och 2700 mAh totalt, se figur 24. De har s k tamiya-kontakter som är vanliga i RC-hobbyvärlden. Batterierna placeras med fördel högt upp i robotkroppen, för att få upp tyngdpunkten. (RC står här för ”Radio Controlled”, och avser radiostyrda bilar, flygplan etc.)

(39)

Figur 24. Batteri. I roboten sitter två stycken.

5.8 Signalbehandling, mätsignal

Innan sensorers utsignal kan AD-omvandlas, måste den anpassas, relevant data extraheras, så att endast den del av signalen som är intressant kommer in i processorn. Det viktigaste vid övergång (läs: AD-omvandling) från tidskontinuerliga signaler till tidsdiskreta är att undvika vikningsdistorsion, (aliasing). Detta görs med ett lågpassfilter med brytfrekvens under halva samplingsfrekvensen (Nyquistfrekvensen) hos AD-omvandlaren. Framtagning av filtret visas i nästa avsnitt.

I kretsschemat för processorkortet är sensorsignalen först buffrad med OP-förstärkare kopplad som spänningsföljare, för att sedan passera ett lågpassfilter, sedan ett förstärkarsteg, och till sist en biasinställning. Buffern finns främst till för att kunna ansluta andra mätgivare, t ex gyro och accelerometer, då dessa har högohmiga utgångar.

Innan signalen når AD-omvandlaren ställs en referensnivå kring 2.5 V in med en 20-varvig trimpotentiometer för exakthet. Även förstärkningen kan trimmas noggrant 0-15,5 dB (0-6 ggr). Eventuella högfrekventa störningar som nu kommit igenom mätkedjan filtreras bort med en avkopplingskondensator, tätt placerad intill mikrokontrollerns AD-ingång. De instrumentförstärkare som används i mätkedjan har vissa krav på sig för att inte introducera brus eller annat oönskat på mätsignalen. Viktigt är att brus eller offsetfel i OP-förstärkarna är lägre än AD-omvandlarens upplösning på 5 mV. Valet av LM837 verkar lämplig då den har följande egenskaper:

• Offsetspänning typiskt 0.3 mV

• Låg distorsion och lågbrusig

• ±15V matning. Kräver DC-DC omvandlare, minst ±40 mA

(40)

5.9 Framtagning av lågpassfilter

Innan mätsignalen från golvavkännaren når mikrokontrollern passerar den ett lågpassfilter. Detta för att undvika vikningsdistorsion vid samplingen (AD-omvandlingen). Det har visat sig att en brytfrekvens (-3 dB) på 25 Hz är tillräcklig, bland annat grundat på resonemanget om hjärnans/ögats tröghet i avsnitt 2. Filtret får inte ge för stor fasförskjutning, då detta introducerar oönskad tidsfördröjning i reglerloopen. Då förstärkningen i filtret ska vara 1 (0 dB), väljs ett aktivt filter med en OP-förstärkare, där uppbyggnaden är av typen ’Sallen-key’, med positiv återkoppling. Polparametrar tas fram för ett andra ordningens filter som approximerar en filterfunktion med Butterworthkaraktäristik.

Ett snävare övergångsband hade önskats, t ex ett Chebychev-II (s k invers chebychev-I) har på grund av ekvirippel i stoppbandet ett snävt övergångsband, även för lägre ordningens filter. Polparametrar för Chebychev-II är även relativt enkla att få fram, men på grund av att filtret har ändliga nollställen kan det tyvärr inte realiseras med aktiva OP-filter. Polparametrar för Butterworthfiltret tas fram t ex med hjälp av MATLAB. Nedan visas ett kort program i MATLAB som använder standardfunktionerna

buttord.m och buttap.m för att räkna ut poler samt plotta beloppsfunktion

och grupplöptid, givet en brytfrekvens vid 25 Hz. (Amin, Amax och ωs är av

underordnad betydelse, men är anpassade och valda iterativt ”för hand” för att passa ett andra ordningens filter).

(41)

% LP-Butterworth-filter %Filterspecifikationer: wc=2*pi*25; ws=2*pi*80; Amax=3; Amin=20;

N = BUTTORD(wc, ws, Amax, Amin, 's') %Tar fram filterordning [Z,P,G] = BUTTAP(N)%Returnerar poler och nollställen

%Denormalisera poler: epsilon = sqrt(10^(0.1*Amax)-1); r0p = wc*epsilon^(-1/N) P = P*r0p G = G*r0p^N; %Plotta Grupplöptid: W=linspace(0,1000,1000); tg=groupdelay(Z,P,G,W,wc); figure(1); plot(W/(2*pi),tg); axis([0 60 0 0.015]); set(gca,'FontName','times','FontSize',16) ylabel('t [s]','FontName','times','FontSize',18); xlabel('\omega [Hz]','FontName','times','FontSize',18); %Plotta Beloppsfunktion: [NUM, DEN] = zp2tf(Z,P,G); H=freqs(NUM,DEN,W); magdB = 20*log10(abs(H)); figure(2); plot(W/(2*pi),magdB); axis([0 90 -25 2]); set(gca,'FontName','times','FontSize',16) ylabel('|H(j\omega)| [dB]','FontName','times','FontSize',18); xlabel('\omega [Hz]','FontName','times','FontSize',18);

(42)

Vid en körning av ovanstående program fås: N = 2

Z= [ ] (inga ändliga nollställen) P1 = -0.7071 + 0.7071i

P2 = -0.7071 - 0.7071i

(polerna komplexkonjugerade och i VHP) r0p = 157.2662 Efter denormalisering: P1 = 1.0e+002 * -1.1120 + 1.1120i P2 = 1.0e+002 * -1.1120 - 1.1120i

0

20

40

60

80 -25

-20

-15

-10

-5

0 |H

(j

ω

)|

[

d

B

]

ω

[Hz]

Figur 25. Beloppsfunktionen för filtret. Amin = 20 dB, Amax = 3 dB,

(43)

0

10

20

30

40

50

60

0

0.005

0.01

0.015 t

[s

]

ω

[Hz]

Figur 26. Grupplöptid för filtret.

Figur 25 visar beloppsfunktionen, med två rektanglar inritade i efterhand, som visar att filtret uppfyller kraven på Amin, Amax, ws och ωc. Hellre än att

titta på fasförskjutningen, har jag istället valt att rita en kurva över filtrets grupplöptid, figur 26. Det är önskvärt att ha en så liten grupplöptid som möjligt, eller kanske ännu hellre, så liten variation i grupplöptiden som möjligt. Under filtrets brytfrekvens är fördröjningen omkring 10 ms, vilket är acceptabelt i sammanhanget.

Komponentvärden tas fram enligt t ex [5]. Avståndet (radien) från origo ut till en pol i komplexa talplanet, r0p, räknades ut i MATLAB-programmet ovan till r0p = 157,2662.

(44)

C1

R1 R2 IN

UT C2

Figur 27. Sallen-key länk. (Lågpassfilter)

Filtret visas i förenklad form i figur 27. Förutsättningarna är att C = C1 = C2, R = R1 = R2. Kondensatorerna väljs till 680 nF för enkelhets skull.

R ges av 1/r0p*C till 9351 Ω, närmaste E96 standardvärde är 9.31 kΩ.

Filterfunktionen med komponentvärden togs fram enligt ovanstående i MATLAB, därefter simulerades filtret i Pspice.

5.10 DC-DC omvandlare, TED0522 + EMI-filtret

Denna lämnar en dubbel matningsspänning ut på 15V (±65mA), vid en enkel inspänning på 5V. Detta är ett enkelt sätt att förse OP-förstärkare med rätt matning. Eftersom den arbetar med switchteknik, behöver utsignalen filtreras ytterligare om man har högre krav på sin matningsspänning. Detta görs i två steg, dels med tantalelektrolyter som avkopplingskondensatorer, dels med s k EMI-filter, som är ett LC-nät av T-typ, se Z1-Z2 i kretsschema processorkort. Detta ger en effektiv avstörning av högfrekvent kopplingsbrus.

(45)

5.11 Processorkort

Vissa utvalda delar av konstruktionen provuppkopplades först på experimentkort, för att enkelt utvärdera och verifiera kretslösningar. I ett senare skede etsades kretskort till roboten. Kretskorten är dubbelsidiga med största delen jordplan som topplager, se figur 28 nedan.

Figur 28. Foto på processorkortet

1 Spänningsregulator 5V 2 DC-DC omvandlare

3 20-varvig triminställning av sensorsignalens förstärkning 4 20-varvig triminställning av sensorsignalens vilopunkt 5 Vertikalt monterat sensorkort med gyro och accelerometer

6 Operationsförstärkare, buffrar, förstärker och filtrerar sensorsignaler. 7 ISP-kontakt för programmering av mikroprocessorn

8 Mikroprocessor ATmega16

9 20-varvig inställning av robotens balanspunkt. 10 Inställningsratt för regulatorns proportionella länk. 11 Inställning regulatorns integratorlänk.

(46)

5.12 Tillverkning av kretskort

Efter utvecklingsarbetet som till stor del gjordes på experimentkort, behövdes bättre kretskort, för att få bättre struktur och störmarginaler. Layouter ritades i ExpressPCB, och skrevs sedan ut på en genomskinlig plastfilm, som placerades på ett kretskort med positiv fotoresist. Kretskortslaminatet med plastfilmen belystes med UV-ljus, och mönstret var överfört till fotoresisten. Efter framkallning och etsning erhölls dugliga mönsterkort. Kopparfolien har tjockleken 35 µm. I bottenlagret förekommer ytmonterade komponenter, främst avkopplingskondensatorer, som med fördel placerats under processorn, då denna yta annars hade varit outnyttjad. Överlag har hänsyn även tagits till att avkopplingskondensatorer ska sitta nära respektive IC-krets som ska avkopplas.

Mönsterkorten är dubbelsidiga där topplagret till största delen fungerar som ett jordplan. Minsta ledarbredd är 0.7 mm vilket hör samman med att det är svårt att få bra resultat med mindre ledarbredder vid manuell tillverkning av enstaka kort. Där högre strömmar förväntas gå, t.ex. på motorkortet, har ledarbanorna ökats något. I CAD-programmet används måttsystemet ”MIL”, där 1 MIL = 1/1000 tum. Minsta ledarbredd som används är 12 MIL (0.3mm). Maximala strömmen som kan gå i ledaren kan beräknas enligt: 0174 , 0 ⋅ ⋅ = L V a I

Källa: Elfa faktasidor, [www.elfa.se]

Där

a = ledarens area i mm2

V = maximalt tillåtet spänningsfall L = ledarens längd

0.0174 = specifika motståndet för koppar

Tillåter man ett spänningsfall på 0.1 V med en längd på 10 cm erhålles max ström: 0.6A för en folieledare med 12 MIL bredd och 35 µm tjocklek. Detta är fullt tillräckligt för de flesta förbindelser på kortet. Dock har vissa ledarbredder manuellt fått ökas, där högre strömmar skall gå, t ex

(47)

folieledningar benämns som isolationsavståndet, och är som minst 10 MIL (0.25 mm) för hela kortet, i praktiken mycket större.

Komponenterna har olika krav på sig vad gäller noggrannhet, brus, effekttålighet m.m. Avkopplingskondensatorer är icke kritiska och där kan billiga keramiska kondensatorer användas. I mätkedjan, och särskilt där filterbestämmande komponenter förekommer ställs högre krav. Där har metallfilmsmotstånd valts för exakthet och lågbrusighet. Av samma anledning används polypropylenkondensatorer, som är en avvägning av hög stabilitet, låga förluster och pris.

(48)

(49)

6 Beskrivning av mjukvara

6.1 Tillvägagångssätt vid programmeringen

Kodning sker i C då detta ger en högre abstraktionsnivå än assembler. Det finns flera stora fördelar med att koda i C. Programmet blir mer överskådligt, tillgången till if- while- satser, och att hanteringen av flyttal och negativa (tvåkomplementära) tal sker automatiskt. Porthantering och initieringar måste fortfarande göras på liknande sätt som vid assemblerprogrammering, och viss bithantering görs även ”manuellt”. C-kompilatorn initierar själv stacken, till skillnad från vid assemblerprogrammering, där all stackhantering måste göras ”för hand”. Programmeringsarbetet sker med viss struktur, först måste programmets uppgift noga definieras, varefter flödesscheman och pseudokod skrivs. Därefter kan små testprogram skrivas för att verifiera funktioner var för sig. När testprogrammen är anpassade för det färdiga programmet kan de sättas ihop och provas tillsammans.

Utveckling av mjukvara har gjorts i WinAVR, ett gratis utvecklingsprogram med följande komponenter:

Programmers Notepad Mfile

I Programmers Notepad skrivs ren C-kod, därifrån kompileras även koden. För att sätta upp riktlinjer för sitt projekt skrivs även en s k makefile i Mfile, som innehåller uppgifter om målprocessorn, hur kompilering ska gå till, vilken debugger och programmerare som ska användas, om någon optimering av koden ska ske, vilken C-standard som ska följas (gnu99 etc.) m.m. Programmet kan simuleras och ”debuggas” i AVRstudio4, där programmet kan stegas igenom rad för rad och stimuli kan läggas på portar. Man kan även se det assemblerade programmet, men beroende på optimeringsnivå kan det vara svårt att läsa ut vad som händer, dock kan vissa ledtrådar ges vid ett eventuellt fel. Vid assemblerprogrammering använder man olika register som man måste hålla reda på, i C används istället variabler av olika datatyper. Ingen hänsyn behöver då tas till vilka register som verkligen används. Vanliga grunddatatyper som används är

int, float och char. Tillägget unsigned kan användas om man vet att en

(50)

Datatypen unsigned char är mycket användbar då den har ett talområde 0-255, som ju motsvarar 8-bitar. Detta används mycket i processorn då den ju är en ren 8-bitars processor. Skrivning och läsning av portar i processorn sker alltid med 8 bitar.

Från AVRstudio överförs programmet i form av maskinkod (.hex-fil) till en AVRISP mkII, se figur 29, som är en programmerare som kan programmera processorn där den sitter i sin konstruktion. Detta klarar den med en 6-polig kontakt, innehållande ett s k ISP-interface (In-Socket-Programming) se figur 30. Programmeraren strömförsörjs externt från robotens egna strömförsörjning.

Figur 29. AVRISP mkII som använts vid ISP-programmeringen av roboten.

MISO Master In – Slave Out MOSI Master Out – Slave In SCK Serial ClocK

RST ReSeT VTG VolTaGe GND GrouND

(51)

6.2 Programmet

En fullständig programlistning återfinns i appendix B.

Programmet är avsett för en ATmega16 processor, med en klockhastighet på 4 MHz. Globala deklarationer är av typen volatile, därför kan de användas mellan funktioner och samtidigt behålla sina värden. Där endast 8 bitars positiva heltalsvariabler behövs, används med fördel unsigned char, för att undvika onödig minnesallokering. AD-omvandling görs som mest med 10 bitar (sensormätningar), och för parameterinställningar (P, I, D, offset) räcker 8 bitar. Varje omvandling görs "två gånger", då första omvandlingen efter ett AD-kanalbyte inte är att lita på. Samplingsintervallet, T = 1/256 s, för golvavkännarsensorn är vald med tanke på minst 10 x bandbredden hos golvavkännaren, som ju är bandbegränsad till 25 Hz av ett lågpassfilter. Processorn klockas med 4MHz varför den hinner med en hel del mellan varven då reglerloopen körs var 1/256 sekund. Pulser från hjulkodskivan genererar som mest ett avbrott var 1/315 sekund.

Ett sätt att undvika att register flödar över/under är att ta till s k ”mättad aritmetik”. Figur 31a visar ett register utan någon kontroll om det flödar över eller under. I figur 31b däremot, ges ett största och minsta värde oavsett om värdet skulle öka eller minska mer än vad som är tillåtet. Ett enkelt sätt att lösa detta är att kontrollera om registret nått sitt maxvärde, har det gjort det, sätter man registret lika med maxvärdet, oavsett om registret vill ”räkna över”.

Figur 31a, Överflödat register, 31b samma register men som åtgärd har s k mättad aritmetik använts

(52)

AD-omvandling sker med 10 bitar, i en del fall räcker dock 8 bitars upplösning, varför de två LSB helt enkelt kastas bort (trunkeras). Programmet hanterar flyttal, t ex PID-parametrarna. Utstyrningen till motorerna via pulsbreddsmodulering sker dock med ett heltal, 0-255, varför en avrundning sker uppåt på detta sätt i C-kod: PWM=ceil(u).

I stora drag är programmet skrivet efter flödesschemat i figur 32.

Det består av en mainfunktion, som förutom initieringar av Timer0 och ADC, även avläser parameterpotentiometrar varje gång efter ett Timer0-avbrott. I övrigt väntar mainfunktionen endast på avbrott från Timer0.

(53)

Timeravbrott Timer0 T sek

Läs ADC

Beräkna felet e(n) MAIN

Start

Uppdatera variabler

Styr ut till motorer

reti

Beräkna utsignal u=(P+I+D) Vänta på Avbrott Deklarationer och initieringar ADC Timer0 Tillåt avbrott

(54)

6.2.1 Mainfunktionen

Efter att ha AD-omvandlat PID-parametrar en gång, väntar mainloopen endast på timeravbrott. Efter ett timeravbrott AD-omvandlas PID-parametrar igen, därefter ställer sig mainfunktionen på nytt i vänteläge.

6.2.2 Reglerfunktion

Denna är ett timeravbrott som genereras varje T, dvs. var 1/256 sekund, eller omkring var fjärde millisekund. Alla vidare beräkningar av PID-parametrar är beroende av T, varför en ändring av samplingstiden också måste medföra en omberäkning av värdet som timer0 ska laddas med. Sensorerna AD-omvandlas för att få 10-bitars upplösning (1024 nivåer), och normeras kring noll, eftersom sensorernas nollnivå ska vara kalibrerade till 2.5V. Därefter sker beräkning av motorutstyrningen i PID-regulatorn. Motorutstyrningen delas upp i respektive rotationsrikning, och två av processorns PWM-kanaler (OCR0A och OCR0B) laddas med utstyrningsvärden. Utöver motorutstyrningsvärden finns en offset på 0x24 i programmet, vilket motsvarar en Duty-cycle på 63 %. Detta för att motorerna snabbt ska kunna ge respons vid små utstyrningsvärden. Motorerna ligger så att säga och drar lite ström för att minska starttiden. Valet av T till 1/256 s ger en för processorn enkel beräkning då detta motsvaras av åtta högerskift.

Vinkelavkänningen ges ett fönster på ca ±20 grader, kommer golvavkännaren utanför denna vinkel, ser programmet till att stänga av motorerna, som säkerhetsåtgärd, då 20 grader felvinkel åt ett håll innebär att roboten är i "fallet" och har ingen chans att hämta upp.

(55)

7 Slutsatser

7.1 Balansering

Det var inga större svårigheter att få roboten balanserad med golvavkännare, efter en del inställning av PID-parametrarna. Inställningsrattarna gör det möjligt att i realtid visa vad som händer vid ändrade parametrar. Balansering förutsätter att underlaget är plant, avvikande lutning verkar som en extra offset, och får roboten att skena. Inkörningen av regulatorn medförde en del praktiska problem, minsta felinställning och roboten slår i golvet. Det är inte fråga OM, utan NÄR. Inte lika förlåtande som regleringen av vattennivån i en tank.

(56)

7.2 Balansering med gyro och accelerometer

Arbetet har från första början bedrivits med sikte på att få roboten balanserande med sensordata från gyro och accelerometer, utan lyckat resultat. Trots en hel del laborerande med placeringen av sensorerna, kunde jag inte upptäcka något entydigt svar angående var sensorerna bäst skulle placeras. Det gick inte heller att rent intuitivt säga var bästa placeringen var. Mina funderingar var att om sensorerna placerades högt, skulle små förändringar märkas tidigt, men avläsningen skulle också påverkas negativt av motorernas kompensering. Placerades sensorerna lågt, nära hjulens rotationscentrum, skulle en mer sann bild mätas, men med väldigt små utslag, som skulle kräva hög förstärkning. Hade robotkroppen varit längre och lättare kanske uppförandet hade varit stabilare. Uppgiften kräver mer tid än vad som kan anses rimligt för detta exjobb.

7.3 Förbättringar

Robotkroppen är ganska tung, och har inte tyngdpunkten tillräckligt högt, detta medför svårigheter att få den balanserad. Balansering kan även ske med mätdata från accelerometer och gyro, med något mer arbete för processorn då extra signalbehandling tillkommer.

Roboten måste helt klart förses med hjulåterkoppling. Detta för att fullt ut kontrollera dess position. I nuläget finns hjulencoders på roboten, men då detta kom upp som en nödvändighet i slutet av arbetet med den, hann jag inte implementera detta till fullo i mjukvaran.

7.4 Avslutning

Till sist en bild som på ett enastående sätt löser balansproblemet, och på samma gång gör hela detta arbete onödigt?!

(57)

Figur 34. Segway? Jämför med figur 1 i inledningen [http://maddox.xmission.com]

(58)

8 Referenser

[1] T. Glad & L. Ljung (2006) Reglerteknik, grundläggande teori. Studentlitteratur.

[2] Ibrahim, Dogan (2006) Microcontroller based applied digital control. Wiley. [3] John Geen & David Krakauer (2003) New iMEMS angular-rate-sensing gyroscope. Analog dialogue.

[4] J. Charais & R. Lourens (2004) AN964 Software PID control of an inverted pendulum using the PIC16F684. Microchip technology inc.

[5] Wanhammar, Lars (2007) Tables and formulas for analog and digital filters. Electronic systems, department of electrical engineering, Linköpings Universitet.

[6] Albert-Jan Baerveldt & Robert Klang, A low-cost and low-weight attitude estimation system for an autonomous helicopter. Teknisk rapport, center for computer systems architecture, Halmstad universitet.

[7] Wanhammar L. & Johansson H. (2007) Digital filters. Department of electrical engineering, Linköpings Universitet.

[8] Geen J. & Krakauer D. (2003) New iMEMS® Angular-Rate-Sensing Gyroscope. ADI Micromachined Products Division. Hämtad 2008 från http://www.analog.com/library/analogDialogue/archives/37-03/gyro.html

(59)

(60)

Da tu m U tf ö rd G ra n ska d G o d kä n d Go d k Gra n sk Da tu m Ko n s tr / R itad Än d ri n g Bla d Ri tn ingsf ö rt . Sammans t. Ska la P rog ra mv a ra: P rot e l D X P , A lt iu m De s ig n e r 6 Fi lnamn : av ISY / Kretsschema

Exjobb balanserande robot

Pr oces sorko rt * * 1 1

--ISY / M C U. S c hD oc 090 12 6 F redrik N ils so n

Appendix

A

10 k R1 7 10 k R1 8 10 k R1 9 PB 0 ( X C K /T0) 1 PB1 ( T 1 ) 2 PB 2 ( A IN 0/I N T2 ) 3 PB 3 ( A IN 1/O C 0 ) 4 PB4 ( S S ) 5 PB5 ( M O S I) 6 PB6 ( M ISO ) 7 PB7 ( S C K ) 8 RESET 9 PD 0 ( R X D ) 14 P D 1 ( T XD) 15 PD 2 ( IN T 0 ) 16 PD 3 ( IN T 1 ) 17 P D 4 ( O C1 B) 18 PD 5 ( O C 1 A ) 19 PD 6 ( IC P ) 20 PD 7 ( O C 2 ) 21 XT A L 2 12 XT A L 1 13 GND 11 PC0 ( S C L ) 22 P C 1 (S DA) 23 PC2 (TCK ) 24 PC3 ( T M S ) 25 P C 4 ( T DO) 26 PC5 (TD I) 27 PC 6 ( T O S C1) 28 PC 7 ( T O S C2) 29 ARE F 32 AVCC 30 GND 31 PA 7 ( A D C 7) 33 PA 6 ( A D C 6) 34 PA 5 ( A D C 5) 35 PA 4 ( A D C 4) 36 PA 3 ( A D C 3) 37 PA 2 ( A D C 2) 38 PA 1 ( A D C 1) 39 PA 0 ( A D C 0) 40 VC C 10 U1 AT m eg a16 -1 6 P C 1 2 X3 22 p C9 22 p C1 0 100n C8 100n C7 10uH L1 +5 V 0V 0V +5 V 4.0 MH z 1 2 G1 10 k R1 6 0V 0V 0V 0V +5 V Of fse t D I P +5 V 2x3-po l 12 34 56 BY1 10 0 R2 2 100 R2 1 100 R2 0 10 0 R2 3 MO S I MIS O SCK RS T 0V +5 V MO S I MIS O SCK RST 1 k R2 4 +5 V ISP p rogrammeringsko n takt 0, 1 µF C3 0, 1 µF C4 0, 1 µF C5 0,1 µ F C6 0V 0V 0V 0V PW M H PW M V en co d er B en co d er A DI R H DI R V ac c gy ro ex tr a 1 ex tr a 2 1 2 3 4 X2 DI R V DI R H ex tr a 1 ex tr a 2 PWM V PWM H Vi n 1 GND 2 +Vou t 3 N1 L 7 8 0 5 C V +5 V 100n C1 7 10 µF / 16 V C1 5 1N4 0 0 1 V1 TE D 0 5 2 2 +/ -1 5 V , +/ - 6 5 m A +Vou t 3 GND 2 +Vi n 1 Co m 4 DC /DC -Vo u t 5 N2 3 1 2 Z1 EM I-filte r 3 1 2 Z2 EM I-filte r 4,7µ /2 5V C1 2 4,7µ /2 5 V C1 3 + 15V _-15V 0V 5 6 7 ++ -B LM 8 3 7 ++ -9 ₁₀ 8 C LM 837 1 12 13 14 ++ -D LM 837 4 _V+ V- 11 E LM 83 7 9.31k R4 9.31k R5 10k R6 10k R1 1k R2 1k R8 10 0 k R1 1 10 0 k R1 5 20k R1 3 50k R3 68 0 n C2 68 0 n C1 0V 0V 0V C1 1 10 0 n +5V + 15V _-15V C1 6 10 0 n 0V L P-filter 25 H z F ö rs tä rk are 0 -15 ,5 d B Bias ADC6 ADC 6

(61)

if ik at io n Da tu m U tf ö rd G ra n ska d G o d kä n d Go d k Gra n sk Da tu m Ko n s tr / R itad Än d ri n g Bla d Ri tn ingsf ö rt . Sammans t. Ska la P rog ra mv a ra: P rot e l D X P , A lt iu m De s ig n e r 6 Fi lnamn : av ISY / Kretsschema

Exjobb balanserande robot

Moto rkort * * 1 1

--ISY / Mot o rkort. Sc h D o c 090 12 6 F redrik N ils so n

Appendix

A

1 2 3 4 MC T 2 V2 1 2 3 4 MC T 2 V5 rö d V4 FEL

M

10n C1 8 10 n C1 9 LM D 1 8 2 0 0 bt st r1 1 Ou t1 2 Di r 3 Br ak e 4 PW M 5 GN D +V s 6 7 CSO 8 TFO 9 Ou t2 10 bt st r2 11 N3 0V + 14.4V 1k R2 5 + 1 4. 4V 0V 0V 0V 6,8k R2 8 6, 8 k R2 9 33 0 R2 7 33 0 R2 6 + 1 4. 4V +5V 1 2 X4 DIR PW M M1 M o to rbry gg a ( x 2) : 74 H C T 1 4D 3 4 D1 B 74H C T 14D 1 2 D1 A 74 H C T14 D 9 8 D1 D 74H C T 14D 5 6 D1 C 74H C T 14D 11 10 D1 E 74H C T 14D 13 12 D1 F 74H C T 14D 7 14 +5V 0V D1 G 0V 0V +5 V 10 0 n C2 0 22k R3 2 22k R3 3 1 2 3 4 IT R 8307 V6 1 2 3 4 ITR 8 3 0 7 V3 0V 0V 18 0 R3 1 180 R3 0 +5 V +5 V +5 V R e fl exd e tekto rer m o nte rad e 90 g rad e r mo t vara n d ra 1 2 X5 A B S c h m it t-tr igger s

(62)

Appendix B Källkodslistning

/***************************************************************** Detta program implementerar en PID-regulator på s k velocityform till en balanserande robot. Sensordata AD-omvandlas på ADC0, för att sedan beräkna PWM styrdata till två motorer.

Samplingstiden är 1/256 sek. Avsett för ATmega16, 4MHz klockfrekvens. För ändring av reglerloopens samplingstid, ändra BÅDE timervärdet i TCNT0 (två ställen!) och konstanten T.

Titel: main.c

Version: 1.0

Datum: 080514

Kompilator: WinAVR

Av: Fredrik Nilsson Ei3

*****************************************************************/ // Inkluderingar #include<avr/io.h> #include<math.h> #include<avr/signal.h> #include<avr/interrupt.h> #include<avr/delay.h> #include<stdint.h> SIGNAL (SIG_OVERFLOW0); // Deklarationer

volatile int angle;

volatile float kp, ki, kd, Ti, Td, u, a, b, c, v1, v2, AD_angle; volatile int temp1,temp2,n;

volatile unsigned int temp; void main(void)

{

uint8_t data, addr; // IO

DDRA=0x10; //PA0-6 = IN, PA7 = OUT

DDRB=0xFF; //PB0-7 = OUT

DDRC=0x00; //PC0-7 = IN

DDRD=0x3F; //PD0-5 = OUT, PD6-7 = IN

// Timerinitiering

TCCR0=0x04; //Normal mode, prescale= 256