Distributed path planning method based on double ellipsoids

Abstract

The invention discloses a distributed path planning method based on double ellipsoids, and belongs to the field of artificial intelligence planning, and the method comprises the steps: introducing an ellipsoid generation model in a two-dimensional working space W with disordered obstacles of a modular robot system, enabling the ellipsoid generation model to grow two ellipsoids CPT *, and generating nodes connected with the ellipsoids, obtaining a bidirectional ellipsoid model of the bidirectional ellipsoid tree; outputting node path candidates including the optimal formation path according to the upper bound and the lower bound of the node transverse cost J (phi); and further converting the candidate node paths into a plurality of quadratic optimization problems with convex constraints, and solving an optimal formation path. According to the distributed path planning method based on the double ellipsoids provided by the invention, the probability of extending and connecting the ellipsoids is increased; in a two-dimensional working space, the bidirectional ellipsoid model is faster than CPT * and COM, and is greatly improved compared with a multi-robot formation constraint optimization method.

Description

Technical Field

[0001] The present invention relates to the field of artificial intelligence planning technology, and in particular to a distributed path planning method based on a double ellipsoid. Background Art

[0002] Reconfigurable modular robotic systems include multiple robots that can be connected to each other and formed into various arrangements to achieve new functions. In tasks such as collaboratively moving heavy payloads or jointly rejecting excessive interference, multiple robots in a team are connected to each other to form a rigid formation and may change their arrangement at certain locations along the path when necessary.

[0003] Sampling-based Optimal Path Planning (SBP) methods are state-of-the-art approaches for path generation in high-dimensional spaces. Classical SBPs may not be applicable to configuration spaces embedded with formation arrangements because the choice of arrangement is discrete in nature and cannot be interpolated. In fact, in contrast to monolithic robots, reconfigurable modular robotic systems involve multiple collision-free pose-configuration spaces because each formation arrangement has its own unique geometry. Direct deployment of existing SBPs, such as RRT*, will produce samples in these collision-free pose-configuration spaces. A straight-line interpolation between two sampled nodes may require reconfiguration to different formation arrangements. The feasibility of the reconfiguration depends on whether there is a region along the straight-line interpolation that is collision-free for both arrangements. Feasibility may not be easy to determine and the resulting path may involve unnecessary transformations.

[0004] Therefore, the connectivity in the workspace can be exploited in a branch-and-bound manner to reduce the search area in the configuration space. Alonso formulates multi-robot formation planning as a constrained optimization problem and shows its superior performance through simulation, where the constraints are constructed by sampling ellipsoids in the workspace. Summary of the invention

[0005] The purpose of the present invention is to provide a distributed path planning method based on a double ellipsoid to solve the problems mentioned in the background technology.

[0006] To achieve the above object, the present invention provides a distributed path planning method based on a double ellipsoid, comprising the following steps:

[0007] S1. In the two-dimensional workspace W of the modular robot system with cluttered obstacles, an ellipsoid generation model is introduced. The ellipsoid generation model takes the low-dimensional encoding of the ellipsoid and the obstacles near the modular robot as input, so that the ellipsoid generation model grows two ellipsoids and generates nodes connecting the ellipsoids, thereby obtaining a bidirectional ellipsoid model.

[0008] S2. Output the node path candidate containing the optimal formation path according to the upper and lower bounds of the node lateral cost J(φ);

[0009] S3. The candidate node paths are further transformed into multiple quadratic optimization problems with convex constraints to solve the optimal formation path.

[0010] Preferably, the process of establishing the bidirectional ellipsoid model in S1 is as follows:

[0011] The bidirectional ellipsoid model consists of two ellipsoids T a and T b , respectively, from the root vertex and the target vertex. In the bidirectional ellipsoid model, in the i-th iteration of the growing bidirectional ellipsoid model, a new ellipsoid P is sampled from the collision-free workspace. i To carry out T a The new ellipsoid P i With the existing ellipsoid {P l} l =1,…,i-1 intersect, and get a new node {V k+j} j =1,…,n i , where n is the number of new nodes in the i-th iteration;

[0012] New nodes are connected to T by selecting their parent node a , new node According to the distance limit, it is connected to one of its neighbor nodes as its parent node. The distance between the nodes of the bidirectional ellipsoid model is represented by the upper and lower bounds. and its neighbor nodes The upper and lower bounds are estimated using and express:

[0013]

