A method for speed and density planning applicable to a cluster passing through a virtual pipeline

Abstract

The present invention proposes a method for speed and density planning applicable to a swarm of holonomic robots passing through a virtual pipeline, and the specific steps are as follows: Step 1: Establish a mathematical model for the passage of a swarm of holonomic robots in a two-dimensional virtual pipeline with complete constraints; Step 2: Based on the problem of the passage of a swarm of robots in a two-dimensional virtual pipeline, define the average forward speed and density of the swarm; Step 3: Plan the average forward speed v a (l) and the swarm density ρ a (l); The planned density ρ a (l) should be close to the desired density ρ d to ensure that there is no collision between robots during the entire passing process; Step 4: Perform distributed tracking control on the planned average forward speed v a *(l) and the swarm density ρ a *(l). The present invention solves the problem of a swarm of holonomic robots with speed limitations passing through a virtual pipeline with variable width, can greatly improve the safety and efficiency of the swarm passing process, and can be applied to fields such as unmanned aerial vehicle air traffic, swarm of robots passing through tunnels, corridors, doors and windows, and searching in complex environments.

Description

Technical Field

[0001] The present invention belongs to the field of multi-agent, including a complete constrained robot cluster such as unmanned vehicles, unmanned aerial vehicles, etc., and particularly relates to a method for speed and density planning of a complete constrained robot cluster with speed limitation passing through a virtual pipeline. Background Art

[0002] Cluster planning and control in complex environments have attracted increasing attention. Its main goal is to plan an optimal route for each holonomic robot restricted by kinematic conditions from the starting point to the end point without colliding with other robots and obstacles, and to control the robot cluster to track the planning result. How to make the robot cluster pass through the complex environment faster and safer is an important issue that researchers have been constantly exploring.

[0003] At present, there are many methods to solve the problem of robot cluster passing in complex environments. For example, formation control aimed at maintaining formation. Robot trajectory planning algorithms are also widely used to plan geometric paths that do not conflict with obstacles and other robots in the cluster. Researchers have also proposed control-based methods to generate speed or acceleration commands based on the global path and current local information to guide the robot through the complex environment. Traditional control-based methods include artificial potential field method, vector field method, control barrier function method, etc. However, when a cluster composed of a large number of robots has to pass through some narrow spaces, these methods may not be able to ensure the safety of cluster passing. In this case, the robustness and scalability of formation control are limited, the computational complexity of trajectory planning increases sharply and highly depends on the communication between robots, and the control-based methods are prone to congestion. For this reason, we proposed control based on virtual pipeline in our previous work, that is, all robots share a pre-planned virtual pipeline, and distribute control to the robots in the cluster within this virtual pipeline to ensure the safe passage of the cluster in the complex environment. This idea of virtual pipeline is similar to vehicles driving on the same road, and the vehicles are controlled by drivers to pass safely in the complex environment. However, for robots with speed limitation, when entering the narrowing virtual pipeline, the cluster cannot stop to avoid mutual collision, as Figure 1a shown, which will bring serious safety risks. In addition, the cluster may be congested when passing through the pipeline, thus reducing its passing speed.

[0004] To solve the problems of collision and congestion when a large number of speed-limited robots move in a virtual pipeline, the present invention proposes a method for planning the average forward speed and cluster density in a virtual pipeline to ensure the safety and efficiency of cluster movement. Then, each robot is distributedly controlled to track the planned average forward speed and cluster density. Here, the concept of density applies to large-scale robot clusters. Existing research shows that density-based control encapsulates most robot-level interactions, which can avoid collisions and achieve the desired density distribution. Summary of the Invention

[0005] The present invention proposes a new method for solving the problem of the movement of a complete constrained robot cluster with speed limitations in a complex environment, including establishing a mathematical model for the movement of a complete constrained robot cluster in a two-dimensional virtual pipeline, defining the average forward speed and cluster density of the cluster, planning the average forward speed and cluster density of the robot cluster during the movement in a known two-dimensional virtual pipeline, and distributedly controlling the robot cluster to track the planning results. The present invention first applies speed and density planning to virtual pipeline control, solves the problem of unsafe movement of speed-limited clusters in complex environments, especially narrow spaces, balances movement efficiency and safety, and is more suitable for large-scale cluster control.

[0006] To achieve the above object, the present invention proposes a speed and density planning method for a complete constrained robot cluster with speed limitations to pass through a virtual pipeline, as Figure 1b shown, and the implementation steps are as follows:

