Institutionen för systemteknik

Transkript

1 Institutionen för systemteknik Department of Electrical Engineering Examensarbete Observatörer för skattning av verktygspositionen hos en industrirobot Design, simulering och experimentell verifiering Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Robert Henriksson LITH-ISY-EX--9/427--SE Linköping 29 Department of Electrical Engineering Linköpings universitet SE Linköping, Sweden Linköpings tekniska högskola Linköpings universitet Linköping

2

3 Observatörer för skattning av verktygspositionen hos en industrirobot Design, simulering och experimentell verifiering Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Robert Henriksson LITH-ISY-EX--9/427--SE Handledare: Examinator: Erik Wernholt isy, Linköpings universitet Mikael Norrlöf, Stig Moberg ABB AB Robotics Thomas Schön isy, Linköpings universitet Linköping, februari, 29

4

5 Avdelning, Institution Division, Department Division of Automatic Control Department of Electrical Engineering Linköpings universitet SE Linköping, Sweden Datum Date Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport ISBN ISRN LITH-ISY-EX--9/427--SE Serietitel och serienummer Title of series, numbering ISSN URL för elektronisk version Titel Title Observatörer för skattning av verktygspositionen hos en industrirobot Design, simulering och experimentell verifiering Observers for estimation of the tool position for an industrial robot Design, simulation and experimental verification Författare Author Robert Henriksson Sammanfattning Abstract This thesis approaches the problem of estimating the arm angles of an industrial robot with flexibilities in joints and links. Due to cost-cutting efforts in the industrial robots industry, weaker components and more cost-effective structures have been introduced which in turn has led to problems with flexibilities, nonlinearities and friction. In order to handle these challenging dynamic problems and achieve high accuracy this study introduces state observers to estimate the tool position. The observers use measurements of the motor angles and an accelerometer and the different evaluated observers are based on an Extended Kalman Filter and a deterministic variant. They have been evaluated in experiments on an industrial robot with two degrees of freedom. The experimental verification shows that the state estimates can be highly accurate for medium frequency motions, ranging from 3-3Hz. For this interval the estimate were also robust to model inaccuracies. The estimation of low-frequency motions was relatively poor, due to problems with drift for the accelerometer, and it also showed a significant dependence on the accuracy of the model. For industrial robots it is mainly the medium frequency motions which are hard to estimate with existing techniques and these observers therefore carries great potential for increased precision. Nyckelord Keywords Observatör, Industrirobot, Flexibla manipulatorer, Accelerometer, Extended Kalman Filter

6

7 Abstract This thesis approaches the problem of estimating the arm angles of an industrial robot with flexibilities in joints and links. Due to cost-cutting efforts in the industrial robots industry, weaker components and more cost-effective structures have been introduced which in turn has led to problems with flexibilities, nonlinearities and friction. In order to handle these challenging dynamic problems and achieve high accuracy this study introduces state observers to estimate the tool position. The observers use measurements of the motor angles and an accelerometer and the different evaluated observers are based on an Extended Kalman Filter and a deterministic variant. They have been evaluated in experiments on an industrial robot with two degrees of freedom. The experimental verification shows that the state estimates can be highly accurate for medium frequency motions, ranging from 3-3Hz. For this interval the estimate were also robust to model inaccuracies. The estimation of low-frequency motions was relatively poor, due to problems with drift for the accelerometer, and it also showed a significant dependence on the accuracy of the model. For industrial robots it is mainly the medium frequency motions which are hard to estimate with existing techniques and these observers therefore carries great potential for increased precision. Sammanfattning Detta projekt belyser problematiken att estimera verktygspositionen för en industrirobot med flexibiliteter i leder och armar. På grund av kostnadsbesparingar används i dagens industrirobotar allt vekare komponenter och svagare strukturer vilket leder till problem med flexibiliteter, olinjäriteter och friktion. För att hantera den svårmodellerade dynamiken och uppnå god noggrannhet introducerar denna rapport olika observatörer för att skatta verktygets position. Observatörerna använder mätningar av motorvinklarna och en accelerometer och de baseras på Extended Kalman Filter och en deterministisk variant. Observatörerna har utvärderats vid experiment på en robot med två frihetsgrader och skattningen blir mycket god för medelhöga frekvenser (3-3Hz) medan långsammare rörelser (-3Hz) är betydligt svårare att skatta. Testerna visar att för att uppnå större noggrannhet för lågfrekventa rörelser behövs en väl överensstämmande matematisk modell för att beskriva systemet, medan skattningen av de medelsnabba rörelserna är mer robusta för modellfel. Eftesom det framförallt är de medelsnabba rörelserna för en industrirobot som är svåra att skatta med dagens tekniker, visar dessa resultat på att det finns stor potential i accelerometerbaserade observatörer för industrirobotar. v

8

9 Tack Jag vill börja med att tacka avdelningen för rörelsestyrning (PRCM) på ABB Robotics som gett mig förtroendet att genomföra detta projekt. Det har varit ett mycket roligt och utmanande projekt. Jag vill främst tacka Mikael Norrlöf och Stig Moberg på ABB Robotics som tålmodigt tagit sig an frågor och problem som jag stött på under arbetets gång. De har funnits med hela tiden och hjälpt till med allt som involverat projektet. Ett särskilt tack för all hjälp med mätningar på sena kvällar, matematiska modeller för robotarna och mycket givande diskussioner. Jag vill också tacka Erik Wernholt och Thomas Schön på ISY i Linköping som kommit med mycket bra återkoppling och synpunkter och framförallt hjälpt till vid utformandet av rapporten. Patrik Axelsson, som arbetat med ett annat examensarbete parallelt med mig på ABB Robotics, vill jag även tacka för hjälp med implementering på robot, tips och trix under arbetets gång samt trevligt sällskap. Slutligen vill jag också tacka min kära flickvän Andrea som alltid finns med och gör allting så mycket roligare och lyckas slita bort mig från de matematiska modellerna och Matlab på sena kvällar när ögonen blivit fyrkantiga. Tack för förtroendet! Jag hoppas att detta projekt kan vara till nytta både inom vidare forskning men också för att bidra till ABB Robotics konkurrenskraft i framtiden. Robert Henriksson, Västerås januari 29 vii

10

11 Innehåll Introduktion. Bakgrund Syfte Mål Metod Avgränsningar Tidigare forskning ABB Robotics och LINK-SIC Rapportdisposition Grundläggande robotik 5 2. Grundläggande begrepp Koordinatsystem Kinematik Dynamik - stelkroppsmodell Utökad robotmodell Flexibiliteter i leder Matematisk modell Tillståndsmodell Accelerometer 3. Accelerometerns koordinatsystem Matematisk modell Modellering av störningar Kalibrering Estimeringsmetoder 5 4. Matematisk modell Systemmodell och diskretisering Tillgängliga mätningar Extended Kalman Filter Linjärisering Filteralgoritm Alternativa EKF-formuleringar Deterministisk observatör ix

12 x Innehåll 4.3. Matematisk modell Linjärisering Polplacering Diskret implementering Trimning av observatörer Trimning av kovarianser för EKF Kovariansskattning Formulering av optimeringsproblem Trimning av polplacering för deterministisk observatör Simuleringar 3 6. Simulering tvåaxlig robot Utvärderade observatörer Simuleringsresultat Resultat EKF-observatörer Resultat deterministisk observatör Analys av modellfel Slutsatser från simuleringar Experimentella resultat Accelerometermätningar och kalibrering Använda accelerometrar Resultat för EKF-observatörer Skattningsresultat EKF med Crossbow-sensor Analys av kovarianser Trimningskänslighet för EKF Skattningsresultat EKF med Xsens-sensor Deterministisk observatör Skattningsresultat med Crossbow-sensor Analys av polplacering Trimningskänslighet för DL Beräkningskomplexitet Diskussion och slutsatser Generella slutsatser Observatörsspecifika slutsatser Tillämpbarhet och användningsområden Vidare forskning 67 Litteraturförteckning 69 A Definitioner 7 A. Notation A.2 Förkortningar

13 Kapitel Introduktion. Bakgrund Inom industriell robotik har en utveckling mot mer och mer kostnadseffektivitet skett under de senaste åren. Av den anledningen utnyttjas billigare och lättare komponenter, såsom mindre motorer och vekare strukturer. Den totala kostnaden i produktion minskar då komponent- och materialkostnaden går ner. Dock ökar även komplexiteten för rörelsestyrning som en följd av flexibiliteter som uppstår när strukturen påverkas i större utsträckning av belastning och rörelser. Detta innebär svårigheter vid rörelsestyrning och reglering, särskilt eftersom dagens robotar endast kan mäta motorvinklar, och inte verktygets position. På grund av flexibiliteterna i strukturen kommer den uppmätta motorvinkeln att avvika från den faktiska armvinkeln, som bestämmer verktygspositionen. Denna problematik har lagt grunden till intresset för att använda tröghetssensorer för att på ett billigt sätt förbättra skattningen av armvinklar, och därmed verktygets position..2 Syfte Syftet med detta examensarbete är att analysera problematiken med flexibla strukturer och studera hur accelerometrar kan användas i estimeringsmodeller för att förbättra skattningen av armvinklar och verktygsposition. Olika estimeringsmetoder kommer att jämföras och anpassas för att utröna uppnåelig prestanda och erhålla förståelse för hur skattningen av armvinklar kan förbättras på ett kostnadseffektivt sätt. Hur accelerometrarna ska tas in i modellen och hur de kan kalibreras kommer att belysas..3 Mål Målsättningen är att ta fram en välfungerande estimeringsmodell att använda vid simulering och tester på robot. Till att börja med kommer en tvåaxlig robot att modelleras, men om möjligt kommer den även att utökas till tre axlar.

