An Optimal Virtual Pipeline Planning Method for Robot Swarms

Abstract

The present invention provides a method for planning an optimal virtual pipeline applicable to a robot cluster, including the following steps: Step 1: The input terminal #imgabs0# inputs the start and end terminals #imgabs1# and the target terminal #imgabs2# of the virtual pipeline, which are respectively composed of the convex hulls of the finite sets {q 0,k},{q m,k}; Step 2: Construct an ordered pair set #imgabs3# through the mapping f; Step 3: Path search; perform path search on the ordered pairs in the ordered pair set respectively, and perform normalized parameterization on the paths; Step 4: Optimize the trajectory; the generated path is not applicable to the dynamic model of the individual robot due to the existence of sharp corners. Therefore, based on the generated path, the optimal trajectory is obtained by solving the optimization problem; Step 5: Construct the optimal virtual pipeline; Step 6: Output infinitely many optimal trajectories. The present invention can obtain equivalent optimal trajectories through the affine combination of known optimal solutions without solving the optimization problem, greatly reducing the computational amount, having strong versatility, and providing a simple and easy-to-use solution for a robot cluster to pass through an obstacle environment.

Description

Technical Field

[0001] The present invention relates to a method for planning an optimal virtual pipeline applicable to a robot cluster, in particular to a method for generating an optimal virtual pipeline based on a path search algorithm and an optimization quadratic programming problem under the constraints of start and end regions and obstacle avoidance to solve cluster trajectory planning and obstacle environment crossing, belonging to the field of cluster trajectory planning. Background Art

[0002] At present, cluster trajectory planning has a wide range of applications in many fields. For example, when an unmanned aerial vehicle (UAV) cluster collaboratively crosses a dense obstacle environment, it involves the problem of cluster collision avoidance trajectory planning. For the task of a cluster crossing an obstacle environment, it is necessary to plan a collision avoidance trajectory for each cluster individual, and the iterative calculation process of the optimal trajectory planning for each individual requires a large amount of computing power. Therefore, it is of great significance to study how to generate the optimal trajectory of a large-scale cluster with the least computing power.

[0003] In previous related research, a virtual pipeline was defined as a space suitable for the flow of a robot cluster. In an obstacle environment, the cluster moves in the pre-planned virtual pipeline, so it can safely and smoothly pass through to the target area with less computational effort. The research on virtual pipelines can be summarized into two problems, namely the virtual pipeline planning problem and the virtual pipeline traversal control problem. The content of the present invention belongs to the virtual pipeline planning problem.

[0004] For the trajectory planning problem, a currently popular solution is to solve an optimization problem for each individual in the cluster separately and obtain the optimal solution through continuous iteration, that is, the parametric representation of the optimal trajectory. This method can achieve relatively good results when the number of clusters is small, but when the number of clusters is large and the environment is complex, its disadvantages become apparent, and the total computational amount increases with the increase in the number of clusters. Therefore, in order to avoid waste of computing resources, the present invention proposes a method for generating an optimal virtual pipeline based on a path search algorithm and an optimization quadratic programming problem to solve cluster trajectory planning and obstacle environment crossing. This method generates an optimal virtual pipeline through a limited number of path searches and optimal trajectory planning processes in the environment that are much less than the number of clusters. Any position of the cluster individual in the optimal virtual pipeline can obtain an optimal trajectory equivalent to solving the optimization problem through a simple affine operation, because this method has a fast operation speed and high efficiency. After the trajectory generation is completed, the existing mature trajectory tracking control method is used to control the cluster, so that the individuals in the cluster can complete the task without collision and reach the target point. Summary of the Invention

[0005] The present invention provides a method for planning an optimal virtual pipeline applicable to a fully constrained robot swarm. It solves the problems of large total computational amount and easy dispersion when the robot swarm collaboratively crosses an obstacle environment, provides a simple and easy-to-use solution, and has the characteristics of low computational amount and strong generality.

[0006] The present invention first establishes a fully constrained robot swarm model and then establishes a related model of the virtual pipeline.

