DE102009006256B4

DE102009006256B4 - Verfahren zur Vermeidung von Kollisionen gesteuert beweglicher Teile einer Anlage

Info

Publication number: DE102009006256B4
Application number: DE102009006256.4A
Authority: DE
Inventors: Udo Frese; Holger Täubig
Original assignee: Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
Current assignee: Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
Priority date: 2009-01-27
Filing date: 2009-01-27
Publication date: 2019-01-03
Anticipated expiration: 2029-01-28
Also published as: DE102009006256A1; WO2010085944A1

Abstract

Verfahren zur Vermeidung von Kollisionen gesteuert beweglicher Teile (10; 12; 14; 16) einer Anlage mit einer Anlagensteuerung, wobei das oder die gesteuert bewegliche(n) Teile(e) aus mindestens zwei durch einen gesteuert beweglichen Mechanismus verbundenen starren Körpern (19) besteht/bestehen und an jedem Körper ein körperfestes 3D-Koordinatensystem definiert wird und jeder Körper in besagtem körperfesten 3D-Koordinatensystem als 3D-Volumen beschrieben und das 3D-Volumen als konvexe Hülle einer endlichen Menge von Punkten (p)zzgl. eines Pufferradius r>=0 nach der Definitiondargestellt wird, umfassend die Durchführung der nachfolgenden Schritte in jedem Takt der Anlagensteuerung:a) für jeden gesteuert beweglichen Mechanismus Bestimmen einer Grenze bzgl. seiner Position, innerhalb derer er bei einer sofortigen Bremsung zum Stillstand kommen würde,b) für jeden Körper der Anlage Berechnen von Bremszonen als 3D-Volumina in der Darstellung nach (1) im körperfesten und im weltfesten 3D-Koordinatensystem und in allen Koordinatensystemen dazwischen in der Kette gesteuert beweglicher Mechanismen, die den jeweiligen Körper mit der weltfesten Umgebung verbinden, wobei die Bremszonen berechnet werden, indem, von der bekannten Darstellung nach (1) des Körpers startend, sukzessive der Effekt jedes gesteuert beweglichen Mechanismus entlang dieser Kette eingerechnet wird, indem für jeden ursprünglichen Punkt pseine Bewegung beim Bremsen des gesteuert beweglichen Mechanismus nach a) durch ein Volumen V(r;(p)), dargestellt mit Pufferradien rund Punkten p, nach (1) überdeckt wird, die resultierenden Punkte zu einer konvexen Hülle zusammengeführt werden und der ursprüngliche Pufferradius um das Maximum der rerhöht wird,c) Bestimmen über alle Paare von Körpern die Distanz zwischen ihren Bremszonen im ersten gemeinsamen Koordinatensystem der beiden in b) auftretenden Ketten von gesteuert beweglichen Mechanismen,d) wenn mindestens eine der bestimmten Distanzen den Wert Null aufweist, Anhalten oder Verlangsamen zumindest eines Teilbereichs der Anlage.

Description

Die vorliegende Erfindung betrifft ein Verfahren zur Vermeidung von Kollisionen gesteuert beweglicher bzw. bewegter Teile einer Anlage (Anlagenteile), insbesondere von Roboterarmen und Fahrzeugen, speziell von fahrerlosen Transportsystemen (FTS), und ganz allgemein ein Verfahren zur Vermeidung von Kollisionen gesteuert relativ zueinander beweglicher bzw. bewegter Objekte. Insbesondere dient das Verfahren dazu, gesteuert bewegte Teile einer Anlage zu überwachen und rechtzeitig einen Halt auszulösen, bevor diese miteinander oder mit anderen (unbeweglichen) Teilen bzw. der unbeweglichen Umgebung kollidieren. Gesteuert bewegliche Teile in diesem Sinne sind insbesondere, aber nicht ausschließlich: a) Roboterarme, Achsentische, Portalkräne oder allgemein jede zyklenfreie Hintereinanderschaltung von rotatorischen oder linearen Gelenken und außerdem b) Fahrzeuge und Fahrzeuge, auf denen solche Hintereinanderschaltungen von Gelenken montiert sind.
Es gibt sehr ausgefeilte Lösungen für a-priori-Bahnplanung, bei denen die Bahn im Computer auf Kollision getestet und dann in eine Anlage (z. B. einen Roboterarm) geladen wird. Dazu bedarf es aber einer festen Bahn. Bei einer manuellen oder sensorgeführten Steuerung der Roboterbewegung geht dies nicht.
Zum Stand der Technik gehört ebenfalls das Testen der aktuellen Stellung eines Roboters auf Distanz zu Hindernissen, so dass in einem festen Sicherheitsabstand zum Hindernis gebremst werden kann. Dies ist aber nur für kleine Geschwindigkeiten praktikabel. Unterschiedliche Teile des Roboters bewegen sich nämlich sehr unterschiedlich schnell und brauchen sehr unterschiedliche Bremswege. Außerdem hat der Bremsweg eine Richtung, nämlich die Bewegungsrichtung, während ein Sicherheitsabstand in allen Richtungen und an allen Punkten eines Roboters gleich wirkt. Dadurch wird bei höheren Geschwindigkeiten der Sicherheitsabstand so konservativ, dass das System oft stockt, obwohl dies unnötig wäre.
US 6 678 582 A von W. El-Houssaine, „Method and control device for avoiding collisions between cooperating robots“, schlägt vor, statt der aktuellen Stellung die Stellung auf Kollision zu testen, bei der ein Roboter anhalten wird. Das ist besser, weil es keinen Sicherheitsabstand in allen Richtungen aufschlägt, sondern die Bewegungsrichtung berücksichtigt. Es ist aber nicht wirklich sicher, weil es nicht ausschließt, dass es während des Bremens zu einer Kollision kommt, die am nominellen Haltepunkt dann schon wieder vorbei wäre. Das ist praktisch unerwünscht, vor allen Dingen aber entsteht dadurch ein (rechtliches) Risiko, das Hersteller nicht eingehen möchten.
Da viele Lösungen sehr rechenintensiv sind, existieren bereits viele Ansätze zur Reduzierung der Rechenzeit. Werden Roboter und Umgebung in n Teile zerlegt, müssen im Prinzip n² Paare auf Kollision getestet werden. In dem US-Patent US 5 056 031 A von M. Nakano et al., „Apparatus for detecting the collision of moving objects“, wird eine Hierarchie von Volumina aufgebaut, so dass, wenn ein Paar von Obervolumina eine bestimmte Distanz hat, alle ihre Paare von Untervolumina mindestens dieselbe Distanz haben. Diese Strategie funktioniert gut, ist aber nicht ganz einfach zu implementieren. Außerdem hängt die Rechenzeit von der Stellung des Roboters ab. Weiterhin ist es bei solchen Hierarchien schwierig, Bremswege mit einzubeziehen, weil sich dann die auf Kollision zu überprüfenden Volumina mit dem Betriebszustand des Roboters ändern und es unmöglich wird, Hilfsinformationen aus der Robotergeometrie vorab zu berechnen.
Die EP 1 901 150 A1 offenbart ein Verfahren zur Vermeidung von Kollisionen gesteuert beweglicher Teile einer Anlage mit einer Anlagensteuerung, wobei das oder die gesteuert bewegliche (n) Teile (n) aus mindestens zwei durch einen gesteuert beweglichen Mechanismus verbundenen starren Körpern besteht/bestehen und an jedem Körper ein körperfestes 3D-Koordinatensystem definiert wird und jeder Körper in besagtem körperfesten 3D-Koordinatensystem als 3D-Volumen beschrieben wird. In besagter Druckschrift geht es um die Bestimmung einer Stoppzeit.
IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL., 18, NO. 3, JUNE 2002 betrifft eine Technik zur Detektion von Kollisionen basierend auf der Anwendung einer Hough-Transformation. Die Hough-Transformation dient zur Berechnung eines Abstands und zur Vermeidung einer Kollision.
US 6 099 573 A betrifft ein Verfahren und eine Vorrichtung zur Modellierung von Interaktionen.
Proceedings of the 30th Conference on Decision and Control, Brighton, England - December 1991 betrifft die Navigation und Steuerung eines mobilen Roboters zwischen sich bewegenden Hindernissen.
DE 102 26 140 A1 offenbart ein Verfahren und eine Vorrichtung zum Vermeiden von Kollisionen zwischen Industrierobotern und anderen Objekten. Das Verfahren beinhaltet Bestimmen einer Anhaltezeit für eine automatisch oder manuell gesteuerte Roboterbewegung auf der Grundlage gegenwärtiger und früherer Gelenkstellungen und Gelenkgeschwindigkeiten für jedes Robotergelenk; Vorhersagen einer Konfiguration einer Bewegungsbahn des Roboters bei der Anhaltezeit; Überprüfen der vorhergesagten Konfiguration mittels Abstands-/Behinderungsalgorithmen hinsichtlich Behinderungen von Teilen des Roboters durch Teile der anderen Objekte oder umgekehrt; und Anhalten des Roboters und/oder der anderen Objekte im Falle einer drohenden Kollision.
DE 103 61 132 A1 betrifft ein Verfahren zur Überwachung der Bewegung eines sich in mehreren Freiheitsgraden bewegenden Gefahr bringenden Objektes wie Handhabungsmasse und/oder der beweglichen Masse eines Handhabungsgerätes, wobei eine Geschwindigkeit und/oder eine Position im Bezug zu einem still stehenden oder sich bewegenden gefährdeten Objekt wie im Arbeitsraum des Gefahr bringenden Objektes stehende oder bewegenden Person und/oder im Arbeitsraum des Gefahr bringenden Objektes definiert kartesische oder achsenspezifische Schutzzone, wobei eine kinetische Energie des sich bewegenden Gefahr bringenden Objektes ständig berechnet wird, wobei eine Entfernung des sich bewegenden Objektes zu einem potenziellen Kollisionspunkt mit dem gefährdeten Objekt ständig berechnet wird, wobei die Geschwindigkeit des Gefahr bringenden Objektes ständig nachgeregelt und überwacht wird, damit in Abhängigkeit der kinetischen Energie des Gefahr bringenden Objektes und der Entfernung zwischen dem Gefahr bringenden Objekt und dem gefährdeten Objekt derart eingestellt wird, dass die kinetische Energie bei einer Bewegung in Richtung des gefährdeten Objekts bis zu einem gewünschten Wert abgebremst wird.
Die EP 1 901 151 A1 betrifft ein Verfahren und eine Vorrichtung zur Vermeidung von Kollisionen zwischen einem Industrieroboter und einem Objekt.
Der vorliegenden Erfindung liegt somit die Aufgabe zugrunde, ein Verfahren zur Vermeidung von Kollisionen gesteuert relativ zueinander bewegter Objekte, insbesondere Anlagenteile, wie Roboterarme und fahrerloser Transportsysteme, bereitzustellen, das Bremswege detailliert mit einbezieht und bei vergleichbarer Rechengenauigkeit gegenüber dem Stand der Technik weniger Rechenzeit benötigt.
Erfindungsgemäß wird diese Aufgabe gelöst durch ein Verfahren zur Vermeidung von Kollisionen gesteuert beweglicher Teile einer Anlage mit einer Anlagensteuerung, wobei das oder die gesteuert bewegliche(n) Teile(e) aus mindestens zwei durch einen gesteuert beweglichen Mechanismus verbundenen starren Körpern besteht/bestehen und an jedem Körper ein körperfestes 3D-Koordinatensystem definiert wird und jeder Körper in besagtem körperfesten 3D-Koordinatensystem als 3D-Volumen beschrieben und das 3D-Volumen als konvexe Hülle einer endlichen Menge von Punkten (p_i)ⁿ _i=1 zzgl. eines Pufferradius r>=0 nach der Definition $V (r; {(p_{i})}_{i = 1}^{n}) = {p_{r} + \sum_{i = 1}^{n} λ_{i} p_{i} | λ_{i} \geq 0 \forall i, \sum_{i = 1}^{n} λ_{i} = 1, | p_{r} | \leq r}$
dargestellt wird, umfassend die Durchführung der nachfolgenden Schritte in jedem Takt der Anlagensteuerung:

