METHOD AND DEVICE FOR DETERMINISTIC SAMPLING-BASED MOTION PLANNING

Abstract

A computer-implemented method for planning an optimized motion path. The optimized motion path is determined applying a sampling-based motion-planning algorithm depending on a sample node set including sample nodes. The sample nodes in the sample node set are deterministically selected from a configuration node set including all obstacle free nodes. The sample nodes are selected to optimize a given dispersion criterion. The dispersion criterion selects the sample nodes so that the largest uncovered area/space within the configuration node set is as small as possible.

Claims

1. A computer-implemented method for planning an optimized motion path, the method comprising the following steps: determining the optimized motion path is determined applying a sampling-based motion-planning algorithm depending on a sample node set including sample nodes, wherein the sample nodes in the sample node set are deterministically selected from a configuration node set including all obstacle free nodes, and the sample nodes are selected to optimize a given dispersion criterion, wherein the dispersion criterion selects the sample nodes so that a largest uncovered area/space within the configuration node set is minimized.

2. The method according to claim 1, wherein the sample nodes are chosen based on a dispersion matrix including dispersion values for each of the nodes of the configuration node set, wherein the nodes with the highest dispersion values are selected as the sample nodes.

3. The method according to claim 2, wherein the dispersion values are updated with a flood-fill algorithm.

4. The method according to claim 1 wherein the sample node set is determined as an obstacle-free node set.

5. The method according to claim 1, wherein the sampling-based motion-planning algorithm correspond to a PRM* algorithm or an FMT* algorithm.

6. The method according to claim 1, wherein the dispersion criterion depends on a given steering function which defines constraints of movements in the configuration space.

7. The method according to claim 1, wherein a start node and a goal node are added to the sample node set to obtain a configuration node set, wherein collision-free or allowed edges between two sample nodes of the configuration node set being less distanced than a given radius are determined, wherein a shortest motion path between the start node and the goal node is determined along the determined edges according to a given steering function which defines constraints of movements in the configuration space.

8. A method for operating an agent, comprising the following steps: determining an optimized motion path is determined applying a sampling-based motion-planning algorithm depending on a sample node set including sample nodes, wherein the sample nodes in the sample node set are deterministically selected from a configuration node set including all obstacle free nodes, and the sample nodes are selected to optimize a given dispersion criterion, wherein the dispersion criterion selects the sample nodes so that a largest uncovered area/space within the configuration node set is minimized; and operating the agent using the optimized motion path.

9. A device for planning an optimized motion path, wherein the device is configured to: determine the optimized motion path applying a sampling-based motion-planning algorithm depending on a sample node set including sample nodes, wherein the sample nodes in the sample node set are deterministically selected from a configuration node set including all obstacle free nodes; and select the sample nodes by optimizing a dispersion criterion, wherein the dispersion criterion selects the sample nodes so that a largest uncovered area/space within the configuration node set is minimized.

10. A non-transitory computer readable medium on which is stored a computer program for planning an optimized motion path, the computer program, when executed by a computer, causing the computer to perform the following steps: determining the optimized motion path is determined applying a sampling-based motion-planning algorithm depending on a sample node set including sample nodes, wherein the sample nodes in the sample node set are deterministically selected from a configuration node set including all obstacle free nodes, and the sample nodes are selected to optimize a given dispersion criterion, wherein the dispersion criterion selects the sample nodes so that a largest uncovered area/space within the configuration node set is minimized.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Example embodiments of the present invention are described in more detail in conjunction with the figures.

[0024] FIG. 1 schematically shows a system including a robot arm to be controlled by means of a planned motion trajectory, in accordance with an example embodiment of the present invention.

[0025] FIG. 2 schematically shows a system with an agent to be moved along a path generated by means of a motion-planning algorithm, in accordance with an example embodiment of the present invention.

[0026] FIG. 3 shows a flowchart for illustrating a motion-planning algorithm, in accordance with an example embodiment of the present invention.

[0027] FIG. 4 shows a flowchart for illustrating an algorithm for determining a sample node set, in accordance with an example embodiment of the present invention.

[0028] FIG. 5 shows a pseudocode for the motion planning algorithm of FIG. 3, in accordance with an example embodiment of the present invention.

[0029] FIG. 6 shows a pseudocode for the algorithm of FIG. 4, in accordance with an example embodiment of the present invention.

[0030] FIG. 7 shows a pseudocode for the algorithm to determine if a border node is affected, in accordance with an example embodiment of the present invention.

DETAILED DESCRIPTION OF EXAMPLE EMBODIMENTS