[0014] Among them, α is the weight of the rotation distance;

[0015] The path Φcan be estimated as follows:

[0016]

[0017] for Each node in First, get its a The set N of adjacent vertices in ; then, the bidirectional ellipsoid model planner converts each After that, the parent node of each newly sampled node is selected and the estimated or The range of and N, get the sorted list, for the sorted list The upper and lower bounds are as follows:

[0018]

[0019] set up and Indicates about For all sorted lists and The process "ChooseParent" starts to select the node that can achieve the best lower bound among all the newly sampled nodes. Connect the newly sampled nodes to T in sequence a , the node is added to T a , will also be added to the neighbor set of other newly sampled nodes. Assuming that k+J nodes are added to the bidirectional ellipsoid model, andj≠Jsum and j≠J, Added to the neighbor sets of other newly sampled nodes, the same process continues until all new nodes are connected to the bidirectional ellipsoid model;

[0020] All new nodes added to the bidirectional ellipsoid model are rewired. In the i-th iteration, the planner tries to connect the nodes to Connect to the opposing tree T b The vertices in the set are only selected if the cost of the resulting solution is lower than the cost of the candidate set. and C * The node path found from the previous iteration. If the node exists in T Its Any node in is close enough so that the ellipsoid can be partially covered by the window workspace; then, CPGM#2 is used to sample and connect multiple ellipsoids to generate new nodes connect and Finally, with the new node Find the connection between them.

[0021] Preferably, the ellipsoid generation method is:

[0022] The obstacles are first encoded into a low-dimensional representation using a contractive autoencoder (CAE). However, it is not feasible to consider all obstacles in the workspace. In addition, only obstacles near the ellipsoid can determine the boundary of the ellipsoid. It is reasonable to design a window workspace ω with sizes l and d, i.e. in and In each iteration, ω is placed on the region of interest where expansion and connection should occur. The size of ω is fixed, making the training of CPGM easier. The position of ω is then determined when growing the bidirectional ellipsoid model in each iteration. The fixed-size window covers the local region of interest and is a subset of the workspace W. Let O represent the point cloud of obstacles within ω.

[0023] In the second step, the conditional mixture density network samples an ellipsoid position in ω, except for obstacles in ω Encoding In addition, CMDN also takes the ellipsoid that the sampled ellipsoid intersects with as input;

[0024] The third step creates a maximized ellipsoid by iterating between updating the ellipsoid hyperplanes and maximizing the ellipsoid within the area enclosed by the hyperplanes; these hyperplanes separate the ellipsoid (or the sampled ellipsoid positions in the first iteration) from the obstacle boundary and are updated as the ellipsoid grows.

[0025] Preferably, assuming is a path from Ξ, which is actually a node sequence; there is a formation configuration in each node, and the connection between nodes is guaranteed; therefore, each node path will generate an optimization problem to solve an optimal formation path φ(τ),τ∈[0,1]; then compare all the optimal formation paths and output the optimal formation path;

[0026] In Φ * In the enumeration of the formation arrangement, each combination brings a constrained convex optimization problem; assuming q i Yes i The formation configuration contained in makes Aq i ∈φ i , due to φ i and φ i+1 The arrangement of each segment between is fixed. i Continuous entries of i The optimization problem is stated as follows: i Convex constraints It can be determined by the relationship Aq i ∈φ i Get, by solving Obtain the key optimal continuous formation state

[0027]

[0028] in,

[0029] By comparing J(Φ) for all possible node paths and formation permutations, the optimal node path can be found, and Let τ1=0,Φτcard(IΦ)=1,for all values ​​less than and τ i <τ i+1 By assigning these key continuous formation states to points along the formation path Q, a formation path Q* can be obtained such that By filling the states between these key optimal states, the path segments between consecutive key formation states are constructed by a straight line in the pose configuration space, Q(τ i ) and Q(τ i+1 ) is given by:

[0030]

[0031] where τ∈(τ i ,τ i+1 ].

[0032] Preferably, each node is generated through four steps:

[0033] a. Sample a position of the ellipsoid;

[0034] b. Generate an ellipsoid with the maximum size in the collision-free workspace;

[0035] c. Check whether the sampled ellipsoid can intersect with the existing ellipsoid;

[0036] d. Get the intersection points and add them as nodes to the bidirectional ellipsoid model.

