Method for guiding robot cluster to pass through virtual pipeline by using density control

Abstract

The invention relates to the technical field of robot cluster automatic control, in particular to a method for guiding a robot cluster to pass through a virtual pipeline by using density control, which comprises the following steps: determining the shape of the virtual pipeline and the position of an obstacle in the virtual pipeline, and determining the initial position of each robot in the robot cluster; a robot cluster navigation model control instruction is determined, wherein the control instruction comprises a forward item along a generation line, a saturated safety control item and a saturation density control item; on the basis of the virtual pipeline shape and the initial position of each robot, according to the robot cluster navigation model, updating the position of each robot in each time step in a plurality of time steps, and obtaining the position of each robot in the plurality of time steps; determining the motion trails of all robots, and completing the navigation of the robot cluster passing through the virtual pipeline; according to the invention, the congestion collision risk of crossing the virtual pipeline can be reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of robot cluster automatic control, and in particular to a method for guiding a robot cluster to cross a virtual pipeline by utilizing density control. Background Art

[0002] In many key areas such as environmental monitoring, drug delivery, and disaster response, guiding robot swarms to accurately navigate in complex environments is an extremely challenging task. In particular, in scenarios with densely distributed obstacles, traditional navigation methods often fail to work. To this end, the academic and engineering communities have proposed the concepts of "general virtual pipelines" and "optimal virtual pipelines" to build safe and barrier-free virtual space areas, transforming the navigation problem of robot swarms in complex environments into a problem of safe travel in preset virtual pipelines.

[0003] However, although existing research has explored control strategies for achieving safe navigation in general virtual pipes, there are still many difficulties in guiding robot clusters to pass safely and efficiently in narrow virtual pipes with internal obstacles, such as frequent congestion and increased collision risks, which seriously restrict the application efficiency of robot clusters in actual complex tasks. In addition, when there are many obstacles in the environment, general virtual pipes cannot be effectively constructed. Therefore, if you still want to build a virtual pipe in a complex environment and achieve safe navigation of the robot cluster in the virtual pipe, it is necessary to consider the virtual pipe model with internal obstacles.

[0004] How to dynamically control the robot cluster based on the geometric characteristics of the virtual pipeline and the distribution of internal obstacles so that it can maintain a safe distance and stable flow has become a technical problem that needs to be solved urgently. Summary of the invention

[0005] In view of the above problems, the present invention provides a method for guiding a robot cluster to cross a virtual pipeline using density control, which solves the technical problems of congestion, high collision risk and low operating efficiency when the navigation method in the prior art passes through an obstacle pipeline.

[0006] The present invention provides a method for guiding a robot cluster to cross a virtual pipeline by using density control, comprising the following steps:

[0007] Step S1, determining the shape of the virtual pipeline and the positions of obstacles in the virtual pipeline, and determining the initial positions of each robot in the robot cluster;

[0008] Step S2, determining a robot cluster navigation model, and determining control instructions for the robot cluster navigation model, wherein the control instructions include a forward item along a generated line, a saturated safety control item, and a saturated density control item;

[0009] Step S3: Based on the virtual pipeline shape and the initial positions of each robot, according to the robot cluster navigation model and the control instructions, update the positions of each robot at each time step in multiple time steps, and obtain the positions of each robot at multiple time steps;

[0010] Step S4: Determine the motion trajectories of all robots from the positions of each robot at the multiple time steps, and complete the navigation of the robot cluster through the virtual pipeline.

[0011] Preferably, step S1 specifically includes:

[0012] Step S1-1: Determine the shape of the virtual pipeline, including: determining the generating line γ(l), the unit tangent vector t c (l), the unit normal vector n c (l), and the widths r d (l) and r u (l) values;

[0013] Step S1-2: Determine the positions of obstacles in the virtual pipeline; determine the initial positions of each robot in the robot cluster, including: arranging the robots in a square pattern, and the distance between any two adjacent robots is equal.

[0014] Preferably, in step S2, the step of determining the control instructions of the robot cluster navigation model specifically includes:

[0015] Determine the forward term along the generating line based on the tangent vector of the arc length on the generating line and the maximum forward speed;

[0016] Determine the saturated safety control term based on the positional relationship between each robot, the positional relationship between the robot and the pipeline boundary, and the positional relationship between the robot and the obstacle;

[0017] Determine the saturated density control term based on the distribution density of the robot cluster.

[0018] Preferably, in step S2, the expression of the forward term u 1,i along the generating line is:

[0019]

[0020] where i is the robot index, k1 represents the maximum forward speed, is the tangent vector when the arc length parameter on the generating line is .

[0021] Preferably, in step S2, the expression of the saturated safety control term is:

[0022] u s,i = sat(u2,i +u 3,i +u 4,i ,v s,m )

[0023] where v s,m is the saturation upper limit of the safety control term, and sat(·,·) represents the function that limits the maximum value;

[0024]

[0025] where, u 2,i represents the collision avoidance term between robots, and k2>0 is the control gain of the collision avoidance term, represents the relative position between the i-th robot and the j-th robot, represents the partial derivative of V m,ij with respect to ; represents the set of robots that the i-th robot can sense, which is called the neighbor set of the i-th robot, where r s is the safety radius of the robot, and r a is the collision avoidance radius of the robot; is the potential function to prevent collisions between robots, For the function σ(x, d1, d2), its definition is:

[0026]

[0027] where the polynomial coefficients are: A = -2 / (d1 - d2) 3 , B = 3(d1 + d2) / (d1 - d2) 3 , C = -6d1d2 / (d1 - d2) 3 , s(x, ∈ s ) is defined as:

[0028]

[0029] where x is the independent variable of the σ and s functions, and d1, d2, ∈ m , ∈ s are all control parameters; in the design of the control term, d1 = 2r s , d1 = r s +r a , ∈ m = ∈ s = 10 -6 ;

[0030]

[0031] where, u 3,iItems that restrict the robot within the pipeline, where k3>0 is the control gain for restricting the robot within the pipeline, is the potential function that restricts the robot inside the pipeline, where d t,i represents the position p of the current robot i from the pipeline boundary, σ t (d t,i ) = σ(d t,i , r s , r a ), ∈ t = 10 -6 ; represents the partial derivative of V t,i with respect to p i ;

[0032]

[0033] where, u 4,i represents the collision avoidance term between the robot and the obstacle, represents the relative position between the i-th robot and the k-th obstacle, p o,ik represents the k-th obstacle that the i-th robot can sense, is defined as the potential function to prevent collisions between robots, r o represents the radius of the obstacle; represents the set of obstacles that the i-th robot can sense, k4>0 is the control gain; ∈ o = 10 -6 ; I2 is the 2nd order identity matrix, is the tangent vector when the arc length parameter on the generating line is , is transpose of

[0034] Preferably, in step S2, the expression of the saturated density regulation term u ρ,i is:

[0035] u ρ,i = sat(u 5,i , v ρ,m )

[0036]

[0037] where, v ρ,m is the saturation upper limit of the density regulation term, α(p i , t)>0 represents the control gain, represents calculating the gradient, represents the estimator of the spatial density function of the i-th robot, ρd (p i , t) represents the expected spatial density function of the i-th robot.

[0038] Preferably, the step S3 specifically includes:

[0039] Step S3-1, initialize the motion duration, time step, and time parameter, and divide the motion duration into multiple time steps based on the time step;

[0040] Step S3-2, for the time parameter of the current time step, calculate the forward term along the generation line, saturated safety control term, saturated density regulation term, and saturated total velocity command; update the positions of each robot by the saturated total velocity command; finally, update the time parameter of the current time step;

[0041] Step S3-3, return to step S3-2 until the time parameter of the current time step is greater than the motion duration, then complete the calculation and obtain the positions of each robot after each update of the time parameter.

[0042] Preferably, the step S3-1 specifically includes:

[0043] Determine the motion duration T of the robot cluster, set the time step as ΔT k , and divide T into K steps based on the time step, initialize the time parameter t = 0;

[0044] The step S3-2 specifically includes:

[0045] Calculate the forward term along the generation line, saturated safety control term, and saturated density regulation term of the current time step through the robot cluster navigation model determined in step S2; considering the constraint of the maximum speed v max , obtain the saturated velocity control command applied to the i-th robot:

[0046] v i = sat(u 1,i + u s,i + u ρ,i , v max )

[0047] Update the position of the robot through the saturated velocity command v i , and the expression is:

[0048] p i = p i + v i ·ΔV k

[0049] Update the time parameter, and the expression is:

[0050] t = t + Δt

[0051] Among them, p i represents the position of the i-th robot, and t represents the time parameter.

[0052] Compared with the prior art, the present invention has at least the following beneficial effects:

[0053] (1) By setting the forward term to move along the generation line at a constant maximum speed in the robot swarm navigation model, the present invention simplifies the calculation process and improves the calculation efficiency of the navigation algorithm. The setting of the constant maximum speed enables the robot swarm to cross the virtual pipeline at the fastest speed, effectively improving the overall operation efficiency.

[0054] (2) When designing the collision avoidance term, the present invention only considers the component in the direction orthogonal to the generation line, so that the collision avoidance effect does not interfere with the forward movement of the robot along the generation line. At the same time, by reducing the repulsive potential field of the collision avoidance term, unnecessary collision avoidance actions are reduced, energy consumption is lowered, and the movement efficiency of the robot is further optimized.

