SE509443C2 - Förfarande för övervakning av en manipulators rörelsestyrning - Google Patents

Description

5 10 15 20 25 30 35 r 509 445 Vid användning av industrirobotar med tillhörande externa axlar är det av stor vikt att övervaka rörelsestyrningen sa att inga axlar i systemet erhàller icke önskade rörelser eller utsätts för onormala störmoment. En mängd olika fel kan uppstà och sá mäste felet detekteras snabbt och säkert och rörelsestyrningen mäste ge robot och oberoende av vilket fel som uppstått, externa axlar sàdana rörelser att konsekvenserna av felet minimeras. Felen kan uppstàr hos robot, externa axlar eller hos den processutrustning som används i robotinstallationen.

Exempel pà komponenter som kan ge plötsliga felsituationer är kablar, styrelektronik. drivdon, motorer, sensorer, mätelektronik och Fel kan även uppstà i den process som är koordinerad med robotens rörelse, t ex kan en punktsvetstàng bränna fast eller ett griopdon fastna i fel läge. Dessutom kan fel uppstà pà grund av att operatören gör fel, t ex joggar roboten mot ett hinder, startar en extern axel för sent eller glömmer sätta en i/o-utgáng som skall styra en lucka genom vilken roboten skall passera. Om inte dessa felsituationer detekteras och àtgärdas snabbt finns stor risk att personer och utrustning kommer till skada. Robot och/eller externa axlar kan orsaka kraftiga kollisioner eller att utrustning som fastnat slits sönder.

Dessa felsituationer kan ge upphov till felaktig position, felaktig hastighet, felaktig acceleration eller felaktigt moment hos en eller flera av de i systemet ingående axlarna.

För övervakning av robotens rörelse används idag normalt befintliga signaler hos servot och en felsituation definieras av att nagon av dessa signaler passerat en larmgräns. För att undvika falsklarm krävs ibland filtrering av signalerna och förutom att signalerna pssserar en larmgräns kan ocksa krävas att signalen ligger över/under larmgränsen en förutbestämd tid.

Det finns också lösningar där larmgränsen beror av nagon signal hos servot, t ex den uppmätta axelhastigheten. Som filter används oftast làgpassfilter för att undvika falsklarm pà grund av kortvariga "ofarliga" signaltransienter. En speciell typ av filter som ocksa förekommer är s k observerarfilter. 10 15 20 25 30 35 53 509 443“” I patentet US 5 304 906 beskrivs t ex ett observerarfilter som används för att erhàlla en signal, som skall användas för att övervaka störmoment pà den till servot kopplade axeln.

Störmoment är moment som normalt inte påverkar robotens axlar och kan t ex genereras av för hög verktygslast, verktyg som fastnar eller av kollisioner. Dessa störmoment är normalt inte mätbara utan maste rekonstrueras för att kunna detekteras. En observerare är egentligen ett àterkopplat filter, som konstrueras i tidsplanet med hjälp av en dynamisk modell av det system, i vilket filtret skall användas. Denna filtertyp är speciellt intressant för att dynamiskt rekonstruera icke mätbara signaler. I fallet övervakning kan en rekonstruerad signal användas som den signal som skall övervakas.

En nackdel med observerarfilter är dock att de ej blir optimala för ett generellt övervakningsändamál och att de endast fyller en funktion dà man skall övervaka en signal som kan rekonstrueras. Sàledes kan en observerare utformas för övervakning av omotiverade störmoment pà axeln, men den kan inte direkt utformas för att t ex detektera felaktig position, felaktig hastighet och felaktig acceleration. Den kan t ex inte med tillräcklig selektivitet detektera om en axel erhåller en oväntat hög eller lag hastighet sàsom är fallet vid rusning.

Dessutom blir en observerare relativt långsam och är svar att fa stabil dá mekaniska resonanser stör regleringen, vilket är fallet hos veka robotar. Med en vek robot avses en robot med lag mekanisk egenfrekvens. Ytterligare ett problem är att kompensera för mekanikens dynamiska inflytande pà störsignaler och styrsignaler.

SAMMANFATTNING AV UPPFINNINGEN Uppfinningen avser att àstadkomma ett förfarande för övervakning av en manipulators rörelsestyrning, som kan detektera felaktig position, felaktig hastighet, felaktig acceleration, felaktig last, felaktigt axelmoment och felaktiga störmoment. 10 15 20 25 30 35 509 443 Vad som kännetecknar ett förfarande enligt uppfinningen framgár av bifogade patentkrav.

Förfarandet har följande fördelar: - mycket hög känslighet, - mycket snabb detektering, - liten sannolikhet för falsklarm, - detekterar alla förekommande felsituationer, - kan även användas hos veka axlar, dvs axlar som är fjädrande.