14 2 Introduktion Projektets mål kan sammanfattas som huvudmål samt extramål enligt nedan. Huvudmål - Skall vara uppfyllda vid projektets avslut Skapa observatör som, från motor- och accelerometermätningar, skattar armvinklar och verktygsposition för två-axlig robot i Matlab Erhålla kunskap och förståelse för hur accelerometrar kan installeras för att tillsammans med en observatör uppnå förbättrade skattningar för ett robotsystem Utveckla kalibreringsmetodik för accelerometer Färdigställa en utförlig rapport som beskriver problematiken, redogör för resultat samt möjliga utvidgningar och vidareutvecklingar Extramål - Utföres i mån av tid och endast då huvudmålen är uppfyllda Samma som ovan men för treaxlig robot Utveckla en observatör som nyttjar flera accelerometrar Utveckla en observatör som även använder mätningar från gyron för att skatta armvinklar Utvärdera prestanda då den utvecklade observatören används för ILC (Iterative Learning Control).4 Metod I ett första skede kommer olika observatörer att utvecklas och utvärderas för en robotmodell i simuleringar i Matlab. När en tillfredsställande precision och robusthet uppnås kommer systemet att testas och utvärderas för körningar på robot. Accelerometern kommer då att kalibreras och analyseras, med tillhörande modellförändringar om nödvändigt. Estimeringsmodellerna kommer sedan att testas och utvärderas för att se över vidareutvecklingar som kan vara aktuella för att hantera eventuella svårigheter som uppkommer. Observatörernas skattningar av verktygspositionen kommer att utvärderas med hjälp av lasermätutrustning från Leica som beskrivs i []..5 Avgränsningar Observatörsmodellen kommer att begränsas till att hantera två axlar, men möjlighet för utvidgning skall lämnas. Observatören kommer ej att implementeras som en realtidsversion eftersom mycket tid då måste läggas på att optimera beräkningstid och implementering, vilket ligger ur fokus för detta projekt. Projektet kommer framförallt att belysa implementering av en accelerometer vid verktyget, och inte multipla accelerometrar eller gyron. Då denna studie ämnar utvärdera olika observatörsmetoder för industrirobotar finns det ej en avsikt att i rapporten ge en utförlig beskrivning av ämnesområdet robotik eller dess matematiska formalism

15 .6 Tidigare forskning 3 utöver det som är nödvändigt för att förstå implementering och modellering av observatörer och accelerometer. Den intresserade hänvisas istället till exempelvis [24] för en uttömmande genomgång av robotikområdet. Ytterligare en avgränsning är att skattningen av verktygspositionen kommer att fokuseras på rörelser vid medelhöga frekvenser (3-25Hz). Anledningen till detta är att stora modellfel kan ge upphov till lågfrekventa skattningsfel, trots att de snabbare svängningarna skattas mycket väl. Det är framförallt de snabba förloppen och stora svängningarna som är av intresse att skatta med denna metod eftersom den lågfrekventa delen idag kan estimeras väl med befintliga metoder..6 Tidigare forskning Inom området observatörer för flexibla manipulatorer har en hel del forskning genomförts där exempelvis Nicosia et al. [9] var tidiga på området, och beskrev tillvägagångssätt att hantera den ytterst olinjära dynamiken i flexibla robotstrukturer. Att utveckla observatörer som nyttjar tröghetsmätningar har studerats i stor utsträckning där bland annat De Luca et al. [7] beskriver en observatör för ledflexibiliteter, baserad på en treaxlig accelerometer. Li och Chen [6] går in på ett tillvägagångssätt att hantera armflexibiliteter med hjälp av ett lasermätsystem för positions- och vinkelbestämning samt accelerometer. Även Norrlöf och Karlsson [2] har undersökt hur en accelerometer kan användas i en observatör baserad på Extended Kalman Filter (EKF) och Partikelfilter. Problematiken som kan uppkomma för flexibla mekanismer på grund av gravitationskraften beskrivs av De Luca et al. i [6] där gravitationskompensering gås igenom. Bland olika observatörstekniker har det linjära Kalmanfiltret eller EKF använts i ett flertal studier. Rock och Alder använder ett linjärt Kalmanfilter för en observatör till en enaxlig flexibel robotarm med okänd belastningsdynamik [] samt en enaxlig flexibel robotarm med okänd deformation [2]. EKF används av bland andra Norrlöf [2] och Li [6] men även Lertpiriyasuwat [5] där en EKF observatör för flexibla armar för en tvåaxlig robot studeras. Deras Kalmanfilter använde mätningar av verktygsposition samt vinkelhastigheter i lederna. Jankovic beskriver en observatör för en flexibel robot då mätningar endast finns tillgängliga på motorsidan [3]. Det finns ett antal studier på området men det är ändock relativt ungt, eftersom den beräkningskraft som krävs för exempelvis en EKF för ett treaxligt system inte varit enkelt tillgänglig för enbart några år sedan. Nu med ökande kapacitet för signalbehandling och beräkningar kommer med stor sannolikhet antalet praktiska tillämpningar och studier att växa..7 ABB Robotics och LINK-SIC ABB Robotics är ett bolag inom ABB-koncernen som utvecklar och tillverkar industrirobotar för automationstillämpningar inom framförallt tillverkande industri, där fordonsindustrin länge varit en stor kund. ABB Robotics hade 27 intäkter på ca $.4 miljarder och hade 45 anställda globalt. Examensarbetet har utförts

16 4 Introduktion på avdelningen för rörelsestyrning (PRCM) och är ett av de första projekten inom forskningssamarbetet LINK-SIC som är ett VINNOVA Industry Excellence Center. LINK-SIC startades 28 och är ett -årigt forskningssamarbete mellan LiTH, ABB Robotics, ABB Corporate Research, Saab AB, GM Powertrain - Sweden AB och Scania CV..8 Rapportdisposition - Introduktion Detta avsnitt beskriver projektets bakgrund, syfte och målsättning samt en kort genomgång av tidigare forskning och avgränsningar. 2 - Grundläggande robotik Här ges en redogörelse för den matematiska bakgrunden som behövs för att beskriva en robot. Avsnittet inleds med en härledning av en grundläggande robotmodell och avslutas med en grundlig beskrivning av dynamik, samt tillståndsekvationer för en flexibel robotmodell. 3 - Accelerometer I detta avsnitt beskrivs den accelerometer som använts, dels kinematiskt och dynamiskt när den är fastmonterad på roboten, men även praktiska tekniska detaljer. En beskrivning av störningsegenskaper och kalibreringsmetodik ges också. 4 - Estimeringsmetoder Detta avsnitt presenterar de olika observatörer som undersökts i denna studie. Den matematiska bakgrunden till observatörerna ges också en grundlig genomgång. 5 - Trimning av observatörer I detta avsnitt ges en grundlig genomgång av hur trimningen av observatörerna har utförts. Förfarandet presenteras som ett minimeringsproblem under givna bivillkor för att på ett kompakt sätt definiera trimningsmetodiken. 6 - Simuleringar Här redovisas de slutliga simuleringar som utförts och de resultat som erhållits. Resultaten analyseras och jämförelser görs mellan de olika observatörerna som utvärderats. 7 - Experimentella resultat De praktiska tester på robot som utförts beskrivs här. En genomgång ges av praktiska implementationsfrågor samt de resultat som erhållits. En analys av resultaten samt diskussion om möjliga förbättringar och vidareutvecklingar ges även. 8 - Slutsatser I detta avsnitt dras relevanta slutsatser av användbarhet och praktisk tillämpbarhet av de estimeringsmetoder som undersökts. 9 - Vidare forskning Ett flertal olika områden som skulle vara intressanta att undersöka vidare presenteras i detta avsnitt. Det är framförallt metoder och undersökningar som varit av intresse under studiens gång men inte rymts inom ramarna för projektet.

17 Kapitel 2 Grundläggande robotik Detta avsnitt ger en genomgång av den grundläggande matematik som behövs för att beskriva en industriell robot. Definitioner av variabler finns i bilaga A. 2. Grundläggande begrepp De sammanlänkade kroppar som utgör en robot kan beskrivas av ett antal armar och leder. Antalet leder definierar robotens frihetsgrader (DOF) och för varje DOF används en generaliserad koordinat, q ai, som beskriver armvinkeln för respektive led med avseende på en given referens, såsom angivet för tvåaxelsmodellen i figur 2.. En robot som har N leder har då N frihetsgrader (DOF) och då behövs N generaliserade koordinater q ai, i =,..., N för att beskriva systemet fullständigt. 2.. Koordinatsystem Robotens geometri beskrivs utifrån ett grundkoordinatsystem, {w}, som är fixt vid robotens bas och i detta koordinatsystem anges positionen p w för verktygets centrumpunkt, T CP. I figur 2.(a) och 2.(b) anges koordinatsystemet för en tvåaxlig robot då roboten står i startpositionen resp. i ett aktivt läge med armvinklarna q a > och q a2 > Kinematik En robots geometriska sammansättning och sambandet mellan verktygsposition och armvinklar ges av dess kinematiska beskrivning. Robotverktygets position p w beror av armarnas vinklar och definieras av p w = x w (q a ) y w (q a ) z w (q a ) 5 = Γ(q a )

18 Grundkinematik Grundkinematik 6 Grundläggande robotik TCP [x w z w ] Z w Z w q a2 p w q a p w TCP [x w z w ] X w (a) Startposition X w (b) Aktivt läge Figur 2.. Exempel på konfigurationer för en tvåaxlig robot. För en tvåaxlig robot som verkar i xz-planet kommer definitionen av Γ(q a ) ske i enlighet med figur 2.. Funktionen Γ är också beroende av hur robotens parametrar har definierats. Exempelvis kan armvinklarna och andra parametrar definieras på olika sätt, där en vanlig parameterdefinition baseras på Denavit-Hartenbergnotationen som beskrivs i exempelvis [24]. Om vinklarna definieras i enlighet med figur 2. och armarna har längd L resp. L 2 skulle följande form för Γ(q a ) erhållas [ ] L sin(q p w = Γ(q a ) = a ) + L 2 cos(q a + q a2 ) (2.) L cos(q a ) L 2 sin(q a + q a2 ) 2..3 Dynamik - stelkroppsmodell I detta avsnitt presenteras den grundläggande matematiska modellen som bygger på antagandet att en robot kan beskrivas av stela kroppar utan friktion och utan flexibiliteter i länkar och leder. Eftersom robotmodellen betraktas som ideal utan flexibiliteter eller friktion kan dynamikekvationerna härledas via Lagrange ekvation [24] enligt nedan L = T U ( ) d L L = u ai dt q ai q ai (2.2a) (2.2b) där u ai är pålagt moment på arm i, T är robotens totala kinetiska energi och U dess potentiella energi som ges av T = 2 qt a M(q a ) q a U = U(q a ) (2.3a) (2.3b) med robotens konfigurationsberoende tröghetsmatris betecknad M(q a ). Eftersom den potentiella energin endast beror av q a kommer den bara in i (2.2) som derivatan med avseende på q a, vilket är definitionen på G(q a ), momentverkan av

