DE102022112879B3

DE102022112879B3 - Pose estimation of an object gripped by a robotic gripper

Info

Publication number: DE102022112879B3
Application number: DE102022112879.2A
Authority: DE
Inventors: Martin Pfanne
Original assignee: Deutsches Zentrum fuer Luft und Raumfahrt eV
Current assignee: Deutsches Zentrum fuer Luft und Raumfahrt eV
Priority date: 2022-05-23
Filing date: 2022-05-23
Publication date: 2023-08-03
Anticipated expiration: 2042-05-24

Abstract

Die Erfindung betrifft das Ermitteln einer Pose eines Objekts (7) in einem Greifer (3), mit einer Kameraeinheit (11) zum Erfassen von Greifer (3) und Objekt (7), wobei eine Recheneinheit (9) ein Kalman Filter wiederholt mit zwei Schritten ausführt, wobei im ersten nach einer mit einer Positionsermittlungseinheit (13) ermittelten Greiferverschiebung eine resultierende Bewegung des Objekts (7) prädiziert wird und damit eine Schätzung der Pose geändert wird, und in einem zweiten das inkonsistente Vorliegen eines angenommenen partiellen Eindringens des Objekts (7) in den Greifer (3) mit Daten einer Kameraeinheit (11) und eine Korrektur ermittelt werden, sodass die jeweils in einer nachfolgenden Iteration ausgeführten zwei Schritte in der Ausführung des Kalman Filters auf Basis der im ersten Schritt geänderten Schätzung der Pose, und bei Vorliegen einer Inkonsistenz auf Basis der im zweiten Schritt korrigierten geänderten Schätzung der Pose, erfolgt.The invention relates to determining a pose of an object (7) in a gripper (3), with a camera unit (11) for capturing the gripper (3) and object (7), with a computing unit (9) repeating a Kalman filter with two Executes steps, whereby in the first step a resulting movement of the object (7) is predicted after a gripper displacement determined with a position determination unit (13) and thus an estimate of the pose is changed, and in a second step the inconsistent presence of an assumed partial penetration of the object (7 ) in the gripper (3) with data from a camera unit (11) and a correction are determined, so that the two steps in the execution of the Kalman filter carried out in a subsequent iteration are based on the estimate of the pose changed in the first step, and if an inconsistency based on the modified estimate of the pose corrected in the second step.

Description

Die Erfindung betrifft ein System zum Ermitteln einer Pose eines in einem Greifer eines Robotermanipulators gegriffenen Objekts, sowie eine Robotermanipulatoreinheit mit einem solchen System.The invention relates to a system for determining a pose of an object gripped in a gripper of a robot manipulator, and a robot manipulator unit with such a system.

Ist von einem Robotermanipulator ein Gegenstand aufzunehmen, so weist der Robotermanipulator typischerweise als Endeffektor einen Greifer an seinem distalen Glied auf. Ein solcher Greifer wird in vielen Anwendungen bei der Automatisierung von verschiedensten Fertigungs- und Logistikprozessen verwendet. Wenn ein Gegenstand von einem Robotermanipulator an einem Ort aufgenommen werden soll, um ihn an einem exakt bestimmten anderen Ort wieder abzulegen, wird die Aufgabe häufig abgekürzt mit „Pick & Place“ beschrieben. Häufig ergibt sich jedoch das Problem, dass die Position des Gegenstandes vor dem Aufnehmen nicht exakt bekannt ist, während dennoch die gewünschte Position zum Ablegen des Gegenstands exakt definiert ist.If an object is to be picked up by a robotic manipulator, the robotic manipulator typically has a gripper on its distal member as an end effector. Such a gripper is used in many applications in the automation of a wide variety of manufacturing and logistics processes. If an object is to be picked up by a robotic manipulator at one location in order to set it down again at a precisely defined location, the task is often abbreviated as "pick & place". However, the problem often arises that the position of the object is not exactly known before it is picked up, while the desired position for putting the object down is nevertheless precisely defined.

Die folgenden Informationen beziehen sich dabei nicht auf ein bestimmtes Dokument aus dem Stand der Technik, sondern sind im allgemeinen Fachwissen bekannt: Wegen des nicht exakt bekannten Orts des Gegenstands, obwohl bekannt ist, dass sich der aufzunehmende Gegenstand innerhalb eines Arbeitsbereichs des Robotermanipulators befindet, sind Perzeptionsverfahren anzuwenden, mithilfe derer der Gegenstand genau lokalisierbar ist, sodass dieser mit dem Greifer des Robotermanipulators angefahren werden kann. Es können dafür Bildverarbeitungsalgorithmen genutzt werden, welche die Bestimmung der Position und der Orientierung des Gegenstands anhand von Farbbildern und/oder anhand von Tiefenbildern erlauben. Hierbei wird typischerweise eine bekannte Objektgeometrie des Gegenstands ausgenutzt. Die im Stand der Technik bekannten Algorithmen unterscheiden sich insbesondere nach der Art der genutzten Kameras für eine optische Detektion (beispielsweise die Arten: Monokular, Stereo, RGB-D), sowie nach dem genutzten Rechenverfahren (maschinelles Lernen oder traditionelle Bildverarbeitung). Verschiedene Vorgehensweisen führen daher im Allgemeinen auch zu einer unterschiedlichen Robustheit gegenüber geometrischen Verdeckungen, d. h. wenn durch eine verwendete Kamera der Gegenstand dann nicht mehr optisch erfasst oder zumindest nur noch teilweise erfasst werden kann, wenn Teile des Robotermanipulators, insbesondere des Greifers, beim Aufnehmen des Gegenstands zumindest Teile dieses Gegenstands verdecken. Dies ist ein genereller Nachteil von bildbasierten Ansätzen, da naturgemäß gewisse Teile des Gegenstands beim und nach dem Greifvorgang beim Aufnehmen des Gegenstands optisch verdeckt werden können. Eine zuverlässige visuelle Re- Lokalisierung des im Greifer aufgenommenen Gegenstands, wenn dieser sich im Greifer verschoben hat, ist somit nicht oder nur eingeschränkt möglich. Eine Konsequenz dessen ist, dass beispielsweise in Montagevorgängen die Fehlerquote von fehlerhaften Montagen oder die Zahl von gänzlich erfolglosen Montagen des Gegenstands auf einem Zielobjekt steigen. Alternativ zur optischen Erfassung des Gegenstands auf der Basis von Kameras werden im Stand der Technik außerdem taktile Sensoren verwendet, um einen Gegenstand im Arbeitsraum des Robotermanipulator zu lokalisieren. Hierbei handelt es sich um Sensoren, die typischerweise in die Kontaktflächen eines Greifers integriert sind. Über die erfassten Kontaktkräfte erlauben es die taktilen Sensoren, die Greifkräfte zwischen dem Gegenstand und dem Greifer zu erfassen und für eine Regelung zu verwenden. Auch hiermit kann jedoch nicht zuverlässig eine Verschiebung des Gegenstands bezüglich des Greifers in Relation zur initialen Pose detektiert werden. Ferner sind diese zusätzlichen Sensoren (die taktilen Sensoren) notwendig, was die Komplexität und die Kosten des Robotersystems steigert.The following information does not refer to a specific document from the prior art, but is known in general specialist knowledge: Because the location of the object is not exactly known, although it is known that the object to be picked up is within a working range of the robotic manipulator Apply perception methods that can be used to locate the object precisely so that it can be approached with the gripper of the robotic manipulator. Image processing algorithms can be used for this, which allow the position and the orientation of the object to be determined using color images and/or using depth images. In this case, a known object geometry of the object is typically used. The algorithms known in the prior art differ in particular according to the type of cameras used for optical detection (for example the types: monocular, stereo, RGB-D) and according to the calculation method used (machine learning or traditional image processing). Different approaches therefore generally lead to different levels of robustness to geometric occlusions, i.e. H. if the object can no longer be optically detected or at least only partially detected by a camera used, if parts of the robot manipulator, in particular the gripper, cover at least parts of this object when the object is picked up. This is a general disadvantage of image-based approaches, since certain parts of the object can naturally be optically covered during and after the gripping process when picking up the object. A reliable visual re-localization of the object picked up in the gripper when it has shifted in the gripper is therefore not possible or only possible to a limited extent. One consequence of this is that, for example in assembly processes, the error rate of erroneous assemblies or the number of completely unsuccessful assemblies of the object on a target object increases. As an alternative to the optical detection of the object on the basis of cameras, tactile sensors are also used in the prior art in order to localize an object in the working space of the robotic manipulator. These are sensors that are typically integrated into the contact surfaces of a gripper. Using the recorded contact forces, the tactile sensors allow the gripping forces between the object and the gripper to be recorded and used for regulation. However, this also cannot reliably detect a displacement of the object with respect to the gripper in relation to the initial pose. Furthermore, these additional sensors (the tactile sensors) are necessary, which increases the complexity and cost of the robotic system.

