NL8800272A

NL8800272A - Ultrasonic detector for automatic milking device - uses ultrasonic sensors for feeding and-or following teat of cow, and terminal appts. for controlling milking operation

Info

Publication number: NL8800272A
Application number: NL8800272A
Authority: NL
Original assignee: Multinorm Bv
Priority date: 1988-02-04
Filing date: 1988-02-04
Publication date: 1989-09-01

Abstract

The ultrasonic detector comprises a transmitter and a receiver for ultrasonic waves and a conical reflection surface disposed in the vicinity of the transmitter for reflection of the ultrasonic waves, in order to obtain a detection field of predetermined dimensions. In the vicinity of the receiver a conical reflection surface is disposed for determining the area within which the ultrasonic waves are reflected back to the receiver. The transmitter and receiver are integrated into a transmitter/ receiver unit. The transmitter/receiver unit is disposed in lying position while the conical reflection surface is located above it. The reflections from the moving teat measured by the ultrasonic sensors (52, 69, 71) are converted into sampled digital values/delta x, delta y) being relative position coordinates w.r.t. the sensors.

Description

-1- / H AL/CP/469 Mult.-1- / H AL / CP / 469 Mult.

WERKWIJZE VOOR HET MET BEHULP VAN EEN ROBOT ZOEKEN ΕΝ/OF VOLGEN VAN EEN BEWEGEND VOORWERP, ROBOTSYSTEEM GEBRUIKMAKEND VAN DEZE WERKWIJZE EN ROBOTSYSTEEM VOOR HET AUTOMATISCH MELKEN VAN EEN VROUWELIJK ZOOGDIER.METHOD FOR SEARCHING A ROBOT AND / OR TRACKING A MOVING OBJECT, ROBOT SYSTEM USING THIS METHOD AND ROBOT SYSTEM FOR AUTOMATIC MILKING OF A FEMALE MAMMALIAN.

Bestaande robotsystemen voor het zoeken, vinden en/of volgen van een (bewegend) voorwerp dat deel uitmaakt van een groter geheel, bleken niet in staat te voldoen aan vereisten voor wat betreft betrouwbaarheid, snelheid en/of 5 mogelijkheden van een foutcorrectie.Existing robot systems for searching, finding and / or tracking a (moving) object that is part of a larger whole, were found to be unable to meet requirements in terms of reliability, speed and / or error correction capabilities.

De onderhavige uitvinding verhelpt bovengenoemde nadelen.The present invention overcomes the above drawbacks.

Door, volgens conclusie 1, gebruik te maken van positie- en snelheidsinformatie, wordt een nauwkeurigere 10 positiebepaling verkregen en wordt informatie verkregen omtrent het bewegingspatroon van het (bewegend) voorwerp; ook bij heftige bewegingen zal de snelheidsinformatie hieromtrent beschikbaar zijn.By, according to claim 1, using position and speed information, a more accurate position determination is obtained and information is obtained about the movement pattern of the (moving) object; speed information will also be available in case of violent movements.

Bij de werkwijze volgens conclusie 2 wordt informa-15 tie verkregen omtrent fouten en/of andere onregelmatigheden in hetzij het meetproces, hetzij het regelproces dan wel in de bewegingen van het voorwerp.In the method according to claim 2 information is obtained about errors and / or other irregularities in either the measuring process, the control process or in the movements of the object.

Verdere voordelen, kenmerken en details van de onderhavige uitvinding zullen duidelijk worden aan de hand 20 van een uitvoeringsvoorbeeld daarvan, dat beschreven is aan de hand van een tekening, waarin tonen: fig. 1 een perspectivisch aanzicht van een robot-systeem voor implementatie van een gedeelte van een voorkeur suitvoerings vorm van de werkwijze volgens de uitvinding; 25 fig. 2 een blokschema van de werking van het robot- systeem uit fig, 1; en fig. 3 een blokschema van een andere voorkeurswerking van het robotsysteem uit fig. 1.Further advantages, features and details of the present invention will become apparent from an embodiment thereof, which is described with reference to a drawing, in which: Fig. 1 shows a perspective view of a robot system for implementation of a part of a preferred embodiment of the method according to the invention; Fig. 2 is a block diagram of the operation of the robot system of Fig. 1; and FIG. 3 is a block diagram of another preferred operation of the robot system of FIG. 1.

