avigering, sensorfusion och styrning för autonom marfarost amensarbete utfört i Reglerteni vid inöpings tenisa högsola av irgitta Wingqvist Mattias Källstrand ih-isy-x--5/3749--s
avigering, sensorfusion och styrning för autonom marfarost amensarbete utfört i Reglerteni vid inöpings tenisa högsola av irgitta Wingqvist Mattias Källstrand ih-isy-x--5/3749--s aminator: Ricard Karlsson Handledare: Christina rönwall Hans ohlin Miael Aelsson Runo irholm
Avdelning, Institution Division, Department Division of Automatic Control, Dept. of lectrical ngineering 58 83 inöping Datum Date 2 december 25 Språ anguage Svensa/Swedish ngelsa/nglish Rapporttyp Report category icentiatavhandling amensarbete C-uppsats D-uppsats Övrig rapport IS ISR IH-ISY-X 5/3749 S Serietitel och serienummer itle of series, numbering ISS UR för eletronis version http://www.control.isy.liu.se http://www.ep.liu.se/ejobb/isy/25/3749/ itel itle avigering, sensorfusion och styrning för autonom marfarost avigation, Sensor fusion and Control of an Autonomous round Vehicle Författare Author irgitta Wingqvist and Mattias Källstrand Sammanfattning Abstract he aim of the Master s hesis wor is to study and develop algorithms for autonomous travel of a UV Unmanned round Vehicle. A vehicle for the mounting of sensors has been constructed in order to perform the wor. Since the UV is to be used outdoor in urban areas, PS can be used. o improve precision and robustness, inertial navigation is used in addition to PS, since PS reception is liely to be diminished in such areas. he sensors used for navigation are consequently PS, magnetometers, accelerometers, gyroscopes, tachometers and ultra sonic sensors measuring distance to be used in detection of obstacles. he system has been implemented in Matlab. wo alternative methods of navigation with sensor fusion have been developed; one is a decentralized method with Kalman filtering using an error model and the other is a centralized particle filter using an all-embracing model of the vehicle. he two methods have been evaluated and compared. est results show that the two methods perform equivalently. he autonomous travel is undertaen between predetermined waypoints. In order to steer the vehicle a PID-controller based on the error between heading and its reference value is used. he computation of the reference value is based on position and heading in comparison to the desired path. he system has been tested using different routes and the results show an evident improvement of the precision in navigation compared to using only PS-data. his holds for both navigation methods. Simulation of collision avoidance using virtual force fields shows satisfying results as well as terrain navigation with coordinate map referencing. ycelord Keywords Autonomy, UV, Kalman filtering, particle filtering, navigation, potential field, virtual force fields.
Sammanfattning amensarbetet är en studie i utvecling av algoritmer för autonom förflyttning av en UV eng Unmanned round Vehicle. För ändamålet har en farost onstruerats där budgetsensorer för navigering används. Farosten är tänt att färdas utomhus i tätbebyggt område och PS används. För förbättring av noggrannhet och robusthet vid dålig PSmottagning används även sensorer för tröghetsnavigering vilet här innebär magnetometrar, accelerometrar, gyron och tachometrar. För hinderdetetering finns avståndsmätande ultraljudssonar. Systemet som tagits fram har implementerats i realtid i Matlab. vå olia navigeringsmetoder med sensorfusion har utprovats; en decentraliserad variant med almanfilter som är uppbyggd ring felmodeller och en centraliserad variant med ett partielfilter som använder en helhetsmodell för farosten. De båda navigeringsmetoderna har utvärderats och jämförts. Resultat visar att de båda metoderna presterar livärdigt. Den autonoma förflyttningen utförs mellan förutbestämda brytpunter. För att styra farosten har en PID-regulator baserad på felet mellan urs och börvärde använts. örvärdet på urs baseras på nuvarande position och ritning relativt den önsade färdvägen. Olia örsituationer har testats och resultaten visar en marant förbättring av navigeringsprecisionen jämfört med endast PS-mätningar för både alman- och partielfilter. Simuleringar på vetorfältsstyrning med virtuella raftfält för att undvia hinder har utförts med goda resultat lisom simuleringar av artreferenspositionering. ycelord: Autonomitet, UV, almanfilter, partielfilter, sensorfusion, navigering, vetorfält, virtuella raftfält.
Abstract he aim of the Master s hesis wor is to study and develop algorithms for autonomous travel of a UV Unmanned round Vehicle. A vehicle for the mounting of sensors has been constructed in order to perform the wor. Since the UV is to be used outdoor in urban areas, PS can be used. o improve precision and robustness, inertial navigation is used in addition to PS, since PS reception is liely to be diminished in such areas. he sensors used for navigation are consequently PS, magnetometers, accelerometers, gyroscopes, tachometers and ultra sonic sensors measuring distance to be used in detection of obstacles. he system has been implemented in Matlab. wo alternative methods of navigation with sensor fusion have been developed; one is a decentralized method with Kalman filtering using an error model and the other is a centralized particle filter using an all-embracing model of the vehicle. he two methods have been evaluated and compared. est results show that the two methods perform equivalently. he autonomous travel is undertaen between predetermined waypoints. In order to steer the vehicle a PID-controller based on the error between heading and its reference value is used. he computation of the reference value is based on position and heading in comparison to the desired path. he system has been tested using different routes and the results show an evident improvement of the precision in navigation compared to using only PS-data. his holds for both navigation methods. Simulation of collision avoidance using virtual force fields shows satisfying results as well as terrain navigation with coordinate map referencing. Keywords: Autonomy, UV, Kalman filtering, particle filtering, navigation, potential field, virtual force fields, guidance.
Förord amensarbetet, som denna rapport är ett resultat av, har utförts på Aerotechelub A. Vi är glada för att ha fått möjligheten att göra ett eamensarbete med sådan bredd och djup som arbetet har inneburit. Vi vill rita ett speciellt tac personalen på Aerotechelub som givit värdefull avoppling och ett gott sratt vid fiaraster. Vi vill även nämna ett antal personer som har hjälpt oss på vägen. För ovärderlig hjälp med onstrution av farosten vill vi taca Kjell risson och för engagemang och goda råd till navigeringssystemen tacar vi Miael Aelsson och Hans ohlin. Vi vill ocså taca Per Magnusson för assistans vid inmätning av referenspunter. För att se eamensarbetet i sitt sammanhang och visioner om framtida möjligheter för UV-systemet ritar vi även ett tac till Runo irholm. Vi vill även taca eaminator Dr. Ricard Karlsson för att du tog dig tid att ge oss råd angående partielfiltret under din föräldraledighet och handledare ic. Christina rönwall från inöpings universitet för tips med rapport, avgränsningar och problemdefinition. Vi vill ocså taca nära och ära för all uppmuntran och tålamod vid långa timmars arbete. irgitta Wingqvist och Mattias Källstrand inöping, ovember 25
IDI.... AVRÄSIAR...4.2 RAPPORS DISPOSIIO...4 2 SYSMÖVRSIK...5 3 SYSMKOSRUKIO...7 3. DSIFIOSOFI...7 3.2 PAFORM...7 3.3 SSORR...8 3.4 SSORKOR...8 3.5 DAOR...9 3.6 SAMPISHASIHR... 4 KOORDIASYSM... 4. ODIC FRAM... 4.2 V FRAM... 4.3 ODY FRAM...2 4.4 POSIIOSAIVS...2 4.5 SKVOPRAOR...3 5 SSORMODR...5 5. SSORF...5 5.2 MAOMRAR...6 5.3 ACCROMRAR...7 5.4 YRO...8 5.5 ACHOMRAR...9 5.6 PS...2 5.7 US-SSORR...2 5.8 SAMMASÄI AV SSORMODR...2 6 FARKOSMODR...23 6. RÖRSMOD...23 6.2 MOD ÖVR SYRSIAS PÅVRKA AV KURS...25 6.3 MOD FÖR KOMPRSSIO AV HJURADIR...27 6.4 AIYD OCH HÖR ORDIS DYAMIK...28 6.5 IDSKOIURI OCH IDSDISKR SKRIVI...29 7 SIMRISORI...3 7. AYSIASK SIMRISORI...3 7.2 KAMAFIRRI...32 7.3 PARIKFIRRI...34 8 AVIRISSYSM...4 8. IMPMRIAVKAMAFIR...4 8.2 ODOMRISKA RÄKIAR...44 8.3 UISSÄMI...45 8.4 KURSSÄMI...5 8.5 POSIIOSSÄMI...53 8.6 IMPMRI AV PARIKFIR...55 9 SYRSYSM...59 9. RRI...59 9.2 HARI AV WAYPOIS...6 9.3 SYRI MD VIRUA KRAFFÄ...62 XPRIM, SIMURIAR OCH UVÄRDRI...65
. AMÄ OM XPRIM...65.2 POSIIORI MD KAMAFIR...66.3 POSIIORI MD PARIKFIR...72.4 JÄMFÖRS MA AVIRISÖSIAR...78.5 RRI...79.6 SIMURIAR...8 SUSASR...83. RSUA...83.2 FÖRSA PÅ FORSA AR...84 A CKIAR...87 PS OCH OIKA RFRSSYSM...89. JORDMODR OCH RFRSSYSM...89
IDI I en stridssituation är det vitigt att värna om förbandets säerhet genom att undvia eller få övertag över potentiella hot. tt hot an till eempel vara en fientlig stridsvagn, stridande förband, terrorister eller emis/radioativ ontaminering. Speciellt vitigt blir det att ha ännedom om fientliga förbands positioner vid strid i tätbebyggt område då det där finns många platser och sätt att gömma sig på. Sedan slutet av 9-talet har världsläget förändrats och försvarsmater i många länder har omformats från invasionsförsvar mot ett insatsförsvar som sall unna neutralisera små stridande mycet oförutsägbara grupper. För att unna hämta information från ocuperade områden med hög ris för fientlig eldgivning undvis gärna framrycning av grupp i dessa områden. För spaning är det då naturligt att använda sig av en obemannad marfarost eng. Unmanned round Vehicle förortad UV. Det svensa försvaret har ett intresse av att utvärdera funtionalitet och prestanda för UV:er i olia miljöer och situationer. SOK SPIO I SPIO II SPIO III Figur. Överst till vänster visas den sladdstyrda Snoen och till höger den fjärrstyrda SPIO I med PS. ere till vänster finns den radiostyrda SPIO II till höger den autonoma utveclingsfarosten SPIO III som tagits fram i det här eamensarbetet.
På Aerotechelub UV: SPIO I, SPIO II och KH Snoen, PUO har det tidigare utveclats UV-system för utvärdering till det svensa försvaret, se figur.. De UV:er som tidigare utveclats på Aerotechelub i inöping är fjärrstyrda och utrustade med amera, ritmirofon och PS eng. lobal Positioning System för spaning i tätbebyggt område. SPIO II används idag av marstridssolan i Kvarn och ingår i en större studie av effetiviteten på grupp- och plutonnivå [][2]. Preliminära resultat av de studier som gjorts visar att gruppens effetivitet säns. Soldaten som styr farosten blir upptagen med spaning och har svårigheter att oncentrera sig på den atuella stridssituationen med avfyrade sott, eplosioner, sri och rörelse. n möjlig förbättring av situationen an fås genom att införa autonoma funtioner. I samsprå med svens militärpersonal framom önsemål om autonom förflyttning av en spaningsfarost. amensarbetet avigering, sensorfursion och styrning för autonom marfarost behandlar förutsättningar och metoder för att uppnå autonom förflyttning i tätbebyggt område. Figur.2. Illustration av autonom funtionalitet vid urban stridssituation. reträttvägar sicar gruppen ut en spanings-uv med amera. För att undersöa möjliga 2
I figur.2 illustreras en tänbar funtionalitet vid strid i urban miljö. De allierade blir besjutna av fientlig eld och måste retirera. För att undersöa möjliga reträttvägar sicar gruppen ut en autonom spanings-uv med amera. n video-/radiolän sicar information till en videodisplay för vidare tolning av gruppen. För navigering i urban miljö är inte PSpositionering tillräcligt bra på grund av radiosugga och flervägsutbredning av signaler. Andra sensorer och metoder behövs därför för att lara denna uppgift. I detta eamensarbete har en plattform byggd på ett chassi linande SPIO II tagits fram. Därför linar farostens dynami SPIO II och systemet är i viss grad anpassat för en ommersialisering. Farosten allas SPIO III förortning för SPaning I det Omedelbara ärområdet, se figur.. SPIO III har onstruerats som en sensorplattform med en bärbar dator och sensorer för navigering och hinderdetetering. Samtliga omponenter som använts är av budgettyp med lägre prestanda. För att erhålla ett prisvärt system tillförs omple signalbehandling och styr- och reglerfuntioner vila medför en total prestandaförbättring. Figur.3. SPIO III systemöversit. Den UV som framtagits i eamensarbetet har till sillnad från tidigare arbeten ingen möjlighet till fjärrstyrning, då fous har varit att studera navigering och autonomitet. Farosten manövreras därför från en PC på plattformen. SPIO III är utrustad med tröghetssensorer i form av gyron och accelerometrar. Även referenssensorer som PS och magnetometrar ingår. Dessutom använder sig SPIO III av dödräning med tachometersensorer samt ultraljudssensorer för hinderdetetering. Från sensordata som sicas till UV-datorn i realtid estimeras flera tillstånd för farosten, bland annat position och urs utifrån vila farosten styrs. Hela systemet har implementerats i Matlab. Initialt identifierades flera delproblem i designarbetet, de var hårdvaruonstrution, mjuvara för datatransmission, estimering av tillstånd, reglering och hinderdetetering. De metoder som 3
utvärderats är navigering med almanfilter, partielfilter och hinderdetetering med vetorfältstyrning.. Avgränsningar Farosten förutsätts färdas i ett loalt område. Området är änt så tillvida att en arta finns tillgänglig för utmarering av initial position. Positioneringen ser endast i UM-oordinater eng. Universal ransverse Mercator, se bilaga, förflyttning i höjdled antas obefintlig men däremot förutsätts maren unna luta inom rimliga intervall..2 Rapportens disposition Som introdution till de övriga apitlen finns i apitel 2 en översit av systemet och i apitel 3 redogörs för hårdvaruonstrutionen av farosten. I apitel 4 definieras de olia oordinatsystemen som används för att besriva ritningar och storheter. Sensorerna med modeller presenteras i apitel 5 och övriga modeller besrivande farostens dynami återges i apitel 6. Rapporten fortsätter med teori för de båda estimeringsmetoderna almanfilter och partielfilter i apitel 7. Kapitel 8 behandlar de navigeringssystemen baserade på estimeringsmetoderna i apitel 7. Styrsystemet, inluderande reglering och ollisionshantering, besrivs i apitel 9. n redovisning av tester tillsammans med utvärderingar finns i apitel och avslutningsvis sammanfattas eamensarbetet i apitel där även förslag på fortsatt arbete presenteras. I bilaga A besrivs den notation som har använts och en fördjupning av PS och jordmodeller finns i bilaga. 4
2 SYSMÖVRSIK avigering av farosten ser med data från sensorer. Data lagras undan för att unna återsapa förloppet senare i utvärderings- och utveclingssyfte. Utifrån den beränade positionen från navigeringssystemet sall en styrsignal genereras. Den beror på den förprogrammerade rutten, som an fås fram genom att välja punter ur en arta. n översit av systemet återges i figur 2.. Datorn som används är en laptop placerad på farosten och sensorer innefattar PS, magnetometrar, accelerometrar, gyron, tachometrar och ultraljudssensorer. I ett senare sede an datorn ersättas av en mindre typ, till eempel en dedicerad dator. vå alternativ till navigeringssystem har tagits fram; ett decentraliserat almanfilter och ett centraliserat partielfilter. Figur2.. Översitavdeoliadelsystemen. Utveclingsarbetet började med onstrution av farosten för att sedan gå över till analys av sensorerna. fter det tog arbete med de båda varianterna av navigeringssystem vid. ill sist utveclades ett styrsystem med reglering och ollisionshantering. De olia blocen behandlas i denna rapport i separata apitel. 5
6
3 SYSMKOSRUKIO I följande apitel redogörs för onstrutionen av systemet vilet innefattar farost, hårdvara och datainsamling tillsammans med designöverväganden. Målet är att få en tillförlitlig och tillräclig ommuniation mellan dator, sensorer och atuatorer. De ingående delarna i systemet visas i figur 3.. Figur 3. Översit av systemets hårdvara. 3. Designfilosofi Farosten har onstruerats för utvecling av algoritmer och användarvänlighet har prioriterats i ombination med ostnad. Inga rav på vädertålighet har funnits. tt modulärt tänande har använts för att underlätta framtida behov av byte av ensilda sensorer och omponenter. 3.2 Plattform På farosten finns plats för sensorer som används för navigering och detetering av hinder, men även datorapacitet för beräningar och styralgoritmer. Då Snoen II till sitt format är uppsattad av sina användare beslutades att plattformen som används i det här eamensarbetet sulle ha linande egensaper vad gäller dynami och storle. Detta för att förbättra möjligheten till integration i systemet för Snoen II i framtiden, men även underlätta byggandet genom att unna ta tillvara på tidigare erfarenheter. 7
Farosten som valdes är baserad på Kyosho Mad Force II [3], vilen har modifierats meanist för vårt ändamål. Framdrivning ser med en eletris trefasmotor som styrs av ett fartreglage. Styrservot är opplat till farostens framhjul. åde fartreglage och styrservo styrs med pulsbreddsmodulering. Montering av sensorer och annat materiel görs på en fiturplattform bestående av en aluminiumplåt som sruvats fast i farostens chassi. 3.3 Sensorer Farosten innehåller ett antal sensorer för navigering såsom PS, magnetometrar, accelerometrar, gyron och tachometrar men även US-sensorer ultraljudssensorer, eng. Ultra Sonic för att unna detetera hinder. För ändamålet används sensorer av olia aratär och informationen vägs samman med sensorfusion. n sensor som har god precision på ort sit ompletteras med fördel med en sensor som anse har ett större fel ränat på ort sit men som sanar drift. edan motiveras varför de olia sensorerna har valts och i apitel 5 besrivs de olia sensorerna mer ingående. Positionen bestäms utifrån PS och dödräning. amnet dödräning ommer troligen ifrån engelsans dead reconing som ursprungligen stavades ded reconing och är en förortning av deduced reconing. För att utföra dödräningen behövs information om urs och farostens förflyttade sträca. Sträcan beränas med hjälp av vinelhastighetsmätare på hjulen och ursen i huvudsa av en magnetompass som behöver stödjas med vetsap om farostens lutning. PS-mottagaren som används är en handburen armin ere Venture, [4], med en seriedatautgång via RS-232. Den har en display som gör det lätt att se valiteten på satellitmottagningen. n magnetompass HMR33 [5], med inbyggd lutningsompensering med accelerometrar införsaffades, men då accelerometrarnas detetion av gravitationsfältet störs av accelerationer vid fartförändring och sväng beslutades att endast använda magnetometerdata och göra en etern luntningsompensering. ill sattning av lutning används en tvåalad accelerometer, Memsic 225,[6], och två gyron av typ CRS4,[7]. Vinelhastighetsmätarna på de båda främre hjulen består av optisa sensorer,[8] som änner av fälten i en hålsiva roterandes med hjulet. Hålsivan har åtta avänningsytor per varv. Ultraljudssensorerna är av typerna SRF8 och SRF, [9]. 3.4 Sensorort vå processorer av typen asicatom med tillhörande multiappliationsort används för förbehandling av data [8]. Fördelen med dessa är att de lätt an programmeras från en vanlig PC med den medföljande programvaran i språet PASIC []. Språet är specialutveclat och innehåller högnivåfuntioner av avgörande betydelse i vår appliation, såsom instrutionsset för serieommuniation, I2C och pulsbreddsmodulering. Processorn an även generera avbrott och har stöd för ontinuerlig servopulsgenerering. Inspiration till serieommuniation mellan asicatom och Matlab hämtades från []. 3.4. Mjuvara yrodata räver högst samplingsfrevens och funtioner för det distribueras därför till ett separat ort tillsammans med överföring av styrommandon. På det andra ortet tas tachometerdata, accelerometerdata och US-sensordata om hand vilet illustrerades i figur 3.. 8
Vilen typ av förbehandling som ser för respetive sensor på sensororten listas i tabell 3. nedan. Sensor Utdata sensor Förbehandling av data från sensorort achometer eller beroende på om ljus refleteras eller inte tt avbrott genereras för varje omslag till. iden sedan förra omslaget ränas ut och yro Accelerometer Analog signal proportionell mot vinelhastigheten Pulsbreddsmodulerad signal med pulsbredd proportionell mot mätt specifi raft. sicas vidare till datorn. A/D omvandling och medelvärdesbildning av tre värden. Pulsbredden beränas. US-sensor Avstånd i cm Ingen förbehandling abell 3. Sensorortens förbehandling av sensordata. otera att data från tachometrarna är tiden sedan förra omslaget och inte en ränare av hur många omslag som har sett. Detta är för att varje paet som ommer fram till datorn självständigt sa innehålla tillräclig information om farostens fart. Uppgiften att behandla och sica vidare data orsaar ett realtidsproblem. tt vitigt rav är att all sampling måste vara periodis och ha små gitter. Data får ej heller acumuleras eller förloras. Det löstes med en schemaläggning av round robin-typ. Då det är utom eamensarbetets omfång att optimera realtidsod har ej systematisa metoder för schemaläggning använts. Hänsyn har doc tagits till så allad cross-tal [2] för USsensorerna och samtidigt har maimal samplingshastighet för alla sensorer eftersträvats. Programmeringen av sensororten har därför gjorts ad hoc i små steg genom mätningar och tester för att erhålla så små gitter som möjligt vid sampling och dataöverförning. 3.5 Dator n liten bärbar PC utför de nödvändiga beräningarna och finns monterad på farosten. Det underlättar mycet i utveclingssyfte då programvaran an programmeras om på plats och data utvärderas diret under testörningar. Den enda programvaran som används är Matlab. 3.5. Datainsamling Kommuniationen mellan dator och de utomstående delarna ser med RS-232 via adapter till datorns US-portar. Programvaran i Matlab bygger på funtionsanrop som genereras då data på en viss port finns tillgängligt. För de båda sensororten, som sicar data ontinuerligt, innebär det att data tas om hand när indatabufferten är full. Data från magnetometrarna hämtas med ett bestämt tidsintervall av en timerfuntion och PS-data tas om hand så fort nytt data ommit in. För sensororten innebär detta att samplingstiden an minsas med en 9
mindre buffert, men en större buffert öar säerhet i dataöverförning då flera mätningar av samma slag hinner omma in i varje tidssteg. Viss databehandling görs alltså även av Matlab innan navigeringsalgoritmerna tar vid för att förfiltrera värden. 3.6 Samplingshastigheter n förutsättning för att få systemet att fungera är att samplingstiderna för datainsamling är acceptabla. Högst prioritet på låg samplingstid har de relativa sensorerna gyro och tachometrar. I tabell 3.2 redogörs för uppnådda samplingstider tillsammans med ommentarer ring resultatet. Sensor Sampelfrevens Hz Kommentar achometer 5- eroende på fart yro 2 Detta inluderas delvis integration i sensorortet. Accelerometer 5-6 US-sensorer 4 Magnetometrar 2 Samplingstid går att välja med Matlabs timerfuntion som hämtar ett värde då den anropas. PS nligtgränssnitthospsmottagaren. abell 3.2 Systemets sampelhastigheter.
4 KOORDIASYSM För att enelt unna besriva farostens rörelser behövs ett antal olia oordinatsystem, så allade frames. Relationerna mellan dessa oordinatsystem besrivs av DCM-samband eng. Direction Cosine Matri; rotationsmatriser. I detta apitel definieras oordinatsystemen och dess transformationer. etecningarna är sådana att en vetor, r, uttryct i oordinatsystemet A an transformeras till oordinatsystem med A r C A r. 4. ranfrormationsmatriserna är ortonormerade O baser och därför an r uttrycas i r A enligt r A A C r C A r. 4.2 4. eodetic frame eodetic frame, O, har origo i farostens centrum och alarna peandes åt norr, öster respetive nedåt. Det allas även ofta D-system eng. orth, ast, Down och som ett eempel blir gravitationen uttryct i O alltså g g z där g 2 z 9.8 m / s. 4.3 4.2 evel frame evel frame, V, har lisom O z-omponenten ritad nedåt I gravitationens ritning och origo i farostens centrum men är ritad i farostens färdritning och y åt höger vilet illustreras i figur 4.. Sambandet mellan V och O definieras av ursen enligt cos sin C sin cos. 4.4
Figur 4.. Sambandet mellan oordinatsystemen O och V besrivs utifrån ursen,. 4.3 ody frame ody frame, ODY, är ett roppsfast system i farosten med peandes framåt på farosten, y åt höger och z nedåt. Det är detta system som ompass, tachometrar, gyro och accelerometrar förutsätts mäta i. ODY relateras till O med eulervinlarna roll-, tipp- och ursvinlarna enligt C cos sin cos sin cos sin sin cos sin cos sin cos. 4.5 Resterande samband an tas fram utifrån de redan nämnda. ill eempel an ODY uttrycas utifrån V enligt r C r, där C C C C C cos sin cos sin cos sin sin. 4.6 cos 4.4 Positionsangivelse Farostens position anges i meter i nordlig och östlig position, northing och easting, där origo är lagt enligt UM-onverteringen besriven i bilaga. Ingen höjdangivelse görs. För beränande av position används storheter angivna i O eftersom, och, y endast siljer sig åt genom en förflyttning av origo. 2
4.5 Sevoperatorn Vinelhastigheten för oordinatsystem A:s rotation relativt oordinatsystem uttryct i A:s A oordinater betecnas A. För att enelt unna hantera sådana rotationer införs sevoperatorn,, som definieras enligt sambandet [3] A A r A A A r A 4.7 där r är en godtyclig vetor. Utränat blir det A A r A A A r A Az A Ay A A A A A Az A A A Ay Az r r r A A y A z A Ay A A A A A A r A ry A rz r A Ay z A Azr A Ary A A A r A Az y A Arz A Ayr 4.8 vilet ger att A A Az Ay A A A A Az A. 4.9 A A Ay A Derivatan av en DCM-matris an nu uttrycas utifrån definitionen för hastighet vid cirulär A rörelse, r A r, enligt C A A A C A C C. 4. A A A edan visas ytterligare några egensaper hos den sevsymmetrisa operatorn: A A AC A C 4. 3
4
5 SSORMODR Förståelse för vad sensorerna egentligen mäter behövs för att unna använda informationen på rätt sätt. I följande apitel besrivs allmänna sensorfel och de olia sensorerna redogörs för mer ingående med alibreringsförfarande och sensormodeller. 5. Sensorfel Informationensomerhållsfrånensensorärinteendiretöversättningavdenverliga storheten utan innehåller en del fel. fter studerande av datablad har arbetet begränsats till att studera biasfel, salfel och normalfördelat brus. Sensorerna an även innehålla andra fel som diverse olinjäriteter och vantiseringsfel men dessa fel ommer ej att behandlas då de enligt specifiationer är så små att de an baas in i övriga fel. Mätningen av den allmänna fysialisa storheten an alltså modelleras som ~. 5. Ovanstående nämnda fel gäller för ensilda sensorer. Då sensorer verar tillsammans inverar även fel i ortogonalitet vid sensormonteringen. Hur det uppommer visas i figur 5.. ~ y y ~ Figur 5. Illustration över ortogonalitetsfel. Då felvinlarna är små < för accelerometern an mätningen srivas som ~ cos ~ y sin sin cos y y y y. 5.2 och eftersom de atuella mätområdena är relativt onstanta ommer ortogonalitetsfel ej att modelleras utan till största delen tas om hand av biasfelet. Sensorerna ommer därför att antas sitta ortogonalt och mäta i oordinatsystemet ODY. 5
5.2 Magnetometrar Magnetfält mäts med magnetometrar. Då en magnetometer mäter det magnetisa flödet i en ritning används tre stycen för att mäta på magnetfältets storle och ritning ring och igenom farosten. Kompensering för stationära magnetfält i farosten görs innan utdata används för att bestämma farostens urs. 5.2. Mätprincip Den huvudsaliga ällan till jordens magnetfält är eletrisa strömmar i jordens yttre ärna vilet gör att jorden blir en stor dipol. Det så allade primärfältet varierar långsamt med tiden, vilet påverar både intensitet och de magnetisa polernas läge gentemot de geografisa. Det finns två globala modeller som besriver primärfältet och dess förändringar, World Magnetic Model WMM och International eomagnetic Reference Field IRF. Då de magnetisa polerna ej ligger vid de geografisa behövs ett mått på var den geografisa nordpolen är relativt den magnetisa uttryct i vinelfel från platsen man befinner sig. Avvielsen benämns delination och an ränas ut med hjälp av WMM [4]. Sommaren 25 var den magnetisa delinationen 3.9 i inöping. 5.2.2 Felällor Fel i mätning av magnetfält består av sensorfel såsom salfel och biasfel. Magnetfältet som mäts är det totala magnetfältet, vilet innefattar både det jordmagnetisa fältet och den avböjning som farosten orsaar. Avböjningen som uppommer allas för hårdjärnsfel och bidrar med ett onstant magnetfält ring farosten adderat till det jordmagnetisa fältet. Då det är onstant ommer det att yttra sig som bias på magnetometrarna och i detta fall alibreras bort. Då magnetometrarna endast ommer att användas för att bestämma ritning hos magnetfält är endast magnetometrarnas relativa värde av vit, salfatorfel bortses därför ifrån. Fatorer i omvärlden som an orsaa fel i magnetfältsmätningen är till eempel brunnsloc och elledningar. 5.2.3 Kalibrering iasfel hos magnetometrarna och det fält som genereras av bilen an ompenseras för med alibrering. I alibreringen fås det totala biasfelet fram, dvs både sensorfel och magnetfältet som är onstant relativt fordonet. Det görs enlast genom att samla in magnetometerdata från en plan cirelbana och plotta -värdena mot y-värdena. Då inget biasfel är närvarande ommer värdena bilda en cirel centrerad ring origo, annars ommer centrum hamna i b,b y därb motsvarar biasfelet i -led och b y iy-led. För att illustrera och testa behovet av alibrering monterades magnetometrarna på en rullvagn av metall som orsaar stort hårdjärnsfel. Som testbana användes en tennisbana för att få ett plant underlag och raa, vinelräta linjer att följa som referens. Vagnen rullades längs orridoren på tennisbanans ena sida och sedan längs nätet till andra orridoren som följdes runt för att till slut omma tillbaa till startpunten vilet visas i figur 5.2 a. 6
Med ritning utränad från magnetometerdata i - och y-led och antagande om onstant fart an vägen plottas med dödräning. Det syns tydligt i figur 5.2 b att den utränade ritningen inte är orret då linjer som borde vara parallella ej är det. fter alibrering för hårdjärnsfel hos vagnen fås det önsade utseendet hos urvan med parallella linjer och räta vinlar vilet visas i figur 5.2c. Kaliberingen utförs med rotation ring alla tre alar för att få fram ompenseringsvärden i alla tre ritningar. a b c Figur 5.2. a esrivning av testbana. b Dödräning utifrån magnetometerdata. c Dödräning utifrån magnetometerdata efter alibrering. 5.2.4 Modell Utdata från magnetometrarna modelleras som m ~ m mag C m mag. 5.3 5.3 Accelerometrar n accelerometer mäter specifi raft i en viss ritning. Den specifia raften består både av gravitationsraft och av acceleration orsaad av farostens rörelser. I denna appliation är syftet med accelerometrar att detetera lutning vilet görs med beräning av gravitationsraftens ritning relativt farosten. 5.3. Mätprincip Accelerometern som används innehåller två ortogonala mätalar orienterade i och y. Accelerometrarna använder sig av termis avänning där det inte behövs några lösa delar. I mitten av ett isolerat hålrum fyllt med gas finns en värmeälla och på var sida i änslighetsritningen ring den sitter temperaturavännare. Då ingen acceleration finns är temperaturen på de båda sidorna lia, men vid specifi raft i viss ritning fås en temperatursillnad beroende på osymmetris värmeöverföring. 7
5.3.2 Kalibrering Sensorerna avger signalen med pulsbreddsmodulering och sensorortet vidarebefordrar informationen i form av en storhet proportionell mot mätt raft. n enel alibrering gjordes med mätningar med änslighetsaeln vertialt och horisontellt. Mätpunterna vid de förväntade rafterna bands samman med en rät linje, ur vilen bias och salfatorfel beränades. 5.3.3 Modell Den specifia raft f som mäts består både av gravitationsraft g och av farostens acceleration a enligt f g a C g a. 5.4 Modellen för utsignalen efter alibrering blir ~ f f f. 5.5 5.4 yro tt rategyro mäter vinelhastighet ring en viss ael och integration av vinelhastigheten an användas som en vridningsvinel. 5.4. Mätprincip De gyron som används är rate-gyron, [5], som mäter vinelhastighet ring sin änslighetsael med hjälp av coriolieffeten. Vinelhastigheten som mäts är relativt ett inertialsystem fit avlägsna stjärnor. Rotationen är uppbyggd av flera omponenter då farosten rör sig relativt maren, jorden roterar ring sin ael och så vidare. 5.4.2 Felällor De stora felällorna hos gyrona är brus och biasfel. Visst vantiseringsfel föreligger även på grund av A/D-omvandlingen. ias och salfator an mätas genom alibrering enligt nedan, men då de är temperaturberoende är en statis ompensering ej tillräclig. Variationer i salfator an antas ha ett långsamt förlopp och därför bör en dynamis sattning och ompensering av biasfelet vara tillräclig. Av rotationen som nämns i 5.4., är farostens rotation relativt maren intressant. Jordens rotation relativt inertialsystemet an ses som ett mycet litet biasfel då storleen på 5 /timme är för liten för att deteteras med dessa gyron. 8
5.4.3 Kalibrering För att säerställa prestanda hos de rategyron som öpts in och satta de parametrar som definierar utsignalen gjordes en gyroalibrering. Kalibreringen gjordes på ett vridbord i Aerotechelubs gyrolaboratorium. Vridbordet har en platta som roterar med en onstant vinelhastighet, vilen är änd med hög noggrannhet. Mätningar vid ett antal olia vinelhastigheter gjordes och data studerades för att notera om antagen felarateristi var orret. n variation av salfatorfelet på cira 4% noterades men då det inte var större an det försummas och antas bli försvinnande litet relativt övriga fel. 5.4.4 Modell yrona mäter rotation ring alarna och y, det vill säga rotationen mellan ODY och V. Den mätta signalen modelleras enligt ~ gyro gyro, 5.6 där biasfel är det som återstår efter alibrering. 5.5 achometrar achometrar mäter hjulens rotationer och utifrån de an farostens förflyttning beränas. 5.5. Mätprincip n infraröd lysdiod monterad på hjulaeln sänder ut ljus som refleteras i en hålsiva som roterar med hjulet. Det refleterade ljuset deteteras av en fototransistor som returnerar en disret signal då ljus refleteras, det vill säga vid ice-hål. 5.5.2 Felällor Varje övergång mellan ice-hål till hål i hålsivan genererar ett avbrott eng. interrupt i sensorortets processor och övergången tidsmärs. iden från senaste detetionen ränas ut och tidsdifferensen sicas vidare till datorn. idsenheten är änd så när som på salfatorn och enstaa omslag an missas i dataöverföringen. Avvielser i hålbredd på hålsivan an ocså föreomma. För uträning av navigeringsstorheter är vetsap om hjulradier av stor betydelse. Fel i dessa orsaar drift i efterföljande beräningar. Fel i hjulradier och fel i tidssalfator orsaar tillsammans en salfator. 5.5.3 Modell Med åtta omslag per varv an tidsdifferenserna ränas om till vinelhastigheter på höger hjul, h, respetive vänster hjul, v, enligt 9
2 v, h. 5.7 8 t v, h Den mätta vinelhastigheten modelleras med brus och salfator enligt ~, v h v, h. 5.8 5.6 PS Förortningen PS står för lobal Positioning System och baserar sin positionering utifrån 24 satelliter. Med hjälp av signaler från dessa an en PS-mottagare räna ut sin position i ett givet referenssystem. Mer om referenssystem finns i bilaga. 5.6. Mätprincip Positionen beränas med hjälp av transporttiderna för signalerna som beror på avståndet till satelliterna. Det behövs ontat med minst fyra satelliter för att beräning av position sa unna se. 5.6.2 Felällor n PS-mätning är lisom andra mätningar behäftade med fel och osäerheter. De tre huvudfatorerna är DOP eng. Dilution Of Precision, flervägsutbredning eng. multipathing och mottagarens osäerhet. DOP-värdet är en osäerhetsfator beränad ur de använda satelliternas positioner jämfört med mottagaren. Ju mindre värde på DOP desto högre noggrannhet an uppnås. Osäerhetsvärdet är modellbaserat och beränas av mottagaren. I bebyggt område fås problem med flervägsutbredning då signalernas färdväg förlängs vid refletion i eempelvis byggnader. PS-noggrannheten är även beroende på prestanda hos mottagaren och valiteten på insignalerna. Osäerheten an modelleras på ytterligare sätt. empelvis om PS-mottagaren befinner sig nära en vertial begränsningsyta öar osäerheten i ritning vinelrät mot begränsningsplanet [6]. Positionen anges i WS84 longitud och latitud. Dessa onverteras därefter till northing,, och easting,. På grund av att disreta mätvärden fås ut av mottagaren blir signalen vantiserad. Omränad till och motsvarar vantiseringen cira en meter. n annan nacdel med PS-mottagaren är att den filtrerar data innan den sicas ut. Därför är utsignalerna beroende. Problem som eventuellt unde uppomma löses genom att använda PS-mätningarna mer sällan enligt disussionen som uppommer i 8..2. 2
5.6.3 Modell Om mätvärden inte används för ofta an felet i PS-position modelleras med vitt brus där variansen på bruset an varieras med DOP-värdet. Detta ger ~ ~ PS PS. 5.9 5.7 US-sensorer Ultraljudssensorerna är främst ämnade för hinderdetetering men an även användas för artreferensnavigering. Sensorerna är av typen sonar för användning i luft. 5.7. Mätprincip n högfrevent ljudpuls sänds ut, pulsen studsar alltid mot objet, om den refleterade pulsen överstiger en viss trösel lagras ljudpulsens transporttid. ransporttiden mellan utsändning och mottagning är proportionell mot avståndet. Ultraljudets egensaper är sådan att vågloben är relativt smal. Dessutom ger den orta våglängden relativt god upplösning, det vill säga den studsar mot mindre objet och är änslig för detetion av ytor som inte är vinelräta mot infallande vågfront. 5.7.2 Felällor Dämpning och flervägsutbredning av ljudpulsen som ger upphov till uteblivet eo är en älla till fel och försämrad prestanda. Mätsignalen är ett avstånd mätt i centimeter vilet har ett vantiseringsfel på cm. Uppmätt sträca påveras av variationer i ljudets hastighet i luft vilen varierar med temperaturen. Under normala förhållanden är dessa variationer små och ommer ej att beatas. Andra felällor an omma från dataöverföringsfel. 5.7.3 Modell Uppmätt avstånd levereras från respetive sensor i enheten centimeter enligt ~ d d US. 5. 5.8 Sammanställning av sensormodeller I tabell 5. listas sensormodellerna framtagna i det här apitlet. 2
Sensor Modell v.nr. Magnetometrar m ~ m mag C m 5.3 mag Accelerometrar ~ f f 5.5 f yron ~ gyro 5.6 gyro achometrar ~ 5.8 PS US-sensorer v, h v, h ~ PS 5.9 ~ PS d ~ d 5. US abell 5.. Sammanställning av sensormodellerna. 22
6 FARKOSMODR ra modeller är av vit i filtreringsproblem. Med en bra modell menas att den sall stämma överens med verligheten, men även numeris stabilitet är ett riterium. Olia modeller har provats men här återges endast de som använts. 6. Rörelsemodell Farostens position anges i och och en modell för förflyttningen an härledas utifrån figur 6. där farten, v, är ritad i ursritning och an ses som en onstant vinelhastighet ring ett rotationscentrum i varje tidsintervall. etecning motsvarar tiden mellan sampel + och sampel. Figur 6.. Illustration till härledning av rörelsemodell. Den förflyttade sträcan, s, mellan två samplingstillfällen fås med trigonometri ur figuren till 2 v s sin 2 och ritningen som förflyttningen har sett i fås med liformiga trianglar till 2 Rörelsemodellen an nu srivas som 6.. där w pos 2v cos sin 2 2 w 2v sin sin 2 2 betecnar processbrus. pos, 6.2 23
6.. Odometrisa beräningar Utifrån vinelhastigheterna hos hjulen, v, h, mätta av tachometrarna an ett antal storheter som behövs i meaniseringen av rörelsen ränas fram. Farten hos höger respetive vänster hjul ränas fram enligt v v v h r v h v r h. 6.3 där r v och r h är radien på vänster respetive höger hjul. Då bilen svänger orsaas en vinelhastighet z som an ses som en rotation ring en punt i farostens plan enligt figur 6.2 där b betecnar bredden hos farosten och hjulens vinelutslag. Figur 6.2. Rotation av farosten orsaad av förflyttning ftersom vinelhastigheten hos höger och vänster sida är lia stor och förhållandet mellan och Rv är änt an vinelhastigheten ränas ut enligt Rh v h R z h R v vv b cos z vv v b cos b cos vrv hrh b cos h z. 6.4 24
Rotation av hjulen orsaar en hastighet framåt men ingen i sidled. Däremot har farosten acceleration både framåt, orsaad av fartförändring, och i sidled orsaad av den cirulära rörelsen, vilet ger v v v v 2 h vrv hr 2 h 6.5 v y 6.6 a v 6.7 a y v. 6.8 z 6.2 Modell över styrsignalens påveran av urs Då styrsignalen som sicas ut är änd an den användas till att beräna. För härledning används en modell av en fyrhjuling med styrning på framhjulen. yngdpunten, vilen är den punt som farostens rörelse uttrycs i, approimeras vara i farostens centrum. I figur 6.3a ses en siss över en fyrhjuling med införda betecningar där anges relativt O och anges relativt ODY. Den an approimeras med den tvåhjuling enligt [7] som visas i figur 6.3b där vinlarna, 2, 3 och 4 motsvarar 5 och 6. a b Figur 6.3. a Modell för en fyrhjuling b Modell förenlad till tvåhjuling. Då farostens maimala styrutslag är litet och bilens längd är stor relativt styrvineln an bahjulens hastighetsomposant i sidled approimeras till noll. Även vineln approimeras vara noll. u an en modell för hur styrsignalen påverar vinelhastigheten tas fram och en principsiss för detta finns i figur 6.4. De ljusa cirlarna representerar gammal position för fram respetive ba på bilen och de möra ny position. Hastigheten i färdritningen betecnas v och styrvineln betecnas med. 25
Figur 6.4. Illustration till härledning av styrsignalens påveran av ursen. Vinelhastigheten och styrsignalen :s inveran på denna modelleras utifrån figur 6.4. Utgående från definition v r med avseende på bahjulen, som antas vara fi i sidled, ger v sin w. 6.9 l l 2 2 Modellen är giltig för små och maimalt styrutslag är ±. 7 6.2. Modell styrning Den meanisa onstrutionen för styrning av hjulen är av principen i figur 6.5. tt digitalt servo arbetar inom vinelområdet och eftersom ett så stort vinelområde för 2 2 servot används an det inte approimeras som linjärt. Funtionen från servovinel, vilet är den styrsignal som används, till hjulvinel är rs sin arcsin. 6. rst 26
Figur 6.5. Meanis besrivning för styrningen av hjulen, ljusa ellipser representerar hjulen, små runda cirlar representerar leder. För att sydda det änsliga servot mot etrem påfrestning sitter en hårt spänd fjäder som ger via endast vid raftig belastning. Vid normal belastning överförs doc raft till hjulens vridning statist. Servot ger alltid en lia stor raft och vid olia underlag verar vridfrition på däcet. Det i ombination med olia hastigheter ger styrningen en dynami som siljer sig mellan olia underlag och farter. 6.3 Modell för ompression av hjulradier Hastigheten med vilen varje hjul förflyttar sig är beroende av hjulradien. Då hjulens onstrution är av svagt pumpad gummi deformeras de raftigt ett relativt mått vid öad belastning uppifrån. Specifit är fallet så vid urvtagning och speciellt vid höga farter och liten svängradie. Farosten tenderar då att luta utåt i svängen, vilet får yttre hjulparet att deformeras medan det inre hjulparet återfår den form det har vid avsanad av belastning. Figur 6.6. Illustration över hjulompressioner vid sväng. betecnar normalrafter verande på de båda hjulen vid a ej sväng och b sväng. Hjulradierna an enligt [8] uttrycas med 27
r r v h r r c v c v där onstanten c sattas offline., 6. 6.4 Attityd och högre ordningens dynami De modeller som presenteras nedan används endast i helhetsmodellen i partielfiltret. För att ringgå loopar i modellen har termer med högre ordningens derivata ansatts endast med brus. Alla införda brustermer antas vara gaussist normalfördelade. I evationerna besrivs den ansatta modellen för tippvinel och tippvinelhastighet. ulers metod har använts och liformig sampling har antagits i samtliga evationer. Ansatt modell för tippvinel och dess derivata blir. 6.2 På samma sätt fås för rollvineln. 6.3 nligt styce 5.4.2 bör biasfel hos gyrona sattas och modellen för gyrobias ansätts med. 6.4 Farostens acceleration har ansatts vara gaussist brus enligt a a a a, 6.5 vilet ger hastigheterna v. v 6.6 Farostens position modelleras enligt 6.2. För modellansats till används evationen 6.9 och 6.. Modell för ursvinel ansätts som n n. 6.7 n 28
29 Högre ordningens derivator än vad som behövs för opplingen till sensordata har utelämnats i modellen. Farostens dämpningssystem är olinjärt med flera dödzoner. Farosten har dessutom ett omplicerat tröghetsmoment och är upphängd med olia fjäderonstanter med både visös frition och intensitetsupplagring. Det är utom eamensarbetets omfattning att modellera dessa och de inluderas i systembrus. Påveran från fjäderonstanter inluderas i brus vars varians öas med öad hastighet och vinelhastighet. Den totala modellen som används för partielfiltret är w w l l v v v 3 2 sin 2 2 sin 2 cos 2 2 sin 2 sin 2. 6.8 6.5 idsontinuerlig och tidsdisret besrivning Vid implementering av modeller i datorn används tidsdisret besrivning av system på formen v H z w F, 6.9 där besriver tillståndsvetorn och z betecnar mätstorheterna. Då tillstånden ej är ända an dessa sattas med hjälp av en observatör. F och betecnar modellevationerna på matrisform. illståndsbesrivning an även ges på tidsontinuerlig form enligt
3 v C z w A, 6.2 där mätningarna är samplade. I de fall då besrivning av tillstånden finns givet enligt 6.2 har dessa samplas om till ett tidsdisret system enligt [9] vilet ger.! om A inverterbar C H I e A F F A A A t dt e A e 6.2
7 SIMRISORI Den teoretisa bagrunden till de två sattningsmetoderna alman- respetive partielfiltrering som används för navigering i arbetet bygger på ayesians estimeringsteori. I följande apitel presenteras först de teoretisa grunderna och därefter följer specifi teori för alman- respetive partielfilter. 7. ayesians estimeringsteori Filtrering är att satta tillstånden hos ett system givet observationer och systemmodell. För realtidstillämpningar är det vitigt att sattningen an göras reursivt. Ur rent teoretis synvinel är den reursiva lösningen till filtreringsproblemet given utifrån följande resonemang. Om px Y betecnar den efterommande eng. posterior sannolihetstätheten för tidpunt, därx ={,, 2,, } betecnar tillstånden fram till tidpunt och Y ={y, y 2,, y }mätningarna. enom att reursivt beräna marginaliseringen av den efterommande sannolihetstäthetsfuntionen erhålles den så allade filtreringstätheten p Y. Med p Y måste inte hela tillståndsvetorns historia tas i beatande och algoritmen an göras reursiv. Den generella tillståndsmodellen för är y f, u, w h, u, v 7. där y är observation, u är insignal, w är processbrus och v är mätbrus. n probablistis tillståndsmodell enligt Marov införs här för att ge en tydligare oppling till partilelfilterteorin. Där p är den initiala fördelningen, p - är övergångssannolihetsfördelningen och py marginalfördelningen. Antag att tillstånd följer en första ordningens marovmodell samt att observationerna är oberoende givet tillstånden, det vill säga p,,...,, p. 7.2 2 Då ges med ayes sats följande: 3
32. Y y Y y Y Y y Y Y Y y Y Y y Y Y y Y y Y y Y Y Y,,,, p p p p p p p p p p p p p p p p p p p p p p 7.3 Ovanstående härledning visar mätuppdateringen. Den efterommande sannolihetstätheten är observeringtätheten för mätningen multiplicerat med marginalfördelningen från tidsuppdateringen dividerat med p Y y. Mätuppdateringsevation 7.3 är en förutsättning för reursiv estimering 2. idsuppdateringen ges av n d p p p Y Y. 7.4 I ett linjärt och gaussist fall löses integralen av almanfiltret optimalt. Vid olinjära system linjäriseras och lösningen blir suboptimal. Partielfiltret approimerar sannolihetstätheten och ger optimal lösning då antalet partilar se 7.8 går mot oändligheten. 7.2 Kalmanfiltrering Kalmanfiltret är ett optimalt linjärt filter i den meningen att det minimerar variansen hos sattningsfelet för en tillståndsvetor,, utifrån mätningar av olia signaler och en signalmodell. Kalmanfiltret bygger på en reursiv metod som på tillståndsform fic sitt genombrott i en artiel av R.. Kalman 96. n begränsning hos filtret är att det är linjärt, vilet innebär att om stoastien i ett system ej låter sig besrivas av normalfördelade variabler an ett olinjärt filter åstadomma en bättre sattning. tt olinjärt system an doc linjäriseras ring sattningspunterna för att få ett linjärt system. I sådant fall allas systemet för ett KF eng. Étended Kalman Filter men något sådant har ej använts i detta eamensarbete. Fullständig härledning av almanfiltret finns i [2]. I följande apitel besrivs först evationerna som används vid almanfiltreringen och sedan besrivs ytterligare saer att täna på vid almanfiltrering.
33 7.2. idsdisret almanfilter I arbetet används tisdisreta system och därmed ett tidsdisret almanfilter. Systemet besrivs av. v H y w F 7.5 Kalmanfiltret består av två steg; tidsuppdatering och mätuppdatering. Stegen an användas på olia sätt men här används varianten att i varje tidssteg först göra en tidsuppdatering, som är en simulering ifrån föregående mätuppdatering och därefter göra en ny mätuppdatering. Detta för att mätuppdateringen här är den bästa sattningen av ett tillstånd i en viss tidpunt och det är den som används i navigeringssystemet. idsuppdateringen ses här endast som en intern variabel inom varje tidssteg. n mät- respetive tidsuppdatering består av en tillståndsvetor och dess ovariansmatris P. Först prediteras det nya tillståndet från det gamla och osäerheten för preditionen beränas. Det resulterar i den så allade tidsuppdateringen som är den bästa sattningen av ett tillstånd vid en viss tidpunt utifrån tidigare tillstånd och modellen, det vill säga utan information om mätning vid tidpunten. Då sattningen endast baseras på tidigare information allas den även för en predition och den beränas enligt ˆ ˆ F 7.6 Q F P F P. fter det vägs mätningen, y, in och innovationen,, är ett mått på felet mellan mätning och preditering ˆ H y. 7.7 Kovariansen för innovationen betecnas som. ˆ ˆ ˆ ˆ Cov R H P H y y H H H y H y 7.8 Kalmanförstärningen, K, bestämmer hur mycet av innovationen som sa vägas in i varje tidssteg för optimalitet. Den ränas ut genom R H P H H P K. 7.9 ill sist görs mätuppdateringen som används som estimat för det verliga tillståndet i den givna tidpunten,
ˆ P ˆ K I K H P. 7. 7.3 Partielfiltrering Under den senaste tioårsperioden har ett oräneligt antal artilar srivits inom området bayesians estimering och annat relaterat till partielfilter. illämpningarna inom olia områden väer ständigt. Partielfiltret an besrivas som en numeris metod för att bestämma sannolihetstäthetsfuntionen för tillståndsvetorer i icegaussisa och olinjära filtreringsproblem; det vill säga verligheten. Partielfilter är en simuleringsbaserad seventiell MonteCarlometod vila generellt an sägas vara mycet beräningsrävande. Med den snabba utveclingen av datorer är metodens framtid lovande ur många aspeter. Det har tagits fram många olia metoder för att reducera beräningsbördan och förbättra noggrannhet. Inom UV-området används partielfilter flitigt specifit för positionering, navigering ring hinder eng. collision avoidance och beslutsstöd. Filtreringsproblemet består i att givet en tillståndsmodell och oänt tillstånd satta tillståndet. Partielfiltret är en så allad SMC Seventiell Monte Carlo-metod, som i partielfilterfallet innebär att en slumpfuntion gissar ett antal olia tillståndsvetorer inom värdemängden. tt antal,, tillståndsvetorer väljs slumpmässigt utifrån en modell. Varje sådan gissning testas utifrån hur väl den stämmer överens med givna observationer. Utifrån testet får gissningen eller, den så allade, partieln en vit som talar om hur väl den överensstämmer med observationen. Detta steg bruar benämnas som mätuppdatering av filtret. idsuppdatering görs genom att propagera varje partiel genom modellevationen med modellbrusrealisering. Den partiel med högst vit, alternativt ett vitat medelvärde av alla partilar, är det giltiga estimatet. Vid nästa reursion testas de prediterade partilarna mot nästa mätning och så vidare. Vid behov omsamplas partilarna för att undvia utarmning. Återsampling innebär att de bäst överensstämmande partilarna väljs ut och blir den nya partielpopulationen. De dåligt överensstämmande ger upphov till få eller inga nya partilar, vilet illustreras i figur 7.. edan presenteras teoretisa grunder för den variant av partielfiltret som använts. Inspiration till presentationen av teorin har hämtats ur [2][22][23][24]. 34
Figur 7. Illustration av omsampling i partielfiltret modifierad figur ur [22]. De ljusa ringarna i figurens överant representerar partilarna, de möra ringarna representerar partielviter, vilet är ett sampelset representerande filterdensiteten p Y. Kurvorna representerar observeringsdensiteten förslagsfördelningen. fter testet mot denna omsamplas mätuppdatering partilarna de ljusa ringarna under de möra. Partilarna tidsuppdateras prediteras, vilet representeras av de spridda pilarna. Processen startas om vid den tredje nivån av ljusa ringar. 7.3. eori för partielfilter Partielfilter besrivs i avsnitt 7. som en approimativ beräning av integralen 7.4. mätuppdateringen 7.3 an srivas om som p Y se avsnitt 7. p y p y p p p Y Y d d 7. Ovanstående härledning visar hur den efterommande sannolihetstätheten är lielihooden för mätningen multiplicerat med den tidigare sannolihetstätheten. Vilet är en förutsättning för reursiv estimering [22]. Under ideala förhållanden gäller att perfet sampling diret från den efterommande fördelningen an åstadommas. Som en del i teorigrunderna antas att lia, identist fördelade sampel dras från p t Y t. Då ges den disreta empirisa sattningen av p i i Y. 7.2 i där motsvarar diracfuntionen. Med denna MonteCarlo-sattning an [g ] sattas enligt 35
36 i i g d p g g Y, 7.3 där i betecnar sampelinde ur fördelningen. nligt stora talens lag fås då g g onvergens säer nästan. 7.4 Det är oftast omöjligt att sampla diret från den sanna efterommande sannolihetsdensiteten p Y, vilet sulle leda till perfet sampling. Det går doc att sampla utifrån en änd förslagsfördelning eng. proposal distribution betecnad q Y. Ur 7.3 erhålles genom förlängning med q Y. q q g d q d q g d q q p p p d q g p d q p g q p p d q q p p p g d q q p g g Y Y Y Y Y Y Y Y Y Y Y Y Y Y Y Y Y Y Y Y Y 7.5 Uttrycet för är partielviterna vilet motsvarar en sampelrepresentation av marginalfördelningen py. enom att ta sampel ur förslagsfördelningen an väntevärdet [g ] beränas som en approimativ integral ~ i i i i i i i i g g g ; 7.6 där
37 j j i i ~ 7.7 är de normaliserade partielviterna. Det an ses som ett vitat medelvärde av tillstånden över partilarna. enom att behandla partielviterna reursivt som en vit i den sattade efterommande täthetsfuntionen fås med tillståndsbesrivningen 7. att, ~ q p p Y X y och i i i d p ~ Y då 7.8 ttproblemmeddennatypavbayesiansvitighetssampligärattvariansenför öar stoastist med varje uppdatering. Detta benämns under begreppet utarmning eng. depletion av partielpopulationen. tt mått på utarmningen är i Y q eff Var 2, 7.9 där det är upp till användaren att definiera den nivå där eff visar på utarmning, tröseln är en designparameter till filtret. Metoden används lämpligtvis då tillräclig beräningsraft finns tillgänglig mellan indata, det vill säga om sampelhastigheten är låg. Metoden förortas SIS eng. Sampling Importance Sampling [22]. n metod för att ringgå problemet med utarmning är att omsampla, eller som det allas mätuppdatera, vilet oftast ser naturligt då tidssampel inommer. Den grundläggande principen baom SIR Sampling Importance Resampling är att behålla partilar med hög partielvit lielihood och öa dess representation i partielpopulationen på beostnad av partilar med låg partielvit det vill säga öa antalet av dem. Algoritmen mappar om olia vitade slumpade partilar i ursprungspopulationen till en ny uppsättning med lia vitade partilar enligt Resampl. ~,, j i i. 7.2 I figur 7.2 visas principen för omsampling.
Figur 7.2 Illustration av SIR, där CDF står för cumulative density function. Inde i dras från en liformig fördelning. För att åstadomma ommappningen samplas i liformigt från den disreta uppsättningen ; i,..., med sannolihet ~ i w ; i,...,. I figur 7.2 ses hur det uniforma sampelindeet dras från fördelningen och projiceras på fördelningsomfånget efter bildandet av CDF som i sin tur ommappas till fördelningsdomänen pi. enomsärningen med domänen bildar de nya sampelindeen j och den nya vetorn accepteras som de nya samplen. På detta sätt ommer tillståndsvetorerna med de större sampelviterna att få en större representation genom multipliation efter omsamplingen med den föreommande fördelningen [22]. I SIR-algoritmen sätts förslagsfördelningen q approimativt till q Y p, 7.2 vilet är sannolihetsfördelningen för tillståndsövergång enligt marovantagandet. 7.3.2 Algoritmen för det allmänna partielfiltret edan följer en sammanfattning av algoritmen för partielfiltret, som används i navigeringen som implementerats i UV-systemet. I figur 7. visades en illustration av funtionen hos det allmänna partielfiltret. --------------------------------------------------------------------------------------- Algoritmen för det allmänna partielfiltret eng. eneric Particle Filter[2][22] --------------------------------------------------------------------------------------- Initialisering: för i =,,, initialisera partilarna enligt i ~p. i 2 Mätuppdatering för i =,,, evaluera partielviterna enligt 7.8 och normalisera. 3 idsuppdatering: för i =,, preditera nya partilar enligt i i ~ p, y det vill säga propagera varje partiel genom tillståndsmodellen för att generera nya, med modellering av bruset i tillståndsmodellen ser här diversifiering av de partilar som lonats vid omsamplingen. 4 Sätt :=+ och iterera från steg 2 ---------------------------------------------------------------------------------------- 38
7.3.3 Designparametrar n av de mest uppenbara designparametrarna när ett partielfilter implementeras är partielantalet. Ju fler partilar desto bättre approimation av den efterommande täthetsfuntionen px Y men samtidigt är det en ompromiss gentemot beräningsompleitet. eräningsbördan öar linjärt med antal partilar [25]. tt annat övervägande är beräningen av sattningen, i huvudsa finns två alternativ varav det ena använder sig av maimum aposteriori MAP, det vill säga ˆ väljs sådan att ˆ MAP arg ma p Y. 7.22 I pratien väljs således den partiel med högst vit ut som gällande estimat. Det andra alternativet är att använda minimum mean square MMS, då väljs MMS 2 ˆ arg min ˆ Y, 7.23 vilet är ett vitat medelvärde över alla partilar med respetive partielvit. De två metoderna har speciellt stor betydelse om efterommande fördelningen är bimodal, det vill säga har flera höga densitetstoppar, vilet orsaar att MMS-estimatet ofta blir sämre. I gaussist fall sammanfaller de om partielantalet är stort. 39
4
8 AVIRISSYSM Användandet av estimeringsmetoderna i apitel 7 i de båda navigeringssystemen, besrivs i det här apitlet. I det första delapitlet besrivs aspeter att ta i beatande vid implementering av de decentraliserade almanfiltren och senare modellerna som används i vart och ett av almanfiltren. Kapitlet avslutas med designöverväganden gjorda vid implementering av partielfiltret. 8. Implementering av almanfilter avigering ser med hjälp av sensorfusion av de olia sensorerna. I översiten i figur 8. visas hur navigeringsstorheterna tas fram med hjälp av sammanvägning av de olia sensorerna med almanfilter. De olia delsammanvägningarna ommer att besrivas var för sig. Inspiration till den här navigeringslösningen har hämtats från [26]. Figur 8. Översit över navigeringssystemet med almanfiltrering KF. 8.. Definition av attityd och attitydfel Farostens attityd eller ritning delas här upp i lutning och urs. ftersom almanfiltret är ett linjärt filter behöver attityd och attitydfel ges på linjär form. Detta görs genom att definiera attityd och attitydfel med transformationsmatriser enligt [3]. utning besrivs av roll- och tippvinlarna genom matrisen C och ursen med matrisen C. utning bestäms både utifrån accelerometrar och gyron och lutning utifrån respetive mätdata betecnas C a för accelerometern och C g för gyrot. n allmän sattad attityd betecnas här som C och den verliga betecnas C. Felet i attityd an då besrivas med matrisen 4
C C C 8. som an ses som en rotation med eulervinlarna y. z Då felvinlarna är små an matrisen aylorutveclas och trunering efter andra termen ger C z y z y I 8.2 och då an C beränas enligt C z y z y I 8.3 Den verliga attityden an då besrivas med hjälp av den sattade enligt C C C C I C C C 8.4 och på samma sätt an den sattade attityden uttrycas i den verliga enligt C CC C I C C. 8.5 C tt nytt srivsätt införs C C, 8.6 respetive 42
C C. 8.7 8..2 Problemet med decentraliserade almanfilter Decentraliserade almanfilter används och därmed bör vissa saer hållas i åtane. Det går inte att hur som helst modularisera decentraliserade almanfilter och oppla dem i asad. Att göra så leder till suboptimal filtrering. Anledningen är att de sattade tillstånden från almanfiltren är orrelerade. Autoorrelationen för almanfiltrets estimat ser typist ut som i figur 8.2 där det visas att data, precis som med alla filter av lågpasstyp, blir start orrelerat inom orta tidsintervall med avlingande beroende. Figur 8.2 ypis autoorrelation för en filtrerad signal. Med utseende enligt ovan finns ingen mening med att uppdatera almanfiltren med ny information oftare än autoorrelationen avlingat. Då nästlade almanfilter används är det ocså lätt att man litar för mycet på de mätningar som erhålls från det tidigare filtret, vilet leder till att sattningen an bli dålig. För att ringgå några av problemen an ovariansen för det senare filtret öas, det an doc vara svårt att veta hur mycet ovariansen sall öas. n annan åtgärd är att göra uppdateringar mer sällan ju närmre utsignalen delfiltret är. I vår tillämpning faller det sig naturligt att uppdatera mer sällan i slutet av filtret. Därmed ger eventuella problem försumbar inveran. 8..3 Modelltyper vå olia typer av modeller ommer att användas i det decentraliserade almanfiltret; meaniseringsmodeller och felmodeller. Meaniseringsmodellen används i almanfiltret för att satta de meanist sammanopplade tillstånden. Utifrån meaniseringsmodeller an felmodeller härledas, där tillstånden består av fel mellan två mätningar. Sensorfusionen som används bygger på att två mätningar av samma tillstånd jämförs, där den enasattningenbyggerpåenrelativmetodochdenandrapåenabsolut.feletmellandebåda mätningarna sattas med almanfilter baserat på felmodeller för mätningarna och den relativa 43
sattningen ompenseras med felet. Anledningen till att felen mellan mätningarna sattas är att få ett stabilare system då en storhet i sig varierar mer än dess fel. 8..4 Intrimming av brusparametrar Vid framtagandet av värden på process- och mätbrus har förhållandet mellan innovationen och dess ovarians studerats, vilet görs enlast genom att presentera dem i samma graf. n sådan graf för ett intrimmat filter visas i figur 8.3. n bra tumregel är att efter intrimmning av brusparametrarna sa innovationen hålla sig innanför ovariansens 3-gräns vid cira 9% av mätningarna. Figur 8.3 Innovationen i intrimmat almanfilter plottad med 3-gräns. 8.2 Odometrisa beräningar nligt styce 6.. an diverse storheter ränas ut från tachometerdata. Från 6.5 och utan hänsyn till olia hjulradier i styce 6.3 fås v v h r. 8.8 2 Vinelhastigheten ring z, z vrv hr b cos h z härledd i 6.4, approimeras här enligt v h r. 8.9 b illsammans med 6.7 utgör 8.8 och 8.9 meaniseringsmodellen 44
45 z h v odo odoa odov z z v r b r r b r a v a v 2 2 ~ ~, 8. som används i almanfiltret för odometrisa beräningar. I efterföljande filter behövs en felmodell för v och z. De mätta hjulrotationerna innehåller ett salfatorfel enligt 5.8 och tillsammans med eventuellt salfatorfel hos hjulradien orsaar de ett sammanslaget fel odo på hastigheten odov odo v v. 8. I positioneringsfiltret 8.55 sattas denna fator och mätningarna ompenseras med denna fator innan vidare beräningar ser. På grund av osäerheter i styrutslag och i ompressioner av hjulradier orsaa ett fel som ommer att approimeras som ett biasfel enligt odo odo z. 8.2 8.3 utningsbestämning I bestämning av lutning tas hänsyn till accelerometerdata och gyrodata vilet visas i figur 8.2. Då vinelhastigheten från gyrona behöver integreras för att få fram vinel an fel i denna vinel lätt driva iväg på grund av sensorfel. Det ommer att ompenseras för med accelerometervärden, som ej är behäftade med drift utan endast har en ortsitig felarateristi. Kalmanfiltret sattar vinelfel och bias hos gyrona.
Figur 8.4 Översit av lutningsompensering 8.3. utning utifrån accelerometerdata I avsnitt 5.3 nämns att accelerometrarna mäter den specifia raften, det vill säga både gravitationsraft och acceleration. I lutningsbestämningen är endast gravitationsraften intressant och därför an accelerationen sattad i odometrin ompenseras bort om den är så bra sattad att den tillför något. Annars betratas accelerationen som en felälla. Ur 5.4 fås f g a f a C g, 8.3 vilet ger att f f y a a y g z g sin z cos sin. 8.4 Härur an farostens roll- och tippvinel ränas ut genom f arcsin arcsin g a g f y a y. z z cos 8.5 Dessa vinlar definierar transformationsmatrisen som a C och felet a C an enligt 8.2 srivas 46
C a I. 8.6 a Detta ger att C a C a C I a C 8.7 För härledning av felmodell behövs modeller över möjliga fel och i mätningen an fel i vinlarna srivas som f f f y f y a a a a y y. 8.8 Ur enelhets- och observerbarhetssynpunt ommer alla fel ej att modelleras utan felet i lutning från accelerometerdata modelleras endast med vitt brus enligt. 8.9 a a 8.3.2 utning utifrån gyrodata yrona mäter g och gy.dåc per definition ej innehåller en rotation ring z sätts mätning ~ gz. Från 4. fås en differentialevation för oordinattransformationen enligt C C g g g 8.2 Integrationen från gyrosignal till lutning an nu, med små tidssteg, göras enligt C g g t C t I g. 8.2 För framtagandet av felmodell att använda i sammanvägningen av lutning differentieras först 8.2 vilet ger C C C. 8.22 g g Definitionen för vinelfelet enligt 8.7 ger i sin tur 47
C g C, 8.23 g insatt i 8.22 ger det C C C. 8.24 g g Derivering av 8.23 resulterar i C g g C g C g C g C. 8.25 Sedan an 8.24 och 8.25 jämföras för att få fram ett uttryc för g enligt g g g C g C C C C, 8.26 där ses som ett biasfel hos gyron enligt 5.6 och hädanefter betecnas gyro därefter. 8.3.3 Sammanvägning av lutning Sillnaden mellan lutning från gyrodata och accelerometerdata srivs som C g a C g C a C C C C g a I g a g a 8.27 I g a I g a I ga. Sillnaden i attityd mellan de båda mätningarna fås genom medelvärdesbildning ur genom g C a 48
ga gay gaz C C C g a g a g a g 3,2 C 2,3 2 g,3 C 3, 2 g 2, C,2 2 a a a. 8.28 Felvinlarna an delas upp i felvinlar för respetive sensor vilet ger ga, 8.29 g a g a som efter derivering och insättning av felmodeller 8.9 och 8.26 resulterar i C. 8.3 ga g a gyro l Detta ger följande tillståndsmodell ~ ga gyro ga C ga I. gyro ga gyro obs gyrobias 8.3 ftersom tillståndsmodellen är baserad på taylorutveclade feltillstånd är det av vit att felen inte driver iväg. Därför ompenseras gyronas attityd med sattat biasvärde. För att tillstånden sa förbli ontinuerliga, det vill säga att det sattade biasfelet inte sa påveras av återopplingen, införs återopplingen som insignal till systemet. illståndsmodellen utöas därmed med en insignal enligt ~ ga gyro ga C ga I. gyro ga gyro obs gyrobias C ˆ gyro 8.32 Vinelfelet beror till största delen av fel i gyrosattningen eftersom bruset från accelerometerdata filtreras bort. Detta ger C g I I. 8.33 g ga 49
Den sammanvägda lutningen fås genom C C C. 8.34 g g 8.4 Kursbestämning Kursen bestäms först och främst med magnetometrarna men även ursförändring deteterad i hjulen används. n översit av sammanvägningen visas i figur 8.5. Figur 8.5 Översit av ursbestämning. 8.4. utningsompensering av magnetometerdata För att beräna ursvineln utifrån magnetometerdata behövs magnetfältsomponenterna utrycta i V. m C m 8.35 Kursvineln an sedan beränas med hjälp av magnetometervärden och vetsap om delinationen, se setion 5.2., enligt m y arctan del.. 8.36 m Uttryct i ODY blir det 5
m arctan y cos m sin sin mz sin cos m cos mz sin n felmodell för ursen behövs senare och utifrån ovanstående an ursfelet srivas som del. m m m y m y m z m z. 8.37 ftersom biasfelen i magnetometrarna är små på grund av alibrering och vinlarna redan är estimerade, modelleras felet för detta tillstånd endast som vitt brus enligt mag mag. 8.38 Vinelfelet för ursen sattad med magnetometrar är enligt styce 8.. C m m m C C C I, 8.39 där m. 8.4 mag 8.4.2 Kurs från odometrisa beräningar Vinelhastigheten från de odometrisa beräningarna an ses som ett gyro med änslighetsaeln z och mäter. Vinelhastighet för ursen är och per definition oz innehåller den vetorn endast en rotationsomponent ring z vilen uttryct i mätningen blir odo oz cos cos oz. 8.4 Med återoppling av biasfelet i definierat i 8.2 och sattat i 8.47 fås odo oz cos cos oz ˆ w. 8.42 Kursen från de odometrisa beräningarna blir alltså odo odo odo t t, 8.43 med ett fel på formen 5
odo cos cos w. 8.44 8.4.3 Sammanvägning av urs Sillnaden mellan de båda sattningarna gjorda av ompass och odometri sattas i ett odo almanfilter och resultatet tolas som felet i. odo mag cos cos w. 8.45 mag Detta resulterar i cos cos mag ~. 8.46 För att förhindra drift i ursförändringar återopplas ˆ i ursberäningen och ommer in som en insignal i systemet enligt cos cos ~. cos cos ˆ 8.47 52
8.5 Positionsbestämning För bestämning av positionen används både dödräning, DR, och PS. Principsiss över hur sammanvägningen går till visas i figur 8.6. Figur 8.6 Översit av positionsbestämning. 8.5. Dödräning Dödräning är en relativ navigeringsmetod som givet en startpunt tar fram en ny position utifrån mätningar av hastighet och ritning. ill detta används här en förenlad meaniseringsmodell med endast fart och urs. Med ett antagande om små tidssteg an vinelhastigheten i evation 6.2 försummas vilet ger DR t DR t DR t DR t v cos v sin 8.48 Felet i dödräningen blir DR t DR t DR t DR t DR DR t t v DR t t v DR v v, 8.49 där felet i hastighet i sin tur beror på det totala salfatorfelet i hjulhastigheter vilet ger v v. 8.5 odo Ovanstående uttryc ger tillsammans felmodellen 53
54 odo DR t DR t odo DR t DR t v v v v sin cos cos sin. 8.5 8.5.2 Integrering av dödräning och PS Felet mellan PS-mätningarna och dödräningen bildas enligt PS DR PS DR PS DR PS DR PS DR PS DR. 8.52 Utvecling av detta och insättning av 5.9 och 8.5 ger PS odo DR t PS t DR t t PS odo DR t PS t DR t t v v v v sin cos cos sin. 8.53 ftersom det filtrerade endast består av fel i dödräningen an DR ersättas av.då fås en felmodell på tillståndsform enligt. ~ ~ sin cos cos sin sin cos cos sin PS PS t odo t t odo t odo v v v v v v v v 8.54 Vid återoppling av sattat salfatorfel odo ˆ till de odometrisa beräningarna fås en insignal till systemet enligt
55. ~ ~ ˆ sin ˆ cos sin cos cos sin sin cos cos sin PS PS t t odo odo t odo t odo v v v v v v v v v v 8.55 Den slutgiltiga positionen ränas ut med DR DR. 8.56 8.6 Implementering av Partielfilter I följande styce behandlas specifit hur partielfiltret implementerats och speciella designöverväganden och problem som hanterats. 8.6. Filteraritetur Vid uppdatering av partielfiltret, vilet onstruerats som en centraliserad variant, uppom problem då sampel från de olia sensorerna om in osynroniserat och med mycet olia samplingsintervall. Då partielfiltret är centraliserat är den optimala implementeringen att för varje nytt sampel beräna lielihood för mätningen och göra tidsuppdatering utan ny information på övriga sensorer. Sampelfrevensen för de gyron som används är ca -2 Hz. Med partilar och de 4 tillstånden i evation 6.8 blir implementeringen beräningsrävande. Det optimala är att preditera så ofta som möjligt, filtret örs så ofta som en ny mätning ommer in och partilar omsamplas med avseende på lielihood för atuellt sampel, tidsuppdatering görs därefter för alla tillstånd.
Figur 8.7 Implementering av centraliserat filter med olia datatat på indata I figur 8.7 visas tanen baom filterstruturen. llipserna representerar sensorer, de yttre pilarna visar dataflöde, mät- och tidsuppdateringar ser alltid med avseende på inommet data då det finns tillgängligt. På så sätt blir modellen inte observerbar i någon av de ensilda uppdateringarna för sig, vilet an ge onvergensproblem se avsnitt 8.6.2. Uppdateringar ser doc på ett seventiellt sätt som med stor mängd partilar och den sensorfusion som görs i partielfiltret är tänt att hantera problemet. 8.6.2 Problem vid lågt partielantal Det finns många orsaer till att ett partielfilter an ge dåliga prestanda. I avsnitt 7.3.3 nämns viten av ett stort antal partilar. Det finns många situationer där det ansatta partielantalet snabbt blir otillräcligt. Vid osynroniserade och siljande sampelfrevenser uppommer problem som måste hanteras. I realtidssattningen ommer sampel från gyron tätast och med fallande sampelfrevens till PS-mätningarna som inommer glesast enligt tabell 3.2. Problemet uppommer vid approimationen av förslagsfördelningen 7.2. Mellan till eempel PSmätningar, inommer det stoastist 3-5 sampel från andra sensorer vila filtreras. Förslagsfördelningen förvansas stoastist vid omsampling med avseende på främmande information. Det vill säga så fort ett nytt sampel inommer vilet inte tillhör atuell sensor. Det är då sannolit att lielihooden för den nya mätningen är spetsig relativt den föreommande och att den ligger i svansen av densamma. I förlängningen an det leda till divergens och mycet dåliga prestanda hos filtret, speciellt om partielantalet är litet. För att lösa problemet har tre alternativ identifierats, öat partielantal, decentraliserat filter och hjälppartielfilter eng. auiliary particle filter. Med ett hjälppartielfilter borde partielns evolution unna spåras och därmed an hänsyn tas till tidigare fördelningar på förslagsfördelningen. Den metoden har inte implementerats i arbetet och nämns i syfte att åsådliggöra möjlig lösning på problemet. 56
8.6.3 Designöverväganden I tillämpningen med navigering där vissa sensorer levererar data mer sällan och med avsanad på drift är det troligt att den efterommande fördelningen inte är gaussis. För att på ett raftfullt sätt ompensera drivande fel med onstantreferenssignaler har estimat på PS gjorts med MAP och övriga med MMS. Om en mätning som motsäger farostens dynami raftigt inommer finns en stor ris att dess lielihood blir noll eller mycet nära noll. Algoritmen omsamplar då endast en partiel den första på grund av numerisa problem och det finns stor ris att filtret divergerar på grund av för få partilar. Problemet åtgärdas genom att öa processbruset och hoppa över omsamplingssteget i algoritmen för partilefilter. Metoden som används betecnas som adaptiv och går under namnet prior editing [24]. I implementeringen har ett av målet varit att unna använda så många partilar som möjligt. Förslagsfördelningen som nämns i avsnitt 7.3 representeras i algoritm 7.3.2 av de omsamplade partilarna. Då dessa förvansas enligt 8.6.2 ger en öning av partielantalet inte lia stor betydelse för sattningsprestanda som an väntas. 57
58
9 SYRSYSM För att utföra autonom förflyttning rävs ett styrsystem som hanterar styrning, en översit av systemet visas i figur 9.. Regleringen baserar sig på nuvarande position och ritning relativt en målpunt. Denna målpunt an vara nästa waypoint eller en virtuell målpunt som ränats ut för att undvia eventuella hinder. Principerna baom de olia delarna presenteras i detta apitel. Figur 9. Översit av styrsystem. 9. Reglering Regleringen består av två huvuddelar, beräning av referensvärde och regulatorn och hur de hänger ihop visas i figur 9.2. Figur 9.2 Översit av reglersystemet 9.. eräning av referensvärde För beräning av börvärde i urs vid ett visst tillfälle används måtten X eng. cross trac error och CS eng. course to steer beränade utifrån planerad färdväg, se figur 9.3. 59
Figur 9.3 Illustration över X och CS. Där P M är målpunt och P S är startpunt. Andelen av den tillryggalagda sträcan längs färdvägen fås från definitionen av salärprodut,, och blir s P PS cos P P M s P P P P S P M P M 2 S S. 9. Med hjälp av s an X beränas enligt X P P s P P S M S 9.2 där då farost vänster om färdväg då farost höger om färdväg CS är vinelfelet mellan farostens urs,, SM, enligt och ritningen hos den önsade färdvägen, CS SM. 9.3 tt börvärde på ursen baserat på både X och CS ges av X, 9.4 ref 2 l l CS där viterna l och l 2 är designparametrar. 6
9..2 Regulator n PID-regulator används för att styra farosten mot rätt urs. Reglerfelet definieras enligt e, 9.5 ref Styrsignalen u i tidpunt beränas med u K P e K I S K D e e där S S e och S., 9.6 Styrsignalen måste begränsas med u u u u ma min om u om u om u min u u u ma min u ma 9.7 och för att undvia integratoruppvridning vid mättning av styrsignalen uppdateras det S som sicas vidare till nästa tidpunt enligt [27], vilet ger S omu umin eller u uma S. 9.8 S annars 9.2 Hantering av waypoints Innan farosten startas planeras en färdväg genom utplacering av punter i en arta eller en förprogrammerad bana. n lista över punterna, de så allade waypoints, laddas då in. Den första talar om farostens initiala position medan resterande är punter som sa passeras. Den första punten som sa passeras är punt 2, då sätts punt till startpunt och punt 2 till målpunt i styrsystemet. är punten är passerad, siftas punterna och punt 2 blir startpunt för nästa delsträca. är den sista punten i listan är passerad, stannar farosten. n punt ränas som passerad då det är mindre än m var till motsvarande sträcade linje i figur 9.4. Detta för att få en bättre ingång på nästa delsträca och minimera översläng. 6
Figur 9.4 Illustration av passering av waypoints. Villoret för passering av punt beränas med s från 9. enligt s P M P punt passerad S 9.9 9.3 Styrning med virtuella raftfält n enel men effetiv metod för att undvia ollision med hinder är att arbeta med vetorfält. n farost befinner sig i en miljö som figur 9.5. är ett hinder deteteras är den intuitiva lösningen är att styra undan från hindret i ritning I, frågan är vid vilet avstånd undanmanöver bör påbörjas och hur mycet som är lämpligt att styra undan. 9.3. Virtuella raftfält n metod för att beräna referenssignalen vid deteterat hinder allas virtuella raftfält, den presenterades först i [28]. n modifiering av den ursprungliga metoden som passar bra ihop med reglersystemet har använts. Situationen som uppstår visas i A i figur 9.5. tt hinder deteteras med vinel det relativt farostens orientering. För att beräna en ny referenssignal ansätts virtuella rafter verande på farosten. Farostens målposition ansätts agera med en attraherande raft på farosten, deteterade hinder ansätts agera med en repellerande raft vilen vitats med d där d är uppmätt avstånd till hinder. Den resulterande raften beränas enligt F Res i i F F d. 9. Att Rep I reglersystemet som använts är det lämpligt att flytta målpositionen temporärt så länge som hinder deteteras. Utflyttningen som görs är fi till en given radie från farosten sett och vineln fås från den resulterande raftens ritning enligt i figur 9.5. 62
Figur 9.5 Visualisering av vetorfältstyrning. Feta sträcade linjer representerar rafter. i c Ursprungsmetoden har ocså modifierats såtillvida att vitfuntioner d i d anpassats från inverterad sträca till mål till en tröslad eponentialvariant har i c2d i d c e t d, 9. i vilen visat sig ha bättre stabilitetsegensaper. röslingen av vitfuntionen får till effet att i det tidsdisreta fallet erhålles en stor variation i referensvärde. Om farosten ör mot målet och deteterar hinder byts ögonblicligen referenspunt, ny referenssignal ger utträde ur ritist detetionsområde, referensen byts tillbaa till ursprunglig och sevensen börjar om. röghet i systemet gör att rörelsen förblir mju även vid puntbyte. 9.3.2 Problem med potentialfältsmetoder Som nämndes tidigare är det inte helt problemfritt att arbeta med potentialfält även om metoderna i allmänhet är enla och intuitiva. I [29] besrivs några problem som an uppomma. De av störst betydelse är Stoppläge vid loala minima bestående av eempelvis återvändsgränder Ingen passage mellan tätt placerade hinder Oscillationer i närvaro av flera hinder 63
Det först nämnda problemet är det mest ända vid vetorfältssstyrning och an hanteras genom detetion och så allad bactracing. Farosten färdas då samma väg som den om in i återvändsgränden. Problemet med oscillationer visar sig doc unna lösas genom att använda andra vitfuntioner och minimera tidsfördröjningar genom snabbare sampling [2]. 64
XPRIM, SIMURIAR OCH UVÄRDRI I det här apitlet redovisas de uppnådda resultaten från det system som onstruerats och simuleringar av de algoritmer som använts. ftersom farosten är tänt att färdas i terräng är det svårt att påvisa resultaten med precision. ällande eempelvis gyrobias, roll och tipp och dessa tillstånds derivator sanas verliga värden som referens. I apitlet ommer resultat ifrån positioneringssystemet med almanfilter, partielfilter, reglering, hinderdetetering samt en jämförelse mellan de båda navigeringsmetoderna att redovisas. Kalmanfiltret används vid örning av systemet medan partielfiltret örts på lagrade, verliga data.. Allmänt om eperimenten För att unna utvärdera systemet bör på förhand ända referenspunter finnas att jämföra med. För ändamålet mättes ett antal referenspunter upp på en fotbollsplan med gräsunderlag. Deras inbördes relationer mättes upp med gyroompass och måttband. Inritade på artan visas de i figur.. vå örningar ommer att användas i utvärderingen i detta apitel; den ena refererad till som örning har gått längs punterna i figuren i nummerordning medan den andra, örning 2, har gått längs samma punter fast i omvänd ordning. Vid byte av waypoint ger farosten ifrån sig en ljudsignal och avståndet till waypoint an mätas och ritas in i positionssattningsplot. Figur. Placering av referenspunter på fotbollsplanen... Samspelet mellan positionering och reglering eroendet mellan positionering och reglering bör behandlas innan testredogörelserna tar vid. I de fall den autonoma förflyttningen ej är tillfredställande an det bero både på positionering och reglering. Det är omöjligt att styra farosten utan vetsap om position. I positionen är en noggrann urs av mycet stor betydelse eftersom det är ursen som reglerfelet till största del beränas utifrån. Dålig reglering an i sin tur försämra prestanda hos positioneringen. Då insvängningsförloppet är långsamt med stora översvängar an positioneringssystemet bli eciterat och sattningen försämrad. 65
.2 Positionering med almanfilter Positionering med almanfiltrering är decentraliserad vilet gör att delar av sattningen av farostens tillstånd an utvärderas var för sig..2. Odometrisa beräningar I almanfiltret för odometrisa beräningar sattas farostens hastighet, acceleration och vinelhastighet relativt maren utifrån data i form av framhjulens vinelhastigheter. Dessa visas för örning i figur.. Överst visas indata till almanfiltret i form av hjulens hastigheter och därunder visas de sattade variablerna. n relativt onstant hastighet estimerades med undantag från två spiar vid tiderna 6 och 85s vila an förlaras av orrupt indata. Avvielserna i hastighet mellan och 2s är orreta. Farosten färdades vid denna tidpunt upp i slänten bredvid fotbollsplanen. Därmed orsaas först en sänning i hastighet vid färd uppför och därefter följer en marant höjning i hastighet när farosten återvände ner mot fotbollsplanen. Accelerationssattningen noteras vara relativt brusig men systembruset för den är avsitligt höjd eftersom ett lägre värde orsaar att hastighetssattningen följer tillståndet allt för långsamt vid acceleration i startögonblic inbromsning. Positiva utslag ges på vinelhastigheten vid byte av waypoint, vilet stämmer överens med verligheten. 66
Figur.2 Data från örning..2.2 utningssattning n rimlig sattning av sensorplattformens lutning är vitig eftersom den raftigt påverar hela positioneringssystemet. Initialt planerades att sattningen av lutning sulle baseras på data 67
från både gyron och accelerometrar enligt styce 8.3. Det var doc inte möjligt vid verlig örning vilet har två huvudsaliga förlaringar, dels beror det på felatig data från ett av gyrona och dels på yttre förhållanden såsom vibrationer. Samplingsfrevensen av gyrosignalen är även dålig, vilet an orsaa viningsdistorsion från vibrationer. Under ideala förhållanden inomhus fungerar doc sattning av gyrobias bra. I figur.3 visas rollvinel av en sådan örning både med och utan biasåteroppling. Figur.3 utningssattning med och utan biasåteroppling under ideala förhållanden. Utomhus fungerar inte sattning av bias och därmed inte heller lutningsompensering. n orsa är att det ena gyrot, av oänd anledning, ibland levererar en onstant vinelhastighet på -5 /s, vilet är orimligt. I de fall orret data levereras fungerar ändå inte lutningssattingen tillfredsställade vilet visas i figur.4. Figuren visar en örning mellan referenspunter där farosten fastnat i en högersväng. I den första bilden visas den estimerade positionen jämfört med PS-mätningar. I fallet syns tydligt att positioneringen inte fungerar och då speciellt den estimerade ursen. ftersom farosten efter halva tiden endast svänger åt höger bör ursen öa i den andra figuren vilet ej är fallet. 68
Figur.4 empel på ej fungerande lutningssattning. Kurssattningen är felatig och vid analys av lutning inses att roll- och tippvinel bör befinna sig nära noll, eftersom farosten örs på plan mar. Det är inte fallet, se tippvinel i den tredje figuren i figur.4. Sensorfusion av gyron och accelerometrar för lutningssattningen bygger på principen att accelerometrar ger fel i form av brus medan sensorfel från gyron har biasfel. är farostens sensorplattform vibrerar translation och rotation vid örning varierar vinelfrevensen snabbare än nyqvistriteriet med sampling, viningsdistortion uppstår då. illsammans med osynroniserade mätningar uppstår svårigheter att satta bias hos gyron. yrosignalen förstör då mer än den tillför sattningen. Den största anledningen till de raftiga rotationsvibrationerna härrör från den smala aelbredden. Därför beslutades att endast använda accelerometrar för lutningssattning med almanfilter. Kompensering av accelerometerdata med farostens acceleration togs ocså bort. Det för att farosten håller en relativt låg fart och att accelerationen därmed an ses som ett litet fel i lutningssattningen, då den inte är större än.5m/s 2 vilet visas i figur.2. utningssattning baserad endast på accelerometerdata förbättrar urssattningen vilet medför en bättre positionering. Resultatet visas i figur.5. Figur.5 orttagande av gyrodata ger en bättre lutningssattning. 69
.2.3 Kursbestämning Sammanvägningen av ursen från ompassen lutningsorrigerat magnetometerdata och urs från odometrisa beräningar görs med almanfiltret för 8.47. Resultatet för örning visas i figur.6 där den heldragna linjen i första bilden visar den estimerade ursen och den strecade ompassursen. Den estimerade ursen har ett jämnare utseende vilet stämmer bättre överens med örningen än ompassens brusiga urs. I den andra bilden visas innovationen för ursfelet och därmed valiteten på intrimningen av filtret, vilen ser bra ut enligt riteriet i styce 8..4. Figur.6 Den estimerade ursen och innovationen för almanfiltret som används vid estimeringen. Data från örning. I ursbestämningen ingår en återoppling av sattat biasfel i ursbestämningen i de odometrisa beräningarna. Processbruset för biasfelet är satt väldigt lågt för att undvia överompensering och därför påverar inte återopplingen den estimerade ursen nämnvärt vilet visas i figur.7. Däremot märs en förbättring av ursen från de odometrisa beräningarna. I lutningssattningen motiveras återopplingen av att felet inte får driva iväg på grund av att ett antagande om små fel gjorts. ågot sådant antagande har ej gjorts i sattning av ursen och av den anledningen örs filtret bättre utan återoppling. 7
Figur.7 Den estimerade ursen med och utan återoppling av biasfel..2.4 Positionsbestämning Positionen ur almanfiltret är sammanvägd enligt styce 8.6. Den sattade positionen från örning visas i figur.8. Inritat finns den estimerade positionen och ringarna får symbolisera verlig position när den estimerade positionen är enligt figur. Värt att notera är att den estimerade positionen är bättre än PS-data, vilet syns vid punt 3. Vid punt 6 ligger sattningen ett par meter fel, vilet har orsaat en sväng upp i slänten bredvid fotbollsplanen, men positionsestimeringen hämtar sig därefter. stimeringen av avståndet till punt 6 stämmer ocså dåligt med verligheten på grund av återopplade salfatorfel sattade utifrån PS-mätningarna. 7
Figur.8. Presentation av örning ring referenspunter med position estimerad med almanfilter. De marerade tiderna visar sattad position jämfört med PS-data vid samma tidpunt..3 Positionering med partielfilter Vid simuleringar offline visade det sig att sattningarna från eeveringarna ofta siljde sig åt. I figur.9 visas 5 filterörningar med partilar plottade i samma graf. Dataunderlaget som användes vid simuleringarna är lia. tt mått på beräningsbördan vid simuleringarna är eeveringstiden. På en 3 Hz Pentium 4 var den genomsnittliga eeveringstiden 4 seunder. iden för farostens örning på referensbanan var 5 seunder från start i punten, =, till mål vilet är samma som startpunten. Alltså förlöpte beräningarna snabbare än realtid. I figur.9 ses hur sattningarna sprids vid olia örningar men med samma parametrar. n förlaring till det spridda resultatet an vara otillräcligt partielantal, olia brusrealiseringar och de problem som besrivits i 8.7. Men även brister i modellen för farosten, sensorerna och brus på dessa an förlara urvans utseende. Körningen görs motsols och till i början, vid det urviga beteendet, är sattningarna relativt samlade för att sedan spridas mot slutet av rasträcan. n förlaring är att systemet eciteras vid raftiga svängar då data från tachometrar, PS och magnetometrar i ombination med styrsignalen ger en bättre urvalsgrund för partilarna. tt parti med ra örning avspeglar alla osäerheter i de sensorer som används. Ingen av sensorerna motsäger således en spridning av partilarna tillräcligt start och stoastien i filtret får större utrymme och slår igenom i sattningarna. 72
Figur.9. Presentation av sattad position i örning 2 efter 5 filterörningar med samma data partilar, en så allad scatterplot. I figur. visas ursen. I vissa intervall har sattningen mycet liten spridning medan i andra intervall visar den sig ha större spridning. Den dåliga prestanda sattningen uppvisar är svår att ange en entydig orsa till eftersom de flesta tillstånden är opplade på något sätt. Men det är troligt att det härstammar från dålig modellering av roll och tippvinlar vilet slår igenom vid simuleringen av sensordata. 73
Figur.. Presentation av sattad urs efter 5 filterörningar med samma data och sensorfusion partilar, en så allad scatterplot. Intrimning av partielfiltrets parametrar gjordes genom att först placera in de uppmätta brusen på standardavvielsen i lielihooden. Som steg två ansattes systembrus inom gränser som ansågs rimliga men tilltagna. Vid mer eat justering av systembrusen blir uppgiften omple som följd av opplingen mellan tillstånd som urs, position och acceleration. n naturlig start var att trimma in modellen bruset för de tillstånd som påverar lite först och öa nivån allt eftersom godända resultat uppnås. Om lämpliga mätmetoder funnits till hands sulle modellens osäerhet ha unnat valideras, varvid de värden som uppmätts unnat användas i filtret. Kurs- och lutningssattning justerades först till acceptabla nivåer, därefter urs-, rolloch tippvinelhastighet, avslutningsvis acceleration, hastighet och position. De parametrar som används i resultatplottarna är enligt tabell...3. 3.27. 38.27. 27.44. 38.4. 4. 35. 35 w w.2. 2 2 abell.. Sammanställning av använda systembrusparametrar uttrycta som respetive V betecnar höger och vänster hjul v v.därh 74
DOP 7 PS MA :, y, z.4 / 36 ACC :, y YRO :, ACHO: H, V : HV.5.3 2 abell.2. Sammanställning av använda mätbrusparametrar uttrycta som respetive V betecnar höger och vänster hjul.. Där H I figur. visas en typis positionssattning med de parametrar som visas i tabell.-2. I figuren ses att positionen är maimalt cira en meter fel i de punter som mätts upp. Resultatet lassas som bra. Figuren visar även att styrsignalerna inte slår igenom diret efter att waypoint nåtts. Det beror dels på tidsfördröjningar i systemet och filtrets sattningar. Vid örningen gavs alltid steg i styrsignal stra efter att waypoint blivit deteterad, doc slår det igenom långsammare på sattningen specifit efter waypoint fyra. Positionssattningens ojämnhet härstammar från processbruset. Processbruset är som störst på styrsignalen och ger spår ut från varje position i olia ritningar, när ett sampel ommer in väljs då endast de bästa partilarna ut och det blir lätt ett hopp i sattningen som ses tydligt efter waypoint fyra. Vid den näst sista waypointen uppommer problem i lutningssattning på grund av att farosten åer upp i en bace och processbruset är då otillräcligt. Figur.. Figuren visar farostens sattade position angett i enheten meter. De marerade tiderna visar sattad position jämfört med PS-data vid samma tidpunt. 75
I figur.2 visas sattningen av farostens fart vilen orret ligger stra under m/s. Vissa naturliga variationer ses i figuren, men även spiar som genomslag från data. Vid sattning minsar farten snabbt till drygt.5 m/s som följd av att den åer upp i en bace. Där vänder den och får sjuts neråt vilet ses som den raftiga accelerationen som följer. Processbrusetärrelativthögtpåfarten,anledningenärattdetinteärettliavitigttillstånd som position och urs, tillräcligt snabb stigtid har prioriterats före brusigt beteende. Figur.2. Figuren visar sattningen av farostens fart i enheten m/s. I figur.3 visas sattningen av farostens ursvinel angiven i grader. Kurssattningen är relativt stabil och brusar endast ett fåtal grader, vilet är vitigt vid styrningen av farosten. De fåtal spiar se precis i början i data som föreommer förlaras med orrupt indata. 76
Figur.3. Sattning av farostens ursvinel i enheten grader. I figur.4 visas sattning av farostens attitydvinlar roll och tipp vila används för ompensering av magnetompassen. Även i sattningen av attitydvinlarna har outliers slagit igenom på sattningen. Det är alltid en avvägning vid intrimning av en modell, som är svår att verifiera, hur stort processbrus som sa användas. I UV-tillämpningen prioriterades ett bra system på plan mar. Om farosten ommer in i ett område med uperad terräng deteteras det och processbruset öas för att undvia divergens, om detetionen av lutningsförändringar som följd av förvansad gyrosignal ser på fals grund blir filtret mer benäget att acceptera outliers. ärmare analys visar att det är vad som händer vid sattningsvärde 3 i tippled. Stra före värde visas hur bacen uppför påbörjas och tippvineln öar. Accelerometrarna ger en mycet brusig sattning av attitydvinlar, anledningen till att det tillåts, är att det bedöms som vitigare att filtret onvergerar än att attityden blir brusfri. aturligtvis går det att erhålla en god estimering av attitydvinel men det förutsätter en modell med bättre precision. 77
Figur.4. Farostens lutning i enheten grader..4 Jämförelse mellan navigeringslösningar vå olia navigeringssystem har tagits fram och de har två huvudsaliga sillnader. Den ena är decentraliserat medan det andra är centraliserat. Det decentraliserade bygger på almanfiltrering medan det centraliserade estimeras med ett partielfilter. I utveclingsarbetet har ett antal aspeter på för- och nacdelar identifieras vila finns sammanställda i tabell.3 och tabell.4. Decentraliserat system +Intuitivt p.g.a. översådlighet +ätt att testa olia delar vilet medför större ontroll -Systemet behöver ofta förenlas Centraliserat system +Större möjligheter till oppling av tillstånd -Svårt att trimma in abell.3 För- och nacdelar med decentraliserat respetive centraliserat navigeringssystem. 78
Kalmanfilter +Stabilt +nel algoritm +eräningseffetiv -Hanterar endast linjära problem optimalt -Kan bli otillräcligt p.g.a. begränsade utbyggnadsmöjligheter. Partielfilter +Kan utöas med fler funtioner såsom artreferensnavigering. +Hanterar olinjära mer omplicerade modeller bra. -Stoastist resultat försvårar intrimning. -Ris för divergens -eräningsrävande. abell.4 För- och nacdelar med alman- respetive partielfilter. avigeringsprestanda för de båda systemen är i stora drag livärdiga vid örning på plan mar. Vid raftig lutning har partielfiltret en tendens att divergera vilet får stora negativa onsevenser på sattningarna. enerellt an även sägas att almanfiltret är bättre på jämn och glättad örning medan partielfiltret är bättre vid örsituationer med mycet urvor. I örning märs sillnad i sattning av tillryggalagd färdväg vilet eempelvis syns tydligt på sträcan mellan punt 5 och 6 där almanfitret ej i tid uppfattar att punt 6 passerats. Förlaringen till det är att almanfiltret sattar salfel i tachometrarna utifrån PS-mätningar som återopplas. Sattningen i avstånd ommer därför ej att silja sig marant från PS-data. Partielfiltret sanar den återopplingen och använder sig i stället av den bästa sattningen av hastigheten i varje punt. Sattningen i en punt sanar det direta beroende genom återoppling som felmodellerna i almanfiltret har. Man an därför säga att partielfilterimplementeringen har en större glömsefator..5 Reglering Regleringen fungerar bra vid simulering men vid implementering i farosten observerades tecen på instabilitet. fter analys onstaterades orsaen vara tidsfördröjningar upp till.7 seunder i systemet. n sänning av farten minsade effeterna därav. Därför örs farosten i farter ring m/s. tt naturligt tillvägagångssätt för att omma tillrätta med tidsfördröjningar vore att använda en modellbaserad regulator, men med tidsfördröjningar i både atuator och i mätsignaler sulle en modell med högre precision behövas. Parametrarna i PID-regulatorn trimmades in för att få ett så stabilt beteende som möjligt. n annan aspet är hur väl systemet svarar på genererade styrsignalerna. Vid örning på gräs svarar systemet sämre än på asfalt. Orsaen är att fritionen mellan hjulen och underlaget är större vid vridning på gräs än asfalt. n örning på asfalt blir med parametrar inställda för gräsunderlag svängig. De måste då manuellt sänas. tt annat problem som lätt uppommer då regulator för fart sanas är att farten öar mer i nerförsbace på asfaltsunderlag än gräsunderlag. n utvärdering av örning visar att regleringen fungerar tillfredsställande vid örning i låg fart. Slutsatsen dras med tane på att farosten når punterna den önsar nå, att sedan detta inte är de verliga punterna beror på positioneringen. 79
.6 Simuleringar För att unna utvärdera modeller för rörelse och US-sensorer, partielfilteralgoritmen, artreferensnavigering och hinderdetetering gjordes en simuleringsmiljö..6. Kartreferensnavigering Som understöd till positionssensorerna med PS och dödräning an positionering även göras utifrån en arta över det område farosten befinner sig. Metoden har mycet att ge om en noggrann arta och robusta avståndssensorer används. n sådan arta är lämpligtvis en objetarta där simulering av inmätningar går beränigsmässigt snabbt och objet an identifieras. Det vill säga ett hinder som farosten styr undan för an avbildas och matchas mot en i artan lagrad arateristi [35]. För att transformera en metris arta till en objetarta har en algoritm utarbetats. Algoritmen är idealiserad och förutsätter att artan som sall onverteras och omprimeras till så allad line-objetform har objet med räta vinlar, som ett typist innerstadsområde. Med en arta på line-objetform an snabbt en inmätning från vilen position som helst i artan simuleras och jämföras med verliga inmätningar som residualer. För att simulera och testa regleralgoritmer samtidigt som positioneringssystemet används, rävs mycet avancerade modeller av omvärlden och bilen. Det är utom arbetets omfattning att modellera det. Därför har artreferensnavigering uteslutande använts i simuleringsmiljön vid utprovning av reglering och vetorfältstyrning. På grund av brist på detaljerade artor har systemet ej implementerats i farosten..6.2 Hinderdetetering Simulering av en örning mellan tre punter visas i figur.5. Farosten befinner sig i målpositionen efter en simulering av förloppet med start i A. röseln är satt till,5 meter och vitfuntioner är av typen 9.. I figuren ses hur farosten vid närmande till hinder mjut styr undan med ett rörelsemönster som påminner om en elastis studs. Detetionen är gjord med de förenlade modellerna av US-sensorerna som presenterades i 5.7. Precis innan farosten passerar ett hörn påbörjar den svängen ring hindret. Orsaen är att sensorerna med detetionsområde i centrumorientering på farosten inte deteterar hinder i det läget. Vid en snäv sväng ring ett hörn, flyttas återigen referensen ut och farosten får ett mjut och jämnt beteende. De strecade linjerna i figuren visar referenslinjen från startpunt till mål. 8
Figur.5 Simulering av vetorfältsstyrning. I bilden befinner sig bilen i målpositionen efter att ha startat i nedre vänstra hörnet A. De grå ytorna är hinder och verar med repellerande raft. Salan är i meter. 8