[0007] Step 1: Establish a mathematical model for the movement of a complete constrained robot cluster in a two-dimensional virtual pipeline, which includes establishing a kinematic model of speed-limited robots, establishing models of physical areas, safety areas, and obstacle avoidance areas required for robot movement, and establishing a two-dimensional virtual pipeline mathematical model.

[0008] Step 1.1: Establish a kinematic model of speed-limited robots. Each robot in the cluster is regarded as a particle. The complete constrained robot cluster consists of N homogeneous robots. In the Cartesian coordinate system, the motion model of the i-th robot is:

[0009]

[0010] where p i represents the position of the i-th robot, v c,i represents the speed control instruction of the i-th robot, and N represents the number of robots in the cluster.

[0011] According to the kinematic limitations of the robot, the robot is subject to a maximum speed v max , a minimum speed v min , and a maximum tangential acceleration a vAnd the maximum normal acceleration a n is limited as follows:

[0012] 0 < v min ≤ ||v c,i || ≤ v max (1)

[0013]

[0014]

[0015] where r t represents the radius of curvature of the robot's motion trajectory.

[0016] Step 1.2: Establish models for the physical area, safety area, and obstacle avoidance area required for the robot to pass. Specifically, concentric circles of different sizes are used to represent the physical area, safety area, and obstacle avoidance area of the robot, as Figure 2 shown, where r p , r s , r a represent the radii of the physical area, safety area, and obstacle avoidance area, respectively. In addition, there is r p ≤ r s ≤ r a . In particular, in the present invention, the obstacle avoidance radius r a is a control variable for tracking the planned cluster density. Specifically, for the i-th robot, an obstacle avoidance radius controller is designed, that is, by changing the size of r a,i to track the planned cluster density, that is where r ac,i is the obstacle avoidance radius controller, which will be described in detail later.

[0017] Step 1.3: Establish a mathematical model of a two-dimensional virtual pipeline. The virtual pipeline is a regular pipeline designed on a two-dimensional plane for the passage of a robot cluster in a complex environment. The virtual pipeline is established according to the complex environment, and there are no obstacles inside the pipeline. As Figure 3 shown, the virtual pipeline in the two-dimensional plane is represented as:

[0018]

[0019] where θ = {0, π}, l ∈ [0, L], ρ ∈ [0, 1]. The curve γ(l) is the generatrix (center line) of the virtual pipeline, n(l) represents the normal vector of the generatrix, l represents the length of the generatrix starting from the point γ(0), and γ(l) represents the position on the generatrix of the virtual pipeline where the length of the generatrix from the point γ(0) is l. In addition, L > 0 represents the total length of the generatrix, that is, the arc length from the starting point γ(0) to the end point γ(L) of the generatrix. λ(l) is continuous and represents the width of the virtual pipeline. rt (l) represents the radius of curvature of the generation line. In the present invention, the position of the i-th robot in the virtual pipeline is defined as Specifically, each p i corresponds to a unique l i , where l i ∈[0,L].

[0020] Step 2: Based on the problem of the passage of a large-scale robot cluster in a two-dimensional virtual pipeline, define the average forward speed and density of the cluster.

[0021] Step 2.1: Define the density of the cluster in the two-dimensional virtual pipeline. In the present invention, the density of the cluster is defined as the number of robots per unit area, that is

[0022]

[0023] where S is the area occupied by the cluster in the virtual pipeline, such as the light gray area in Figure 4 . The calculation method of S is introduced below. Assume that the cluster passes through the virtual pipeline from the starting point γ(0) to the ending point γ(L). Let p e represent the position of the robot in the cluster that is the farthest from γ(L) (the last robot in the forward direction of the cluster), and let p s represent the position of the robot in the cluster that is the closest to γ(L) (the foremost robot in the forward direction of the cluster). In addition, let m(p i ) represent the projection of the i-th robot on the center line of the pipeline. Then

[0024]

[0025]

[0026] where s(m(p i ),γ(L)) represents the arc length of the center line of the pipeline between m(p i ) and γ(L).

[0027] Therefore, the area of the region occupied by the cluster in the virtual pipeline is:

[0028]

[0029] Step 2.2: Define the average forward speed of the cluster. Assume that macroscopically the cluster can be regarded as a point, called the cluster center point, such as the five-pointed star in Figure 4 . The average forward speed v a , the density ρ a of the cluster are regarded as the attributes of the cluster center point. Let v f,i represent the forward speed of the i-th robot, then the average forward speed of the cluster is