Dessa fördelar uppnás genom att bilda ett larmvillkor som 'innefattar ett larmvärde och en larmgräns. Larmvärdet tar modellbaserat hänsyn till báde stelkroppsegenskaper och vekhetsegenskaper hos manipulatorn. Vekheternas inverkan pà hur normala situationer och fel dynamiskt påverkar de reglerade manipulatoraxlarna modelleras i larmvillkoret med signalfilter, som har väsentligen samma överföringsfunktioner som kritiska överfunktioner mellan olika storheter, t ex moment och axelrörelse, i manipulatorn. För att erhàlla ett optimalt larmvillkor härleds detta enligt kända stelkroppsmodeller och vekhetsmodeller för manipulatorn, utgående fràn en signal- kombination som ger en utsignal direkt beroende av den feltyp som skall övervakas.

Larmvillkoret har egenskapen att vara mycket känsligt för felaktiga störmoment, laster, accelerationer etc, samtidigt som det ej löser ut vid normala driftfall. Detta beror pà att larmvillkoret bygger pà signalkombinationer som direkt beskriver de fel som skall detekteras och att larmvillkoret är modellbaserat bade med avseende pà en dynamisk stelkroppsmodell och en dynamisk vekhetsmodell av manipulatorn.

I en utföringsform av uppfinningen kan larmgränsen utgöras av en konstant. I en föredragen utföringsform av uppfinningen används istället en modellbaserad larmgräns, som med kännedom om modelleringsfelen ger en minimal larmnivá vid alla typer av rörelser. Larmgränsen bygger pà en modell av dynamikmodellernas 10 15 20 25 30 35 509 443 fel och kommer automatiskt att höjas då t.ex. acceleration och hastighet ökar.

FIGURBESKRIVNING Figur 1 visar schematiskt en industrirobot.

Figur 2 visar ett framkopplat servo.

Figur 3 visar en enkel modell av motor och axel med en enkel vekhet.

Figur 4 visar en dynamisk modell av motor och axel med enkel 'vekhet.

Figur 5 visar ett blockschema för generering av ett larmvärde och en larmgräns enligt ett första utföringsexempel.

Figur 6 visar ett blockschema för generering av ett larmvärde och en larmgräns enligt ytterligare ett utföringsexempel.

Figur 7 visar ett blockschema för övervakning av en manipulators rörelsestyrning enligt uppfinningen BESKRIVNING AV UTFÖRINGSFORMER Figur l visar schematiskt en industrirobot. På ett underlag är robotens fot l fast monterad. Roboten har ett basstativ 2, vilket är vridbart i förhållande till foten 1 runt en vertikal axel Al. lagrad och vridbar i förhållande till basstativet runt en andra axel A2. vridbar i förhållande till den första armen runt en axel A3.

I basstativets övre ände är en första robotarm 3 I armens ytterände är en andra arm 4 lagrad och Robotarmen 4 består av två delar 4a och 4b, varvid den yttre delen 4b är vridbar i förhållande till den inre delen 4a runt en med armens längdaxel sammanfallande vridaxel A4. Den andra armen 4 uppbär i sin yttre ände en s k robothand 5, vilken är vridbar runt en mot armens längdaxel vinkelrät vridningsaxel A5. Robothandens yttre del är vridbar i förhållande till sin 10 15 20 25 30 35 509 443 inre del runt en vridningsaxel A6. Varje axel drivs av en motor.

Ett förfarande enligt uppfinningen bygger på följande tekniska lösningar: - Användning av en signalkombination för bildande av ett modellbaserat larmvärde och en modellbaserad larmgräns.

- Generering av en larmsignal i beroende av en jämförelse mellan larmvärdet och larmgränsen.

- Användning av sàväl en stelkroppsmodell som en modell, vilken beskriver dynamiken med avseende pà robotens vekheter, vid framtagningen av uttrycken för larmgränsen och larmvärdet.

- Möjlighet att komplettera metoden med s k Learning Control.

- Möjlighet att använda momentframkoppling i servot.

Att använda momentframkoppling i servot innebär att modellerade axelmoment framkopplas enligt figur 2. En bangenerator 6 beräknar börvärden för axelns position mk, hastighet mk och accceleration Öw. En dynamisk modell 7 som beskriver robotens statiska och dynamiska egenskaper beräknar dynamikparametrar.

Med utgångspunkt frán bangeneratorns börvärden för position, hastighet och acceleration beräknar den dynamiska modellen de dynamikparametrar som krävs för framkopplingen. Dynamik- parametrarna är masströghet 1:, kopplad masströghet 1%, X gravitationsmoment I moment orsakade av centrifugalkrafter fl' vid andra axlars rörelser ILM1, moment orsakade av coriolis- krafter i samverkan med andra axlars rörelser Iàfioch moment orsakade av statisk och dynamisk friktion tkm.

En framkopplingsgenerator 8 beräknar referensvärden för position och hastighet Qwwom, och ett framkopplingsmoment Tffw till servot 9, som styr motorn och den pà motorn monterade transmissionen och axeln ll. Framkopplingsmoment Tffw beräknas utgående frän dynamikparametrarna och börvärdena för axels position. Fràn servot styrs motorn och därmed axeln i enlighet 10 15 20 25 30 35 'axelvinkel ma 50.9 443 med referensvärdena. En lägesgivare med tillhörande mätsystem 10 avger en utsignal som definierar axelns aktuella position mm och hastighet mm som pàföres servot.

