Method for collision-free motion planning

Abstract

A method and corresponding apparatus for collision-free motion planning of a first manipulator in a first working space and a second manipulator in a second working space, wherein the first and second working spaces at least partially overlap. The method includes the steps of importing a first dynamic roadmap for a first configuration space of the first manipulator, wherein the first dynamic roadmap includes a first search graph and a first mapping between the first working space and the first search graph, and importing a second dynamic roadmap for a second configuration space of the second manipulator, wherein the second dynamic roadmap includes a second search graph and a second mapping between the second working space and the second search graph. Furthermore, the motion of the first manipulator and the second manipulator are coordinated based on the first dynamic roadmap and the second dynamic roadmap.

Claims

1. A method for collision-free motion planning of a first manipulator in a first working space and a second manipulator in a second working space, the first working space and the second working space at least partially overlapping, the method comprising: importing a first dynamic roadmap for a first configuration space of the first manipulator, wherein the first dynamic roadmap includes a first search graph and a first mapping between the first working space and the first search graph; importing a second dynamic roadmap for a second configuration space of the second manipulator, wherein the second dynamic roadmap includes a second search graph and a second mapping between the second working space and the second search graph; determining a first motion path for the first manipulator based on the first dynamic roadmap; determining a second motion path for the second manipulator based on the second dynamic roadmap; linking, to a coordination graph, at least one of (i) the first motion path and the second motion path, (ii) the first motion path and the second search graph, and (iii) the first search graph and the second search graph; and coordinating the motion of the first manipulator and the second manipulator based on the first dynamic roadmap and the second dynamic roadmap using the coordination graph.

2. The method of claim 1, wherein coordinating the motion includes checking for collisions between the first manipulator and the second manipulator by using the first mapping and the second mapping.

3. The method of claim 1, wherein the first motion path is determined independently of the second motion path.

4. The method of claim 1, wherein: the first motion path defines an occupied area in the first working space, and the second motion path is determined outside the occupied area.

5. The method of claim 1, wherein: for determining the first motion path for the first manipulator, a collision with the second manipulator is ignored, and for determining the second motion path for the second manipulator, a collision with the first manipulator is ignored.

6. The method of claim 1, wherein: the first motion path for the first manipulator is a first linear graph, the second motion path for the second manipulator is a second linear graph, and the coordination graph includes the full graph product of the first linear graph and the second linear graph.

7. The method of claim 1, wherein: the first motion path for the first manipulator is a first linear graph, and the coordination graph is the Cartesian product of the first linear graph and the second search graph.

8. The method of claim 1, wherein the coordination graph is the Cartesian product of the first search graph and the second search graph.

9. A method for collision-free motion planning of a first manipulator in a first working space and a second manipulator in a second working space, the first working space and the second working space at least partially overlapping, the method comprising: importing a first dynamic roadmap for a first configuration space of the first manipulator, wherein the first dynamic roadmap includes a first search graph and a first mapping between the first working space and the first search graph; importing a second dynamic roadmap for a second configuration space of the second manipulator, wherein the second dynamic roadmap includes a second search graph and a second mapping between the second working space and the second search graph; and coordinating the motion of the first manipulator and the second manipulator based on the first dynamic roadmap and the second dynamic roadmap, wherein at least one of the first dynamic roadmap and the second dynamic roadmap is an extended dynamic roadmap including an extended search graph with nodes comprising additional time information, and wherein coordinating the motion of the first manipulator and the second manipulator is further based on the extended dynamic roadmap.

10. The method of claim 9, wherein coordinating the motion includes checking for collisions between the first manipulator and the second manipulator by using the first mapping and the second mapping.