[0007] In the present invention, the fully constrained robot swarm is described by a second-order particle model. For any robot i in the swarm, its kinematic model is expressed as:

[0008]

[0009]

[0010] Where represents the position of robot i, represents the velocity of robot i, and u i,c represents the input control instruction. As Figure 1 shown, the virtual pipeline is a set in an n-dimensional space, represented by the quadruple , where:

[0011] 1. and are called terminals, which are disjoint bounded convex subsets in the n-dimensional space.

[0012] 2. f is a diffeomorphism: Therefore, there is a set of ordered pairs

[0013] 3. h is a smooth mapping: Where such that h((q 0 , q m ), 0) = q 0 , h((q 0 , q m ), 1) = q m . The function h((q 0 , q m ), t) is called the trajectory of the ordered pair (q 0 , q m ).

[0014] A cross-section of the virtual pipeline at is defined as

[0015] ​

[0016] The surface of the virtual pipeline is the boundary, defined as

[0017] Before defining the optimal virtual pipeline, it is first necessary to define the optimal trajectory: Let g be the objective function related to the trajectory h((q 0 , q m ), t). If

[0018]

[0019] where is the set of trajectory candidates, then the trajectory h of the ordered pair (q 0 , q m ) * is the optimal trajectory with respect to the penalty function g.

[0020] Based on the above definition, the optimal virtual pipeline is defined as: If each trajectory in the virtual pipeline is optimal with respect to the objective function g, then the virtual pipeline is the optimal virtual pipeline, that is

[0021] Based on some specific properties of the pipeline, the linear pipeline is defined as: For a virtual pipeline if f is linear and h is linear with respect to any ordered pair , then the virtual pipeline is linear.

[0022] The linear virtual pipeline with convex hull terminals is defined as: For a virtual pipeline if it satisfies:

[0023] 1. The terminals are the convex hulls of the finite sets {q 0,k} and {q m,k}, respectively.

[0024] 2. The virtual pipeline is linear.

[0025] Then the virtual pipeline is called a linear virtual pipeline with convex hull terminals.

[0026] The present invention proposes optimization-related lemmas and theories to provide a theoretical basis for the planning method.

[0027] Lemma 1: The standard form of a convex optimization problem. Assume the problem is in the following form:

[0028]