För att härleda ett larmvärde ställes en optimal modellbaserad signalkombination upp. Hur detta görs visas nedan för en enkel modell av motor och axel enligt figur 3. I denna modell utgör motorns rotor av en mindre cylinder 12 och manipulatorarmen av en större cylinder 13. Rotorn har masströgheten Jm och armen har masströgheten Ja. Pà rotorn verkar ett störmoment Tümn och pà armen ett störmoment Iüm. Mellan rotor och arm finns en vek axel 14 med fjäderkonstanten k och dämpningen c. Da vekheten är spänd kommer rotorns axelvinkel mm att skilja sig fràn armens Rotorns axelvinkel mätes med en vinkelgivare och pàföres tillsammans med motorns axelhastighet mm servot 9. I servot 9 är GpOS(s) överföringsfunktion för positionsregulatorn, Gve1(s) är överföringsfunktion för hastighetsregulatorn, Im är det reglermoment som beror av reglerfelen i hastighetsregulatorerna (mm,-mm) och i positionsregulatorerna (md-mm), tm är totalt motorreferens- moment och är summan av tu, och framkopplingsmomentet Tñ,.

I =1m+1fl I =k,4 Hlf kT är momentkonstanten hos drivdon och motor.

Härledning av ett larmvillkor exemplifieras med fallet rusning.

En rusning innebär att axeln erhàller en rörelse, som inte motsvaras av de signaler som levereras till servot frán fram- kopplingsgeneratorn. För att erhálla maximal känslighet och minimal responstid används axelns acceleration som bas vid framtagningen av larmvillkoret. Av största vikt är att hitta en signalkombination som blir så okänslig som möjligt för normala rörelser, vilket kan göras med hjälp av kännedom om dynamiken hos motor och axel.