De voorkeursuitvoeringsvorm betreft een werkwijze 30 en robotsysteem voor het melken van een in fig. l niet ge- .8800272 t -2- # toonde koe. Een robotsysteem 1 omvattende een aantal ten opzichte van elkaar scharnierbare armen, is beweegbaar langs rails 7, 8 ten einde in staat te zijn bij verschillende loka-ties zoek-, vind- en/of volgoperaties uit te voeren voor het 5 aanbrengen van melkbekers 15 van een melkrek 2 bij een koe.The preferred embodiment concerns a method and a robotic system for milking a cow shown in Fig. 1, not shown in 8800272 -2. A robot system 1 comprising a number of arms pivotable relative to each other, is movable along rails 7, 8 in order to be able to carry out search, find and / or follow operations at different locations for arranging teat cups 15. of a milk rack 2 in a cow.

De koe wordt met haar kop in de richting van een voederbak 21 geleid, opdat haar uier zich in de nabijheid van een ruststand van het melkrek 2 zal bevinden; een voerderbak 21 is beweegbaar volgens pijl A, opdat de uier van de koe zich 10 veelal op ongeveer dezelfde of eenduidige positie ten opzichte van het melkrek 2 zal bevinden.The cow is guided with its head in the direction of a feed trough 21, so that its udder will be in the vicinity of a rest position of the milking rack 2; a feed trough 21 is movable according to arrow A, so that the udder of the cow will usually be in approximately the same or unambiguous position relative to the milking rack 2.

Veelal zal de koe automatisch herkend zijn en zal informatie omtrent positie van haar uier en tepels bijv. in een niet getoond centrale stuurorgaan voorzien van elek-15 tronica-hardware en daarin opgeslagen programmatuur, beschikbaar zijn; vanuit een bekrachtigingsorgaan in kast 22 wordt (op niet getoonde wijze) een mechanisme 11 langs staande rails 25, 26 gestuurd, opdat de ruststand van het mechanisme ll en derhalve het melkstel 2 zoveel mogelijk, afhankelijk 20 van de grootte van de koe en haar plaats van de uier, vooraf wordt ingesteld. Onafhankelijk van de koe is het mechanisme 11 langs liggende rails 23, 24 in een geijkte langsrusttoe-stand ingesteld. Vanuit de elektronicakast 22 wordt eveneens een actuator 6 gestuurd, die via een hefboom 5 wordt bediend, 25 die twee Bowdenkabels stuurt, die een arm 27 waaraan het melkrek 2 bevestigd is, in een voorafbepaalde rust- of nulstand trekken.Often the cow will be automatically recognized and information about the position of her udder and nipples, for instance in a central control device (not shown) provided with electronics hardware and software stored therein, will be available; a mechanism 11 is moved (in a manner not shown) along standing rails 25, 26 from an actuator in box 22, so that the rest position of the mechanism 11 and therefore the milking cluster 2 as much as possible, depending on the size of the cow and its location of the udder, is preset. Irrespective of the cow, the mechanism 11 along recessed rails 23, 24 is set in a calibrated longitudinal resting state. An actuator 6 is also controlled from the electronics box 22, which is operated via a lever 5, which controls two Bowden cables, which pull an arm 27 to which the milk rack 2 is attached into a predetermined rest or zero position.

Boven het melkrek 2, zijdelings daarvan, is een sensoreenheld 12 vast opgesteld, ten einde met behulp van 30 twee sensoreenheden 13 resp. 14 in drie dimensies in een liggend vlak een bewegend voorwerp, in de getoonde voorkeursuitvoeringsvorm een speen van een koe, te kunnen detecteren en informatie daaromtrent aan een centraal stuurorgaan (niet getoond) door te geven. Boven elke melkbeker 15 van het melk-35 stel 2 kunnen extra sensoreenheden 29, 31 zijn opgesteld, ten einde bij het omhoog bewegen van de melkbekers een speen ten opzichte van die beker gecentreerd te houden; voor de overzichtelijkheid zijn slechts bij één melkbeker 15 dergelijke .S800272 L-., ------------ -3-.Above the milking rack 2, laterally thereof, a sensor unit 12 is fixedly arranged, in order to use two sensor units 13 and 13, respectively. 14 to be able to detect a moving object, in the shown preferred embodiment a teat of a cow, in three dimensions in a horizontal plane and to transmit information thereto to a central control member (not shown). Additional sensor units 29, 31 may be arranged above each teat cup 15 of the milking set 2, in order to keep a teat centered relative to said cup when the teat cups are moved upwards; for clarity, only one teat cup 15 contains such .S800272 L-., ------------ -3-.