[0055] (3) The present invention adopts a multi-level saturation constraint strategy to finely manage the forward term, the safety control term, and the density regulation term. This hierarchical constraint method not only ensures that each control signal operates within the effective range, improves the system stability and safety, but also effectively prevents the over-response of the control signal, reduces energy consumption, and realizes more coordinated navigation control.

[0056] (4) The control method of the present invention is not only applicable to the task of passing through a virtual pipeline with obstacles in a narrow space, but can also be flexibly applied to other virtual pipelines with complex geometric shapes, such as trapezoidal pipelines, curved pipelines, and annular pipelines. In various types of virtual pipeline environments, the present invention can achieve stable and efficient robot swarm navigation, and the control effect is better than that of traditional methods. This wide applicability makes the present invention have important application value in the field of robot swarm navigation in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] The drawings are only for the purpose of illustrating specific embodiments and are not considered to be a limitation of the present invention.

[0058] Figure 1 is a flowchart of the method for guiding a robot swarm to cross a virtual pipeline using density control provided by the present invention.

[0059] Figure 2 is a schematic diagram of a virtual pipeline provided by the present invention.

[0060] Figure 3 is a detailed step flowchart of the method for guiding a robot swarm to cross a virtual pipeline using density control provided by the present invention

[0061] Figure 4Schematic diagram of the initial distribution of a narrow virtual pipeline with obstacles and a robot swarm provided by the present invention.

[0062] Figure 5 Schematic diagram of the positions of robots at different times provided by the present invention.

[0063] Figure 6 Schematic diagram of the evolution of the shortest distance between robots provided by the present invention.

[0064] Figure 7 Schematic diagram of the evolution of the shortest distance between a robot and the pipeline boundary provided by the present invention.

[0065] Figure 8 Schematic diagram of the evolution of the shortest distance between a robot and a static obstacle provided by the present invention.

[0066] Figure 9 Schematic diagram of the motion trajectories of all robots provided by the present invention. Detailed implementation manners

[0067] In order to more clearly understand the above objects, features and advantages of the present invention, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific implementation manners. It should be noted that, without conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. In addition, the present invention can also be implemented in other ways different from those described herein. Therefore, the protection scope of the present invention is not limited by the specific embodiments disclosed below.

[0068] Aiming at the problems such as high congestion and collision risks faced by robot swarms in narrow virtual pipelines in the prior art, the present invention provides a method that can guide robot swarms to safely and efficiently cross narrow virtual pipelines with obstacles by using density control, and solves the challenges such as congestion and high collision risks faced by the existing control methods in narrow virtual pipelines.

[0069] The present invention first establishes a general virtual pipeline model on a two-dimensional plane and defines the area and flowability of the virtual pipeline. Based on this, a narrow virtual pipeline model is established; then a kinematic model of a robot with constrained speed is established; then a spatial density function of the robot swarm in the virtual pipeline is defined, and a density evolution model of the robot swarm is established; finally, a controller is determined to guide the robot swarm to safely and efficiently cross the narrow virtual pipeline with obstacles.

[0070] The following content first describes the virtual pipeline model, the robot kinematic model, the density evolution model of the robot swarm, and the controller model provided by the present invention.

[0071] Virtual pipeline model

[0072] A general virtual pipeline model consists of a generating line γ(l) and a cross-section where l is the arc length parameter. As Figure 2 shown, consider a point p(l) on the generating line γ(l), and let the unit tangent vector at this point be t c (l), and the unit normal vector be m c (l). Then define the cross-section passing through the point p(l) as where x is a two-dimensional vector representing a point on the cross-section of the virtual pipeline, λ(l) is a pipeline definition variable, and its value range is [-r d (l), r u (l)], and r d (l) and r u (l) are the widths of the lower and upper halves of the cross-section respectively. A general virtual pipeline is a set in a two-dimensional plane defined as:

[0073]

[0074] where represents the union of the cross-sections of the arc length parameter l in the range [0, L], and L is the maximum value of the arc length parameter l.

[0075] The boundary of the virtual pipeline is defined as where p d (l) = p(l) - r d (l)n c (l), p u (l) = p + r u (l)n c (l).

[0076] where is the cross-section of the virtual pipeline at l = 0, is the cross-section of the virtual pipeline at l = L, p d (l) is the point on the lower boundary of the virtual pipeline, and p u (l) is the point on the upper boundary of the virtual pipeline.

[0077] Narrow virtual pipeline model

[0078] Define the expression for the area S of the virtual pipeline as:

[0079]