Med utgångspunkt fràn figur 3 kan följande uttryck ställas upp för motorns acceleration: 10 15 20 25 30 35 509 443 (ñmü) = GI(5)'[Tm(s)+Tdistm(s),]+ G2(s)'ltdisra(s) (1) där s är Laplace-operatorn i frekvensplanet Ön : motoraxelns acceleration tm , av motorn genererat moment till motoraxeln I¿iStm momentstörning pà motoraxeln rdista momentstörning pá armen G1(s) överföringsfunktionen mellan tillfört moment pà motor- axeln och motoraxelns acceleration G2(s) överföringsfunktionen mellan tillfört moment pà arm- sidan och motoraxelns acceleration I G1(s) och G2(s) ingàr följande parametrar: Jm: motorns masströghet Ja: armens masströghet K: fjäderkonstant mellan motor och arm c: dämpning mellan motor och arm Normalt används en växellåda med utväxling mellan motorsida och armsida. Samtliga parametrar och signaler är emellertid konverterade till motorsidan, vilket t ex betyder att moment reducerats med faktorn 1/n, masströgheter med faktorn l/n2. där n är utväxlingen, och Figur 4 visar en dynamisk modell för motor/axel-mekaniken enligt figur 3. Med hjälp av denna modell kan överföringsfunk- tionerna G1(s) och G2(s) härledas: u Im c .. U k u H (pm: jm_s(('pu_(pm)+jm_s2 (Qpu _('pm) där .. c-s+k ..

Ja-sz +c-s+k 10 15 20 25 30 50èi44š“i Detta ger: s = G() .Ia-sz +c-s+k (4) ' Jm-Ju-sz+(Jm+J“)~c-s+(J,,,+J“)-k -s+k G2= ° <5) .Im-Ju -sz + (Jm +Ju)-c-s + (Jm +Ja)-k I sambandet 1 àterstàr nu att ta fram uttryck för Im, Tdistm och rdista. Med momentframkoppling enligt figur 2 och figur 3 erhàlls följande uttryck för nn: i ' 'tm = kT (har + + Jrïàíöref _ Tçtagfric: 'sign (qsref) _ fåwnfríct (qsref )P _ (6) 2 _ 'trippel _ Tšravity _ fifcerurj (qsrejj J _ Zfcoqj 'iqïrefi (qeß ) _ šjjk 'örefjï 1 1,1 .

Vidare gäller: . . P Tdistm = 'tsmxfricr ' sign + fdynfricx ' + Trippel 141m = 73mm,- + zfurm, + flzjfwn, êljk (8) 1 1. .

Här innebär * att dynamikparametern är modellerad och därmed behäftad med ett modellfel. För bàde modellerade och verkliga dynamikparametrar gäller följande definitioner: kT : momentkonstanten hos motor inklusive drivkretsar.

Momentkonstanten är kvoten mellan verkligt motormoment och referensmoment.

Terr momentreferens genererad av hastighets- och positionsregulatorerna i beroende av aktuella reglerfel rmwm-sænßßfl : enkel beskrivning av den statiska friktionen som ett moment multiplicerat med tecknet hos hastigheten 10 15 20 25 30 35 . '1o' 509 445 _ P -filynjfiu ' (mm ) I hastigheten upphöjt till exponenten p den dynamiska friktionen, som är proportionell mot I momentstörning pà grund av rotor- och statorrippel hos rippel ITIOCOITI tmmw :gravitationsmomentet pà armsidan ~ 2 . 1 ïgjgmyfwj) : moment pà armsidan pà grund av centrifugalkrafter 1 frän axlarna med index j moment pà armsidan pà grund av corioliskrafter fràn samverkande axelpar i och j ÉEJÉ-éj moment pà armsidan pà grund av kopplad masströghet jJ: fràn axlarna j Friktion pà armsidan har försummats eftersom friktionsmomenten kommer att divideras med utväxlingen dà omräkning sker till motorsidan. Samtliga samband är omräknade till motorsidan.

Vi har nu en fullständig beskrivning av alla ingående komponenter i samband l och vi kan nu bilda en signal- kombination pm: som kan användas för framtagning av ett larmvillkor. pm = G,..,- (<fi;-<ßm) <9> Grus(s) är ett filter som i huvudsak används för brusreducering av accelerationssignalerna. w;Lfl är ej känd och mäste härledas för att erhàlla en användbar signalkombination. Vidare mäste de larmvillkor och larmgränser som skall implementeras härledas.

Hur detta görs exemplifieras med rusningsfallet, varvid uttrycket för pmjs) direkt kan användas.

Med ekvation 1 i ekvation 9 erhalls: l0 15 20 25 30 Jflïjlrl Jfll-:Jml I I '11' 509 445"“ pmm = Gnum- (G,(s)-[f,,,(s)+1,,,.,,,,,(s)] + G2(s)-1d,-_,,a(s) - ømw) (10) Enligt ekvation 6 , 7 och 8: 'fm = krÜ; +JÄ)' Ekvation ll i lO ger: PMU) = GMÜT (G1(S)'[k7"'(-/;+ Jfnfiref (S) " kr 'TÄiSmK-Y) _ (12) - kr -«;,-,,., + kf-felxs) + fdmml+ G2 -fd.-,,a - ømßø) i i c=c k=k' k,=k;, 'tama = 'faim 'Ptam Tdrktm = Tdisrrn +Tdevm där 12,, är estimerad max avvikelse mellan verkligt störmoment och modellerat störmoment. Detta ger för fallet enkel vekhet enligt modellen i figur 4 och med Grus(s)=l: JJ-sz +c°-s+k' z g-JJ-s* +(1;+J,j)-c'-s+(J,',+J;)-k' -[1<, -(J; +-1;_)-qs,,,(s) + (qümçv) + qmnm - k; -1;¿,,m(s) + k; -r,,,(s)) + + c'-s+k' J,',-J,j-s2 +(J;+J,j)-@'-s+(J,j,+1;)-k' .IJ-sz +c'-s+k' _ J,j,-J;-s2 +(J,',+J;)-c'-.«+(J;+1,j)-k pmm . . (13) ' (Tdisla (s) +1deva _ v 'Tflixm (s) _ (ämßs) ] Vid perfekt reglering och i stelkroppsfallet gäller att k->°<> och 1 (s)=0 och villkoret för larm blir då. det förväntade: 87)' AMPLßgsw, (s) - :än (n) z EPS där AMPLU innebär amplituden hos frekvensspektrum inom det studerade frekvensområdet och EPS är ett litet tal. 10 15 20 25 12 509 443 Om regleringen inte är perfekt blir i stelkroppsfallet i stället villkoret: AMPL(',,, (s) -<ö,. + ,¿l,¿ -'=.,,)2 EPS 1 (s) beror av modellfel vid normaldrift. Det kan antagas att Cfl' J; varierar så làngsamt att det kan betraktas som konstant i det aktuella frekvensområdet.

Uttrycket 13 kan överföras till ett larmvillkor genom att införa följande definitioner: k; =1+k;'dev _ 1 122 '- +16 1 _ G1= . .- ff” s = . .~G3 (14) (1,,+1,,) (Jm-fu) , . k. (1,.+1,,) -----s +<:-s+ (1;.+1;) _ 1 c'-s+k' 1 , Gzçs) = J: +ja ' J: .Ja = Jß-FJ; 'G4(s) m “ -jåäjír-s24-c'-s+-k' m “ Gfflfl är en modell av överfunktionen mellan tillfört moment pà motorsidan och motoraxels acceleration.

C§(s) är en modell av överfunktionen mellan tillfört moment pà armsidan och motoraxels acceleration.