[0030] Step 3: Plan the average forward speed v of the robot cluster during the passage in the two-dimensional virtual pipeline a (l) and the cluster density ρ a (l). To ensure the safety of the cluster, plan the density ρ a (l) should be as close as possible to the desired density ρ d to ensure that there is no collision between robots during the entire passage. The constant ρ d is a reasonable value preset according to experience. In addition, the purpose of planning the average forward speed v a (l) is to enable the cluster to pass through the virtual pipeline as quickly as possible.

[0031] Given a pre-planned virtual pipeline. According to the above analysis, the following plan is obtained. The objective function and constraints are as follows:

[0032]

[0033] v min ≤v a (l)≤v max (7)

[0034]

[0035]

[0036] 0<ρ a (l)≤ρ max (10)

[0037]

[0038]

[0039]

[0040] Among them, L, v min , v max , a v , a n , ρ max , N are known constants. r t (l) represents the centerline curvature radius at the centerline arc length of l. ρ max represents the maximum cluster density allowed without collision between robots. a ρ represents the maximum change rate of the cluster density. ρ f (l + Δl) represents the predicted cluster density based on ρ a (l) after the robot moves forward Δl along the centerline without relative position change. represents Round up.

[0041] Equation (6) is the objective function. The first term of the objective function represents the total time for the cluster to pass through the virtual pipeline. When the first term is minimized, the time for the cluster to pass through the entire virtual pipeline is shortened as much as possible. At the same time, when the second term is minimized, the density of the cluster at each position approaches the desired density ρ. d 。

[0042] Equation (7) limits the magnitude of the average forward speed of the robot, which is determined by the physical characteristics of the robot itself in Equation (1).

[0043] Equation (8) limits the rate of change of the magnitude of the average forward speed of the robot, that is, considering the physical characteristics of the actual robot in Equation (2), its speed cannot change instantaneously. Equation (8) can be specifically written as

[0044]

[0045] Equation (9) can be derived from Equation (3). It limits the magnitude of the average forward speed at different positions of the virtual pipeline to ensure that the cluster does not exceed the pipeline boundary at the positions where the centerline of the virtual pipeline is more curved. Specifically, if the curvature of the pipeline centerline is large, then v a (l) cannot be too large.

[0046] Equation (10) limits that the density of the cluster at any position cannot be greater than the maximum density to ensure the safety of the cluster passage. The minimum area occupied by the cluster is Therefore, the maximum density is:

[0047]

[0048] Equation (11) limits the rate of change of the density of the cluster in the virtual pipeline, that is, the cluster cannot instantaneously change the area it occupies in the pipeline. The maximum rate of change of density can be calculated from Equation (13). Equation (11) can be specifically written as

[0049]

[0050] Equation (12) is the density prediction plan for the virtual pipeline with known parameters. As Figure 5 shown, ρ f (l + Δl) represents the predicted density of the cluster based on ρ a (l) after the robot moves forward by Δl along the centerline without relative position change. ρ f (l + Δl) can be calculated according to Equations (4) and (5), and it is related to N and λ(l). Therefore, according to Equation (12), there is

[0051]

[0052] where f(N, λ(l), ρ a (l), v a (l), Δl) represents a function related to N, λ(l), ρ a (l), v a (l), Δl. The meaning of this formula is: if the robot moves forward without relative position change, then the rate of change of cluster density caused by the change in pipeline width cannot exceed the maximum rate of change of cluster density calculated by Equation (13). It can be found from the subsequent planning results that this constraint condition plans a smaller cluster density for the area before the narrowest part of the virtual pipeline. After the controller tracks the planning results, the cluster will spread before entering the narrowest part of the virtual pipeline, that is, the cluster can offset the passive increase in cluster density caused by the change in pipeline width through early active diffusion. Equation (12) can effectively avoid cluster collisions and congestion phenomena before entering the narrowed part of the virtual pipeline, ensure the safety of cluster passage, and improve the passage efficiency.

[0053] Equation (13) shows that the maximum rate of change of cluster density depends on N, v max , r a . Intuitively, the larger the maximum speed v max , the faster the cluster can spread, so a ρ is larger. For a rough calculation of a ρ , assume that the area occupied by the cluster is square. As Figure 6 shown, the fastest spreading strategy of the cluster is that the robots at the four corners of the original square area (dashed line) move away from the center point of the square at the maximum speed v max to become the four corner points of the new square area (solid line). When the cluster spreads fastest, the cluster density changes fastest. Let the side length of the original square area be 2nr a . After a time Δt, the cluster spreads into a new square area, and its side length is Therefore, the maximum rate of change of cluster density is

