Quadrocopter med egendesignat styrsystem.

[LHelge]

Under tiden jag väntar på att kunna testa det flygplan jag berättat om i denna tråd så kan man väl lika gärna sätta igång med nästa projekt. Jag har i flera år funderat på att bygga en quadrocopter men det har inte blivit av. Nu tänkte jag att jag skulle ta tag i detta och se hur det lyckats.

Det jag fastnat på tidigare har varit att jag vill bygga styrsystemet själv och jag har snöat in i väldigt komplicerade filter för att estimera pitch, roll och yaw som oftast resulterat i en orgie i flyttalsberäkningar och trigonometri som inte skulle vara möjligt att köra i en microkontroller.

För ett tag sedan läste jag om någon som hade förenklat problemet genom att dela upp det i ett filter för pitch och ett för roll vilket egentligen är så simpelt att jag inte förstår varför jag inte tänkte på det själv. Tänker man efter borde detta fungera väldigt bra så länge vinklarna inte är allt för nära ±90°

Sagt och gjort, jag klottrade ihop ett linjärt kalmanfilter med tre states, vinkel, vinkelhastighet och gyrobias. Ägnade någon timme på att utveckla och optimera alla matrisberäkningar. Det gick att optimera ganska mycket pga att flera matriser är diagonala. Jag gissade på lite schyssta värden på Q- & R-matriserna, kopplade upp en dspic med ett 3-axligt gyro och en 3-axlig accelerometer via I2C.

test1.jpg

Ett testprogram i C# som tar emot pitch och roll beräknat på två olika sätt via com-porten.

testprog.jpg

Till vänster visas det som beräknas direkt på accelerationen och till höger visas det filtrerade värdet.

Måste erkänna att jag blev lite förvånad när det fungerade ganska bra direkt bortsett från lite trial and error för att hitta rätt tecken och axlar på sensorerna.http://www.youtube.com/watch?v=92R0YRvJAlY. Lägger jag ned lite tid på att finjustera Q- & R-matriserna så skulle det nog kunna bli ännu bättre. Jag ska nog till exempel straffa accelerometern lite hårdare för att minska förändringarna då jag skakar på kortet.

Själva algoritmen körs i 100 Hz med 32-bitars flyttal i en dsPIC33F @ 40MIPS. Nu kör jag algoritmen i 100Hz och då står processorn och väntar ca 5/6 av tiden så det är nog inte några problem att få till resten av funktionerna också.

Någon som vet hur ofta det går att skicka uppdateringar till borstlösa ESC:er? Till analoga servon vet jag att man inte ska skicka pulser oftare än var 20:e millisekund. Men en digital ESC borde teoretiskt sett kunna uppdateras betydligt oftare. Hade varit snyggt om man kunde köra reglersnurran lite fortare än så. 100Hz-200Hz borde vara lagom. Vinkelregleringen blir förmodligen en PID till att börja med.

I hårdvaruväg har jag tänkt att använda samma typ av motor och propeller som jag har till mitt depronplan. Det borde ge en lyftkraft på ca 3-4 Kg. Det borde vara tillräckligt för att ta med ett ordentligt batteri och kanske en liten kamera så småningom. Till att börja med ska jag nog bygga den i trä för att sedan göra den i aluminium när jag är nöjd med storlek och flygprestanda.

[ekman]

Jag drömmer om den dagen jag jag kan plocka ihop en Quadrocopter. Tills dess så kommer jag följa det här projektet med spänning
Och var inte rädd att gå in på detaljer, kanske man lär sig nått samtidigt.

[Andax]

Snyggt jobbat!

Håller också som bäst på med mitt quadrocopter projekt. Som IMU har jag en 9 DoF Razor IMU från sparkfun. Ser att du använder dig av accelerometar etc från sparkfun. Razorn har redan program för ihopfiltrering av gyro och accelerometerdata så att man får ut roll, pitch och yaw via en serieport.
Längre fram vill jag nog försöka filtrera ihop IMU och GPS för bra höjd och positionshållning. Bollar gärna tankar kring det med dig fram över.

Som kontroller har jag en STM32F103 modul, dvs en 32-bitars ARM Cortex M3 som kör på 72 MHz. Så den räknar jag med ska hinna med en del. Vet inte hur den står sig mot en dpPIC.

Lägger väl också upp en projekt tråd så småning om....


Än en gång, snyggt jobbat!

[Fagge]

Riktigt snyggt jobbat.
Nu ångrar man ihjäl sig att man inte la ner mer energi på mattelektionerna i skolan.
Ser verkligen fram emot detta, & kanske till & med en byggsats till oss vanliga dödliga EF'are.