[0029] where f i (x) (i = 0, 1,..., m) is with respect to x = [x 0 x1 ... x n T Convex function Is linear with respect to x. When b = b i The optimal solution x can be obtained i . Define i = 1, 2,..., q, Then Is feasible and optimal.

[0030] Theorem 1: For a linear virtual pipeline with convex hull terminals Whose terminals Are the convex hulls of the finite sets {q 0,k} and {q m,k} respectively. If

[0031] 1. For each ordered pair, its trajectory can be linearly represented as h((q 0 , q m ), t) = C(t)x, where C(t) is a matrix that depends only on t, and x is a parameter vector.

[0032] 2. For all ordered pairs (q 0,k , q m,k ), their trajectories are optimal with respect to the objective function f 0 , and the constraints of its optimization problem f i (x) ≤ 0, i = 1,..., m are convex.

[0033] Then Is optimal with respect to the objective function f 0 .

[0034] According to Theorem 1, the present invention can generate an optimal virtual pipeline by optimizing a finite number of trajectories under specific conditions. There are infinitely many optimal trajectories in the optimal virtual pipeline. These optimal trajectories can all be represented in the form of a convex combination of a finite number of optimal trajectories, which is equivalent to the solution of directly solving the optimization problem.

[0035] The present invention proposes a method for generating an optimal virtual pipeline based on a path search algorithm and an optimization quadratic programming problem to solve cluster trajectory planning and obstacle environment crossing. Its planning flowchart is as shown in Figure 2 Shown. Based on the previous definitions, its implementation steps are as follows:

[0036] Step 1: Input the terminals

[0037] Input the start and end terminals of the virtual pipeline And the target terminal Which are respectively composed of the finite sets {q​0,k , {q m,k} is composed of the convex hulls, which can be expressed by the formula as

[0038]

[0039] Step 2: Construct an ordered pair set through the mapping f

[0040] There are various ways to define the linear mapping f. Here, without loss of generality, we define the mapping f as

[0041] q m,k = f(q 0,k ), k = 1, 2,..., q. (5)

[0042] Therefore, an ordered pair set can be constructed where there exists an ordered pair (q 0,k , q m,k ).

[0043] Step 3: Path search

[0044] According to the results of the ordered pair set in Step 2, path searches are respectively performed on the ordered pairs in the ordered pair set, and the paths are normalized and parameterized.

[0045] S31. Path search

[0046] For each ordered pair (q 0,k , q m,k )(k = 1, 2,..., q), using the existing path search algorithms (such as RRT*, A*), m + 1 path points {q i,k}(i = 0, 1,..., m) are obtained. Here, it is assumed that the number of path points for all ordered pairs is the same; otherwise, it will lead to different lengths of the optimization variables, thus not meeting the condition requirements of Lemma 1.

[0047] S32. Parameterize path points

[0048] For each ordered pair (q 0,k , q m,k )(k = 1, 2,..., q), in order to parameterize the path points {q i,k}(i = 0, 1,..., m), u i,k is assigned to the corresponding path point q i,k . Therefore, a partition regarding u k is first designed, which is called the node {u i,k}. The chord length parameterization method, as a method widely used in engineering, is used here to generate the node {u i,k}, which can be expressed as

[0049]

[0050] It should be noted that here for each ordered pair, due to the different chord lengths between the path points, the generated nodes are usually different. For convenience, all ordered pairs are expected to have consistent nodes. Inspired by the tensor product surface parameterization method, the common nodes {u i} of all ordered pairs are designed as the arithmetic mean of all nodes, that is

[0051]

[0052] S33. Parameter normalization

[0053] Perform parameter transformation on the common nodes {u i} to obtain the time nodes {t i}, that is

[0054]

[0055] Step 4: Optimize the trajectory

[0056] The path generated in Step 3 is not applicable to the dynamic model of the robot individual due to the existence of sharp corners. Therefore, based on the generated path, the optimal trajectory is obtained by solving the optimization problem.

[0057] S41. Trajectory parameterization

[0058] For each ordered pair (q 0,k , q m,k )(k = 1, 2,..., q), its trajectory h(t) can be expressed as an nth-order piecewise polynomial as

[0059]

[0060] where For any t ∈ [t k-1 , t k , the trajectory h(t) can be expressed in matrix form

[0061] h k (t) = C(t)x k (10)

[0062] where C(t) = [I d tI d t 2 I d ... t n I d ,

[0063]

[0064] S42. Construction of Constraints

[0065] The constraints include terminal constraints, intermediate condition constraints, and other constraints including obstacle constraints. The terminal constraints and intermediate point constraints are linear equality constraints of the problem, while other constraints are regarded as inequality constraints.

[0066] S421. Terminal Constraints

[0067] The terminal constraints can be expressed as

[0068]

[0069] where and are the p - order conditions at the starting point and the target point.

[0070] S422. Intermediate Condition Constraints

[0071] The intermediate condition constraints can be expressed as

[0072]

[0073] where p = 0, 1, 2,... k r , i = 1, 2,..., m - 1

[0074]

[0075] For convenience, we define

[0076]

[0077] Thus, (12) can be rewritten as

[0078]

[0079] Based on (14), the equality constraint is

[0080]

[0081] where

[0082]

[0083]

[0084]

[0085] S423. Inequality Constraints

[0086] To ensure that the trajectory does not collide with obstacles, it is necessary to construct inequality convex constraints. Other constraints can also be expressed in the form of inequality convex constraints, that is

[0087] f i (x) ≤ 0, i = 1, ... m. (16)

[0088] S43. Construct the objective function

[0089] The optimization problem can take minimizing energy consumption as the objective, and construct the objective function of the quadratic programming problem as follows

[0090]

[0091] S44. Construct the optimization problem

[0092] Based on the content of Step 42 and Step 43, the following optimization problem can be obtained

[0093]

[0094] That is the form of Lemma 1. By solving q optimization problems, q ordered pair sets {(q 0,k , q m,k )} of the optimal trajectory h * ((q 0,k , q m,k ), t) can be obtained.

[0095] Step Five: Construct the optimal virtual pipeline

[0096] Based on Theory 1, the virtual pipeline (C 0 , C 1 , f, h * ) is optimal.

[0097] Step Six: Output infinitely many optimal trajectories

[0098] For any point q in the starting and ending terminals 0 (θ), its corresponding target point is assigned by the mapping f as q m (θ), and its optimal trajectory is expressed as

[0099]

[0100] Among them, Assign the corresponding trajectory to each holonomic constraint robot, take the second derivative of the trajectory as the reference instruction, and the robot tracks the reference instruction to achieve the trajectory planning of the cluster.

[0101] A method for planning an optimal virtual pipeline applicable to a robot cluster has the advantage that an equivalent optimal trajectory can be obtained through an affine combination of known optimal solutions without solving an optimization problem, greatly reducing the computational amount, having strong versatility, and providing a simple and easy-to-use solution for a robot cluster to pass through an obstacle environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0102] Figure 1 It is a schematic diagram of a virtual pipeline.

[0103] Figure 2 It is a flowchart of optimal virtual pipeline planning.

[0104] Figure 3a It is a diagram of the terminal position of the virtual pipeline.

[0105] Figure 3b It is a schematic diagram of path search.

[0106] Figure 3c It is a schematic diagram of an optimized trajectory.

[0107] Figure 3d It is a schematic diagram of the optimal virtual pipeline.

[0108] Figure 4 It is a schematic diagram of interpolation error.

[0109] The symbols in the figure are explained as follows:

[0110] Explanation of the symbols in Figure 3: q 0,1 , q 0,2 represents the basis vector of the convex hull terminal , q m,1 , q m,2 represents the basis vector of the convex hull terminal of, represents the optimal trajectory. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0111] The following further describes the technical solutions of the present invention with reference to the drawings and embodiments.

[0112] Embodiment: As a typical holonomic robot, an unmanned aerial vehicle (UAV) is suitable for trajectory planning applications. Eleven existing UAVs are evenly placed on a plane in an environment, and target position allocation and path planning need to be completed. The implementation steps are as follows:

[0113] Step 1: Input the terminal

[0114] Input the terminal position in the obstacle map, as Figure 3a shown, the terminals are respectively represented as

[0115]

[0116]

[0117] Step 2: Construct an ordered pair set through the mapping f

[0118] Define the mapping f as q m (θ) = f(q 0 (θ)), then the ordered pair is constructed

[0119]

[0120] Among them, there are ordered pairs (q 0,1 , q m,1 ), (q 0,2 , q m,2 ).

[0121] Step 3: Path search

[0122] According to the ordered pair set result in Step 2, perform path search on the ordered pairs (q 0,1 , q m,1 ), (q 0,2 , q m,2 ) in the ordered pair set respectively, and perform normalized parameterization on the paths.

[0123] S31. Use the path search algorithm RRT* to find 6 path points for the ordered pairs (q 0,1 , q m,1 ), (q 0,2 , q m,2 ) respectively, that is, m = 5, as shown by the broken line in Figure 3b . Its specific position coordinates are shown in Table 1 below, with the unit of meters:

[0124]

[0125] Table 1

[0126] S32. Use the chord length parameterization method to parameterize the path points of the two ordered pairs respectively to obtain the nodes {u i,1}}, {u i,2}}, as shown in Table 2 below, with the unit of meters:

[0127]

[0128] Table 2

[0129] Calculate the arithmetic mean of all nodes to obtain the common node {u i} of all path points, and its specific value is shown in Table 3 below, with the unit of meters:

[0130]

[0131] Table 3

[0132] S33. Normalize the parameters of the common node {u i} to obtain the normalized node {t i}, as shown in Table 4:

[0133]

[0134] Table 4

[0135] Step Four: Optimize the trajectory

[0136] The path generated in Step Three is not applicable to the dynamic model of the robot due to the presence of sharp corners. Therefore, based on the generated path, two ordered pairs of optimal trajectories are obtained by solving the optimization problem and as Figure 3c shown.

[0137] Step Five: Construct the optimal virtual pipeline

[0138] Based on Theory 1, the virtual pipeline (C 0 , C 1 , f, h * ) is optimal.

[0139] Step Six: Output infinitely many optimal trajectories

[0140] Given that the drones are placed at equal intervals, the optimal trajectory of each drone can be obtained, as Figure 3d shown, expressed as

[0141]

[0142] At this point, the optimal pipeline planning is completed, the path planning for each drone is carried out, and compared with directly solving the optimization problem, the results are the same. The error between the solution obtained by solving the optimization problem, as Figure 4 shown, is extremely small and can be ignored.

Claims