[0080] Define the cross-section based on the area​ The expression for the fluidity σ(l) is as follows:

[0081]

[0082] where ΔS represents the change in area S, Δl represents the change in arc length parameter l, and r c (s) represents the cross-section at arc length parameter s radius, and r c (l) represents the cross-section at arc length parameter l radius. The calculation expression for r c (l) is r c (l) = (r d (l) + r u (l)) / 2.

[0083] Set r s as the safety radius of the robot. If there exists l ∈ [0, L] such that r s < σ(l) ≤ 2r s , then the virtual pipeline is called a narrow virtual pipeline.

[0084] As Figure 4 shown, there are several circular static obstacles in the virtual pipeline, and the radius of the obstacle is r o . During the process of the robot cluster passing through the virtual pipeline, it cannot collide with the obstacles.

[0085] Single robot kinematic model

[0086] The present invention considers a typical single robot kinematic model:

[0087] dp i = v i dt, i = 1, …, N

[0088] where N is the number of robots in the cluster, represents the position of the i-th robot, dp i represents the differential of p i , dt represents the differential of time t, represents the speed of the i-th robot, and v(p i , t) represents the value of the global velocity vector field v acting on the position p i of the i-th robot at time t. Additionally, v i is subject to an energy constraint and has a maximum speed v max , so v i can be expressed as:

[0089] v i = sat(ui , v max ) = κ m (u i )(u i )

[0090] where u i is the unsaturated velocity of the i-th robot, sat(u i , v max ) and κ m (u i ) are expressed as:

[0091]

[0092] where ∥·∥ represents calculating the modulus of a vector.

[0093] Robot swarm density evolution model

[0094] For any point inside the virtual pipeline take a circular neighborhood around it ∈(p, t) represents the radius of the circular neighborhood. Denote the number of robots in U(p, ∈(p, t)) as N U , and the area of U(p, ∈(p, t)) as S U = π∈(p, t) 2 . Then the density of robots at point p can be estimated as:

[0095]

[0096] Introduce a normalization factor dS represents the area element of the virtual pipeline. Then define the expression of the spatial density function ρ(p, t) as:

[0097]

[0098] Similar to the continuity equation in fluid mechanics, an evolution model of ρ(p, t) can be established:

[0099]

[0100] where, represents the partial derivative of the spatial density function with respect to time t, represents the divergence of ρv obtained by multiplying the spatial density by the velocity vector field, represents that the value range of point p is any point inside the virtual pipeline , the value range of time t is [0, T], T represents the total time; ρ0 represents the initial spatial density function, represents that the value range of point p is the virtual pipeline For any point within, time t only takes 0; n is the unit outer normal vector on the boundary where The value range of point p indicates that it is the boundary of the virtual pipeline and the value range of time t is [0, T].

[0101] Controller Design

[0102] The controller determined by the present invention includes 3 control items: the forward item u along the generation line 1,i , the saturated safety control item u s,i (specifically including: the collision avoidance item u between robots 2,i , the item u that restricts the robot within the pipeline 3,i and the collision avoidance item u between the robot and the obstacle 4,i ), and the saturated density regulation item u ρ,i . The following is a detailed description:

[0103] (1) The forward item u along the generation line 1,i

[0104] To enable the navigation robot cluster to cross the virtual pipeline along the generation line, first set the expression of the forward item along the generation line as:

[0105]

[0106] where k1 represents the maximum forward speed, is the tangent vector when the arc length parameter on the generation line is . By setting the forward item in the above manner, the forward item is expressed as moving forward along the generation line at a constant maximum speed. The calculation method is simple, which can improve the calculation efficiency of the navigation algorithm. At the same time, setting a constant maximum speed enables the robot cluster to cross the virtual pipeline as quickly as possible.

[0107] (2) The saturated safety control item u s,i

[0108] The safety control item consists of 3 sub-items, namely: the collision avoidance item u between robots 2,i , the item u that restricts the robot within the pipeline 3,i and the collision avoidance item u between the robot and the obstacle 4,i . The detailed introduction of each sub-item is as follows. First, it is the collision avoidance item u between robots 2,i :

[0109]

[0110] where k2 > 0 is the collision avoidance item control gain, represents the relative position between the i-th robot and the j-th robot, Denote V m,ij with respect to the partial derivative of, Denote the set of neighbors of the i-th robot, where r s is the safety radius of the robot, r a is the collision avoidance radius of the robot; is the potential function to prevent collisions between robots, For the function σ(x, d1, d2), its definition is:

[0111]

[0112] where the polynomial coefficients are: s(x, ∈ s ) is defined as:

[0113]