fijnsensoreenheden 29, 31 opgesteld. Deze fijnsensoreenheden kunnen evenals sensoreenheid 12 van een ultrasoon type zijn.fine sensor units 29, 31 arranged. These fine sensor units, like sensor unit 12, can be of an ultrasonic type.

Een grijper 10 van de robot 1 grijpt aan op een daarvoor aangebrachte aangrijpopening van het melkrek 2. Het 5 robotmechanisme 1 is in staat het melkrek 2 in drie dimensies te bewegen, terwijl het mechanisme 11 de bewegingsveranderingen in hoogterichting kan blokkeren; de besturingen van het mechanisme 1 en het mechanisme 11 zijn op niet getoonde wijze - elektrisch en programmatisch - met elkaar gekoppeld.A gripper 10 of the robot 1 engages a gripping opening of the milking rack 2 arranged therefor. The robot mechanism 1 is able to move the milking rack 2 in three dimensions, while the mechanism 11 can block the changes in height in the direction of movement; the controls of the mechanism 1 and the mechanism 11 are coupled - not shown - electrically and programmatically.

10 De werking van het robotsysteem uit fig. 1 wordt aan de hand van het schema uit fig. 2 (en later aan de hand van het schema uit fig. 3) verduidelijkt; het (bewegend) voorwerp, in de getoonde voorkeursuitvoeringsvorm de positie van een speen van een koe, bijv. de speen rechtsvoor van de 15 koe, ten opzichte van de desbetreffende sensor, vormt de ingang I voor de sensoren 12, 19, 31. De door de ultrasone sensoren 12, 19, 31 gemeten reflecties worden op algemeen bekende wijze omgezet in bemonsterde digitale waarden in een liggend vlak, in fig. 2 aangegeven met Δ x resp. Δ y, zijn-20 de relatieve positiecoördinaten ten opzichte van. de sensoren. Vanaf de robotarm 1 komen bepaalde waarden xp en yp, zijnde coördinaten van de robot ten opzichte van het nulpunt van de robot, waarbij Λx en Δ y - eventueel gecorrigeerd -moeten worden opgeteld, zodat, zoals schematisch in de figuur 25 is aangegeven, de waarde xm en ym, zijnde de coördinaten van de speen ten opzichte van het robotnulpunt, ontstaan. Deze waarden worden op nader beschreven wijze gecorrigeerd, opdat de waarden xc en yc - zogeheten doelcoördinaten - ontstaan, die aan een schematisch met SC aangegeven blok kunnen worden 30 toegevoerd, ten einde een nieuwe positie aan de robot 1 op te geven, waarnaar toe deze het melkstel 2 moet bewegen.The operation of the robot system of Fig. 1 is explained with reference to the diagram of Fig. 2 (and later with reference to the diagram of Fig. 3); the (moving) object, in the shown preferred embodiment the position of a teat of a cow, eg the teat at the front right of the cow, relative to the relevant sensor, forms the input I for the sensors 12, 19, 31. The Reflections measured by the ultrasonic sensors 12, 19, 31 are converted in a generally known manner into sampled digital values in a horizontal plane, indicated in Fig. 2 by Δ x resp. Δ y, -20 are the relative position coordinates with respect to. the sensors. Certain values xp and yp, which are the coordinates of the robot relative to the zero point of the robot, come from the robot arm 1, whereby Λx and Δy - possibly corrected - must be added, so that, as shown schematically in figure 25, the value xm and ym, being the teat coordinates relative to the robot zero point. These values are corrected in a further described manner, so that the values xc and yc - so-called target coordinates - are created, which can be applied to a block indicated schematically by SC, in order to indicate a new position to the robot 1, to which it the milking cluster 2 must move.