[0054]

[0055] where ρ 0 represents the initial density of the cluster, and ρ 1 represents the density of the cluster after time Δt.

[0056] Step 4: Perform distributed tracking control on the planned average forward speed and cluster density , which includes the basic design of the robot controller for passage in the virtual pipeline and the design of the tracking controller for the planning results. The purpose is to make the real-time average forward speed of the cluster and the real-time density of the cluster track the planning results as well as possible.

[0057] Step 4.1: Design the basic controller of the robot moving in the virtual pipeline.

[0058] In the present invention, the robot moving in the virtual pipeline receives speed control instructions. As Figure 7 shown, the designed control speed is divided into three components, namely the speed control component v f,i for guiding the cluster to fly forward along the pipeline, the speed control component v m,i for collision prevention between robots, and the speed control component v xy,i for restricting the cluster within the virtual pipeline. The design of each speed control component will be introduced below.

[0059] (1) The speed control component for guiding the cluster to fly forward along the pipeline

[0060] In the cluster, the control component for the i-th robot to move forward along the pipeline is

[0061] v f,i = v f,i t c (p i )

[0062] where v f,i is the magnitude of the control component for moving forward along the pipeline, and t c (p i ) is the unit tangent vector of the forward direction of the point m(p i ) corresponding to the i-th robot on the center line of the virtual pipeline. To meet the kinematic constraints of the robot, it is required here that

[0063] v f,i ∈ [v min , v max

[0064] (2) The speed control component for collision prevention between robots

[0065] In the present invention, the traditional artificial potential field method is used to achieve collision prevention between robots. First, two smooth functions are defined.

[0066] ① The smooth function σ(x, d 1 , d 2 ), which is a second-order continuously differentiable impact function:

[0067]

[0068] A = -2 / (d 1 - d 2 ) 3

[0069] B = 3(d 1 + d 2 ) / (d1 -d 2 ) 3

[0070] C = -6d 1 d 2 / (d 1 -d 2 ) 3

[0071]

[0072] The schematic diagram of this smooth function is shown in Figure 8a .

[0073] ② The smooth function s(x, s ), used to approximate the saturation function:

[0074]

[0075] x 2 = 1 + ∈ s / tan67.5°

[0076] x 1 = x 2 -sin45° s

[0077] The schematic diagram of this smooth function is shown in Figure 8b .

[0078] Define the position error between the i-th and j-th robots as That is:

[0079]

[0080] Construct a Lyapunov barrier function:

[0081]

[0082] where ε m > 0, ε s > 0, k 2 > 0 are design parameters.

[0083] For the i-th robot, define the set of other robots within its obstacle avoidance range as Then the anti-collision control component of the i-th robot is

[0084]

[0085] (3) The speed control component that restricts the swarm within the virtual pipeline

[0086] Define the distance error between the $i$-th robot and its corresponding virtual pipeline boundary as $d$. xy,i , that is:

[0087] $d$ xy,i $=\lambda(m(p$ i )) - \|p$ i - m(p$ i )\|$

[0088] Construct a Lyapunov barrier function:

[0089]

[0090] where $\varepsilon$ t $>0$, $\varepsilon$ s $>0$, $k$ 3 $>0$ are design parameters.

[0091] Limit the control component of the $i$-th robot within the virtual pipeline to be

[0092]

[0093] According to the design of the above three velocity control components, construct a distributed cluster controller. For the $i$-th robot, its two-dimensional velocity command is

[0094] $v'$ c,i $=\text{sat}(v$ f,i $ + v$ m,i $ + v$ xy,i , $v$ min , $v$ max )$

[0095] where $\text{sat}(\cdot)$ is a direction-preserving vector saturation function, specifically defined as

[0096]

[0097] Step 4.2: Make the real-time average forward speed of the cluster track the planned average forward speed at each position of the virtual pipeline In the present invention, a simplified design is carried out for this part, and the planned average forward speed is directly used as the magnitude $v$ f,i of the forward control component of the $i$-th robot along the pipeline, that is:

[0098]

[0099] Step 4.3: Make the real-time cluster density track the planned cluster density at each position of the virtual pipeline The tracking of the planned density is achieved by changing the obstacle avoidance radius $r$ a . In order to make the real-time cluster density $\rho$r (l) Follow the planned cluster density throughout the passage By changing the obstacle avoidance radius r a The setting of the size, and then change v in the speed control instruction m,i 、v xy,i Component, so as to realize the change of the area occupied by the cluster in the two-dimensional virtual pipeline, and control the real-time density to approach the planned cluster density. The obstacle avoidance radius controller of the i-th robot is designed as follows:

[0100]

[0101] Among them, Is the obstacle avoidance radius control coefficient, l i ∈[0,L].

[0102] When the real-time cluster density ρ r (l) is greater than the planned cluster density At this time, the obstacle avoidance radius r a Will increase, as Figure 7 Shown, under the action of r a The robots in the cluster spread, the area occupied by the cluster in the pipeline increases, and the real-time density decreases and approaches the planned density. This avoids collisions between robots and ensures the safety of the cluster passage process. When the real-time density of the cluster is less than the planned density, density tracking is no longer considered because according to Equation (10), there is no safety risk for the cluster at this time.

[0103] The present invention proposes a speed and density planning with tracking control to solve the problem of a fully constrained robot cluster with speed limitations passing through a virtual pipeline with variable width. This method can greatly improve the safety and efficiency of the cluster passage process and can be applied to fields such as unmanned aerial vehicle air traffic, robot clusters passing through tunnels, corridors, doors and windows, and searching in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0104] Figure 1a 、 1b Are the passing conditions of the cluster in the virtual pipeline without planning and with planning.