11. An apparatus comprising: a first manipulator in a first working space; a second manipulator in a second working space, wherein the first working space and the second working space at least partially overlap; and a control unit for dynamic motion planning, wherein the control unit is configured to perform: importing a first dynamic roadmap for a first configuration space of the first manipulator, wherein the first dynamic roadmap includes a first search graph and a first mapping between the first working space and the first search graph; importing a second dynamic roadmap for a second configuration space of the second manipulator, wherein the second dynamic roadmap includes a second search graph and a second mapping between the second working space and the second search graph; determining a first motion path for the first manipulator based on the first dynamic roadmap; determining a second motion path for the second manipulator based on the second dynamic roadmap; linking, to a coordination graph, at least one of (i) the first motion path and the second motion path, (ii) the first motion path and the second search graph, and (iii) the first search graph and the second search graph; and coordinating the motion of the first manipulator and the second manipulator based on the first dynamic roadmap and the second dynamic roadmap using the coordination graph.

12. The apparatus of claim 11, wherein coordinating the motion includes checking for collisions between the first manipulator and the second manipulator by using the first mapping and the second mapping.

13. The apparatus of claim 11, wherein the first motion path is determined independently of the second motion path.

14. The apparatus of claim 11, wherein: the first motion path defines an occupied area in the first working space, and the second motion path is determined outside the occupied area.

15. The apparatus of claim 11, wherein: for determining the first motion path for the first manipulator, a collision with the second manipulator is ignored, and for determining the second motion path for the second manipulator, a collision with the first manipulator is ignored.

16. The apparatus of claim 11, wherein: the first motion path for the first manipulator is a first linear graph, the second motion path for the second manipulator is a second linear graph, and the coordination graph includes the full graph product of the first linear graph and the second linear graph.

17. The apparatus of claim 11, wherein: the first motion path for the first manipulator is a first linear graph, and the coordination graph is the Cartesian product of the first linear graph and the second search graph.

18. The apparatus of claim 11, wherein the coordination graph is the Cartesian product of the first search graph and the second search graph.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) Exemplary embodiments of the invention are explained in more detail in the following description and are represented in the drawings, in which:

(2) FIG. 1 shows an exemplary embodiment of an apparatus according to the present disclosure,

(3) FIG. 2 shows a block diagram of an exemplary embodiment of a method for collision-free motion planning,

(4) FIG. 3 shows a block diagram of another exemplary embodiment of a method for collision-free motion planning, and

(5) FIG. 4 shows embodiments of various coordination graphs.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

(6) FIG. 1 shows a first exemplary embodiment of an apparatus according to the present disclosure. The apparatus is designated as a whole by reference numeral 10.

(7) Apparatus 10 according to this embodiment is a robot system with a first manipulator 12 and a second manipulator 14. The first manipulator 12 and the second manipulator 14 are independent robots that are controlled by a common control system (not shown here). The first and the second manipulator 12, 14 each have seven axes of rotation over which they can move in space. The robot system thus comprises 14 degrees of freedom.

(8) Each manipulator is assigned a working space 16, 18 in which it can move. The shape and size of the working space is essentially determined by the number and arrangement of the robot's rotary axes as well as their form of motion. In addition, the working space can also be restricted to a certain form, for example by setting software limits for the control system. The working space is not limited to the cuboid shape shown here, but can also be a cylinder, a sphere or a polyhedron in other embodiments.

(9) The first working space 16 of the first manipulator 12 overlaps the second working space 18 of the second manipulator 14. The common working space 20 is indicated by a dashed line in FIG. 1. Both the first and the second manipulators 12, 14 can be located in the common working space 20 and a collision between the two manipulators 12, 14 may occur. Consequently, the motion of the two manipulators 12, 14 in relation to each other must be coordinated in the common working space 20. For this purpose, the common control system of the two manipulators 12, 14 carries out the method according to the invention, which is explained in detail in relation to the following figures.

(10) It goes without saying that the scenario presented here can only be understood as an example. In other embodiments, the robot system can also have a single robot with two manipulators arranged on it. It is also conceivable that the individual manipulators or robots may have independent control systems as long as joint coordination of the control systems is possible. This can be made possible, for example, via suitable data communication interfaces between the control systems. Likewise, it does not have to be two manipulators of the same type that interact with each other. In other embodiments, the manipulators may be of different design. The working spaces of the two manipulators can also be different in size and shape. Furthermore, the disclosed apparatus and method are not limited to two manipulators. In other embodiments, the robot system may also include other manipulators that interact with the first and/or second manipulator. In addition, it goes without saying that when coordinating the manipulators, it is also possible to take into account effectors such as a tool or a grab as well as any loads arranged on them.