Prinzipiell sind somit auch im Stand der Technik die Bestimmung der Position und Orientierung eines Gegenstands zunächst möglich. Die so ermittelte Position und Orientierung des Gegenstands bilden zusammen eine geschätzte Pose des Gegenstands, auf Grundlage derer der Greifer in seinem geöffnetem Zustand am Gegenstand positioniert werden kann, um durch Schließen des Greifers den Gegenstand zu greifen. Anschließend wird der Gegenstand mithilfe einer Bewegung des Robotermanipulators zu der vorgegebenen Zielposition bewegt, um den Gegenstand dort abzulegen. Die Bewegung des Robotermanipulators ist dabei grundsätzlich abhängig von der Konstruktion und Konfiguration des Robotermanipulators, wobei die meisten Robotermanipulatoren eine Vielzahl von durch Gelenke miteinander verbundenen Gliedern aufweisen. Alternativen sind jedoch ebenfalls bekannt, beispielsweise lineare Freiheitsgrade, insbesondere durch parallele Verschiebung zweier Glieder zueinander.In principle, therefore, the determination of the position and orientation of an object is also initially possible in the prior art. The position and orientation of the object determined in this way together form an estimated pose of the object, on the basis of which the gripper can be positioned on the object in its open state in order to grip the object by closing the gripper. The object is then moved to the specified target position with the aid of a movement of the robot manipulator in order to place the object there. The movement of the robotic manipulator is fundamentally dependent on the design and configuration of the robotic manipulator, with most robotic manipulators having a large number of links connected to one another by joints. However, alternatives are also known, for example linear degrees of freedom, in particular by parallel displacement of two members in relation to one another.

In einem nominalen Fall, in dem die Position und Orientierung des Gegenstands unmittelbar vor dem Greifen exakt bekannt ist, und der Gegenstand im Greifer nicht durch den Greifvorgang selbst verrutscht, kann auch die gewünschte Pose des Gegenstands beim Ablegen am Zielort exakt eingehalten werden. Treten beim Greifvorgang Störungen wie durch Verrutschen des Gegenstands im Greifer verursacht auf und ebenfalls Ungenauigkeiten in der Ermittlung solcher Störungen auf, ist die Pose des Gegenstands im Greifer nicht mehr genau bekannt, was dazu führen kann, dass beim Ablegen des Gegenstands dieser nicht exakt die gewünschte Pose aufweist. Dieses Szenario tritt beispielsweise ein, wenn beim Greifen des Gegenstands von der Aufnahmeposition der Gegenstand im Greifer bei seinem Schließvorgang verrutscht, oder durch das Überdecken des Greifers nicht mehr durch Sensorik exakt erfasst werden kann, sodass auf eine frühere, ungenaue Schätzungen der Pose des Gegenstands im Greifer zurückgegriffen werden muss. Insbesondere führen kleine Abweichungen der geschätzten initialen Pose gegenüber der tatsächlichen Pose des Gegenstands noch vor dem Aufnehmen, d. h. vor dem Schließen des Greifers, zu diesen unvorhergesehenen Bewegungen im Greifer.In a nominal case, in which the position and orientation of the object is known exactly immediately before it is gripped, and the object does not slip in the gripper as a result of the gripping process itself, the desired pose of the object when it is deposited at the target location can also be maintained exactly. If disturbances occur during the gripping process, such as the object slipping in the gripper, and if there are also inaccuracies in the determination of such disturbances, the position of the object in the gripper is no longer exactly known, which can lead to the object not being exactly the desired one when it is set down pose. This scenario occurs, for example, when gripping the object from the pick-up position, the object slips in the gripper during its closing process, or can no longer be precisely detected by sensors due to the gripper being covered, so that earlier, inaccurate estimates of the pose of the object in the Gripper must be used. In particular, small deviations of the estimated initial pose from the actual pose of the object before it is picked up, ie before the gripper is closed, lead to these unforeseen movements in the gripper.

Im Stand der Technik sind hierzu folgende Dokumente bekannt, die sich mit dem Greifen, Aufnehmen und Handhaben von Objekten durch Robotersysteme befassen:

DE 10 2018 220 569 A1 ,
DE 11 2017 002 498 T5 ,
DE 10 2016 207 942 A1 ,
DE 10 2013 017 895 B4 ,
DE 10 2010 053 002 B4 , und
EP 3 482 885 B1 .

The following documents are known in the prior art, which deal with the gripping, picking up and handling of objects by robot systems:

DE 10 2018 220 569 A1 ,
DE 11 2017 002 498 T5 ,
DE 10 2016 207 942 A1 ,
DE 10 2013 017 895 B4 ,
DE 10 2010 053 002 B4 , and
EP 3 482 885 B1 .

Es ist daher eine Aufgabe der Erfindung, das oben beschriebene Problem von Abweichungen der tatsächlichen Pose von einer vorgegebenen, gewünschten Pose, beim Ablegen eines Gegenstands durch einen Robotermanipulator mit einem Greifer an einem Zielort zu lösen und somit die sogenannte „Pick & Place“ Aufgabe eines Robotermanipulators bezüglich eines Gegenstands zu verbessern.It is therefore an object of the invention to solve the above-described problem of deviations in the actual pose from a predetermined, desired pose when an object is placed by a robot manipulator with a gripper at a target location and thus the so-called "pick & place" task of a to improve the robot manipulator in relation to an object.

Die Erfindung ergibt sich aus den Merkmalen der unabhängigen Ansprüche. Vorteilhafte Weiterbildungen und Ausgestaltungen sind Gegenstand der abhängigen Ansprüche.The invention results from the features of the independent claims. Advantageous developments and refinements are the subject matter of the dependent claims.

Ein erster Aspekt der Erfindung betrifft ein System zum Ermitteln einer Pose eines in einem Greifer eines Robotermanipulators gegriffenen Objekts, aufweisend eine Recheneinheit, sowie eine Kameraeinheit, die dazu dient, mit ihrem Erfassungsbereich den Greifer des Robotermanipulators beim Greifen eines Objekts und das Objekt zu erfassen, und aufweisend eine Positionsermittlungseinheit zum Ermitteln eines jeweils aktuellen Schließzustands des Greifers, wobei die Recheneinheit dazu ausgeführt ist, ein für nichtlineare Systeme bestimmtes Kalman Filter iterativ an einer Vielzahl aufeinanderfolgender Zeitpunkte auszuführen, und in jeder der iterativen Anwendungen des Kalman Filters zwei jeweils aufeinander folgende Schritte auszuführen, wobei in einem ersten, prädiktivem Schritt aus einer mittels der Positionsermittlungseinheit ermittelten Verschiebung einer Greiferschließposition eine resultierende Bewegung des Objekts durch Berührung des Greifers mit dem Objekt prädiziert wird und auf Basis der prädizierten Bewegung des Objekts eine vorherige Schätzung der Pose des Objekts zum Greifer geändert wird, und in einem zweiten, aktualisierenden Schritt von der Recheneinheit das Vorliegen einer nach dem ersten Schritt verbliebenen Inkonsistenz der geänderten Schätzung der Pose des Objekts relativ zum Greifer geprüft wird, die ein zumindest partielles Eindringen des Objekts in den Greifer bedeuten würde, und im Fall des Vorliegens dadurch behoben wird, dass von der Recheneinheit auf Basis der Daten der Kameraeinheit eine Korrektur der im ersten Schritt geänderten Schätzung der Pose auf Basis einer angenommenen Eindringtiefe des Objekts in den Greifer unter Beachtung der Bedingung der Ortsidentität von Kontaktpunkten auf der jeweiligen Oberfläche von Greifer und Objekt bei Kontakt des Greifers mit dem Objekt zum Auflösen des angenommenen Eindringens des Objekts in den Greifer ermittelt wird, sodass die jeweils in einer nachfolgenden Iteration ausgeführten zwei Schritte in der Ausführung des Kalman Filters auf Basis der im aktuellen ersten Schritt geänderten Schätzung der Pose, und bei Vorliegen einer Inkonsistenz auf Basis der im zweiten Schritt korrigierten geänderten Schätzung der Pose, erfolgt.A first aspect of the invention relates to a system for determining a pose of an object gripped in a gripper of a robotic manipulator, having a computing unit and a camera unit, which is used to capture the gripper of the robotic manipulator when gripping an object and the object with its detection range. and having a position determination unit for determining a respective current closed state of the gripper, the computing unit being designed to iteratively execute a Kalman filter intended for non-linear systems at a large number of successive points in time, and to execute two successive steps in each of the iterative applications of the Kalman filter In a first, predictive step, a movement of the object resulting from contact of the gripper with the object is predicted from a displacement of a gripper closed position determined by means of the position determination unit, and a previous estimate of the pose of the object relative to the gripper is changed on the basis of the predicted movement of the object , and in a second, updating step, the computing unit checks the presence of an inconsistency in the changed estimate of the pose of the object relative to the gripper that remained after the first step, which would mean at least partial penetration of the object into the gripper, and in the case of The present situation is remedied by the computing unit, based on the data from the camera unit, correcting the estimate of the pose changed in the first step on the basis of an assumed penetration depth of the object into the gripper, taking into account the condition of the location identity of contact points on the respective surface of the gripper and Object is determined upon contact of the gripper with the object to resolve the assumed intrusion of the object into the gripper, so that the two steps in the execution of the Kalman filter, each executed in a subsequent iteration, are based on the estimate of the pose changed in the current first step, and if there is an inconsistency based on the modified estimate of the pose corrected in the second step.