[0037] Preferably, given a node, the neighbor nodes of the node are defined as vertices in a spherical region with a radius of r and centered on the node.

[0038] Therefore, the present invention adopts the above-mentioned distributed path planning method based on double ellipsoids, and the sampling of polygons is based on two valid cpgms, which increases the probability of extending and connecting ellipsoids; in a two-dimensional workspace, the bidirectional ellipsoid model is faster than CPT* and COM, and is greatly improved over the constrained optimization method of multi-robot formations.

[0039] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 A flow chart of a distributed path planning method based on double ellipsoids of the present invention;

[0041] Figure 2 is a topological diagram of a bidirectional ellipsoid model in an embodiment of the present invention;

[0042] Figure 3A schematic diagram of estimating boundaries between nodes according to an embodiment of the present invention;

[0043] Figure 4 Schematic diagram of the structure of CPGM in an embodiment of the present invention. DETAILED DESCRIPTION

[0044] The following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0045] See also Figure 1-Figure 4 , a distributed path planning method based on double ellipsoids, comprising the following steps:

[0046] S1. In the two-dimensional workspace W of the modular robot system with cluttered obstacles, an ellipsoid generation model is introduced. The ellipsoid generation model takes the low-dimensional encoding of the ellipsoid and obstacles near the modular robot as input, so that the ellipsoid generation model grows two ellipsoids and generates nodes connecting the ellipsoids, and obtains a bidirectional ellipsoid model.

[0047] The process of establishing the bidirectional ellipsoid model is as follows:

[0048] The bidirectional ellipsoid model consists of two ellipsoids T a and T b , respectively, from the root vertex and the target vertex. In the bidirectional ellipsoid model, in the i-th iteration of the growing bidirectional ellipsoid model, a new ellipsoid P is sampled from the collision-free workspace. i To carry out T a The new ellipsoid P i With the existing ellipsoid {P l} l =1,…,i-1 intersect, and get a new node {V k+j} j =1,…,n i , where n is the number of new nodes in the i-th iteration.

[0049] New nodes are connected to T by selecting their parent node a , new node According to the distance limit, it is connected to one of its neighbor nodes as its parent node. The distance between the nodes of the bidirectional ellipsoid model is represented by the upper and lower bounds. and its neighbor nodes The upper and lower bounds are estimated using and express:

[0050]

[0051] Among them, α is the weight of the rotation distance.

[0052] The path Φcan be estimated as follows:

[0053]

[0054] for Each node in First, get its a The set N of adjacent vertices in ; then, the bidirectional ellipsoid model planner converts each After that, the parent node of each newly sampled node is selected and the estimated or The range of and N, get the sorted list, for the sorted list The upper and lower bounds are as follows:

[0055]

[0056] set up and G k+j Indicates about For all sorted lists and The process "ChooseParent" starts to select the node that can achieve the best lower bound among all the newly sampled nodes. Connect the newly sampled nodes to T in sequence a , the node is added to T a , will also be added to the neighbor set of other newly sampled nodes. Assuming that k+J nodes are added to the bidirectional ellipsoid model, andj≠Jsum and j≠J, The neighbor sets of other newly sampled nodes are added, and the same process continues until all new nodes are connected to the bidirectional ellipsoid model.

[0057] All new nodes added to the bidirectional ellipsoid model are rewired. In the i-th iteration, the planner tries to connect the nodes to Connect to the opposing tree T b The vertices in the set are only selected if the cost of the resulting solution is lower than the cost of the candidate set. and C * The node path found from the previous iteration. If the node exists in T Its Any node in is close enough so that the ellipsoid can be partially covered by the window workspace; then, CPGM#2 is used to sample and connect multiple ellipsoids to generate new nodes

[0058] connect and Finally, with the new node Find the connection between them.

[0059] Ellipsoid generation method:

[0060] The obstacles are first encoded into a low-dimensional representation using a contractive autoencoder (CAE) in the top box. However, it is not feasible to consider all obstacles in the workspace. In addition, only obstacles near the ellipsoid can determine the boundary of the ellipsoid. It is reasonable to design a window workspace ω with size l and d, i.e. in and In each iteration, ω is placed over the region of interest where expansion and connection should occur. The size of ω is fixed to make the training of CPGM easier. The position of ω is then determined while growing the bidirectional ellipsoid model in each iteration. The fixed-size window covers the local region of interest and is a subset of the workspace W. Let O denote the point cloud of obstacles within ω.