Uttrycket 13 kan nu modifieras med hjälp av 14 och 15 vilket ger följande larmvillkor dà Grus delas upp i Grusl och Grusgz lä 509 44s*M AMPL «G; ~ mm + ma] 2 J". + J., . . .. GÄMAS) _ , . '>“ (GrmZ ' Glmax (s) ' kTdev .cprej (s) + I (Idel/m (s) _ kTdzv .Talixlm (s) ) + . . m . u dev t 63mm; (s) e G4max(s) -G3max(s) (lqkkTdev) 1- ' kfdev 'Terr (s) + ' Tdista (s) + C Û Û Jm-i-Ja-'Idev Jm+Ja_Jd" J; + _ JIICV -1;.v.,),,,,, Här har följande definitioner införts: 5 G;mx(s) är G3(s) med maximal avvikelse hos J;,,J;,c',k' där denna 'maximala avvikelse ger maximal amplitud hos (g(Q i det aktuella frekvensområdet. kk” är maximal positiv avvikelse fràn värdet 1 hos k; lÛ J¿, är maximal avvikelse hos J;+=ß CflM“(Q definieras pá samma sätt som (ÄMu(9. 15 Gš, G' G;,CfimM är modellstyrda filter medan Grusl och Grusg 3rnax' är làgpassfilter.