Vorliegend wird der Begriff „Pose“ entsprechend der DIN EN ISO 8373 verstanden. Die Pose des Objekts ist somit die Kombination von Position und Orientierung des Objekts im dreidimensionalen Raum, im vorliegenden Zusammenhang insbesondere relativ zur Position und Orientierung des Greifers.Here, the term "pose" according to the EN ISO 8373 understood. The pose of the object is thus the combination of the position and orientation of the object in three-dimensional space, in the present context in particular relative to the position and orientation of the gripper.

Das Kalman Filter ist ein probabilistischer Algorithmus, der iterativ die Pose des Objekts relativ zum Greifer schätzt. Das verwendete Kalman Filter beruht insbesondere auf nichtlinearen Modellen, sodass für das Kalman Filter eine Erweiterung des Standard Kalman Filters zu verwenden ist. Während Unscented Kalman Filter oder der Partikel Filter grundsätzlich für nichtlineare Systeme geeignet sind, benötigen diese jedoch für das beschriebene Szenario eine deutlich höhere Rechenleistung als beispielsweise das Erweiterte Kalman Filter („Extended Kalman Filter“, oder kurz „EKF“). Sehr bevorzugt wird daher das erweiterte Kalman Filter EKF verwendet. Das Erweiterte Kalman Filter, abgekürzt EKF, ist eine nichtlineare Version des Kalman Filters. Das EKF ist eine Erweiterung des klassischen Kalman Filters für nichtlineare Systeme, bei denen die Nichtlinearität insbesondere durch eine erste und/oder zweite Ableitung mit geschätzt wird.The Kalman filter is a probabilistic algorithm that iteratively estimates the pose of the object relative to the gripper. The Kalman filter used is based in particular on non-linear models, so that an extension of the standard Kalman filter must be used for the Kalman filter. While the Unscented Kalman Filter or the Particle Filter are basically suitable for non-linear systems, they require significantly more computing power for the scenario described than, for example, the Extended Kalman Filter (“Extended Kalman Filter”, or “EKF” for short). The extended Kalman filter EKF is therefore very preferably used. The Extended Kalman Filter, abbreviated EKF, is a nonlinear version of the Kalman Filter. The EKF is an extension of the classic Kalman filter for non-linear systems in which the non-linearity is estimated in particular by a first and/or second derivation.

Der Robotermanipulators weist bevorzugt eine Vielzahl von durch Gelenke miteinander verbundenen Gliedern auf, auch Glieder mit translatorischen Freiheitsgraden zueinander sind für die Aufgabe „Pick & Place“ in manchen Umständen geeignet. Typischerweise am distalen Glied des Robotermanipulators ist ein Endeffektor angeordnet, um Aufgaben im Sinne eines Manipulators auszuführen. Der Endeffektor ist hierbei als Greifer ausgebildet, um ein Objekt zugreifen, um es vom Ausgangsort an einen Zielort innerhalb des Arbeitsraums des Robotermanipulators bewegen zu können.The robotic manipulator preferably has a large number of links connected to one another by joints, including links with translatori cal degrees of freedom to each other are suitable for the "Pick &Place" task in some circumstances. An end effector is typically located at the distal limb of the robotic manipulator to perform manipulator-like tasks. The end effector is in this case designed as a gripper in order to grip an object in order to be able to move it from the starting point to a destination within the working space of the robotic manipulator.

Den Greifvorgang beobachtet eine Kameraeinheit, welche stationär, d. h. auf einem eigenen Sockel oder dem Sockel des Robotermanipulators angeordnet sein kann, oder auch an einem der beweglichen Glieder des Robotermanipulators. Die Kameraeinheit dient insbesondere dazu, den Ort und die Orientierung des aufzunehmenden Objekts durch Objekterkennung aus den Bilddaten zu ermitteln.A camera unit, which is stationary, i. H. may be arranged on its own base or the base of the robotic manipulator, or also on one of the movable members of the robotic manipulator. The camera unit serves in particular to determine the location and the orientation of the object to be recorded by object recognition from the image data.

Die Positionsermittlungseinheit ist dazu ausgeführt, einen aktuellen Schließzustand des Greifers zu ermitteln. Unter dem Schließzustand wird insbesondere die aktuelle Position der Greiferbacken zueinander verstanden, es kann jedoch auch in Kombination dazu oder alleinig die Geschwindigkeit der Greiferbacken beim Schließen verwendet werden. Unabhängig vom verwendeten Ableitungsgrad der Schließposition der jeweiligen Greiferelemente ermittelt die Recheneinheit eine entsprechende Verschiebung des Objekts aufgrund der Schließbewegung des Greifers. In diesem Sinne führt eine beobachtete Schließgeschwindigkeit des Greifers zu einer angenommenen Verschiebungsgeschwindigkeit des Objekts dann, wenn von einem Kontakt eines Greiferelements mit dem Objekt ausgegangen wird. Wird davon nicht ausgegangen, ist die entsprechende mathematische Abbildung eine auf Bewegungsgeschwindigkeit des Objekts von Null, bzw. auf eine unveränderte Pose des Objekts.The position determination unit is designed to determine a current closed state of the gripper. The closed state is understood to mean in particular the current position of the gripper jaws relative to one another, but the speed of the gripper jaws during closing can also be used in combination with this or alone. Regardless of the degree of derivation used for the closed position of the respective gripper elements, the computing unit determines a corresponding displacement of the object based on the closing movement of the gripper. In this sense, an observed closing speed of the gripper leads to an assumed displacement speed of the object when contact of a gripper element with the object is assumed. If this is not assumed, the corresponding mathematical mapping is one based on the object's speed of movement of zero, or on an unchanged pose of the object.

Dies wird im ersten Schritt, der sogenannten „Prädiktion“ bei der wiederholten Ausführung des Kalman Filters ermittelt. Im zweiten Schritt dagegen werden Unsicherheiten berücksichtigt, die fortwährend während des Greifvorgangs auftreten. Solche Unsicherheiten, beispielsweise verursacht durch die Verdeckung des Objekts durch einen der Greiferbacken, können zu Inkonsistenzen in der Schätzung der Pose des Objekts bezüglich des Greifers führen, beispielsweise indem die angenommene Pose des Objekts zu einer räumlichen Überlappung von Objekt und Greifer führen müsste, was physisch aber nicht zu erwarten ist. Eine solche Inkonsistenz wird im zweiten Schritt bei der Ausführung des Kalman Filters, dem Schritt der „Aktualisierung“ aufgelöst, indem die geschätzte Pose des Objekts so korrigiert wird, dass sich diese Inkonsistenz auflöst und nach der aktualisierten Schätzung das Objekt nicht länger in den Greifer als eindringend angenommen wird, d. h. dass sich beide höchstens berühren, nicht jedoch räumlich überlappen.This is determined in the first step, the so-called "prediction" in the repeated execution of the Kalman filter. In the second step, on the other hand, uncertainties that constantly occur during the gripping process are taken into account. Such uncertainties, caused for example by the occlusion of the object by one of the gripper jaws, can lead to inconsistencies in the estimation of the object's pose with respect to the gripper, for example in that the assumed pose of the object would have to result in a spatial overlap of object and gripper, which is physically but is not to be expected. Such an inconsistency is resolved in the second step in the execution of the Kalman filter, the "update" step, by correcting the estimated pose of the object in such a way that this inconsistency is resolved and, after the updated estimate, the object is no longer in the gripper than is strongly accepted, d. H. that both touch at most, but not spatially overlap.

Der erste, prädizierende Schritt zum jeweiligen Ermitteln einer geänderten Pose ausgehend von der aktuell geschätzten Pose, sowie der zweite aktualisierende Schritt, der verbleibende Inkonsistenzen in der Schätzung der Pose des Objekts bereinigt, werden in der genannten Reihenfolge wiederholt ausgeführt. Bei einer zeitdiskreten Ausführung des Kalman Filters mit einer vorgegebenen Frequenz von beispielsweise 100 Hz werden beide Schritte alle 0,1 Sekunden ausgeführt, um mit ausreichender Bandbreite den Greifvorgang des Objekts durch den Greifer beobachten und modellieren zu können.The first, predictive step for determining a changed pose based on the currently estimated pose, and the second updating step, which cleans up remaining inconsistencies in the estimation of the pose of the object, are carried out repeatedly in the order mentioned. In a time-discrete execution of the Kalman filter with a predetermined frequency of, for example, 100 Hz, both steps are carried out every 0.1 seconds in order to be able to observe and model the gripping process of the object by the gripper with sufficient bandwidth.

Demnach ergibt sich eine wiederholte Ausführung des Kalman Filters so, dass nach einer Initialisierung eine erste Schätzung der Pose des Objekts vorliegt, und ab dann wiederholt der erste Schritt und der darauf folgende zweite Schritt ausgeführt werden. In anderen Worten wird im ersten Schritt eine jeweils aktuell vorliegende Schätzung der Pose geändert, und im Falle von verbleibenden Inkonsistenzen die geänderte Schätzung der Pose aktualisiert, sodass in der nächsten Iteration der Ausführung des Kalman Filters im ausgeführten ersten Schritt die Schätzung der Pose aus der vorhergehenden Iteration der Ausführung des Kalman Filters zugrunde liegt. Das Ergebnis jeder Iteration ist daher entweder eine lediglich im ersten Schritt geänderte Schätzung der Pose, oder beim Vorliegen einer Inkonsistenz die im zweiten Schritt aktualisierte Schätzung der Pose, die im ersten Schritt geändert wurde.Accordingly, a repeated execution of the Kalman filter results in such a way that after an initialization there is a first estimate of the pose of the object, and from then on the first step and the second step that follows it are executed repeatedly. In other words, in the first step, a currently available estimate of the pose is changed, and in the event of remaining inconsistencies, the changed estimate of the pose is updated, so that in the next iteration of the execution of the Kalman filter in the first step, the estimate of the pose is from the previous one Iteration underlying the execution of the Kalman filter. The result of each iteration is therefore either an estimate of the pose that has only been changed in the first step, or if there is an inconsistency, the estimate of the pose that has been updated in the second step and changed in the first step.