19 2.2 Utökad robotmodell 7 gravitationen på lederna. Det innebär att (2.2) kan skrivas L = T (q a, q a ) U(q a ) (2.4a) ( ) U(qa ) G i (q a ) = (2.4b) q ai u ai = d ( ) T (qa, q a ) T (q a, q a ) + G i (q a ) (2.4c) dt q ai q ai Derivatorna av den kinetiska energin kan nu härledas T (q a, q a ) = 2 qt a M(q a ) q a (2.5a) T (q a, q a ) = q a T M(q a ) q a = M(q a ) q a (2.5b) q a 2 q a d dt (M(q a) q a ) = Ṁ(q a) q a + M(q a ) q a (2.5c) T (q a, q a ) = q T dm(q a) a dq a q a q a 2. (2.5d) q T a dm(q a) dq an I ekvationerna uppkommer nu ett par termer med tillsynes komplicerad form, men dessa brukar oftast samlas ihop i vad som populärt kallas Coriolis- och centrifugaltermer, C(q a, q a ) enligt C(q a, q a ) = Ṁ(q a) q a 2 q T a q T a q a dm(q a) dq a. dm(q a) dq an q a q a Om sedan (2.5) används i (2.4) med definitionen av C(q a, q a ) ovan och ekvationen skrivs på vektorform erhålls den enklaste formen av dynamikekvationen enligt M(q a ) q a + C(q a, q a ) + G(q a ) = u a (2.6) Denna ekvation ger en robots dynamik då den kan betraktas som en stel kropp utan friktion eller flexibiliteter. I verkligheten uppkommer en viss avböjning och vridning av armar och leder på grund av belastningen, och friktionen är inte alltid försumbar. Den ideala modellen kan då vara otillräcklig för att uppnå en noggrann beskrivning. En mer realistisk modell tar hänsyn till friktion och flexibiliteter och ett vanligt sätt att modellera dessa är att betrakta leder som visko-elastiska med friktion, och därmed införa fjäderstyvheter, viskös dämpning och en friktionsmodell. Ett sätt att bygga upp en mer fullständig robotmodell presenteras i Utökad robotmodell Det finns flera olika modeller för att beskriva ett robotsystem mer realistiskt och i detta avsnitt ges en beskrivning av en utökad robotmodell som hanterar flex-

20 Ledflexibiliteter 8 Grundläggande robotik ns läge Armens läge q a2 q m2 q m, q a Figur 2.2. En robotarm med flexibiliteter i led 2. ibiliteter i leder med olinjär styvhet och friktion samt linjär viskös dämpning. Denna modell beskrivs av Moberg et al. i [7] och alla variabler finns definierade i bilaga A. Det är också denna modell som observatörerna utvecklats utefter Flexibiliteter i leder Då en robot utsätts för belastning, antingen från den egna tyngden eller från yttre krafter, kommer en viss avböjning att ske på grund av vekheter i konstruktionen, vilket brukar modelleras som flexibiliteter. En vanlig modell, som också används i denna studie, är att roboten modelleras som om flexibiliteterna sitter i lederna. För att kunna beskriva ledflexibiliteter fördubblas antalet generaliserade koordinater och för varje led införs en motorvinkel q mi. Det innebär att varje led beskrivs av dels armens vinkel q ai och dels motorns vinkel q mi. Avvikelsen q ai q mi beskriver då avböjningen i led i, se figur 2.2 där avböjningseffekten överdrivits kraftigt. En industrirobot har i praktiken en viss utväxling mellan angivelsen av motorns läge och den faktiska motorvinkeln som nämns ovan. I vissa formuleringar inkluderas därför detta utväxlingstal i den matematiska modellen. I denna rapport har däremot variabler och parametrar definierats så att utväxlingstalet inte behöver tas in i modellen Matematisk modell Dynamikekvationen som beskriver rörelsen för en robot med flexibla leder och armar kan härledas med Lagrange ekvation på motsvarande sätt som härledningen av den grundläggande modellen i 2..3 men den härledningen utelämnas här. För att förenkla beskrivningen av dynamikekvationen kommer en robot med två leder

21 2.2 Utökad robotmodell 9 att presenteras i detta kapitel, och följande definitioner införs u = u m u m2 q = q a q a2 q m q m2 = [ qa q m ] (2.7) där u är externa krafter som verkar på systemet. Det ger då följande momentjämviktsekvation som beskriver systemets dynamik, u = M(q a ) q + C(q a, q a ) + G(q a ) + D( q a q m ) + τ s (q a, q m ) + κ( q m ) (2.8) där M(q a ) är systemets massmatris, C(q a, q a ) Coriolis- och centrifugaltermer och G(q a ) gravitationens inverkan på lederna i enlighet med definitionerna för den stela modellen. I utökningen av modellen för att inkludera flexibiliteter och friktion introduceras nu även viskös dämpning D( q a q m ), olinjär fjäderkraft τ s (q a, q m ) samt olinjär friktion κ( q m ). Dessa storheter definieras nedan och de robotspecifika parametrar som ingår i uttrycken finns definierade i bilaga A. Massmatris: M(q a ) = [ ] Ma (q a ) M m = J (q a ) J 2 (q a ) J 2 (q a ) J 22 (q a ) j m j m2 J (q a ) = j + m ξ 2 + j 2 + m 2 (l 2 + ξ 2 2 2l ξ 2 sin(q a2 )) J 2 (q a ) = j 2 + m 2 (ξ 2 2 l ξ 2 sin(q a2 )) J 2 (q a ) = J 2 (q a ) J 22 (q a ) = j 2 + m 2 ξ 2 2 Gravitationens momentinverkan på lederna: G(q a ) = [ g (q a ) g 2 (q a ) ] T g (q a ) = g(m ξ sin(q a ) + m 2 (l sin(q a ) + ξ 2 cos(q a + q a2 ))) g 2 (q a ) = gm 2 ξ 2 cos(q a + q a2 ) Coriolis- och centrifugaltermer: [ Ca (q C(q a, q a ) = a, q a ) ] = m 2 l ξ 2 cos(q a2 )(2 q a q a2 + q 2 a2) m 2 l ξ 2 cos(q a2 ) q 2 a

22 Grundläggande robotik Olinjär fjäderkraft: τ s (q) = τ s ( q ) τ s2 ( q 2 ) τ s ( q ) τ s2 ( q 2 ) q i = q ai q mi, i = τ si = k i q i + k i3 qi 3, q i ψ i τ si = sign( q i )(K i + K i ( q i ψ i )), k i = ki low k i3 = (k high i ki low )/(3ψi 2 ) K i = k i ψ i + k i3 ψi 3 K i = k high i q i > ψ i Dämpningsmatris som beskriver viskös dämpning: [ ] d D = d 2 Modell av olinjära friktionsmoment som beror av motorernas vinkelhastigheter: κ( q) = [ κ ( q) κ 2 ( q) ] T κ i ( q) = f di q mi + f ci (µ ki + ( µ ki ) cosh (β i q mi )) tanh(α i q mi ), i =,..., Tillståndsmodell Utgående från definitionerna i och (2.8) kan nu en tillståndsmodell formuleras med insignal u och tillstånd x x q a u = u, x = x 2 x 3 = q ṁ q a x 4 q m samt tillhörande olinjära tillståndsekvation, härledd från (2.8) ẋ = x 3 x 4 M (x )( C(x, x 3 ) G(x ) D(x 3 x 4 ) τ s (x, x 2 ) κ(x 3 )) (2.9) + M (x ) u (2.)

23 Kapitel 3 Accelerometer I detta avsnitt beskrivs hur en accelerometer kan modelleras och implementeras med observatörerna. En genomgång av accelerationsekvationer och koordinatsystem samt kalibreringsmetodik ges även. 3. Accelerometerns koordinatsystem En accelerometer registrerar sina mätningar i ett kroppsfixt koordinatsystem, som benämns {s}, och detta roterar med accelerometern när den rör sig. Detta koordinatsystem måste givetvis relateras till det robotfixa baskoordinatsystemet {w} eftersom det är i detta som beräkningar sker. I figur 3. illustreras baskoordinatsystemet samt sensorkoordinatsystemet för två olika konfigurationer för en tvåaxlig robot som rör sig i xz-planet och har en accelerometer monterad på undersidan av andra armen. Positionen för sensorn uttryckt i baskoordinatsystemet, anges av ρ w = T (q a ) och är beroende av robotkonfigurationen. I det första läget, då q a = q a2 = sammanfaller riktningarna för sensorns koordinatsystem med baskoordinatsystemet. I det andra läget är istället roboten i en position med q a >, q a2 > och där är {w} och {s} ej längre parallella. 3.2 Matematisk modell Den acceleration som accelerometern påverkas av kan härledas matematiskt genom att derivera positionsbeskrivningen, ρ w = Γ(q a ), två gånger med avseende på tiden. Med nedanstående definition Γ(q a ) = J(q a ) q a erhålls följande uttryck för accelerationen ρ w = Γ(q a ) = J(q a ) q a ρ w = Γ(q a ) = J(q a ) q a + J(q a ) q a (3.a) (3.b)

24 Sensorkoordinatsystem Sensorkoordinatsystem 2 Accelerometer Z s Z w ρ w X s Sensor Z w q a2 q a Z s X w ρ w Sensor X w X s (a) Grundposition (b) Aktivt läge Figur 3.. Koordinatsystem för robot och sensor. där ρ w och ρ w är sensorns hastighet resp. acceleration angiven i baskoordinatsystemet {w}. Jacobianens tidsderivata blir något komplicerad och anges av J(q a ) = N i= J q ai q ai där N är antalet frihetsgrader. Eftersom sensorn är fastmonterad på en rörlig axel på roboten kommer dess koordinatsystem att röra sig och dess mätning kommer att ge värden uttryckta i dess egna koordinatsystem {s}. För att transformera accelerations- eller kraftvektorer mellan sensorkoordinater {s} och baskoordinater {w} används rotationsmatriser {w} {s} : R s w(q a ) och {s} {w} : R w s (q a ). Eftersom en accelerometer mäter faktisk kraftpåverkan kommer även gravitationen att mätas upp, och följande mätekvation erhålls således ρ s = R s w(q a ) ρ w R s w(q a )G w = R s w(j(q a ) q a + J(q a, q a ) q a ) R s w(q a )G w (3.2) där gravitationen modelleras som G w = [,, g ] T med g som gravitationskonstanten, och från detta samband kan armaccelerationerna härledas q a = (R s w(q a )J) (q a )( ρ s R s w(q a )( J(q a, q a ) q a G w )). (3.3) Denna uppskattning av armaccelerationerna, eller den faktiska accelerationen ρ w kan sedan användas i en observatör för att förbättra skattningar av armvinklar och verktygsposition. 3.3 Modellering av störningar Som nämndes ovan mäter en accelerometer faktisk kraftpåverkan och dessa mätningar kommer givetvis att innehålla vissa störningar. Dessa störningar brukar modelleras som dels en långsamt varierande drift samt en normalfördelad störning