[0105] Figure 2 Are the physical area, safety area and obstacle avoidance area of the robot.

[0106] Figure 3 Is a schematic diagram of a two-dimensional virtual pipeline.

[0107] Figure 4 Is an illustration of the distribution of the robot cluster in the virtual pipeline.

[0108] Figure 5 Is at the place where the length of the center line of the virtual pipeline is l (the cluster density is ρa (l)) and move to the center line with a length of l + Δl without changing the relative positions of the robots (the cluster density is ρ f (l + Δl)).

[0109] Figure 6 It is a schematic diagram for calculating the maximum change rate of cluster density.

[0110] Figure 7 It is a schematic diagram of the speed control instructions for the i-th and j-th robots.

[0111] Figure 8a It is a schematic diagram of the smooth function σ(x, d 1 , d 2 ).

[0112] Figure 8b It is a schematic diagram of the smooth function s(x, s ).

[0113] Figure 9a 、 9b They are the simulation result diagrams of speed tracking and density tracking when the UAV cluster passes through a narrowed quadrilateral splicing pipeline.

[0114] Figure 10a 、 10b They are the simulation result diagrams of the UAV cluster passing through a narrowed quadrilateral splicing pipeline and a narrowed curved pipeline.

[0115] Figure 11a 、 11b They are the simulation result diagrams of the minimum distance between UAVs and the minimum distance between UAVs and the pipeline boundary when the UAV cluster passes through a narrowed quadrilateral splicing pipeline. Specific implementation method

[0116] The present invention provides a speed and density planning method for a complete constrained robot cluster suitable for passing through a virtual pipeline with speed constraints. Taking the scenario where the initial positions of 20 UAVs are inside a two-dimensional curved pipeline as an example, the specific implementation method is further described. The simulation and calculation process are carried out on MATLAB R2022b under the Win10 operating system on a computer with a main frequency of 3.20Ghz and a memory of 32.0GB.

[0117] Example: There are 20 UAVs that need to autonomously pass through a narrowed quadrilateral splicing pipeline and a narrowed curved pipeline. The implementation steps are as follows:

[0118] Step 1: Establish a mathematical model for the UAV cluster to pass through the two-dimensional virtual pipeline.

[0119] Step 1.1: Establish a kinematic model of speed - limited drones. The drone swarm consists of N = 20 homogeneous drones. According to the kinematic limitations of the drones, the minimum speed of the drones is designed to be v min = 2 m / s, and the maximum speed is v max = 5 m / s. The maximum tangential acceleration is a v = 1 m / s 2 , and the maximum normal acceleration is a n = 1 m / s 2 . The initial speed of all drones is zero.

[0120] Step 1.2: Establish models for the physical area, safety area, and obstacle - avoidance area required for drone passage. The physical radius of the drone is designed to be r p = 0.3 m, the safety radius r s = 0.4 m, and the obstacle - avoidance radius r a = 0.8 m.

[0121] Step 1.3: Establish a mathematical model of a two - dimensional virtual pipeline. In this embodiment, in the Cartesian rectangular coordinate system, the center line of the narrowed quadrilateral - spliced pipeline is designed as y = 0, x ∈ [0, 80], and the pipeline width is

[0122]

[0123] The center line of the narrowed curved pipeline is designed as y = 3sin(0.05πx), x ∈ [0, 80], and the pipeline width is