Es ist eine vorteilhafte Wirkung der Erfindung, dass Informationen des Greifers über seine Schließposition mit Greifer und Objekt beobachtenden kamerabasierten Daten fusioniert werden, sodass eine genauere Schätzung der Pose des gegriffenen Gegenstands relativ zum Greifer erhalten wird. Insbesondere ist es im Gegensatz zu rein kamerabasierten Verfahren möglich, die Pose eines Gegenstands im durch den Greifer gegriffenen Zustand sehr genau zu schätzen, auch wenn der Gegenstand insbesondere durch Elemente des Greifers teilweise oder vollständig verdeckt ist. Dadurch kann eine höhere Positioniergenauigkeit beim Ablegen des Gegenstands an einem Zielort erreicht werden. Des Weiteren ist die Verwendung der Greifer-Position möglich, ohne vorherige Modifikationen oder Erweiterungen am Robotermanipulator und am Greifer vornehmen zu müssen. Auch sind keine zusätzlichen Sensoren wie die eingangs genannten taktilen Sensoren notwendig, sodass zusätzliche Kosten nicht auftreten. Durch die probabilistische Sensorfusion der Greiferposition und der Bilddaten werden die Ungenauigkeiten beider Modalitäten (Greiferposition und Bilddaten) explizit berücksichtigt. Im Unterschied zu existierenden Ansätzen wird dadurch der Einfluss der verschiedenen Messungen (Greiferposition und Bilddaten) auf die geschätzte Pose automatisch anhand der jeweils aktuellen Unsicherheiten gewichtet.It is an advantageous effect of the invention that information from the gripper about its closed position is fused with camera-based data observing gripper and object, so that a more accurate estimate of the pose of the gripped object relative to the gripper is obtained. In particular, in contrast to purely camera-based methods, it is possible to very precisely estimate the pose of an object when it is gripped by the gripper, even if the object is partially or completely covered by elements of the gripper. As a result, greater positioning accuracy can be achieved when depositing the object at a destination. Furthermore, the use of the gripper position is possible without having to make prior modifications or extensions to the robot manipulator and the gripper. Also, no additional sensors such as the tactile sensors mentioned at the outset are necessary, so that additional costs do not arise. The probabilistic sensor fusion of the gripper position and the image data eliminates the inaccuracies of both modes factors (gripper position and image data) are explicitly taken into account. In contrast to existing approaches, the influence of the various measurements (gripper position and image data) on the estimated pose is automatically weighted based on the current uncertainties.

Gemäß einer vorteilhaften Ausführungsform ist die Recheneinheit dazu ausgeführt, zum Prüfen des Vorliegens der Inkonsistenz die angenommene Eindringtiefe des Objekts in den Greifer oder einen Abstand zwischen Objekt und Greifer durch eine Minkowski-Differenz aus Objekt und Greifer unter Anwendung des Gilbert-Johnson-Keerthi Distanzalgorithmus zu ermitteln, wobei die jeweilige geometrische Ausdehnung von Objekt und Greifer als konvexe Menge modelliert ist.According to an advantageous embodiment, the processing unit is designed to check the presence of the inconsistency to the assumed penetration depth of the object in the gripper or a distance between the object and gripper by a Minkowski difference between the object and gripper using the Gilbert-Johnson-Keerthi distance algorithm determine, with the respective geometric extent of the object and gripper being modeled as a convex set.

Gemäß einer weiteren vorteilhaften Ausführungsform ist die Recheneinheit dazu ausgeführt, mit der Anwendung des Gilbert-Johnson-Keerthi Distanzalgorithmus eine Konfigurationsmenge als Abstand zwischen Ursprung und repräsentierendem Körper aus der Minkowski- Differenz zwischen Objekt und Greifer zu ermitteln, sodass bei einem angenommenen Eindringen des Objekts in den Greifer der Ursprung innerhalb der Konfigurationsmenge liegt und andernfalls außerhalb der Konfigurationsmenge liegt, wobei die Recheneinheit zum Ermitteln ausgeführt ist, ob das Objekt in den Greifer mit der angenommenen Pose eindringen würde, indem durch Anwendung des Gilbert-Johnson-Keerthi Distanzalgorithmus iterativ ein Simplex von Punkten innerhalb der Konfigurationsmenge konstruiert wird, um den Ursprung von der Konfigurationsmenge zu umschließen.According to a further advantageous embodiment, the computing unit is designed to use the Gilbert-Johnson-Keerthi distance algorithm to determine a configuration set as the distance between the origin and the representative body from the Minkowski difference between the object and the gripper, so that if the object is assumed to penetrate into the gripper the origin lies within the configuration set and otherwise lies outside the configuration set, wherein the computing unit is designed to determine whether the object would enter the gripper with the assumed pose by iteratively using the Gilbert-Johnson-Keerthi distance algorithm a simplex of Points within the configuration set are constructed to enclose the origin of the configuration set.

Gemäß einer weiteren vorteilhaften Ausführungsform ist die Recheneinheit dazu ausgeführt, beim Liegen des Ursprungs innerhalb der Konfigurationsmenge das Simplex schrittweise zu vergrößern, bis das dem Ursprung am nächsten liegende Oberflächenelement des Simplex identifiziert ist.According to a further advantageous embodiment, the processing unit is designed to gradually enlarge the simplex when the origin lies within the configuration set, until the surface element of the simplex that is closest to the origin is identified.

Gemäß einer weiteren vorteilhaften Ausführungsform ist die Recheneinheit dazu ausgeführt, die jeweilige konvexe Menge von Objekt und Greifer durch ein jeweiliges Polygonnetz auf Basis einer bekannten Geometrie von Objekt und Greifer zu modellieren.According to a further advantageous embodiment, the processing unit is designed to model the respective convex set of object and gripper using a respective polygon network based on a known geometry of object and gripper.

Gemäß einer weiteren vorteilhaften Ausführungsform ist die Recheneinheit dazu ausgeführt, Initialwerte für die Pose des Objekts vor wiederholter Ausführung der zwei Schritte in der iterativen Ausführung des Kalman Filters durch Vorgabe und/oder auf Basis der Daten der Kameraeinheit zu ermitteln.According to a further advantageous embodiment, the computing unit is designed to determine initial values for the pose of the object before the repeated execution of the two steps in the iterative execution of the Kalman filter by default and/or on the basis of the data from the camera unit.

Gemäß einer weiteren vorteilhaften Ausführungsform ist die Recheneinheit dazu ausgeführt, Initialwerte für die Pose des Objekts alleine auf Basis von Daten der Kameraeinheit zu ermitteln.According to a further advantageous embodiment, the computing unit is designed to determine initial values for the pose of the object solely on the basis of data from the camera unit.

Gemäß einer weiteren vorteilhaften Ausführungsform ist die Recheneinheit dazu ausgeführt, in dem ersten, prädiktivem Schritt auf Basis der Daten der Positionsermittlungseinheit eine Schließgeschwindigkeit des Greifers zur ermitteln und eine resultierende Bewegungsgeschwindgkeit des Objekts durch Berührung des Greifers mit dem Objekt zu prädizieren.According to a further advantageous embodiment, the computing unit is designed to determine a closing speed of the gripper based on the data from the position determination unit in the first, predictive step and to predict a resulting movement speed of the object when the gripper touches the object.

Gemäß einer weiteren vorteilhaften Ausführungsform ist die Recheneinheit dazu ausgeführt, die iterative Anwendung des Kalman Filters dann zu beenden, wenn die Schließbewegung des Greifers beendet ist.According to a further advantageous embodiment, the computing unit is designed to end the iterative application of the Kalman filter when the closing movement of the gripper has ended.

Ein weiterer Aspekt der Erfindung betrifft eine Robotermanipulatoreinheit mit einem Robotermanipulator und mit einem System wie oben und im Folgenden beschrieben.A further aspect of the invention relates to a robot manipulator unit with a robot manipulator and with a system as described above and below.

Vorteile und bevorzugte Weiterbildungen der vorgeschlagenen Robotermanipulatoreinheit ergeben sich durch eine analoge und sinngemäße Übertragung der im Zusammenhang mit dem vorgeschlagenen System vorstehend gemachten Ausführungen.Advantages and preferred developments of the proposed robotic manipulator unit result from an analogous and analogous transfer of the statements made above in connection with the proposed system.

Weitere Vorteile, Merkmale und Einzelheiten ergeben sich aus der nachfolgenden Beschreibung, in der - gegebenenfalls unter Bezug auf die Zeichnung - zumindest ein Ausführungsbeispiel im Einzelnen beschrieben ist. Gleiche, ähnliche und/oder funktionsgleiche Teile sind mit gleichen Bezugszeichen versehen.Further advantages, features and details result from the following description, in which at least one exemplary embodiment is described in detail-if necessary with reference to the drawing. Identical, similar and/or functionally identical parts are provided with the same reference symbols.