25 3.4 Kalibrering 3 med låg amplitud. I enlighet med notationen i [9] benämns den långsamma driften δ s och det normalfördelade mätbruset e s. Den faktiska mätningen som accelerometern registrerar definieras av ρ M s och följande modell beskriver accelerometerns mätning i sensorkoordinatsystemet {s} ρ M s = ρ s + δ s + e s = R s w(q a )( ρ w G w ) + δ s + e s (3.4) Som tidigare är R s w koordinattransformationsmatrisen mellan baskoordinater och sensorkoordinater, ρ w är den faktiska accelerationen för accelerometern och G w anger gravitationskraften. För en mer utförlig beskrivning av accelerometrar och andra tröghetssensorer samt en beskrivning av störningarnas faktiska utseeende för verkliga mätningar, se exempelvis [9]. 3.4 Kalibrering En accelerometer måste i praktiken kalibreras, både för orientering och position efter att den monterats på robot. Detta förfarande kan utföras på flera olika sätt, där en metod presenteras i [9]. För denna studie har en metod tagits fram och kalibreringen utförs genom ett antal fördefinierade robotrörelser utifrån vilka en rotationsmatris och position beräknas. Denna metodik granskas för närvarande för att utröna huruvida en patentansökan kommer att vara aktuell eller inte. Av den anledningen kan inte detta kapitel ge en mer utförlig beskrivning av tillvägagångssättet.

26

27 Kapitel 4 Estimeringsmetoder Då en matematisk modell av ett system används i praktiken behövs ofta olika estimeringsmetoder eftersom verkligheten skiljer sig från modellen. I denna tillämpning handlar det om att estimera verktygsposition samt armvinklar för roboten då mätningar av accelerationen vid TCP samt motorvinklar finns tillgängliga. Det finns ett flertal olika tillvägagångssätt för att hantera detta problem och i denna studie har fokus lagts på Extended Kalman Filter (EKF) samt en deterministisk observatör. EKF beskrevs tidigt i [3] och nyare referenser finns också såsom [4]. Den deterministiska observatör som använts i denna studie har tagits fram av De Luca et al. och beskrivs i [7]. Båda observatörerna använder motorvinkelmätningar samt accelerometrar för att skatta armvinklar och verktygsposition. 4. Matematisk modell Den olinjära kontinuerliga tillståndsmodellen som beskriver robotens dynamik kan formuleras enligt ẋ = f(x, u), x = x x 2 x 3 x 4 = q a q ṁ q a q m (4.) där f är den olinjära funktion som beskriver dynamiken för den tvåaxliga modellen som ges i Utskrivet med endast tillståndsvariablerna erhålls följande tillståndsmodell ẋ = x 3 x 4 M (x )(u C(x, x 3 ) G(x ) D(x 3 x 4 ) τ s (x, x 2 ) κ(x 3 )) 5 (4.2)

28 6 Estimeringsmetoder 4.. Systemmodell och diskretisering I praktiken implementeras ett system på en robot i diskret form varför den kontinuerliga tillståndsekvationen beskriven i (4.) och (4.2) måste diskretiseras. Detta görs enklast med en första ordningens Taylorapproximation för vilken en utförlig härledning ges i exempelvis [22] x k+ x k + T s ẋ k = x k + T s f(x k, u k ) = F (x k, u k ) (4.3) där F (x k, u k ) betecknar den nya systemfunktionen. Ett sådant system kommer givetvis även att innehålla systemstörningar och dessa modelleras som normalfödelade med kovarians Q k, alltså v k N (, Q k ) och följande systemfunktion erhålles således x k+ = F (x k, u k ) + v k (4.4) 4..2 Tillgängliga mätningar De mätningar z som finns tillgängliga är motorvinklar qm M och accelerationen för en punkt på roboten ρ M s uppmätt i sensorns koordinatsystem {s} och mätningarna sker givetvis i diskret tid, styrt av aktuell sensors samplingstid. En mätning vid tidpunkt k betecknas med superindex M för uppmätt storlek enligt [ ] q M z k = mk (4.5) Ett matematiskt samband kan också härledas mellan robotens tillståndsvariabler x, pålagt moment u, och uppmätta storheter z. I realiteten kommer detta samband inte vara exakt och därför modelleras skillnaden genom en mätstörning w k N (, R k ) vid tidpunkt k. Detta samband mellan x k, u k och z k ges av funktionen h(x k, u k ), med uttrycket för ρ M s hämtat från (3.2) [ z k = h(x k, u k ) + w k = ρ M sk x 2k R s w(x k )(J(x k )ẍ k + J(x k )x 3k G w ) ] + w k (4.6) Eftersom armaccelerationerna ẍ k ej ingår som tillståndsvariabel ersätts ẍ k i (4.6) med uttrycket för armaccelerationen som ingår i tillståndsekvationen från (2.) ẍ k = M (x k )(u k C(x k, x 3k ) G(x k ) D(x 3k x 4k ) τ s (x k, x 2k ) κ(x 3k )) (4.7) 4.2 Extended Kalman Filter Inom filterteori är Kalmanfiltret ett välkänt begrepp eftersom det är ett optimalt filter för linjära system med normalfördelade process- och mätstörningar. I fallet med robotar är det istället ett olinjärt dynamiskt system som skall hanteras. I sådana fall används ofta Extended Kalman Filter, vilket är en utökning av Kalmanfiltret för att kunna hantera olinjära system. Principen för EKF är mycket

29 4.2 Extended Kalman Filter 7 enkel; i varje iteration linjäriseras systemet runt aktuell skattning och sedan används en variant av det ordinarie Kalmanfiltret. Detta är en mycket välbeprövad metodik men dock kan kvalitén i skattningarna variera kraftigt beroende på hur väl modellen kan beskrivas med linjäriseringar i varje tidssteg. Mer utförlig information om EKF finns i många filter- och signalteoriböcker där två exempel är [3] och [4]. En översikt ges även i [23] Linjärisering I 4. definierades den matematiska modell som beskriver roboten som ett tidsdiskret olinjärt dynamiskt system och system- och mätfunktionen tecknas enligt x k+ = F (x k, u k ) + v k z k = h(x k, u k ) + w k (4.8a) (4.8b) Dessa funktioner är olinjära och därför måste en linjärisering genomföras vid varje tidssteg för att de ska kunna användas i EKF. Detta görs som en första ordningens Taylorapproximation kring det senast skattade tillståndet, ˆx k k respektive ˆx k k. F (x k, u k ) F (ˆx k k, u k ) + F (x, u k) x h(x k, u k ) h(ˆx k k, u k ) + h(x, u k) x (x k ˆx k k ) x=ˆxk k (x k ˆx k k ) x=ˆxk k (4.9a) (4.9b) För att förenkla uttrycken görs följande definitioner A k = F (x, u k) x x=ˆxk k H k = h(x, u k) x x=ˆxk k (4.a) (4.b) Med dessa variabler kan nu de linjäriserade system- och mätfunktionerna slutligen beskrivas enligt x k+ = F (ˆx k k, u k ) + A k (x k ˆx k k ) + v k z k = h(ˆx k k, u k ) + H k (x k ˆx k k ) + w k (4.a) (4.b) Filteralgoritm EKF bygger på en iterativ algoritm och utan vidare härledning anges nu tids- och mätuppdateringarna vid varje tidssteg. För en mer utförlig beskrivning av algoritmen hänvisas till exempelvis [3] eller [4]. Mätuppdatering ˆx k k = ˆx k k + K k (z k h(ˆx k k, u k )) P k k = P k k K k H k P k k

30 8 Estimeringsmetoder Tidsuppdatering ˆx k+ k = F (ˆx k k, u k ) P k+ k = A k P k k A T k + Q k K k = P k k H T k (H kp k k H T k + R k) Initialvärden ˆx = x P = P där P och x är valda initialvärden för observatören. Initialvärden för tillstånden kan exempelvis väljas till x = [ q M m() q M m2() q M m() q M m2() ] T om systemet startar i vila. Initialvärden till P är däremot svårare att välja intuitivt och en skattning kan göras. Det finns givetvis ett flertal praktiska svårigheter med att använda ett EKF där den största svårigheten ligger i att välja kovarianser för system- och mätstörningar på ett lämpligt sätt. Detta är i praktiken svårt eftersom det brus som påverkar mätningar och system sällan är exakt känt och därför måste någon form av approximation göras. Hur detta kan utföras behandlas i Alternativa EKF-formuleringar En risk med utformningen av EKF:ens mätfunktion, som den ser ut i (4.6) är att ett kraftigt modellberoende införs på grund av det komplicerade uttrycket för ẍ k i (4.7). Eftersom ett syfte med att använda accelerometern är att förbättra robustheten gentemot modellfel är detta problematiskt. Det finns fler EKF-varianter som kan undersökas än den som hittills presenterats och i detta avsnitt presenteras ytterligare tre metoder som, utöver den fullständiga EKF-observatören som redan beskrivits, kommer att undersökas i denna studie. Integrerad accelerometer En möjlighet att hantera den komplexa mätfunktionen är att integrera accelerometermätningarna till hastigheter, så att bara kinematiken behövs i mätfunktionen. Alltså ρ M w (k) = k ρ M w (t)dt = k R w s (t) ρ M s (t) + G w dt (4.2) där integreringen i (4.2) är en numerisk integrering av någon sort eftersom mätningarna är diskreta. Olika numeriska metoder för integrering finns beskrivna i exempelvis [8]. Den uppmätta accelerationen ρ M s (t) i (4.2) måste givetvis först transformeras till baskoordinater för att sedan kompenseras för gravitationen, såsom i (3.4), eftersom verktygets faktiska hastighet ska beräknas. Denna integrering kan givetvis införa en del drift över tid, men om banan är sådan att små stopp görs under körningar, exempelvis punktsvetsning, så kommer integreringen aldrig behöva ske under speciellt lång tid eftersom den kan nollställas vid varje stillastående tillfälle. Det kommer säkerligen ändock att kunna uppstå svårigheter