[0031] FIG. 1 schematically shows a robot arm 1 for handling objects in a partly occupied environment. The environment has static or dynamic obstacles 3 to which a collision shall be avoided. The robot arm 1 is configured to convey an object 2 in all space dimensions by gripping, moving and releasing the object 2 at predefined positions along a given trajectory.

[0032] The robot arm 1 is controlled by means of a control unit 10. The control unit 10 is coupled with a sensor system 11 capable of acquiring information about the actual state of the environment indicating blocked/occupied positions and free-to-move positions.

[0033] The trajectory planning algorithm, which is continuously performed in the control unit 10 considers the information about the environment and performs a motion planning algorithm to find an optimized trajectory of the motions of the robot arm 1. This allows to grip and release objects 2 in the environment and move them along an optimized path of the trajectory even if the environment dynamically changes. The method for planning the corresponding motion path is described in detail below.

[0034] FIG. 2 shows another example of a system wherein an agent, such as a self-moving robot 11, shall move through an environment from a start position S to a goal position G along an optimized path on a roadmap R. The roadmap R may change, e.g., due to “traffic”, dynamic objects or other environmental conditions so that a replanning of the motion path may be required regularly. The robot 11 has a control unit 15 coupled with a sensor system 16. The sensor system 16 is capable of sensing or obtaining environment data and to provide map data based thereon. The map data may be used to determine a configuration space. The control unit 15 is configured to perform a motion planning algorithm to continuously update an optimized motion path based on a changed roadmap.

[0035] FIG. 3 shows a flowchart illustrating a sampling-based motion-planning algorithm. Furthermore, FIG. 5 shows a pseudocode indicating the process flow for the motion planning algorithm of FIG. 3. The described motion-planning algorithm is a sampling-based algorithm and substantially is based on a PRM* algorithm. Other algorithms such as FMT* can be applied as well. The motion planning algorithms shall make use of a deterministic sample node set S so that it can also be verified and used for safety-critical applications.

[0036] Referring to the PRM* algorithm, the sampling-based motion-planning algorithm basically starts in step S1 with determining a number of node samples each corresponding to an allowed position in the configuration space through which an optimized path shall be determined. This sample node set S contains a set of sample nodes x which represent locations each of which is obstacle-free (and free of positions which are too close to an obstacle). The detailed method for determining the sample node set S is described below in conjunction with FIG. 4.

[0037] In step S2, once the sample node set S has been determined, the start node x.sub.start and the goal node x.sub.goal are added to the sample node set S to obtain a configuration node set V.

[0038] According to a standard PRM*, a shortest path from the start node x.sub.start to the goal node x.sub.goal through the nodes x of the configuration node set V is determined. As a first step the PRM* algorithm builds a graph (composed of the node set V and edge set E), see Lines 3-11 of the pseudocode of FIG. 5. The process starts applying a first (or current) node v of the configuration node set V in step S3 in a NEAR function NEAR (v, V, r). The NEAR function returns for the considered node v all nodes of the configuration node set V which are distanced closer than r.

[0039] In other words, a resulting node subset U is generated which comprises all nodes which are neighboring the considered node within a predefined radius r.

[0040] In step S4 the resulting nodes are appended to a node subset U.

[0041] In steps S5, for each node u in the node subset U, it is determined if a direct movement from the current node v to the respective node u in the node subset U is collision free or allowed.

[0042] In step S6, the resulting collision-free or allowed edges between the current node v and each of the subset nodes u are appended to an edge set E.

[0043] In step S7 it is checked if a further node v of the configuration node set V is to be considered for the edge determination of step S5. If positive (alternative: yes) the process is continued with step S3. If negative (alternative: no) the process is continued with step S8.

[0044] In step S8 according to a standard function SHORTESTPATH (x.sub.start, x.sub.goal, (V,E)) the shortest motion path between the start node x.sub.start and the goal node X.sub.goal through the nodes of the configuration space using the collision-free edges determined by the edge set E is determined. The determination of the shortest path from the start node x.sub.start to the goal node x.sub.goal is straight-forward and not explained in detail herein. The shortest path procedure is generic. Standard algorithms that can be used are described in Hart, P. E. et al., “A formal basis for the heuristic determination of minimum cost paths”, IEEE transactions on Systems Science and Cybernetics, 4(2), 100-107 and in Dijkstra, E. W., “A note on two problems in connexion with graphs.” Numerische Mathematik 1.1 (1959): 269-271.

[0045] Referring to FIG. 4 and the pseudocode of FIG. 6, the step S2 for determining the sample node set S is described in more detail.