Es zeigen:

1: Ein System mit einem Robotermanipulator gemäß einem Ausführungsbeispiel der Erfindung.
2: Einen Greifvorgang eines Objekts in einer beispielhaften Situation.
3: Beschreibungen am Greifer beim Ermitteln der Pose des Objekts gemäß einem Ausführungsbeispiel der Erfindung.
4: Die Architektur eines Kalman Filters gemäß einem Ausführungsbeispiel der Erfindung.
5: Eine mögliche Implementierung für den ersten Schritt in der Ausführung des Kalman Filters nach 4.
6: Eine mögliche Implementierung für den zweiten Schritt in der Ausführung des Kalman Filters nach 4.
7: Eine Erläuterung zur Bildung der Minkowski- Differenz gemäß einem Ausführungsbeispiel der Erfindung.
8: Die schrittweise Vergrößerung eines Simplex gemäß einem Ausführungsbeispiel der Erfindung.

Show it:

1 : A system with a robotic manipulator according to an embodiment of the invention.
2 : A grasping action of an object in an exemplary situation.
3 : Descriptions at the gripper when determining the pose of the object according to an embodiment of the invention.
4 : The architecture of a Kalman filter according to an embodiment of the invention.
5 : A possible implementation for the first step in the execution of the Kalman filter 4 .
6 : A possible implementation for the second step in the execution of the Kalman filter 4 .
7 : An explanation of the formation of the Minkowski difference according to an embodiment of the invention.
8th : The stepwise enlargement of a simplex according to an embodiment of the invention.

Die Darstellungen in den Figuren sind schematisch und nicht maßstäblich.The representations in the figures are schematic and not to scale.

1 zeigt ein System 1 zum Ermitteln einer Pose eines in einem Greifer 3 eines Robotermanipulators 5 zu greifenden Objekts 7. Im Sockel des Robotermanipulators 5 ist hierzu eine Recheneinheit 9 angeordnet, die Daten von einer Kameraeinheit 11 empfängt, die am oberen Ende des distalen Glieds angeordnet ist und dazu dient, mit ihrem Erfassungsbereich den Greifer 3 des Robotermanipulators 5 beim Greifen eines Objekts 7 und das Objekt 7 selbst zu erfassen. Der Greifer 3 weist eine Positionsermittlungseinheit 13 zum Ermitteln eines jeweils aktuellen Schließzustands des Greifers 3 auf. Der Schließzustand gibt hierbei an, wie weit die Greiferbacken des Greifers 3 noch voneinander entfernt sind. Die Recheneinheit 9 ist dazu ausgeführt, das für nichtlineare Systeme 1 geeignete „Erweitertes Kalman Filter“ iterativ an einer Vielzahl aufeinanderfolgender Zeitpunkten auszuführen. Details hierzu werden ab 4 beschrieben. 1 shows a system 1 for determining a pose of an object 7 to be gripped in a gripper 3 of a robotic manipulator 5. For this purpose, a computing unit 9 is arranged in the base of the robotic manipulator 5, which receives data from a camera unit 11, which is arranged at the upper end of the distal limb and serves to detect the gripper 3 of the robot manipulator 5 when gripping an object 7 and the object 7 itself with its detection range. The gripper 3 has a position determination unit 13 for determining a respective current closed state of the gripper 3 . The closed state indicates how far apart the gripper jaws of the gripper 3 are. The arithmetic unit 9 is designed to iteratively execute the “extended Kalman filter” suitable for non-linear systems 1 at a large number of consecutive points in time. Details on this will come off 4 described.

2 zeigt hierbei die einzelnen Stationen im Ablauf beim Greifen des Objekts 7 durch den Greifer 3 des Robotermanipulators 5. In der 2 (A) ist der Ausgangszustand eines typischen Szenarios gezeigt, in dem der Greifer 3 im Bereich des Objekts 7 positioniert wird, um dieses aufzunehmen, zu transportieren und an einem Zielort wieder abzulegen. 2 shows the individual stations in the course of gripping the object 7 by the gripper 3 of the robotic manipulator 5. In FIG 2 (A) shows the initial state of a typical scenario in which the gripper 3 is positioned in the area of the object 7 in order to pick it up, transport it and put it down again at a destination.

Die initiale Schätzung der Pose des Objekts 7 ist dabei über eine kamerabasierte Schätzung gemacht worden. Aufgrund von Ungenauigkeiten bei der Positionsschätzung und der Orientierungsschätzung des Objekts 7 wird jedoch der Greifer 3 vor dem Schließen der Greiferbacken nicht exakt so um das Objekt 7 positioniert, dass das Objekt 7 sich exakt mittig zwischen den Greiferbacken befindet. Beim symmetrischen Schließen der Greiferbacken wird daher das Objekt 7 durch Kollision mit einem der Greiferbacken etwas bewegt. Dadurch unterscheidet sich ab diesem Zeitpunkt die geschätzte, initiale Pose des Objekts 7 (dargestellt in 2 (B) als durchgezogene Linie) von der tatsächlichen Pose des Objekts 7 (dargestellt in 2 (B) als gestrichelte Linie). Aufgrund von Verdeckungen des Objekts 7 durch Bauelemente des Greifers 3, insbesondere der Greiferbacken, kann diese Verschiebung des Objekts 7 nicht alleine kamerabasiert erkannt werden. Unter Verwendung des Schließzustands der Greiferbacken, der bekannten und abgespeicherten Geometrie des Objekts 7 sowie der bekannten und abgespeicherten Geometrie des Greifers 3 selbst kann jedoch die Kollision zwischen dem Objekt 7 und einem der Greiferbacken erkannt werden. Diese Kollisionsfläche mit einem angenommenen, aber nicht realen, Eindringen des Objekts 7 in den Greifer 3 ist in 3 (C) am rechten der Greiferbacken an der Kontaktfläche zum gestrichelt gezeichneten Objekt 7 als dunkel ausgefüllte Fläche skizziert. Eine solches angenommenes Eindringen deutet auf eine Inkonsistenz zwischen der ursprünglich geschätzten Pose des Objekts 7 und der tatsächlichen Pose des Objekts 7 vor dem Greifen des Objekts 7 durch Schließen der Greiferbacken hin, vgl. 3 (D). Es wird in einem solchen Fall im zweiten Schritt der Ausführung des Kalman Filters eine Korrektur der ursprünglich geschätzten Pose des Objekts 7 dadurch ermittelt, dass die jeweiligen Positionen der in 3 (E) gezeigten Punkte a und b ermittelt wird. Diese beiden Punkte liegen auf der Oberfläche des Objekts 7 bzw. eines der Greiferbacken. Die Differenz der Orte der beiden Punkte a und b ist eine Konsequenz der Ungenauigkeit, d. h. der Abweichung zwischen der ursprünglich geschätzten Pose des Objekts 7 und der tatsächlichen Pose des Objekts 7 vor dem Schließen der Greiferbacken. Umgekehrt beschreibt damit jedoch auch diese Differenz der beiden Punkte a und b die kleinstmögliche Verschiebung die zur Auflösung des Eindringens des Objekts 7 in den Greifer 3 führen und gibt damit das Maß einer Korrektur für die im ersten Schritt geänderte Schätzung der Pose des Objekts 7 an, vgl. hierzu die Beschreibung der 4 bis 8.The initial estimate of the pose of the object 7 was made using a camera-based estimate. Due to inaccuracies in the estimation of the position and the orientation of the object 7, however, the gripper 3 is not exactly positioned around the object 7 before the gripper jaws are closed in such a way that the object 7 is located exactly in the middle between the gripper jaws. When the gripper jaws close symmetrically, the object 7 is therefore moved slightly by colliding with one of the gripper jaws. As a result, the estimated, initial pose of object 7 (shown in 2 B) as a solid line) from the actual pose of the object 7 (shown in 2 B) as a dashed line). Due to the fact that the object 7 is covered by components of the gripper 3, in particular the gripper jaws, this displacement of the object 7 cannot be detected solely on the basis of the camera. However, using the closed state of the gripper jaws, the known and stored geometry of the object 7 and the known and stored geometry of the gripper 3 itself, the collision between the object 7 and one of the gripper jaws can be detected. This collision area with an assumed, but not real, penetration of the object 7 into the gripper 3 is in 3 (c) sketched on the right of the gripper jaws at the contact surface to the dashed object 7 as a dark filled area. Such an assumed intrusion indicates an inconsistency between the originally estimated pose of the object 7 and the actual pose of the object 7 before the object 7 was gripped by closing the gripper jaws, cf. 3 (D) . In such a case, in the second step of executing the Kalman filter, a correction of the originally estimated pose of the object 7 is determined by the respective positions of the in 3 (E) shown points a and b is determined. These two points lie on the surface of the object 7 or one of the gripper jaws. The difference in the locations of the two points a and b is a consequence of the inaccuracy, ie the deviation between the originally estimated pose of the object 7 and the actual pose of the object 7 before the gripper jaws were closed. Conversely, however, this difference between the two points a and b also describes the smallest possible displacement that leads to the resolution of the penetration of the object 7 into the gripper 3 and thus indicates the extent of a correction for the estimate of the pose of the object 7 that was changed in the first step, cf. the description of the 4 until 8th .

