WO2010085944A1 - Verfahren zur vermeidung von kollisionen gesteuert beweglicher teile einer anlage - Google Patents

Verfahren zur vermeidung von kollisionen gesteuert beweglicher teile einer anlage Download PDF

Info

Publication number
WO2010085944A1
WO2010085944A1 PCT/DE2010/000090 DE2010000090W WO2010085944A1 WO 2010085944 A1 WO2010085944 A1 WO 2010085944A1 DE 2010000090 W DE2010000090 W DE 2010000090W WO 2010085944 A1 WO2010085944 A1 WO 2010085944A1
Authority
WO
WIPO (PCT)
Prior art keywords
braking
vehicle
distance
controlled
volume
Prior art date
Application number
PCT/DE2010/000090
Other languages
English (en)
French (fr)
Inventor
Udo Frese
Holger Täubig
Original Assignee
Deutsches Forschungszentrum für künstliche Intelligenz GmbH
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Deutsches Forschungszentrum für künstliche Intelligenz GmbH filed Critical Deutsches Forschungszentrum für künstliche Intelligenz GmbH
Publication of WO2010085944A1 publication Critical patent/WO2010085944A1/de

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39082—Collision, real time collision avoidance
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39097—Estimate own stop, brake time, then verify if in safe distance
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/49—Nc machine tool, till multiple
    • G05B2219/49141—Detect near collision and slow, stop, inhibit movement tool
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Definitions

  • the present invention relates to a method for avoiding collisions of controlled moving parts of a plant (parts of installations), in particular of robot arms and vehicles, especially of automated guided vehicle systems (FTS), and quite generally a method for avoiding collisions controlled relative to one another moving objects.
  • the method is used to monitor controlled moving parts of a plant and to trigger a stop in time before they collide with each other or with other (immovable) parts or the immobile environment.
  • Controlled moving parts in this sense are in particular, but not exclusively: a) robotic arms, axis tables, gantries or in general any cycle-free series connection of rotary or linear joints and also b) vehicles and vehicles on which such series connections of joints are mounted.
  • the state of the art also includes the testing of the current position of a robot at a distance to obstacles, so that it can be braked at a fixed safety distance to the obstacle.
  • this is only practicable at low speeds. Different parts of the robot move very different speeds and need very different braking distances.
  • the braking distance has one direction, namely the direction of movement, while a safety distance works the same in all directions and at all points of a robot. As a result, the safety margin becomes so conservative at higher speeds that the system often falters, although this would be unnecessary.
  • the present invention is therefore based on the object, a method for avoiding collisions controlled relative to each other moving objects, in particular equipment parts, such as robot arms and driverless transport systems to provide, the braking distances includes in detail and requires comparable computing accuracy compared to the prior art less computing time.
  • the method prevents any collision of bodies of the system, both different controlled moving parts together, as well as a self-collision of a controlled moving part, such.
  • the bodies in the respective SD volume shown in (1) are only included and not directly represented. This still prevents collision of the bodies, since the method prevents collision of the 3D volumes in which they are contained, but the 3D volume may be geometrically simpler than the body itself, thus requiring fewer points in (1) and computing time is saved.
  • the or at least one controlled movable mechanism comprises a rotary joint with drive and angular position sensor.
  • step b) for a swivel joint:
  • the or at least one controlled movable mechanism comprises a linear joint with drive and position sensor.
  • step b) for a linear joint:
  • the or at least one controlled movable mechanism may include a chassis of a vehicle with driven and / or steered wheels and position sensor.
  • the vehicle may also have another drive, such as a chain drive, have.
  • the limit of its position after step a) is defined as distance traveled s and completed angle of rotation ⁇ , wherein
  • h is a natural number and the multiplication of T (s, ⁇ ) and Q ( ⁇ ) is interpreted with a 3 D vector in the sense of a homogeneous matrix by adding a 1.
  • the number h is a freely selectable quality parameter. The larger h is, the more accurate the approximation, but also the more points that arise and the greater the computing time.
  • the system comprises bodies which are connected by various controlled movable mechanisms.
  • step a) measurement uncertainties of the position sensor are opened to the limits.
  • reaction times in determining the limits are advantageously opened in step a).
  • the impact comprises a product of reaction time and speed.
  • step c) the distances are determined by means of the GJK (Gilbert-Johnson-Keerthi) algorithm.
  • GJK Global-Johnson-Keerthi
  • other algorithms can be used.
  • the computing time can be particularly reduced and limited to a fixed value. The latter enables or facilitates the integration of the method into a system or robot controller.
  • the position encoders are linear or angle encoders.
  • a protective field for a laser scanner attached to said body is calculated from at least one braking zone calculated for a body.
  • the method is extended to obstacles detected by a laser scanner, in particular persons.
  • the 3D volume of the braking zone is transformed or projected into a preferably horizontal plane.
  • step a) may be supplemented to stop earlier and more smoothly.
  • pre-determined pairs of bodies may be excluded from the determination of the distance in c) and the check in d) because their collision is part of the plant's scheduled work.
  • the invention is based on the surprising finding that due to the special representation of the bodies, namely as convex hulls with buffer radius, and due to the possible special calculation of the braking zones, a smaller amount of data and hence less computational effort are required to calculate collisions.
  • the methods according to claims 1 to 21 operate exclusively on a preconfigured geometry, i. on a known geometry of the plant, ie the moving parts, workpieces and environment, and measured positions and do not use sensorial detection of obstacles.
  • the use of the braking zones for the determination of protective fields according to particular embodiments in claims 22 and 23 of the invention additionally serve to protect persons from collisions with parts of a system, in particular with robot arms and AGVs or vehicles.
  • FIG. 1 shows a schematic representation of an exemplary application situation for the method according to the invention in a plant (industrial plant);
  • Fig. 2 shows by way of example the representation of a 3 D volume according to (1);
  • Fig. 3 shows two approximations for the calculation of the braking zone of a rotary joint
  • Fig. 4 shows two further approximations for the calculation of the braking zone of a rotary joint
  • Fig. 5 shows a further approximation possibility for the calculation of the braking zone of a rotary joint
  • Figures 6 to 9 show steps of calculating the braking zones of parts of a plant in different coordinate systems
  • Fig. 10 shows a braking zone in two consecutive cycles of a plant control
  • Fig. 11 shows a vehicle braking along a circular path and the coordinate systems involved
  • Fig. 12 shows two approximate options for calculating the braking zone of a vehicle.
  • Fig. 1 shows a system comprising the following parts of the plant: a vehicle 10, a robot arm 12 on the vehicle 10, a stationary robot arm 14, a conveyor belt 16 and a partition wall 18.
  • the plant components are made of rigid bodies, of which only a few by way of example the reference numeral 19 are marked, on each of which a 3 D coordinate system is defined (see triplets of arrows in the figure).
  • the bodies are each controlled by controlled mechanisms, i. Mechanisms for controlled movement, connected (in the figure in the origins of the coordinate systems).
  • the pivots of the robot arms 12 and 14, the vehicle 10 and the conveyor 16 in this example represent the controlled moving mechanisms.
  • a world coordinate system (world coordinate system) 20 is defined for the fixed part of the plant.
  • the geometry of the system is configured. It is represented as a set of rigid, convex bodies, possibly connected by controlled movable mechanisms, here movable joints.
  • the braking distance i.
  • a limit is calculated with respect to its position within which it would come to a standstill in an immediate braking. If a joint has only one degree of freedom, the limit is an interval, for example, an angular interval for a rotary joint or a distance interval for a linear joint. Uncertainties in the position encoder can be opened here.
  • step b With the help of the limits determined in step a), braking zones, ie the swept volumes during braking, are calculated ("swept volume") .Therefore, each body is steered step by step from its body-fixed coordinate system into the world-wide coordinate system along the chain In each of these steps, the effect of the controlled-motion mechanism under consideration is calculated in. Methods that accomplish this for the various types of controlled moving mechanisms are described below.
  • the key concept in the process is the notion of the braking zone of a given body in a given coordinate system.
  • This is abstractly a 3D volume consisting of all points in said coordinate system that are located in at least one combination of positions for the controlled mobile mechanisms of the system in said body, each position moving within the limits defined in a).
  • this is the volume in said coordinate system that is swept by said body ("swept volume") when the controlled mobile mechanisms of the system are within the limits defined in a).
  • the braking zones are calculated in an approximate manner so that the brake zones actually represented in the computer may be larger than the abstract braking zone defined above Method, but makes a calculation in real time and thus a use in a plant control possible.
  • all occurring 3D volumes are expressed as a convex hull of a finite set of points plus a buffer radius r according to Eq. (1)
  • a 3D volume is represented as an array of 3D points with the points p, and a buffer radius r.
  • V (r; (p)) describes a sphere, V (r; (p l5 p 2 )) a cylinder with spherical caps and V (r; (pi, p 2 , p 3 )) a rounded triangular plate.
  • the buffer radius also allows approximations to be made in calculations such that the methods presented calculate conservative approximations, ie that the calculated volume guarantees the exact volume but may be slightly larger. This presentation is the surprising realization which allows calculation in steps a) -d) so fast that the method can be used in a plant control in real time.
  • FIG. 2 shows an example of which is shown in two dimensions for purposes of illustration.
  • the convex hull 30 is enlarged in all directions by the radius r, giving an effective volume 32.
  • step c) the part of step c) is described, in which the effect of a controlled movable mechanism in the braking zone is taken into account.
  • This calculation is respectively based on a representation according to (1) the braking zone of the respective body in the coordinate system, which moves with the controlled moving mechanism, as input. It calculates a representation according to (1) the braking zone of this body in the coordinate system relative to which the controlled moving mechanism moves as a result.
  • the calculation is described as a formula, with a different formula being required for each type of controlled mechanism.
  • Several alternative formulas are described for each type of controlled moving mechanism, which usually realize different trade-offs between accuracy and computation time.
  • Kl A 3 D volume can easily be converted to another coordinate system by means of a Coordinate transformation are transformed by all the points p; be transformed. For a coordinate transformation and translation is true
  • step c is used in step c) to convert a 3D volume to a coordinate system where the controlled moving mechanism is at the origin because the following formulas presuppose it.
  • V (r; (Pi) 1 V 1 ) a volume in coordinates, which move with a linear joint. It is to be calculated, the volume that this sweeps when the linear joint between the lower limit position t 0 and position upper limit ti along the translation direction of the linear joint moves a. The result is calculated in the coordinate system relative to which the linear joint moves. This means that the effect of braking this linear joint in the volume should be taken into account.
  • the volume to be calculated is therefore abstract
  • Tl The second possibility is exact, but doubles the number of points in the presentation. It generates a point pi + toa and a point pi + tia for each original point pi. All further Points pi + ⁇ a, ⁇ e [to. -ti] are then automatically enclosed by the definition of the representation as a convex hull.
  • V (r; (p;) V 1 ) a volume that this sweeps when the pivot between the angular position lower limit ⁇ 0 and angular position upper limit O 1 moves about its axis of rotation a.
  • the result is calculated in the coordinate system relative to which the hinge is moving. This means that the effect of braking this hinge in the volume should be taken into account.
  • the volume to be calculated is therefore abstract.
  • red (a, ⁇ ) is the rotation matrix about the origin, the axis a and the angle ⁇ , according to the Rodriguez formula according to "A mathematical introduction into robotic manipulation", R. Murray, Z. Li and S. Sastry, CRC , 1st edition, March 22, 1994, 2006, which is incorporated herein by reference to the full extent.
  • O 0 the lower and ⁇ i the upper limit of step a) for the angular position of the considered rotary joint and V the braking zone before the effect of the rotary joint.
  • a point p moves; circular between the angles ⁇ 0 (pj °) and G 1 (pi 1 ).
  • the circular arc (40) swept over is covered by a circular area (42) represented by formula (14).
  • the center p; 1/2 lies in the middle of the circular arc and the radius corresponds to the length of the arc segment>
  • the circular arc (40) is covered by a circle according to formula (16) whose center (46) at cos ⁇ p, 1/2 , ie in the middle of both end points (pi °), (p; 1 ) lies.
  • the radius is
  • the circular arc (40) is covered by the oval (54), namely the convex hull of its shifted end points pi ° + dpi 1/2 / 2 (56) and (58) according to formula (22) plus a buffer radius of (l-cos ⁇ )
  • RIc For conceivable simplifications of the method, it may be advantageous to dispense with the buffer radius. It is also possible to calculate a conservative approximation that does not increase the buffer radius, but instead makes three points from each point to enclose the arc in a triangle, as shown in FIG. If the arc is covered without a buffer radius, three points are needed. However, this conservative approximation is noticeably less narrow, even than the 2-point approximation (see RIb).
  • the circular arc (40) is covered by the convex hull (60) from the starting point (p; °) and end point (p; 1 ) and the intersection q, of the tangents in the starting and ending points.
  • each body is connected to the world-fixed environment by a chain of controlled mobile mechanisms.
  • This chain corresponds to a chain of coordinate transformations, from the body-fixed coordinate system to the world-wide coordinate system.
  • Each controlled movable mechanism has two coordinate systems, both originating at the point where the movement starts (eg the axis of rotation) and one of which is controlled by the moving one Mechanism moves and moves the controlled moving mechanism relative to the other.
  • the chain of coordinate transformations alternately consists of a fixed transformation between two controlled moving mechanisms, which depends only on the geometry of the plant, and a variable coordinate transformation between the two above-described coordinate systems of a controlled moving mechanism, that of the variable position of the controlled moving mechanism itself depends.
  • step b) according to a particular embodiment of the method according to the invention successive braking zones in all coordinate systems in the above chain, from the body-fixed coordinate system to the world-fixed coordinate system are calculated for each body. All these braking zones are abstract 3D volumes and are shown concretely in the computer as convex hulls with a buffer radius according to equation (1).
  • the procedure starts with the braking zone of the body in the body-fixed coordinate system, which is the pre-configured 3 D volume of the body itself. Thereafter, all coordinate transformations of the chain described above are successively applied to the 3D volume.
  • a fixed coordinate transformations via Kl (3) is applied, the variable coordinate transformation of a linear joint optionally via TO (7) or Tl (9) and the variable coordinate transformation of a rotary joint optionally via ROa (14), ROb (16), Ria (19) , RIb (22), RIc (24) or RId.
  • ROb (14) or RIb (22) is preferred.
  • the choice between the alternatives TO or Tl and ROb or RIb is a compromise between accuracy and computing time and could, for. B. be preconfigured in advance. It is especially recommended for a robotic arm to calculate the front pivot joints with ROa and the rear with RIb to achieve a good compromise.
  • FIGS. 6 to 9 illustrate an example of the procedure according to a particular embodiment of the method.
  • Fig. 6 shows a robot 80 in plan view from above. It consists of a hinge J 1 , followed by a fork (90, 92) and two parallel further hinges J 2a , J 2b .
  • the environment is fixed in the world coordinate system defined by 4 bodies: 3 lines, represented as a convex hull Two points without buffer radius (82, 84, 86), and an oval, shown as a convex hull of two points with buffer radius (88).
  • the fork is fixed in the coordinate system J 1 , which rotates with the first joint, and consists of two bodies, represented as ovals (90, 92).
  • the bodies 94 and 96 are fixed in the coordinate systems J 2a and J 2b , which rotate with the joints J 1 and J 2a and J 2b , respectively, and are shown as an oval.
  • the points used to define the bodies in this representation as V (r, (pOVi) are each marked as gray crosses, in this example assume for the calculations in Figures 7-9 that all joints are at some velocity to turn left.
  • FIG. 7 shows the braking zones in the coordinate systems J 2a and J 2b of FIG. 6.
  • the braking zone of body 94 in J 2a is the original body since it is fixed in J 2a .
  • Other bodies have no braking zones in this coordinate system, as braking zones of a body are calculated from the body-fixed to the world-fixed coordinate system.
  • Fig. 8 shows the braking zones in the coordinate system J 1 of Fig. 6.
  • the two bodies (90), (92) are fixed in J 1 so that their braking zones are the original bodies.
  • the braking zones of the bodies 94 and 96 of Figure 7, which are the bodies themselves the braking of the joints J 2a and J 2b is calculated (according to RIb (24)). This creates a rotated and an unrotated copy from each of the two points.
  • the result (98, 100) is the convex hull of these four points, coincidentally two points identical because they are on the axis of rotation.
  • the distance between body 94 and body 96 is calculated with these braking zones 98 and 100 so that joint 1 has no influence on them.
  • Fig. 9 shows the braking zones in the world coordinate system for the example of Fig. 6.
  • the environment (82, 84, 86, 88) is fixed in the world so that the original bodies are at the same time braking zones in the world coordinate system.
  • the braking zones (90, 92, 98, 100) in J 1 the braking of joint J 1 is calculated (RIb (24)). This doubles the points in the four affected braking zones (102, 104, 106, 108). The distance between all robot parts and the world is calculated with these braking zones.
  • step c) the distance of its brake zones with, for example, the known GJK algorithm is determined for each pair of bodies.
  • the GJK algorithm requires a previously unknown number of iterations.
  • the algorithm is started with the result from the last cycle of the system control and the iteration is aborted as soon as the algorithm has calculated a lower bound for the distance> 0. Both considerably reduce the computing time.
  • braking zones in a common coordinate system expediently in the first common coordinate system.
  • the distance between bodies 94 and 96 with the braking zones 98 and 100 in the coordinate system J 1 is determined, ie the movement of J 1 does not affect the distance.
  • pairs of bodies between which there is no controlled movable mechanism, which therefore does not move relative to each other are not tested against each other.
  • pairs of bodies can be preconfigured as untested. This is necessary because z. B. the two sides of a controlled moving mechanism geometrically always collide.
  • the method described above according to a particular embodiment of the present invention is relatively fast. But there may still be two calculation time problems. On the one hand, all pairs of bodies are checked or tested. As a result, the computing time grows quadratically with the number of bodies.
  • a second computational time problem is that calculating the distance with the GJK algorithm requires a previously unknown number of iterations. In a system control (as well as a robot control) but all processes run in a fixed cycle. As a result, a multiple of the average computing time would have to be reserved for the procedure, so that the cycle time can be maintained even under unfavorable conditions.
  • This procedure is based on two considerations. First, as long as a distance bound is 0, a GJK iteration must be performed on it, because if it stays 0, a collision can not be ruled out and it must be stopped in step d). If all distance bounds are> 0, it still makes sense to perform GJK iterations, as this could make the updated distance bounds larger, and in later cycles more GJK iterations can be used for other distance bounds. The algorithm works as it were.
  • the inventive method has important advantages, at least in particular embodiments:
  • the algorithm is adaptive in terms of speed and distance and overcomes the problem of quadratic computation time.
  • the ⁇ roughly correspond to the distance traveled in time. This means that as the system moves more slowly, more bars, ie more GJK iterations, are available. For many pairs of braking zones, the distances are anyway large, z. B. because the plant just far away from the environment, or for those couples who belong to bodies that are too far away. Then, this distance barrier rarely needs to be recalculated and almost always a single GJK iteration is enough. In addition, of course, no body moves close to all parts of the environment. The algorithm is therefore always secure, requires few iterations, and only in exceptional cases an unnecessary stop.
  • a method according to a particular embodiment of the present invention can be avoided with the collisions of controlled moving vehicles, in particular driverless transport systems (FTS) with a lower computational cost than before.
  • FTS driverless transport systems
  • Robot arms are variable in such a vehicle position and / or orientation with respect to the world, have a braking distance, but are not controlled by a sequence of movable mechanisms with a single degree of freedom, such.
  • a vehicle is considered to be a single, three-degrees-of-freedom, controlled-motion mechanism.
  • the vehicle must therefore measure its position (x, y) and orientation ( ⁇ ) in the plane and its time derivative, ie its speed.
  • the vector (x, y) describes the position of a fixed reference point on the vehicle in the world and, together with ⁇ , a transformation from the body-fixed coordinate system on the vehicle to the world-wide coordinate system.
  • the position limits of a vehicle are instead defined in step a) as a convex subset of its configuration space, the configuration space consisting of SD vectors with the components (x, y, ⁇ ) and said subset being a convex hull of finitely many Configurations in configuration space is displayed:
  • Each configuration k defines a coordinate transformation from the body-fixed coordinate system on the vehicle to the world-wide coordinate system.
  • V is said 3D volume and K is said set of configurations.
  • Fla The procedure is analogous to approximation Ria at swivel joints.
  • Fig. 11 shows the underlying situation.
  • the configuration of the vehicle at the end of the brake 120 is assumed relative to the configuration at the beginning of the brake 122 a circular path 126 by two parameters (s, ⁇ ) described.
  • the parameter s describes the braking distance traveled on the circular path (signed) and ⁇ the change of the orientation (with respect to ⁇ ).
  • Vehicles can change continuously from a straight-ahead curve.
  • the center of rotation wanders to infinity. Therefore, unlike the hinge, a version of the formulas is used, when driving straight ahead is not a special case and in particular the zero point of the coordinate system does not have to be in the center of rotation.
  • the erfmdungssiee type of representation does this.
  • step a By specifying the configuration of the vehicle at the beginning of braking 122 and the parameters (s, ⁇ ), the limits of the position of the vehicle are thereby defined in step a).
  • the following formula defines the transformation from the body-fixed coordinate system of the vehicle at the end of the braking to the body-fixed coordinate system at the beginning of the braking.
  • a braking zone in co-ordinates that move with the vehicle, to calculate the 3 D volume that it sweeps over when the vehicle is moving
  • the result is calculated in the body-fixed coordinate system of the vehicle at the beginning of the braking, a conversion into the world-wide coordinate system can later be done via KI
  • the effect of braking this vehicle into the volume should be taken into account, so the volume to be calculated is abstract
  • FIc According to FIG. 12, on the left, an approximation corresponding to RIc is given, which for each point p; from the original 3D volume out of p; resulting arc 126 in a triangle 130 includes. This will make every point pi in the original 3D volume three points.
  • the triangle consists of the starting point pi °, the end point p; 1 and additionally the intersection q; the tangents in the start and end points.
  • the points are calculated as follows:
  • the circular arc 126 of FIc is divided into h equal parts, each part approached individually.
  • the partial arc length is in each case s / h and the angle change ⁇ / h. Of the entire arc 126 is then covered by the convex hull of all these points.
  • a convex-hull algorithm eg, Graham-Scan
  • the polygon is expanded by circles of r around the corners and parallels to the edges by r. All possible beams of the laser scanner are cut with all edges and circles. For each ray, the protective field is the largest distance of such a cut.
  • the movement is monitored by one or more robotic arms and timed to stop before the robotic arm collides itself or the robotic arms collide with each other or with the environment.
  • This avoids accidents in situations where the robot does not always travel exactly the same lane, e.g. B. when learning courses, the so-called teach-in, or sensor-guided work.
  • the method is used to avoid collision with geometrically previously known obstacles, so in the basic version not for the protection of persons.
  • the method can be extended or tailored to controlled mobile vehicles or FTS, on which robot arms can also be located, and to sensor-detected obstacles. As a result, the method also serves to avoid collision with persons.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

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 (pi)n i=1 zzgl. eines Pufferradius r>=0 nach der Definition (I) dargestellt wird.