[0114] where x1 = x2 - sin45° ∈ s , x is the independent variable of the σ and s functions, d1, d2, ∈ m , ∈ s are all control parameters. In the design of the control term, d1 = 2r s , d1 = r s + r a , ∈ m = ∈ s = 10 -6 .

[0115] The term u that restricts the robot inside the pipeline 3,i has the expression:

[0116]

[0117] where k3 > 0 is the control gain that restricts the robot inside the pipeline, is the potential function that restricts the robot inside the pipeline, where d t,i represents the distance between the position p of the current robot i and the pipeline boundary, σ t (d t,i ) = σ(d t,i , r s , r a ), ∈ t is generally chosen as: ∈ t = 10 -6 ; Denote the partial derivative of V t,i with respect to p i of.

[0118] Collision avoidance term u between the robot and the obstacle 4,i The expression is as follows:

[0119]

[0120] where p o,ik represents the k-th obstacle that the i-th robot can sense, represents the relative position between the i-th robot and the k-th obstacle. is defined as the potential function to prevent collisions between robots, r o represents the radius of the obstacle; represents the set of obstacles that the i-th robot can sense, k4>0 is the control gain; ∈ o is generally selected as: ∈ o = 10 -6 ; I2 is the 2nd order identity matrix, is the tangent vector when the arc length parameter on the generation line is , is transpose of.

[0121] By setting the collision avoidance term in the above way, the component parallel to the tangent direction of the generation line is not considered, and the obtained value is orthogonal to u . The effect of the collision avoidance term is only applied to the direction orthogonal to the generation line direction. In this way, the collision avoidance effect will not affect the advancement of the robot along the generation line, so that the robot can cross the virtual pipeline more quickly. In addition, by setting the collision avoidance term in the above way, the repulsive potential field of the collision avoidance term is reduced, which can effectively avoid unnecessary collision avoidance actions, reduce energy consumption, and improve the overall efficiency of the robot movement. 1,i

[0122] Combining the above 3 control sub-items, the safety control item can be directly determined as u 2,i + u 3,i + u 4,i . However, considering the actual control cost constraint and ensuring that the safe navigation item will not affect the advancement item u 1,i along the generation line too much, a saturation constraint is imposed on the safety control item, and the saturated safety control item is:

[0123] u s,i = sat(u 2,i + u 3,i + u 4,i , v s,m )

[0124] where v s,m ​is the saturation upper limit of the safety control term. v s,m is a human-designed and adjustable hyperparameter, generally not exceeding the maximum speed constraint v of the robot max , that is, v s,m ≤v max .

[0125] Applying a saturation constraint to the safety control term can effectively limit the maximum value of the safety control term, thus avoiding excessive interference with the forward term along the generation line. This makes the control signal change more smoothly, reduces the risk of system instability, and improves the efficiency and safety of the overall operation.

[0126] (3) The saturated density regulation term u ρ,i

[0127] Before determining the density regulation term, it is necessary to determine the desired spatial density function ρ d , as follows:

[0128]

[0129] Among them, and respectively represent the arc length parameters corresponding to the positions of the last and the frontmost robots in the cluster on the generation line; represents the occupied area of the robot cluster, r c (l p ) represents the radius of the cross-section at the arc length parameter l p , represents the area in the virtual pipeline excluding the occupied area of the robot cluster.

[0130] Since the actual spatial density function ρ(p,t) is difficult to be accurately calculated, the present invention uses kernel density estimation (KDE) to estimate ρ(p,t), and the estimator of the spatial density function is expressed as:

[0131]

[0132] Among them, is the Gaussian kernel function, y represents the independent variable of the Gaussian kernel function, and h is the bandwidth.

[0133] With ρ d and Based on the density evolution model of the robot cluster established above, the density regulation term can be determined:

[0134]

[0135] Among them, α(p i ,t)>0 represents the control gain, Denotes the calculated gradient, Denotes the estimator of the spatial density function of the i-th robot, ρ d (p i , t) Denotes the expected spatial density function of the i-th robot.

[0136] Similar to the reason for determining the saturated safety control term u s,i , it is necessary to impose a saturation constraint on the density regulation term. Therefore, the saturated density regulation term is determined as:

[0137] u ρ,i = sat(u 5,i , v ρ,m )

[0138] Where v ρ,m Is the saturation upper limit of the density regulation term. v ρ,m Is an artificially designed and adjustable hyperparameter.

[0139] (4) Saturated velocity control command

[0140] Combining the forward term u 1,i , the saturated safety control term u s,i And the saturated density regulation term u ρ,i , and at the same time considering the constraint of the maximum velocity v max , the saturated velocity control command can be obtained as:

[0141] v i = sat(u 1,i + u s,i + u ρ,i , v max ) = κ m (u 1,i + u s,i + u ρ,i )