[0046] Generally, for deterministic sampling-based motion planning algorithm, it may be a goal to optimize a dispersion criterion. The basic idea for the described algorithm is to place sample nodes x into the sample node set S so that the largest uncovered area/space is as small as possible. Basically, the dispersion may be defined as the volume of the largest empty “sub-Riemannian ball” for non-holonomic robotic systems around the considered sample node or a hyperdimensional sphere for Euclidean spaces.

[0047] In step S11 a configuration space X is initialized which is approximated and discretized into a fine grid of N.sub.cs equidistant nodes x wherein the distance between neighboring nodes x can be equal or different per dimension. The configuration space X is a representation of the environment an agent or a part of an agent can move. A node is a representation of a position in this environmental space. An initial sample node x is chosen. Furthermore, an empty sample node set S is initialized.

[0048] A dispersion matrix D is initialized which keeps track of the minimum distance to either the border or the closest neighboring node for each node. The elements of the dispersion matrix (dispersion values) are each associated to one node in the configuration space X thereby having the same size and dimension of the configuration space X. Each element of the dispersion matrix is initially set to a high value which e.g. may be at least the full length of the corresponding dimension of the environmental space. In other words, each element of the dispersion matrix corresponds to the dispersion value of the related grid cell (the one used to approximate the configuration space).

[0049] The dispersion value for each sample node may be defined as a metric of a portion of the sample node set S that contains no other sample nodes. The metric may be measured using a steering function.

[0050] If it is possible to compute the distance to the border quickly, the dispersion matrix D can be initialized with the distance to the border for each node, otherwise the dispersion tensor D is initialized with ∞.

[0051] In step S12, the algorithm iteratively adds new optimized sample nodes x (up to m.sub.samples<N.sub.cs).

[0052] At each iteration, it is checked in step S13 if the initial selected sample node x or (for further iterations) the previous generated sample node x does not affect the border of the configuration space X (Function: AFFECTEDBORDERPOINT).

[0053] FIG. 7 shows a pseudocode for an algorithm for determining if the sample node x affects a border node. The node c referred to in the dispersion matrix which has a smaller distance to the sample node x than Dc or which affect a border point is correspondingly returned.

[0054] If the sample node x affects the border of the configuration space X (alternative: yes), the process is continued with step S14, otherwise (alternative: no) the process is continued with steps S15.

[0055] In step S14 the function AFFECTEDBORDERPOINT returns a node b (c in the pseudocode of FIG. 7) as an affected border node and the dispersion matrix is updated with the border point b (Function: UPDATEDISTANCEMATRIX). The dist function is the length of the path connecting two states (in this case c and x).

[0056] It is noted that this length may be different from the Euclidean distance. Instead, the length corresponds to a path which can be computed considering a given steering function, thus the nonholonomic constraints of the robot. The steering function includes the constraints of the movement of real world objects. For instance, a four-wheeled robot may be not allowed to turn on the spot, therefore its path between two points can be longer than the Euclidean distance between these points.

[0057] For a given node, the dispersion matrix D may be updated with a flood-fill algorithm. A flood-fill algorithm is often used to compute a set of connected cells. Herein, with the flood-fill algorithm it is determined the distance field between the cells. Ideally, the flood-fill algorithm connects a given cell to its neighbors and updates the cost to reach the latter. Also in this case a Dijkstra Algorithm to visit all the cells of a grid can be used, as described in Dijkstra, E. W., “A note on two problems in connexion with graphs.” Numerische Mathematik 1.1 (1959): 269-271.

[0058] In step S15, the current sample node x is added to the sample node set S and used to update the dispersion matrix D in step S16.

[0059] In step S17 a next current node x is determined by optimizing x=argmax.sub.c D.sub.c where Dc is a dispersion value of the element of the dispersion matrix. This means that the node c of the element of the dispersion matrix D is determined which has the highest value D.sub.c.

[0060] In step S18 it is checked if the requested number m.sub.samples of sample nodes x has been reached. If positive (alternative: yes), the process will return the sample node set S to the algorithm of FIG. 3 (step S19). Otherwise (alternative: no) the process is continued with step S12.

[0061] In an alternative embodiment, the iteration to build the sample node set S can also be ended once a defined dispersion value has been reached. Here, the maximum dispersion value in the dispersion matrix can be checked. If this is lower and equal than a desired value, the method can be halted

[0062] The above algorithm for motion-planning ensures that an appropriate path can be determined if an allowed path through the environment exists. If a minimum clearance (distance between obstacles or minimum width of corridors or the like) is known in advance, the algorithm can be used to find a motion path with desired clearance. To achieve this, the defined maximum dispersion may be set to 2*(desired clearance). Further, the algorithm clearly indicates if an appropriate motion path through the environment does not exist.