Description

„Verfahren zur Vermeidung von Kollisionen gesteuert beweglicher Teile einer Anlage"
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 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 n2 Paare auf Kollision getestet werden. In dem US-Patent Nr. 5,056,031 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.
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 SD- Koordinatensystem definiert wird und jeder Körper in besagtem körperfesten SD- Koordinatensystem als 3D-Volumen beschrieben und das 3D-Volumen als konvexe Hülle einer endlichen Menge von Punkten (Pi)V1 zzgl. eines Pufferradius r>=0 nach der Definition
Figure imgf000005_0001
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; seine Bewegung beim Bremsen des gesteuert beweglichen Mechanismus nach a) durch ein Volumen
Figure imgf000005_0002
dargestellt mit Pufferradien rj und Punktenpij, nach (1) überdeckt wird, die resultierenden Punkte zu einer konvexen Hülle zusammengeführt werden und der ursprüngliche Pufferradius um das Maximum der η erhöht wird, c) Bestimmen über alle Paare von Körpern die Distanz zwischen ihren Bremszonen in einem gemeinsamen, insbesondere 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 SD- 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:
Figure imgf000007_0001
Figure imgf000008_0001
wobei θ0 die Unter- und O1 die Obergrenze aus Schritt a) für die Winkelposition des betrachteten Drehgelenkes ist und die Darstellung der Bremszone vor Einrechnung des Effektes
Figure imgf000008_0002
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:
Figure imgf000009_0001
oder
Figure imgf000009_0002
wobei to die Unter- und U die Obergrenze aus Schritt a) für die Position des betrachteten Lineargelenkes ist und
Figure imgf000009_0003
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 (kj)j=i im Konfigurationsraum dargestellt wird:
Figure imgf000010_0001
wobei die Komponenten der Konfigurationen kj wie folgt bezeichnet werden:
Figure imgf000010_0002
Vorteilhafterweise kann vorgesehen sein, dass für ein Fahrzeug folgende Formel für die Einberechnung des Effektes des Fahrzeugbremsens verwendet wird:
Figure imgf000010_0003
und
Figure imgf000011_0001
wobei die Darstellung der Bremszone vor Einrechnung des Effektes dieses
Figure imgf000011_0004
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
Figure imgf000011_0002
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:
Figure imgf000011_0003
mit
Figure imgf000012_0001
oder mit
Figure imgf000013_0001
wobei 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 3 D- 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 Ausfuhrungsform 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 δr; 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:
Figure imgf000014_0001
wobei
Figure imgf000014_0003
die Darstellung der Bremszone j im letzten Takt und die
Figure imgf000014_0002
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:
Fig. 1 eine schematische Darstellung einer beispielhaften Anwendungssituation für das erfindungsgemäße Verfahren in einer Anlage (Industrieanlage) zeigt; Fig. 2 beispielhaft die Darstellung eines 3 D- Volumens nach (1) zeigt;
Fig. 3 zwei Näherungsmöglichkeiten für die Berechnung der Bremszone eines Drehgelenks zeigt;
Fig. 4 zwei weitere Näherungsmöglichkeiten für die Berechnung der Bremszone eines Drehgelenks zeigt;
Fig. 5 eine weitere Näherungsmöglichkeit für die Berechnung der Bremszone eines Drehgelenks zeigt;
Fig. 6 bis 9 Schritte der Berechnung der Bremszonen von Teilen einer Anlage in verschiedenen Koordinatensystemen zeigen;
Fig. 10 eine Bremszone in zwei aufeinanderfolgenden Takten einer Anlagensteuerung zeigt;
Fig. 11 ein Fahrzeug beim Bremsen entlang einer Kreisbahn und die dabei relevanten Koordinatensysteme zeigt; und
Fig. 12 zwei Näherungsmöglichkeiten zur Berechnung der Bremszone eines Fahrzeugs zeigt.
Fig. 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 3 D-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 vollumfanglich 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)
Figure imgf000019_0001
dargestellt. D. h., dass im Computer ein 3D-Volumen
Figure imgf000019_0002
als ein Array von 3D-Punkten mit den Punkten p, und ein Pufferradius r repräsentiert wird.
In dem Spezialfall r=0, beschreibt einen konvexen Polyeder mit p; als potentiellen
Figure imgf000019_0003
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; 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; (pl5 p2)) einen Zylinder mit Kugelkappen und V (r; (pi, p2, p3)) 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.
Figur 2 zeigt ein Beispiel für
Figure imgf000020_0002
das aus Darstellungsgründen zweidimensional gezeigt ist. Das Beispiel hat n=3 Punkte pj, p2, P3 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
Figure imgf000020_0001
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; (pOVi ), im Rechner konkret gegeben durch den Radius r und die Punkte pj. 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 pi 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 3 D- Volumen ausfuhrt.
In den meisten Fällen steht in der Gleichung kein „=", sondern ein
Figure imgf000021_0005
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
Kl Ein 3 D- Volumen kann einfach in ein anderes Koordinatensystem mittels einer
Figure imgf000021_0004
Koordinatentransformation transformiert werden, indem alle Punkte p; transformiert werden. Für eine Koordinatentransformation
Figure imgf000021_0002
und Translation
Figure imgf000021_0003
gilt also
Figure imgf000021_0001
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. Kl 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; (Pi)1V1 ), 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 t0 und Positionsobergrenze ti 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
Figure imgf000022_0001
Dabei ist to die Unter- und ti die Obergrenze aus Schritt a) für die Position des betrachteten Lineargelenkes und V die Bremszone vor Einrechnung des Effektes des Lineargelenkes.
TO Die erste Möglichkeit zur Näherung besteht darin, in Gleichung (4) X=(^t1)/! festzusetzen und den entstehenden Fehler |a| (ti-to)/2 auf r aufzuschlagen:
Figure imgf000022_0002
Tl Die zweite Möglichkeit ist exakt, verdoppelt aber die Zahl der Punkte in der Darstellung. Sie erzeugt für jeden ursprünglichen Punkt pi einen Punkt pi+toa und einen Punkt pi+tia. Alle weiteren Punkten pi+λa, λ e [to. -.ti] sind dann durch die Definition der Darstellung als konvexe Hülle automatisch eingeschlossen.
Figure imgf000023_0002
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;) V1), 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 θ0 und Winkelpositionsobergrenze O1 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.
Figure imgf000023_0001
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 O0 die Unter- und θi 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, (pj) nj=1) dargestelltes Volumen
Figure imgf000024_0002
die rotierten Punkte zwischen Anfang (λ=0) und Ende (λ=l) des Kreisbogens (40), den p; durchläuft, wenn sich das Drehgelenk von Winkelposition θ0 nach O1 bewegt.
ROa Die einfachste Näherung setzt analog zu TO λ = (to+tO/2 und schlägt den daraus resultierenden Fehler von < max,|pi| (QI-QQ)II auf den Radius r auf (s. Fig. 3 links). Dies liefert
Figure imgf000024_0001
Gemäß Fig. 3 links bewegt sich ein Punkt p; kreisförmig zwischen den Winkeln θ0 (pj°) und G1 (pi1). Der dabei überstrichene Kreisbogen (40) wird durch eine Kreisfläche (42), dargestellt durch Formel (14), überdeckt. Der Mittelpunkt p;1/2 liegt in der Mitte des Kreisbogens und der Radius entspricht der Länge des Bogensegmentes >
ROb Eine bessere Näherung erhält man nach dem Prinzip in Fig. 3 rechts durch die Formel
Figure imgf000024_0003
Figure imgf000025_0001
Gemäß Fig. 3 rechts wird der Kreisbogen (40) durch einen Kreis gemäß Formel (16) überdeckt, dessen Mittelpunkt (46) bei cosφ p,1/2, also in der Mitte beider Endpunkte (pi°), (p;1) liegt. Der Radius ist sind
Ria Eine noch bessere Näherung erhält man, indem für jeden ursprünglichen Punkt pj Anfangspunkt pi° und Endpunkt p;1 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 φ)|pi|. und wird auf den Radius aufgeschlagen. Es ergibt sich folgende Formel:
Figure imgf000025_0002
Der Preis für die genauere Näherung ist eine Verdoppelung der Punktzahl. Fig. 4 links zeigt, dass der Kreisbogen (40) durch die konvexe Hülle seiner Endpunkte (p;0, p;1) zuzüglich eines Radius von (l-cosφ)|pi| überdeckt wird. RIb Eine noch bessere Näherung wird dadurch erreicht, dass die Strecke (56-58) zwischen den beiden eingefügten Punkten, wie in Fig. 4 rechts gezeigt, in die Mitte des Kreisbogens gelegt wird. Dann ergibt sich der geringste Radius.
Figure imgf000026_0001
Gemäß Fig. 4 rechts wird der Kreisbogen (40) überdeckt durch das Oval (54), nämlich die die konvexe Hülle seiner verschobenen Endpunkte pi°+dpi1/2/2 (56) und
Figure imgf000026_0002
(58) gemäß Formel (22) zuzüglich eines Pufferradius von (l-cosφ)|pi|/2.
RIc 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 Fig. 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 RIb).
Figure imgf000026_0003
mit
Figure imgf000027_0001
Gemäß Fig. 5 wird der Kreisbogen (40) überdeckt von der konvexen Hülle (60) von Anfangspunkt (p;°) und Endpunkt (p;1) und dem Schnittpunkt q, der Tangenten in Anfangs- und Endpunkt.
RId Die in Ria bis RIc 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 3 D- 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 Kl (3) angewandt, die variable Koordinatentransformation eines Lineargelenkes wahlweise über TO (7) oder Tl (9) und die variable Koordinatentransformation eines Drehgelenkes wahlweise über ROa (14), ROb (16), Ria (19), RIb (22), RIc (24) oder RId. Dabei wird ROb (14) oder RIb (22) bevorzugt. Die Wahl zwischen den Alternativen TO oder Tl und ROb oder RIb 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 ROa und die hinteren mit RIb zu berechnen um einen guten Kompromiss zu erreichen.
Fig. 6 bis 9 stellen ein Beispiel für das Vorgehen gemäß einer besonderen Ausführungsform des Verfahrens dar.
Fig. 6 zeigt einen Roboter 80 in Draufsicht von oben. Er besteht aus einem Drehgelenk J1, gefolgt von einer Gabel (90, 92) und zwei parallelen weiteren Drehgelenken J2a, J2b. 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 J1, 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 J2a bzw. J2b, die sich mit den Gelenken J1 und J2a bzw. J2b mitdrehen, und werden dargestellt als Oval. Die Punkte, die zur Definition der Körper in dieser Darstellung als V(r, (pOVi) dienen, sind jeweils als graue Kreuze markiert. In diesem Beispiel sei für die Rechnungen in Fig. 7-9 angenommen, dass sich alle Gelenke mit gewisser Geschwindigkeit nach links drehen.
Fig. 7 zeigt die Bremszonen in den Koordinatensystemen J2a und J2b aus Fig. 6. In (a) von Fig. 7 ist die Bremszone von Körper 94 in J2a der ursprüngliche Körper, da dieser in J2a 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 Fig. 7 gilt analoges für Körper 96 in J2b.
Fig. 8 zeigt die Bremszonen im Koordinatensystem J1 aus Fig. 6. Die beiden Körper (90), (92) sind fest in J1, so dass ihre Bremszonen die ursprünglichen Körper sind. In die Bremszonen der Körper 94 und 96 aus Fig 7, welche die Körper selbst sind, wird die Bremsung der Gelenke J2a bzw. J2b einberechnet (nach RIb (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 Fig. 9 die Bremszonen im weltfesten Koordinatensystem für das Beispiel von Fig. 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 J1 wird die Bremsung von Gelenk J1 einberechnet (RIb (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 Ausfuhrungsform 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 Figur 8 wird beispielsweise die Distanz zwischen Körpern 94 und 96 mit den Bremszonen 98 und 100 im Koordinatensystem J1 bestimmt, d.h. die Bewegung von Jl 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 erfmdungsgemäßen Verfahrens auch möglich, ist aber grundsätzlich eher kompliziert. Daher soll sie zumindest in einer besonderen Ausfuhrungsform des erfϊndungsgemäß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 δη abgeschätzt. So ein δη muss die Eigenschaft haben, dass sich kein Punkt der Bremszone j seit dem letzten Takt um mehr als δr,- bewegt hat. Er wird berechnet als
Figure imgf000031_0001
wobei
Figure imgf000031_0002
die Darstellung der Bremszone j aus dem aktuellen Takt und
Figure imgf000031_0003
die Darstellung der Bremszone j aus dem letzten Takt ist.
Fig. 10 zeigt die Berechnung des Änderungsradius nach (26) aus einer Bremszone
Figure imgf000031_0004
des letzten Taktes (110) und der entsprechenden Bremszone
Figure imgf000031_0005
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 δrj+δrj 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 δη 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 SD- Vektoren mit den Komponenten (x, y, θ) besteht und wobei besagte Untermenge als konvexe Hülle von endlich vielen Konfigurationen im Konfigurationsraum dargestellt wird:
Figure imgf000033_0001
Figure imgf000034_0001
wobei die Komponenten der Konfigurationen kj wie folgt bezeichnet werden:
Figure imgf000034_0002
Jede Konfiguration k definiert eine Koordinatentransformation vom körperfesten Koordinatensystem am Fahrzeug ins weltfeste Koordinatensystem.
Figure imgf000034_0003
Dies ist eine homogene Transformationsmatrix, die Rotation und Translation kombiniert. Zur Vereinfachung der Notation sei daher, wie üblich vereinbart, dass 3 D-Orts vektoren, besonders die Pi bei Multiplikation mit einer homogenen 4x4 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((kj)mj=1) definierten Konfigurationen durchläuft. Dies lässt sich abstrakt wie folgt darstellen:
Figure imgf000035_0001
wobei V besagtes 3D-Volumen und K besagte Menge von Konfigurationen ist.
Fla Die Vorgehensweise ist analog zu Näherung Ria bei Drehgelenken. Für ein nach (1) definiertes 3D-Volumen V(r; (pi)ni=1), konkret eine Bremszone und eine als konvexe Hülle definierte Menge von Konfigurationen K((kj)m j=i), wird für jedes Paar i, j der Punkt pj vom körperfesten Koordinatensystem ins weltfeste Koordinatensystem gemäß kj 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.
Figure imgf000035_0002
Vereinfachte Definition der Grenzen der Position beim Bremsen eines Fahrzeugs
Nimmt man an, dass das Fahrzeug 122 (siehe Fig. 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 ROa-RId für ein Drehgelenk.
Fig. 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 erfmdungsgemäße Art der Darstellung leistet dies. Eine Geradeausfahrt entsteht für α=0 und s≠O.
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.
Figure imgf000036_0001
Gegeben ist ein 3D-Volumen V(r; (pi)"i=i), konkret eine Bremszone, in Koordinaten, die sich mit dem Fahrzeug mitbewegen. Es soll das 3 D- 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 Kl 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
Figure imgf000036_0002
Nachfolgend werden zwei Näherungen von Rot(V(r; (pOVi), s, α) beschrieben, die ohne einen Aufschlag auf den Pufferradius r auskommen.
FIc Gemäß Fig. 12 links wird eine RIc entsprechende Näherung angegeben, die für jeden Punkt p; aus dem ursprünglichen 3D- Volumen den aus p; entstehenden Kreisbogen 126 in ein Dreieck 130 einschließt. Dadurch werden aus jedem Punkt pi im ursprünglichen 3D-Volumen drei Punkte. Das Dreieck besteht aus dem Anfangspunkt pi°, dem Endpunkt p;1 und zusätzlich dem Schnittpunkt q; der Tangenten im Anfangs- und Endpunkt. Die Punkte werden wie folgt berechnet:
Figure imgf000037_0001
FId Gemäß Fig. 12 rechts wird der Kreisbogen 126 von FIc 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^ 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 pj° und der Endpunkt p;1 des Kreisbogens 126 und die Tangentenschnittpunkte (q^ für k=0..h-l) aller Teilbögen. Es entstehen damit h+2 anstatt drei Punkte zu jedem Punkt p; des ursprünglichen 3D-Volumens. V(r; (pi)"i=i).
Figure imgf000038_0001
Zum gleichen Ergebnis gelangt man, indem man alle q^ für k=l..h-l aus q*0 errechnet nach
Figure imgf000038_0002
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; (pi)ni=i) 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 Kollisions vermeidung mit Personen.
Die in der vorstehenden Beschreibung, in den Zeichnungen sowie in den Ansprüchen offenbarten Merkmale der Erfindung können sowohl einzeln als auch in beliebigen Kombinationen für die Verwirklichung der Erfindung in ihren verschiedenen Ausführungsformen wesentlich sein.

Claims

Ansprüche
1. 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 (Pi)V1 zzgl. eines Pufferradius r>=0 nach der Definition
Figure imgf000041_0001
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 3 D- 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 pj seine Bewegung beim Bremsen des gesteuert beweglichen Mechanismus nach a) durch ein Volumen
Figure imgf000041_0002
dargestellt mit Pufferradien T1 und Punkten py, nach (1) überdeckt wird, die resultierenden Punkte zu einer konvexen Hülle zusammengeführt werden und der ursprüngliche Pufferradius um das Maximum der Tj erhöht wird,
c) Bestimmen über alle Paare von Körpern die Distanz zwischen ihren Bremszonen in einem gemeinsamen, insbesondere 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.
2. Verfahren nach Anspruch 1, dadurch gekennzeichnet, dass die Körper in dem jeweiligen SD- Volumen, das nach (1) dargestellt ist, nur enthalten sind und nicht direkt dargestellt werden.
3. 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.
4. 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.
5. Verfahren nach Anspruch 4, dadurch gekennzeichnet, dass für ein Drehgelenk folgende Formel in Schritt b) benutzt wird:
Figure imgf000042_0001
Figure imgf000043_0001
Figure imgf000044_0001
durchgeführt wird, wobei θ0 die Unter- und G1 die Obergrenze aus Schritt a) für die Winkelposition des betrachteten Drehgelenkes ist und V(r,
Figure imgf000044_0002
die Darstellung der Bremszone vor Einrechnung des Effektes des Drehgelenkes ist.
6. 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.
7. Verfahren nach Anspruch 6, dadurch gekennzeichnet, dass für ein Lineargelenk folgende Formel in Schritt b) benutzt wird:
Figure imgf000045_0001
oder
Figure imgf000045_0002
wobei to die Unter- und tt die Obergrenze aus Schritt a) für die Position des betrachteten Lineargelenkes ist und V(r, (pi)ni=i) die Darstellung der Bremszone vor Einrechnung des Effektes des Lineargelenkes ist.
8. 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.
9. 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 θ den Winkel der Orientierung des Fahrzeugs angibt und wobei besagte Untermenge als konvexe Hülle von endlich vielen Konfigurationen (k)m j=i im Konfigurationsraum dargestellt wird:
Figure imgf000046_0004
wobei die Komponenten der Konfigurationen kj wie folgt bezeichnet werden:
Figure imgf000046_0001
10. Verfahren nach Anspruch 9, dadurch gekennzeichnet, dass für ein Fahrzeug folgende Formel für die Einberechnung des Effektes des Fahrzeugbremsens verwendet wird:
Figure imgf000046_0002
mit
Figure imgf000046_0003
und
Figure imgf000047_0001
wobei V(r, (pi)ni=0 die Darstellung der Bremszone vor Einrechnung des Effektes dieses Fahrzeugs ist.
11. 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
Figure imgf000047_0002
die Position des Fahrzeugs nach dem Bremsen (120) relativ zur Position vor dem Bremsen (122) als homogene Matrix angibt.
12. Verfahren nach Anspruch 11, dadurch gekennzeichnet, dass für ein Fahrzeug folgende Formel für die Einberechnung des Effektes des Fahrzeugbremsens verwendet wird:
Figure imgf000047_0003
mit
Figure imgf000048_0001
oder mit
Figure imgf000049_0001
wobei V(r, (Pi)Vi) 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.
13. Verfahren nach einem der vorangehenden Ansprüche, dadurch gekennzeichnet, dass die Anlage Körper aufweist, die durch verschiedene gesteuert bewegliche Mechanismen verbunden sind.
H. Verfahren nach einem der Ansprüche 4 bis 13, dadurch gekennzeichnet, dass in Schritt a) Messunsicherheiten der Positionsgeber auf die Grenzen aufgeschlagen werden.
15. Verfahren nach einem der Ansprüche 4 bis 14, dadurch gekennzeichnet, dass in Schritt a) Reaktionszeiten bei der Bestimmung der Grenzen aufgeschlagen werden.
16. Verfahren nach Anspruch 15, dadurch gekennzeichnet, dass der Aufschlag ein Produkt aus Reaktionszeit und Geschwindigkeit umfasst.
17. Verfahren nach einem der vorangehenden Ansprüche, dadurch gekennzeichnet, dass zum Pufferradius die Hälfte eines globalen Sicherheitsabstands addiert wird.
18. Verfahren nach einem der vorangehenden Ansprüche, dadurch gekennzeichnet, dass in Schritt c) die Distanzen mittels des GJK (Gilbert- Johnson-Keerthi)-Algorithmus bestimmt werden.
19. 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 δr; 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 δrj+ δ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.
20. Verfahren nach Anspruch 19, dadurch gekennzeichnet, dass als pauschaler Änderungsradius
Figure imgf000050_0001
verwendet wird, wobei die Darstellung der Bremszone j im letzten Takt und
Figure imgf000050_0002
Figure imgf000050_0004
Figure imgf000050_0003
die Darstellung der Bremszone j im aktuellen Takt ist.
21. Verfahren nach einem der Ansprüche 4 bis 20, dadurch gekennzeichnet, dass die Positionsgeber Linear- oder Winkelgeber sind.
22. 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.
23. Verfahren nach Anspruch 21, dadurch gekennzeichnet, dass zur Berechnung des Schutzfeldes das 3D- Volumen der Bremszone in eine Ebene transformiert oder projiziert wird.
24. Verfahren nach einem der vorangehenden Ansprüche, dadurch gekennzeichnet, dass in Schritt a) ein zusätzlicher Aufschlag auf die Grenzen erfolgt.
25. 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.
PCT/DE2010/000090 2009-01-27 2010-01-26 Verfahren zur vermeidung von kollisionen gesteuert beweglicher teile einer anlage WO2010085944A1 (de)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
DE102009006256.4 2009-01-27
DE102009006256.4A DE102009006256B4 (de) 2009-01-27 2009-01-27 Verfahren zur Vermeidung von Kollisionen gesteuert beweglicher Teile einer Anlage