a) für jeden gesteuert beweglichen Mechanismus Bestimmen einer Grenze bzgl. seiner Position, innerhalb derer er bei einer sofortigen Bremsung zum Stillstand kommen würde,
b) für jeden Körper der Anlage Berechnen von Bremszonen als 3D-Volumina in der Darstellung nach (1) im körperfesten und im weltfesten 3D-Koordinatensystem und in allen Koordinatensystemen dazwischen in der Kette gesteuert beweglicher Mechanismen, die den jeweiligen Körper mit der weltfesten Umgebung verbinden, wobei die Bremszonen berechnet werden, indem, von der bekannten Darstellung nach (1) des Körpers startend, sukzessive der Effekt jedes gesteuert beweglichen Mechanismus entlang dieser Kette eingerechnet wird, indem für jeden ursprünglichen Punkt p_i seine Bewegung beim Bremsen des gesteuert beweglichen Mechanismus nach a) durch ein Volumen V(r_i ;(p_ij)_j=1 ⁿ), dargestellt mit Pufferradien r_i und Punktenp_ij , nach (1) überdeckt wird, die resultierenden Punkte zu einer konvexen Hülle zusammengeführt werden und der ursprüngliche Pufferradius um das Maximum der r_i erhöht wird,
c) Bestimmen über alle Paare von Körpern die Distanz zwischen ihren Bremszonen im ersten gemeinsamen Koordinatensystem der beiden in b) auftretenden Ketten von gesteuert beweglichen Mechanismen,
d) wenn mindestens eine der bestimmten Distanzen den Wert Null aufweist, Anhalten oder Verlangsamen zumindest eines Teilbereichs der Anlage.

Das Verfahren verhindert jegliche Kollision von Körpern der Anlage, sowohl verschiedener gesteuert beweglicher Teile miteinander, als auch eine Selbstkollision eines gesteuert beweglichen Teils, wie z. B. einem Roboterarm.
Gemäß einer besonderen Ausführungsform der Erfindung sind die Körper in dem jeweiligen 3D-Volumen, das nach (1) dargestellt ist, nur enthalten und werden sie nicht direkt dargestellt. Dadurch wird unverändert eine Kollision der Körper verhindert, da das Verfahren eine Kollision der 3D-Volumina, in denen sie enthalten sind, verhindert, aber das 3D-Volumen kann geometrisch einfacher gestaltet sein als der Körper selbst, wodurch in (1) weniger Punkte benötigt und Rechenzeit gespart wird.
Weiterhin kann vorgesehen sein, dass ein oder mehrere Körper in der Vereinigung von mehreren 3D-Voluminaen, die nach (1) dargestellt sind, enthalten sind und für die Berechnungen in Schritt b) alle diese drei 3D-Volumina verwendet werden. Dies erlaubt, nicht konvexe Körper darzustellen, indem sie in konvexe 3D-Volumina unterteilt werden.
Gemäß einer weiteren besonderen Ausführungsform der Erfindung kann vorgesehen sein, dass der bzw. mindestens ein gesteuert beweglicher Mechanismus ein Drehgelenk mit Antrieb und Winkelpositionssensor umfasst.
Günstigerweise wird für ein Drehgelenk folgende Formel in Schritt b) benutzt: $V (r + \frac{θ_{1} - θ_{0}}{2} max_{i} | p_{i} |; {(p_{i}^{\frac{1}{2}})}_{i = 1}^{n})$
mit $p_{i}^{λ} = Rot (a \cdot (1 - λ) θ_{0} + λ θ_{1}) p_{i}$
oder $V (r + sin ϕ max_{i} | p_{i} | : {(cos ϕ p_{i}^{\frac{1}{2}})}_{i = 1}^{n})$
mit $ϕ = min {\frac{θ_{1} - θ_{0}}{2}, \frac{π}{2}}$
oder $V (r + d max_{i} | p_{i} | : {(p_{i}^{0} \cdot p_{i}^{1})}_{i = 1}^{n})$
mit $d = 1 - cos ϕ$
oder $V (r + \frac{d}{2} max_{i} | p_{i} | \cdot {(p_{i}^{0} + \frac{d}{2} p_{i}^{\frac{1}{2}} \cdot p_{i}^{1} + \frac{d}{2} p_{i}^{\frac{1}{2}})}_{i = 1}^{n})$
oder $V (r, {(p_{i}^{0}, p_{i}^{1}, q_{i})}_{i = 1}^{n}),$
mit $q_{i} = \frac{1}{c o s ϕ} p_{i}^{\frac{1}{2}},$
wobei θ₀ die Unter- und θ₁ die Obergrenze aus Schritt a) für die Winkelposition des betrachteten Drehgelenkes ist und V(r, (p_i)ⁿ _i=1) die Darstellung der Bremszone vor Einrechnung des Effektes des Drehgelenkes ist.
Auch kann vorgesehen sein, dass der bzw. mindestens ein gesteuert beweglicher Mechanismus ein Lineargelenk mit Antrieb und Positionssenor umfasst.
Vorteilhafterweise wird für ein Lineargelenk folgende Formel in Schritt b) benutzt: $V (r + \frac{t_{1} - t_{0}}{2} | a |; {(p_{i} + \frac{t_{0} + t_{1}}{2} a)}_{i = 1}^{n})$
oder $V (r; {(p_{i} + t_{0} a, p_{i} + t_{1} a)}_{i = 1}^{n}),$
wobei t₀ die Unter- und t₁ die Obergrenze aus Schritt a) für die Position des betrachteten Lineargelenkes ist und V(r, (p_i)ⁿ _i=1) die Darstellung der Bremszone vor Einrechnung des Effektes des Lineargelenkes ist.
Alternativ oder zusätzlich kann der bzw. mindestens ein gesteuert beweglicher Mechanismus ein Fahrwerk eines Fahrzeugs mit angetriebenen und/oder gelenkten Rädern und Positionssensor umfassen. Selbstverständlich kann das Fahrzeug auch einen anderen Antrieb, wie z.B. einen Kettenantrieb, aufweisen.
Insbesondere kann dabei vorgesehen sein, dass für ein Fahrzeug die Grenze nach Schritt a) als Untermenge seines Konfigurationsraums definiert wird, wobei der Konfigurationsraum aus dreidimensionalen Vektoren besteht, deren ersten beiden Komponenten die x- und y- Position eines gewählten Referenzpunktes des Fahrzeuges angeben und dessen dritte Komponente θ den Winkel der Orientierung des Fahrzeugs angibt und wobei besagte Untermenge als konvexe Hülle von endlich vielen Konfigurationen (k_j)_j=1 ^m im Konfigurationsraum dargestellt wird: $K ({(k_{j})}_{j = 1}^{m}) = {\sum_{j = 1}^{m} λ_{j} k_{j} | λ_{j} \geq 0 \forall j, \sum_{j = 1}^{m} λ_{j} = 1} . k_{j} \in ℝ^{3},$
wobei die Komponenten der Konfigurationen k_j wie folgt bezeichnet werden: $k_{j} = (\begin{matrix} k_{j x} \\ k_{j y} \\ k_{j θ} \end{matrix})$
Vorteilhafterweise kann vorgesehen sein, dass für ein Fahrzeug folgende Formel für die Einberechnung des Effektes des Fahrzeugbremsens verwendet wird: $V (r + (1 - c o s ϕ) max_{i} | p_{i} | : {(T (k_{j}) \cdot p_{i})}_{i, j = 1}^{n, m})$
mit $ϕ = min (\frac{{max}_{j} k_{j θ} - {min}_{j} k_{j θ}}{2}, \frac{π}{2}),$
und $T (k) = (\begin{matrix} cos k_{θ} & - sin k_{θ} & 0 & k_{x} \\ sin k_{θ} & cos k_{θ} & 0 & k_{y} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}) . k = (\begin{matrix} k_{x} \\ k_{y} \\ k_{θ} \end{matrix}) \in ℝ^{3}$
wobei V(r, (p_i)ⁿ _i=1) die Darstellung der Bremszone vor Einrechnung des Effektes dieses Fahrzeugs ist.
Insbesondere kann dabei vorgesehen, dass für ein Fahrzeug die Grenze seiner Position nach Schritt a) als zurückgelegte Strecke s und vollzogener Drehwinkel α definiert wird, wobei $T (s, a) = (\begin{matrix} cos a & - sin a & 0 & s sin c \frac{a}{2} cos \frac{a}{2} \\ sin a & cos a & 0 & s sin c \frac{a}{2} sin \frac{a}{2} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix})$
die Position des Fahrzeugs nach dem Bremsen (120) relativ zur Position vor dem Bremsen (122) als homogene Matrix angibt.
Gemäß einer weiteren besonderen Ausführungsform kann vorgesehen sein, dass für ein Fahrzeug folgende Formel für die Einberechnung des Effektes des Fahrzeugbremsens verwendet wird: $V (r + {(p_{i}^{0}, p_{i}^{1}, q_{i})}_{i = 1}^{n})$
mit $p_{i}^{λ} = T (λ s, λ a) \cdot p_{i}$
und $q_{i} = p_{i}^{0} + Q (a) \cdot \frac{1}{2} (p_{i}^{1} - p_{i}^{0})$
und $Q (a) = (\begin{matrix} 1 & tan \frac{a}{2} & 0 & 0 \\ - tan \frac{a}{2} & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix})$
oder $V (r \cdot {(p_{i}^{0} \cdot p_{i}^{1} \cdot {(q_{i}^{\frac{k}{h}})}_{k = 0}^{h - 1})}_{i = 1}^{n})$
mit $q_{i}^{λ} = p_{i}^{λ} + Q (\frac{a}{h}) \cdot \frac{1}{2} (p_{i}^{λ + \frac{1}{h}} - p_{i}^{λ}),$
oder mit $q_{i}^{λ} = T (λ s, λ a) \cdot q_{i}^{0},$
wobei V(r, (p_i)ⁿ _i=1) die Darstellung der Bremszone vor Einrechnung des Effektes dieses Fahrzeugs ist, h eine natürliche Zahl ist und die Multiplikation von T(s, α) und Q(α) mit einem 3D-Vektor im Sinne einer homogenen Matrix durch Anfügen einer 1 interpretiert wird. Die Zahl h ist dabei ein frei wählbarer Qualitätsparameter. Je größer h ist, desto exakter ist die Näherung, aber auch desto mehr Punkte entstehen und desto größer ist die Rechenzeit.
Gemäß einer weiteren besonderen Ausführungsform der Erfindung weist die Anlage Körper auf, die durch verschiedene gesteuert bewegliche Mechanismen verbunden sind.
Vorteilhafterweise werden in Schritt a) Messunsicherheiten der Positionsgeber auf die Grenzen aufgeschlagen.
Alternativ oder zusätzlich werden vorteilhafterweise in Schritt a) Reaktionszeiten bei der Bestimmung der Grenzen aufgeschlagen.
Insbesondere kann dabei vorgesehen sein, dass der Aufschlag ein Produkt aus Reaktionszeit und Geschwindigkeit umfasst.
Zweckmäßigerweise wird zum Pufferradius die Hälfte eines globalen Sicherheitsabstands addiert.
Gemäß einer besonderen Ausführungsform der Erfindung werden in Schritt c) die Distanzen mittels des GJK (Gilbert-Johnson-Keerthi)-Algorithmus bestimmt. Es können aber auch andere Algorithmen verwendet werden.
In einer besonders bevorzugten Ausführungsform kann vorgesehen sein, dass es über mindestens zwei Takte der Anlagensteuerung durchgeführt wird und in Schritt c) für jede Bremszone i deren Änderung seit dem letzten Takt als pauschaler Änderungsradius δ_ri abgeschätzt und daraus eine Schranke für die Distanzen aller Paare von Bremszonen hergeleitet wird, indem von der Distanz der Paare von Bremszonen i und j aus dem letzten Takt die Änderungsradien δ_ri+δ_rj abgezogen werden, sowie eine feste Anzahl von Iterationen des GJK-Algorithmus durchgeführt wird, zuerst für die Paare von Bremszonen, bei denen die berechnete Distanz den Wert 0 aufweist, danach reihum, und wenn danach immer noch die Distanz eines Paares den Wert 0 aufweist, zumindest ein Teilbereich der Anlage angehalten oder verlangsamt wird. Auf diese Weise lässt sich die Rechenzeit besonders stark reduzieren und auf einen festen Wert beschränken. Letzteres ermöglicht erst bzw. erleichtert die Integration des Verfahrens in eine Anlagen- bzw. Robotersteuerung.
Insbesondere kann als pauschaler Änderungsradius einer Bremszone j die folgende Formel verwendet werden: $δ r_{j} = max_{i} | p_{i} - p_{i}^{'} | + r' - r,$
wobei V(r, (p_i)ⁿ _i=1) die Darstellung der Bremszone j im letzten Takt und V(r', (p_i')ⁿ _i=1) die Darstellung der Bremszone j im aktuellen Takt ist.
Weiterhin kann vorgesehen sein, dass die Positionsgeber Linear- oder Winkelgeber sind.
Vorteilhafterweise wird aus mindestens einer für einen Körper berechneten Bremszone ein Schutzfeld für einen an besagtem Körper angebrachten Laserscanner berechnet. Dadurch wird das Verfahren auf von einem Laserscanner sensoriell erfasste Hindernisse, insbesondere Personen, erweitert.
Günstigerweise wird zur Berechnung des Schutzfeldes das 3D-Volumen der Bremszone in eine vorzugsweise horizontale Ebene transformiert oder projiziert.
Günstigerweise kann auf die Grenzen in Schritt a) ein zusätzlicher Aufschlag erfolgen, um frühzeitiger und sanfter anzuhalten.
Zweckmäßigerweise können vorher festgelegte Paare von Körpern von der Bestimmung der Distanz in c) und der Überprüfung in d) ausgenommen werden, weil ihre Kollision zur planmäßigen Arbeit der Anlage gehört. Insbesonders gilt dies für Paare von Körpern, die durch einen gesteuert beweglichen Mechanismus verbunden sind, weil sie sich an der Stelle, wo dieser Mechanismus sitzt, automatisch berühren.
Der Erfindung liegt die überraschende Erkenntnis zugrunde, dass durch die spezielle Darstellung der Körper, nämlich als konvexe Hüllen mit Pufferradius, und durch die dadurch mögliche spezielle Berechnung der Bremszonen eine geringere Datenmenge und damit ein geringerer Rechenaufwand zur Berechnung von Kollisionen erforderlich sind.
Die Verfahren gemäß den Ansprüchen 1 bis 21 arbeiten ausschließlich auf einer vorkonfigurierten Geometrie, d.h. auf einer bekannten Geometrie der Anlage, also der beweglichen Teile, Werkstücke und Umgebung, und gemessenen Positionen und verwenden keine sensorielle Erfassung von Hindernissen. Die Verwendung der Bremszonen zur Bestimmung von Schutzfeldern gemäß besonderen Ausführungsformen in Ansprüchen 22 und 23 der Erfindung dienen zusätzlich der sensoriellen Absicherung von Personen vor Kollisionen mit Teilen einer Anlage, insbesondere mit Roboterarmen und FTS bzw. Fahrzeugen.
Weitere Merkmale und Vorteile der Erfindung ergeben sich aus den beigefügten Ansprüchen und der nachfolgenden Beschreibung, in der Ausführungsbeispiele anhand der schematischen Zeichnungen im einzelnen erläutert sind, in denen:

1 eine schematische Darstellung einer beispielhaften Anwendungssituation für das erfindungsgemäße Verfahren in einer Anlage (Industrieanlage) zeigt;
2 beispielhaft die Darstellung eines 3D-Volumens nach (1) zeigt;
3 zwei Näherungsmöglichkeiten für die Berechnung der Bremszone eines Drehgelenks zeigt;
4 zwei weitere Näherungsmöglichkeiten für die Berechnung der Bremszone eines Drehgelenks zeigt;
5 eine weitere Näherungsmöglichkeit für die Berechnung der Bremszone eines Drehgelenks zeigt;
6 bis 9 Schritte der Berechnung der Bremszonen von Teilen einer Anlage in verschiedenen Koordinatensystemen zeigen;
10 eine Bremszone in zwei aufeinanderfolgenden Takten einer Anlagensteuerung zeigt;
11 ein Fahrzeug beim Bremsen entlang einer Kreisbahn und die dabei relevanten Koordinatensysteme zeigt; und
12 zwei Näherungsmöglichkeiten zur Berechnung der Bremszone eines Fahrzeugs zeigt.

1 zeigt eine Anlage, die folgende Anlagenteile umfasst: Ein Fahrzeug 10, einen Roboterarm 12 auf dem Fahrzeug 10, einen stationären Roboterarm 14, ein Förderband 16 und eine Trennwand 18. Die Anlagenteile bestehen aus starren Körpern, von denen lediglich einige beispielhaft mit der Bezugszahl 19 gekennzeichnet sind, an denen jeweils ein 3D-Koordinatensystem festgelegt ist (s. Dreiergruppen von Pfeilen in der Figur). Die Körper sind jeweils durch gesteuert bewegliche Mechanismen, d.h. Mechanismen zur gesteuerten Bewegung, verbunden (in der Figur in den Ursprüngen der Koordinatensysteme). Die Drehgelenke der Roboterarme 12 und 14, das Fahrzeug 10 und das Förderband 16 stellen in diesem Beispiel die gesteuert beweglichen Mechanismen dar. Für den unbeweglichen Teil der Anlage ist ein weltfestes Koordinatensystem (Weltkoordinatensystem) 20 definiert.
Nachfolgend werden zuerst Verfahren zur Vermeidung von Kollisionen gesteuert beweglicher Teile einer Anlage ohne Berücksichtigung von Fahrzeugen und ohne Berücksichtigung von sensoriell erfassten Personen als Hindernisse beschrieben. Die Verfahren lassen sich grob wie folgt skizzieren:
Überblick
Vor Inbetriebnahme des Systems wird die Geometrie der Anlage einkonfiguriert. Sie wird als eine Menge starrer, konvexer Körper, die gegebenenfalls durch gesteuert bewegliche Mechanismen, hier bewegliche Gelenke, verbunden sind, dargestellt.
Die nachfolgenden Schritte a)-d) werden in jedem Takt einer Anlagensteuerung durchgeführt:

a) Für jeden gesteuert beweglichen Mechanismus wird aus dem Messwert des Positionsgebers und dessen Ableitung der Bremsweg, d.h. eine Grenze bzgl. seiner Position, innerhalb derer er bei einer sofortigen Bremsung zum Stillstand kommen würde, berechnet. Hat ein Gelenk nur einen Freiheitsgrad, ist die Grenze ein Intervall, beispielsweise für ein Drehgelenk ein Winkelintervall oder für ein Lineargelenk ein Streckenintervall. Unsicherheiten im Positionsgeber können hier aufgeschlagen werden.
b) Mit Hilfe der in Schritt a) bestimmten Grenzen werden für jeden Körper Bremszonen, also die beim Bremsen überstrichenen 3D-Volumina berechnet („swept volume“). Dazu wird jeder Körper schrittweise von seinem körperfesten Koordinatensystem ins weltfeste Koordinatensystem überführt entlang der Kette gesteuert beweglicher Mechanismen, die den jeweiligen Körper mit der weltfesten Umgebung verbinden. Bei jedem dieser Schritte wird der Effekt des jeweils betrachteten gesteuert beweglichen Mechanismus mit einberechnet. Verfahren, die dies für die verschiedenen Arten gesteuert beweglicher Mechanismen leisten, werden im folgenden beschrieben.
c) Für alle (relevanten) Paare von Körpern wird die Distanz zwischen ihren Bremszonen in einem gemeinsamen Koordinatensystem, zweckmässigerweise im ersten gemeinsamen, bestimmt. Dazu kann der GJK-Algorithmus verwendet werden. Dieser ist bspw. in E. Gilbert, D. Johnson, S. Keerthi, „A fast procedure for computing the distance between complex objects in 3D space", IEEE Journal on Robotics and automation, Band 4, Nr. 2, 1988, und C. Ericson, „The Gilbert-Johnson-Keerthi (GJK) algorithm", in Sicgraph Conference Plenary Talk, 2004, beschrieben, deren Inhalt durch Bezugnahme hierin voll umfänglich aufgenommen wird.
d) Ergibt sich für mindestens eine der Distanzen der Wert 0, wird die Anlage gestoppt oder verlangsamt.