[LHelge]

Då jag inte hade någon digital kompass så har jag hoppat över yaw än så länge men det borde gå att använda exakt samma filter med en något annorlunda tunig för yaw också. Till att börja med skiter jag nog i aktiv kontroll av yaw. Har en känsla av att det är lättare att styra själv än pitch och roll.

Något som hade varit mer intressant att testa skulle vara att skita i kompass och använda styrsignalen till motorerna som input istället. Det som behövs är ju någon referens för att filtret ska kunna hitta ett bra estimat av gyrots bias. Kan man anta att utsignalen till motorerna är det enda som påverkar yaw så borde detta funka. Det kan hända att man måste lägga till ett extra state till för att modellera dynamiken från styrsignal till motorvarvtal. Tyvärr kan jag inte testa detta eftersom jag inte har någon quad att styra än.

Jag har också sneglat lite på STM32 för detta, kanske främst denna: 73-872-39 men även på en PIC32. Det hade varit skoj att testa en 32-bitars processor. Att ST dessutom strösslat med timers gör det ju ganska angenämt att generera styrpulser till ESC:er och läsa styrpulser från RC-mottagaren. Vad gäller prestanda så ska den nog vara minst likvärdig med dsPIC 24 MHz 1.25 MIPS/MHz ger 30 MIPS dvs 10 färre än dsPIC:en men däremot så kan ju varje instruktion räkna med dubbelt så många bitar. Jag vet at tman inte kan jämföra så rakt av men det borde ge en fingervisning iaf.

[LHelge]

Jag släpper gärna scheman och kod när det börjar bli klart, och är inte helt främmande för att sätta ihop en byggsats heller. Beställer jag något kretskort till detta så kan man lika gärna beställa ett par stycken, brukar inte bli så mycket dyrare.

[PeterH]

Jag blir alltid lika avundsjuk på er som kan matte så bra som ni faktiskt kan, kommer också följa detta med stort intresse (och beundran)

[jesper]

Jag gjorde ett kort med en STM32 för min quad. Tyvärr har jag inte haft tid att pilla mycket med det som jag hoppades.
Mer info här: http://elektronikforumet.com/forum/view ... 7&start=32

[hubben]

Låter RIKTIGT spännade detta, men varför använder du inte samma metod med gyro/accelerometer för Yaw?

Som standard vill en ESC ha samma uppdateringsfrekvens som ett servo, men det finns folk som programmerat om sina Turnigy Plush ESC:ar till att ta uppdateringar snabbare. Det går tom att få ESC:en att snacka I2C direkt...
http://www.rcgroups.com/forums/showthre ... 89&page=45

[Mattsson81]

Jag släpper gärna scheman och kod när det börjar bli klart, och är inte helt främmande för att sätta ihop en byggsats heller. Beställer jag något kretskort till detta så kan man lika gärna beställa ett par stycken, brukar inte bli så mycket dyrare.

Kan hålla med om det med korten, man får ofta 5 - 10st kort för samma pris som 1 - 2. Det skiljer bara småkronor ibland, jag brukar köpa av ett lokalt företag i GBG. De är bra för de har en genomgång av korte innan produktion, så de ger feedback direkt som "lagar" min caddmissar. och det brukar det kunna vara lite härt och där...

Vart brukar du köpa kort? Va kostar de för 2 - 4 laers kort?

[LHelge]

Ska skriva en mer utförlig beskrivning ikväll av hur jag gjort men kort kan man säga att kalmanfiltret arbetar i följande steg. låt säga att vi ska mäta vinkeln kring x-axeln (där z pekar upp/ned)

1. Räkna ut vad vinkeln, vinkelhastigheten och gyrobias borde vara baserat på förra mätningen. dvs
vinkel = förra vinkeln + förra vinkelhastigheten * samplingstiden - förra gyrobias * samplingstiden
vinkelhastigeten = förra vinkelhastigheten (konstant)
gyrobias = förra gyrobias (konstant)

2. Statistiskt mumbo-jumbo för att bestämma hur mycket du tror på de tro olika faktorerna i förra steget.

3. Mät vinkeln på accelerationsvektorn i y/z-planet och anta att den enbart består av tyngdaccelerationen. och mät vinkelhastigheten runt x-axeln med gyrot.

4. Kompensera uträkningen i steg 1 med mätningarna i steg tre beroende på det statistiska mumbo-jumbot i steg två och hur mycket du litar på dina givare.

5. Mer statistiskt mumbojumbo för att räkna ut hur säker du är på dina mätningar efter de kompenserats med nya mätvärden.

