SE513949C2

SE513949C2 - Milk robot and method for controlling the movement of the robot arm

Info

Publication number: SE513949C2
Application number: SE9802731A
Authority: SE
Inventors: Michael Street; Robert Christopher Hall; Sue Spencer
Original assignee: Alfa Laval Agri Ab
Priority date: 1998-08-14
Filing date: 1998-08-14
Publication date: 2000-12-04
Also published as: WO2000008921A1; AU5664499A; DE19983466T1; SE9802731L; GB0101895D0; GB2357419A; SE9802731D0

Abstract

The present invention relates to a method and a device for controlling the movement of a robot arm (3) in a milking robot (1). For each axis of movement of the robot arm (3), a maximum permitted rate of operational movement and/or maximum positional rate of movement is stored in a memory (8', 8''). The demanded movement of the robot arm (3) is compared against the maximum permitted rate of operational movement and/or the maximum positional rate of movement and the robot arm (3) is moved at a rate of movement corresponding to the lowest of the demanded rate or the maximum permitted rate.

Description

15 20 30 515 949 2 till ytterligare stora, plötsliga fel följda av en ytterligare snabb robotarmförﬂyttning och ytterligare oro för djuret. 15 20 30 515 949 2 to further large, sudden faults followed by a further rapid robotic arm movement and further concern for the animal.

Uppﬁnningens syfte Den föreliggande uppñnningens anordning har syftet att lösa den tidigare teknikens problem.Purpose of the invention The object of the present invention is to solve the problems of the prior art.

Sammanfattning av uppﬁnningen Uppﬁnningens syfte åstadkoms enligt föreliggande uppfinning med hjälp av meto- der och anordningar som har de särdrag som anges i de självständiga kravens kän- netecknande delar. Ytterligare utvecklingar och förbättringar av den föreliggande uppﬁnningen nämns i de osjälvständiga kraven.SUMMARY OF THE INVENTION The object of the invention is achieved according to the present invention by means of methods and devices which have the features stated in the characterizing parts of the independent claims. Further developments and improvements of the present invention are mentioned in the dependent claims.

Sålunda innefattar företrädesvis en metod i enlighet med uppfinningen för reglering av förﬂyttningen av ett objekt, som ska förflyttas ﬁån ett befintligt läge till ett öns- kat läge där avståndet mellan det beﬁntliga läget och det önskade läget sarnplas vid förutbestämda samplingsintervaller, stegen: tillhandahållande av information om de maximalt tillåtna driﬁsrörelsehastighetema för varje objektsaxel och information om de maximalt tillåtna lägesförﬂyttiiingshastigheterna för varje objektsaxel; beräkning av avståndet ﬁ-ån det beﬁntliga läget till det önskade läget för varje nämnd axel; be- räkning av den fórﬂyttningshastighet som krävs för att förflytta objektet från det be- fintliga läget till det önskade läget för varje axel; jämförelse av, för varje axel, den förﬂyttningshastighet som behövs mot de maximalt tillåtna förﬂyttningshastigheter- na, för att bestämma vilken förﬂyttningshastighet som är lägst: och användning av den lägsta förﬂyttningshastigheten på varje axel. Detta har effekten att förhindra för- flyttningshastigheten i en axel vid ett visst läge under en viss manövrering av robo- ten från att överskrida ett förutbestämt maximalt värde för den axeln. Metoden kan genomföras med en anordning innefattande ett objekt som iir förtlyttbart av manöv- 10 15 20 30 515 949 3 reringsorgan där manövreringsorganen regleras av reglermedel, varvid reglerrnedlet innefattar ett minne innehållande information om de maximalt tillåtna driﬁsförﬂytt- ningshastighetema för varje objektsaxel och information om de maximalt tillåtna lägesförﬂyttningshastighetema för varje objektsaxel, vari den innefattar reglermedel för järnförelse av de maximalt tillåtna driftsförflytmingshastighetema för varje ob- jektsaxel och de maximalt tillåtna lägesñrﬂyttningshastigheterna för varje objekts- axel mot en befmtlig, krävd förﬂyttningshastighet för varje objektsaxel och regler- medel (8) för förflyttning av objektet med en förﬂyttningshastighet som motsvarar den lägre av den befintliga, krävda förﬂyttningshastigheten eller de maximalt tillåt- na förﬂytmingshastigheterna.Thus, preferably, a method in accordance with the invention for controlling the movement of an object to be moved from an existing position to a desired position where the distance between the center position and the desired position is matched at predetermined sampling intervals comprises the steps of providing information. information on the maximum permissible speeds of movement for each object axis and information on the maximum permissible positioning speeds for each object axis; calculating the distance ﬁ- from the fixed position to the desired position for each said axis; calculating the feed speed required to move the object from the existing position to the desired position for each axis; comparing, for each axle, the ﬂ speed of rotation required against the maximum permissible ﬂ speeds, to determine which ﬂ speed is lowest: and using the lowest ﬂ speed on each axis. This has the effect of preventing the speed of movement in an axis at a certain position during a certain operation of the robot from exceeding a predetermined maximum value for that axis. The method can be carried out with a device comprising an object which can be relied on by operating means, the operating means being controlled by control means, the control means comprising a memory containing information about the maximum permissible operating speeds for each object axis and information about the the maximum permissible positioning ﬂ velocities of displacement of each object axis, wherein it includes means for regulating the maximum permissible operating displacement velocities of each object axis and the maximum permissible position ﬂ the velocities ﬂ the velocities of each object axis against an existing movement of the object at a ﬂ speed that corresponds to the lower of the existing one, required for the eller speed or the maximum permissible ﬂ speeds.

En alternativ metod i enlighet med uppﬁnningen för reglering av förﬂyttningen av ett objekt innefattar stegen: tillhandahållande av information om de maximalt tillåt- na driftsförﬂyttrringshastighetema för varje objektsaxel; beräkning av avståndet från det befintliga läget till det önskade läget för varje nämnd axel; beräkning av den för- ﬂyttningshastighet som krävs för att förﬂytta objektet från det beﬁntli ga läget till det önskade läget för varje nämnd axel; jämförelse av, för varje axel, den förﬂyttnings- hastighet som krävs mot de maximalt tillåtna driftsförﬂytmingshastigheterna för att bestämma vilken förﬂytmingshastighet som är lägst; och användning av den lägsta förﬂyttningshastigheten på varje axel. Detta har effekten att förhindra förﬂyttnings- hastigheten i en viss axel under en viss manöver av roboten ﬁån att överskrida ett förutbestämt maximalt värde för den axeln. Metoden kan utföras av en anordning innefattande ett objekt som är förflyttbart av manövreringsorgan vari manövrerings- organen regleras av reglerrnedel där reglerrnedlet innefattar ett minne innehållande information om de maximalt tillåtna driitsförﬂyttningshastighetema för varje ob- jektszmel, där den vidare innefattar reglerrnedel förjämförelse av de maximalt tillåt- na driftstörtlyttningshastigheterna för varje objektsaxel mot en befintlig, krävd för- flyttningshastighet for varje objektsaxel och reglermedel för att förflytta objektet med en förﬂyttningshastighet som motsvarar den lägre av den befintliga, krävda tör- flyttningshastigheten eller den maximalt tillåtna förtlyttningshastigheten. 10 15 20 25 30 53.13 949 4 En förenklad metod för reglering av förﬂyttningen av ett objekt, som ska förﬂyttas ﬁån ett beﬁntligt läge till ett önskat läge, där avståndet mellan' det befintliga läget och det önskade läget samplas vid förutbestämda samplingsintervaller, innefattar stegen: tillhandahållande av information om de maximalt tillåtna förﬂyttningshas- tigheterna för varje objektsaxel; beräkning av avståndet från det befintliga läget till det önskade läget för varje axel; beräkning av den förﬂytmingshastighet som behövs för att förﬂytta objektet från det befintliga läget till det önskade läget för varje axel; jämförelse av, för varje axel, den förﬂyttningshastighet som behövs mot de maxi- malt tillåtna lägesförflyttningshastigheterna för att bestämma vilken förﬂytmings- hastighet som är lägst; och användning av den lägsta fórﬂyttningshastigheten på varje axel. Detta har effekten att förhindra förﬂyttningshastigheten i en axel vid ett visst läge från att överskrida ett förutbestämt maximalt värde för den axeln. Meto- den kan utföras av en anordning innefattande ett genom manövreringsorgan rörligt Objekt, vari manövreringsorganen regleras av reglermedel och där reglerrnedlet in- nefattar ett minne innefattande information om de maximalt tillåtna lägesfórﬂytt- ningshastighetema för varje objektsaxel, där den vidare innefattar reglermedel för jämförelse av de maximalt tillåtna lägesförﬂyttningshastigheterna för varje objekts- axel mot en befmtlig, krävd förﬂyttningshastifghet för varje objektsaxel och regler- medel för att förflytta objektet med en förflyttningshastighet som motsvarar den läg- re av den befintliga, krävda förﬂyttningshastigheten eller den maximalt tillåtna för- flyttningshastigheten.An alternative method in accordance with the invention for controlling the movement of an object comprises the steps of: providing information on the maximum permissible operating speeds of the expression axis of each object axis; calculating the distance from the existing position to the desired position for each said axis; calculating the displacement speed required to move the object from the initial position to the desired position for each said axis; comparing, for each axis, the ﬂ speed of krävs required against the maximum permissible operating för speeds to determine which ﬂ speed is lowest; and using the minimum for the ﬂ speed of each axis. This has the effect of preventing the ﬂ speed of a certain axis during a certain maneuver of the robot ﬁ from exceeding a predetermined maximum value for that axis. The method may be performed by a device comprising an object movable by actuators wherein the actuators are controlled by control means wherein the control means comprises a memory containing information about the maximum allowable operating speeds of each object, further comprising control means comparing the maximum control means. the operating override velocities of each object axis against an existing, required displacement speed for each object axis and control means for moving the object at a displacement speed corresponding to the lower of the existing, required dry displacement velocity or the maximum allowable displacement speed. 10 15 20 25 30 53.13 949 4 A simplified method of controlling the movement of an object to be moved from a fixed position to a desired position, where the distance between the existing position and the desired position is sampled at predetermined sampling intervals, comprises the steps of: providing information on the maximum allowable för velocities for each object axis; calculating the distance from the existing position to the desired position for each axis; calculating the displacement speed required to move the object from the existing position to the desired position for each axis; comparing, for each axis, the ﬂ displacement speed required against the maximum permissible position travel speeds to determine which ﬂ surface velocity is lowest; and using the lowest feed speed on each axle. This has the effect of preventing the ast velocity of a shaft at a certain position from exceeding a predetermined maximum value for that shaft. The method may be performed by a device comprising an object movable by actuators, wherein the actuators are controlled by control means and wherein the control means includes a memory comprising information about the maximum permissible position travel speeds of each object axis, further comprising control means for comparing the the maximum permissible positioning för velocities of each object axis against an existing, required for ﬂ velocity for each object axis and control means for moving the object at a displacement speed corresponding to the lower of the existing, required for the till velocity or the maximum moving speed.

Metoderna enligt ovan är speciellt användbara för att reglera en robotarm i en mjölkrobot med hjälp av vilka skaderisken på djur och skaderisken pâ maskineriet reduceras.The methods as above are particularly useful for regulating a robot arm in a milking robot by means of which the risk of injury to animals and the risk of injury to machinery is reduced.

Metoden enligt ovan kan också användas till ett verktyg anslutet till en robotarm i en mjölkrobot. 10 15 20 25 30 =~51z 949 5 Upplinningen kommer nedan att beskrivas mer detaljerat med hjälp av utföringsf formsexempel och de bifogade ﬁgurema.The method as above can also be used for a tool connected to a robot arm in a milking robot. 10 15 20 25 30 = ~ 51z 949 5 The winding will be described in more detail below with the aid of exemplary embodiments and the accompanying figures.

Sarmnanfattande beskrivning av ritningarna Pig. 1 visar schematiskt en mjölkrobot för användning med metoden enligt uppﬁn- ningen; F ig. 2 visar ett ﬂödesdiagrarn för en metod enligt en utfóringsform av uppfinningen.Summary description of the drawings Pig. 1 schematically shows a milking robot for use with the method according to the invention; F ig. 2 shows a fate diagram for a method according to an embodiment of the invention.

Detaljerad beskrivning av uppﬁnningen Fig. 1 visar schematiskt en mjölkrobot 1 med en robotarrn 3. Robotannen 3 är ﬁxe- rad till en ram 5 och är ansluten till reglermedel såsom en dator 7. Dator 7 har ett minne 8 innehållande drifts-programvara. Robotarrn 3 är manövrerbar i x-, y- och z- axeln med hjälp av lämpliga manövreringsmedel. I den här figuren är endast u-e ma- növreringsmedel visade lör enkelhets skull, nämligen pneumatiska manövreringsor- gan 9, 10, ll och respektive svängbara leder 13, 14, 15, men det är självklart möjligt att använda varje lämpligt nummer och typ av manövreringsmedel. Således kan ro- botarrn 3 vara anordnad med teleskopiska lederjärnte, eller istället lör, svängbara leder. Robotarm 3 är vid en ände 17 försedd med verktygsmedel, t. ex. ett tvätt- ningsmedel, borsmingsmedel eller, som visas här, en spenkopp 19 och avkännings- medel, t. ex. en kamera 21. Programvaran i dator 7 reglerar fórﬂyttningen av robot- arm 3 användande information från kamera 21 i syfte att positionera spenkoppen 19 i det önskade läget (som visas med prickade linjer) runt en spene 23 på juvret 25 hos djuret som mjölkas.Detailed description of the invention Fig. 1 schematically shows a milking robot 1 with a robot arm 3. The robot tooth 3 is attached to a frame 5 and is connected to control means such as a computer 7. Computer 7 has a memory 8 containing operating software. The robot 3 can be operated in the x-, y- and z-axis by means of suitable operating means. In this figure, only u-e actuators are shown for simplicity, namely pneumatic actuators 9, 10, 11 and respective pivotable joints 13, 14, 15, but it is of course possible to use any suitable number and type of actuator. Thus, the robot 3 can be arranged with telescopic hinge irons, or instead pivotable, pivotable joints. Robot arm 3 is provided at one end 17 with tool means, e.g. a detergent, brushing agent or, as shown here, a teat cup 19 and sensing agent, e.g. a camera 21. The software in computer 7 regulates the movement of robot arm 3 using information from camera 21 in order to position the teat cup 19 in the desired position (shown in dotted lines) around a teat 23 on the udder 25 of the animal being milked.

Ett llödesdiagram lör en metod lör reglering av en robotann i enlighet med upplin- ningen visas i lig. 2. F lödesdiagrammet härrör sig till lörllyttningar i en axel. t. ex. x-axeln och det ska lörstâs att y-axeln och z-axeln kan regleras på ett liknande sätt. 10 15 20 30 ”513 949 Funktion I syfte att förenkla beskrivningen ska uppfinningen illustreras genom ett exempel i vilket robotaimens reglerrnedel innefattar en dator 7 och programvara i form av ett reglerprogram innefattar i ett minne 8 i datom, fastän det är möjligt att reglerfunk- tionen kan utföras användande ett fast, programmerat regler-medel. ReglerprogTam- met reglerar förﬂyttningen av robotarm 3 genom att sända instruktioner till ro- botarmens 3 pneumatiska manövreringsorgan 9, 10, 11. Helst innehåller reglerpro- grammet information om hinder i robotarmens 3 region så att robotarmen inte be- ordras att kollidera med t. ex. mjölkmaskinens ram 5. instruktionerna ﬁân regler- programmet innehåller information om hur långt varje manövreringsorgan 9, 10, 11 får utvidgas eller dras tillbaka och med vilken förﬂyttningshastighet (d.v.s. farten) utvidgningen eller indragningen sker. F öredragna törﬂytmingshastigheter lagras i ett minne, t. ex. minne 8, i datorn. Förﬂyttningshastigheten kan vara vilket godtyckligt värde som helst eller, i syﬁe att spara mirmesutrymme, så kan rörelsers hastighet väljas från ett begränsat område av hastighet för förﬂyttningar. T. ex. kan förflytt- ningshastigheten delas in i fyra olika fórﬂytmingshastigheter såsom "snabb" mot- svarande 400 mm per sekund, "medel" motsvarande 200 mm per sekund, "sakta" motsvarande 100 mm per sekund och "krypande" motsvarande 50 mm per sekund.A solder diagram for a method for regulating a robot tooth in accordance with the alignment is shown in FIG. 2. The flow chart is derived from ear movements in an axis. e.g. the x-axis and it should be understood that the y-axis and the z-axis can be adjusted in a similar way. 10 15 20 30 ”513 949 Function In order to simplify the description, the invention will be illustrated by an example in which the control part of the robot comprises a computer 7 and software in the form of a control program comprises in a memory 8 in the computer, although it is possible that the control function can be performed using a fixed, programmed control means. The control program regulates the ﬂ the movement of robot arm 3 by sending instructions to the robot arm 3's pneumatic actuators 9, 10, 11. Preferably, the control program contains information about obstacles in the robot arm 3 region so that the robot arm is not ordered to collide with e.g. . the frame of the milking machine 5. the instructions in the control program contain information on the extent to which each actuator 9, 10, 11 may be extended or retracted and the speed at which the expansion or retraction takes place. Preferred drying speeds are stored in a memory, e.g. memory 8, in the computer. The velocity of the displacement can be any value or, in order to save mirror space, the velocity of movements can be selected from a limited range of velocities for displacements. Eg. For example, the travel speed can be divided into four different flow rates such as "fast" corresponding to 400 mm per second, "medium" corresponding to 200 mm per second, "slow" corresponding to 100 mm per second and "creeping" corresponding to 50 mm per second.

Den förinställda förﬂyttningshastigheten, d.v.s. förﬂyttningsfarten, för robotarmen kan sättas på "snabb" hastigheten i syﬁe att undvika införning av en onödigt låg gräns på robotarmens förﬂyttningsfart. Användaren eller installatören av mjölkma- Skinen är dessutom i stånd till att sätta olika maximalt tillåtna förﬂyttningshastighe- ter för olika manövreringskrav och/eller för olika manövreringszoner. Så när robot- armen arbetar under en period när det inte finns något djur i mjölkmaskinen, kan den maximalt tillåtna förﬂyttningshastigheten sättas till "snabb" i syfte att maximera armens förﬂyttningstart och därmed spara tid när det _inte finns någon risk att något djur blir skadat. Samtidigt kan en zon bildas runt ramverksdelar av maskinen i vil- ken den maximalt tillåtna törﬂyttningshastigheten sätts till "krypande" för att för- hindra att robotarmen närmar sig ramverket med en hög hastighet och kanske träffar 10 -15 20 25 30 51.3 949 7 ramverket om den passerar sitt avsedda läge p. g. a. sitt stora moment. De maximalt tillåtna törflyttningshastighetema definierade av användaren eller installatören kan lagras i aa taban s” for maximalt :iiiama daffsrofﬂynaiagahaaíighataf aan aa faaan 8” för maximalt tillåtna lägestörﬂytmingshastigheter i datorns 7 minne 8. De maximalt tillåtna driftstörﬂytmingshastighetema gäller särskilda förfaranden i driﬁsprogrammet medan de maximalt tillåtna förﬂyttningshastigheterna gäller ro- botarrnens beﬁntliga läge.The preset for the ﬂ speed, i.e. for the speed of movement, for the robot arm can be set to the "fast" speed in order to avoid the introduction of an unnecessarily low limit on the speed of the robot arm. The user or installer of the milking machine is also able to set different maximum permissions for ﬂ speed speeds for different operating requirements and / or for different operating zones. So when the robot arm works for a period when there is no animal in the milking machine, the maximum allowable speed can be set to "fast" in order to maximize the arm's start start and thus save time when there is no risk of any animal being injured. At the same time, a zone can be formed around the frame parts of the machine in which the maximum permissible dry ﬂ speed is set to "creep" to prevent the robot arm from approaching the frame at a high speed and perhaps hitting the frame 10 -15 20 25 30 51.3 949 7 it passes its intended position due to its large torque. The maximum permissible dry moving speeds defined by the user or installer can be stored in aa taban s "for maximum: iiiama daffsrof ﬂ ynaiagahaaíighataf aan aa faaan 8" for maximum permissible position disturbance ﬂ permissions speeds in the computer's 7 memory 8. applies to the robot's dangerous position.

Under användning vid reguljära intervaller på t. ex. 20 ms, d.v.s. 50 gånger per se- kund, jämför datom 7 det befintliga läget för robotannen i x-, y- och z-axeln med robotarmens önskade läge i x-, y- och z-axeln. Med en samplingshastighet på 50 gånger per sektmd motsvarar en snabb iörﬂyttningshastighet en förﬂyttriing på 8 mm mellan varje samplingsintervall, en medeltörﬂytmingshastighet motsvarar 4 mm, en långsam förﬂyttningshastighet motsvarar 2 mm och en krypfórtlytmings- hastighet motsvarar 1 mm. Om det ﬁnns ett fel mellan ett beﬁntligt läge med avse- ende på en axel och det önskade läget på samma axel som är större än ett förutbe- Stämt, tillåtet fel, t. ex. 1 mm, bestämmer datom vilket manövreringsorgan 9, 10, ll som behöver utvidgas eller dras tillbaka och beräknar hur långt det behöver utvidgas eller dras tillbaka för att rätta till felet. Datom 7 beräknar sedan den krävda förﬂytt- ningshastighet som är nödvändig i syﬁe att reducera felet till noll före nästa jämfö- relse mellan det beﬁntliga och önskade läget. Så om det finns ett fel på 6 mm blir den krävda törﬂytmingshastigheten 300 mm per sekund. Datom jämför denna kräv- da förﬂyttningshastighet mot de maximalt tillåtna törﬂyttningshastighetema i tabel- lema 8°, 8” för de maximalt tillåtna driﬁs- och lägestörﬂyttningshastighetema för den pågående arbetsfasen och robotarmens läge. Om någon av eller båda de maxi- malt tillåtna förﬂyttningshastighetema är mindre 300 mm per sekund sätter då datorn den krävda förﬂyttningshastigheten till det lägsta av de maximalt tillåtna för- ﬂyttningshastighetema. Om ingen av de maximalt tillåtna törtlyttningshastighetcrna iir mindre än 300 mm per sekund sätts den krävda törtlyttningshastigheten till det förinställda värdet, t. ex. 400 mm per sekund eller alternativt sätts i en annan uttö- 10 15 20 g 513 949 8 ringsform av uppfinningen den krävda hastigheten till den av datorn beräknade krävda hastigheten, d.v.s. i detta fall 300 mm per sekund.During use at regular intervals of e.g. 20 ms, i.e. 50 times per second, the computer 7 compares the existing position of the robot tooth in the x, y and z axis with the desired position of the robot arm in the x, y and z axis. With a sampling rate of 50 times per sectmd, a fast i ﬂ velocity corresponds to an ﬂ expression of 8 mm between each sampling interval, an average dry ﬂ velocity corresponds to 4 mm, a slow to ﬂ velocity corresponds to 2 mm and a creep pre-emptiness rate corresponds to 1 mm. If there is an error between a fixed position with respect to an axis and the desired position on the same axis which is greater than a predetermined, permissible error, e.g. 1 mm, the computer determines which actuator 9, 10, ll needs to be extended or retracted and calculates how far it needs to be extended or retracted to correct the error. The date 7 then calculates the required for the speed of expression that is necessary in order to reduce the error to zero before the next comparison between the exact and desired mode. So if there is an error of 6 mm, the required dry surface speed will be 300 mm per second. The date compares this required for ﬂ velocity with the maximum permitted dry ast velocities in tables 8 °, 8 ”for the maximum permissible operating and position disturbances ﬂ velocities for the current working phase and the position of the robot arm. If one or both of the maximum allowable speed speeds are less than 300 mm per second, then the computer sets the required speed speed to the lowest of the maximum allowable speed speeds. If none of the maximum permissible torque listening speeds is less than 300 mm per second, the required torque tapping speed is set to the preset value, e.g. 400 mm per second or alternatively, in another embodiment of the invention, the required speed is set to the required speed calculated by the computer, i.e. in this case 300 mm per second.

Om således robotarmen 3 är i ett läge under djuret och är i den sökande arbetsfasen för att fästa en spenkopp 19 till en spene 23, kan den maximalt tillåtna driftsförﬂytt- níngshastigheten t. ex. sättas till medel ñr alla tre axlarna. Vid samma tid kan de maximalt tillåtna lägesförﬂyttningshastighetema sättas till krypning för alla tre ax- larna i utrymmet direkt under juvret 25 och den kan sättas till sakta i regionen under djuret, omgivande utrymmet direkt under juvret. När robotarmen 3 inte är under dju- ret kan då de maximalt tillåtna lägestörﬂyttningshastigheterna vara snabba i x- och y-axeln och långsam i z-axeln. När således spenkoppen 19 förflyttas mot djuret ﬁ-ån ett läge vilket inte är under djuret registrerar datorn 7 ett stort avstånd mellan spenen 23 och spenkoppen 19, vilket leder till en krävd förﬂyttningshastighet på t. ex. 1800 mm per sekund. Datorn 7 jämför denna krävda förﬂytmingshastighet mot tabellen för de maximalt tillåtna driftsförflytminghastigheterna för denna drivning och ser till att de maximalt tillåtna förﬂyttningshastighetema är medel (200 mrn/s) i alla tre axlarna. Datorn 7 jämför sedan den krävda hastigheten mot tabellen för de maximalt tillåtna lägesförﬂyttningshastighetema för det rådande läget och ser till att de maxi- malt tillåtna förflyttriingshastigheterna är snabba (400 mm/s) i x- och y-axeln och långsamma (100 mm/s) i z-axeln. Datorn väljer det lägsta värdet för varje axel och förﬂyttar därför robotarmen 3 med de maximalt tillåtna driftsförflyttningshastighe- terna på 200 mm/s i x- och y-axlania och med den maximalt tillåtna lägesförﬂytt- ningshastigheten på 100 mm/s i z-axeln. En ny sampling tas och samma resultat uppnås. Detta pågår tills robotarmen kommer in i regionen under djuret där en ny uppsättning av lägre maximalt tillåtna lägesförﬂyttningshastigheter träder i kraft. i vilket fall farten reduceras till de lägsta maximalt tillåtna värdena eller tills armen är så nära målet att den krävda hastigheten ger det lägsta värdet. d.v.s. den krävda has- tigheten är mindre det lägsta maximalt tillåtna värdet. 10 15 20 -513 949 9 I en förenklad utföringsform av uppﬁnningen används bara en tabell för de maki- malt tillåtna förﬂytmingshastigheterna, antingen en drifts- eller lägestabell för de ß maximalt tillåtna förﬂyttningshastigheterna.Thus, if the robot arm 3 is in a position below the animal and is in the searching working phase for attaching a teat cup 19 to a teat 23, the maximum permitted operating speed can e.g. set to means ñr all three axes. At the same time, the maximum allowable position displacement velocities can be set to creep for all three axes in the space directly below the udder 25 and it can be set slowly in the region below the animal, surrounding the space directly below the udder. When the robot arm 3 is not under the animal, then the maximum permissible position disturbance ﬂ velocities can be fast in the x- and y-axis and slow in the z-axis. Thus, when the teat cup 19 is moved towards the animal ﬁ- from a position which is not below the animal, the computer 7 registers a large distance between the teat 23 and the teat cup 19, which leads to a required for ﬂ speed of e.g. 1800 mm per second. The computer 7 compares this required for the ﬂ velocity speed with the table for the maximum permissible operating displacement speeds for this drive and ensures that the maximum permissible for the ﬂ velocity velocities are means (200 mn / s) in all three axes. The computer 7 then compares the required speed with the table for the maximum permissible positioning speeds of the prevailing position and ensures that the maximum permitted travel speeds are fast (400 mm / s) in the x- and y-axis and slow (100 mm / s ) in the z-axis. The computer selects the lowest value for each axis and therefore ﬂ moves the robot arm 3 with the maximum permissible operating travel speeds of 200 mm / s in x- and y-axes and with the maximum permissible positioning speed of 100 mm / s in the z-axis. A new sample is taken and the same result is achieved. This continues until the robot arm enters the region under the animal where a new set of lower maximum allowable movement speeds takes effect. in which case the speed is reduced to the lowest maximum permissible values or until the arm is so close to the target that the required speed gives the lowest value. i.e. the required speed is less than the minimum maximum permissible value. 10 15 20 -513 949 9 In a simplified embodiment of the invention, only a table for the maximum permissible ﬂ velocities is used, either an operating or position table for the ß maximum permissible for the ﬂ velocities.

Det är tänkbart att robotarmen är begränsad till ett begänsat antal tillåtna förflytt- ningshastigheter. I det här fallet, i händelse av att den önskade hastigheten ligger mellan två tillåtna hastigheter, används då den mest långsamma av de två tillåtna hastigheterna. Även om uppﬁnningen har illustrerats med en robotarrn som är rörlig i alla tre orto- gonala axlarna är det tänkbart att ha en robotarm som inte är förﬂyttbar i z-axeln. I detta fall kan förﬂytlningen som behövs i z-axeln ges genom en vertikalt förskjutbar verktygshållare monterad på robotarmen. Även om uppﬁnningen visas med användande av ett exempel där robotarmens be- ﬁntliga läge jämförs mot ett önskat läge, är det naturligtvis också möjligt att jämföra det befmtliga läget och det önskade läget för t. ex. verktyget som är anbringat på ro- botarrnen eller någon rörlig del av anordningen.It is conceivable that the robot arm is limited to a limited number of permitted travel speeds. In this case, in the event that the desired speed is between two permitted speeds, then the slower of the two permitted speeds is used. Although the design has been illustrated with a robot arm that is movable in all three orthogonal axes, it is conceivable to have a robot arm that is not too movable in the z-axis. In this case, the required displacement in the z-axis can be provided by a vertically displaceable tool holder mounted on the robot arm. Although the invention is shown using an example where the robot's significant position is compared to a desired position, it is of course also possible to compare the existing position and the desired position for e.g. the tool mounted on the robot or any moving part of the device.

Claims

1. 0 15 20 25 30 513 94910 Patentkrav1. 0 15 20 25 30 513 94910 Patent claims

1. Metod för en mjölkrobot (1) vid reglering av förﬂyttning av ett objekt (3, 19) som ska förflyttas från ett beﬁntligt läge till ett önskat läge där avståndet mellan det befintliga läget och det önskade läget samplas vid förutbestämda samplingsintervaller, kännetecknad av stegen: tillhandahållande av information om maximalt tillåtna förﬂyttningshastigheter för varje axel hos objektet (3, 19); beräkning av avståndet från det beﬁntliga läget till det önskade läget för var och en av axlarna; beräkning av den förﬂyttningshastighet som behövs för att fórﬂytta objektet (3, 19) från det befintliga läget till det önskade läget för var och en av axlarna; jämförelse av, för var och en av axlama, den förﬂyttningshastighet som behövs mot de maximalt tillåtna förﬂyttningshastigheterna för att bestämma vilken förflyttningshastighet som är lägst; och användning av den lägsta förﬂyttningshastigheten för var och en av axlarna. . Method of a milking robot (1) in controlling the movement of an object (3, 19) to be moved from a fixed position to a desired position where the distance between the existing position and the desired position is sampled at predetermined sampling intervals, characterized by the steps : providing maximum allowable till velocity information for each axis of the object (3, 19); calculating the distance from the steady state to the desired position for each of the axes; calculating the displacement speed required to move the object (3, 19) from the existing position to the desired position for each of the axes; comparing, for each of the axles, the for ﬂ velocity of speed required against the maximum permissible for ﬂ the velocities of speed to determine which movement speed is lowest; and using the lowest ﬂ speed for each of the axles. .

Metod enligt krav l, kännetecknad av att de maximalt tillåtna förﬂyttningshastig- hetema för var och en av axlama hos objektet (3, 19) är maximalt tillåtna drifts- förflyttningshastigheter för var och en av axlarna hos objektet (3, 19). _ Metod enligt krav 1, kännetecknad av att de maximalt tillåtna förﬂyttningshastig- hetema för var och en av axlarna hos objektet (3, 19) är maximalt tillåtna lägesför- ﬂyttningshastigheter för var och en av axlarna hos objektet (3, 19). . Method according to Claim 1, characterized in that the maximum permissible för speed velocities for each of the axes of the object (3, 19) are the maximum permissible operating travel speeds for each of the axes of the object (3, 19). Method according to claim 1, characterized in that the maximum permissible för speed velocities for each of the axes of the object (3, 19) are the maximum permissible position displacement för velocities for each of the axes of the object (3, 19). .

Metod enligt krav l, kännetecknad av att de maximalt tillåtna förﬂyttningshastig- hetema för var och en av axlarna hos objektet (3, 19) är maximalt tillåtna lägesför- ﬂyttningshastigheter och driﬁsförﬂyttningshastigheter för var och en av axlarna hos objektet (3, 19). . Method according to claim 1, characterized in that the maximum permissible för speed velocities for each of the axes of the object (3, 19) are maximum permissible position ﬂ displacement velocities and three ﬁ displacement ﬂ velocities for each of the axes of the object (3, 19). .

Metod enligt något av de föregående kraven, kännetecknad av att objektet (3) är en robotarm (3). 10 15 20 25 513 949 11 . Method according to one of the preceding claims, characterized in that the object (3) is a robot arm (3). 10 15 20 25 513 949 11.

Metod enligt något av kraven 1-5, kännetecknad av att objektet (19) är ett verktyg (19) anbringat på en robotarm (3). . Method according to one of Claims 1 to 5, characterized in that the object (19) is a tool (19) mounted on a robot arm (3). .

Mjölkrobot innefattande ett objekt (3, 19) som är fórﬂyttriingsbart av manövreringsorgan (9, 10, 1 1) där manövreringsorganen regleras av reglermedel (7), kännetecknar] av att reglerrnedlet (7) innefattar ett minne (8) innehållande information (8', 8”) om maximalt tillåtna fórﬂytmingshastigheter för varje axel hos objektet (3, 19), där objektet innefattar reglerrnedel (7) för jämförelse av de maximalt tillåtna förﬂyttningshastighetema för var och en av axlarna hos objektet (3, 19) mot en befintlig, krävd förﬂyttningshastighet för var och en av axlarna hos objektet (3, 19) och regler-medel (7) for forﬂytming av objektet (3, 19) med en förﬂyttningshastighet som motsvarar den lägre av den beﬁntliga, krävda fórﬂytmingshastigheten eller de maximalt tillåtna förﬂyttningshastighetema. . Milking robot comprising an object (3, 19) which can be expressed by operating means (9, 10, 1 1), the operating means being controlled by control means (7), characterized in that the control means (7) comprises a memory (8) containing information (8 '). , 8 ") of the maximum permissible flow velocities for each axis of the object (3, 19), the object including control element (7) for comparing the maximum permissible ﬂ velocity velocities of each of the axes of the object (3, 19) against an existing one, required for the displacement velocity of each of the axes of the object (3, 19) and control means (7) for displacing the object (3, 19) with a displacement velocity corresponding to the lower of the actual required displacement velocity or the maximum allowable displacement velocities. .

Mjölkrobotnenligt patentkrav 7, kännetecknad av där de maximalt tillåtna fórflyttningshastigheterna for varje axel av objektet (3, 19) är maximalt tillåtna driﬁsförﬂytmingshastigheter. . Milk robot according to claim 7, characterized in that the maximum permissible feed displacement speeds for each axis of the object (3, 19) are the maximum permissible drive flow velocities. .

Mjölkrobot enligt patentkrav 7, kännetecknad av där de maximalt tillåtna fórﬂyttningshastighetema for varje axel av objektet (3, 19) är maximalt tillåtna lägesfórﬂyttningshastigheter.Milking robot according to claim 7, characterized in that the maximum permissible feed velocities for each axis of the object (3, 19) are maximum permissible position feed velocities.

10. Mjölkrobot enligt patentkrav 7, kännetecknad av där de maximalt tillåtna fórﬂyttningshastighetema för varje axel av objektet (3, 19) är maximalt tillåtna driftsfórﬂyttningshastigheter och maximalt tillåtna lägesfórﬂyttningshastigheter.Milking robot according to claim 7, characterized in that the maximum permissible feed velocities for each shaft of the object (3, 19) are maximum permissible operating feed velocities and maximum permissible position feed velocities.