(11) FIG. 2 shows in a block diagram an example of a method according to the present disclosure. The method is described here in its entirety with the reference numeral 100 and explained in more detail below in connection with the exemplary embodiment of the apparatus according to FIG. 1.

(12) A first method step 110 comprises importing a first dynamic roadmap 22 for a first configuration space of the first manipulator 12, wherein the first dynamic roadmap includes a first search graph 24 and a first mapping 26 between the first working space 16 and the first search graph 24.

(13) A second method step 120 comprises importing a second dynamic roadmap for a second configuration space of the second manipulator 14, wherein the second dynamic roadmap 28 includes a second search graph 30 and a second mapping 32 between the second working space 18 and the second search graph 30.

(14) The respective configuration space of a manipulator describes the spatial state of the manipulator with a minimum set of independent coordinates. The configuration space thus comprises a set of attainable positions of the manipulator in (three-dimensional) space, whereby the elements of the manipulator are regarded as stiff bodies. In general, the minimum number of coordinates required to describe a system is equal to the number of degrees of freedom.

(15) The configuration space is therefore usually multidimensional and includes all possible states that a manipulator can assume. A state is also referred to as a configuration.

(16) A dynamic roadmap (DRM) includes a graph G=(V,E) in the configuration space and a mapping ϕ. The graph has nodes V and edges E, where the nodes represent a defined configuration and the edges represent a motion from a first configuration to a second configuration. The graph of a dynamic roadmap is formed from random samples in the configuration space, whereby in the case of a DRM an empty environment, i.e. an environment without obstacles, is assumed. The assumption of an empty environment allows the samples to be taken from an equal distribution and only collisions with themselves have to be considered.

(17) Furthermore, the dynamic roadmap includes a mapping ϕ from the working space to the configuration space (or vice versa ϕ.sup.−1). The mapping can be stored in the form of a lookup table. To create the lookup table, the working space is divided into a set of volume elements (so-called voxel) and for each volume element w∈W in the working space a mapping ϕ.sub.v:W.fwdarw.V and ϕ.sub.e:W.fwdarw.E is determined for the nodes and edges of the graph G=(V,E) in the configuration space. If a volume element is occupied by an obstacle, the mappings specify the colliding configurations and motions in the search graph. The tabular representation thus enables a space in the working space to be easily assigned to a number of nodes and edges in the configuration space.

(18) The dynamic roadmap for a manipulator can be generated according to the principle described above independently of the other manipulators of the system. In particular, the dynamic roadmap can be pre-calculated, i.e. the dynamic roadmap for a manipulator or for an independent robot can be calculated and stored in an offline phase. In an online phase, the dynamic roadmap is imported and the search graph is used to determine a motion path. When the motion path is determined, parts of the graph are dynamically invalidated using the mapping. This is done by mapping dynamic obstacles, i.e. obstacles that have not been taken into account in the pre-calculation of the dynamic roadmap, to edges and nodes in the configuration space online via the tabular. The invalidated nodes and edges are then not taken into account for subsequent path planning. By the pre-calculation and the dynamic invalidation of the search graph new plans can be realized within less than 100 milliseconds.

(19) Using the two dynamic roadmaps, the motion of the first manipulator and the second manipulator are coordinated in process step 130. Four different approaches are explained below.

(20) A first approach, hereinafter referred to as fixed separation, can be classified as prioritized planning. The approach pursues the idea of strictly separating the working space of the two manipulators so that no collisions can occur independently of time. For this purpose, the first manipulator is prioritized and a motion path u=[u.sub.0, u.sub.1, . . . , u.sub.n] is determined based on the first dynamic roadmap, whereby the second manipulator is ignored. The working space W.sub.M1(u), which is occupied by the first manipulator when moving along the path u, can be represented as follows:

(21) $W_{M1}\left(u\right)=\stackrel{n}{\underset{i=0}{.Math.}}{\varphi }_{v,M1}^{-1}\left(u_i\right).Math.\stackrel{n-1}{\underset{i=0}{.Math.}}{\varphi }_{e,M1}^{-1}\left(u_i,u_{i+1}\right)$