6. tillbaka till punkt 1.

På grund av att punkt tre utnyttjar tyngaccelerationen som i normalfallet är 0 i x/y planet så går inte detta att använda för att beräkna yaw. Istället brukar man normalt använda en kompassgivare där man istället beräknar magnetfältet i x/y-planet istället.

Vad gäller köpa mönsterkort så har jag aldrig gjort det förut. Håller med om att det skulle vara smidigt att köpa dem från Göteborg. Tror det ligger ett företag som heter Multiteknik några hundra meter hemifrån som gör sådant bland annat. Säkert dyrare än asien, men man sparar lite pengar på frakt när man kan promenera och hämta dem.

[Andax]

Då jag inte hade någon digital kompass så har jag hoppat över yaw än så länge men det borde gå att använda exakt samma filter med en något annorlunda tunig för yaw också. Till att börja med skiter jag nog i aktiv kontroll av yaw. Har en känsla av att det är lättare att styra själv än pitch och roll.

Något som hade varit mer intressant att testa skulle vara att skita i kompass och använda styrsignalen till motorerna som input istället. Det som behövs är ju någon referens för att filtret ska kunna hitta ett bra estimat av gyrots bias. Kan man anta att utsignalen till motorerna är det enda som påverkar yaw så borde detta funka. Det kan hända att man måste lägga till ett extra state till för att modellera dynamiken från styrsignal till motorvarvtal. Tyvärr kan jag inte testa detta eftersom jag inte har någon quad att styra än.

Även om du inte skattar upp en perfekt yaw som ligger still i tiden så man du ju ändå använda den för att stabilisera. Ver inte hur du tänkt styra referenssignalen. Mn jag tänker mig nog att jag styr vinkelhastigheten på yaw. Nominellt ligger den på noll.

Jag har också sneglat lite på STM32 för detta, kanske främst denna: 73-872-39 men även på en PIC32. Det hade varit skoj att testa en 32-bitars processor. Att ST dessutom strösslat med timers gör det ju ganska angenämt att generera styrpulser till ESC:er och läsa styrpulser från RC-mottagaren. Vad gäller prestanda så ska den nog vara minst likvärdig med dsPIC 24 MHz 1.25 MIPS/MHz ger 30 MIPS dvs 10 färre än dsPIC:en men däremot så kan ju varje instruktion räkna med dubbelt så många bitar. Jag vet at tman inte kan jämföra så rakt av men det borde ge en fingervisning iaf.

Den STM32 som ligger på modulen jag länkade till kör ju på 72MHz, dvs 90 MIPS. Den har ju inte inbyggt floating point stöd, frågan är vad man får för prestanda på soft floating point.
Den har riktigt kraftfulla och många TIMERs. Eftersom de har många capture-register (4 st) kunde jag med en timer generera 4 kanaler PWM till ESCarna. 16 bitars räknare och 20 ms cykel-tid ger 3000 steg för pwm-pulsen när den varierar mellan 1 och 2 ms vilket är myket mer än nödvändigt. Gissar att ESCarna inte mäte pulsen så noggrannt.
Med en annan timer och lite interruptkod har jag gjort en 4 kanals pwm-input capture så att jag kan läsa av läget på 4 kanaler från min radiomottagare. Postar koden så småning om om någon är intresserade.

[LHelge]

Jag kör ju soft floatingpoint på dsPICen också och jag skulle tro att STM32 är betydligt snabbare med tanke på den dubbla registerbredden. Reglerloopen kommer jag däremot skriva med fixed-point.

[hubben]

Med tanke på hur duktigt ett gyro till en modellhelikopter är på att hålla svansen stilla omerbeonde av yttre påverkan så borde det gå att lösa med gyro/accelerometer... dessa använder ju bara ett gyro.
Men de kanske använder en annan filtreringsalgoritm...

Hursom haver låter detta väldigt spännade!
Är även jag nyfiken på var ni beställer era kort.

[LHelge]

Ja, hålla den stadigt borde inte vara några problem. Den allra simplaste lösningen som jag kommer börja med är att ta ett medelvärde på z-gyro någon sekund innan jag aktiverar motorerna. Dvs quaden står still. Då har jag min bias och kan utifrån det använda den bias-kompenserade yaw-rate direkt som input till regulatorn och hämta referensvärdet direkt från mottagaren. Gyrot har ett inbyggt lågpassfilter så signalen är inte alls brusig. Denna lösning tilsammans med lite manuella korrektioner medans man flyger borde fungera utmärkt, i alla fall lika bra som en modellhelikopter.

Absolutvärde av yaw är egentligen bara viktigt om man använder den som UAV.