31 4.2 Extended Kalman Filter 9 med drift men då detta framförallt är ett lågfrekvent problem bör det inte påverka skattningen för högre frekvenser (> 3Hz). Om nu ρ M w används som mätning istället för ρ M w kommer den nya mätekvationen att bli betydligt enklare eftersom det, i enlighet med (3.a), bara är jacobianen J(q a ) som kommer in i beräkningarna och mätekvationen blir således [ h(x k, u k ) = ρ w = J(q a ) q a (4.3) ] x 2k J(x k )x 3k (4.4) vilket är en betydligt enklare form än den som presenteras i (4.6). Här uppkommer istället problemet med att integreringen av accelerometern kan introducera stora mätfel, som trots att mätekvationen är enklare, ger stora fel. Det blir således en avvägning mellan hur väl accelerometern kan integreras, och hur stora modellfelen är. Förenklad modell Ett annat tillvägagångssätt för att reducera komplexiteten är att förenkla beskrivningen av olinjäriteterna i systemet. Det kan göras genom att styvheten betraktas som linjär och friktionen elimineras. En förenklad modell skulle då få dynamikekvationen u k = M(x k )ẍ k + C(x k, ẋ k ) + G(x k ) + D(x 3k x 4k ) + K(x k x 2k ) (4.5) där K(x k x 2k ) beskriver den linjära fjäderkraften. Om denna modell används i EKF:en blir system- och mätfunktionen betydligt mindre komplexa men givetvis kan det medföra svårigheter med modellfel. För mätfunktionen i (4.6) där ẍ k måste ersättas med ett uttryck härlett från dynamikekvationen blir nu detta uttryck ẍ k = M (x k )(u k C(x k, x 3k ) G(x k ) D(x 3k x 4k ) K(x k x 2k )). Ett alternativt sätt att använda denna förenklade modell beskrivs även för den deterministiska observatören i 4.3. EKF med bara motormätningar För att undersöka vilken prestanda som kan uppnås med robotens befintliga hårdvara undersöks även ett EKF som enbart använder motormätningarna i mätfunktionen, som då får en mycket enkel form. h(x k ) = x 2k Denna observatör kan då även användas som en referens att jämföra resultaten för de andra accelerometerbaserade observatörerna med. Denna EKF använder den fullständiga dynamikmodellen för systemfunktionen, men kommer att ha en mycket enkel mätfunktion. Denna observatör kan implementeras utan modifiering av robotarna eftersom ingen ny hårdvara behövs. Här kommer istället skattningen av armvinklarna att beräknas enbart utifrån den matematiska modellen av roboten, och inga tröghetssensorer alls. En sådan observatör kan då förväntas vara känsligare för modellfel, jämfört med om tröghetssensorer används.

32 2 Estimeringsmetoder 4.3 Deterministisk observatör I föregående avsnitt beskrevs EKF som är en statistiskt baserad observatör men det finns också observatörer som är deterministiska, och alltså inte baseras på en statistisk modell. Det finns ett flertal olika varianter av deterministiska observatörer som kan användas för denna tillämpning men en observatör som beskrivs av De Luca et al. i [7] har valts för utvärdering. Denna observatör uppvisar ett innovativt sätt att använda en accelerometer för att skatta verkygspositionen och dess struktur är mycket enkel, varför den är lätt att implementera och inte så beräkningstung som EKF:erna. Det var framförallt strukturen och skillnaden i beräkningskomplexitet som gjorde denna observatör intressant att jämföra med EKF:erna, särskilt då realtidsimplementeringar kan bli aktuella. I detta avsnitt följer en grundlig genomgång av den deterministiska observatören Matematisk modell Denna observatör är uppbyggd för en flexibel manipulator med N armar sammanlänkade av N visko-elastiska leder, liksom den som beskrivs i och som ges en utförligare beskrivning i [7]. Varje led modelleras dock som friktionsfri med en linjär fjäder och viskös dämpare, och är således betydligt förenklad. För att underlätta notationen i observatören införs parametriseringar av u, M, K, D, C, samt G från enligt följande [ ] [ ] [ ] qa q qa q q =, = m q m q q a q m K = D = [ K K 2 ] [ D D 2 ] = = k k 2 k k 2 d d 2 d d 2 [ ] Ca (q C(q a, q a ) = a, q a ), G(q a ) = [ ], u = u m, M(q a) = [ Ga (q a ) ] [ ] Ma (q a ) M m (4.6) (4.7) Dynamikekvationen som beskrivs i förenklas och linjär fjäderkraft, med fjäderkonstanter k i, och noll friktion införs. Detta ger en betydligt enklare dynamikekvation jämfört med (2.8). u = M(q a ) q + C(q a, q a ) + G(q a ) + D q + K q (4.8) En uppdelning av ekvationen ger nu följande två dynamikekvationer, med nydefinierade variabler enligt (4.6). = M a (q a ) q a + C a (q a, q a ) + G a (q a ) + K q + D q (4.9a) u m = M m q m + K 2 q + D 2 q (4.9b)

33 4.3 Deterministisk observatör 2 Samma tillståndsmodell som i (4.) används, med följande nya beteckningar där u m betraktas som insignal och mätsignaler är q M m och ρ M s. Mätsignalen ρ M s kommer från en eller flera accelerometrar monterade på roboten, och den uppmätta accelerationen kan beräknas, i enlighet med 3, med hjälp av följande ekvation ρ s = R s w(q a )(J(q a ) q a + J(q a, q a ) q a + G w ) som, då R s w(q a )J(q a ) ej är singulär, ger en approximation av q a som benämns q M a. q M a = (R s w(q a )J(q a )) (q)( ρ M s R s w(q a )( J(q a, q a ) q a + G w )) (4.2) Om R s wj skulle bli singulär, alltså att (R s w(q a )J(q a )) inte existerar, innebär det praktiskt att armaccelerationerna inte kan särskiljas. Detta skulle exempelvis kunna uppstå om robotarmen är fullt utsträckt. Om roboten rör sig från ett sådant läge går det inte att beräkna accelerationen för vardera axel eftersom det inte går att avgöra vilken axel som rör sig från enbart accelerometermätningarna, och (4.2) kan således inte beräknas. För att den vanliga inversen ska kunna användas i (R s w(q a )J(q a )) förutsätter det att endast en accelerometer används. Om flera accelerometrar skulle användas måste istället dessa mätningar vägas samman och ett lämpligt sätt att göra det vore via exempelvis Moore-Penrose pseudoinversen som i detta fallet tar fram den, i minsta-kvadratmening, närmaste skattningen av de kartesiska accelerationerna, se exempelvis [2]. Utgående från (4.9) kan nu derivatan av varje tillstånd härledas ẋ = x 3 ẋ 2 = x 4 ẋ 3 = Ma (x )(C a (x, x 3 ) + G a (x ) + K (x x 2 ) + D (x 3 x 4 )) ẋ 4 = Mm ( K 2 (x x 2 ) D 2 (x 3 x 4 )) + Mm u m där den enda olinjära delen återfinns i ẋ 3 = q a. Därför införs q a = ẋ 3 = f olinj för att underlätta notationen. f olinj (x) = M a (x )(C a (x, x 3 ) + G a (x ) + K (x x 2 ) + D (x 3 x 4 )) Tillståndsekvationen kan nu skrivas om på följande sätt ẋ = M x 3 x 4 m ( K 2 (x x 2 ) D 2 (x 3 x 4 )) + f olinj (x) M m u m (4.2) Det må tyckas att tillståndsmodellen nu bara har möblerats om, men nu är det tydligare hur den kan skrivas om på en linjär form. Det är alltså endast ẋ 3 som är olinjär, svarande mot q a. Om därför armaccelerationerna q a kan estimeras från en annan källa kan systemet linjäriseras, vilket beskrivs i nästa avsnitt

34 22 Estimeringsmetoder Linjärisering Eftersom det endast är funktionen för ẋ 3 = q a i (4.2) som är olinjär, kan istället en uppskattning av denna tas från accelerometermätningen. Därför sätts f olinj (x) = och uppskattningen av q a tas istället in från accelerometern, q a M. Därmed kan tillståndsekvationen skrivas på följande form ẋ = M x 3 x 4 m ( K 2 (x x 2 ) D 2 (x 3 x 4 )) + q M a M m u m (4.22) Med en del algebra kan systemet skrivas om med matriser och för detta behövs följande definitioner av systemmatriser A R 4N 4N och G R 4N 3N I A = G = Mm K 2 I M m M m K 2 M m D 2 M m D 2 och med dessa definitioner skrivs (4.22) om till ẋ = A x + G [ q M a um ] (4.23) Detta linjära dynamiska system använder således samma matematiska modell för roboten som den förenklade EKF:en i (4.5) men den största skillnaden härrör från att accelerometermätningen tas in direkt i beräkningen för armaccelerationerna, istället för att, som i EKF:erna, linjärisera dynamiken. I denna observatör räknas alltså armaccelerationerna som insignal till systemet medan EKF:erna gör denna beräkning från accelerometermätningarna via mätfunktionen. För detta system kan nu en observatör definieras som stabiliseras med förstärkningen L på felet i motorvinklar och det skattade tillståndet benämns ξ ξ = A ξ + G [ q M a um ] + L(q M m C ξ) (4.24) där förstärkningen L R 4N N och matrisen C R N 4N definieras enligt L = [ L L 2 L 3 L 4 ] T, Li = diag(l i,,..., L i,n ) C = [ I ] Uttrycket i (4.24) ger den deterministiska observatören uttryckt på standardform, som också är mycket enkel att implementera i exempelvis Matlab. För att tolka

35 4.3 Deterministisk observatör 23 observatörens dynamik rent intuitivt kan sägas att den använder accelerometermätningar för att uppdatera armhastigheterna, och sedan korrigerar med hjälp av de uppmätta motorvinklarna. I teorin bör detta kunna fånga upp det dynamiska beteendet via accelerometern och läget korrigeras med motorvinklarna som ger en relativt god bild av det lågfrekventa beteendet Polplacering Det enda verktyget för trimning som existerar för den deterministiska observatören är förstärkningarna i matrisen L och dessa justeras enklast genom att justera polplaceringen. Eftersom observatören är linjär är det tillräckligt att analysera egenvärdena för A LC och sätta förstärkningarna i L så att önskade poler erhålls. En noggrannare beskrivning av tillvägagångssättet ges i Diskret implementering I praktiken implementeras observatören tidsdiskret, och ovanstående modell måste således diskretiseras. Eftersom observatören är ett linjärt dynamiskt system kan en diskretisering enkelt göras med grundläggande verktyg i exempelvis Matlab.

36

37 Kapitel 5 Trimning av observatörer I detta kapitel ges en beskrivning av hur trimning av kovarianser och polplaceringar genomfördes. I kapitel 4 beskrivs observatörerna och i dessa beskrivningar nämns kovarianser och polplaceringar som trimningsbara delar i observatörerna. Denna trimning är mycket svår att göra för hand eftersom det är så många parametrar inblandade, varför en automatisk rutin är eftersträvansvärd. Detta kapitel beskriver just detta, hur trimningen kan genomföras automatiskt med en dator. Detta problem är givetvis inte enkelt och för att reducera komplexiteten i beskrivningarna begränsas dessa till att omfatta en robot med två frihetsgrader som rör sig i xz-planet. Trimningsmetodiken kan utan problem även användas för godtyckligt antal frihetsgrader, men beskrivningen blir då svåröverskådlig. 5. Trimning av kovarianser för EKF Till EKF:en krävs en modell av bruset som påverkar det dynamiska systemet, och då särskilt kovarianser för störningarna. En noggrann beskrivning av dessa är viktig för att EKF:en ska ge en bra skattning av armvinklarna, varför trimningen har en central roll. För det dynamiska systemet x k+ = F (x k, u k ) + v k z k = h(x k, u k ) + w k (5.a) (5.b) behöver skattningar beräknas för kovarianserna av v k och w k. Felet i systemekvationen, v k, uppkommer på grund av diskretiserings- och linjäriseringsfel, externa omodellerade störningar samt avvikelser mellan matematisk modell och verklighet. Dessa fel är svåra att undvika och de påverkas dels av vilken samplingstid som används, hur exakt modellen är, samt den specifika uppgift som roboten arbetar med. För mätekvationen kan felen härledas från rent mätbrus i accelerometern och motormätningarna. Det största mätfelet kan dock härledas från mätfunktionen för accelerometern om modellen eller skattningarna är felaktiga. Dessa modellfel är också svåra att förutsäga strukturen för och på vilket sätt de får genomslag i mätekvationen, särskilt som det skiljer sig för olika konfigurationer och banor. För 25