De dynamiska modellfelsparametrarna 1¿W och ILW bildas med utgångspunkt fràn kännedom om felspridningen hos de modellerade 20 parametrarna. Frán uttrycken 6, 7 och 8 erhálls: s x :u _ . f: n: . P* s s: Tdevm = dstadrict 'Tstafríct ° *Ngn (wref ) + ddynfrict ' fdynfricl ' (wref ) + drippel 'Trippel Ü Ü i i Ü , 2 Tdeva = dgravity 'Tgravíty + Xdcemrj centrj 'ßpref ) + J (13) + :fdcorq- ' fcorïj '(cpref¿ )((prefi)+ dJjk ' Jjk ' (Prefj HJ J- 25 d'-parametrarna anger max felfaktor hos respektive dynamikparameter. Om felet är stort blir d' stort (men < 1) och 10 15 20 25 30 509 443 l “4 om den modellerade parametern överensstämmer väl med verkligheten kommer d' att ligga nära O.

Den modellbaserade larmgränsen kommer automatiskt att ändras med manipulatoraxlarnas position, hastighet och acceleration.

Om t ex hastigheten sänks hos samtliga axlar kommer feltermerna fràn dynamisk friktion, centrifugalkrafter och corioliskrafter att minska och larmgränsen sänks automatiskt.

Tvà filter GA och GB införes. Sätt nu enligt 14 och 15: Û 1 fl G = -r-ï_*--G lmax Jm-l-Ja-Jdev smax 1 .i i G2max = * G4max Jm+°];_']e;ev i li IF Jm: = Jm+Ja Ik * 3 i Jtotmín = Jm + 'Ia _ Jdev Ik III * Jrormax = Jrommin +2Jdev GA = Grusl Gß==Cåu2 Vi erhàller nu det generella larmvillkoret: m (Gí- (1,1, +f,,,) - 2 Û Ü u Ü Ü Û lmax I [kTde-v I Jzormnx 'çprzf + Tdevm _ kTdev 'Tdisrm _ _ + kTtJevJ-rzçixlu + kïtïlevltzrr] + l [Tiçixlu +I|:'evu]X = LGI GE-(G LV1 z larmvillkor LG1: larmgräns (19) 10 15 20 25 30 'is - 509 445 GA och GB är filter av lagpasstyp eller av bandpasstyp och syftet med att använda dessa filter är att minska inflytandet av icke önskade störningar pà larmvillkoret.

I normalfallet skall LV1 < LG1. Om LV1 > LG1 ges larm. Figur 5 visar ett blockschema av en övervakning som implementerar ekvation 19. De signaler som används för beräkning av larmvärdet LV är $w,1m,öm. Larmgränsen LG beräknas med hjälp av signalerna Qmfoch 1",. Följande parametrar beräknas av den ska modellen z Jx-ør ' Jr-øl/KAX ' kåhv 'I :in-rn 'I Qinm 'I Lina 11 d-zvu och de parametrar som ingar i Gf,G,'m,G2'm .

'Larmgränsen bestämmer känsligheten för detektering av olika fel- tillstànd sàsom felaktig position, felaktig hastighet (rusning), felaktig last, aktiga störmoment (t ex fastkörning). Larmgränsen LG1 i ekvation 19 felaktig acceleration, felaktigt axelmoment och fel- är särskilt känslig med avseende pá detektering av rusning, men den kan även användas vid de övriga feltillstanden om en omodellerad larmgränskomponenet lägges till. I sitt enklaste fall kan larm- gränsen vara en konstant. Den konstanta larmgränsen maste ha ett värde som är större än det maximala larmvärdet för normala drift- situationer hos roboten.

En viss logik krävs för att generera tecknet hos kan sá att larmvillkoret blir maximerat med avseende pà tecknet hos kàn.

För att klara de fall da IW begränsas av en momentgräns, vilket ger I används i stället för T", skillnaden mellan T mnlimiz ' och Tfiv, dvs Twmm,-Im”. mrlimir För en styv manipulator gäller att k'-èw, vilket ger följande: 10 15 20 25 30 509 445 16 I i Gl = i 110! 1 (20) i Gl max = J* to: min l Jm: min i G2 max = I härledningen av G; och G; används en enkel motor/axelmodell med en vekhet mellan motorsidan och armsidan hos axeln. Vid mer komplexa flexibla armsystem med kopplade svängningar mellan olika axlar som ger upphov till flera svängningsmoder kommer G; och (Å att ha högre ordningstal, men larmvillkoret 19 'kommer att vara detsamma.

En analys motsvarande den som gjorts för övervakning av rusning kan även göras för andra feltillstànd. Om t ex icke modulerade störmoment skall övervakas, kan man utgà fràn följande signalkombination: pdmnf) = Gdmw» (räknas) -1d¿.,,,) (21) Nu är ej Iümfs) direkt mätbar som ÖMQQ i fallet med rusning där mm används som mätsignal. Ett samband màste därför tas fram mellan 1üm(s) och Qm(Q.

Ur ekvation l erhàlls: Tdistaßg) = _ G1 [rm(s) +Tdíslm(s)]} Uttrycket för IMO) erhàlls ur ll och kan sättas in i 22: Larmvillkoret för det flexibla fallet blir nu: Ll/l = GA ' ßçllíul 'Örzf +Terr)_(ämj 2 (23a) GB ' '[k7"|lev ' 'Ilhlmax lön] +ratlevm _ krïlcv 'Terr]+ -Tdzvu *GZrIux 'Tdevu = LGZ 10 15 20 25 30 35 '17 509 443' Skillnaden mellan larmvillkoret för rusning, ekvation l9, och larmvillkoret för störmoment, ekvation 23a ligger sàledes i uttrycket för den modellbaserade larmgränsen.

Metoden kan även användas för att under ett nödstopp detektera när de mekaniska bromsarna börjar ta. Detta ger möjlighet till noggrann reglering även under ett nödstoppsförlopp.

För att härleda larmvillkoret för lastfel är det lämpligt att finna en signalkombination för felet hos masströgheten. Alla rörelser kräver ju minst en accelerationsperiod och en retardationsperiod och alla rörelser kommer därför att påverkas av fel i masströgheten. Sáledes kan man utgà fràn följande Isignalkombinationz pload = (JIE: _ Jmf) För att hänföra denna signalkombination till mätbara signaler görs följande approximation: pload = (Jin 'örzf _ Jm: Med hjälp av sambanden l och ll kan man visa att även i detta fall erhälles larmvillkoret enligt 19 med nagra mindre modifikationer av larmgränsen. Metoden kan även användas för att under ett nödstopp detektera när bromsarna börjar ta. Detta ger möjlighet till noggrann reglering även under ett nödstoppsförlopp.

Det generella larmvillkoret 19 kan omarbetas för att erhálla former som är lättare att implementera. Med användande av samband ll kan sàledes följande larmvillkor erhållas: LVZ :IGA I q (Tmr +13ix:m)+G2. 'Tcçütu -öm 2 = LG, (23b) 2 'GB ' ' ' Tmr 'i-Tgevm) + Gšnnxrsevu) Genom multiplikation med GQ" och Gåm erhålles följande larmvillkor: 10 15 20 25 30 35 118 509 443 =flGA u (CZ-P ' Gl. ' (Tmr -i-tzšzlrlm) í-Tšllvm _ G24' 2 =¿¿¿ (23c) 2 'GB ' ' Gl-rmx I (kTTz/ev 'TW -I-Taçevm) +Tl^vu) Figur 6 visar ett blockschema för generering av larmvillkoret enligt ekvation 23c. Den dynamiska modellen, block 7, beräknar följande parametrar: k¿M,1¿m,I¿m,1¿W,1¿W.

För enkel vekhet gäller: Gg” = G;" -Gf - J; + J; .Detta ger larmvillkoret: LV4 :IGA (Gz-l' 'G1' 'GW _11; +T;Lfun)+T;a-:a “JJ 'ÖIÛIE I 4 (23d) 2kÜ'Ü%mu'Gmm ~<1<;,., m, - 12.”. +1;,,,.>+f;,.,., + 15,, -<§,.>!= LGs Learning Control innebär att man under program-exekveringen lagrar undan signaler fràn servot och att man i en senare programexekvering utnyttjar dessa signaler för att förbättra prestanda hos regleringen. Learning Control finns t ex beskriven i en artikel “Learning Control of Robot Manipulators” av Robert Horowitz i Journal of Dynamic Systems, Measurement and Control June 1993, Vol. 115 page 402-411.

Larmvillkoren 23a~23d har egenskapen att vara mycket känsliga för felaktiga störmoment, laster, accelerationer etc, samtidigt som de ej löser ut vid normala driftfall. Detta beror pà följande: - Larmvillkoren bygger pà signalkombinationer som direkt beskriver de fel som skall detekteras.

- Larmvillkoren är modellbaserad báde med avseende pá en dynamisk stelkroppsmodell och en dynamisk vekhetsmodell av manipulatorn.

- Larmgränsen bygger pà en modell av dynamikmodellernas fel och kommer automatiskt att höjas modellbaserat da t.ex. accelerationen och hastigheter ökar. 10 15 20 25 30 35 19 509 443 Larmvillkoren ger mycket snabb detektering och kan även användas hos mycket veka axlar genom att inverkan av vekheterna och axeldämpningar modelleras genom filter, som implementeras överföringsfunktionerna G, (s), G2(s), G2"(s), och G2* (s) - G1 (s) Dessutom visar härledningen frán olika signalkombinationer att larmvillkoret är generellt användbart.

Learning Control innebär att man under program-exekveringen lagrar undan signaler fràn servot och att man i en senare programexekvering utnyttjar dessa signaler för att förbättra prestanda hos regleringen. Learning Control finns t ex beskriven i en artikel “Learning Control of Robot Manipulators” av Robert Horowitz i Journal of Dynamic Systems, Measurement and Control June 1993, Vol. 115 page 402-411.

Om Learning Control används kommer uttrycket ll att kompletteras: 'tm = kT'(J; +]r:1)'(§ref _ kïfltâistm _ kTJrzçísza + kTJterr + kTJtLC (24) där ILC är framkopplingsmomentet fràn Learning Control loopen.