4 zeigt die Architektur eines von der Recheneinheit 9 verwendeten Erweiterten Kalman Filters EKF. Das Erweiterte Kalman Filter EKF erlaubt die probabilistische Fusionierung von Daten über den Schließzustand bzw. der Schließbewegung des Greifers 3 mit visuellen Informationen aus der Kameraeinheit 11, die den Schließvorgang beobachtet, wodurch explizit die Unsicherheiten der kamerabasierten Erfassung sowie der berührungsbasierten Erfassung berücksichtigt werden. Hierbei bezeichnen M ein Messmodel, und S1 den ersten Schritt und S2den zweiten Schritt. Es erfolgt eine Schleifenausführung, wobei in jeder der iterativen Anwendungen des Kalman Filters EKF die zwei jeweils aufeinander folgenden Schritte S1 und S2 ausgeführt werden. Einzelheiten zur Implementierung zum ersten Schritt S1 sind in der 5 gezeigt, zum zweiten Schritt S2 in der 6. 4 shows the architecture of an extended Kalman filter EKF used by the processing unit 9 . The Extended Kalman Filter EKF allows the probabilistic fusion of data about the closing state or the closing movement of the gripper 3 with visual information from the camera unit 11, which observes the closing process, which explicitly takes into account the uncertainties of the camera-based detection and the touch-based detection. Here, M denotes a measurement model, and S1 denotes the first step and S2 denotes the second step. A loop is executed, with the two successive steps S1 and S2 being executed in each of the iterative applications of the Kalman filter EKF. Implementation details for the first step S1 are in the 5 shown, to the second step S2 in the 6 .

5 erläutert eine Implementierung des ersten, prädiktiven Schritts S1. In diesem ersten, prädiktivem Schritt wird aus einer mittels der Positionsermittlungseinheit 13 ermittelten Verschiebung einer Greiferschließposition eine resultierende Bewegung des Objekts 7 durch Berührung des Greifers 3 mit dem Objekt 7 prädiziert und auf Basis der prädizierten Bewegung des Objekts 7 eine vorherige Schätzung der Pose des Objekts 7 zum Greifer 3 geändert. Die Gleichungen (1) bis (5) erläutern dies. Hierbei repräsentiert der Zustand y die Pose des Objekts 7 und die Kovarianzmatrix P deren Unsicherheit. Der Vektor u beschreibt den Regelungseingang in den EKF und z ist der Vektor der Beobachtungen, die von den Daten der Kameraeinheit 11 gebildet werden. Der skalare Index t steht für die Zeit an den Zeitpunkten, sodass der Zeitpunkt t einen Zeitschritt nach dem Zeitpunkt t-1 folgt. Eine Variable mit dem Index t-1 bezeichnet daher den Wert der Variablen zum Zeitpunkt t-1, der einen Schritt vor dem Zeitpunkt tliegt. Ebenso bezeichnet der Block t+1 den Sprung zu einem neuen Zeitschritt an einem weiteren Zeitpunkt. Die Gleichungen (1) bis (3) geben den ersten Schritt, den prädiktiven Schritt, im engeren Sinne wieder. Die Matrix R in Gleichung (3) beschreibt das geschätzte Prozessrauschen. Der Operator (d/dt) bezeichnet ferner die zeitliche Ableitung der darauf folgend notierten Variablen. Für die Berechnung der Prädiktion werden die Gelenkgeschwindigkeiten (d/dt)q (mit q als den Gelenkwinkeln) als der Regelungseingang u verwendet. Unter Nutzung der Jacobi Matrix J werden in Gleichung (4) die Gelenkgeschwindigkeiten auf eine Bewegung der identifizierten Kontaktpunkte umgerechnet, dies ist auch der folgenden Veröffentlichung zu entnehmen: Prattichizzo, D. and Trinkle, J. C. (2008). Grasping. In Springer Handbook of Robotics, chapter 28, pages 671-700. Springer. Dabei ist (d/dt)c der Vektor der Geschwindigkeiten der Kontaktpunkte c. Diese werden anschließend in Gleichung (5) mithilfe der Pseudo-Inversen der Greifmatrix G auf die Objektgeschwindigkeit (d/dt)y umgerechnet. Diese Prädiktion erlaubt es, Bewegungen des Objekts 7, die aus einer Änderung der Greiferposition resultieren, abzuschätzen. Eventuelle Inkonsistenzen im geschätzten Greifzustand, wie eine Durchdringung des Objektes 7 und des Greifers 3, werden durch diese Berechnung allerdings nicht behoben. Dies erfolgt im zweiten Schritt des EKF, der Korrektur (Aktualisierung), vgl. 6. 5 explains an implementation of the first, predictive step S1. In this first, predictive step, a movement of the object 7 resulting from contact of the gripper 3 with the object 7 is predicted from a displacement of a gripper closed position determined by the position determination unit 13 and based on the predicted th movement of the object 7 a previous estimate of the pose of the object 7 to the gripper 3 changed. Equations (1) to (5) explain this. Here, the state y represents the pose of the object 7 and the covariance matrix P its uncertainty. The vector u describes the control input into the EKF and z is the vector of the observations that are formed from the data from the camera unit 11. The scalar index t represents the time at the points in time such that point in time t follows one time step after point in time t-1. A variable with index t-1 therefore denotes the value of the variable at time t-1, which is one step before time t. Likewise, the block t+1 designates the jump to a new time step at a further point in time. Equations (1) to (3) reflect the first step, the predictive step, in the narrower sense. The matrix R in equation (3) describes the estimated process noise. The operator (d/dt) also designates the time derivative of the variables noted below. For the computation of the prediction, the joint velocities (d/dt)q (with q as the joint angles) are used as the control input u. Using the Jacobi Matrix J, the joint velocities are converted to a movement of the identified contact points in Equation (4), this can also be taken from the following publication: Prattichizzo, D. and Trinkle, JC (2008). grassping. In Springer Handbook of Robotics, chapter 28, pages 671-700. jumper. where (d/dt)c is the vector of the velocities of the contact points c. These are then converted to the object speed (d/dt)y in equation (5) using the pseudo-inverse of the gripping matrix G. This prediction allows movements of the object 7 resulting from a change in the position of the gripper to be estimated. Any inconsistencies in the estimated gripping state, such as penetration of the object 7 and the gripper 3, are not corrected by this calculation, however. This takes place in the second step of the EKF, the correction (update), cf. 6 .

6 zeigt eine Implementierung des zweiten, aktualisierenden Schritts. Hierbei wird von der Recheneinheit 9 das Vorliegen einer nach dem ersten Schritt verbliebenen Inkonsistenz der geänderten Schätzung der Pose des Objekts 7 relativ zum Greifer 3 geprüft, die ein zumindest partielles Eindringen des Objekts 7 in den Greifer 3 bedeuten würde, und im Fall des Vorliegens dadurch behoben wird, dass von der Recheneinheit 9 auf Basis der Daten der Kameraeinheit 11 eine Korrektur der im ersten Schritt geänderten Schätzung der Pose auf Basis einer angenommenen Eindringtiefe des Objekts 7 in den Greifer 3 unter Beachtung der Bedingung der Ortsidentität von Kontaktpunkten auf der jeweiligen Oberfläche von Greifer 3 und Objekt 7 bei Kontakt des Greifers 3 mit dem Objekt 7 zum Auflösen des angenommenen Eindringens des Objekts 7 in den Greifer 3 ermittelt wird. Die im zweiten Schritt verwendete Korrekturberechnung beruht auf der Tatsache, dass für einen physisch existierenden Kontakt zwischen dem Greifer 3 und dem Objekt 7 der Kontaktpunkt auf der Oberfläche des Objektes 7 identisch zu dem Kontaktpunkt auf der Oberfläche des Greifers 3 sein muss. Das heißt, für die identifizierten Kontaktpunkte a und b (vgl. Erläuterungen der 2 und 3) muss a=b gelten. Da dies im Falle des Eindringens des Objekts 7 in den Greifer 3 allerdings nicht gilt, ist das Ziel des zweiten Schritts eine aktualisierte Schätzung der Pose des Objekts 7 zu bestimmen, die die Differenz zwischen den beiden Punkten a und b aufhebt. Der zweite Schritt des EKF wird durch das Beobachtungsmodell (Measurement Model M) h(y_t) und den Beobachtungsvektor z_t mit den Daten der Kameraeinheit 11 ermöglicht. Das Beobachtungsmodell M mit der Funktion h(y_t) ist in den Gleichungen (6) und (7) definiert. Weitere Details zum Beobachtungsmodell M sind in den 7 und 8 gezeigt. Die Berechnung der eigentlichen Korrektur erfolgt mithilfe der EKF Gleichungen (8) und (9). Hierbei beschreibt die Matrix Q das Messrauschen. Um die Ableitung des Beobachtungsmodells H zu bestimmen, wird wiederum die Greifmatrix G genutzt, da diese den Zusammenhang zwischen der Bewegung der Kontaktpunkte und des Objektes 7 nach Gleichung (10) beschreibt. 6 shows an implementation of the second, updating step. In this case, the computing unit 9 checks the presence of an inconsistency in the changed estimate of the pose of the object 7 relative to the gripper 3 that remained after the first step, which would mean at least partial penetration of the object 7 into the gripper 3, and in the event of the presence thereby is rectified that the computing unit 9, on the basis of the data from the camera unit 11, corrects the estimate of the pose changed in the first step on the basis of an assumed penetration depth of the object 7 into the gripper 3, taking into account the condition of the location identity of contact points on the respective surface of Gripper 3 and object 7 is determined upon contact of gripper 3 with object 7 in order to resolve the assumed intrusion of object 7 into gripper 3 . The correction calculation used in the second step is based on the fact that the contact point on the surface of the object 7 must be identical to the contact point on the surface of the gripper 3 for a physically existing contact between the gripper 3 and the object 7 . That is, for the identified contact points a and b (cf. explanations of the 2 and 3 ) a=b must apply. However, since this does not apply if the object 7 penetrates the gripper 3, the aim of the second step is to determine an updated estimate of the pose of the object 7 that cancels out the difference between the two points a and b. The second step of the EKF is made possible by the observation model (measurement model M) h(y _t ) and the observation vector z _t with the data from the camera unit 11 . The observation model M with the function h(y _t ) is defined in equations (6) and (7). Further details on the observation model M are in the 7 and 8th shown. The actual correction is calculated using the EKF equations (8) and (9). Here, the matrix Q describes the measurement noise. In order to determine the derivation of the observation model H, the gripping matrix G is again used, since this describes the connection between the movement of the contact points and the object 7 according to equation (10).