[0061] In the second step, the conditional mixture density network in the middle dashed box samples an ellipsoid position in ω, except for obstacles in ω. Encoding In addition, CMDN also takes as input the ellipsoid with which the sampled ellipsoid intersects.

[0062] The third step creates a maximized ellipsoid by iterating between updating the ellipsoid hyperplanes and maximizing the ellipsoid within the area enclosed by the hyperplanes; these hyperplanes separate the ellipsoid (or the sampled ellipsoid positions in the first iteration) from the obstacle boundary and are updated as the ellipsoid grows.

[0063] Each node is generated in four steps:

[0064] a. Sample a position of the ellipsoid;

[0065] b. Generate an ellipsoid with the maximum size in the collision-free workspace;

[0066] c. Check whether the sampled ellipsoid can intersect with the existing ellipsoid;

[0067] d. Get the intersection points and add them as nodes to the bidirectional ellipsoid model.

[0068] Given a node, the neighbor nodes of the node are defined as the vertices centered on the node within a sphere with a radius of r.

[0069] S2. According to the upper and lower bounds of the node lateral cost J(φ), output the node path candidate containing the optimal formation path.

[0070] S3. The candidate node paths are further transformed into multiple quadratic optimization problems with convex constraints to solve the optimal formation path.

[0071] Assumptions is a path from Ξ, which is actually a node sequence; there is a formation configuration in each node, and the connection between nodes is guaranteed; therefore, each node path will generate an optimization problem to solve an optimal formation path φ(τ), τ∈[0,1]; then compare all the optimal formation paths and output the optimal formation path;

[0072] In Φ * In the enumeration of the formation arrangement, each combination brings a constrained convex optimization problem; assuming q i Yes i The formation configuration contained in makes Aq i ∈φ i , due to φ i and φ i+1 The arrangement of each segment between is fixed. i Continuous entries of i The optimization problem is stated as follows: i Convex constraints It can be determined by the relationship Aq i ∈φ i Get, by solving Obtain the key optimal continuous formation state

[0073]

[0074] in,

[0075] By comparing J(Φ) for all possible node paths and formation permutations, the optimal node path can be found, and Let τ1=0,Φτcard(IΦ)=1, for all values ​​less than and τ i <τ i+1 By assigning these key continuous formation states to points along the formation path Q, a formation path Q* can be obtained such that By filling the states between these key optimal states, the path segments between consecutive key formation states are constructed by a straight line in the pose configuration space, Q(τ i ) and Q(τ i+1 ) is given by:

[0076]

[0077] where τ∈(τ i ,τ i+1 ].

[0078] Therefore, the present invention adopts the above-mentioned distributed path planning method based on double ellipsoids, and the sampling of polygons is based on two valid cpgms, which increases the probability of extending and connecting ellipsoids; in a two-dimensional workspace, the bidirectional ellipsoid model is faster than CPT* and COM, and is greatly improved over the constrained optimization method of multi-robot formations.

[0079] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A distributed path planning method based on double ellipsoids, characterized in that: The following steps are involved: S1. In the two-dimensional workspace W of the modular robot system with cluttered obstacles, an ellipsoid generation model is introduced. The ellipsoid generation model takes the low-dimensional encoding of the ellipsoid and the obstacles near the modular robot as input, so that the ellipsoid generation model grows two ellipsoids and generates nodes connecting the ellipsoids, thereby obtaining a bidirectional ellipsoid model. S2. Output the node path candidate containing the optimal formation path according to the upper and lower bounds of the node lateral cost J(φ); S3. The candidate node paths are further transformed into multiple quadratic optimization problems with convex constraints to solve the optimal formation path.