(22) Apart from the mappings of a start and an end point and their connection to the graph of the dynamic roadmap, all other mappings are pre-calculated and thus directly available. After the configurations for the start and end point as well as their connections to the roadmap have been determined, a motion path for the second manipulator can be determined, whereby only the remaining configuration space W.sub.M2=W/W.sub.M1(u) is considered.

(23) Referring to FIGS. 3 and 4, the further three approaches for coordinating the motion of the first manipulator and the second manipulator, all of which comprise a further method step, are explained in more detail below.

(24) FIG. 3 shows the method from FIG. 2 with an additional method step 121 between steps 120 and 130. Apart from this, the method according to FIG. 3 is identical to the method according to FIG. 2.

(25) The further method step 121 comprises generating a coordination graph based on the first dynamic roadmap and the second dynamic roadmap, based on which motion planning of the first manipulator and the second manipulator is performed. The approaches differ from each other in the generation of this coordination graph.

(26) The second approach, hereinafter also referred to as path coordination, is a less restrictive approach compared to the strict separation of the working spaces applied in fixed separation coordination. The second approach creates two independent motion paths for the first and second manipulators and then coordinates the motion along these paths so that no collision occurs.

(27) Using dynamic roadmaps as a basis for path planning makes it possible for coordination to test whether common intersections of the motion paths are given. This is done on the basis of the mappings ϕ or ϕ.sup.−1. A costly collision check can thus be avoided.

(28) The result of the independent planning is a first motion path u=[u.sub.0, u.sub.1, . . . , u.sub.n] for the first manipulator and a second motion path v=[v.sub.0,v.sub.1, . . . , v.sub.m] for the second manipulator. Both paths can be interpreted as linear graphs PG.sub.M1 and PG.sub.M2. The coordination graph CG.sub.PC is then the full graph product CG.sub.PC=PG.sub.M1PG.sub.M2. Such a coordination graph CG.sub.PC for path coordination is indicated graphically in FIG. 4 with reference numeral 121a.

(29) Each node in CG.sub.PC is a pair (u.sub.i, v.sub.j) with i=0, . . . , n and j=0, . . . , m. Edges to (u.sub.i+1, v.sub.j), (u.sub.i, v.sub.j+1) and (u.sub.i+1, v.sub.j+1) correspond to motions of either one manipulator alone or a parallel motion of both manipulators.

(30) A node (u.sub.i, v.sub.j) of the coordination graph CG.sub.PC is valid when
ϕ.sub.v,M1.sup.−1(u.sub.i)∩ϕ.sub.v,M2.sup.−1(v.sub.j)=Ø
is satisfied.

(31) A parallel motion of both manipulators is valid when
(ϕ.sub.v,M1.sup.−1(u.sub.i)∪ϕ.sub.e,M1.sup.−1(u.sub.i,u.sub.i+1)∪ϕ.sub.v,M1.sup.−1(u.sub.i+1))∩(ϕ.sub.v,M2.sup.−1(v.sub.j)∪ϕ.sub.e,M2.sup.−1(v.sub.j,v.sub.j+1)∪ϕ.sub.v,M2.sup.−1(v.sub.i+1))=Ø
is satisfied.

(32) For the motion of a single arm it is sufficient to test for the following conditions:
ϕ.sub.e,M1.sup.−1(u.sub.i,u.sub.i+1)∩ϕ.sub.v,M2.sup.−1(v.sub.j)=Ø
or
ϕ.sub.v,M1.sup.−1(u.sub.i)∩ϕ.sub.e,M2.sup.−1(v.sub.j,v.sub.j+1)=Ø

(33) As soon as the coordination graph has been generated, coordination can take place by searching for the shortest valid path in the coordination graph. In a preferred embodiment, parallel motion of the arms can be preferred over successive motion of individual arms by weighting the latter negatively in the graph search.

(34) In path coordination, no time parameterization is performed for the respective paths, nor is path coordination dependent on such a time parameterization. A time parameterization, which takes into account the permissible velocities and accelerations, can then be performed on the coordinated path. If a motion path of the first or second manipulator has fewer nodes than the motion path of the respective other manipulator, then one manipulator must in any case wait at a time while the other manipulator is in motion.