[0124] λ(x)=2sin(0.05πx)+2.5, x ∈ [0, 80].

[0125] Step Two: Based on the problem of drone swarm passage in a two - dimensional virtual pipeline, define the average forward speed and density of the swarm.

[0126] Step 2.1: Define the swarm density in the two - dimensional virtual pipeline. The swarm density is defined as the number of robots per unit area, that is

[0127] Step 2.2: Define the average forward speed of the swarm in the two - dimensional virtual pipeline. The average forward speed of the swarm is defined as

[0128]

[0129] Step Three: Plan the average forward speed v a (l) and the swarm density ρ a . Set the maximum swarm density to ρ max = 0.9974 robots / m 2 , and the desired density is ρd = 0.2 pieces / m 2 . In this embodiment, v a (l) and ρ a (l) are represented by a third-order polynomial, that is, let

[0130] v a (l) = c 3 l 3 + c 2 l 2 + c 1 l + c 0

[0131] ρ a (l) = b 3 l 3 + b 2 l 2 + b 1 l + b 0

[0132] where c 3 , c 2 , c 1 , c 0 , b 3 , b 2 , b 1 , b 0 are the coefficients of the third-order polynomial, which are optimized according to equations (6)-(13). Finally, the planned result v a *(l) is as shown in Figure 9a the planned speed, and the planned result is as shown in Figure 9b the planned density.

[0133] Step 4: Perform distributed tracking control on the planned average forward speed and the cluster density .

[0134] Step 4.1: Design the UAV basic controller as v′ c,i = sat(v f,i + v m,i + v xy,i , v min , v max ). Set the parameters of the controller as ε m = ε t = ε s = 10 -6 , k 2 = k 3 = 1.

[0135] Step 4.2: Make the real-time average forward speed of the cluster track the planned average forward speed at each position in the virtual pipeline Directly use the planned average forward speed As the magnitude v of the forward control component of the i-th UAV along the pipeline f,i , that is:

[0136]

[0137] Step 4.3: Make the real-time cluster density track the planned cluster density at each position of the virtual pipeline Set the parameters of the obstacle avoidance radius controller as

[0138] After setting the above parameters, the obtained simulation results are as Figure 10a , 10b shown. It can be seen that the UAV cluster always moves forward in the pipeline along the two-dimensional virtual pipeline, and the cluster diffuses before entering the narrowest areas of the two types of pipelines. This is the result of the real-time density of the cluster tracking the planned density, effectively ensuring the safety of the cluster passing through the pipeline, which proves the effectiveness of the speed and density planning method proposed by the present invention. Define the passing time of the UAV cluster in the virtual pipeline as the time from when the cluster starts to move to when the last UAV in the cluster leaves the pipeline. Then the passing times of the cluster in the narrowed quadrilateral splicing pipeline and the narrowed curved pipeline are 45 seconds and 56 seconds respectively. The minimum distance between any two UAVs during the entire passing process of the cluster is as Figure 11a shown, and this value always remains above 2r p = 0.6m. The minimum distance between all UAVs and the boundary of the virtual pipeline during the entire passing process is as Figure 11b shown, and this value always remains above r p = 0.3m. This shows that during the whole process, no collision occurs between the UAVs, and no UAV collides with the pipeline wall. Figure 9a is the tracking curve of the real-time forward speed of the 5th UAV in the cluster with respect to the average forward speed of the cluster, Figure 9b is the tracking curve of the real-time cluster density with respect to the planned cluster density. It can be seen that both the real-time speed and density track the planned results well, which proves the effectiveness of the tracking controller designed by the present invention. The above results prove the effectiveness of the proposed speed and density planning method for a complete constrained robot cluster applicable to speed-limited virtual pipeline crossing.

Claims

1. A method for speed and density planning applicable to a cluster passing through a virtual pipeline, characterized in that: The specific steps are as follows: Step 1: Establish a mathematical model for the complete constraint of a robot cluster passing through a two-dimensional virtual pipeline, including establishing a kinematic model of a robot with speed limitation, establishing models of physical regions, safety regions, and obstacle avoidance regions required for robot passage, and establishing a two-dimensional virtual pipeline mathematical model; Step 2: Based on the problem of a robot cluster passing through a two-dimensional virtual pipeline, define the average forward speed and density of the cluster; Step 3: Plan the average forward speed v of the robot cluster during the passage in the two-dimensional virtual pipeline a (l) and the cluster density ρ a (l); Plan the density ρ a (l) should be close to the expected density ρ d , ensuring that there is no collision between robots during the entire passing process; The constant ρ d is a reasonable value preset according to experience; Make v a (l) quickly pass through the virtual pipeline; Step 4: Perform distributed tracking control on the planned average forward speed and cluster density including the basic design of the robot controller for passing within the virtual pipeline and the design of the tracking controller for the planning results; enabling the robot cluster to better track the planning results in terms of the real-time average forward speed and the real-time cluster density.

