Institutionen för systemteknik
Department of Electrical Engineering
Examensarbete
Observatörer för skattning av
verktygs-positionen 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--09/4271--SE Linköping 2009
Department of Electrical Engineering Linköpings tekniska högskola
Linköpings universitet Linköpings universitet
Observatörer för skattning av
verktygs-positionen 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--09/4271--SE
Handledare: Erik Wernholt
isy, Linköpings universitet Mikael Norrlöf, Stig Moberg
ABB AB Robotics Examinator: Thomas Schön
isy, Linköpings universitet Linköping, 10 februari, 2009
Avdelning, Institution
Division, Department
Division of Automatic Control Department of Electrical Engineering Linköpings universitet
SE-581 83 Linköping, Sweden
Datum Date 2009-02-10 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport
URL för elektronisk version
http://www.control.isy.liu.se http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-17176 ISBN — ISRN LITH-ISY-EX--09/4271--SE
Serietitel och serienummer
Title of series, numbering
ISSN
—
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 indust-rial 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-30Hz. For this interval the estimate were also robust to model inaccuraci-es. 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 Kal-man Filter
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 indus-trial 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-30Hz. For this interval the estimate were also robust to model inaccura-cies. 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 indu-strirobot med flexibiliteter i leder och armar. På grund av kostnadsbesparingar används i dagens industrirobotar allt vekare komponenter och svagare struktu-rer 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örer-na använder mätningar av motorvinklarObservatörer-na och en accelerometer och de baseras på Extended Kalman Filter och en deterministisk variant. Observatörerna har utvär-derats vid experiment på en robot med två frihetsgrader och skattningen blir myc-ket god för medelhöga frekvenser (3-30Hz) medan långsammare rörelser (0-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 industrirobo-tar.
Tack
Jag vill börja med att tacka avdelningen för rörelsestyrning (PRCM) på ABB Ro-botics 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 diskus-sioner. 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äll-skap. 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 in-om vidare forskning men också för att bidra till ABB Robotics konkurrenskraft i framtiden.
Robert Henriksson, Västerås januari 2009
Innehåll
1 Introduktion 1 1.1 Bakgrund . . . 1 1.2 Syfte . . . 1 1.3 Mål . . . 1 1.4 Metod . . . 2 1.5 Avgränsningar . . . 2 1.6 Tidigare forskning . . . 31.7 ABB Robotics och LINK-SIC . . . 3
1.8 Rapportdisposition . . . 4 2 Grundläggande robotik 5 2.1 Grundläggande begrepp . . . 5 2.1.1 Koordinatsystem . . . 5 2.1.2 Kinematik . . . 5 2.1.3 Dynamik - stelkroppsmodell . . . 6 2.2 Utökad robotmodell . . . 7 2.2.1 Flexibiliteter i leder . . . 8 2.2.2 Matematisk modell . . . 8 2.2.3 Tillståndsmodell . . . 10 3 Accelerometer 11 3.1 Accelerometerns koordinatsystem . . . 11 3.2 Matematisk modell . . . 11 3.3 Modellering av störningar . . . 12 3.4 Kalibrering . . . 13 4 Estimeringsmetoder 15 4.1 Matematisk modell . . . 15
4.1.1 Systemmodell och diskretisering . . . 16
4.1.2 Tillgängliga mätningar . . . 16
4.2 Extended Kalman Filter . . . 16
4.2.1 Linjärisering . . . 17
4.2.2 Filteralgoritm . . . 17
4.2.3 Alternativa EKF-formuleringar . . . 18
4.3 Deterministisk observatör . . . 20
x Innehåll 4.3.1 Matematisk modell . . . 20 4.3.2 Linjärisering . . . 22 4.3.3 Polplacering . . . 23 4.3.4 Diskret implementering . . . 23 5 Trimning av observatörer 25 5.1 Trimning av kovarianser för EKF . . . 25
5.1.1 Kovariansskattning . . . 26
5.1.2 Formulering av optimeringsproblem . . . 26
5.2 Trimning av polplacering för deterministisk observatör . . . 28
6 Simuleringar 31 6.1 Simulering tvåaxlig robot . . . 31
6.2 Utvärderade observatörer . . . 32
6.3 Simuleringsresultat . . . 32
6.3.1 Resultat EKF-observatörer . . . 33
6.3.2 Resultat deterministisk observatör . . . 38
6.4 Analys av modellfel . . . 42
6.5 Slutsatser från simuleringar . . . 43
7 Experimentella resultat 45 7.1 Accelerometermätningar och kalibrering . . . 45
7.1.1 Använda accelerometrar . . . 48
7.2 Resultat för EKF-observatörer . . . 48
7.2.1 Skattningsresultat EKF med Crossbow-sensor . . . 48
7.2.2 Analys av kovarianser . . . 56
7.2.3 Trimningskänslighet för EKF . . . 57
7.2.4 Skattningsresultat EKF med Xsens-sensor . . . 58
7.3 Deterministisk observatör . . . 59
7.3.1 Skattningsresultat med Crossbow-sensor . . . 59
7.3.2 Analys av polplacering . . . 60
7.3.3 Trimningskänslighet för DL . . . 61
7.4 Beräkningskomplexitet . . . 61
8 Diskussion och slutsatser 63 8.1 Generella slutsatser . . . 63
8.2 Observatörsspecifika slutsatser . . . 65
8.3 Tillämpbarhet och användningsområden . . . 66
9 Vidare forskning 67 Litteraturförteckning 69 A Definitioner 71 A.1 Notation . . . 71
Kapitel 1
Introduktion
1.1
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 ro-botar 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.
1.2
Syfte
Syftet med detta examensarbete är att analysera problematiken med flexibla struk-turer och studera hur accelerometrar kan användas i estimeringsmodeller för att förbättra skattningen av armvinklar och verktygsposition. Olika estimeringsme-toder 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 kostnads-effektivt sätt. Hur accelerometrarna ska tas in i modellen och hur de kan kalibreras kommer att belysas.
1.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.
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)
1.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 busthet uppnås kommer systemet att testas och utvärderas för körningar på ro-bot. Accelerometern kommer då att kalibreras och analyseras, med tillhörande modellförändringar om nödvändigt. Estimeringsmodellerna kommer sedan att tes-tas 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 [10].
1.5
Avgränsningar
Observatörsmodellen kommer att begränsas till att hantera två axlar, men möjlig-het 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äknings-tid 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 ob-servatö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
1.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 me-delhö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.
1.6
Tidigare forskning
Inom området observatörer för flexibla manipulatorer har en hel del forskning ge-nomförts där exempelvis Nicosia et al. [19] var tidiga på området, och beskrev tillvägagångssätt att hantera den ytterst olinjära dynamiken i flexibla robotstruk-turer. 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 [16] 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 [20] har undersökt hur en accelerometer kan användas i en observatör baserad på Extended Kalman Filter (EKF) och Partikelfilter. Problematiken som kan upp-komma för flexibla mekanismer på grund av gravitationskraften beskrivs av De Luca et al. i [6] där gravitationskompensering gås igenom. Bland olika observa-tö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 [1] samt en enaxlig flexibel ro-botarm med okänd deformation [2]. EKF används av bland andra Norrlöf [20] och Li [16] men även Lertpiriyasuwat [15] där en EKF observatör för flexibla armar för en tvåaxlig robot studeras. Deras Kalmanfilter använde mätningar av verk-tygsposition samt vinkelhastigheter i lederna. Jankovic beskriver en observatör för en flexibel robot då mätningar endast finns tillgängliga på motorsidan [13].
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.
1.7
ABB Robotics och LINK-SIC
ABB Robotics är ett bolag inom ABB-koncernen som utvecklar och tillverkar in-dustrirobotar för automationstillämpningar inom framförallt tillverkande industri, där fordonsindustrin länge varit en stor kund. ABB Robotics hade 2007 intäkter på ca $1.4 miljarder och hade 4500 anställda globalt. Examensarbetet har utförts
4 Introduktion
på avdelningen för rörelsestyrning (PRCM) och är ett av de första projekten in-om forskningssamarbetet LINK-SIC sin-om är ett VINNOVA Industry Excellence Center. LINK-SIC startades 2008 och är ett 10-årigt forskningssamarbete mellan LiTH, ABB Robotics, ABB Corporate Research, Saab AB, GM Powertrain -Sweden AB och Scania CV.
1.8
Rapportdisposition
§ 1 - Introduktion Detta avsnitt beskriver projektets bakgrund, syfte och
mål-sä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
bak-grunden som behövs för att beskriva en robot. Avsnittet inleds med en här-ledning av en grundläggande robotmodell och avslutas med en grundlig be-skrivning 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
be-skrivs 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.
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.1
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, qai, som beskriver armvinkeln för respektive
led med avseende på en given referens, såsom angivet för tvåaxelsmodellen i fi-gur 2.1. En robot som har N leder har då N frihetsgrader (DOF) och då behövs
N generaliserade koordinater qai, i = 1, . . . , N för att beskriva systemet
fullstän-digt.
2.1.1
Koordinatsystem
Robotens geometri beskrivs utifrån ett grundkoordinatsystem, {w}, som är fixt vid robotens bas och i detta koordinatsystem anges positionen pwför verktygets
cent-rumpunkt, T CP . I figur 2.1(a) och 2.1(b) anges koordinatsystemet för en tvåaxlig robot då roboten står i startpositionen resp. i ett aktivt läge med armvinklarna
qa1> 0 och qa2> 0.
2.1.2
Kinematik
En robots geometriska sammansättning och sambandet mellan verktygsposition och armvinklar ges av dess kinematiska beskrivning. Robotverktygets position pw
beror av armarnas vinklar och definieras av
pw= xw(qa) yw(qa) zw(qa) = Γ(qa) 5
6 Grundläggande robotik TCP [xwzw] Xw Zw
Grundkinematik
pw (a) Startposition TCP [xwzw] pwGrundkinematik
qa2 qa1 Xw Zw (b) Aktivt lägeFigur 2.1. Exempel på konfigurationer för en tvåaxlig robot.
För en tvåaxlig robot som verkar i xz-planet kommer definitionen av Γ(qa) ske i
enlighet med figur 2.1. 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-Hartenberg-notationen som beskrivs i exempelvis [24]. Om vinklarna definieras i enlighet med figur 2.1 och armarna har längd L1resp. L2skulle följande form för Γ(qa) erhållas
pw= Γ(qa) = L1sin(qa1) + L2cos(qa1+ qa2) L1cos(qa1) − L2sin(qa1+ qa2) (2.1)
2.1.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 ut-an 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 (2.2a) d dt ∂L ∂ ˙qai − ∂L ∂qai = uai (2.2b)
där uai är pålagt moment på arm i, T är robotens totala kinetiska energi och U
dess potentiella energi som ges av
T =1
2q˙
T
aM (qa) ˙qa (2.3a)
U = U (qa) (2.3b)
med robotens konfigurationsberoende tröghetsmatris betecknad M (qa). Eftersom
den potentiella energin endast beror av qa kommer den bara in i (2.2) som
2.2 Utökad robotmodell 7
gravitationen på lederna. Det innebär att (2.2) kan skrivas
L = T (qa, ˙qa) − U (qa) (2.4a) Gi(qa) = ∂U (qa) ∂qai (2.4b) uai= d dt ∂T (qa, ˙qa) ∂ ˙qai −∂T (qa, ˙qa) ∂qai + Gi(qa) (2.4c)
Derivatorna av den kinetiska energin kan nu härledas
T (qa, ˙qa) = 1 2q˙ T aM (qa) ˙qa (2.5a) ∂T (qa, ˙qa) ∂ ˙qa =1 2 ∂ ˙qT aM (qa) ˙qa ∂ ˙qa = M (qa) ˙qa (2.5b) d dt(M (qa) ˙qa) = ˙M (qa) ˙qa+ M (qa)¨qa (2.5c) ∂T (qa, ˙qa) ∂qa =1 2 ˙ qT a dM (qa) dqa1 q˙a .. . ˙ qT a dM (qa) dqaN q˙a (2.5d)
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 cent-rifugaltermer, C(qa, ˙qa) enligt C(qa, ˙qa) = ˙M (qa) ˙qa− 1 2 ˙ qTa dM (qa) dqa1 q˙a .. . ˙ qT a dM (qa) dqaN q˙a
Om sedan (2.5) används i (2.4) med definitionen av C(qa, ˙qa) ovan och ekvationen
skrivs på vektorform erhålls den enklaste formen av dynamikekvationen enligt
M (qa)¨qa+ C(qa, ˙qa) + G(qa) = ua (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 friktionsmo-dell. Ett sätt att bygga upp en mer fullständig robotmodell presenteras i § 2.2.
2.2
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-8 Grundläggande robotik qm1 , qa1 qa2 Motorns läge Armens läge qm2
Ledflexibiliteter
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 [17] och alla variabler finns definierade i bilaga A. Det är också denna modell som observatörerna utvecklats utefter.
2.2.1
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 qmi. Det innebär att varje led beskrivs av
dels armens vinkel qaioch dels motorns vinkel qmi. Avvikelsen qai− qmibeskriver
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är-emot variabler och parametrar definierats så att utväxlingstalet inte behöver tas in i modellen.
2.2.2
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.1.3 men den härledningen utelämnas här. För att förenkla beskrivningen av dynamikekvationen kommer en robot med två leder
2.2 Utökad robotmodell 9
att presenteras i detta kapitel, och följande definitioner införs
u = 0 0 um1 um2 q = qa1 qa2 qm1 qm2 = qa qm (2.7)
där u är externa krafter som verkar på systemet. Det ger då följande momentjäm-viktsekvation som beskriver systemets dynamik,
u = M (qa)¨q + C(qa, ˙qa) + G(qa) + D( ˙qa− ˙qm) + τs(qa, qm) + κ( ˙qm) (2.8)
där M (qa) är systemets massmatris, C(qa, ˙qa) Coriolis- och centrifugaltermer och
G(qa) 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( ˙qa− ˙qm), olinjär fjäderkraft τs(qa, qm)
samt olinjär friktion κ( ˙qm). Dessa storheter definieras nedan och de robotspecifika
parametrar som ingår i uttrycken finns definierade i bilaga A. Massmatris: M (qa) = Ma(qa) 0 0 Mm = J11(qa) J12(qa) 0 0 J21(qa) J22(qa) 0 0 0 0 jm1 0 0 0 0 jm2 J11(qa) = j1+ m1ξ21+ j2+ m2(l12+ ξ 2 2− 2l1ξ2sin(qa2)) J12(qa) = j2+ m2(ξ22− l1ξ2sin(qa2)) J21(qa) = J12(qa) J22(qa) = j2+ m2ξ22
Gravitationens momentinverkan på lederna:
G(qa) = g1(qa) g2(qa) 0 0 T
g1(qa) = −g(m1ξ1sin(qa1) + m2(l1sin(qa1) + ξ2cos(qa1+ qa2)))
g2(qa) = −gm2ξ2cos(qa1+ qa2)
Coriolis- och centrifugaltermer:
C(qa, ˙qa) = Ca(qa, ˙qa) 0 = −m2l1ξ2cos(qa2)(2 ˙qa1q˙a2+ ˙q2a2) m2l1ξ2cos(qa2) ˙qa12 0 0
10 Grundläggande robotik Olinjär fjäderkraft: τs(q) = τs1(∆q1) τs2(∆q2) τs1(−∆q1) τs2(∆q2) ∆qi= qai− qmi, i = 1 . . . 2. τsi= ki1∆qi+ ki3∆q3i, |∆qi| ≤ ψi τsi= sign(∆qi)(Ki0+ Ki1(|∆qi− ψi)), |∆qi| > ψi ki1= klowi ki3= (k high i − k low i )/(3ψ 2 i) Ki0= ki1ψi+ ki3ψi3 Ki1= k high i
Dämpningsmatris som beskriver viskös dämpning:
D =
d1 0
0 d2
Modell av olinjära friktionsmoment som beror av motorernas vinkelhastigheter:
κ( ˙q) = 0 0 κ1( ˙q) κ2( ˙q)
T
κi( ˙q) = fdiq˙mi+ fci(µki+ (1 − µki) cosh−1(βiq˙mi)) tanh(αiq˙mi), i = 1, . . . , 2.
2.2.3
Tillståndsmodell
Utgående från definitionerna i § 2.2.2 och (2.8) kan nu en tillståndsmodell formu-leras med insignal u och tillstånd x
u = u, x = x1 x2 x3 x4 = qa qm ˙ qa ˙ qm
samt tillhörande olinjära tillståndsekvation, härledd från (2.8)
˙ x = x3 x4 M−1(x1)(−C(x1, x3) − G(x1) − D(x3− x4) − τs(x1, x2) − κ(x3)) (2.9) + 0 0 M−1(x1) u (2.10)
Kapitel 3
Accelerometer
I detta avsnitt beskrivs hur en accelerometer kan modelleras och implementeras med observatörerna. En genomgång av accelerationsekvationer och koordinatsy-stem samt kalibreringsmetodik ges även.
3.1
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 koor-dinatsystem måste givetvis relateras till det robotfixa baskoorkoor-dinatsystemet {w} eftersom det är i detta som beräkningar sker. I figur 3.1 illustreras baskoordinat-systemet samt sensorkoordinatbaskoordinat-systemet för två olika konfigurationer för en två-axlig robot som rör sig i xz-planet och har en accelerometer monterad på un-dersidan av andra armen. Positionen för sensorn uttryckt i baskoordinatsystemet, anges av ρw = T (qa) och är beroende av robotkonfigurationen. I det första
lä-get, då qa1 = qa2 = 0 sammanfaller riktningarna för sensorns koordinatsystem
med baskoordinatsystemet. I det andra läget är istället roboten i en position med
qa1> 0, qa2> 0 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= Γ(qa), två gånger med avseende på tiden.
Med nedanstående definition ˙
Γ(qa) = J (qa) ˙qa
erhålls följande uttryck för accelerationen ˙
ρw= ˙Γ(qa) = J (qa) ˙qa (3.1a)
¨
ρw= ¨Γ(qa) = J (qa)¨qa+ ˙J (qa) ˙qa (3.1b)
12 Accelerometer Xw Zw
Sensorkoordinatsystem
ρw Xs Zs Sensor (a) Grundposition ZwSensorkoordinatsystem
qa2 qa1 Xw Zw ρw Xs Zs Sensor (b) Aktivt lägeFigur 3.1. Koordinatsystem för robot och sensor.
där ˙ρw och ¨ρw är sensorns hastighet resp. acceleration angiven i
baskoordinatsy-stemet {w}. Jacobianens tidsderivata blir något komplicerad och anges av
˙ J (qa) = N X i=1 ∂J ∂qai ˙ qai
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} 7→ {s} : Rs
w(qa) och {s} 7→ {w} : Rws(qa).
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= Rsw(qa) ¨ρw− Rsw(qa)Gw= Rsw(J (qa)¨qa+ ˙J (qa, ˙qa) ˙qa) − Rsw(qa)Gw (3.2)
där gravitationen modelleras som Gw= [0, 0, −g0]T med g0 som
gravitationskon-stanten, och från detta samband kan armaccelerationerna härledas ¨
qa = (Rsw(qa)J )
−1
(qa)( ¨ρs− Rws(qa)( ˙J (qa, ˙qa) ˙qa− Gw)). (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ät-ningar kommer givetvis att innehålla vissa störmät-ningar. Dessa störmät-ningar brukar modelleras som dels en långsamt varierande drift samt en normalfördelad störning
3.4 Kalibrering 13
med låg amplitud. I enlighet med notationen i [9] benämns den långsamma driften
δs och det normalfördelade mätbruset es. Den faktiska mätningen som
accelero-metern registrerar definieras av ρMs och följande modell beskriver accelerometerns
mätning i sensorkoordinatsystemet {s} ¨
ρMs = ¨ρs+ δs+ es= Rws(qa)( ¨ρw− Gw) + δs+ es (3.4)
Som tidigare är Rs
wkoordinattransformationsmatrisen mellan baskoordinater och
sensorkoordinater, ¨ρw är den faktiska accelerationen för accelerometern och Gw
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äga-gångssättet.
Kapitel 4
Estimeringsmetoder
Då en matematisk modell av ett system används i praktiken behövs ofta olika es-timeringsmetoder eftersom verkligheten skiljer sig från modellen. I denna tillämp-ning 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 [14]. 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.1
Matematisk modell
Den olinjära kontinuerliga tillståndsmodellen som beskriver robotens dynamik kan formuleras enligt ˙ x = f (x, u), x = x1 x2 x3 x4 = qa qm ˙ qa ˙ qm (4.1)
där f är den olinjära funktion som beskriver dynamiken för den tvåaxliga model-len som ges i § 2.2.3. Utskrivet med endast tillståndsvariablerna erhålls följande tillståndsmodell ˙ x = x3 x4 M−1(x1)(u − C(x1, x3) − G(x1) − D(x3− x4) − τs(x1, x2) − κ(x3)) (4.2) 15
16 Estimeringsmetoder
4.1.1
Systemmodell och diskretisering
I praktiken implementeras ett system på en robot i diskret form varför den konti-nuerliga tillståndsekvationen beskriven i (4.1) 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]
xk+1≈ xk+ Tsx˙k = xk+ Tsf (xk, uk) = F (xk, uk) (4.3)
där F (xk, uk) betecknar den nya systemfunktionen. Ett sådant system kommer
givetvis även att innehålla systemstörningar och dessa modelleras som normal-födelade med kovarians Qk, alltså vk ∼ N (0, Qk) och följande systemfunktion
erhålles således
xk+1= F (xk, uk) + vk (4.4)
4.1.2
Tillgängliga mätningar
De mätningar z som finns tillgängliga är motorvinklar qmM 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
zk = qM mk ¨ ρM sk (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 wk ∼
N (0, Rk) vid tidpunkt k. Detta samband mellan xk, uk och zk ges av funktionen
h(xk, uk), med uttrycket för ¨ρMs hämtat från (3.2)
zk= h(xk, uk) + wk = x2k Rs w(x1k)(J (x1k)¨x1k+ ˙J (x1k)x3k− Gw) + wk (4.6)
Eftersom armaccelerationerna ¨x1kej ingår som tillståndsvariabel ersätts ¨x1ki (4.6)
med uttrycket för armaccelerationen som ingår i tillståndsekvationen från (2.10) ¨
x1k = M−1(x1k)(uk− C(x1k, x3k) − G(x1k) − D(x3k− x4k) − τs(x1k, x2k) − κ(x3k))
(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 Kal-manfiltret för att kunna hantera olinjära system. Principen för EKF är mycket
4.2 Extended Kalman Filter 17
enkel; i varje iteration linjäriseras systemet runt aktuell skattning och sedan an-vä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 infor-mation om EKF finns i många filter- och signalteoriböcker där två exempel är [3] och [14]. En översikt ges även i [23].
4.2.1
Linjärisering
I § 4.1 definierades den matematiska modell som beskriver roboten som ett tids-diskret olinjärt dynamiskt system och system- och mätfunktionen tecknas enligt
xk+1= F (xk, uk) + vk (4.8a)
zk= h(xk, uk) + wk (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, ˆxk|krespektive ˆxk|k−1.
F (xk, uk) ≈ F (ˆxk|k, uk) + ∂F (x, uk) ∂x x=ˆx k|k (xk− ˆxk|k) (4.9a) h(xk, uk) ≈ h(ˆxk|k−1, uk) + ∂h(x, uk) ∂x x=ˆx k|k−1 (xk− ˆxk|k−1) (4.9b)
För att förenkla uttrycken görs följande definitioner
Ak= ∂F (x, uk) ∂x x=ˆx k|k (4.10a) Hk= ∂h(x, uk) ∂x x=ˆx k|k−1 (4.10b)
Med dessa variabler kan nu de linjäriserade system- och mätfunktionerna slutligen beskrivas enligt
xk+1= F (ˆxk|k, uk) + Ak(xk− ˆxk|k) + vk (4.11a)
zk = h(ˆxk|k−1, uk) + Hk(xk− ˆxk|k−1) + wk (4.11b)
4.2.2
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 algo-ritmen hänvisas till exempelvis [3] eller [14].
Mätuppdatering
ˆ
xk|k= ˆxk|k−1+ Kk(zk− h(ˆxk|k−1, uk))
18 Estimeringsmetoder Tidsuppdatering ˆ xk+1|k= F (ˆxk|k, uk) Pk+1|k= AkPk|kATk + Qk Kk= Pk|k−1HkT(HkPk|k−1HkT + Rk)−1 Initialvärden ˆ x1|0 = ¯x1 P1|0 = ¯P1
där ¯P1och ¯x1är valda initialvärden för observatören. Initialvärden för tillstånden
kan exempelvis väljas till ¯ x1= qM m1(0) qm2M (0) qm1M (0) qm2M (0) 0 0 0 0 T
om systemet startar i vila. Initialvärden till ¯P1ä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 § 5.
4.2.3
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 ¨x1k
i (4.7). Eftersom ett syfte med att använda accelerometern är att förbättra robust-heten gentemot modellfel är detta problematiskt. Det finns fler EKF-varianter som kan undersökas än den som hittills presenterats och i detta avsnitt presenteras yt-terligare 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 accelerome-termätningarna till hastigheter, så att bara kinematiken behövs i mätfunktionen. Alltså ˙ ρMw(k) = k Z 0 ¨ ρMw(t)dt = k Z 0 Rws(t) ¨ρMs (t) + Gwdt (4.12)
där integreringen i (4.12) är en numerisk integrering av någon sort eftersom mät-ningarna är diskreta. Olika numeriska metoder för integrering finns beskrivna i exempelvis [8]. Den uppmätta accelerationen ¨ρM
s (t) i (4.12) 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 inte-grering 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
4.2 Extended Kalman Filter 19
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 ˙ρMw 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.1a), bara är jacobianen J (qa) som kommer in i beräkningarna
˙
ρw= J (qa) ˙qa (4.13)
och mätekvationen blir således
h(xk, uk) = x2k J (x1k)x3k (4.14) 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 beskriv-ningen 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å dynami-kekvationen
uk= M (x1k)¨xk+ C(x1k, ˙x1k) + G(x1k) + D(x3k− x4k) + K(x1k− x2k) (4.15)
där K(x1k− x2k) 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 ¨x1k
mås-te ersättas med ett uttryck härlett från dynamikekvationen blir nu detta uttryck ¨
x1k = M−1(x1k)(uk− C(x1k, x3k) − G(x1k) − D(x3k− x4k) − K(x1k− x2k)).
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ård-vara undersöks även ett EKF som enbart använder motormätningarna i mätfunk-tionen, som då får en mycket enkel form.
h(xk) = x2k
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 ro-boten, 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.
20 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 observa-tö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.
4.3.1
Matematisk modell
Denna observatör är uppbyggd för en flexibel manipulator med N armar samman-länkade av N visko-elastiska leder, liksom den som beskrivs i § 2.2.2 och som ges en utförligare beskrivning i [17]. 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 § 2.2.2 enligt följande
q = qa qm , ∆q ∆ ˙q = qa− qm ˙ qa− ˙qm K = K1 K2 = k1 0 0 k2 −k1 0 0 −k2 , u = 0 um (4.16) D = D1 D2 = d1 0 0 d2 −d1 0 0 −d2 , M (qa) = Ma(qa) 0 0 Mm C(qa, ˙qa) = Ca(qa, ˙qa) 0 , G(qa) = Ga(qa) 0 (4.17)
Dynamikekvationen som beskrivs i § 2.2.2 förenklas och linjär fjäderkraft, med fjäderkonstanter ki, och noll friktion införs. Detta ger en betydligt enklare
dyna-mikekvation jämfört med (2.8).
u = M (qa)¨q + C(qa, ˙qa) + G(qa) + D∆ ˙q + K∆q (4.18)
En uppdelning av ekvationen ger nu följande två dynamikekvationer, med nydefi-nierade variabler enligt (4.16).
0 = Ma(qa)¨qa+ Ca(qa, ˙qa) + Ga(qa) + K1∆q + D1∆ ˙q (4.19a)
4.3 Deterministisk observatör 21
Samma tillståndsmodell som i (4.1) används, med följande nya beteckningar där
umbetraktas som insignal och mätsignaler är qMm och ¨ρMs . Mätsignalen ¨ρMs
kom-mer 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= Rsw(qa)(J (qa)¨qa+ ˙J (qa, ˙qa) ˙qa+ Gw)
som, då Rs
w(qa)J (qa) ej är singulär, ger en approximation av ¨qa som benämns ¨qMa .
¨
qaM = (Rsw(qa)J (qa))−1(q)( ¨ρMs − R s
w(qa)( ˙J (qa, ˙qa) ˙qa+ Gw)) (4.20)
Om Rs
wJ skulle bli singulär, alltså att (Rsw(qa)J (qa))−1 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.20) kan således inte beräknas. För att den vanliga inversen ska kunna användas i (Rs
w(qa)J (qa))
−1
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 [21]. Utgående från (4.19) kan nu derivatan av varje tillstånd härledas
˙ x1= x3 ˙ x2= x4 ˙ x3= −Ma−1(x1)(Ca(x1, x3) + Ga(x1) + K1(x1− x2) + D1(x3− x4)) ˙ x4= Mm−1(−K2(x1− x2) − D2(x3− x4)) + Mm−1um
där den enda olinjära delen återfinns i ˙x3= ¨qa. Därför införs ¨qa = ˙x3= folinj för
att underlätta notationen.
folinj(x) = −Ma−1(x1)(Ca(x1, x3) + Ga(x1) + K1(x1− x2) + D1(x3− x4))
Tillståndsekvationen kan nu skrivas om på följande sätt
˙ x = x3 x4 0 Mm−1(−K2(x1− x2) − D2(x3− x4)) + 0 0 folinj(x) Mm−1um (4.21)
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 ˙x3 som
är olinjär, svarande mot ¨qa. Om därför armaccelerationerna ¨qa kan estimeras från
22 Estimeringsmetoder
4.3.2
Linjärisering
Eftersom det endast är funktionen för ˙x3= ¨qai (4.21) som är olinjär, kan istället en
uppskattning av denna tas från accelerometermätningen. Därför sätts folinj(x) = 0
och uppskattningen av ¨qa tas istället in från accelerometern, ¨qMa . Därmed kan
tillståndsekvationen skrivas på följande form
˙ x = x3 x4 0 M−1 m (−K2(x1− x2) − D2(x3− x4)) + 0 0 ¨ qaM M−1 m um (4.22)
Med en del algebra kan systemet skrivas om med matriser och för detta behövs följande definitioner av systemmatriser A0∈ R4N ×4N och G0∈ R4N ×3N
A0= 0 0 I 0 0 0 0 1 0 0 0 0 −M−1 m K2 Mm−1K2 −Mm−1D2 Mm−1D2 G0= 0 0 0 0 I 0 0 Mm−1
och med dessa definitioner skrivs (4.22) om till
˙ x = A0x + G0 ¨ qMa 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.15) 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 ξ
˙ ξ = A0ξ + G0 ¨ qMa um + L(qMm − C0ξ) (4.24)
där förstärkningen L ∈ R4N ×N och matrisen C
0∈ RN ×4N definieras enligt
L =
L1 L2 L3 L4
T
, Li= diag(Li,1, . . . , Li,N)
C0=
0 I 0 0
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
4.3 Deterministisk observatör 23
observatörens dynamik rent intuitivt kan sägas att den använder accelerometer-mä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.
4.3.3
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 A0− LC0 och sätta förstärkningarna i L så att önskade poler
erhålls. En noggrannare beskrivning av tillvägagångssättet ges i § 5.
4.3.4
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.
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 beskri-ver just detta, hur trimningen kan genomföras automatiskt med en dator. Detta problem är givetvis inte enkelt och för att reducera komplexiteten i beskrivning-arna 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.1
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
xk+1= F (xk, uk) + vk (5.1a)
zk= h(xk, uk) + wk (5.1b)
behöver skattningar beräknas för kovarianserna av vk och wk. Felet i
systemekva-tionen, vk, 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
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.1.1
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 = 1 . . . NT beräknas för alla körningar i = 1 . . . NR som
svarar mot olika banor och störningar. ˆ vki = xik+1− F (xik, u i k) wˆ i k = z i k− h(x i k, u i k) ˆ vi= vi 1 .. . vi NT wˆ i = wi 1 .. . wi NT
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 kova-rianserna med den ordinarie väntevärdesriktiga samplingskovariansen, som bland annat beskrivs i [18]. ¯ v = 1 NTNR NR X i=1 NT X k=1 ˆ vki w =¯ 1 NTNR NR X i=1 NT X k=1 ˆ wik b R = 1 NTNR− 1 NR X i=1 NT X k=1 (ˆvki − ¯v)(ˆvki − ¯v)T (5.2) c W = 1 NTNR− 1 NR X i=1 NT X k=1 ( ˆwik− ¯w)( ˆwik− ¯w)T (5.3)
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.
5.1.2
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 bR ∈ R8×8 och b
Q ∈ R4×4.
Skatt-ningarna 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 cW och bR som
utgångs-punkter. Först måste dock en målfunktion definieras och för att minimera antalet variabler förenklas kovariansmatriserna till diagonalmatriser.
e R = diagRb f W = diagWc
5.1 Trimning av kovarianser för EKF 27
Ytterligare ett antagande förutsätter att det inbördes förhållandet mellan varian-serna för motorer, accelerometrar samt vinkelpositionsuppdateringar och vinkel-hastighetsuppdateringar 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 skat-tade definieras optimeringsvariabler Λ =
λ1 λ2 λ3 λ4 λ5 . För given Λ
definieras kovarianser enligt nedan.
e Rλ= e R1 0 0 0 0 Re2 0 0 0 0 Re3 0 0 0 0 Re4 = λ1I2×2 0 0 0 0 λ2I2×2 0 0 0 0 λ3I2×2 0 0 0 0 λ4I2×2 e R (5.4) f Wλ= " f W1 0 0 fW2 # = λ5I2×2 0 0 I2×2 f 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 variabelupp-sättning Λ definieras alltså kovarianser eRλ och fWλ 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 fre-kvensområ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. 30 Hz. Därav införs följande notation
b
X = BP303 (ˆx) Z = BPb 303 (ˆz)
X = BP303 (x) Z = BP
30 3 (z)
och med dessa filtrerade positioner och skattningar formuleras nu följande mål-funktion fobjx ( bX) = Np X p=1 ||Xp− bXp||2 (5.6) fobjz ( bZ) = Np X p=1 ||Zp− bZp||2 (5.7) fobj b X, bZ= fobjx b X+ fobjz b Z (5.8)
och denna målfunktion beräknar 2-normen av den filtrerade avvikelsen i x- och
28 Trimning av observatörer
beräkningarna görs över definieras av Np medan Xp och Zp anger BP-filtrerade
värden av den faktiska rörelsen över område p och bXp resp. bZp anger skattningen
med kovarianser eRλ och fWλ. Antalet sampel för område p anges av Nkp.
Anled-ningen 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 rela-tivt 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 fobj( bX, bZ) Under bivillkoren λj> 0 j = 1 . . . 5 e Rλ= λ1I2×2 0 0 0 0 λ2I2×2 0 0 0 0 λ3I2×2 0 0 0 0 λ4I2×2 e R f Wλ= λ5I2×2 0 0 I2×2 f W [ˆx, ˆz] = EKFReλ, fWλ b X = BP30 3 (ˆx) Z = BPb 303 (ˆz) b Xp⊆ bX Zbp⊆ bZ
I detta uttryck betecknar [ˆx, ˆz] = EKFR, fe W
att en skattning av xz-banan ut-förs med EKF:en för kovarianser eR och fW , och dessa skattningar filtreras sedan
med ett bandpassfilter. Detta är inte ett konvext optimeringsproblem, på grund av bivillkoret [ˆx, ˆz] = EKFR, fe W
och därför kan ett globalt optimum ej garante-ras, 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 analy-tisk 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
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 egenfrekven-ser ω för varje polpar. Optimeringsproblemet formuleras analogt med § 5.1.2 där skattningarna också filtrerats.
Minimera fobj( bX, bZ) Under bivillkoren ωj> 0, ζj> 0 j = 1 . . . 4 s+j = ωj −ζj+ i q 1 − ζ2 j s−j = ωj −ζj− i q 1 − ζj2 Sj= {s+j, s − j} j = 1 . . . 4 [ˆx, ˆz] = DL (S) b X = BP30 3 (ˆx) Z = BPb 303 (ˆz) b Xp⊆ bX Zbp⊆ bZ
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 ob-servatö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 al-goritmen, se exempelvis [4].