38 26 Trimning av observatörer att ändå kunna hantera problemet utförs en skattning av kovarianserna i Matlab och detta används sedan i en optimeringsrutin för att ta fram en lämplig kovarians till EKF:en. 5.. Kovariansskattning För att i ett första skede erhålla en uppskattning av kovarianser utförs en rutin som beräknar felet i varje iteration i simuleringar där all data finns tillgänglig. Felet i varje sampel k =... N T beräknas för alla körningar i =... N R som svarar mot olika banor och störningar. ˆv k i = x i k+ F (x i k, u i k) ŵk i = zk i h(x i k, u i k) ˆv i = v i. v i N T ŵ i = Anledningen till att flera banor används är för att de skattade kovarianserna ska vara generella i största möjliga mån. Från dessa loggade fel skattas sedan kovarianserna med den ordinarie väntevärdesriktiga samplingskovariansen, som bland annat beskrivs i [8]. w i. w i N T N R N T v = ˆv i N R N T k w = N T N R N T N R R = Ŵ = i= k= N R N T N R i= k= N R N T N R i= k= ŵ i k N T (ˆv k i v)(ˆv k i v) T (5.2) N T (ŵk i w)(ŵk i w) T (5.3) i= k= Dessa skattningar ger relativt bra resultat för simuleringarna men kan förstås inte användas vid riktiga körningar på robot eftersom all data som används ovan då inte är mätbar. Av den anledningen kan denna skattning användas som initial kovarians för optimeringsrutinerna som beskrivs i nästa avsnitt, vilket också görs för de simulerade körningarna för att erhålla ännu bättre resultat Formulering av optimeringsproblem För att förenkla formuleringen begränsas beskrivningen till en robot med två leder och därmed blir dimensionerna för kovarianserna R R 8 8 och Q R 4 4. Skattningarna av kovarianserna R och W i (5.2) resp. (5.3) ger ett första riktmärke, men det har visat sig att EKF:en ej uppnår bästa resultat med dessa skattningar. Av den anledningen introduceras en optimeringsrutin med Ŵ och R som utgångspunkter. Först måste dock en målfunktion definieras och för att minimera antalet variabler förenklas kovariansmatriserna till diagonalmatriser. ( ) ) R = diag R W = diag (Ŵ

39 5. Trimning av kovarianser för EKF 27 Ytterligare ett antagande förutsätter att det inbördes förhållandet mellan varianserna för motorer, accelerometrar samt vinkelpositionsuppdateringar och vinkelhastighetsuppdateringar antas vara såsom skattningarna i (5.2) resp. (5.3) anger. För att nu konstruera och utvärdera nya kovariansmatriser baserade på de skattade definieras optimeringsvariabler Λ = [ ] λ λ 2 λ 3 λ 4 λ 5. För given Λ definieras kovarianser enligt nedan. R λ = W λ = [ W W2 R R2 R3 R4 ] = = [ λ5 I 2 2 I 2 2 λ I 2 2 λ 2 I 2 2 λ 3 I 2 2 λ 4 I 2 2 R (5.4) ] W (5.5) Anledningen till att fem variabler, istället för sex, används är för att den absoluta skalningen för kovarianserna inte har någon betydelse, utan det är den inbördes relationen mellan dem som har inverkan på skattningen. För varje variabeluppsättning Λ definieras alltså kovarianser R λ och W λ som sedan används i EKF:en och resultatet utvärderas med en målfunktion. En viktig del att notera i detta läge är att skattningen av verktygspositionen inte kommer att uppnå en absolut noggrannhet på grund av modellfel. Ett lågfrekvent, eller statiskt, fel kommer att uppkomma som är mycket svårt att undgå och det är heller inte det viktigaste. Det är framförallt svängningar med en högre frekvens som är av intresse att skatta och en målfunktion för att mäta prestanda formuleras därför för ett begränsat frekvensområde. Därför filtreras skattningarna innan de förs in i målfunktionen. Ett lämpligt filter att använda är ett bandpassfilter med gränsfrekvenser 3 Hz resp. 3 Hz. Därav införs följande notation X = BP 3 3 (ˆx) Ẑ = BP 3 3 (ẑ) X = BP 3 3 (x) Z = BP 3 3 (z) och med dessa filtrerade positioner och skattningar formuleras nu följande målfunktion N fobj( x X) p = ( X p X ) p 2 p= N p f obj(ẑ) z = ) ( Z p Ẑp 2 p= ( ) ( ) ) f obj X, Ẑ = fobj x X + fobj (Ẑ z (5.6) (5.7) (5.8) och denna målfunktion beräknar 2-normen av den filtrerade avvikelsen i x- och z-led, för ett antal intressanta områden i en bana. Antalet olika områden som

40 28 Trimning av observatörer beräkningarna görs över definieras av N p medan X p och Z p anger BP-filtrerade värden av den faktiska rörelsen över område p och X p resp. Ẑp anger skattningen med kovarianser R λ och W λ. Antalet sampel för område p anges av N p k. Anledningen till att målfunktionen definieras över ett antal utvalda områden av banan och BP-filtreringen görs, är för att det vid verkliga skattningar uppkom ett relativt stort statiskt fel som beror av modellfel, medan den dynamiska skattningen i många fall var god. Målfunktionen kommer således att fånga in det dynamiska felet och inte, som annars skulle vara fallet, domineras av det statiska felet. Detta kan formuleras som ett optimeringsproblem Minimera f obj ( X, Ẑ) Under bivillkoren λ j > j =... 5 R λ = λ I 2 2 λ 2 I 2 2 λ 3 I 2 2 λ 4 I 2 2 R [ ] λ5 I W λ = 2 2 W ( I 2 2 [ˆx, ẑ] = EKF Rλ, W ) λ X = BP 3 3 (ˆx) Ẑ = BP 3 3 (ẑ) X p X Ẑ p Ẑ ( ) I detta uttryck betecknar [ˆx, ẑ] = EKF R, W att en skattning av xz-banan utförs med EKF:en för kovarianser R och W, och dessa skattningar filtreras sedan med ett bandpassfilter. Detta ( är ) inte ett konvext optimeringsproblem, på grund av bivillkoret [ˆx, ẑ] = EKF R, W och därför kan ett globalt optimum ej garanteras, se exempelvis [5]. För att hitta en bra trimning används ComplexRF som är en heurestisk optimeringsalgoritm som beskrivs i [4]. Denna metod valdes för att den är väldigt enkel att implementera och anpassa för detta problem, då den bara använder målfunktionen i optimeringsalgoritmen. Detta är en stor fördel eftersom optimeringsproblemet är mycket komplext. Gradientbaserade optimeringsmetoder är annars mycket vanliga men dessa är svårare att implementera och kräver analytisk eller numerisk gradient för målfunktionen. När det rör sig om ett icke-konvext optimeringsproblem finns det ändock inga garantier för att en sådan metod skulle fungera bättre än exempelvis ComplexRF. 5.2 Trimning av polplacering för deterministisk observatör Till den deterministiska observatören måste åtta poler placeras och detta i sig är en svår uppgift eftersom det är svårt att tolka betydelsen av olika polers placering

41 5.2 Trimning av polplacering för deterministisk observatör 29 i ett så komplicerat system. Eftersom det också är så många poler är det svårt att för hand trimma dessa. Av den anledningen används samma optimeringsalgoritm som för EKF:en, nämligen ComplexRF. Med den algoritmen utvärderas ett antal polkonfigurationer och dessa jämförs enligt samma kostnadsfunktion som i (5.8) och nu används en optimeringsvariabel för dämpingar ζ och en för egenfrekvenser ω för varje polpar. Optimeringsproblemet formuleras analogt med 5..2 där skattningarna också filtrerats. Minimera f obj ( X, Ẑ) Under bivillkoren ω j >, ( ζ j > j = )... 4 s + j = ω j ζ j + i ζj 2 ( ) s j = ω j ζ j i ζj 2 S j = {s + j, s j } j =... 4 [ˆx, ẑ] = DL (S) X = BP 3 3 (ˆx) Ẑ = BP 3 3 (ẑ) X p X Ẑ p Ẑ där DL (S) betecknar att den deterministiska observatören, förkortat DL, ger en skattning av x och z beräknad med polplaceringar S. Den polkonfiguration som undersöks och får lägst kostnad väljs sedan som suboptimal polplacering för observatören. Eftersom ComplexRF är en heuristisk optimeringsalgoritm kan en optimal lösning aldrig förutsättas. Om däremot algoritmen körs tillräckligt länge erhålls förhoppningsvis en bra suboptimal polplacering. För en beskrivning av algoritmen, se exempelvis [4].

42

43 Kapitel 6 Simuleringar I detta avsnitt ges en genomgång av olika simuleringar som utförts. En kort diskussion kring resultaten ges även. Eftersom det visade sig att den simulerade modellen gav avvikande resultat från de verkliga körningarna på robot kommer den huvudsakliga genomgången av de utvärderade observatörerna ges i avsnittet för de experimentella resultaten, se Simulering tvåaxlig robot Simuleringar har genomförts med olika observatörer för den tvåaxliga flexibla modellen redogjord för av Moberg et al. i [7], som också finns beskriven i 2.2. De observatörer som undersökts är dels den deterministiska observatören av De Luca et al. i [7], samt de olika EKF-varianterna som beskrivs i 4. En accelerometer har simulerats, som om den vore placerad vid verktygspositionen, TCP. Robotmodellen styrs av ett enkelt reglersystem som justerats för att erhålla svängningar liknande de som kan uppstå för en verklig körning. Indata till observatörerna är motorvinklar, motormoment samt accelerometermätningar, med pålagt normalfördelat mätbrus. Roboten simuleras även med störningar på motorer och motormoment samt avvikelser i modellparametrarna för friktion, styvhet och dämpning. Robotsimuleringsmodellen beskrivs nedan. Modell Tvåaxlig flexibel, med olinjär styvhet och olinjär friktion. Accelerometer Simulerad för att mäta acceleration vid TCP, inklusive gravitationen. Koordinatsystem {s} roterar med TCP. Indata för estimering positioner och -moment samt accelerometermätningar. Observatörstrimning EKF:en och den deterministiska observatören trimmades enligt 5. Accelerometerstörningar Normalfördelat brus med största värde ca.3m/s 2 och konstant offset ±.3m/s 2. 3

44 32 Simuleringar Accelerometerkalibrering Kalibrerings- och positioneringsfel Arm : ±3mm, Arm 2: ±4mm, orienteringsfel ±2 runt y-axeln. mätstörningar Normalfördelat brus på motorpositionsmätning med amplitud 6 rad/s. Modellfel Modellparametrarna för friktion, olinjär styvhet samt dämpning simulerades med fel på ±%. Kraftstörningar Chirpstörningar på motormomentet. Dessa störningar och modellfel används i alla körningar som presenteras i detta kapitel och de ska ge en realistisk bild av de fel som kan uppstå eftersom alla robotparametrar i verkligheten inte är exakt kända. De fel som anges med ± ovan anger att dessa parametrar har justerats med en faktor motsvarande den angivna storleken och med randomiserat tecken. 6.2 Utvärderade observatörer De olika observatörsmodeller som användes i simuleringarna beskrivs nedan. EKF Full - Fullständig matematisk modell, inklusive olinjär friktion och styvhet. EKF Förenklad - Förenklad matematisk modell. Olinjär styvhet ersatt av linjär styvhet och friktionen negligeras. EKF Integrerad - En fullständig matematisk modell, men accelerometern används för att skatta hastigheten för TCP för att förenkla mätekvationen och reducera inverkan av modellfel. EKF - Fullständig matematisk modell, men endast mätningar av motorernas vinklar används vid estimeringen. DL - Den deterministiska observatören. En utförlig beskrivning av de matematiska modellerna som ligger bakom de olika observatörsvarianterna finns i Simuleringsresultat I figurerna i detta kapitel används följande beteckningar - Referensbanan, alltså så som verktyget faktiskt rörde sig. Eftersom detta är en simulering är denna position exakt känd. - Den av respektive observatör skattade banan. - Beräknad verktygsposition om armvinklarna antas vara samma som motorvinklarna. Alla avböjningseffekter bortses alltså ifrån.

45 6.3 Simuleringsresultat z led [mm] 2 2 z led [mm] x led [mm] (a) EKF Full x led [mm] (b) EKF Förenklad 3 3 z led [mm] 2 2 z led [mm] x led [mm] (c) EKF x led [mm] (d) EKF Integrerad Figur 6.. Skattningar för de olika EKF-varianterna med kovarianser trimmade specifikt för denna rörelse. Simuleringsmodellen som beskrivs i 6. används för att utvärdera observatörerna och en trimning i enlighet med 5 har utförts för den givna körningen för varje observatör. Givetvis kan mer arbete utföras för att ytterligare förbättra trimningen, utöver de algoritmer som använts, men detta arbete lämnas för vidare studier. De trimningar som erhållits antas vara av inbördes jämförbar kvalité och därmed tillräckliga för att dra slutsatser om de olika observatörernas egenskaper Resultat EKF-observatörer Den första bana som observatörerna testades för var en rörelse som skedde rakt i x-led med en del svängningar i z-led. Kovarianser för EKF:erna togs fram enligt optimeringsrutinen beskriven i 5 med denna körning i målfunktionen. Som framkommer från figur 6. är skattningen relativt dålig för alla observatörerna med EKF som den bästa och EKF Integrerad som den sämsta. Om skattningen istället filtreras med ett bandpassfilter med gränsfrekvenser 3-3Hz framträder en annan bild av observatörernas skattningar. Som nu tydligt syns i figur 6.2 är skattningarna betydligt mycket bättre för dessa frekvenser och det är bara EKF som fortfarande ger en bristfällig precision i skattningen. De andra EKFvarianterna ger mycket hög precision i skattningarna och ingen större skillnad kan

46 34 Simuleringar.5 Banföljning i x led.5 Banföljning i x led Position [mm].5 Position [mm] Sampel 2 Hz (a) EKF Full Sampel 2 Hz (b) EKF Förenklad.5 Banföljning i x led.5 Banföljning i x led Position [mm].5 Position [mm] Sampel 2 Hz (c) EKF Sampel 2 Hz (d) EKF Integrerad Figur 6.2. Filtrerade skattningar för de olika EKF-varianterna med kovarianser trimmade specifikt för denna rörelse.

47 6.3 Simuleringsresultat 35 2 Banföljning i z led med kraftstörning 2 Banföljning i z led med kraftstörning Position [mm] Position [mm] Sampel 2 Hz (a) EKF Full Sampel 2 Hz (b) EKF Förenklad 2 Banföljning i z led med kraftstörning 2 Banföljning i z led med kraftstörning Position [mm] Position [mm] Sampel 2 Hz (c) EKF Sampel 2 Hz (d) EKF Integrerad Figur 6.3. BP-filtrerade skattningar i z-led för de olika EKF-varianterna då roboten står stilla och sedan påverkas av en kraftig störning under.2s. Skattningen som visas är från tidpunkten precis innan kraftpåverkan och trimningen som används har tagits fram specifikt för körningen i figur 6.. urskiljas. För att vidare studera observatörernas egenskaper presenteras skattningen vid stora kraftstörningar i nästa avsnitt. Kraftstörningar För att även undersöka hur observatörerna presterar vid kraftiga svängningar läggs en kraft på 5N till vid TCP under.2s med start efter 2s. Resultaten från detta presenteras i figur 6.3 där rörelsen från precis innan kraftpåverkan visas. För detta fall presenteras endast den filtrerade skattningen eftersom det framförallt är de snabba förloppen som är av intresse i detta fall. Som framgår i figur 6.3 uppstår nu stor skillnad även mellan EKF-varianterna som använder en accelerometer. Här ser vi också hur bra skattningen blir då trimningen används för en annan körning, men dock i samma arbetsområde. EKF Integrerad tycks till en början ge en mycket god skattning men driver sedan iväg kraftigt. Detta kan bero på att fel i accelerometern ackumuleras vid integreringen och därmed förvränger skattningen kraftigt. EKF följer nu inte alls med svängningen speciellt bra och detta bör bero på att motorvinkelmätningarna inte fångar upp en så kraftig och snabb störning och observatören hinner då inte med att anpassa skattningen för förloppet.

48 36 Simuleringar 25 6 z led [mm] z led [mm] Sampel 2Hz (a) EKF Full Sampel 2Hz (b) EKF Förenklad 4 8 z led [mm] 3 2 z led [mm] Sampel 2Hz (c) EKF Sampel 2Hz (d) EKF Integrerad Figur 6.4. Skattningar för rörelse i annat arbetsområde för de olika EKF-varianterna med kovarianser framtagna för körningen i figur 6.. EKF Full ger istället en mycket bra skattning över hela banan. Skattning i annat arbetsområde I de föregående skattningarna var rörelserna i samma arbetsområde men under olika förhållanden och för att studera hur generell trimningen är med avseende på arbetsområde utförs därför en skattning med annan startpunkt och rörelse. Motsvarande storleksordning på störningar som beskrivs i 6. används. Som framkommer från figur 6.4 är skattningen relativt dålig för alla observatörerna med EKF som den bästa och EKF Integrerad som den sämsta. Om skattningen istället filtreras med ett bandpassfilter med gränsfrekvenser 3-3Hz framträder en annan bild av observatörernas skattningar, se figur 6.5. Trots att rörelsen i figur 6.5 nu sker i ett helt annat arbetsområde än trimningen tagits fram för blir den filtrerade skattningen mycket bra för alla EKF-varianter förutom EKF integrerad. Detta visar på att alla EKF-observatörerna förutom EKF Integrerad har en relativt robust trimning gentemot arbetsområde för de filtrerade skattningarna.

49 6.3 Simuleringsresultat 37 6 Banföljning i x led 6 Banföljning i x led Position [mm] Position [mm] Sampel 2 Hz (a) EKF Full Sampel 2 Hz (b) EKF Förenklad 6 Banföljning i x led Banföljning i x led Position [mm] Position [mm] Sampel 2 Hz (c) EKF Sampel 2 Hz (d) EKF Integrerad Figur 6.5. Filtrerade skattningar för de olika EKF-varianterna med kovarianser trimmade specifikt för denna rörelse.

50 38 Simuleringar 3 3 z led [mm] 2 2 z led [mm] x led [mm] (a) EKF Full x led [mm] (b) EKF Förenklad 3 3 z led [mm] 2 2 z led [mm] x led [mm] (c) EKF x led [mm] (d) EKF Integrerad Figur 6.6. Skattningar för samma rörelse som i figur 6. men kovarianserna har justerats med ± 3-5%. Robusthet för kovarianserna För samma körning som visas i figur 6. görs nu skattningar för alla EKF-varianterna då kovariansernas element störts med ±3 5%. Detta gjordes genom att slumpgenerera diagonalmatriser med element fördelade med värden mellan antingen.3.5 eller.5.7 och sedan multiplicerades dessa matriser med kovariansmatriserna för varje EKF. De diagonalelement som genererades blev [ ] [ ] och resultaten visas i figur 6.6 och 6.7 Något förvånande framkommer det nu från figur 6.6 och 6.7 att skattningen i princip inte försämras alls trots att kovarianserna rubbats relativt mycket. Detta är positiva resultat som visar på att stor robusthet gentemot varians i kovarianstrimningen finns Resultat deterministisk observatör Motsvarande simuleringskörningar som EKF:erna utvärderades för i figurer i 6.3. har också testats för den deterministiska observatören och resultaten presenteras i figur 6.8. För den deterministiska observatören erhålls resultat

51 6.3 Simuleringsresultat 39.5 Banföljning i x led.5 Banföljning i x led Position [mm].5 Position [mm] Sampel 2 Hz (a) EKF Full Sampel 2 Hz (b) EKF Förenklad.5 Banföljning i x led.5 Banföljning i x led Position [mm].5 Position [mm] Sampel 2 Hz (c) EKF Sampel 2 Hz (d) EKF Integrerad Figur 6.7. Filtrerade skattningar för samma rörelse som i figur 6. men kovarianserna har justerats med ± 3-5%.

52 4 Simuleringar Position z led [mm] Banföljning i xz led Position x led [mm] Banföljning i x led Position x led [mm] (a) DL ordinarie körning Sampel 2 Hz (b) DL ordinarie körning filtrerad 2 Banföljning i x led Banföljning i x led Position x led [mm] 2 Position x led [mm] Sampel 2Hz (c) Körning i annat arbetsområde Sampel 2 Hz (d) Filtrerad körning i annat arbetsområde Figur 6.8. Skattningar med DL för en bana som polerna trimmats för samt en rörelse i ett annat arbetsområde.

53 6.3 Simuleringsresultat 4 Position z led [mm] 2 Banföljning i z led med kraftstörning Sampel 2 Hz (a) DL körning med kraftstörning Position z led [mm] Banföljning i xz led Position x led [mm] Banföljning i x led Position x led [mm] (b) Körning med feljusterade poler Sampel 2 Hz (c) Filtrerad körning med feljusterade poler Figur 6.9. Skattningar med DL för en bana som påverkas av en störkraft i z-led på 5N under.2s samt samma körning som i figur 6.8(a) och 6.8(b) men med polerna feljusterade med ±3 5%. Polerna har trimmats specifikt för körningen i figur 6.8(a). från figur 6.8 som visar på att DL ej lyckas skatta rörelserna till eftersträvansvärd precision. Detta kan bero på att trimmningen inte varit tillräckligt bra. För att undersöka hur väl DL-observatören hanterar feljusterade polplaceringar samt kraftstörningar gjordes tester för detta och resultaten presenteras i figur 6.9. För att omjustera polplaceringarna slumpgenererades skalfaktorer så att varje pol lades om ±3 5% för den imaginära och den reella delen och hänsyn togs så att polparen skulle fortsätta vara konjugerade. De skalfaktorer som genererades blev [ ] [ ] I figur 6.9 syns det tydligt att skattningen inte lyckas följa med och ge väl överensstämmande estimeringar av vertygspositionen. En del brus slår även igenom i

54 42 Simuleringar CDF för prediktionsfelet CDF för prediktionsfelet Kumulativ sannolikhet Mätt Est. Kumulativ sannolikhet Mätt Est Storlek på fel [rad/s] x 3 (a) Prediktionsfel q a Storlek på fel [rad/s] x 3 (b) Prediktionsfel q a2 Figur 6.. Uppmätt CDF för prediktionsfelen i armvinkelhastigheterna och normalfördelad CDF baserad på prediktionsfelens sampelvarians. Den fullständiga matematiska modellen används. skattningen. 6.4 Analys av modellfel I detta avsnitt ges en kort diskussion kring vilka fel som uppstår i modellen på grund av parameterfelen som används. För att undersöka detta har en robotbana simulerats och vid varje iteration har de faktiska tillstånden använts i funktionen för tillståndsuppdateringen, nedan, och den därigenom beräknade prediktionen, ˇx k+ har sedan jämförts med de riktiga tillstånden i steget efter. Denna jämförelse ger således felet i tillståndsmodellen och då prediktionen, vilket är vad variabeln v k skall modellera. I EKF:erna, såsom i de flesta tillämpningarna, används antagandet för störningsmodellen att det är normalfördelade stokastiska variabler. Detta antagande är ofta en relativt bra beskrivning av verkligheten men självfallet återspeglas inte hela sanningen. I figur 6. ges det uppmätta felets CDF (kumulativ fördelningsfunktion) samt motsvarande CDF för en normalfördelning med samma varians. Medelvärdet är bortdraget från felet för att noll i väntevärde skall användas. Figuren visar nu tydligt att felet inte har en rent normalfördelad karaktär och detta grundläggande antagande stämmer således inte fullt ut. CDF:en är genererad från 66 sampel och bör ge en god bild av den faktiska fördelningsfunktionen. Det är inte heller uppenbart om det är någon annan underliggande fördelning utan felet kan förmodligen betraktas som beroende av en mängd faktorer som inkluderar arbetsområde, parameternoggrannhet, hastighet, m.m. Antagandet om normalfördelade störningar bör därför användas med försiktighet, särskilt om fler frihetsgrader används.

55 6.5 Slutsatser från simuleringar Slutsatser från simuleringar Eftersom detta är simuleringar är det viktigt att notera att de experimentella körningarna ger en bättre överensstämmelse med hur observatörerna faktiskt fungerar i praktiken och dessa körningar återfinns i 7. Utvärderingarna i simuleringar har begränsats för att lämna större utrymme till experimentella resultat eftersom det visade sig att simuleringarna skilde sig signifikant gentemot de resultat som erhölls i verkligheten. Utgående från de skattningar som utförts i den simulerade miljön framstår det som att den integrerade EKF:en är mycket känslig för arbetsområde samt att DL är mycket dålig trots anpassade trimningar. Detta är dock inte samma bild av dessa två observatörsmetodiker som framkommer då samma körningar görs i praktiken. Slutligen är det tydligt att den fullständiga EKF:en i simuleringarna är den observatörsmetod som presterar betydligt bättre än alla de andra, och den är även robust både mot olika arbetsområden och omjusterade kovarianser vilket är mycket lovande resultat.

56

57 Kapitel 7 Experimentella resultat Detta kapitel ger en sammanfattning av de experimentella resultaten från körningar på en robot som nyttjat 2 DOF. Resultaten från kalibreringen av accelerometern presenteras i 7. och en detaljerad genomgång av EKF:erna samt den deterministiska observatören ges i 7.2 resp Alla mätningar har utförts på en sexaxlig industrirobot där endast axel 2 och 3 användes för rörelserna. Se figur 7. för en illustration av vilka axlar som använts, samt referensriktningar. 7. Accelerometermätningar och kalibrering Accelerometern, som är en central del av denna studie, studerades noggrant och en metod för kalibrering togs fram. Kalibreringen är nödvändig för att erhålla exakta mått för orientering och position. Den kalibreringsmetod som togs fram under detta projekt kan tyvärr inte offentliggöras i denna stund och nämns därför endast i mycket korta ordalag i denna rapport. Däremot kan resultatet av kalibreringsmetodens faktiska förbättringar presenteras, se figur 7.2. Eftersom de kalibreringsfel som ger upphov till felen i den ursprungliga accelerometermätningen inte är kända, vore det av intresse att jämföra den kalibrerade mätningen med en mätning då ett känt fel lagts på. I figur 7.3 presenteras den kalibrerade mätningen i x-led jämsides med en genererad mätning som är vriden 2 fel kring accelerometerns y- och z-axel gentemot gravitationsreferensen. Den genererade mätningen är således en illustration av vilken mätning som skulle uppkomma om accelerometern var orienterad fel enligt ovan. Bruset är identiskt för de två mätningarna för illustrativa syften. Som också nämndes i 3 påverkas accelerometern av störningar och för att illustrera detta för den accelerometer som användes på roboten ställdes accelerometern i en konstant riktning och mätningarna sparades. Resultatet av detta test syns i figur 7.4. Det blir här tydligt att ett mätbrus påverkar signalen, som beskrivs i 3.3, men någon lågfrekvent del är svårare att urskilja. Däremot framträder detta långsamt varierande fel tydligare då mätningen LP-filtreras med en gränsfrekvens på 6Hz, se figur

58 46 Experimentella resultat 3 2 Figur 7.. För att begränsa rörelsefriheten till 2 frihetsgrader användes endast robotens axel 2 och 3 för rörelserna i detta kapitel. Figuren illustrerar även den positiva rotationsriktningen för axlarna..5 Kalibrering av orientering.5 Kalibrering av orientering Fel i x led, [m/s 2 ].5 Okalibrerad Kalibrerad Nollinjen Fel i z led, [m/s 2 ].5 Okalibrerad Kalibrerad Nollinjen Sampel, 2Hz x 4 (a) Kalibrering av orientering x-axeln Sampel, 2Hz x 4 (b) Kalibrering av orientering z-axeln Figur 7.2. Fel i accelerometermätning gentemot gravitationsreferens - mäter förbättring i mätningarna efter kalibrering.

59 7. Accelerometermätningar och kalibrering 47 Kalibreringsorientering jämfört med givet fel Fel x led, [m/s 2 ].5.5 Kalibrerad Felvriden 2 grader Nollinjen Sampel, 2Hz x 4 Figur 7.3. Jämförelse mellan den kalibrerade accelerometermätningen och en genererad mätning som är vriden 2 fel gentemot referensen. Bruset för de två mätningarna är identiskt för illustrativa syften. Uppmätt acceleration, [m/s 2 ] Stillastående mätning x led Sampel, 2Hz (a) Konstant accelerationsmätning i x-led Uppmätt acceleration, [m/s 2 ] Stillastående mätning z led Sampel 2Hz (b) Konstant accelerationsmätning i z-led Figur 7.4. Accelerometern är vriden så att m/s 2 mättes upp i x-led och gravitationen 9.8m/s 2 mättes upp i z-led. Uppmätt acceleration, [m/s 2 ] Stillastående mätning x led LP filtrerad Sampel, 2Hz (a) Lågfrekvent störning i x-led Uppmätt acceleration, [m/s 2 ] Stillastående mätning z led Sampel 2Hz (b) Lågfrekvent störning i z-led Figur 7.5. Accelerometermätningen från figur 7.4 lågpassfiltreras här för att tydliggöra den lågfrekventa störningen som påverkar signalen.

60 48 Experimentella resultat (a) Crossbow-accelerometer (b) Xsens-accelerometer Figur 7.6. De två accelerometrar från Crossbow respektive Xsens som användes i denna studie. 7.. Använda accelerometrar I detta projekt genomfördes två omgångar av mätningar där den första utfördes med accelerometern MTx från Xsens. Denna använde ett USB-gränssnitt och kopplades således inte via robotens interna mätsystem utan till en extern dator. Denna sensor arbetade med en uppdateringsfrekvens på Hz och en noggrann beskrivning av den återfinns i [2]. Eftersom den kopplades direkt till en extern dator behövdes en manuell synkronisering utföras. Den andra mätningen gjordes med en accelerometer i produktserien GP från Crossbow. Denna använde ett analogt gränssnitt och integrerades med robotens egna system varför accelerometersignalerna automatisk synkroniserades och samma uppdateringsfrekvens som för roboten användes, nämligen 2Hz. En utförlig beskrivning för denna accelerometer finns i []. Båda dessa sensorer finns avbildade i figur Resultat för EKF-observatörer 7.2. Skattningsresultat EKF med Crossbow-sensor I detta avsnitt presenteras ett antal skattningsresultat med EKF-observatörerna då en fastmonterad accelerometer med 2Hz samplingshastighet använts. Accelerometern är inkopplad i robotens egna mätsystem och mätningarna sker alltså automatiskt synkroniserade med moment- och motorvinkelmätningar. Skattning av bana med kraftstörningar Banan som beskrivs i figur 7.7 har skapats genom att roboten kör en given triangulär bana och den störs kraftigt med en gummislägga för att efterlikna verkliga kraftstörningar. Banan skattas sedan med de olika EKF:erna baserat på motormoment, motorpositioner och accelerometermätningar. Anledningen till att kraftstörningar används från början är att de ordinarie banorna som kördes hade mycket svaga störningar och svängningar och därför var mycket enkla att skatta, varför