[0142] v max Is determined by the maneuverability of the robot, i.e., the constraint of the maximum energy that can be input to the robot.

[0143] In the above way, the present invention imposes a saturation constraint based on the maximum velocity on the forward term, the saturated safety control term, and the saturated density regulation term again. By applying the saturation constraint at multiple levels, the present invention can manage and optimize the influence of each control term more precisely. First, imposing a saturation constraint on the forward term, the saturated safety control term, and the saturated density regulation term can ensure that each control signal operates within its effective range. Second, the multi-level saturation constraint strategy of the present invention can not only improve the stability and safety of the system, but also prevent the over-response of the control signal and reduce energy consumption. Finally, it helps to achieve more coordinated navigation and motion control in a complex environment, improving the overall performance and reliability of the system.

[0144] To illustrate the effectiveness of the method proposed by the present invention, the above technical solution of the present invention will be described in detail through a specific embodiment below. A specific embodiment of the present invention is as follows Figure 1 , Figure 3 shown, which discloses a method for guiding a robot cluster to cross a virtual pipeline by density control. The specific implementation steps are as follows:

[0145] Step S1: Determine the shape of the virtual pipeline, determine the position of the obstacles in the virtual pipeline, and determine the initial positions of the robots in the robot cluster;

[0146] The present invention first determines the shape of the virtual pipeline. As Figure 4 shown, the narrow positions in the virtual pipeline are marked with light blue shadows, and there are static obstacles in the virtual pipeline, which are represented by black solid circles.

[0147] When determining the shape of the virtual pipeline, it includes determining the generation line data γ(l), the unit tangent vector data t c (l), the unit normal vector data n c (l), and the pipeline radius related data r d (l) and r u (l) values. With these data, the virtual pipeline as Figure 4 shown can be generated and stored on the computer for convenient calling and calculation in the following steps.

[0148] Determine the initial positions of the robots in the robot cluster. In this embodiment, N = 25 robots are arranged in a 5×5 square, and any two adjacent robots are equidistant. The positions of the 25 robots are respectively stored as p i , i = 1, …, 25.

[0149] Step S2: Determine the robot cluster navigation model, and the robot cluster navigation model includes a forward term, a saturated safety control term, and a saturated density control term;

[0150] (1) Forward term along the generation line

[0151] For the i-th robot, i = 1, …, 25, first obtain the arc length parameter i corresponding to the position p of the robot on the generation line Then obtain the forward term u along the generation line through the following formula 1,i :

[0152]

[0153] (2) Saturated safety control term

[0154] First, calculate the relative position between the $i$-th robot and the $j$-th robot Further calculate the collision avoidance term $u$ between robots 2,i ; then calculate the distance $d$ between the $i$-th robot and the pipeline boundary t,i , and further calculate the control term $u$ that restricts the robot within the pipeline 3,i ; finally, calculate the relative position between the $i$-th robot and the $j$-th obstacle that the robot can sense Further calculate the collision avoidance term $u$ between the robot and the static obstacle 4,i . These three terms constitute the safety control term of the robot in the virtual pipeline, and finally obtain the saturated safety control term:

[0155] $u$ s,i $ = \text{sat}(u$ 2,i $ + u$ 3,i $ + u$ 4,i , v$ s,m )

[0156] where the calculation expressions of $u$ 2,i , $u$ 3,i and $u$ 4,i have been described in detail in the previous text and will not be elaborated here

[0157] (3) Saturated density control term

[0158] Obtain the current position data of the robot Then obtain the cross-sectional radius data $r$ of the area occupied by the robot swarm at the current moment (l), and finally calculate the expected spatial density function $\rho$ based on this data c (l), and finally calculate the expected spatial density function $\rho$ based on this data d . The calculation expression of $\rho$ d has been described in detail in the previous text

[0159] Set the control gain $\alpha = 1$ of the density regulation control term, and then use kernel density estimation (KDE) to calculate the density estimate at the current position Then use the $\rho$ calculated in the previous step d , and calculate the density regulation control term $u$ 5,i . Finally, apply a saturation constraint to $u$ 5,i , and obtain the saturated density control term:

[0160] $u$ ρ,i $ = \text{sat}(u$ 5,i , v$ ρ,m )

[0161] where the calculation expression of $u$ 5,i has been described in detail in the previous text

[0162] Step S3: Based on the virtual pipeline shape and the initial positions of each robot, according to the robot cluster navigation model, update the positions of each robot at each time step in multiple time steps, and obtain the positions of each robot at multiple time steps.

[0163] In this step, to update the positions of each robot at each time step in multiple time steps, the specific steps are as follows:

[0164] Step S3-1: Initialize the motion duration, time step size, and time parameter. Divide the motion duration evenly into multiple time steps based on the time step size.

[0165] Determine that the motion duration T of the robot cluster is 30 s, set the time step size as ΔT k = 0.05 s. Divide T evenly into K steps based on the time step size, initialize the time parameter t = 0, and use the time parameter to mark the time step. Perform subsequent calculations based on the virtual pipeline shape and the initial positions of each robot.

[0166] Step S3-2: For the time parameter of the current time step, calculate the forward term along the generation line, the saturated safety control term, the saturated density regulation term, and the saturated total velocity command; update the positions of each robot with the saturated total velocity command; finally, update the time parameter of the current time step.

[0167] In this step, first calculate the forward term along the generation line, the saturated safety control term, and the saturated density regulation term through the robot cluster navigation model determined in Step S2. Then, considering the above results and the constraint of the maximum velocity v max at the same time, obtain the saturated velocity control command applied to the i-th robot:

[0168] v i = sat(u 1,i + u s,i + u ρ,i , v max ) = κ m (u 1,i + u s,i + u ρ,i )

[0169] Then, update the position of the robot through the saturated velocity command v i , and the expression is:

[0170] p i = p i + v i ·ΔT k

[0171] Finally, update the time parameter, and the expression is:

[0172] t = t + ΔT k

[0173] Step S3-3: Return to Step S3-2. Until the time parameter of the current time step is greater than the motion duration, the calculation is completed, and the positions of each robot after each update of the time parameter are obtained.

[0174] After the loop, if t > T, it is determined that the task of the robot cluster passing through the narrow virtual pipeline within time T is completed. The positions of all drones are updated. The positions of the robots at t = 2s, t = 10s, t = 21s, and t = 30s are as Figure 5 shown. The blue arrows in the figure indicate the speeds of the robots at the current moment. Figure 6 represents the distance between robots during the passing process, Figure 7 represents the distance between the robot and the pipeline boundary, Figure 8 represents the distance between the robot and the static obstacle, Figure 6 , Figure 7 and Figure 8 illustrate that there are no collisions between robots, between the robot and the pipeline boundary, and between the robot and the static obstacle during the passing process.

[0175] Step S4: Determine the motion trajectories of all robots from the positions of each robot at the multiple time steps, and complete the navigation of the robot cluster through the virtual pipeline.

[0176] Connect the positions of each robot at the multiple time steps to determine the motion trajectories of all robots. Figure 9 Draw the motion trajectory diagram of all robots within T = 30s.

[0177] Although the specific implementation manners of the present invention depict various actions or steps in a specific order, this should be understood as requiring such actions or steps to be executed in the specific order shown or in a sequential order, or requiring all the illustrated actions or steps to be executed to achieve the desired result. In certain environments, multitasking and parallel processing may be beneficial. Similarly, although several specific implementation details are included in the above discussion, these should not be construed as limitations on the scope of the present disclosure. Certain features described in the context of separate embodiments can also be implemented in combination in a single implementation. On the contrary, various features described in the context of a single implementation can also be implemented separately or in any suitable sub-combination in multiple implementations.

[0178] The above is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed by the present invention should be covered by the protection scope of the present invention.

Claims

1. A method for guiding a robot cluster to cross a virtual pipeline using density control, characterized in that: The following steps are involved: Step S1, determining the shape of the virtual pipeline and the positions of obstacles in the virtual pipeline, and determining the initial positions of each robot in the robot cluster; Step S2, determining a robot cluster navigation model, and determining control instructions for the robot cluster navigation model, wherein the control instructions include a forward item along a generated line, a saturated safety control item, and a saturated density control item; Step S3, based on the virtual pipeline shape and the initial position of each robot, according to the robot cluster navigation model and the control instruction, updating the position of each robot in each time step in multiple time steps, and obtaining the position of each robot in multiple time steps; Step S4: determining the motion trajectories of all robots based on the positions of the robots at the multiple time steps, and completing the navigation of the robot cluster through the virtual pipeline.

2. The method for guiding a robot cluster to cross a virtual pipeline by using density control according to claim 1, characterized in that: Step S1 specifically includes: Step S1-1, determine the shape of the virtual pipeline, including: determine the generating line γ(l), the unit tangent vector t c (l), unit normal vector n c (l) and the width r of the lower and upper parts of the pipe d (l) and r u (l) value; Step S1-2, determining the position of the obstacle in the virtual pipeline; determining the initial position of each robot in the robot cluster, including: arranging the robots in a square, and any two adjacent robots are equidistant.