7 zeigt die grundlegende Idee der Anwendung der Minkowski- Differenz im Beobachtungsmodell M, anhand der beispielhaften Körper A und B. Die Recheneinheit 9 ist dazu ausgeführt, zum Prüfen des Vorliegens der Inkonsistenz die angenommene Eindringtiefe des Objekts 7 in den Greifer 3 oder einen Abstand zwischen Objekt 7 und Greifer 3 durch eine Minkowski- Differenz aus Objekt 7 und Greifer 3 (vgl. abstrahiert Mengen A und B in 7) unter Anwendung des Gilbert-Johnson-Keerthi Distanzalgorithmus zu ermitteln, wobei die jeweilige geometrische Ausdehnung von Objekt 7 und Greifer 3 als konvexe Mengen modelliert ist. Die jeweilige konvexe Menge von Objekt 7 und Greifer 3 wiederum wird dabei durch ein jeweiliges Polygonnetz auf Basis einer bekannten Geometrie von Objekt 7 und Greifer 3 modelliert. Hierzu wird von der Recheneinheit 9 mit der Anwendung des Gilbert-Johnson-Keerthi Distanzalgorithmus eine Konfigurationsmenge als Abstand zwischen Ursprung und repräsentierendem Körper aus der Minkowski- Differenz zwischen Objekt 7 und Greifer 3 ermittelt, sodass bei einem angenommenen Eindringen des Objekts 7 in den Greifer 3 der Ursprung innerhalb der Konfigurationsmenge liegt und andernfalls außerhalb der Konfigurationsmenge liegt. Anwendung und Spezifikation des Gilbert-Johnson-Keerthi Distanzalgorithmus ist grundsätzlich der Fachwelt bekannt und kann unter anderem der Publikation „Gilbert, E. G., Johnson, D. W., and Keerthi, S. S. (1988). A fast procedure for computing the distance between complex objects in threedimensional space. IEEE Journal on Robotics and Automation, 4(2):193-203“ entnommen werden. Die Lage der Punkte a und b wird mithilfe einer erweiterten Version des Gilbert-Johnson-Keerthi (GJK) Distanzalgorithmus bestimmt. Dieser ist in der Lage den Kollisionszustand zweier Objekte zu ermitteln. Eine in „Cameron, S. (1997). Enhancing GJK: Computing minimum and penetration distances between convex polyhedra. In 1997 IEEE International Conference on Robotics and Automation (ICRA), volume 4, pages 3112-3117. IEEE“ vorgestellte Weiterentwicklung ermöglicht es darauf aufbauend, den euklidischen Abstand zwischen zwei Körpern wie dem Objekt 7 und dem Greifer 3 zu berechnen, wenn diese nicht ineinander eindringen. Allerdings ist es mit diesem bekannten Algorithmus nicht möglich, die Eindringtiefe zweier kollidierender Körper zu berechnen. Die vorgeschlagene Erweiterung der Methode erlaubt die genaue Berechnung der Durchdringung, sowie die Bestimmung der Punkte a und b, deren Differenzvektor die kleinstmögliche Verschiebung zur Aufhebung der Kollision beschreibt. Das Ziel des GJK Algorithmus ist die iterative Prozessierung zweier konvexer Mengen zur Bestimmung des minimalen Abstands zwischen diesen. Für das vorliegende Anwendungsszenario bestehen diese Mengen aus Polygonnetzen, die die Geometrien des Greifers 3 und des zu manipulierenden Objektes 7 repräsentieren. Der GJK Algorithmus reduziert die Komplexität der Berechnung des Abstands zwischen zwei Körpern wie den symbolischen A und B in der 7 auf die Bestimmung des Abstands zwischen einem Körper und dem Ursprung. Diese einzelne Menge, die so genannte Konfigurationsmenge, auch bekannt unter „Configuration Space Obstacle“ oder kurz „CSO“, wird durch die Berechnung der Minkowski Differenz der beiden ursprünglichen Mengen gebildet, wie in 7 symbolisiert. Das linke Bildpaar in 7 gilt hierbei für ein CSO ohne Eindringen der Körper ineinander, das rechte Bildpaar für ein CSO im Falle des Eindringens. Die Recheneinheit 9 ist hierbei zum Ermitteln ausgeführt, ob das Objekt 7 in den Greifer 3 mit der angenommenen Pose eindringen würde, indem durch Anwendung des Gilbert-Johnson-Keerthi Distanzalgorithmus iterativ ein Simplex von Punkten innerhalb der Konfigurationsmenge konstruiert wird, um den Ursprung von der Konfigurationsmenge zu umschließen. Beim Liegen des Ursprungs innerhalb der Konfigurationsmenge CSO wird das Simplex schrittweise vergrößert, bis das dem Ursprung am nächsten liegende Oberflächenelement des Simplex identifiziert ist. Wenn sich die beiden Körper durchdringen, das heißt das Objekt 7 in den Greifer 3 angenommen eindringt, liegt der Ursprung innerhalb des CSO, andernfalls liegt er außerhalb. 7 shows the basic idea of using the Minkowski difference in the observation model M, using the exemplary bodies A and B. The computing unit 9 is designed to check the presence of the inconsistency, the assumed penetration depth of the object 7 in the gripper 3 or a distance between the object 7 and gripper 3 by a Minkowski difference from object 7 and gripper 3 (cf. abstracted sets A and B in 7 ) to be determined using the Gilbert-Johnson-Keerthi distance algorithm, with the respective geometric extent of object 7 and gripper 3 being modeled as convex sets. The respective convex set of object 7 and gripper 3 is in turn modeled by a respective polygon mesh based on a known geometry of object 7 and gripper 3 . For this purpose, the processing unit 9 uses the Gilbert-Johnson-Keerthi distance algorithm to determine a configuration set as the distance between the origin and the representative body from the Minkowski difference between the object 7 and the gripper 3, so that if the object 7 is assumed to penetrate the gripper 3 the origin is within the configuration set and otherwise is outside the configuration set. The application and specification of the Gilbert-Johnson-Keerthi distance algorithm is generally known to experts and can be found in the publication "Gilbert, EG, Johnson, DW, and Keerthi, SS (1988). A fast procedure for computing the distance between complex objects in three-dimensional space. IEEE Journal on Robotics and Automation, 4(2):193-203”. The locations of points a and b are determined using an extended version of the Gilbert-Johnson-Keerthi (GJK) distance algorithm. This is able to determine the collision status of two objects. One in “Cameron, S. (1997). Enhancing GJK: Computing minimum and penetration distances between convex polyhedra. In 1997 IEEE International Conference on Robotics and Automation (ICRA), volume 4, pages 3112-3117. Based on this, the further development presented by the IEEE makes it possible to calculate the Euclidean distance between two bodies such as the object 7 and the gripper 3 if they do not penetrate each other. However, it is not possible with this known algorithm to calculate the penetration depth of two colliding bodies. The proposed extension of the method allows the exact calculation of the penetration, as well as the determination of the points a and b, whose difference vector describes the smallest possible displacement to cancel the collision. The goal of the GJK algorithm is the iterative processing of two convex sets to determine the minimum distance between them. For the present application scenario, these sets consist of polygon meshes that represent the geometries of the gripper 3 and the object 7 to be manipulated. The GJK algorithm reduces the complexity of calculating the distance between two bodies such as the symbolic A and B in the 7 on determining the distance between a body and the origin. This single set, called the configuration set, also known as the "configuration space obstacle" or "CSO" for short, is formed by computing the Minkowski difference of the two original sets, as in 7 symbolizes. The pair of images on the left in 7 applies here to a CSO without intrusion of the bodies into each other, the right pair of images for a CSO in the event of intrusion. The arithmetic unit 9 is designed to determine whether the object 7 would enter the gripper 3 with the assumed pose by iteratively constructing a simplex of points within the configuration set using the Gilbert-Johnson-Keerthi distance algorithm in order to remove the origin from the to enclose configuration set. If the origin lies within the configuration set CSO, the simplex is increased step by step until the surface element of the simplex that is closest to the origin is identified. If the two bodies interpenetrate, that is to say the object 7 enters the gripper 3, the origin is inside the CSO, otherwise it is outside.