1. An optimal virtual pipeline planning method applicable to a robot cluster, characterized in that: It includes the following steps: Step 1: Input terminal The start and end terminals of the input virtual pipeline and the target terminal which are respectively formed by the convex hulls of the finite sets {q 0,k},{q m,k}, and are expressed by the formula as Step 2: Construct an ordered pair set through the mapping f There are various definition methods for the linear mapping f. Here, without loss of generality, the mapping f is defined as q m,k = f(q 0,k ), k = 1, 2, ..., q; (2) Construct a set of ordered pairs in which there are ordered pairs (q 0,k , q m,k ); Step 3: Path search According to the result of the ordered pair set, perform path search on the ordered pairs in the ordered pair set respectively, and perform normalized parameterization on the paths; Step 4: Optimize the trajectory Since the generated path has sharp corners and is not applicable to the dynamic model of the individual robot, based on the generated path, the optimal trajectory is obtained by solving the optimization problem; Step 5: Construct the optimal virtual pipeline Virtual pipeline (C 0 , C 1 , f, h * ) is optimal; Step 6: Output infinitely many optimal trajectories For the starting and ending terminals For any point q 0 (θ), its corresponding target point is assigned as q by the mapping f m (θ), whose optimal trajectory is represented as Among them, θ ki ≥0; Assign corresponding trajectories to each full-constraint robot, take the second derivative of the trajectory as the reference instruction, and the robot tracks the reference instruction, that is, the trajectory planning of the cluster is realized.

2. The optimal virtual pipeline planning method applicable to a robot cluster according to claim 1, characterized in that: In step three, the path search is as follows: for each ordered pair (q 0,k , q m,k ), m + 1 path points {q i,k} are obtained using the existing path search algorithm; it is assumed that the number of path points for all ordered pairs is the same, otherwise it will lead to different lengths of optimization variables, where k = 1, 2,..., q, and i = 0, 1,..., m.

3. The optimal virtual pipeline planning method applicable to a robot cluster according to claim 1, characterized in that: In step three, the parametric path points are: for each ordered pair (q 0,k , q m,k ), in order to parameterize the path point {q i,k}, u i,k is assigned to the corresponding path point q i,k ; a partition of u k is designed, called the node {u i,k}; the chord length parameterization method is used to generate the node {u i,k}, where k = 1, 2,..., q, i = 0, 1,..., m, expressed as: For each ordered pair, since the chord lengths between the waypoints are different, the generated nodes are different; for convenience, all ordered pairs are expected to have consistent nodes, and the common node {u i} of all ordered pairs is designed to be the arithmetic mean of all nodes, that is:

4. The optimal virtual pipeline planning method applicable to a robot cluster according to claim 3, characterized in that: In step three, the parameters are normalized as follows: parameter transformation is performed on the common node {u i} to obtain the time node {t i}, that is 5. The optimal virtual pipeline planning method applicable to a robot cluster according to claim 1, characterized in that: In step four, the trajectory is parameterized as: for each ordered pair (q 0,k , q m,k ), where k = 1, 2,..., q, and its trajectory h(t) is represented by an nth-order piecewise polynomial as: Among them, For any t ∈ [t k-1 , t k , the trajectory h(t) is expressed in matrix form: h k (t) = C(t)x k (8) where C(t) = [I d tI d t 2 I d ...t n I d , 6. The optimal virtual pipeline planning method applicable to a robot cluster according to claim 5, characterized in that: In step 4, the terminal constraint is expressed as: Among them, and are the p-th order conditions at the starting point and the target point.

7. The optimal virtual pipeline planning method applicable to a robot cluster according to claim 6, characterized in that: In step 4, the intermediate condition constraint is expressed as: where p = 0, 1, 2,... k r , i = 1, 2,..., m - 1 For convenience, define: Therefore, equation (10) is rewritten as: Based on equation (12), the equality constraint is: where, 8. The optimal virtual pipeline planning method applicable to a robot cluster according to claim 7, characterized in that: In step 4, the inequality constraint is: In order to ensure that the trajectory does not collide with obstacles, it is necessary to construct an inequality convex constraint; other constraints are expressed in the form of inequality convex constraints, that is: f i \(f(x)\leq0, i = 1,\cdots,m\) (14) The optimization problem can take minimizing energy consumption as the goal, and construct the objective function of the quadratic programming problem as follows: The following optimization problem is obtained: Obtain the optimal trajectory h of q ordered pair sets \(\{(q 0,k ,q m,k )\}\) by solving q optimization problems * \((q 0,k ,q m,k ),t)\).

9. The optimal virtual pipeline planning method applicable to a robot cluster according to claim 1, characterized in that: Describing the complete constraint robot cluster with a second-order particle model, for any robot i in the cluster, its kinematic model is expressed as: Among them, represents the position of robot i, represents the speed of robot i, u i,c represents the input control command; Virtual pipeline is a set in an n-dimensional space, represented by the quadruple where:

1. and is called the terminal, which is a non-intersecting bounded convex subset in the n-dimensional space; 2.f is a diffeomorphism: Therefore, there is a set of ordered pairs 3.h is a smooth mapping: where such that h((q 0 ,q m ),0) = q 0 , h((q 0 ,q m ),1) = q m ; The function h((q 0 ,q m ),t) is called the trajectory of the ordered pair (q 0 ,q m ); A virtual pipe cross-section at is defined as ​ The surface of the virtual pipe is the boundary, defined as Before defining the optimal virtual pipeline, it is first necessary to define the optimal trajectory: Let g be the objective function associated with the trajectory h((q 0 ,q m ),t). If Among them, is a set of trajectory candidates, then for the ordered pair (q 0 , q m ), the trajectory h * is the optimal trajectory with respect to the penalty function g; The optimal virtual pipeline is defined as follows: if each trajectory in the virtual pipeline is optimal with respect to the objective function g, then the virtual pipeline is an optimal virtual pipeline, that is Based on some specific properties of the pipeline, a linear pipeline is defined as follows: for a virtual pipeline if f is linear and h is linear with respect to any ordered pair then the virtual pipeline is linear; A linear virtual pipeline with convex hull terminals is defined as follows: for a virtual pipeline If it satisfies:

1. Terminal are the convex hulls of the finite sets {q 0,k} and {q m,k}, respectively; 2. The virtual pipeline is linear; Then this virtual pipeline is called a linear virtual pipeline with convex hull terminals.

10. The optimal virtual pipeline planning method applicable to a robot cluster according to claim 9, characterized in that: Propose to set as follows: Setting 1: The standard form of a convex optimization problem; Let the form of this problem be as follows: Among them, f i (x) is a convex function with respect to x = [x 0 x 1 ...x n T , where i = 0, 1,..., m, ​ where p < n + 1, ▽f 0 (x) is linear with respect to x; when b = b i , the optimal solution x i is obtained; define then is feasible and optimal; Setting 2: For a linear virtual pipeline with convex hull terminals whose terminals are the convex hulls of the finite sets {q 0,k} and {q m,k}, respectively, if 1. For each ordered pair, its trajectory can be linearly represented as h((q 0 , q m ), t) = C(t)x, where C(t) is a matrix that depends only on t and x is a parameter vector; 2. For all ordered pairs (q 0,k , q m,k ), its trajectory is optimal with respect to the objective function f 0 , and the constraints f i (x) ≤ 0, i = 1,..., m of its optimization problem are convex; then is optimal with respect to the objective function f 0 .

Images