Der Schlüsselbegriff im Verfahren ist der Begriff der Bremszone eines bestimmten Körpers in einem bestimmten Koordinatensystem. Dies ist abstrakt ein 3D-Volumen bestehend aus allen Punkten in besagtem Koordinatensystem, die sich bei zumindest einer Kombination von Positionen für die gesteuert beweglichen Mechanismen der Anlage in besagtem Körper befinden, wobei sich jede Position innerhalb der in a) bestimmten Grenzen bewegt. Anschaulich ist dies das Volumen in besagtem Koordinatensystem, das von besagtem Körper überdeckt wird („swept volume“), wenn die gesteuert beweglichen Mechanismen der Anlage sich innerhalb der in a) bestimmten Grenzen bewegen.
Diese Vorgehensweise wurde z. B. von A. Fuhrmann und E. Schömer, „A general method for computing the regional space of mechanisms“, Proceedings of the ASME Design Engineering Technical Conference 2001, beschrieben, deren Inhalt durch Bezugnahme hierin vollumfänglich aufgenommen wird. Das dort vorgeschlagene Verfahren ist allerdings nicht für den Einsatz in Echtzeit gedacht und dafür auch viel zu langsam. Bei dem erfindungsgemäßen Verfahren werden die Bremszonen genähert berechnet, so dass die konkret im Rechner repräsentierten Bremszonen ggf. größer sind als die oben definierte abstrakte Bremszone. Dies beeinträchtigt nicht die Sicherheit des Verfahrens, macht aber eine Berechnung in Echtzeit und damit einen Einsatz in einer Anlagensteuerung erst möglich.
In der Praxis sind meistens bestimmte Sicherheitsabstände vorgeschrieben und müssen Messunsicherheiten sowie Reaktionszeiten berücksichtigt werden, damit eine Kollision vermieden werden kann. Diese werden in das Verfahren wie folgt eingebracht: Die Hälfte des globalen Sicherheitsabstandes wird im Pufferradius (siehe unten) aller Körper der Szene addiert. Messunsicherheiten sowie Reaktionszeiten multipliziert mit der Geschwindigkeit werden auf die in Schritt a) bestimmten Grenzen aufgeschlagen.
Im Endeffekt heißt dies, dass das Verfahren eine Anlage noch rechtzeitig stoppen kann, wenn es in einem ersten Takt, in dem zwei Bremszonen kollidieren (mathematisch schneiden), eine Bremsung einleitet. Es geht also in dem Verfahren darum, zu bestimmen, ob zwei Bremszonen kollidieren.
Erfindungsgemäß werden alle vorkommenden 3D-Volumina (Körper, Bremszonen, Zwischenergebnisse der Bremszonenberechnungen) als konvexe Hülle einer endlichen Menge von Punkten zzgl. eines Pufferradius r gemäß Gl. (1) $\begin{array}{l} V (r; {(p_{i})}_{i = 1}^{n}) = {p_{r} + \sum_{i = 1}^{n} λ_{i} p_{i} | λ_{i} \geq 0 \forall i, \sum_{i = 1}^{n} λ_{i} = 1. | p_{r} | \leq r} \\ \begin{matrix} r > 0, & p_{i} \in ℝ^{3} \forall i \end{matrix} \end{array}$
dargestellt. D. h., dass im Computer ein 3D-Volumen V(r,(p_i)_i=1 ⁿ) als ein Array von 3D-Punkten mit den Punkten p_i und ein Pufferradius r repräsentiert wird.
In dem Spezialfall r=0, beschreibt V (0; (p_i)ⁿ _i=1) einen konvexen Polyeder mit p_i als potentiellen Ecken.
Im Unterschied zur üblichen Darstellung in der Computational Geometry wird auf eine explizite Verknüpfung der Ecken über Kanten und Flächen verzichtet. Es können sogar einzelne der p_i im Inneren des Polyeders liegen. Dadurch wird die algorithmische Behandlung viel einfacher und ist sie frei von Fehlerquellen durch numerische Sonderfälle, wie z.B. (fast) doppelte Ecken oder Kanten.
Durch den zusätzlichen Pufferradius können runde Geometrien mit wenig Punkten eng umschlossen werden. Beispielsweise beschreibt V (r; (p)) eine Kugel, V (r; (p₁ , p₂ )) einen Zylinder mit Kugelkappen und V (r; (p₁ , p₂ , p₃ )) eine abgerundete Dreiecksplatte. Der Pufferradius erlaubt außerdem, bei Berechnungen Näherungsfehler aufzuschlagen, so dass die präsentierten Verfahren konservative Näherungen berechnen, d.h., dass das berechnete Volumen garantiert das exakte Volumen enthält und aber ggf. etwas größer ist. Diese Darstellung ist die überraschende Erkenntnis, welche erlaubt, in Schritten a) - d) so schnell zu berechnen, dass das Verfahren in einer Anlagensteuerung in Echtzeit eingesetzt werden kann.
2 zeigt ein Beispiel für V (r; (p_i)ⁿ _i=1), das aus Darstellungsgründen zweidimensional gezeigt ist. Das Beispiel hat n=3 Punkte p₁ , p₂ , p₃ deren konvexe Hülle als 30 gezeigt ist. Die konvexe Hülle 30 wird in alle Richtungen um den Radius r vergrößert, was ein effektives Volumen 32 ergibt.
Im folgenden wird der Teil von Schritt c) beschrieben, bei dem der Effekt eines gesteuert beweglichen Mechanismus in die Bremszone mit einberechnet wird. Diese Rechnung geht jeweils aus von einer Darstellung nach (1) der Bremszone des jeweiligen Körpers in dem Koordinatensystem, das sich mit dem gesteuert beweglichen Mechanismus mitbewegt, als Eingabe. Sie berechnet eine Darstellung nach (1) der Bremszone dieses Körpers in dem Koordinatensystem, relativ zu dem sich der gesteuert bewegliche Mechanismus bewegt, als Ergebnis. Die Rechnung wird als Formel beschrieben, wobei für jede Art von gesteuert beweglicher Mechanismus eine andere Formel nötig ist. Es werden für jede Art von gesteuert beweglicher Mechanismus mehrere alternative Formeln beschrieben, die meist verschiedene Kompromisse zwischen Genauigkeit und Rechenzeit realisieren.
Formeln zur Einberechnung des Effektes verschiedener Arten gesteuert beweglicher Mechanismen
Im folgenden werden Gleichungen der Form $Operation (V (r, {(p_{i})}_{i = 1}^{n}) . Grenzen) = V (\dots)$
verwendet. Eine solche Gleichung sagt aus, wie im Algorithmus Operation (...) auf ein im Rechner repräsentiertes Volumen, meist eine Bremszone, angewendet wird. Dieses (Eingabe-)Volumen ist abstrakt V(r; (p_i)ⁿ _i=1), im Rechner konkret gegeben durch den Radius r und die Punkte p_i. Die konkrete Formel steckt in der Klammer von V (...) auf der rechten Seite der Gl. (2), die konkret angibt, was mit dem Radius r und den Punkten p_i geschieht, damit Ergebnisradius und Ergebnispunkte das Ergebnisvolumen darstellen. Operation (V, Grenzen) für ein 3D-Volumen V ist dabei abstrakt definiert als die Menge aller Punkte, die überstrichen werden, wenn ein 3D-Volumen V von einem bestimmten gesteuert beweglichen Mechanismus innerhalb der als Parameter gegebenen Grenzen bewegt.
Insgesamt sagt so eine Gleichung dann aus, dass die konkrete Rechnung auf den konkreten Daten die abstrakte Operation auf den, durch die konkreten Daten beschriebenem abstrakten 3D-Volumen ausführt.
In den meisten Fällen steht in der Gleichung kein „=“, sondern ein „⊂“. Dies bedeutet, dass die angegebene Rechnung nicht exakt sondern näherungsweise ist. Die Näherung erfolgt aber konservativ, so dass das konkrete Ergebnis (rechte Seite von Gl. 2) nur größer, nie kleiner als das abstrakte Ergebnis (linke Seite von Gl. 2) ist. Darin liegt ein wesentlicher Beitrag der Erfindung, da diese Eigenschaft garantiert, dass die Näherungen sicher sind.
Im nachfolgenden werden die verschiedenen Operationen für die unterschiedlichen gesteuert beweglichen Mechanismen und die dazugehörigen Formeln nach dem Muster von Gl. 2 beschrieben. Dort, wo mehrere Formeln angegeben werden, sind diese alternativ.
Koordinatentransformation
K1 Ein 3D-Volumen V(r; (p_i)_i=1 ⁿ) kann einfach in ein anderes Koordinatensystem mittels einer Koordinatentransformation transformiert werden, indem alle Punkte p_i transformiert werden. Für eine Koordinatentransformation R∈ℝ^3×3 und Translation t∈ ℝ³ gilt also $R \cdot V (r; {(p_{i})}_{i = 1}^{n} + t = V {(r; (R \cdot p_{i} + t)}_{i = 1}^{n}) .$
Dies setzt voraus, dass R Längen enthält (Starrkörpertransformation, orthonomal ist); andernfalls muss r mit dem größten Singulärwert von R multipliziert werden. K1 wird in Schritt c) verwendet, um ein 3D-Volumen in ein Koordinatensystem umzurechnen, bei dem der gesteuert bewegliche Mechanismus im Ursprung liegt, weil die folgenden Formeln dies voraussetzen.
Berechnung der Bremszone eines Lineargelenks
Gegeben ist ein Volumen V(r; (p_i)ⁿ _i=1), konkret eine Bremszone in Koordinaten, die sich mit einem Lineargelenk mitbewegen. Es soll das Volumen berechnet werden, das dieses überstreicht, wenn sich das Lineargelenk zwischen Positionsuntergrenze t_o und Positionsobergrenze t₁ entlang der Translationsrichtung des Lineargelenkes a bewegt. Das Ergebnis wird errechnet in dem Koordinatensystem, relativ zu dem sich das Lineargelenk bewegt. Das heißt, dass der Effekt des Bremsens dieses Lineargelenkes in das Volumen mit einberechnet werden soll. Das zu errechnende Volumen ist also abstrakt $Trans (V, a, t_{0}, t_{1}) = {v + λ a | v \in V, λ \in [t_{0}, \dots t_{1}]} .$
$V \subset ℝ^{3}, a \in ℝ^{3}, t_{0}, t_{1} \in ℝ$
Dabei ist t₀ die Unter- und t₁ die Obergrenze aus Schritt a) für die Position des betrachteten Lineargelenkes und V die Bremszone vor Einrechnung des Effektes des Lineargelenkes.
T0 Die erste Möglichkeit zur Näherung besteht darin, in Gleichung (4) λ=(t₀+t₁)/2 festzusetzen und den entstehenden Fehler |a| (t₁-t₀)/2 auf r aufzuschlagen: $\begin{matrix} Trans (V (r; {(p_{i})}_{i = 1}^{n}), a, t_{0}, t_{1}) \subset & (6) \\ V (r + \frac{t_{1} - t_{0}}{2} | a |; {(p_{i} + \frac{t_{0} + t_{1}}{2} a)}_{i = 1}^{n}) & (7) \end{matrix}$
T1 Die zweite Möglichkeit ist exakt, verdoppelt aber die Zahl der Punkte in der Darstellung. Sie erzeugt für jeden ursprünglichen Punkt p_i einen Punkt p_i+t₀a und einen Punkt p_i+t₁a. Alle weiteren Punkten p_i+λa, λ ∈ [t₀...t₁] sind dann durch die Definition der Darstellung als konvexe Hülle automatisch eingeschlossen. $\begin{matrix} Trans (V (r; {(p_{i})}_{i = 1}^{n}), a, t_{0}, t_{1}) = & (8) \\ V (r; {(p_{i} + t_{0} a, p_{i} + t_{1} a)}_{i = 1}^{n}) & (9) \end{matrix}$
Diese Technik wurde im Jahre 2004 in „The Gilbert-Johnson-Keerthi (GJK) Algorithm“ in SICGRAPH Conference Plimary Talk, von C. Ericson vorgestellt, dessen Inhalt hierin durch Bezugnahme vollumfänglich aufgenommen werden soll.
Berechnung der Bremszone eines Drehgelenks
Gegeben ist ein Volumen V(r; (p_i)ⁿ _i=1), konkret eine Bremszone, in Koordinaten, die sich mit einem Drehgelenk mitbewegen. Es soll das Volumen berechnet werden, das dieses überstreicht, wenn sich das Drehgelenk zwischen Winkelpositionsuntergrenze θ_o und Winkelpositionsobergrenze θ₁ um seine Drehachse a bewegt. Das Ergebnis wird berechnet in dem Koordinatensystem, relativ zu dem sich das Drehgelenk bewegt. Das heißt, dass der Effekt des Bremsens dieses Drehgelenkes in das Volumen mit einberechnet werden soll. Das zu berechnende Volumen ist also abstrakt. $Rot (V, a, θ_{0}, θ_{1}) = {Rot (a, λ) \cdot v | v \in V, λ \in [θ_{0}, \dots, θ_{1}]},$
$V \subset ℝ^{3}, a \in ℝ^{3}, θ_{0}, θ_{1} \in ℝ .$
dabei ist Rot (a, α) die Rotationsmatrix um den Nullpunkt, die Achse a und den Winkel α, entsprechend der Rodriguez-Formel gemäß „A mathematical introduction into robotic manipulation“, R. Murray, Z. Li and S. Sastry, CRC, 1.Auflage, 22. März 1994, 2006, deren Inhalt durch Bezugnahme hierin voll umfänglich aufgenommen wird. Weiterhin ist θ₀ die Unter- und θ₁ die Obergrenze aus Schritt a) für die Winkelposition des betrachteten Drehgelenkes und V die Bremszone vor Einrechnung des Effektes des Drehgelenkes.
Es sei im folgenden für ein durch V(r, (p_i)ⁿ _i=1)) dargestelltes Volumen $p_{i}^{λ} = Rot (a, (1 - λ) θ_{0} + λ θ_{1}) p_{i}$
die rotierten Punkte zwischen Anfang (λ=0) und Ende (λ=1) des Kreisbogens (40), den p_i durchläuft, wenn sich das Drehgelenk von Winkelposition θ₀ nach θ₁ ; bewegt.
R0a Die einfachste Näherung setzt analog zu T0 λ = (t₀+t₁)/2 und schlägt den daraus resultierenden Fehler von ≤ max_i|p_i| (θ₁-θ₀)/2 auf den Radius r auf (s. 3 links). Dies liefert $\begin{matrix} Rot (V (r; {(p_{i})}_{i = 1}^{n}), a, θ_{0}, θ_{1}) \subset & (13) \\ V (r + \frac{θ_{1} - θ_{0}}{2} max_{i} | p_{i} |; {(p_{i}^{\frac{1}{2}})}_{i = 1}^{n}) & (14) \end{matrix}$
Gemäß 3 links bewegt sich ein Punkt p_i kreisförmig zwischen den Winkeln θ₀ (p_i ⁰ ) und θ₁ (p_i ¹ ). Der dabei überstrichene Kreisbogen (40) wird durch eine Kreisfläche (42), dargestellt durch Formel (14), überdeckt. Der Mittelpunkt p_i ^1/2 liegt in der Mitte des Kreisbogens und der Radius entspricht der Länge des Bogensegmentes (ϕ|p_i |).
R0b Eine bessere Näherung erhält man nach dem Prinzip in 3 rechts durch die Formel $\begin{matrix} Rot (V (r; {(p_{i})}_{i = 1}^{n}), a, θ_{0}, θ_{1}) \subset & (15) \\ V (r + sin ϕ max_{i} | p_{i} | : {(cos ϕ p_{i}^{\frac{1}{2}})}_{i = 1}^{n}) & (16) \end{matrix}$
$mit ϕ = min {\frac{θ_{1} - θ_{0}}{2}, \frac{π}{2}} .$
Gemäß 3 rechts wird der Kreisbogen (40) durch einen Kreis gemäß Formel (16) überdeckt, dessen Mittelpunkt (46) bei cosϕ p_i ^1/2 , also in der Mitte beider Endpunkte (p_i ⁰ ), (p_i ¹ ) liegt. Der Radius ist sinϕ|p_i |.
R1a Eine noch bessere Näherung erhält man, indem für jeden ursprünglichen Punkt p_i Anfangspunkt p_i ⁰ und Endpunkt p_i ¹ des Kreisbogens (40) ins Ergebnis übernommen werden. Durch die Definition der Darstellung als konvexe Hülle ist damit automatisch die Strecke zwischen beiden Punkten enthalten. Es verbleibt als Fehler der Abstand zwischen dieser und dem exakten Kreisbogen (40). Dieser Abstand ist ≤ (1-cos ϕ)|p_i |. und wird auf den Radius aufgeschlagen. Es ergibt sich folgende Formel: $\begin{matrix} Rot (V (r; {(p_{i})}_{i = 1}^{n}), a, θ_{0}, θ_{1}) \subset & (18) \\ V (r + d max_{i} | p_{i} |; {(p_{i}^{0} \cdot p_{i}^{1})}_{i = 1}^{n}) . & (19) \end{matrix}$
$mit d = 1 - cos ϕ$
Der Preis für die genauere Näherung ist eine Verdoppelung der Punktzahl. 4 links zeigt, dass der Kreisbogen (40) durch die konvexe Hülle seiner Endpunkte (p_i ⁰ , p_i ¹ ) zuzüglich eines Radius von (1-cosϕ)|p_i | überdeckt wird.
R1b Eine noch bessere Näherung wird dadurch erreicht, dass die Strecke (56-58) zwischen den beiden eingefügten Punkten, wie in 4 rechts gezeigt, in die Mitte des Kreisbogens gelegt wird. Dann ergibt sich der geringste Radius. $\begin{matrix} Rot (V (r; {(p_{i})}_{i = 1}^{n}), a, θ_{0}, θ_{1}) \subset & (21) \\ V (r + \frac{d}{2} max_{i} | p_{i} |, {(p_{i}^{0} + \frac{d}{2} p_{i}^{\frac{1}{2}} \cdot p_{i}^{1} + \frac{d}{2} p_{i}^{\frac{1}{2}})}_{i = 1}^{n}) & (22) \end{matrix}$
Gemäß 4 rechts wird der Kreisbogen (40) überdeckt durch das Oval (54), nämlich die die konvexe Hülle seiner verschobenen Endpunkte p_i ⁰+dp_i ^1/2/2 (56) und p_i ¹+dp_i ^1/2/2 (58) gemäß Formel (22) zuzüglich eines Pufferradius von (1-cosϕ)|p_i |/2.
R1c Für denkbare Vereinfachungen des Verfahrens mag es von Vorteil sein, auf den Pufferradius zu verzichten. Es ist auch möglich, eine konservative Näherung zu berechnen, die den Pufferradius nicht erhöht, dafür aber aus jedem Punkt drei Punkte macht, um den Kreisbogen in einem Dreieck einzuschließen, wie dies in 5 gezeigt ist. Wird der Kreisbogen ohne Pufferradius überdeckt, werden drei Punkte benötigt. Diese konservative Näherung ist allerdings merklich weniger eng, selbst als die 2-Punkte-Näherung (siehe R1b). $\begin{matrix} Rot (V (r; {(p_{i})}_{i = 1}^{n}), a, θ_{0}, θ_{1}) \subset & (23) \\ V (r, {(p_{i}^{0}, p_{i}^{1}, q_{i})}_{i = 1}^{n}) & (24) \end{matrix}$
mit $q_{i} = \frac{1}{cos ϕ} p_{i}^{\frac{1}{2}}$
Gemäß 5 wird der Kreisbogen (40) überdeckt von der konvexen Hülle (60) von Anfangspunkt (p_i ⁰ ) und Endpunkt (p_i ¹ ) und dem Schnittpunkt q_i der Tangenten in Anfangs- und Endpunkt.
Rld Die in R1a bis R1c beschriebenen Näherungen lassen sich um den Preis einer erhöhten Punktzahl weiter verbessern, indem der Kreisbogen in Teile geteilt wird, jeder Teil des Kreisbogens getrennt genähert wird und die resultierenden Punkte zusammengefasst werden. Der Fehler an der Außenseite des Kreisbogens lässt sich so beliebig reduzieren. Der Fehler an der Innenseite ergibt sich aber allein schon durch die Beschränkung auf konvexe Volumina, wodurch unvermeidlich immer die konvexe Hülle des Kreisbogens überdeckt wird. Die Anzahl der eingefügten Punkte multipliziert sich mit jedem Gelenk entlang einer Kette gesteuert beweglicher Mechanismen. Daher ist diese Technik eher für den Fall eines Fahrzeugs (siehe unten) als für Roboterarme interessant.
Berechnung der Bremszonen aller Körper eine Anlage
Die vorangehende Beschreibung definiert Operationen, mit denen der Effekt eines gesteuert beweglichen Mechanismus, also eines Lineargelenkes oder eines Drehgelenkes, in eine Bremszone mit einberechnet wird. Im folgenden wird beschrieben, wie diese Operationen nacheinander auf 3-D Volumina angewandt werden, um für alle Körper einer Anlage die Bremszonen in allen relevanten Koordinatensystem zu berechnen. Dieses Verfahren, zusammen mit den vorangehenden Operationen, definiert Schritt b) gemäß einer besonderen Ausführungsform des erfindungsgemäßen Verfahrens.
In einer Anlage, auf die die Erfindung angewendet wird, ist jeder Körper durch eine Kette von gesteuert beweglichen Mechanismen mit der weltfesten Umgebung verbunden. Diese Kette korrespondiert zu einer Kette von Koordinatentransformationen, vom körperfesten Koordinatensystem ins weltfeste Koordinatensystem. Dabei hat jeder gesteuert bewegliche Mechanismus zwei Koordinatensysteme, die beide ihren Ursprung an dem Punkt haben, wo die Bewegung ansetzt (z. B. die Drehachse), und von denen sich eines mit dem gesteuert beweglichen Mechanismus mitbewegt und sich der gesteuert bewegliche Mechanismus relativ zu dem anderen bewegt. Dadurch besteht die Kette von Koordinatentransformationen abwechselnd aus einer festen Transformation zwischen zwei gesteuert beweglichen Mechanismen, die nur von der Geometrie der Anlage abhängt, und einer variablen Koordinatentransformation zwischen den beiden oben beschrieben Koordinatensystemen eines gesteuert beweglichen Mechanismus, die von der veränderlichen Position des gesteuert beweglichen Mechanismus selbst abhängt. Dies entspricht der üblichen Definition einer Vorwärtskinematik, wie bspw. in Kapitel 3 von „A mathematical introduction into robotic manipulation“, R. Murray, Z. Li und S. Sastry, CRC, 1. Auflage, 22. März 1994, 2006, beschrieben ist.
In Schritt b) gemäß einer besonderen Ausführungsform des erfindungsgemäßen Verfahrens werden für jeden Körper sukzessive Bremszonen in allen Koordinatensystemen in obiger Kette, vom körperfesten Koordinatensystem bis zum weltfesten Koordinatensystem berechnet. All diese Bremszonen sind abstrakt 3D-Volumina und werden konkret im Rechner als konvexe Hülle mit Pufferradius nach Gleichung (1) dargestellt. Das Verfahren startet mit der Bremszone des Körpers im körperfesten Koordinatensystem, welches das vorab einkonfigurierte 3D-Volumen des Körpers selbst ist. Danach werden sukzessive alle Koordinatentransformationen der oben beschriebenen Kette auf das 3D-Volumen angewandt. Dabei wird eine feste Koordinatentransformationen über K1 (3) angewandt, die variable Koordinatentransformation eines Lineargelenkes wahlweise über T0 (7) oder T1 (9) und die variable Koordinatentransformation eines Drehgelenkes wahlweise über R0a (14), R0b (16), R1a (19), R1b (22), R1c (24) oder R1d. Dabei wird R0b (14) oder R1b (22) bevorzugt. Die Wahl zwischen den Alternativen T0 oder T1 und R0b oder R1b ist ein Kompromiss zwischen Genauigkeit und Rechenzeit und könnte z. B. vorab vorkonfiguriert werden. Dabei ist besonders bei einem Roboterarm empfehlenswert, die vorderen Drehgelenke mit R0a und die hinteren mit R1b zu berechnen um einen guten Kompromiss zu erreichen.
6 bis 9 stellen ein Beispiel für das Vorgehen gemäß einer besonderen Ausführungsform des Verfahrens dar.
6 zeigt einen Roboter 80 in Draufsicht von oben. Er besteht aus einem Drehgelenk J₁ , gefolgt von einer Gabel (90, 92) und zwei parallelen weiteren Drehgelenken J_2a , J_2b . Die Umgebung ist fest im Weltkoordinatensystem definiert aus 4 Körpern: 3 Strecken, dargestellt als konvexe Hülle zweier Punkte ohne Pufferradius (82, 84, 86), und einem Oval, dargestellt als konvexe Hülle zweier Punkte mit Pufferradius (88). Die Gabel ist fest im Koordinatensystem J₁ , das sich mit dem 1. Gelenk dreht, und besteht aus zwei Körpern, dargestellt als Ovale (90, 92). Die Körper 94 und 96 sind fest in den Koordinatensystemen J_2a bzw. J_2b , die sich mit den Gelenken J₁ und J_2a bzw. J_2b mitdrehen, und werden dargestellt als Oval. Die Punkte, die zur Definition der Körper in dieser Darstellung als V(r, (p_i)ⁿ _i=1) dienen, sind jeweils als graue Kreuze markiert. In diesem Beispiel sei für die Rechnungen in 7-9 angenommen, dass sich alle Gelenke mit gewisser Geschwindigkeit nach links drehen.
7 zeigt die Bremszonen in den Koordinatensystemen J_2a und J_2b aus 6. In (a) von 7 ist die Bremszone von Körper 94 in J_2a der ursprüngliche Körper, da dieser in J_2a fest ist. Andere Körper haben keine Bremszonen in diesem Koordinatensystem, da Bremszonen eines Körpers vom körperfesten bis zum weltfesten Koordinatensystem berechnet werden. In (b) von 7 gilt analoges für Körper 96 in J_2b .
8 zeigt die Bremszonen im Koordinatensystem J₁ aus 6. Die beiden Körper (90), (92) sind fest in J₁ , so dass ihre Bremszonen die ursprünglichen Körper sind. In die Bremszonen der Körper 94 und 96 aus 7, welche die Körper selbst sind, wird die Bremsung der Gelenke J_2a bzw. J_2b einberechnet (nach R1b (24)). Dadurch entsteht aus jedem der beiden Punkte eine gedrehte und eine ungedrehte Kopie. Das Ergebnis (98, 100) ist die konvexe Hülle dieser vier Punkte, bei der zufällig zwei Punkte identisch sind, weil sie auf der Drehachse liegen. Die Entfernung zwischen Körper 94 und Körper 96 wird mit diesen Bremszonen 98 und 100 berechnet, so dass Gelenk 1 keinen Einfluss auf sie hat.
Schließlich zeigt 9 die Bremszonen im weltfesten Koordinatensystem für das Beispiel von 6. Die Umgebung (82, 84, 86, 88) ist fest in der Welt, so dass die ursprünglichen Körper gleichzeitig Bremszonen im weltfesten Koordinatensystem sind. In die Bremszonen (90, 92, 98, 100) in J₁ wird die Bremsung von Gelenk J₁ einberechnet (R1b (24)). Dadurch verdoppeln sich die Punkte in den vier betroffenen Bremszonen (102, 104, 106, 108). Die Distanz zwischen allen Roboterteilen und der Welt wird mit diesen Bremszonen berechnet.
Berechnung der Distanz zwischen den Bremszonen
In Schritt c) gemäß einer besonderen Ausführungsform des erfindungsgemäßen Verfahrens wird für jedes Paar von Körpern die Distanz ihrer den Bremszonen mit bspw. dem bekannten GJK-Algorithmus bestimmt. Der GJK-Algorithmus benötigt dafür eine von vornherein nicht bekannte Anzahl von Iterationen. Wie in dem Artikel von Gilbert et al. vorgeschlagen, wird der Algorithmus mit dem Ergebnis aus dem letzten Takt der Anlagensteuerung gestartet und die Iteration abgebrochen, sobald der Algorithmus eine untere Schranke für die Distanz >0 berechnet hat. Beides reduziert die Rechenzeit erheblich.
Maßgeblich für die Berechnung der Distanz sind Bremszonen in einem gemeinsamen Koordinatensystem, zweckmässigerweise im ersten gemeinsamen Koordinatensystem. In dem Beispiel in 8 wird beispielsweise die Distanz zwischen Körpern 94 und 96 mit den Bremszonen 98 und 100 im Koordinatensystem J₁ bestimmt, d.h. die Bewegung von J1 beeinflusst die Distanz nicht.
Nicht gegeneinander getestet werden außerdem Paare von Körpern, zwischen denen kein gesteuert beweglicher Mechanismus liegt, die sich also nicht relativ zueinander bewegen. Außerdem können Paare von Körpern als nicht zu testen vorkonfiguriert werden. Dies ist nötig, weil z. B. die beiden Seiten eines gesteuert beweglichen Mechanismus geometrisch immer kollidieren.
Beschränkung der Rechenzeit durch Fortschreiben von Distanzschranken
Das vorangehend beschriebene Verfahren gemäß einer besonderen Ausführungsform der vorliegenden Erfindung ist verhältnismäßig schnell. Es können aber trotzdem zwei Rechenzeitprobleme vorliegen. Zum einen werden alle Paare von Körpern überprüft bzw. getestet. Dadurch wächst die Rechenzeit quadratisch mit der Anzahl an Körpern.
Viele Verfahren im Stand der Technik verwenden eine Hierarchie so genannter Bounding-Volumina, um dieses Problem zu lösen. Wenn zwei Obervolumina eine gewisse Distanz aufweisen, so ist die Distanz für alle Paare von Untervolumina mindestens genauso groß. Haben also zwei Bounding-Volumina eine genügend hohe Distanz, braucht die Distanz aller Untervolumina nicht berechnet zu werden. Diese Vorgehensweise ist bei einer besonderen Ausführungsform des erfindungsgemäßen Verfahrens auch möglich, ist aber grundsätzlich eher kompliziert. Daher soll sie zumindest in einer besonderen Ausführungsform des erfindungsgemäßen Verfahrens vermieden werden, um das Verfahren besonders einfach zu halten, insbesondere für einen sicherheitsgerichteten Einsatz. Außerdem berechnen viele Algorithmen zur Distanzberechnung Informationen aus der Geometrie der beteiligten Volumina vor, um später Rechenzeit zu sparen. Dies ist hier nicht möglich, weil Bremszonen sich abhängig von der Geschwindigkeit ändern.
Ein zweites Rechenzeitproblem besteht darin, dass die Berechnung der Distanz mit dem GJK-Algorithmus eine von vornherein unbekannte Anzahl an Iterationen benötigt. In einer Anlagensteuerung (ebenso einer Robotersteuerung) laufen aber alle Prozesse in einem festen Takt. Dadurch müsste ein Vielfaches der mittleren Rechenzeit für das Verfahren reserviert werden, damit auch unter ungünstigen Bedingungen die Taktzeit eingehalten werden kann.
Um diesen beiden Rechenzeitproblemen zu begegnen, wird das vorangehend beschriebene Verfahren noch einmal verfeinert werden. Dadurch wird die Rechenzeit sehr viel kleiner und auf einen festen Wert beschränkt.
Dazu wird für jede Bremszone j ihre Änderung seit dem letzten Takt einer Anlagensteuerung als pauschaler Änderungsradius δr_j abgeschätzt. So ein δr_j muss die Eigenschaft haben, dass sich kein Punkt der Bremszone j seit dem letzten Takt um mehr als δr_j bewegt hat. Er wird berechnet als $δ r_{j} = max_{i} | p_{i} - p_{i}^{'} | + r' - r,$
wobei V(r',(p'_i)ⁿ _i=i) die Darstellung der Bremszone j aus dem aktuellen Takt und V(r,(p_i)ⁿ _i=1) die Darstellung der Bremszone j aus dem letzten Takt ist.
10 zeigt die Berechnung des Änderungsradius nach (26) aus einer Bremszone V(r,(p_i)ⁿ _i=1) des letzten Taktes (110) und der entsprechenden Bremszone V(r',(p'_i)ⁿ _i=1) des aktuellen Taktes (112).
Aus den Änderungradien wird eine Schranke für die Distanz aller Paare von Bremszonen hergeleitet, indem von der Distanz der Bremszonen i und j aus dem letzten Takt die Änderungsradien δr_i+δr_j abgezogen werden. Es ergibt sich eine untere Schranke für die aktuelle Distanz.
An Stelle von neu berechneten Distanzen werden also Distanzschranken von Takt zu Takt fortgeschrieben. Zusätzlich dazu wird eine feste Anzahl an Iterationen des GJK-Algorithmus durchgeführt. Zuerst für die Paare, bei denen obige Fortschreibung Distanzschranke 0 ergeben hat, danach reihum. Ist danach immer noch für ein Paar die Distanzschranke gleich 0, wird die Anlage gestoppt gemäß Schritt d).
Dieses Verfahren begründet sich aus zwei Überlegungen. Erstens, so lange eine Distanzschranke 0 ist, muss auf ihr eine GJK-Iteration durchgeführt werden, denn wenn sie 0 bleibt, kann eine Kollision nicht ausgeschlossen werden und es muss in Schritt d) gestoppt werden. Sind alle Distanzschranken >0, macht es trotzdem Sinn, GJK-Iterationen durchzuführen, weil dadurch die aktualisierten Distanzschranken größer werden könnten und dann in späteren Takten mehr GJK-Iterationen für andere Distanzschranken verwendet werden können. Der Algorithmus arbeitet sozusagen vor.
Das erfindungsgemäße Verfahren hat zumindest in besonderen Ausführungsformen wichtige Vorzüge:
Es ist adaptiv in Bezug auf Geschwindigkeit und Entfernungen und überwindet die Problematik quadratischer Rechenzeit. Die δr_j entsprechen grob der im Takt zurückgelegten Strecke. D. h., wenn sich die Anlage langsamer bewegt, stehen mehr Takte, also mehr GJK-Iterationen, zur Verfügung. Für viele Paare von Bremszonen sind die Distanzen eh groß, z. B. weil sich die Anlage gerade weit von der Umgebung entfernt, oder für solche Paare, die zu weit entfernten Körpern gehören. Dann muss diese Distanzschranke selten neu berechnet werden und es reicht fast immer eine einzelne GJK-Iteration aus. Außerdem bewegt sich natürlich kein Körper nahe an allen Teilen der Umgebung. Der Algorithmus ist demzufolge immer sicher, benötigt wenige Iterationen und leitet nur in absoluten Ausnahmefällen einen unnötigen Halt ein.
Als Beispiel sei ein Paar von Bremszonen betrachtet, das relativ weit, z. B. einen Meter voneinander entfernt ist und von dem sich eine Bremszone pro Takt einen Zentimeter hin- und herbewegt. Eine hohe Entfernung ist durchaus typisch für die meisten Paare von Bremszonen. Durch das Fortschreiben entsprechend dem Änderungsradius sinkt die Distanzschranke in jedem Takt um einen Zentimeter, so dass nach 100 Takten der Algorithmus eine Distanzschranke von 0 hat und mit einer GJK-Iteration nachrechnen muss, wie groß die Distanz wirklich ist (in diesem Beispiel würde eine GJK-Iteration reichen). Effektiv muss in diesem Beispiel nur jeden 100-sten Takt eine GJK-Iteration durchgeführt werden. 99 von 100 Takte kommen mit den zwei Subtraktionen der Änderungsradien aus.
Nachfolgend wird ein Verfahren gemäß einer besonderen Ausführungsform der vorliegenden Erfindung beschrieben, mit dem Kollisionen von gesteuert beweglichen bzw. bewegten Fahrzeugen, insbesondere fahrerlosen Transportsystemen (FTS) mit einem geringeren Rechenaufwand als bisher vermieden werden können. Anders als bei z. B. Roboterarmen sind bei einem derartigen Fahrzeug Position und/oder Orientierung in Bezug auf die Welt variabel, haben einen Bremsweg, aber sind nicht durch eine Abfolge gesteuert beweglicher Mechanismen mit einem einzelnen Freiheitsgrad, wie z. B. Dreh- oder Lineargelenke, beschreibbar. Vielmehr wird in einer Ausführungsform des erfindungsgemäßen Verfahrens ein Fahrzeug als ein einzelner gesteuert beweglicher Mechanismus mit drei Freiheitsgraden betrachtet. Das Fahrzeug muss dazu seine Position (x, y) und Orientierung (θ) in der Ebene und deren zeitliche Ableitung, also seine Geschwindigkeit messen. Der Vektor (x, y) beschreibt dabei die Position eines fest gewählten Referenzpunktes am Fahrzeug in der Welt und zusammen mit θ eine Transformation vom körperfestes Koordinatensystem am Fahrzeug ins weltfeste Koordinatensystem.
Definition der Grenzen der Position beim Bremsen eines Fahrzeugs
Da ein Fahrzeug einen gesteuert beweglichen Mechanismus mit drei Freiheitsgraden darstellt, ist die Definition von Positionsgrenzen, die beim Bremsen eingehalten werden, in Schritt a) komplizierter als das Bestimmen einer Unter- und Obergrenze. Gemäß einer besonderen Ausführungsform werden die Positionsgrenzen eines Fahrzeugs in Schritt a) statt dessen als konvexe Teilmenge seines Konfigurationsraums definiert, wobei der Konfigurationsraum aus 3D-Vektoren mit den Komponenten (x, y, θ) besteht und wobei besagte Untermenge als konvexe Hülle von endlich vielen Konfigurationen (k_j)^m _j=1 im Konfigurationsraum dargestellt wird: $K ({(k_{j})}_{j = 1}^{m}) = {\sum_{j = 1}^{m} λ_{j} k_{j} | λ_{j} \geq 0 \forall j, \sum_{j = 1}^{m} λ_{j} = 1}, k_{j} \in ℝ^{3}$
wobei die Komponenten der Konfigurationen k_j wie folgt bezeichnet werden: $k_{j} = (\begin{array}{l} k_{j x} \\ k_{j y} \\ k_{j θ} \end{array})$
Jede Konfiguration k definiert eine Koordinatentransformation vom körperfesten Koordinatensystem am Fahrzeug ins weltfeste Koordinatensystem. $\begin{matrix} T (k) = (\begin{matrix} cos k_{θ} & - sin k_{θ} & 0 & k_{x} \\ sin k_{θ} & cos k_{θ} & 0 & k_{y} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}), & k = (\begin{array}{l} k_{x} \\ k_{y} \\ k_{θ} \end{array}) \end{matrix} \in ℝ^{3}$
Dies ist eine homogene Transformationsmatrix, die Rotation und Translation kombiniert. Zur Vereinfachung der Notation sei daher, wie üblich vereinbart, dass 3D-Ortsvektoren, besonders die p_i bei Multiplikation mit einer homogenen 4×4 Matrix, mit einer impliziten 1 als vierte Komponente versehen werden.
Berechnung der Bremszone eines Fahrzeugs
Gesucht ist die Menge aller Punkte im Weltkoordinatensystem, die von einem am Fahrzeug körperfesten 3D-Volumen überstrichen werden, wenn das Fahrzeug alle in K((k_j)^m _j=1) definierten Konfigurationen durchläuft. Dies lässt sich abstrakt wie folgt darstellen: $Brake (V, K) = {T (k) \cdot v | v \in V, k \in K},$
wobei V besagtes 3D-Volumen und K besagte Menge von Konfigurationen ist.
Fla Die Vorgehensweise ist analog zu Näherung R1a bei Drehgelenken. Für ein nach (1) definiertes 3D-Volumen V(r; (p_i)ⁿ _i=1), konkret eine Bremszone und eine als konvexe Hülle definierte Menge von Konfigurationen K((k_j)^m _j=1), wird für jedes Paar i, j der Punkt p_i , vom körperfesten Koordinatensystem ins weltfeste Koordinatensystem gemäß k_j transformiert. Alle entstehenden Punkte werden in einer konvexen Hülle zusammengefasst. Wäre die Drehung linear in θ, wären alle Zwischenwerte durch die Definition als konvexe Hülle schon automatisch enthalten. So wird die Nichtlinearität der Drehung abgeschätzt und ihr Effekt auf r addiert. $\begin{matrix} Brake (V (r; {(p_{i})}_{i = 1}^{n}), K ({(k_{j})}_{j = 1}^{m})) \subset & (31) \\ V (r + (1 - cos ϕ) max_{i} | p_{i} |; {(T (k_{j}) \cdot p_{i})}_{i, j = 1}^{n, m}), & (32) \end{matrix}$
wobei $ϕ = min (\frac{{max}_{j} k_{j θ} - {min}_{j} k_{j θ}}{2}, \frac{π}{2})$
Vereinfachte Definition der Grenzen der Position beim Bremsen eines Fahrzeugs
Nimmt man an, dass das Fahrzeug 122 (siehe 11) beim Bremsen einer exakten Kreisbahn 126 folgt, mit dem sogenannten Drehzentrum 124 als Mittelpunkt, ergibt sich eine einfachere Lösung analog zu einer der Näherungen R0a-R1d für ein Drehgelenk.
11 zeigt die zugrunde liegende Situation. Die Konfiguration des Fahrzeugs am Ende des Bremsens 120 wird relativ zur Konfiguration am Anfang des Bremsens 122 unter der Annahme einer Kreisbahn 126 durch zwei Parameter (s, α) beschrieben. Der Parameter s beschreibt dabei die auf der Kreisbahn zurückgelegte Bremsweglänge (vorzeichenbehaftet) und α die Änderung der Orientierung (bezüglich θ). Fahrzeuge können kontinuierlich von einer Kurve in Geradeausfahrt übergehen. Dabei wandert das Drehzentrum ins Unendliche. Deshalb wird, anders als beim Drehgelenk, eine Version der Formeln verwendet, bei der Geradeausfahrt keinen Sonderfall darstellt und insbesondere der Nullpunkt des Koordinatensystems nicht im Drehzentrum liegen muss. Die erfindungsgemäße Art der Darstellung leistet dies. Eine Geradeausfahrt entsteht für α=0 und s≠0.
Durch Angabe der Konfiguration des Fahrzeugs beim Anfang des Bremsens 122 und der Parameter (s, α) werden dadurch die Grenzen der Position des Fahrzeugs im Schritt a) definiert. Als Ersatz für (28) definiert folgende Formel die Transformation vom körperfesten Koordinatensystem des Fahrzeugs am Ende des Bremsens zum körperfesten Koordinatensystem am Anfang des Bremsens. $T (s, a) = (\begin{matrix} cos a & - sin a & 0 & s sinc \frac{a}{2} cos \frac{a}{2} \\ sin a & cos a & 0 & s sinc \frac{a}{2} sin \frac{a}{2} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix})$
Gegeben ist ein 3D-Volumen V(r; (p_i)ⁿ _i=1), konkret eine Bremszone, in Koordinaten, die sich mit dem Fahrzeug mitbewegen. Es soll das 3D-Volumen berechnet werden, das dieses überstreicht, wenn sich das Fahrzeug zwischen Positionsuntergrenze (0, 0) und Positionsobergrenze (s, α) gemäß der Kreisbahnannahme bewegt. Das Ergebnis wird berechnet im körperfesten Koordinatensystem des Fahrzeuges am Anfang des Bremsens, eine Umrechnung ins weltfeste Koordinatensystem kann später über K1 erfolgen. Das heißt, dass der Effekt des Bremsens dieses Fahrzeugs in das Volumen mit einberechnet werden soll. Das zu berechnende Volumen ist also abstrakt $Rot (V . s . a) = {T (λ s, λ a) \cdot v | λ \in [0 \dots 1], v \in V}$
Nachfolgend werden zwei Näherungen von Rot(V(r; (p_i)ⁿ _i=1), s, α) beschrieben, die ohne einen Aufschlag auf den Pufferradius r auskommen.
Flc Gemäß 12 links wird eine R1c entsprechende Näherung angegeben, die für jeden Punkt p_i aus dem ursprünglichen 3D-Volumen den aus p_i entstehenden Kreisbogen 126 in ein Dreieck 130 einschließt. Dadurch werden aus jedem Punkt p_i im ursprünglichen 3D-Volumen drei Punkte. Das Dreieck besteht aus dem Anfangspunkt p_i ⁰ , dem Endpunkt p_i ¹ und zusätzlich dem Schnittpunkt q_i der Tangenten im Anfangs- und Endpunkt. Die Punkte werden wie folgt berechnet: $\begin{matrix} Rot (V (r; {(p_{i})}_{i = 1}^{n}) . s . a) \subset & (36) \\ V (r, {(p_{i}^{0}, p_{i}^{1}, q_{i})}_{i = 1}^{n}) & (37) \end{matrix}$
mit $p_{i}^{λ} = T (λ s, λ a) \cdot p_{i}$
und $q_{i} = p_{i}^{0} + Q (a) \cdot \frac{1}{2} (p_{i}^{1} - p_{i}^{0})$
und $Q (a) = (\begin{matrix} 1 & tan \frac{a}{2} & 0 & 0 \\ - tan \frac{a}{2} & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}) .$
F1d Gemäß 12 rechts wird der Kreisbogen 126 von F1c in h gleiche Teile eingeteilt, jeder Teil einzeln genähert. Dabei ist die Teilbogenlänge jeweils s/h und die Winkeländerung α/h. Der gesamte Kreisbogen 126 wird dann durch die konvexe Hülle aller dieser Punkte überdeckt. Dabei liegen Anfangs- und Endpunkte der Teilbögen p_i ^k/h für k=0..h bis auf den Anfangspunkt (k=0) des ersten Teilbogens und den Endpunkt (k=h) des letzten Teilbogens in der konvexen Hülle der anderen Punkte. Daher werden sie zweckmäßigerweise weggelassen. Wirklich berechnet werden also der Anfangspunkt p_i ⁰ und der Endpunkt p_i 1 des Kreisbogens 126 und die Tangentenschnittpunkte (q_i ^k/h für k=0..h-1) aller Teilbögen. Es entstehen damit h+2 anstatt drei Punkte zu jedem Punkt p_i des ursprünglichen 3D-Volumens. V(r; (p_i)ⁿ _i=1). $\begin{matrix} Rot (V (r; {(p_{i})}_{i = 1}^{n}), s, a) \subset & (41) \\ V (r, {(p_{i}^{0}, p_{i}^{1}, {(q_{i}^{\frac{k}{h}})}_{k = 0}^{h - 1})}_{i = 1}^{n}) & (42) \end{matrix}$
mit $q_{i}^{λ} = p_{i}^{λ} + Q (\frac{a}{h}) \cdot \frac{1}{2} (p_{i}^{λ + \frac{1}{h}} - p_{i}^{λ})$
Zum gleichen Ergebnis gelangt man, indem man alle q_i ^k/h für k=1..h-1 aus q_i ⁰ errechnet nach $q_{i}^{λ} = T (λ s, λ a) \cdot q_{i}^{0}$
Absicherung gegen sensoriell erfasste Hindernisse, insbesondere Personen
Bei den vorangehend beschriebenen Verfahren werden nur Umgebungen betrachtet, die geometrisch vorkonfiguriert sind und bei denen gesteuert bewegliche Teile einer Anlage mit Positions- bzw. Winkelgebern versehen sind, aber bei denen weitere Objekte oder Personen in der Anlage nicht sensoriell erfasst werden. Besagte Verfahren lassen sich auf von einem Laserscanner sensoriell erfasste Hindernisse, insbesondere Personen, erweitern. Dies geschieht, indem aus den Bremszonen Schutzfelder für einen Laserscanner berechnet werden, die dieser dann überwacht. Laserscanner tasten die Umgebung mit einem Laserstrahl ab und messen dadurch in der Ebene in jeder Richtung die Entfernung zum nächsten Hindernis. Ein Schutzfeld gibt für jeden Strahl des Laserscanners, also jeden Winkel in der Ebene an, bis zu welcher Entfernung vom Laserscanner die Umgebung frei von Hindernissen sein muss. Befindet sich in diesem Bereich ein Hindernis, vornehmlich eine Person, stoppt der Laserscanner die Anlage.
In der Praxis gibt es zwei besonders interessante Einsatzfälle: a) ein Roboterarm mit festem Laserscanner und b) ein Fahrzeug oder fahrerloses Transportsystem (FTS) mit einem am Fahrzeug befindlichen Laserscanner. Letztgenannter Fall weist eine weitere Besonderheit auf. Es müssen Position und Orientierung des Fahrzeugs nicht gemessen werden, da das Schutzfeld relativ zum Fahrzeug definiert ist. Trotzdem geht seine Geschwindigkeit in die Berechnung des Bremsweges ein. Diese Sondersituation ergibt sich daraus, dass der Laserscanner am Fahrzeug befestigt ist, aber Hindernisse wahrnimmt, die als fest in der Welt angenommen werden.
Zur Berechnung der Schutzfelder wird ein 3D-Volumen V(r; (p_i)ⁿ _i=1) beispielsweise einfach durch Weglassen der Z-Koordinate in die Ebene transformiert. Auf den resultierenden Punkten wird ein Konvexe-Hüllen-Algorithmus (z. B. Graham-Scan) angewandt, der die Punkte, die Ecken der konvexen Hülle bilden, entgegen den Uhrzeigersinn zu einem Polygon durchnummeriert. Das Polygon wird durch Kreise von r um die Ecken und Parallelen zu den Kanten um r erweitert. Alle in Frage kommenden Strahlen des Laserscanners werden mit allen Kanten und Kreisen geschnitten. Für jeden Strahl ist das Schutzfeld die größte Entfernung eines solchen Schnittes.
Zusammenfassend lässt sich somit folgendes festhalten:
Zumindest in besonderen Ausführungsformen der Erfindung wird die Bewegung von einem oder mehreren Roboterarm(en) überwacht und rechtzeitig ein Halt ausgelöst, bevor der Roboterarm selbstkollidiert oder die Roboterarme miteinander oder mit der Umgebung kollidieren. Dies vermeidet Unfälle in Situationen, wo der Roboter nicht immer exakt dieselbe Bahn abfährt, z. B. beim Einlernen von Bahnen, dem so genannten teach-in, oder bei sensorgeführten Arbeiten. Das Verfahren dient zur Kollisionsvermeidung mit geometrisch vorab bekannten Hindernissen, also in der Basisversion nicht zum Schutz von Personen.
Weiterhin kann das Verfahren auf gesteuert bewegliche Fahrzeuge oder FTS, auf denen sich auch Roboterarme befinden können, und auf sensoriell erfasste Hindernisse erweitert bzw. zugeschnitten werden. Dadurch dient das Verfahren auch zur Kollisionsvermeidung mit Personen.
Bezugszeichenliste