De waarden Xp en yp enerzijds en A x, Δ y anderzijds worden in een MODEL opgenomen - dat in het eenvoudigste geval deze genoemde waarden aan geen enkele bewerking onder-35 werpt, dat wil zeggen zogeheten transparant is - opgenomen, waarbij na statische analyse, bijv. omvattende het bepalen van het gemiddelde, de standaarddeviatie, of het bepalen van een aantal M-waarden uit een reeks van N-waarden (M <. N). Aan .8800272 -4- * è de hand van de (gemoduleerde) waarden kan een voorspelling worden gedaan (PRE) die aan de besturing van de sensoren wordt toegevoegd, ten einde deze PRE-waarde in plaats van duidelijk foutieve waarden, volgens een voorafbepaald crite-5 rium, in plaats te laten treden van de gedigitaliseerde meetwaarden. In het MODEL vindt een Z-transformatie van de gedigitaliseerde bemonsteringen plaats bij de beschreven uitvoeringsvorm, maar een dergelijk MODEL kan eveneens met een zogeheten Kallman-filter of op nog andere wijze geïmplemen-10 teerd zijn. in FPC worden filtercoëfficienten bepaald, ten einde zowel de waarde xm en ym in PF een nieuwe waarde toe te kennen op grond van het MODEL, alsmede de waarde vxm - na differentiatie in D van de waarden xm en ym - ten einde deze gefilterde waarde (vXf, Vyf) na integratie in (I) bij de ge-15 corrigeerde filterwaarde Xf en yf op te tellen.The values Xp and yp on the one hand and A x, Δ y on the other hand are included in a MODEL - which in the simplest case does not subject these mentioned values to any processing, i.e. is so-called transparent -, after static analysis, e.g. comprising determining the mean, standard deviation, or determining a number of M values from a series of N values (M <. N). .8800272 -4- * è using the (modulated) values, a prediction can be made (PRE) which is added to the control of the sensors, in order to replace this PRE value instead of clearly incorrect values, according to a predetermined criterion 5, to replace the digitized measurements. In the MODEL, a Z transformation of the digitized samples takes place in the described embodiment, but such a MODEL can also be implemented with a so-called Kallman filter or in another way. filter coefficients are determined in FPC, in order to assign both the value xm and ym in PF a new value based on the MODEL, as well as the value vxm - after differentiation in D of the values xm and ym - in order to obtain this filtered value ( vXf, Vyf) after integration in (I) to add to the corrected filter value Xf and yf.

De beschreven regellus van Δ x, Λ y via xc, Xy naar xs, ys, wordt al dan niet gesloten op grond van de gegevens uit het MODEL, zoals aan een blok CONTROL doorgegeven; slechts indien aan voorafbepaalde criteria is voldaan, ten 20 aanzien van gemiddelde waarde van Δχ over een bepaalde tijd, gemiddelde waarde van Δγ over een bepaalde tijd, standaarddeviaties daarin, alsmede bepaalde snelheidswaarden (vx, Vy), wordt door een blok aangegeven met FTL, de regellus bij S gesloten.The described control loop from Δ x, Λ y via xc, Xy to xs, ys is closed or not based on the MODEL data, as passed to a CONTROL block; only if predetermined criteria are met, with respect to mean value of Δχ over a given time, mean value of Δγ over a given time, standard deviations therein, as well as certain velocity values (vx, Vy), is indicated by a block with FTL, the control loop at S closed.

25 Allereerst blijft de regellus bij S geopend, daar vanuit een blok F op een wijze algemeen bekend bij robotbesturingen, een globale positiesturing van het robotsysteem 1 naar een speen van een koe wordt gedaan met behulp van in een geheugen opgeslagen informatie. Indien de sensoreenheid 12 -30 sensoreenheden 29, 31 worden in deze zogeheten find-modes niet gebruikt - een bewegend voorwerp is gedetecteerd en aan voorafbepaalde criteria voor wat betreft Δ x, Δγ, vx, vy en standaarddeviaties daarvan is voldaan, wordt in een blok T de besturing overgenomen en bovengenoemde criteria verscherpt, 35 waaraan bovengenoemde informatie moet voldoen bij een verhoogde bemonstersnelheid.First of all, the control loop at S remains open, since from a block F in a manner generally known to robot controllers, a global position control of the robot system 1 to a teat of a cow is made with the aid of information stored in a memory. If the sensor unit 12 -30 sensor units 29, 31 are not used in these so-called find modes - a moving object is detected and predetermined criteria regarding Δ x, Δγ, vx, vy and standard deviations thereof are met, then a block T took over control and tightened the above criteria, which the above information must meet at an increased sampling rate.