ILC kommer att minska 1", och 1m,+ILc kommer att ge en momentframkoppling med litet fel. Detta gör att larmgräsen kan sänkas. Hur mycket larmgränsen kan sänkas beror pà hur mànga inlärningscykler som körts och pà hur effektiv inlärningen är.

Learning Control kan även anpassas direkt till larmvillkoret.

Därvid lagras värdet pà vänsterledet i t ex (23a) fràn cykel till cykel och de lagrade värdena används som larmgränser: VLCUCJ) = |GA'(Gr'(J:ot'(ñref +Terr)_(-p.m1 k [k fd, cy e ,x (25) AVI-CU) = bil-cant) _ VLCUC _ LÛ' en eller flera k-värden 10 15 20 25 30 35 509 443 Larm ges dà av larmvillkoret: LVS = GA '(Gl. I('Ir:n lön] +1rrr)_ (ömlhhu ->- (kst) + Lvm eUer (26) LVS = GA '(Jr:u 'óref +Terr) _ (ömïkflvr _ AVLC = Lvfi; k är aktuell programcykel. t är tidpunkten (eller positionen) inom cykeln.

VLC(k,t) är det inlärda värdet hos vänsterledet i 23a.

AVLC(k,t) är spridningen i VLC mellan olika cykler.

-Lämpligen används den modellbaserade gränsen enligt 23a dà inlärningen startar eller om AVLC är för stor.

Sammanfattningsvis är larmvillkoren 23a-23d generellt mycket användbara och kan med smärre modifikationer användas för felaktig last, friktionsavvikelse, kollision, mätsystemfel, styrelektronikfel, detektering av rusning, fastkörning, drivsystemfel, konfigurationsparameterfel, bromstillslag etc.

Dä larmvillkoren detekterat en larmsituation kan detta användas för att byta reglerstrategi, inleda nödbromsning etc. Vad som skall göras bestämmes av en logikmodul, som ser pà axlarnas hastigheter, positioner, moment etc.

Ovanstående utföringsexempel visar ett momentframkopplat servo, men uppfinningen är även tillämpbar pá ett servo som inte är framkopplat. I ett sàdant utföringsexempel utgör utsignalerna fràn bangeneratorn referenssignaler till servot (wrzf :cpbg Yqörzf =(pbg) ' Figur 7 visar ett generellt blockschema för övervakning av en manipulators rörelsestyrning enligt uppfinningen. Den dynamiska modellen beräknar ett flertal dynamikparametrar, block 7, utgàende fràn beräknade börvärden för robotaxelns position.

Referensvärdet för axelns position Qmf och axelns aktuella position Qm tillföres servot som beräknar motormoment tm och 10 21 509 443 1 som är moment beroende av reglerfel vid positions- och (ff hastighetsregleringen, block 9. Om servot inte är framkopplat så är tm,=I", Ett larmvärde LV beräknas, block 20, i beroende av axelns aktuella acceleration öm, dynamikparametrar och àtminstone nagon av signalerna Ömf(larmvillkor 23a), 1 (larmvillkor 23a), och TW (larmvillkor 23b-23d). En Cff larmgräns LG beräknas, block 21, i beroende av en kombination av dynamikparametrar och àtminstone någon av signalerna öw (larmvillkor 23a), 1", (larmvillkor 23a), Im, (larmvillkor 23b- 23d) och öm (larmvillkor 23d). Larmvärdet jämföres med larmgränsen och en larmsignal LARM genereras i beroende av jämförelsen.

Claims (2)

10 15 20 25 30 35 22 509 445 PATENTKRAV l) Förfarande för övervakning av en manipulators rörelsestyrning vilken manipulator innefattar en rörlig del som kan förflyttas relativt en fast del eller en annan rörlig del, minst en rörelseaxel (Al-A6) för positionering av den rörliga delen, en motor som driver axeln och därmed den rörliga delen, ett servo (9) för styrning av axeln i enlighet med tillförda referensvärden för position, hastighet och acceleration (mfi,@w,öM) och en lägesgivare (10) anordnad för att avge en utsignal som definierar axelns aktuella position (mm), varvid ett flertal dynamikparametrar ( 11:11 ' 'IINMAX ' kïïdev 'Tzçewn 'I šixnn 'I :isla 'T ;zwz ) beräknas i beroende av börvärden för axels position och hastighet och en dynamisk modell (7) som beskriver robotens statiska och dynamiska egenskaper, axelns aktuella acceleration (fim) beräknas utgående fràn axelns aktuella position (WM), en momentsignal (Im) genereras av servot i beroende av servots reglerfelsmoment (I",), kännetecknat av att det vidare innefattar följande steg: ett larmvärde (LV) beräknas som en funktion av den aktuella accelerationen (öm), àtminstone nagon av dynamikparametrarna (];,J;,c,k) och en eller flera av följande signaler: servots reglerfelsmoment (I”,), referensvärdet för accelerationen (Ömf) och momentsignalen fràn servot (Im,), samt med användning av ett eller flera signalfilter (Gf1Ä,G§”), en larmgräns (LG) beräknas som en funktion av en eller flera av följande parametrar: en konstant, aktuell position (wm), aktuell hastighet (om), aktuell acceleration (öm), nagon av dynamikparametrarna (J¿mm,J¿w,kà”,IMwrüv), servots reglerfelsmoment (1",), referensvärdet för accelerationen (ómf) och momentsignalen fràn servot (IW), 10 15 20 25 30 35 23 509 443 larmvärdet jämföres med larmgränsen och en larmsignal (LARM) genereras i beroende av jämförelsen.

2) Förfarande enligt patentkrav 1 kännetecknat av att vid beräkning av larmgränsen utnyttjas uppskattningar av maximala modellfel (1¿WH1LW,kàh,JLW) samt ett eller flera signalfilter ' 3) Förfarande enligt patentkrav 1 eller 2 kännetecknat av att nämnda signalfilter (Gf) är en modell av överförings- funktionen mellan tillfört moment pà motorsidan och mOtOraXElIlS aCCêlêrâtiOIl . Ä) Förfarande enligt nagot av de tidigare patentkraven kännetecknat av att nämnda signalfilter (Gg) är en modell av överföringsfunktionen mellan tillfört moment pà armsidan och motoraxelns acceleration. 5) Förfarande enligt patentkrav 1 kånnatecknat av att larmvärdet (LVl) beräknas i beroende av följande samband: Gl' I (JL: 'cörejf j-Terr) _ (öm 6) Förfarande enligt patentkrav l eller 5 kännetecknat av att larmgränsen (LGl) beräknas i beroenda av följande samband: IGB ' (Gltmu ' ' Jënmax 'dj-ref + 12mm _ kïtdev 'Tzlisrm _ _ (l-kkfilzvytdisla + kTdevTerr-J + GZmax Q [IL-dina é-Tdevajl 7) Förfarande enligt patentkrav 1 eller 5 kännetecknat av att larmgränsen (LG2) beräknas i beroende av följande samband: |GB I (Glam: .[kTdev I 'Iwlmax Döm] +tdevm _ krïlev -Turj-k 'Tølevufagmax -Tdevu 8) Förfarande enligt patentkrav l kännetecknat av att larmvärdet (LV3) beräknas i beroende av följande samband: lO 15 20 25 30 35 ll) 509 445 IG: - +f;,-_.... -G;" -<fi:>| 9) Förfarande enligt patentkrav 1 eller 8 kännetecknat av att larmgränsen (LG4) beräknas i beroende av följande samband: IG, 4021,... -Gfm -<1<;,,,: m., +^=;,m.>+f;.:.>| 10) Förfarande enligt patentkrav 1 kännetecknat av att larmgränsen (LG3) beräknas i beroende av följande samband: GB ' (Grus: ' 'Tmr -l-Tšøwn) + Förfarande enligt patentkrav 1 eller 10 kännetecknat av att larmvärdet (LV2) beräknas i beroende av följande samband: IG: ~+G;-1;:.,., -<ø,,>| 12) Förfarande enligt patentkrav 1 kännetecknat av att larmvärdet (LV4) beräknas i beroende av följande samband: IG,.-+f;,am - 1; wm) 13) Förfarande enligt patentkrav 1 eller 12 kånnetecknat av att larmgränsen (LG5) beräknas i beroende av följande samband: IGB ' ' Glnnx ' (kgdev .TW _ -i-Tzevm) .Ftcieva + Jrševa 14) Förfarande enligt patentkrav 1 kännetecknat av att larmvärdet (LV5) beräknas i beroende av följande samband: G: -(G; -(1;,, w", +11) - m] k-oLr 15) Förfarande enligt patentkrav 1 eller 14 kännetecknat av att larmgränsen (LG6) beräknas i beroende av följande samband: Ivwuaz) iAvLc|