3. The method for guiding a robot cluster to cross a virtual pipeline by using density control according to claim 2, characterized in that: In step S2, the step of determining the control instructions of the robot cluster navigation model specifically includes: Determining the advancement term along the generating line based on the tangent vector of the arc length on the generating line and the maximum advancement speed; Determining the saturated safety control item based on the positional relationship between the robots, the positional relationship between the robot and the pipeline boundary, and the positional relationship between the robot and the obstacle; The saturation density control item is determined based on the distribution density of the robot cluster.

4. The method for guiding a robot cluster to cross a virtual pipeline by using density control according to claim 3, characterized in that: In step S2, the forward term u along the generating line 1,i The expression is: Among them, i is the robot index, k1 represents the maximum forward speed, The arc length parameter on the generated line is The tangent vector at .

5. The method for guiding a robot cluster to cross a virtual pipeline by using density control according to claim 4, characterized in that: In step S2, the expression of the saturated safety control item is: in s,i =hour(in 2,i +in 3,i +in 4,i ,v s,m ) where v s,m is the saturation upper limit of the safety control item, and sat(·,·) represents the maximum limit function; Among them, u 2,i represents the collision avoidance term between robots, k2>0 is the collision avoidance term control gain, represents the relative position between the ith robot and the jth robot, Indicates V m,ij Related to The partial derivative of represents the neighbor set of the ith robot, where r s is the robot's safety radius, r a is the robot’s collision avoidance radius; is the potential function to prevent collisions between robots, For the function σ(x,d1,d2), it is defined as: The polynomial coefficient is: A = -2 / (d1-d2) 3 ,B=3(d1+d2) / (d1-d2) 3 ,C=-6d1d2 / (d1-d2) 3 , s(x,∈ s ) is defined as: Among them, x is the independent variable of σ and s functions, d1,d2,∈ m ,∈ s are control parameters; in the design of control items, d1=2r s , d1=r s +r a ,∈ m =∈ s =10 -6 ; Among them, u 3,i represents the item that restricts the robot in the pipeline, k3>0 is the control gain that restricts the robot in the pipeline, is the potential function that restricts the robot inside the pipe, where d t,i Indicates the current position of the robot p i Distance from the pipe boundary, σ t (d t,i )=σ(d t,i ,r s ,r a ),∈ t =10 -6 ; Indicates V t,i Related to p i The partial derivative of Among them, u 4,i represents the collision avoidance term between the robot and the obstacle, represents the relative position between the ith robot and the kth obstacle, p o,ik represents the kth obstacle that the ith robot can sense, is defined as the potential function that prevents collisions between robots, r o Indicates the radius of the obstacle; represents the set of obstacles that the ith robot can sense, k4>0 is the control gain; ∈ o =10 -6 ; I2 is the unit matrix of order 2, The arc length parameter on the generated line is The tangent vector at yes The transpose of .

6. The method for guiding a robot cluster to cross a virtual pipeline by using density control according to claim 5, characterized in that: In step S2, the saturated density control term u ρ,i The expression is: u ρ,i =sat(u 5,i ,v ρ,m ) Among them, v ρ,m is the saturation upper limit of the density control term, α(p i ,t)>0 indicates control gain, represents the calculation of gradient, represents the estimator of the spatial density function of the ith robot, ρ d (p i ,t) represents the expected spatial density function of the i-th robot.

7. The method for guiding a robot cluster to cross a virtual pipeline by using density control according to claim 6, characterized in that: The step S3 specifically includes: Step S3-1, initializing the movement duration, time step and time parameter, and dividing the movement duration into multiple time steps based on the time step; Step S3-2, for the time parameters of the current time step, calculate the forward term along the generated line, the saturated safety control term, the saturated density control term and the saturated total speed command; update the position of each robot according to the saturated total speed command; and finally update the time parameters of the current time step; Step S3-3, return to step S3-2, until the time parameter of the current time step is greater than the movement duration, complete the calculation, and obtain the position of each robot after each time parameter is updated.

8. The method for guiding a robot cluster to cross a virtual pipeline by using density control according to claim 7, characterized in that: The step S3-1 specifically includes: Determine the movement duration T of the robot cluster and set the time step to ΔT k , based on the time step, T is evenly divided into K steps, and the time parameter t=0 is initialized; The step S3-2 specifically includes: The robot cluster navigation model determined in step S2 calculates the forward term along the generation line, the saturated safety control term, and the saturated density control term of the current time step; considering the maximum speed v max The constraints of , the saturated speed control command applied to the i-th robot is obtained: v i =sat(u 1,i +u s,i +u ρ,i ,v max ) Through the saturation speed command v i Update the robot's position. The expression is: p i =p i +v i ·ΔT k Update time parameter, the expression is: t=t+ΔT k Among them, p i represents the position of the ith robot, and t represents the time parameter.

Images