Indien aan de verscherpte criteria gedurende een voorafbepaalde tijd is voldaan, wordt bij S de regellus ge- . 8800272.If the sharpened criteria are met for a predetermined time, the control loop is applied at S. 8800272.

ν· * * -5- sloten en worden de waarden xc en yc rechtstreeks aan de servobesturing (SC) toegevoerd. Indien gewenst kunnen de uit het MODEL berekende filtercoëfficienten vervangen worden door vanuit de besturing (CONTROL) geleverde filtercoëfficienten, 5 die gedurende een bepaalde tijd een vaste waarde hebben, waarna de coëfficientberekening vanuit het MODEL de positie waarheen de robotarm gestuurd wordt, gaat bepalen.ν · * * -5- and the values xc and yc are applied directly to the servo controller (SC). If desired, the filter coefficients calculated from the MODEL can be replaced by filter coefficients supplied from the control (CONTROL), which have a fixed value for a certain time, after which the coefficient calculation from the MODEL will determine the position to which the robot arm is controlled.

Indien gedurende een voorafbepaalde tijd aan de criteria uit blok T is is voldaan, blijft de beschreven 10 regellus gesloten en neemt blok L de besturing over, waarbij, zoals via lijn PS is aangegeven, kan worden overgeschakeld op fijnsensoreenheden 29, 31, opdat de positie van de melkbeker ten opzichte van de speen (nog) nauwkeuriger bepaald kan worden met behulp van een eventuele correctie door de fijn-15 sensoreenheden, en kan de desbetreffende melkbeker 15 worden aangebracht, indien de speen gedurende een voorafbepaalde tijd gecentreerd blijft ten opzichte van de melkbeker en derhalve de sensoreenheden.If the criteria from block T are satisfied for a predetermined time, the described control loop remains closed and block L takes over control, whereby, as indicated via line PS, it is possible to switch to fine sensor units 29, 31, so that the position of the teat cup relative to the teat can be determined (even) more accurately by means of a possible correction by the fine-15 sensor units, and the relevant teat cup 15 can be fitted if the teat remains centered with respect to the teat for a predetermined time teat cup and therefore the sensor units.

Na aanbrenging van de melkbeker, wordt teruggescha-20 keld naar de T-modus, totdat een andere speen van de koe is gecentreerd, waarna in de L-modus deze melkbeker eveneens kan worden aangebracht.After the teat cup has been fitted, it is switched back to the T mode, until another teat of the cow is centered, after which this teat cup can also be fitted in the L mode.

Het moge duidelijk zijn, dat bij onverwachte bewegingen van het (bewegend) voorwerp het beschreven robotsys-25 teem ogenblikkelijk van L-modusbesturing naar T-modusbestaring of zelfs naar F-modusbesturing zal overgaan.It will be clear that in the event of unexpected movements of the (moving) object the described robot system will immediately switch from L-mode control to T-mode control or even to F-mode control.

Een schema van een andere voorkeursuitvoeringsvorm van de werking van het robotsysteem uit fig. 1 (fig. 3) behoeft geen snelheidsfiltering en hierbij worden de waarden 30 Δ x en Δy slechts geanalyseerd, terwijl de MODEL-vorming achterwege blijft. Predictiewaarden worden via blok CONTROL aan het sensorblok 12, 29, 31 toegevoerd.A schematic of another preferred embodiment of the operation of the robotic system of Fig. 1 (Fig. 3) does not require velocity filtering, and only analyzes the values 30 Δx and Δy, while the MODEL formation is omitted. Prediction values are applied to sensor block 12, 29, 31 via block CONTROL.