10: Fahrzeug
12: Roboterarm
14: stationärer Roboterarm
16: Förderband
18: Trennwand
19: starrer Körper
20: Weltkoordinatensystem
30: konvexe Hülle
32: effektive Hülle
p₁, p₂, p₃: Punkte
r: Radius

Claims

Verfahren zur Vermeidung von Kollisionen gesteuert beweglicher Teile (10; 12; 14; 16) einer Anlage mit einer Anlagensteuerung, wobei das oder die gesteuert bewegliche(n) Teile(e) aus mindestens zwei durch einen gesteuert beweglichen Mechanismus verbundenen starren Körpern (19) besteht/bestehen und an jedem Körper ein körperfestes 3D-Koordinatensystem definiert wird und jeder Körper in besagtem körperfesten 3D-Koordinatensystem als 3D-Volumen beschrieben und das 3D-Volumen als konvexe Hülle einer endlichen Menge von Punkten (p₁)ⁿ _i=1 zzgl. eines Pufferradius r>=0 nach der Definition $V (r; {(p_{l})}_{i = 1}^{n}) = {p_{r} + \sum_{i = 1}^{n} λ_{i} p_{l} | λ_{l} \geq 0 \forall i, \sum_{j = 1}^{m} λ_{l} = 1, | p_{r} | \leq r}$
dargestellt wird, umfassend die Durchführung der nachfolgenden Schritte in jedem Takt der Anlagensteuerung: a) für jeden gesteuert beweglichen Mechanismus Bestimmen einer Grenze bzgl. seiner Position, innerhalb derer er bei einer sofortigen Bremsung zum Stillstand kommen würde, b) für jeden Körper der Anlage Berechnen von Bremszonen als 3D-Volumina in der Darstellung nach (1) im körperfesten und im weltfesten 3D-Koordinatensystem und in allen Koordinatensystemen dazwischen in der Kette gesteuert beweglicher Mechanismen, die den jeweiligen Körper mit der weltfesten Umgebung verbinden, wobei die Bremszonen berechnet werden, indem, von der bekannten Darstellung nach (1) des Körpers startend, sukzessive der Effekt jedes gesteuert beweglichen Mechanismus entlang dieser Kette eingerechnet wird, indem für jeden ursprünglichen Punkt p_i seine Bewegung beim Bremsen des gesteuert beweglichen Mechanismus nach a) durch ein Volumen V(r_i;(p_ij)ⁿ _j=1), dargestellt mit Pufferradien r_i und Punkten p_ij, nach (1) überdeckt wird, die resultierenden Punkte zu einer konvexen Hülle zusammengeführt werden und der ursprüngliche Pufferradius um das Maximum der r_i erhöht wird, c) Bestimmen über alle Paare von Körpern die Distanz zwischen ihren Bremszonen im ersten gemeinsamen Koordinatensystem der beiden in b) auftretenden Ketten von gesteuert beweglichen Mechanismen, d) wenn mindestens eine der bestimmten Distanzen den Wert Null aufweist, Anhalten oder Verlangsamen zumindest eines Teilbereichs der Anlage.
Verfahren nach Anspruch 1, dadurch gekennzeichnet, dass die Körper in dem jeweiligen 3D-Volumen, das nach (1) dargestellt ist, nur enthalten sind und nicht direkt dargestellt werden.
Verfahren nach Anspruch 1 oder 2, dadurch gekennzeichnet, dass ein oder mehrere Körper in der Vereinigung von mehreren 3D-Volumen, die nach (1) dargestellt sind, enthalten sind und für die Berechnungen in Schritt b) alle diese 3D-Volumen verwendet werden.
Verfahren nach einem der Ansprüche 1 bis 3, dadurch gekennzeichnet, dass der bzw. mindestens ein gesteuert beweglicher Mechanismus ein Drehgelenk mit Antrieb und Winkelpositionssensor umfasst.
Verfahren nach Anspruch 4, dadurch gekennzeichnet, dass für ein Drehgelenk folgende Formel in Schritt b) benutzt wird: $V (r + \frac{θ_{1} - θ_{0}}{2} max_{i} | p_{i} |; {(p_{i}^{\frac{1}{2}})}_{i = 1}^{n}),$
mit $p_{i}^{λ} = Rot (a, (1 - λ) θ_{0} \div λ θ_{1}) p_{i}$
oder $V (r \div sin a max_{i} | p_{i} | {(cos ϕ p_{i}^{\frac{1}{2}})}_{i = 1}^{n})$
mit $ϕ = min {\frac{θ_{1} - θ_{0}}{2}, \frac{π}{2}}$
oder $V (r + d max_{i} | p_{i} |; {(p_{i}^{0}, p_{i}^{1})}_{i = 1}^{n})$
mit $d = 1 - cos ϕ$
oder $V (r + \frac{d}{2} \underset{i}{m a x} | p_{i} |, {(p_{i}^{0} + \frac{d}{2} p_{i}^{\frac{1}{2}}, p_{i}^{1} + \frac{d}{2} p_{i}^{\frac{1}{2}})}_{i = 1}^{n})$
oder $V (r, {(p_{i}^{0}, p_{i}^{1}, q_{i})}_{i = 1}^{n})$
mit $q_{i} = \frac{1}{cos ϕ} p_{i}^{\frac{1}{2}}$
durchgeführt wird, wobei θ₀ die Unter- und θ₁ die Obergrenze aus Schritt a) für die Winkelposition des betrachteten Drehgelenkes ist und V(r, (p_i)ⁿ _i=1) die Darstellung der Bremszone vor Einrechnung des Effektes des Drehgelenkes ist.
Verfahren nach einem der Ansprüche 1 bis 3, dadurch gekennzeichnet, dass der bzw. mindestens ein gesteuert beweglicher Mechanismus ein Lineargelenk mit Antrieb und Positionssensor umfasst.
Verfahren nach Anspruch 6, dadurch gekennzeichnet, dass für ein Lineargelenk folgende Formel in Schritt b) benutzt wird: $V (r + \frac{t_{1} - t_{0}}{2} | a | : {(p_{i} + \frac{t_{0} + t_{1}}{2} a)}_{i = 1}^{n})$
oder $V (r, {(p_{i} + t_{0} a . p_{i} + t_{1} a)}_{i = 1}^{n}),$
wobei t₀ die Unter- und t₁ die Obergrenze aus Schritt a) für die Position des betrachteten Lineargelenkes ist und V(r, (p)^n
i=1) die Darstellung der Bremszone vor Einrechnung des Effektes des Lineargelenkes ist.
Verfahren nach einem der Ansprüche 1 bis 3, dadurch gekennzeichnet, dass der bzw. mindestens ein gesteuert beweglicher Mechanismus ein Fahrwerk eines Fahrzeuges mit angetriebenen und/oder gelenkten Rädern und Positionssensor umfasst.
Verfahren nach Anspruch 8, dadurch gekennzeichnet, dass für ein Fahrzeug die Grenze nach Schritt a) als Untermenge seines Konfigurationsraums definiert wird, wobei der Konfigurationsraum aus dreidimensionalen Vektoren besteht, deren ersten beiden Komponenten die x- und y- Position eines gewählten Referenzpunktes des Fahrzeuges angeben und dessen dritte Komponente 0 den Winkel der Orientierung des Fahrzeugs angibt und wobei besagte Untermenge als konvexe Hülle von endlich vielen Konfigurationen (k_j)^m _j=1 im Konfigurationsraum dargestellt wird: $K ({(k_{j})}_{j = 1}^{m}) = {\sum_{j = 1}^{m} λ_{j} k_{j} | λ_{j} \geq 0 \forall j, \sum_{j = 1}^{m} λ_{j} = 1}, k_{j} \in ℝ^{3},$
wobei die Komponenten der Konfigurationen k_j wie folgt bezeichnet werden: $k_{j} = (\begin{array}{l} k_{j x} \\ k_{j y} \\ k_{j θ} \end{array})$
Verfahren nach Anspruch 9, dadurch gekennzeichnet, dass für ein Fahrzeug folgende Formel für die Einberechnung des Effektes des Fahrzeugbremsens verwendet wird: $V (r + (1 - cos ϕ) \underset{i}{m a x} | p_{i} | : {(T (k_{j}) \cdot p_{i})}_{i, j = 1}^{n, m})$
mit $ϕ = mm (\frac{{max}_{j} k_{j} θ - {min}_{j} k_{j θ}}{2}, \frac{π}{2}),$
und $\begin{matrix} T (k) = (\begin{matrix} cos k_{θ} & - sin k_{θ} & 0 & k_{x} \\ sin k_{θ} & cos k_{θ} & 0 & k_{y} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}), & k = (\begin{array}{l} k_{x} \\ k_{y} \\ k_{θ} \end{array}) \end{matrix} \in ℝ^{3}$
wobei V(r, (p)^n
i=1) die Darstellung der Bremszone vor Einrechnung des Effektes dieses Fahrzeugs ist.
Verfahren nach Anspruch 8, dadurch gekennzeichnet, dass für ein Fahrzeug (122) das Bremsen auf einer Kreisbahn angenommen wird und die Grenze seiner Position in a) auf der Kreisbahn als zurückgelegte Strecke s und vollzogener Drehwinkel α berechnet wird, wobei $T (s, a) = (\begin{matrix} cos a & - sin a & 0 & s sinc \frac{a}{2} cos \frac{a}{2} \\ sin a & cos a & 0 & s sinc \frac{a}{2} sin \frac{a}{2} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix})$
die Position des Fahrzeugs nach dem Bremsen (120) relativ zur Position vor dem Bremsen (122) als homogene Matrix angibt
Verfahren nach Anspruch 11, dadurch gekennzeichnet, dass für ein Fahrzeug folgende Formel für die Einberechnung des Effektes des Fahrzeugbremsens verwendet wird: $V (r, {(p_{i}^{0}, p_{i}^{1}, q_{i})}_{i = 1}^{n})$
mit $p_{i}^{λ} = T (λ s, λ a) \cdot p_{i}$
und $q_{i} = p_{i}^{0} + Q (a) \cdot \frac{1}{2} (p_{i}^{1} - p_{i}^{0})$
und $Q (a) = (\begin{matrix} 1 & tan \frac{a}{2} & 0 & 0 \\ - tan \frac{a}{2} & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix})$
oder $V (r, {(p_{i}^{0}, p_{i}^{1}, {(q_{i}^{\frac{1}{h}})}_{k = 0}^{h - 1})}_{i = 1}^{n})$
mit $q_{i}^{λ} = p_{i}^{λ} + Q (\frac{α}{h}) \cdot \frac{1}{2} (p_{i}^{λ + \frac{1}{h}} - p_{i}^{λ})$
oder mit $q_{i}^{λ} = T (λ s, λ a) \cdot q_{i}^{0},$
wobei V(r, (p_i)ⁿ _i=1) die Darstellung der Bremszone vor Einrechnung des Effektes dieses Fahrzeugs ist, h eine natürliche Zahl ist und die Multiplikation von T(s, α) und Q(α) mit einem 3D-Vektor im Sinne einer homogenen Matrix durch Anfügen einer 1 interpretiert wird.
Verfahren nach einem der vorangehenden Ansprüche, dadurch gekennzeichnet, dass die Anlage Körper aufweist, die durch verschiedene gesteuert bewegliche Mechanismen verbunden sind.
Verfahren nach einem der Ansprüche 4 bis 13, dadurch gekennzeichnet, dass in Schritt a) Messunsicherheiten der Positionsgeber auf die Grenzen aufgeschlagen werden.
Verfahren nach einem der Ansprüche 4 bis 14, dadurch gekennzeichnet, dass in Schritt a) Reaktionszeiten bei der Bestimmung der Grenzen aufgeschlagen werden.
Verfahren nach Anspruch 15, dadurch gekennzeichnet, dass der Aufschlag ein Produkt aus Reaktionszeit und Geschwindigkeit umfasst
Verfahren nach einem der vorangehenden Ansprüche, dadurch gekennzeichnet, dass zum Pufferradius die Hälfte eines globalen Sicherheitsabstands addiert wird.
Verfahren nach einem der vorangehenden Ansprüche, dadurch gekennzeichnet, dass in Schritt c) die Distanzen mittels des GJK (Gilbert-Johnson-Keerthi)-Algorithmus bestimmt werden.
Verfahren nach Anspruch 18, dadurch gekennzeichnet, dass es über mindestens zwei Takte der Anlagensteuerung durchgeführt wird und in Schritt c) für jede Bremszone i deren Änderung seit dem letzten Takt als pauschaler Änderungsradius δ_ri abgeschätzt und daraus eine Schranke für die Distanzen aller Paare von Bremszonen hergeleitet wird, indem von der Distanz der Paare von Bremszonen i und j aus dem letzten Takt die Änderungsradien δ_ri+ δ_rj abgezogen werden, sowie eine feste Anzahl von Iterationen des GJK-Algorithmus durchgeführt wird, zuerst für die Paare von Bremszonen, bei denen die berechnete Distanz den Wert 0 aufweist, danach reihum, und wenn danach immer noch die Distanz eines Paares den Wert 0 aufweist, zumindest ein Teilbereich der Anlage angehalten oder verlangsamt wird.
Verfahren nach Anspruch 19, dadurch gekennzeichnet, dass als pauschaler Änderungsradius $δ r_{j} = max_{i} | p_{i} - p_{i}^{'} | + r' - r,$
verwendet wird, wobei V(r, (p_i)ⁿ _i=1) die Darstellung der Bremszone j im letzten Takt und V(r', (p_i')ⁿ _i=1) die Darstellung der Bremszone j im aktuellen Takt ist.
Verfahren nach einem der Ansprüche 4 bis 20, dadurch gekennzeichnet, dass die Positionsgeber Linear- oder Winkelgeber sind.
Verfahren nach einem der vorangehenden Ansprüche, dadurch gekennzeichnet, dass aus mindestens einer für einen Körper berechneten Bremszone ein Schutzfeld für einen an besagtem oder einem anderen Körper angebrachten Laserscanner berechnet wird.
Verfahren nach Anspruch 21, dadurch gekennzeichnet, dass zur Berechnung des Schutzfeldes das 3D-Volumen der Bremszone in eine Ebene transformiert oder projiziert wird.
Verfahren nach einem der vorangehenden Ansprüche, dadurch gekennzeichnet, dass in Schritt a) ein zusätzlicher Aufschlag auf die Grenzen erfolgt.
Verfahren nach einem der vorangehenden Ansprüche, dadurch gekennzeichnet, dass vorher festgelegte Paare von Körpern von der Bestimmung der Distanz in c) und der Überprüfung in d) ausgenommen sind.