8 zeigt weitere Details zur Ermittlung der Punkte a und b. Um den Kollisionszustand zu bestimmen, konstruierter der GJK Algorithmus iterativ ein Simplex von Punkten innerhalb des CSO, um den Ursprung zu umschließen. Falls es nicht möglich ist, den Ursprungspunkt einzuschließen, folgt, dass die beiden Mengen nicht in Durchdringung sind. Sobald der Durchdringungszustand bestimmt wurde, wird das Simplex weiter entwickelt, um den genauen Abstand bzw. die Eindringtiefe zu ermitteln. Bei 3D Geometrien besteht dies darin, den Punkt, die Kante oder die Fläche des CSO zu finden, die dem Ursprung am nächsten ist. Für den nicht überschneidenden Fall, wurde dies in der Veröffentlichung „Cameron, S. (1997). Enhancing GJK: Computing minimum and penetration distances between convex polyhedra. In 1997 IEEE International Conference on Robotics and Automation (ICRA), volume 4, pages 3112-3117. IEEE“ beschrieben. Für den sich schneidenden Fall, ist der Algorithmus zu erweitern, um dieses Problem lösen zu können. Das vorgeschlagene Verfahren realisiert dies, indem es das Simplex innerhalb der Konfigurationsmenge CSO vergrößert, bis das dem Ursprung am nächsten liegende Oberflächenelement identifiziert wurde, siehe 8. Der Punkt, die Kante oder die Fläche der Konfigurationsmenge CSO, die dem Ursprung am nächsten liegt, wird anschließend auf die entsprechenden Oberflächenelemente in den Geometrien der ursprünglichen beiden Körper abgebildet. Schließlich können die genauen Kontaktpunkte a und b auf dem Objekt 7 und dem Greifer 3 berechnet werden und für die Anwendung des zweiten Schritts nach 6 verwendet werden. 8th shows more details on how to determine points a and b. To determine the collision state, the GJK algorithm iteratively constructed a simplex of points within the CSO to enclose the origin. If it is not possible to enclose the origin point, it follows that the two sets are not in intersection. Once the state of penetration has been determined, the simplex is further developed to determine the exact distance or depth of penetration. For 3D geometries, this consists of finding the point, edge, or face of the CSO that is closest to the origin. For the non-overlapping case, this was discussed in the publication Cameron, S. (1997). Enhancing GJK: Computing minimum and penetration distances between convex polyhedra. In 1997 IEEE International Conference on Robotics and Automation (ICRA), volume 4, pages 3112-3117. IEEE” described. For the intersecting case, the algorithm has to be extended in order to be able to solve this problem. The proposed method realizes this by increasing the simplex within the configuration set CSO until the surfel closest to the origin has been identified, see 8th . The point, edge, or face of the configuration set CSO that is closest to the origin is then mapped to the corresponding surfels in the geometries of the original two bodies. Finally, the exact contact points a and b on the object 7 and the gripper 3 can be calculated and used according to the second step 6 be used.

Obwohl die Erfindung im Detail durch bevorzugte Ausführungsbeispiele näher illustriert und erläutert wurde, so ist die Erfindung nicht durch die offenbarten Beispiele eingeschränkt und andere Variationen können vom Fachmann hieraus abgeleitet werden, ohne den Schutzumfang der Erfindung zu verlassen. Es ist daher klar, dass eine Vielzahl von Variationsmöglichkeiten existiert. Es ist ebenfalls klar, dass beispielhaft genannte Ausführungsformen wirklich nur Beispiele darstellen, die nicht in irgendeiner Weise als Begrenzung etwa des Schutzbereichs, der Anwendungsmöglichkeiten oder der Konfiguration der Erfindung aufzufassen sind. Vielmehr versetzen die vorhergehende Beschreibung und die Figurenbeschreibung den Fachmann in die Lage, die beispielhaften Ausführungsformen konkret umzusetzen, wobei der Fachmann in Kenntnis des offenbarten Erfindungsgedankens vielfältige Änderungen, beispielsweise hinsichtlich der Funktion oder der Anordnung einzelner, in einer beispielhaften Ausführungsform genannter Elemente, vornehmen kann, ohne den Schutzbereich zu verlassen, der durch die Ansprüche und deren rechtliche Entsprechungen, wie etwa weitergehende Erläuterungen in der Beschreibung, definiert wird.Although the invention has been illustrated and explained in more detail by means of preferred exemplary embodiments, the invention is not restricted by the disclosed examples and other variations can be derived therefrom by a person skilled in the art without departing from the protective scope of the invention. It is therefore clear that a large number of possible variations exist. It is also understood that the exemplary embodiments given are really only examples and should not be construed as limiting in any way the scope, applications or configuration of the invention. Rather, the preceding description and the description of the figures enable the person skilled in the art to concretely implement the exemplary embodiments, whereby the person skilled in the art, knowing the disclosed inventive concept, can make a variety of changes, for example with regard to the function or the arrangement of individual elements mentioned in an exemplary embodiment. without departing from the scope of protection defined by the claims and their legal equivalents, such as further explanations in the description.

BezugszeichenlisteReference List

11: Systemsystem
33: Greifergripper
55: Robotermanipulatorrobotic manipulator
77: Objektobject
99: Recheneinheitunit of account
1111: Kameraeinheitcamera unit
1313: Positionsermittlungseinheitposition determination unit

Claims

System (1) for determining a pose of an object (7) gripped in a gripper (3) of a robot manipulator (5), having a computing unit (9) and a camera unit (11) which is used to capture the gripper ( 3) the robot manipulator (5) when gripping an object (7) and to detect the object (7), and having a position determination unit (13) for determining a current closed state of the gripper (3), the computing unit (9) being executed for this purpose is to execute a Kalman filter iteratively at a large number of successive points in time, and to execute two successive steps in each of the iterative applications of the Kalman filter, wherein in a first predictive step a displacement of a gripper closed position determined by means of the position determination unit (13) results in a Movement of the object (7) is predicted by contact of the gripper (3) with the object (7) and based on the predicted movement of the object (7) a previous estimate of the pose of the object (7) to the gripper (3) is changed, and in a second, updating step, the computing unit (9) checks the presence of an inconsistency in the changed estimate of the pose of the object (7) relative to the gripper (3) that remained after the first step, which indicates at least partial intrusion of the object (7 ) into the gripper (3) would mean, and if it is present, it is remedied by the computing unit (9) correcting the estimate of the pose, which was changed in the first step, based on an assumed penetration depth based on the data from the camera unit (11). of the object (7) into the gripper (3) taking into account the condition of the location identity of contact points on the respective surface of the gripper (3) and object (7) upon contact of the gripper (3) with the object (7) to resolve the assumed Penetration of the object (7) into the gripper (3) is determined, so that the two steps in the execution of the Kalman filter carried out in a subsequent iteration are based on the estimate of the pose changed in the first step, and if there is an inconsistency on the basis of the modified estimate of the pose corrected in the second step.

System (1) after claim 1 , wherein the computing unit (9) is designed to check the presence of the inconsistency, the assumed penetration depth of the object (7) in the gripper (3) or a distance between the object (7) and gripper (3) by a Minkowski difference To determine object (7) and gripper (3) using the Gilbert-Johnson-Keerthi distance algorithm, the respective geometric extension of object (7) and gripper (3) being modeled as a convex set.

System (1) after claim 2 , wherein the computing unit (9) is designed to determine a configuration set as the distance between the origin and the representative body from the Minkowski difference between the object (7) and the gripper (3) using the Gilbert-Johnson-Keerthi distance algorithm, so that at an assumed intrusion of the object (7) into the gripper (3) the origin lies within the configuration set and otherwise lies outside the configuration set, wherein the computing unit (9) is designed to determine whether the object (7) in the gripper (3) with the assumed pose by iteratively constructing a simplex of points within the configuration set to enclose the origin of the configuration set by applying the Gilbert-Johnson-Keerthi distance algorithm.

System (1) after claim 3 , wherein the computing unit (9) is designed to increase the simplex step by step when the origin lies within the configuration set, until the surface element of the simplex that is closest to the origin is identified.

System (1) according to one of claims 2 until 4 , wherein the computing unit (9) is designed to model the respective convex set of object (7) and gripper (3) by a respective polygon network based on a known geometry of object (7) and gripper (3).

System (1) according to one of the preceding claims, wherein the computing unit (9) is designed to calculate initial values for the pose of the object (7) before repeated execution of the two steps in the iterative execution of the Kalman filter by specification and/or on the basis of the To determine data of the camera unit (11).

System (1) after claim 6 , wherein the computing unit (9) is designed to determine initial values for the pose of the object (7) solely on the basis of data from the camera unit (11).

System (1) according to one of the preceding claims, wherein the computing unit (9) is designed to determine a closing speed of the gripper (3) and a resulting movement speed of the object in the first, predictive step on the basis of the data from the position determination unit (13). (7) to predict by touching the gripper (3) with the object (7).

System (1) according to one of the preceding claims, wherein the computing unit (9) is designed to end the iterative application of the Kalman filter when the closing movement of the gripper (3) has ended.

Robot manipulator unit with a robot manipulator (5) and with a system (1) according to one of the preceding claims.