2. A method for speed and density planning applicable to a cluster passing through a virtual pipeline according to claim 1, characterized in that: In Step 1, establish a kinematic model of a robot with speed limitation; regard each robot in the cluster as a particle; the complete constraint robot cluster consists of N homogeneous robots; in the Cartesian coordinate system, the motion model of the i-th robot is: where p i represents the position of the i-th robot, v c,i represents the speed control command of the i-th robot, and N represents the number of robots in the cluster; According to the kinematic constraints of the robot, the robot is subject to a maximum speed v max , a minimum speed v min , a maximum tangential acceleration a v and a maximum normal acceleration a n constraints, as follows: 0 < v min ≤ ||v c,i || ≤ v max (1) Among them, r t represents the radius of curvature of the robot's motion trajectory.

3. A method for speed and density planning applicable to a cluster passing through a virtual pipeline according to claim 1, characterized in that: In step one, establish models of the physical area, safety area, and obstacle avoidance area required for the robot to move; use concentric circles of different sizes to represent the physical area, safety area, and obstacle avoidance area of the robot; where r p , r s , r a represent the radii of the physical area, safety area, and obstacle avoidance area respectively; in addition, there is r p ≤ r s ≤ r a ; the obstacle avoidance radius r a is a control variable for tracking the planned cluster density; for the i-th robot, design an obstacle avoidance radius controller, that is, by changing the size of r a,i to track the planned cluster density, that is where r ac,i is the obstacle avoidance radius controller.

4. A method for speed and density planning applicable to a cluster passing through a virtual pipeline according to claim 1, characterized in that: In Step 1, establish a mathematical model of a two-dimensional virtual pipeline; the virtual pipeline is a regular pipeline designed on a two-dimensional plane for the passage of a robot cluster in a complex environment; the virtual pipeline is established according to the complex environment, and there are no obstacles in the pipeline; the virtual pipeline in the two-dimensional plane is represented as: where, θ = {0, π}, l ∈ [0, L], ρ ∈ [0, 1]; the curve γ(l) is the generatrix of the virtual pipeline, n(l) represents the normal vector of the generatrix, l represents the length of the generatrix starting from the point γ(0), and γ(l) represents the position on the virtual pipeline generatrix where the generatrix length from the point γ(0) is l; L > 0 represents the total length of the generatrix, that is, the arc length from the starting point γ(0) to the ending point γ(L) of the generatrix; λ(l) is continuous and represents the width of the virtual pipeline; r t (l) represents the radius of curvature of the generatrix; the position of the i-th robot in the virtual pipeline is defined as each p i corresponds to a unique l i , where l i ∈ [0, L].

5. A method for speed and density planning applicable to a cluster passing through a virtual pipeline according to claim 1 or 2 or 3 or 4, characterized in that: In Step 2, define the density of the cluster in the two-dimensional virtual pipeline; define the density of the cluster as the number of robots per unit area, that is Among them, S is the area occupied by the cluster in the virtual pipeline; assume that the cluster travels in the virtual pipeline from the starting point γ(0) to the ending point γ(L); let p e represent the position of the robot farthest from γ(L) in the cluster, and let p s represent the position of the robot closest to γ(L) in the cluster; let m(p i ) represent the projection of the i-th robot on the center line of the pipeline; then Among them, s(m(p i ), γ(L)) represents the arc length of the center line of the pipe between m(p i ) and γ(L); Therefore, the area occupied by the cluster in the virtual pipeline is:

6. A method for speed and density planning applicable to a cluster passing through a virtual pipeline according to claim 5, characterized in that: In step two, define the average forward speed of the cluster; macroscopically, the cluster is regarded as a point, called the cluster center point; take the average forward speed v a , the cluster density ρ a as the attributes of the cluster center point; let v f,i represent the forward speed of the i-th robot, then the average forward speed of the cluster is:

7. A method for speed and density planning applicable to a cluster passing through a virtual pipeline according to claim 6, characterized in that: In Step 3, given a pre-planned virtual pipeline; the objective function and constraints are as follows: v min ≤ v a (l) ≤ v max (7) 0 < ρ a (l) ≤ ρ max (10) where L, v min , v max , a v , a n , ρ max , N are known constants; r t (l) represents the centerline curvature radius at the centerline arc length of l; ρ max represents the maximum cluster density allowed without collision between robots; a ρ represents the maximum change rate of the cluster density; ρ f (l + Δl) represents the predicted cluster density based on ρ a (l) after the robot moves forward Δl along the centerline without relative position change; represents rounding up.

8. A method for speed and density planning applicable to a cluster passing through a virtual pipeline according to claim 7, characterized in that: In step four, design the basic controller for the robot moving in the virtual pipeline; the robot moving in the virtual pipeline receives speed control instructions; the designed control speed is divided into three components, namely the speed control component v that guides the cluster to fly forward along the pipeline f,i , the speed control component v for collision prevention between robots m,i , and the speed control component v that restricts the cluster within the virtual pipeline xy,i ; among them, for the speed control component that guides the cluster to fly forward along the pipeline, in the cluster, the control component for the i-th robot to move forward along the pipeline is: v f,i = v f,i t c (p i ) where v f,i is the magnitude of the control component for advancing along the pipeline, and t c (p i ) is the unit tangent vector of the advancing direction of the point m(p i ) on the virtual pipeline centerline corresponding to the i-th robot; to satisfy the robot kinematic constraints, it is required that v f,i ∈ [v min , v max ​ The speed control component for anti-collision between robots: First, define two smooth functions; ① The smooth function σ(x, d 1 , d 2 ), is a second-order continuously differentiable shock function: A = -2 / (d 1 -d 2 ) 3 B = 3(d 1 + d 2 ) / (d 1 - d 2 ) 3 C = -6d 1 d 2 / (d 1 -d 2 ) 3 ② Smooth function s(x, ∈ s ), used to approximate the saturation function: x 2 = 1 + ∈ s / tan 67.5° x 1 = x 2 -sin45° ∈ s Define the position error between the i-th and j-th robots as That is Construct a Lyapunov barrier function: where ε m > 0, ε s > 0, k 2 > 0 are design parameters; For the i-th robot, define the set of other robots within its obstacle avoidance range as Then the anti-collision control component of the i-th robot is The speed control component for restricting the cluster within the virtual pipeline: Define the distance error between the i-th robot and its corresponding virtual pipeline boundary as d xy,i , that is: d xy,i = λ(m(p i )) - ||p i - m(p i )|| Construct a Lyapunov barrier function: where ε t > 0, ε s > 0, k 3 > 0 are design parameters; Restrict the control component of the i-th robot within the virtual pipeline to be According to the design of the above three speed control components, construct a distributed cluster controller; for the i-th robot, its two-dimensional speed command is v′ c,i = sat(v f,i + v m,i + v xy,i , v min , v max ) where sat(·) is a direction-preserving vector saturation function, specifically defined as 9. A method for speed and density planning applicable to a cluster passing through a virtual pipeline according to claim 8, characterized in that: In Step 4, make the real-time average forward speed of the cluster track the planned average forward speed at each position of the virtual pipeline. Simplify the design of this part and directly use the planned average forward speed. As the magnitude v of the forward control component of the i-th robot along the pipeline f,i , that is:

10. A method for speed and density planning applicable to a cluster passing through a virtual pipeline according to claim 9, characterized in that: In step four, the real-time cluster density is made to track the planned cluster density at each position of the virtual pipeline. The tracking of the planned density is achieved by changing the obstacle avoidance radius r. a To achieve this; in order for the real-time cluster density ρ r (l) to follow the planned cluster density throughout the passage By changing the obstacle avoidance radius r a The size setting is adjusted, and then the v in the speed control command is changed. m,i 、v xy,i components, thereby changing the area occupied by the cluster in the two-dimensional virtual pipeline and controlling the real-time density to approach the planned cluster density; the obstacle avoidance radius controller for the i-th robot is designed as follows: Among them, is the obstacle avoidance radius control coefficient, l i ∈[0, L]; When the real-time cluster density ρ r (l) is greater than the planned cluster density , the obstacle avoidance radius r a will increase. Under the action of r a , the robots in the cluster will spread, the area occupied by the cluster in the pipeline will increase, the real-time density will decrease and approach the planned density; collisions between robots are avoided to ensure the safety of the cluster during passage; when the real-time density of the cluster is less than the planned density, density tracking is no longer considered, and there is no safety risk for the cluster at this time.

Images