Publications (1)

Publication Number Publication Date
WO2010085944A1 true WO2010085944A1 (de) 2010-08-05

Family

ID=42154615

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/DE2010/000090 WO2010085944A1 (de) 2009-01-27 2010-01-26 Verfahren zur vermeidung von kollisionen gesteuert beweglicher teile einer anlage

Country Status (2)

Country Link
DE (1) DE102009006256B4 (de)
WO (1) WO2010085944A1 (de)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3421189A1 (de) 2017-06-28 2019-01-02 Sick AG Verfahren zum überwachen einer maschine
US10384870B2 (en) 2013-11-18 2019-08-20 Grenzebach Maschinenbau Gmbh Method and device for order picking in warehouses largely by machine
US10586463B2 (en) 2014-04-17 2020-03-10 SZ DJI Technology Co., Ltd. Polygon shaped flight-restriction zones
CN113858213A (zh) * 2021-11-09 2021-12-31 南京埃斯顿机器人工程有限公司 一种用于目标同步的机器人动态轨迹规划方法
US11482121B2 (en) 2015-03-31 2022-10-25 SZ DJI Technology Co., Ltd. Open platform for vehicle restricted region

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2428862B1 (de) 2010-09-13 2013-01-09 Sick Ag Vorrichtung und Verfahren zur Sicherheitssteuerung eines Fahrzeuges
JP5144785B2 (ja) * 2011-04-18 2013-02-13 ファナック株式会社 ロボットの着目部位と周辺物との干渉を予測する方法及び装置
US9403275B2 (en) * 2014-10-17 2016-08-02 GM Global Technology Operations LLC Dynamic obstacle avoidance in a robotic system
DE102015009815A1 (de) * 2015-07-28 2017-02-02 Kuka Roboter Gmbh Verfahren zum Steuern eines mobilen redundanten Roboters
DE102016211129A1 (de) * 2016-06-22 2017-12-28 Kuka Roboter Gmbh Verfahren zum Überprüfen einer Kollision zwischen zwei fahrerlosen Transportfahrzeugen, fahrerloses Transportfahrzeug und System mit mehreren fahrerlosen Transportfahrzeugen
DE102020213127B3 (de) 2020-10-19 2022-01-20 Kuka Deutschland Gmbh Verfahren und System zum Betreiben eines Roboters
DE102021200600A1 (de) 2021-01-22 2022-07-28 Kuka Deutschland Gmbh Planen eines Pfads eines fahrerlosen mobilen Roboters
DE102022121539A1 (de) 2022-08-25 2024-03-07 Bayerische Motoren Werke Aktiengesellschaft Fahrerloses Transportsystem

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2227106A (en) * 1988-11-12 1990-07-18 Toyoda Chuo Kenkyusho Kk Detecting collision
US5056031A (en) 1988-11-12 1991-10-08 Kabushiki Kaisha Toyota Chuo Kenyusho Apparatus for detecting the collision of moving objects
WO2002058034A1 (en) * 2001-01-17 2002-07-25 Bhp Billiton Innovation Pty Ltd Anti-collision protection system
EP1366867A2 (de) * 2002-05-30 2003-12-03 KUKA Roboter GmbH Verfahren und Vorrichtung zum Vermeiden von Kollisionen zwischen Industrierobotern und anderen Objekten

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
SE456048B (sv) * 1982-02-24 1988-08-29 Philips Norden Ab Sett och anordning for att bestemma kollisionsrisken for tva inbordes rorliga kroppar
US6099573A (en) * 1998-04-17 2000-08-08 Sandia Corporation Method and apparatus for modeling interactions
DE10226140A1 (de) * 2002-06-13 2004-01-08 Kuka Roboter Gmbh Verfahren und Vorrichtung zum Vermeiden von Kollisionen zwischen Industrierobotern und anderen Objekten
DE10361132B4 (de) 2003-06-18 2013-02-28 Elan Schaltelemente Gmbh & Co. Kg Verfahren zur Überwachung der Bewegung eines sich in mehreren Freiheitsgraden bewegenden Gefahr bringenden Objektes eines Handhabungsgerätes, wie Handhabungsmasse und/oder bewegliche Masse
DE102004027944B4 (de) * 2004-06-08 2012-02-16 Kuka Laboratories Gmbh Verfahren zum Schützen eines Roboters gegen Kollisionen
DE602006003435D1 (de) 2006-09-14 2008-12-11 Abb Research Ltd Verfahren und Vorrichtung zur Vermeidung von Kollisionen zwischen einem Industrieroboter und einem Objekt
ATE459030T1 (de) 2006-09-14 2010-03-15 Abb Research Ltd Verfahren und vorrichtung zur vermeidung von kollisionen zwischen einem industrieroboter und einem objekt

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2227106A (en) * 1988-11-12 1990-07-18 Toyoda Chuo Kenkyusho Kk Detecting collision
US5056031A (en) 1988-11-12 1991-10-08 Kabushiki Kaisha Toyota Chuo Kenyusho Apparatus for detecting the collision of moving objects
WO2002058034A1 (en) * 2001-01-17 2002-07-25 Bhp Billiton Innovation Pty Ltd Anti-collision protection system
EP1366867A2 (de) * 2002-05-30 2003-12-03 KUKA Roboter GmbH Verfahren und Vorrichtung zum Vermeiden von Kollisionen zwischen Industrierobotern und anderen Objekten
US6678582B2 (en) 2002-05-30 2004-01-13 Kuka Roboter Gmbh Method and control device for avoiding collisions between cooperating robots

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
C. ERICSON: "The Gilbert-Johnson-Keerthi (GJK) algorithm", SICGRAPH CONFERENCE PLENARY TALK, 2004
DR. CHRISTOPH LÜTH, DR. UDO FRESE, HOLGER TÄUBIG, DENNIS WALTER: "SAMS-Sicherheitskomponente für AutonomeMobile Serviceroboter", VDI BERICHT 2012, ROBOTIK 2008, LEISTUNGSSTAND-ANWENDUNGEN-VISIONEN-TRENDS, 12 June 2008 (2008-06-12), Deutschland, pages 123 - 126, XP002583616 *
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, vol. 4, no. 2, 1988
R. MURRAY, Z. LI; S. SASTRY: "A mathematical introduction into robotic manipulation", 22 March 1994, CRC
R. MURRAY, Z. LI; S. SASTRY: "Rodriguez-Formel gemäß", 22 March 1994, CRC, article "A mathematical introduction into robotic manipulation"
UDO FRESE; HOLGER TAEUBIG: "Verfahren zur Vermeidung von Kollisionengesteuert beweglicher Teile einer Anlage", DEUTSCHES FORSCHUNGSZENTRUM FUER KUENSTLICHE INTELLIGENZ, April 2009 (2009-04-01), Deutschland, XP002583617 *

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10384870B2 (en) 2013-11-18 2019-08-20 Grenzebach Maschinenbau Gmbh Method and device for order picking in warehouses largely by machine
US10586463B2 (en) 2014-04-17 2020-03-10 SZ DJI Technology Co., Ltd. Polygon shaped flight-restriction zones
US10909860B2 (en) 2014-04-17 2021-02-02 SZ DJI Technology Co., Ltd. Flight control for flight-restricted regions
US11227501B2 (en) 2014-04-17 2022-01-18 SZ DJI Technology Co., Ltd. Flight control for flight-restricted regions
US11462116B2 (en) 2014-04-17 2022-10-04 SZ DJI Technology Co., Ltd. Polygon shaped vehicle restriction zones
US11482119B2 (en) 2014-04-17 2022-10-25 SZ DJI Technology Co., Ltd. Polygon shaped flight-restriction zones
US11810465B2 (en) 2014-04-17 2023-11-07 SZ DJI Technology Co., Ltd. Flight control for flight-restricted regions
US11482121B2 (en) 2015-03-31 2022-10-25 SZ DJI Technology Co., Ltd. Open platform for vehicle restricted region
US11488487B2 (en) 2015-03-31 2022-11-01 SZ DJI Technology Co., Ltd. Open platform for flight restricted region
EP3421189A1 (de) 2017-06-28 2019-01-02 Sick AG Verfahren zum überwachen einer maschine
CN113858213A (zh) * 2021-11-09 2021-12-31 南京埃斯顿机器人工程有限公司 一种用于目标同步的机器人动态轨迹规划方法
CN113858213B (zh) * 2021-11-09 2023-07-25 南京埃斯顿机器人工程有限公司 一种用于目标同步的机器人动态轨迹规划方法