2. A distributed path planning method based on double ellipsoids according to claim 1, characterized in that: The process of establishing the bidirectional ellipsoid model in S1 is as follows: The bidirectional ellipsoid model consists of two ellipsoids T a and T b , respectively, from the root vertex and the target vertex. In the bidirectional ellipsoid model, in the i-th iteration of the growing bidirectional ellipsoid model, a new ellipsoid P is sampled from the collision-free workspace. i To carry out T a The new ellipsoid P i With the existing ellipsoid {P l } l =1,…,i-1 intersect, and get a new node {v k+j } j =1,…,n i , where n is the number of new nodes in the i-th iteration; New nodes are connected to T by selecting their parent node a , the new node v is connected to its - neighbor nodes as parent nodes according to the distance limit. The distance between nodes in the bidirectional ellipsoid model is represented by the upper and lower bounds. Given a node and its neighbor nodes The upper and lower bounds are estimated using and express: Among them, α is the weight of the rotation distance; The path Φcan be estimated as follows: for Each node in First, get its a The set N of adjacent vertices in ; then, the bidirectional ellipsoid model planner converts each After that, the parent node of each newly sampled node is selected and the estimated or The range of and N, get the sorted list, for the sorted list The upper and lower bounds are as follows: set up and G k+j Indicates about For all sorted lists and The ChooseParent procedure starts selecting the node that achieves the best lower bound among all new nodes; Connect the new nodes to T in sequence a , the new node is added to T a , will also be added to the neighbor set of other newly sampled nodes. Assuming that k+J nodes are added to the bidirectional ellipsoid model, andj≠Jsum and j≠J, Added to the neighbor sets of other newly sampled nodes, the same process continues until all new nodes are connected to the bidirectional ellipsoid model; All new nodes added to the bidirectional ellipsoid model are rewired. In the i-th iteration, the planner tries to connect the nodes to Connect to the opposing tree T b The vertices in the set are only selected if the cost of the resulting solution is lower than the cost of the candidate set. and C * The node path found from the previous iteration; if the node exists in T Its Any node in is close enough so that the ellipsoid is partially covered by the window workspace; then, CPGM#2 is used to sample and connect multiple ellipsoids to generate new nodes connect and Finally, with the new node Find the connection between them.

3. A distributed path planning method based on double ellipsoids according to claim 2, characterized in that: Ellipsoid generation method: The first step is to use the contraction autoencoder CAE to encode the obstacle into a low-dimensional representation and design a window working space ω with sizes l and d: in and In each iteration, ω is placed on the target area where expansion and connection occur. The size of ω is fixed, making the training of CPGM easier. The position of ω is then determined when growing the bidirectional ellipsoid model in each iteration. The fixed-size window covers the local area of ​​the target and is a subset of the workspace W. Let O represent the point cloud of obstacles within ω. Step 2: The conditional mixture density network samples an ellipsoid position in ω and removes obstacles in ω. Encoding In addition, CMDN takes as input the ellipsoid that the sampling ellipsoid intersects with; Step 3: Create an ellipsoid with the maximum size by iterating between updating the ellipsoid hyperplane and maximizing the ellipsoid within the area enclosed by the hyperplane; The hyperplane separates the ellipsoid from the obstacle boundary and is updated as the ellipsoid grows.

4. The distributed path planning method based on double ellipsoids according to claim 3 is characterized in that: Assumptions is a path from the candidate set Ξ of the node path output by the bidirectional ellipsoid model, which is a node sequence; there is a formation configuration in each node, and each node path will generate an optimization problem to solve an optimal formation path φ(τ), τ∈[0,1]; then compare all the optimal formation paths and output the optimal formation path; In Φ * In the enumeration of the formations, each combination brings a constrained convex optimization problem; assuming q i Yes i The formation configuration contained in makes Aq i ∈φ i , due to φ i and φ i+1 The arrangement of each segment between is fixed, about q i Continuous entries of i The optimization problem is stated as follows: i Convex constraints By the relationship Aq i ∈φ i Get, by solving Obtain the key optimal continuous formation state in, By comparing J(Φ) for all node paths and formation permutations, find the best node path, and Let τ1=0,Φτcard(IΦ)=1,for all values ​​less than and τ i <τ i+1 By assigning continuous formation states to points along the formation path Q, a formation path Q* is obtained such that By filling the states between the optimal states, the path segments between consecutive formation states are constructed by a straight line in the pose configuration space, Q(τ i ) and Q(τ i+1 ) is given by: whereτ∈(τ i ,t i+1 ]。 5. A distributed path planning method based on double ellipsoids according to claim 4, characterized in that: Each node is generated in four steps: a. Sample a position of the ellipsoid; b. Generate an ellipsoid with the maximum size in the collision-free workspace; c. Check whether the sampled ellipsoid can intersect with the existing ellipsoid; d. Get the intersection points and add them as nodes to the bidirectional ellipsoid model.

6. A distributed path planning method based on double ellipsoids according to claim 5, characterized in that: Given a node, the neighbor nodes of the node are defined as the vertices centered on the node within a sphere with a radius of r.

Images