(35) The third approach, also referred to as dynamic separation, is explained in more detail below.

(36) In dynamic separation coordination, a coordination graph based on the first dynamic roadmap and the second dynamic roadmap is generated first. As with fixed separation, the first manipulator is prioritized and a valid motion path u=[u.sub.0, u.sub.1, . . . , u.sub.n] is determined based on the first dynamic roadmap, whereby the second manipulator is ignored.

(37) In contrast to fixed separation coordination, however, not the entire working space W.sub.M1(u) is blocked during motion planning for the second manipulator. Rather, the motion of the first manipulator is coordinated along its motion path u, while the motion of the second manipulator is coordinated along the search graph of the second dynamic roadmap.

(38) In contrast to path coordination, the coordination graph for dynamic separation coordination is formed from the Cartesian product of the linear graph of path u with the second search graph CG.sub.GS=PG.sub.M1□G.sub.M2. An illustration of such a coordination graph is depicted in FIG. 4 with reference numeral 121b. Using the Cartesian product produces a graph with fewer nodes and edges than using a Tensor product or a full graph product and is therefore less complex. This can be compensated by performing a path coordination subsequent to the dynamic separation coordination.

(39) Each node of the coordination graph CG.sub.GS corresponds to a pair of points (u.sub.i,v), where u.sub.i is an element of the graph PG.sub.M1 and v is a node of the search graph G.sub.M2 of the second dynamic roadmap.

(40) A node is valid when ϕ.sub.v,M1.sup.−1(u.sub.i)∩ϕ.sub.v,M2.sup.−1(v)=Ø is satisfied.

(41) An edge from (u.sub.i,v) to (u.sub.i+1, v) is valid when ϕ.sub.e,M1.sup.−1(u.sub.i, u.sub.i+1)∩ϕ.sub.v,M2.sup.−1(v)=Ø is satisfied.

(42) An edge from (u.sub.i, v′) is valid when ϕ.sub.v,M1.sup.−1(u.sub.i)∩ϕ.sub.e,M2.sup.−1(v, v′)=Ø is satisfied.

(43) The motion coordination of the first manipulator and the second manipulator takes place as before with the path coordination on the basis of the coordination graph by determining the shortest permissible path in a manner known per se. As with path coordination, the majority of the mappings are already pre-calculated and stored in tabular form.

(44) The fourth approach to coordination is called graph coordination. Here, both manipulators are coordinated along their respective search graphs G.sub.M1 and G.sub.M2 and possible overlaps are determined. Similar to dynamic separation, in graph coordination the coordination graph is formed from a Cartesian product.

(45) The coordination graph for graph coordination is the Cartesian product of the first search graph and the second search graph CG.sub.GC=G.sub.M1□G.sub.M2 (FIG. 4, 121c). As before, coordination is then carried out using the coordination graph by searching for the shortest valid path in the coordination graph.

(46) With graph coordination the complexity of the coordination graph CG.sub.GC may become very high with larger dynamic roadmaps and thus make a graph search prohibitively expensive. Therefore, in other embodiments, suboptimal search algorithms can be used for compensation.

(47) The above approaches can be further optimized by replacing at least one of the dynamic roadmaps with a dynamic roadmap having additional time information. This would have the advantage that time aspects can, at least partially, be taken into account during the planning phase. Depending on the scenario, the path search can be further improved and, if necessary, accelerated.

(48) While the above-mentioned approaches have each been listed as alternatives, the individual approaches can also be combined in other embodiments. For example, a path coordination can be performed subsequent to a dynamic separation coordination. Thereby, it is not necessary to follow-up with a different coordination approach each time. In other embodiments, the individual approaches can also be linked dynamically. For example, it is conceivable that in one scenario a first approach is carried out and in another scenario another approach is carried out. Thus, the approaches can be linked arbitrarily, whereby the respective selection can depend on external factors.

(49) It also goes without saying that the coordination procedures described in relation to two manipulators can also be easily extended to multi-arm systems.