Also Published As

Publication number Publication date
DE102009006256B4 (de) 2019-01-03
DE102009006256A1 (de) 2010-07-29

Similar Documents

Publication Publication Date Title
DE102009006256B4 (de) Verfahren zur Vermeidung von Kollisionen gesteuert beweglicher Teile einer Anlage
EP2550227B1 (de) Verfahren zum betreiben eines autonomen flurförderfahrzeugs
EP1366867B1 (de) Verfahren und Vorrichtung zum Vermeiden von Kollisionen zwischen Industrierobotern und anderen Objekten
DE4408982C1 (de) Autonomes Navigationssystem für einen mobilen Roboter oder Manipulator
DE102013109876A1 (de) Verfahren zur Steuerung eines redundanten Roboters
EP3313626A1 (de) Verfahren zum redundanzoptimierten planen eines betriebs eines mobilen roboters
DE102007059480B4 (de) Verfahren und Vorrichtung zur Posenüberwachung eines Manipulators
EP2378383B1 (de) Verfahren zum Betreiben eines holonomen/omnidirektionalen Flurförderfahrzeugs
WO2008113636A1 (de) Verfahren zur berechnung einer kollisionsvermeidenden trajektorie für ein fahrmanöver eines fahrzeugs
EP2353802A2 (de) Verfahren für eine kollisionsfreie Bahnplanung eines Industrieroboters
DE102021107453A1 (de) Schnelle roboterbewegungsoptimierung mit distanzfeld
DE112017002498T5 (de) Robotervorgang-auswertungseinrichtung, robotervorgang-auswertungsverfahren und robotersystem
WO2012052554A2 (de) Autonomes fahrzeug, zugehöriger anhänger und autonomes transportsystem
EP3556521B1 (de) Bremswegüberwachung einer kinematik
DE4425924A1 (de) Autonome mobile Einheit mit raumsparendem Manipulator und zugehöriges Steuerungsverfahren
DE102010046327A1 (de) Überwachung eines mobilen Manipulators
EP1906281A1 (de) Verfahren und System zur Auslegung und Überprüfung von Sicherheitsbereichen eines Roboters
DE102015223258A1 (de) Verfahren zum Bearbeiten der Oberfläche eines dreidimensionalen Objekts
DE10226140A1 (de) Verfahren und Vorrichtung zum Vermeiden von Kollisionen zwischen Industrierobotern und anderen Objekten
EP3166759A1 (de) Maschine und verfahren zum betreiben einer maschine
DE102012206952A1 (de) Verfahren und Vorrichtung zur Steuerung der Bewegung einer beweglichen Einheit im Raum
EP3569367B1 (de) Rechnergestütztes ermitteln einer bewegung einer vorrichtung
EP3225366A2 (de) Positionsüberwachung einer kinematik
EP0449039B1 (de) Verfahren zur Steuerung von Positioniersystemen
EP0907604A1 (de) Verfahren und anordnung zur vermeidung vom lastschwingungen bei einem drehbewegungen ausführenden hängelastbewegungsgerät

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 10708888

Country of ref document: EP

Kind code of ref document: A1

122 Ep: pct application non-entry in european phase

Ref document number: 10708888

Country of ref document: EP

Kind code of ref document: A1