Voorts wordt opgemerkt dat de Z-coördinaat of hoog-tecoördinaat van de speen van de koe veelal constant zal zijn 35 en in het getoonde uitvoeringsvoorbeeld een vaste of door het CONTROL-blok bepaalde waarde zal hebben. Bij de in fig. 3 getoonde uitvoeringsvorm zijn de filtercoëfficienten constant .8800272.It is further noted that the Z-coordinate or height-coordinate of the teat of the cow will often be constant and will have a fixed value or determined by the CONTROL block in the exemplary embodiment shown. In the embodiment shown in Fig. 3, the filter coefficients are constant .8800272.

-6- i r in een bepaalde toestand van het systeem (F-, T- of L-modus), maar hebben ze voor iedere toestand een andere waarde.-6- i r in a certain state of the system (F, T or L mode), but they have a different value for each state.

Uitgebreide analyses en evaluatie van proefnemingen hebben uitgewezen, dat bij bemonstering met 200 Hz van de 5 signalen uit de sensoreenheid 12 en sensoreenheden 29, 31 een nauwkeurig model van de bewegingen van een speen van een koe kan worden opgebouwd, zonder dat - vanwege het zogeheten vouweffect - onnauwkeurigheden in de bemonstering plaatsvinden; de besturing van de robotarm via de servobesturing (SC) 10 vindt bij de getoonde voorkeursuitvoeringsvorm iedere 50 msec plaats. Gezien de mogelijkheid tot schakelen tussen verschillende bewegingsmodi, is dit in de praktijk ruim voldoende gebleken om op nauwkeurige wijze een speen van een koe te kunnen volgen en de melkbekers aan te brengen.Extensive analyzes and evaluation of experiments have shown that with 200 Hz sampling of the 5 signals from the sensor unit 12 and sensor units 29, 31 an accurate model of the movements of a teat of a cow can be built up, without - because of the so-called folding effect - inaccuracies in sampling take place; the control of the robot arm via the servo control (SC) 10 takes place every 50 msec in the shown preferred embodiment. In view of the possibility of switching between different movement modes, this has proved to be more than sufficient in practice to accurately follow a teat of a cow and to apply the teat cups.

.88002728800272

Claims

A method for searching, finding and / or tracking with the aid of a robot which is part of a larger whole, eg a teat on an udder of a cow or other female mammal, whereby at least 5 one sensor position and speed of the object are determined in at least one dimension and a command for the movement of a robot arm is generated on the basis of the speed and position information.

2. Method for searching, finding and / or tracking with a robot a (moving) object, which is part of a larger whole, eg a teat on a cow's udder, whereby with the aid of at least one sensor position and / or speed of the object are determined, wherein a command for the movement of a robot arm is generated on the basis of the speed and / or position information and wherein the static information about the speed and / or or position information is supplied to a controller, with the aid of which information in the controller it is decided whether the object is being tracked sufficiently accurately.

3. Method for following a (moving) object with the aid of a robot, in which position and / or speed of the object is determined with the aid of at least one sensor and in which speed and / or position signals are sampled and / or digitized.

4. Method according to claim 3, wherein a second order model is implemented in hardware and / or software with the aid of the digital speed and position information, in order to adapt the information to be supplied to the robot on the basis of that model. .

The method of claims 1 to 4, wherein standard deviations of both digital position and velocity information are determined.

A method according to any one of claims 1 to 5, wherein data to be supplied to the robot is either in a closed form. 8800272. 4 -8- control loop are fed from the recorded position information or determined from a path stored in a memory.

7. A method according to claim 6, wherein a different sensor is used in the closed control loop than when the robot is moved along a predetermined path.

Method according to any one of claims 1 to 7, wherein the moving object moves mainly in two dimensions, while one dimension thereof mainly has a constant value, and wherein the velocity and position information for two dimensions is sampled and digitized .

9. Method according to any of the preceding claims 2-8, wherein the model is carried out with the aid of a Z-transformation.

Method according to any of the preceding claims, characterized in that the position and velocity information is sampled at a frequency of about 200 Hz.

Robotic system in which a method according to any one of claims 1 to 10 is applied.

12. Robot system suitable for milking female mammals, which have been guided in a predetermined space. 8800272