Multi-robot cluster cooperative formation distributed control method and system

Abstract

The embodiment of the application provides a multi-robot cluster cooperative formation distributed control method and system, which is applied to the technical field of robot control, and the method establishes a local neighborhood window with each robot position as the center, calculates a local density observation value through kernel density estimation, combines an underwater manifold operation map to construct a global expected density distribution, and constructs a density potential field function according to the deviation of the two. The local mass conservation constraint is introduced, the density flux correction term satisfying the neighborhood boundary zero flux characteristic is solved, the distributed drift velocity, the high-pressure water jet reaction force feedforward term and the obstacle avoidance speed term are fused to generate a combined speed vector, the robot is driven to move through thrust distribution, and the neighborhood window is adaptively updated. The application does not need global communication and central scheduling, can offset the jet recoil interference, realizes the balance of obstacle avoidance and formation cooperation, and adapts to the uniform coverage operation under the complex structure of the manifold.

Description

Technical Field

[0001] This application relates to the field of robot control technology, and in particular to a distributed control method and system for collaborative formation of multiple robot swarms. Background Technology

[0002] Current underwater manifold cleaning methods mostly employ single-robot operations, which suffer from low efficiency and numerous blind spots when dealing with large-area manifolds or complex junctions. While multi-robot collaboration can improve efficiency, traditional centralized swarm control relies on global communication and central node scheduling. Underwater acoustic communication suffers from high latency and is prone to interruption. If the central node fails or communication fails, the entire swarm will become uncontrollable, making it difficult to adapt to complex underwater operating environments.

[0003] Some distributed formation methods employ fixed formation control strategies, failing to consider the curved surface structure of underwater manifolds and the cleaning coverage requirements. This can lead to robots exhibiting localized clustering or excessive sparseness, resulting in uneven cleaning. Simultaneously, the reaction force generated by the high-pressure water jet continuously interferes with the robot's pose. Existing solutions often rely on feedback adjustment and compensation, resulting in strong response lag and potential trajectory deviations, thus affecting cleaning quality.

[0004] Underwater robot swarm density perception relies heavily on global positioning information, lacking sufficient local perception capabilities. When robots operate in dense manifold areas, the lack of density quantification methods for local neighborhoods makes it difficult to dynamically adjust swarm distribution. Furthermore, existing obstacle avoidance strategies are prone to conflict with formation control; robots may easily break away from the formation when encountering obstacles, or excessive obstacle avoidance maneuvers may disrupt overall coverage uniformity, increasing the probability of repetitive tasks. Summary of the Invention

[0005] The embodiments of this application provide a distributed control method and system for multi-robot swarm collaborative formation. This method enables multiple underwater robots to adaptively maintain uniform formation without global communication, while simultaneously counteracting jet recoil interference and smoothly avoiding obstacles. It is suitable for cleaning operations involving complex underwater manifold structures. To achieve the above objectives, this application adopts the following technical solution: A distributed control method for cooperative formation of multiple robots, the method comprising: A local neighborhood window is established with each robot's position as the center, and the local density observation value within the window is calculated using the kernel density estimation method. Obtain a map of the operating area of ​​the underwater manifold, construct a global expected density distribution, and query the corresponding expected local density value based on the current location; Calculate the deviation between the observed local density value and the expected local density value, and construct the density potential field function; By introducing a local mass conservation constraint, the normal flux of the normal gradient of the density potential field function on the boundary of the neighborhood window is set to zero. Solve for the density flux correction term based on the normal flux constraint; Calculate the distributed drift velocity of each robot based on the density flux correction term; The resultant velocity vector is generated by integrating the distributed drift velocity, the jet reaction force feedforward term of the high-pressure water jet, and the obstacle avoidance velocity term. The resultant velocity vector is converted into a thrust distribution command for the underwater robot's thruster; The thrust distribution command is executed to drive the robot to move, and the local neighborhood window is updated using the position data after the movement.

[0006] In some possible implementations, solving for the density flux correction term based on the normal flux constraint includes: Obtain the pump pressure data of the high-pressure water jet; The amplitude of the jet reaction force of the high-pressure water jet is calculated based on the pump pressure data; Construct a feedforward gain mapping relationship with the amplitude of the reaction force as the input; The density flux correction term is scaled using the feedforward gain mapping relationship to generate a feedforward compensation component; The feedforward compensation component is superimposed on the density flux correction term, and the compensated density flux correction term is output.

[0007] In some possible implementations, updating the local neighborhood window using the position data after motion includes: Count the number of neighboring robots within the local neighborhood window and generate neighbor connectivity parameters; Compare the neighbor connectivity parameter with a preset connectivity lower limit threshold; When the neighbor connectivity parameter is lower than the connectivity lower limit threshold, the bandwidth parameter currently used in the kernel density estimation method is extracted. The bandwidth parameters are corrected according to a preset magnification ratio to generate updated bandwidth parameters; The original bandwidth parameters are replaced with the updated bandwidth parameters, and the spatial range of the local neighborhood window is redefined according to the updated bandwidth parameters, with the updated position data of each robot as the center. The newly defined local neighborhood window will be used as the current local neighborhood window for the next control cycle.

[0008] In some possible implementations, the calculation of the deviation between the observed local density and the desired local density, and the construction of the density potential field function, includes: Calculate the difference between the observed local density value and the expected local density value to generate a density bias; Set a local density safety threshold; Compare the observed local density value with the local density safety threshold. When the observed local density value is greater than the local density safety threshold, the density deviation is extracted as the density over-limit deviation. Multiply the density excess deviation by a preset repulsion gain coefficient to generate a repulsion potential energy term; Using the density deviation as the input variable and combining it with the repulsive potential energy term, a density potential field function including the basic gravitational component and the additional repulsive component is constructed.

[0009] In some possible implementations, solving for the density flux correction term based on the normal flux constraint includes: Define the graph Laplace operator within the local neighborhood window; Collect local density observations and relative position vectors of neighboring robots; An adjacency matrix is ​​constructed using the relative position vectors, and a degree matrix is ​​constructed by combining the local density observations. The effect of the graph Laplacian operator is calculated based on the adjacency matrix and the degree matrix. The result of the graph Laplace operator is substituted into the Poisson equation solver as a constraint condition. The Poisson equation solver outputs a density flux correction term that satisfies the normal flux constraint.

[0010] In some possible implementations, obtaining the operational area map of the subsea manifold and constructing a global desired density distribution includes: Identify the geometric centerline of the underwater manifold; Multiple path points are extracted along the geometric centerline to construct a path point sequence; Calculate the normal cross section at each point on the path, and generate a set of normal cross sections perpendicular to the geometric center line; A Gaussian density profile is defined on the set of normal sections, wherein the peak value of the Gaussian density profile is located on the geometric center line; Based on the Gaussian density profile and the path point sequence, a global desired density distribution is synthesized.

[0011] In some possible implementations, the process of fusing the distributed drift velocity, the jet reaction force feedforward term of the high-pressure water jet, and the obstacle avoidance velocity term to generate a resultant velocity vector includes: Obtain obstacle distance data collected by the obstacle avoidance sensors mounted on the underwater robot; When the obstacle distance data is less than the preset safe distance, an original obstacle avoidance vector pointing away from the obstacle is generated; Calculate the tangential and normal components of the original obstacle avoidance vector; The density flux correction term is corrected using the normal component to limit the robot's tendency to traverse obstacles; The distributed drift velocity is corrected using the tangential component to guide the robot to glide along the contour of the obstacle; The modified density flux correction term and the modified distributed drift velocity are vector-summed to generate an intermediate velocity vector; The intermediate velocity vector is vector-superimposed with the feedforward term of the jet reaction force of the high-pressure water jet to generate the resultant velocity vector.

[0012] In some possible implementations, the conversion of the resultant velocity vector into thrust distribution instructions for the underwater robot thruster includes: Establish a thruster configuration matrix for an underwater robot, wherein the configuration matrix represents the geometric relationship between the installation position of each thruster and the thrust direction; Acquire the roll and pitch angle data collected by the attitude sensors of the underwater robot; Calculate the decomposition components of gravity in the body coordinate system based on the aforementioned roll and pitch angle data; The gravity decomposition components are superimposed on the resultant velocity vector to generate a total force vector including gravity compensation. Multiply the total force vector on the left by the pseudo-inverse of the thruster configuration matrix to solve for the required rotational speed of each thruster; The rotational speed value is used as the thrust distribution command.

[0013] In some possible implementations, executing the thrust distribution command to drive the robot to move and updating the local neighborhood window using the position data after the movement includes: Record the sequence of actual trajectory points during the underwater robot's execution of the thrust distribution command; Calculate the lateral deviation between the actual trajectory point sequence and the preset ideal cleaning path; When the lateral deviation is greater than a preset trajectory correction threshold, a trajectory correction vector is generated; An additional potential field term is constructed using the trajectory correction vector, and the additional potential field term is superimposed on the density potential field function to generate a corrected density potential field function. Write the modified density potential field function into the memory, replacing the original density potential field function; The local neighborhood window is updated using the position data in the actual trajectory point sequence.

[0014] A distributed control system for collaborative formation of multiple robots, the system comprising: The situational awareness module is used to establish a local neighborhood window centered on each robot's position, calculate the local density observation value within the window using the kernel density estimation method, acquire the operation area map of the underwater manifold, construct the global expected density distribution, and query the corresponding expected local density value based on the current position. The decision planning module is used to calculate the deviation between the observed local density value and the desired local density value, and construct the density potential field function; introduce local mass conservation constraints, and set the normal flux of the normal gradient of the density potential field function at the boundary of the neighborhood window to zero; solve the density flux correction term based on the normal flux constraint, and the correction term characterizes the incompressible characteristics of the robot swarm at the microscale. The motion calculation module is used to calculate the distributed drift velocity corresponding to each robot based on the density flux correction term; to fuse the density flux correction term after high-pressure water jet reaction force feedforward compensation with the distributed drift velocity and obstacle avoidance velocity term to generate a resultant velocity vector; and to convert the resultant velocity vector into a thrust distribution command for the underwater robot thruster. The execution feedback module is used to execute the thrust distribution command to drive the robot to move; when the lateral deviation of the trajectory is greater than a preset threshold, an additional potential field term for trajectory correction is constructed and the density potential field function is updated; the local neighborhood window is updated using the position data after the movement.

[0015] As can be seen from the above technical solution, this application has the following beneficial effects: 1. This application introduces density potential and incompressibility constraints from continuum mechanics into distributed formation control. Each robot only needs to perceive the surrounding density through a local neighborhood window, and can autonomously adjust its movement direction without global communication, completely eliminating dependence on a central node and global information. By constructing a globally desired density distribution that matches the manifold geometry, the robots can spontaneously and uniformly cover along the pipe path, solving the problem of poor adaptability of traditional fixed formations. Simultaneously, the introduced local mass conservation constraint sets the boundary of the neighborhood window as a virtual solid wall, ensuring the incompressibility of the cluster at the microscopic level, avoiding local over-aggregation or sparsity, improving cleaning coverage, and ensuring the cluster maintains stable formation even when node communication is interrupted.

[0016] 2. This application specifically addresses practical interference in underwater cleaning: High-pressure water jet reaction force feedforward compensation preemptively counteracts recoil, preventing trajectory deviation; obstacle avoidance speed is decomposed into tangential and normal components, preventing collisions and guiding the robot to glide smoothly along obstacles, preventing formation disengagement. It also adapts to complex scenarios: the local neighborhood window dynamically adjusts bandwidth based on the number of neighbors, ensuring reliable density estimation; trajectory correction achieves overshoot-free regression through fine-tuning the expected density, coupled with thrust distribution with gravity compensation and thruster constraints, enabling stable operation even at manifold bends and junctions, meeting engineering implementation requirements. Attached Figure Description

[0017] The invention will now be further described with reference to the accompanying drawings.

[0018] Figure 1 The overall control flowchart provided for the embodiments of this application; Figure 2 A flowchart for solving the density flux correction term provided in the embodiments of this application; Figure 3 A flowchart illustrating the adaptive update process of a local neighborhood window provided in this application embodiment; Figure 4 A flowchart illustrating the construction of desired local density values ​​provided in this application embodiment; Figure 5 A flowchart illustrating the process of solving the correction term based on the graph Laplacian operator provided in this application embodiment. Detailed Implementation

[0019] The terms "first," "second," and "third," etc., used in this application specification, claims, and drawings are for distinguishing different objects, not for specifying a particular order.

[0020] In the embodiments of this application, the words "exemplary" or "for example" are used to indicate examples, illustrations, or descriptions. Any embodiment or design described as "exemplary" or "for example" in the embodiments of this application should not be construed as being more preferred or advantageous than other embodiments or designs. Specifically, the use of the words "exemplary" or "for example" is intended to present the relevant concepts in a specific manner.

[0021] Research has revealed that existing centralized formation control relies on global communication and central scheduling. Underwater acoustic communication is prone to interruption and has high latency. Once the central node fails, the cluster will become uncontrollable. Traditional distributed solutions lack density control logic for underwater cleaning scenarios, making it difficult to cope with high-pressure jet backlash interference. They are also prone to leaving the formation when avoiding obstacles and cannot adapt to the uniform coverage requirements of complex manifold structures.

[0022] To address the aforementioned issues, this application provides a distributed control method and system for collaborative formation of multiple robot swarms: Example 1

[0023] To solve the above problems, such as Figures 1-5As shown, this embodiment provides a distributed control method for multi-robot swarm collaborative formation, applied to underwater manifold cleaning operations. In this scenario, multiple underwater robots form a working swarm to perform comprehensive cleaning of the underwater manifold surface. The underwater manifold structure is complex, containing multiple pipe junctions, and each pipe surface is covered with a layer of marine biofouling. The robot swarm needs to perform orderly cleaning along the pipe direction in a collaborative formation. Due to unfavorable factors in the underwater environment such as ocean current disturbances, low visibility, and limited communication, traditional centralized formation control methods are difficult to effectively address. Therefore, this embodiment adopts a distributed control architecture, where each robot makes control decisions based solely on its own sensor information and local neighbor information.

[0024] The method described in this embodiment includes the following steps.

[0025] Step S101: Establish a local neighborhood window centered on each robot's position, and calculate the local density observation value within the window using the kernel density estimation method.

[0026] During collaborative robot swarm operations, each robot needs to perceive the density of robots in its surrounding area in real time. This density information is the fundamental input for subsequent formation control. The core problem addressed in this step is: how to enable each robot to obtain a quantitative estimate of the distribution density of surrounding robots under the condition of only local perception capability.

[0027] For any robot in the cluster, let its position coordinates in the current control cycle be a point in three-dimensional space. Centered on this location point, a preset spatial range is selected as the local neighborhood window of the robot. The window is spherical in shape, and its radius is determined by the bandwidth parameter. The bandwidth parameter is determined by the physical meaning of the window radius: the size of the window radius determines how far the robot "can see." A larger bandwidth parameter results in a wider window coverage, including more neighboring robots in the density calculation, leading to a smoother density estimation result but with less detailed local information. Conversely, a smaller bandwidth parameter results in a narrower window coverage, making the density estimation result more sensitive to local changes but more susceptible to noise interference. The initial value of the bandwidth parameter is set at system startup and adaptively adjusted in subsequent control cycles according to the method described in Example 3.

[0028] After defining a local neighborhood window, the robot obtains the position information of other robots within the window through its onboard underwater acoustic communication and positioning device. The underwater acoustic communication and positioning device broadcasts its own position information at a preset frequency (preferably 10 Hz) and simultaneously receives broadcast information from neighboring robots. In the distributed architecture, each robot does not know the position of all robots globally, but can only obtain position data within a local area through information interaction with neighboring robots.

[0029] Obtain the location data of each neighboring robot within the window. Then, the kernel density estimation method was used to calculate the local density observations. Kernel density estimation is a nonparametric estimation method. Its basic idea is to place a kernel function centered on each sample point (i.e., the location of neighboring robots), and then superimpose the values ​​of all kernel functions within the window to obtain the density estimate at each point in space. In this embodiment, the robot is interested in the density value at its own location; therefore, its own position coordinates are used... Substituting the superimposed kernel density function, the local density observation value at that location is calculated.

[0030] In this embodiment, the kernel function used is the Gaussian kernel function, which is the probability density function form of the standard normal distribution. The Gaussian kernel function possesses excellent mathematical properties such as infinite differentiability and simple form in the frequency domain (its Fourier transform is still a Gaussian function), making it suitable for density estimation. The kernel density estimation formula using the Gaussian kernel is:

[0031] in, This represents the current position coordinates of the robot (3D vector, unit: meters). For the first The location coordinates of the neighboring robots (3D vector, unit: meters). The number of neighboring robots within the local neighborhood window (dimensionless). This is the bandwidth parameter (unit: meters). Spatial dimension (in this embodiment) (i.e., a three-dimensional underwater working space, dimensionless). Indicates the current robot and the first Euclidean distance (in meters) between neighboring robots. It is a natural exponential function (dimensionless). Pi is a dimensionless constant. The physical meaning of this formula is: each neighboring robot forms a Gaussian distribution "density cloud" centered on its own position in space, and the local density observation value at the current robot's location is the superposition value of the "density clouds" formed by all neighboring robots at that point. The dimension of is 1 / cubic meter, which means the number of robots per unit volume.

[0032] Bandwidth parameters The selection of bandwidth is crucial to the quality of density estimation. Too large a bandwidth results in an overly smooth density estimate, losing details of local clustering or sparseness; too small a bandwidth makes the density estimate sensitive to noise and prone to spurious density fluctuations. In this embodiment, the bandwidth parameter is selected by minimizing the asymptotic integral mean square error (AMISE). For 3D spatial density estimation using a Gaussian kernel, the optimal bandwidth... The selection criterion is to minimize the following asymptotic integral mean square error:

[0033] in, The square integral of the kernel function (for a three-dimensional Gaussian kernel, (dimensions in units of 1 / cubic meter) The second moment of the kernel function (for a standard Gaussian kernel, (dimensionless) The Laplace operator for the true density function (dimensions 1 / m³·m² = 1 / m³) By minimizing AMISE, an asymptotic expression for the optimal bandwidth can be obtained:

[0034] In practical applications, the true density function unknown, The term needs to be estimated using sample data. This embodiment uses the "equation solving method" to obtain the numerical solution for the optimal bandwidth through iterative calculation. The specific iterative format is as follows:

[0035] in, To adopt the first Step bandwidth The obtained density estimate, To estimate the square integral of the Laplace operator of the density function, the initial values ​​for the iteration can be... Rule of thumb: ,in This is the average of the standard deviations of the sample across all dimensions (unit: meters).

[0036] The technical contribution of the above bandwidth selection formula is that it enables each robot to adaptively determine the optimal window size based on the actual distribution of neighboring robots within its local window, thus avoiding the problem of excessive variance in sparse regions and excessive bias in dense regions when using fixed bandwidth.

[0037] The mathematical essence of kernel density estimation is that neighbors closer to a robot within a window contribute more to the density observation, while those farther away contribute less. This weighting method aligns with physical intuition; robots that are closer have a stronger "sensory" of local crowding. Through kernel density estimation, each robot can obtain a continuous, smooth, and quantitative estimate of the distribution density of robots around it without relying on global information. The kernel density estimation result is essentially a probability density, with dimensions of 1 / cubic meter. To match the subsequent expected number density, the kernel density estimation result is multiplied by the total number of robots in the cluster. The calibrated local number density observations were obtained. ,Right now The unit is units per cubic meter. All subsequent calculations involving density bias will use this calibrated local number density observation. .

[0038] Step S102: Obtain the operation area map of the underwater manifold, construct the global expected density distribution, and query the corresponding expected local density value based on the current location.

[0039] After the robot senses the current local density, it also needs to know "what the expected density should be," that is, at this location, what density level should the robot swarm achieve to achieve the ideal formation and cleaning coverage effect. This step solves the problem of the source of expected density information.

[0040] Obtain a map of the underwater manifold's operational area. This map is a pre-built 3D spatial map created using multibeam sonar scanning, which marks the location, direction, diameter, and intersection coordinates of each pipe in the underwater manifold. This map is pre-stored in the storage unit of each robot and sent to each robot via an underwater acoustic communication link before the operation begins.

[0041] Construct a global expected density distribution based on the work area map. It is a scalar function defined in the entire working space, giving the value of any point in the space. The desired robot distribution density is defined as follows (unit: robots / cubic meter). The core logic for constructing this distribution is that the robot swarm should be primarily distributed near the surface of the pipe to be cleaned, and should cover the area evenly along the pipe's direction, avoiding over-concentration in some areas or gaps in others. To achieve this goal, the geometric centerline of the underwater manifold is first identified, i.e., the spatial trajectory of the pipe axis. Multiple path points are extracted along the geometric centerline to construct a path point sequence. These path points define the target trajectory that the robot swarm needs to pass through and cover sequentially. At each path point, the normal section of that point is calculated, i.e., the section perpendicular to the geometric centerline. A Gaussian density profile is defined on the normal section, with its peak value located on the geometric centerline and gradually decreasing towards both sides. The physical meaning of this is that robots should be concentrated near the pipe surface (peak area), while areas far from the pipe (such as the water space around the manifold) do not need to be densely distributed. By connecting the Gaussian density profiles at all path points along the geometric centerline, a global desired density distribution covering the entire manifold operation area is synthesized.

[0042] Specifically, the above construction process includes the following sub-steps: Sub-step 1: Identify the geometric centerline of the underwater manifold. Extract the 3D model data of the manifold from the pre-stored work area map, and extract the centerline representing the pipe axis from the 3D model using a skeleton extraction algorithm. The skeleton extraction algorithm can adopt a topology refinement method based on distance transformation. The basic idea is to perform a distance transformation on the 3D model, and then iteratively peel off the boundary voxels until a single-pixel wide skeleton that cannot be further simplified remains.

[0043] Sub-step 2: Extract multiple path points along the geometric centerline to construct a path point sequence. (Along the centerline) With preset arc length intervals Extracting multiple path points This forms a sequence of path points. The selection principle is: use smaller intervals in areas of high pipe curvature and larger intervals in areas of low curvature. (Curvature) The calculation formula is (Dimensions are 1 / meter), where and These are the first and second derivatives of the centerline with respect to the arc length, respectively. The interval is adaptively adjusted to... ,in The basic interval (in meters) This is the curvature adjustment coefficient (in meters). The physical basis for this adaptive adjustment is: at bends, the pipe direction changes drastically, requiring denser path points to accurately describe the geometry, thus ensuring a precise match between the desired density distribution and the pipe direction; in straight pipe sections, the direction change is gradual, and sparser path points are sufficient to meet the accuracy requirements, thereby reducing the computational load.

[0044] Sub-step 3: Calculate the normal cross-section at each point on the path, generating a set of normal cross-sections perpendicular to the geometric centerline. For each path point... Calculate the tangent vector of the centerline at that point. (Dimensionless unit vector), and then construct... And perpendicular to plane, normal section .

[0045] Sub-step four: Define a Gaussian density profile on the normal section set, the peak of which lies on the geometric centerline. On the normal section... Above, define a Gaussian density profile:

[0046] in, normal section The position vector of any point on the graph (unit: meters). Peak density (unit: cells / cubic meter). This is the Gaussian kernel width parameter (unit: meters). The value is determined by the total number of robots required by the task. (Dimensionless) and the surface area of ​​the manifold that needs cleaning (Unit: square meters, calculated from pipe diameter, length, and quantity) These factors collectively determine: ,in The effective length of the density distribution along the pipe direction (unit: meters) means that the total number of robots is evenly distributed within the volume range corresponding to the surface area of ​​the pipe that needs to be cleaned. The selection is based on the pipe diameter. (Unit: meters) and effective cleaning width of the nozzle (Unit: meters) The density should be such that it reaches a higher value at the surface of the pipe and decreases rapidly away from the pipe. The rationale behind this formula is that the pipe surface is located at a distance from the centerline. At this location, the Gaussian distribution is located at a distance from the center line. Attenuation to peak value At a distance from the center line Attenuation to peak value Select This ensures that the pipe surface is within the effective coverage of the density profile, while the density in areas outside the cleaning width decreases rapidly.

[0047] Sub-step 5: Based on the Gaussian density profile and path point sequence, synthesize the global desired density distribution. For any spatial point in the working area... First, find its position on the center line. The nearest projection point on Then determine the path point corresponding to the road segment where the projection point is located. and The desired density value at that point is obtained through linear interpolation:

[0048] in For projection point exist and The normalized arc length parameter (dimensionless) between them is calculated using the following formula: .

[0049] After obtaining the global expected density distribution, each robot determines its current position coordinates. By querying within this distribution, the expected local density value corresponding to the current location can be obtained. This query will... Substitute the values ​​into the above formula for calculating the global expected density distribution to complete the calculation.

[0050] Step S103: Calculate the deviation between the observed local density value and the expected local density value, and construct the density potential field function.

[0051] After obtaining local density observation values (Actual density) and expected local density value After determining the target density, the difference between the two needs to be converted into a control signal to drive the robot's movement. The core problem solved in this step is: how to convert the density deviation into a potential field with clear physical meaning, so that the robot can automatically move towards the desired density distribution under the influence of the potential field.

[0052] First, calculate the density deviation. The dimension of density deviation is 1 / cubic meter (same as density). A positive value indicates that the actual density at that location is higher than the expected density, meaning the robots are too densely packed. A negative value indicates that the actual density is lower than the expected density, meaning the robots are too sparse.

[0053] Based on this, the density potential function is constructed. The construction of the desired local density value follows this principle: the negative gradient direction of the potential field points in the direction of decreasing density deviation. As the robot moves along the negative gradient direction under the influence of the potential field, the local density gradually approaches the desired density. Specifically, the desired local density value is defined as:

[0054] Among them, the first item Based on the fundamental gravitational potential energy term, This is the gravitational gain coefficient (dimensionless). The physical meaning of this term is: when the density deviation is not zero, the system possesses positive potential energy; the larger the deviation, the higher the potential energy; the system reaches a directional potential energy minimum. The trend of movement. The value of determines the intensity of the gravitational response. The larger the value, the stronger the restoring force generated by the density deviation and the faster the density convergence speed; however, an excessively large value will lead to an overreaction in the system response and cause oscillations. The selection of the gain is determined as follows: based on the criterion of minimizing the convergence time of the density deviation and eliminating overshoot oscillations, the Ziegler-Nichols tuning method or simulation debugging is used. The preferred value range is 0.5 to 2.0. Within this range, the system's response to the density deviation maintains sufficient sensitivity without oscillation due to excessive gain. This range is determined based on the following: when... Below 0.5, the convergence speed of the density deviation is too slow, and the robot needs multiple control cycles to approach the desired density distribution; when When the value is higher than 2.0, the system response overshoots, and the robot oscillates back and forth near the desired density location, failing to converge stably.

[0055] Second item An additional repulsive potential term is added to handle cases where the density exceeds the limit:

[0056] in, This is the repulsion gain coefficient (dimensionless). This represents the deviation value corresponding to the density exceeding the threshold (unit: 1 / cubic meter). Local density safety threshold (unit: cells / cubic meter). The value should be greater than This is to ensure that repulsive force dominates when the density exceeds the limit, forcing the robot to evacuate quickly. The preferred value range is 1.0 to 3.0. This range is determined based on... similar, Below 1.0, the repulsive force is insufficient to effectively disperse robots in densely populated areas. A rejection response higher than 3.0 may lead to excessive robot dispersion.

[0057] Local density safety threshold The selection of the sphere requires comprehensive consideration of the robot's physical dimensions and safety clearance. Let the radius of the outer envelope sphere for each robot be... (Unit: meters), the minimum safe distance between robots is (Unit: meters), then in three-dimensional space, the safe space volume required by each robot is approximately... The local density safety threshold can be taken as a percentage of the reciprocal of the volume of the safety space:

[0058] in, This is the safety margin factor (dimensionless), with a value ranging from 0.3 to 0.7. The physical basis for the value is: when When the distance approaches 1, the robots reach almost the theoretical minimum safe distance, with no extra room for maneuver, and any disturbance could lead to a collision; when When the spacing is small, the distance between robots is significantly larger than the minimum safe distance, leaving ample room for maneuver, but this means that the number of robots that can be accommodated in the work area is relatively small. In densely populated work areas (such as pipe junctions), work demands are concentrated but space is limited. Larger values ​​(e.g., 0.6 to 0.7) can be used to allow more robots to work simultaneously, but this requires more precise control to avoid collisions; in open areas, A smaller value (e.g., 0.3 to 0.4) can be selected, as the control precision requirement is lower and the system is easier to stabilize. This range is determined based on the following engineering experience: when... When the value is below 0.3, space utilization is too low and work efficiency decreases significantly; when Above 0.7, the robot's maneuverability is insufficient, and the risk of collision increases dramatically. Therefore, 0.3 to 0.7 is an effective range for balancing operational efficiency and safety.

[0059] Desired local density value Position The gradient gives the direction of the "generalized force" acting on the robot:

[0060] in This is the spatial gradient operator (partial derivative with respect to three-dimensional coordinates, dimensionless 1 / meter). Driven by this generalized force, the robot moves in the direction of decreasing potential energy.

[0061] Step S104: Introduce a local mass conservation constraint and set the normal flux of the normal gradient of the density potential field function on the boundary of the neighborhood window to zero.

[0062] The gradient of the expected local density value gives the direction and magnitude of the "force" driving the robot's motion. However, directly using the negative gradient of this potential field as the velocity command may cause a problem: the robot swarm can be viewed macroscopically as a continuous medium, and its total number (i.e., total mass) is conserved during operation; robots neither appear nor disappear out of thin air. However, the potential field gradient based on density deviation does not inherently satisfy the mass conservation condition, which may lead to unreasonable "net inflow" or "net outflow" at certain boundaries. That is, the potential field implies that robots are entering the window from outside the boundary or flowing out of the boundary from inside the window, while the actual state of the robots outside the window boundary is unknown.

[0063] To address this issue, this step introduces a local mass conservation constraint. The physical meaning of the local mass conservation constraint is: within any local neighborhood window... Within, the rate of change of the number (mass) of robots is equal to the rate of change through the window boundary. The flux entering or leaving the environment. Described mathematically as:

[0064] in, for Density field at time (unit: 1 / cubic meter). Density flux (the number of robots passing through a unit area per unit time, unit: 1 / (square meter·second)). For the boundary The external normal unit vector (dimensionless) on the surface.

[0065] In distributed control scenarios, each robot's local neighborhood window is a manually defined virtual boundary, and information outside the window is unknown to the robot inside. Therefore, to ensure the closure and self-consistency of control decisions, the normal flux of the density potential function's normal gradient at the neighborhood window boundary is set to zero.

[0066] The physical meaning of this constraint is that no net robot flux is allowed to cross the window boundary; the window boundary is considered a "virtual wall," and robots will not be forced to cross it due to the potential field. This constraint ensures the locality and self-consistency of distributed control, and each robot's decision does not depend on unknown information outside the window. This constraint originates from the analogy of "solid wall boundary conditions" in continuum mechanics applied to the local window boundary of a distributed robotic system.

[0067] Step S105: Solve for the density flux correction term based on the normal flux constraint, wherein the correction term characterizes the incompressibility of the robot swarm at the microscale.

[0068] After introducing the normal flux constraint, the original desired local density gradient needs to be modified so that the modified velocity field can both drive the density to converge toward the desired value and satisfy the constraint that the normal flux is zero. This step solves for this modification term.

[0069] Solving for the density flux correction term is essentially a constrained optimization problem. Let the original velocity be... (i.e., the negative gradient direction of the density potential field, unit: m / s), the goal is to find a correction term. This makes the corrected speed Satisfies the constraint that the normal flux is zero, and at the same time The modulus is minimized (i.e., the original velocity is modified the least).

[0070] The optimization problem can be expressed as:

[0071]

[0072] The solution to this optimization problem is: At the border equal to This means subtracting the normal component perpendicular to the boundary from the original velocity, retaining only the tangential component parallel to the boundary. Inside the window, Smooth extension is achieved by solving the following Poisson equation:

[0073] .

[0074] Discretization methods for solving the Poisson equation: Since the robot swarm is a discrete system, the Poisson equation needs to be solved discretized at each robot node. This embodiment uses the finite difference method to discretize the Poisson equation into a system of linear equations.

[0075] In local neighborhood window Inside, the space is discretized into units based on the positions of each robot. The graph is a collection of nodes. For each internal node... (i.e., robots not on the window boundary) use the Graph Laplace operator. Laplace operator Perform discretization approximation:

[0076] in, For nodes With nodes The adjacency weights between them (dimensionless). To correct the potential function at the node The value at that location. Adjacency weight. The definition of is:

[0077] in The scale parameter (in meters) for adjacency weights takes values ​​of The basis for this value is: when the robot spacing is equal to At time (i.e., at the window boundary), the adjacency weight decays to ≈ 1.1%, meaning the mutual influence at the boundary is negligible, which is consistent with the physical model of the "virtual wall" of the window boundary.

[0078] Poisson equation In discrete form, it becomes:

[0079] Among them, source terms The calculation uses a discrete approximation of divergence:

[0080] The physical meaning of this formula is: to approximate the continuous divergence operator in a local neighborhood centered on node j using the velocity differences of neighboring nodes.

[0081] Boundary conditions In discrete form: for those located at the window boundary On the node ,satisfy:

[0082] in Boundary nodes The outward normal unit vector at the boundary point points from the center of the window towards the boundary node. The physical meaning of this boundary condition is that the outward normal derivative of the modified potential function at the boundary is equal to the negative of the original velocity normal component, thus counteracting the tendency of the original velocity to cross the boundary.

[0083] Combining the Poisson equation for the internal nodes and the boundary conditions for the boundary nodes, we construct a equation concerning... of A system of linear equations:

[0084] in, For the coefficient matrix ( ), by the Thulaplace operator It is composed of contributions from internal nodes and boundary nodes; The right-hand vector (m-dimensional) is derived from the source term. It consists of boundary conditions.

[0085] The solution to this system of linear equations employs the conjugate gradient method, one of the most commonly used iterative methods for solving symmetric positive definite linear equation systems. This method offers advantages such as fast convergence, low storage requirements, and suitability for large-scale sparse problems. (Coefficient matrix) It is symmetric positive definite (because the Thulaplace operator) It is semi-positive definite (becomes positive definite after adding boundary conditions), satisfying the applicability conditions of the conjugate gradient method. The initial value of the iteration is taken as... The iteration termination condition is the residual. ,in The convergence tolerance (dimensionless, taking values ​​of) ).

[0086] Solving for the results Afterwards, the density flux correction term is: The gradient is calculated by the difference in φ values ​​between neighboring nodes:

[0087] The density flux correction term characterizes the incompressibility of robot swarms at the microscopic scale. At the local window scale, the robot "flow" satisfies conditions similar to those of incompressible fluids at the boundary. This correction term is a key bridge connecting the microscopic (single robot motion) and macroscopic (swarm density evolution) scales, enabling the originally discrete robot swarm control problem to be uniformly handled within the framework of continuum mechanics.

[0088] Step S106: Calculate the distributed drift velocity corresponding to each robot based on the density flux correction term.

[0089] After obtaining the density flux correction term Next, this needs to be translated into actual speed control commands for each robot. Distributed drift speed Defined as:

[0090] Among them, the first item The negative gradient, derived from the desired local density value, drives the robot to move from a high-density region to a low-density region (making the local density approach the desired value); the second term Derived from the density flux correction term, it corrects the first term by eliminating its component in the direction of the window boundary normal.

[0091] The distributed drift velocity is calculated independently by each robot. Each robot calculates its own drift velocity based on information within its local neighborhood window, without requiring global information or coordination from a central node. This reflects the distributed nature of this method.

[0092] Step S107: Combine the distributed drift velocity, the feedforward term of the high-pressure water jet's reaction force, and the obstacle avoidance velocity term to generate a resultant velocity vector.

[0093] Distributed drift velocity solves the problem of "how to approach the desired density distribution," but in actual underwater operations, the robot faces two additional challenges: First, the high-pressure water jet cleaning device on the robot experiences a reaction force when spraying high-pressure water, which interferes with the robot's trajectory; second, there may be obstacles (such as pipe supports, flanges, valves, etc.) around the underwater manifold, and the robot needs to actively avoid these obstacles to prevent collisions. Feedforward gain coefficient The calibration method is as follows: Fix the robot to the six-degree-of-freedom force sensor in the still water test tank, start the high-pressure water jet system to the rated working pressure, and record the amplitude of the steady-state recoil force on the robot; apply a reverse thrust command to the thruster and gradually adjust... Continue until the robot's steady-state displacement is less than 5mm, then record the result. The value is used as the factory calibration parameter for the robot.

[0094] Calculation of the feedforward term for jet reaction force: High-pressure water jet cleaning systems are one of the core actuators for underwater robot operations. This system pressurizes seawater to a preset pressure value using a high-pressure pump, then ejects it at high speed through nozzles, utilizing the impact force of the water jet to remove marine debris from the manifold surface. The jet reaction force feedforward term is calculated in advance based on known pump pressure data, determining the magnitude and direction of the reaction force. A velocity compensation term of equal magnitude but opposite direction is then generated and added to the resultant velocity to counteract the effect of the reaction force.

[0095] Pump pressure data The pressure (in Pascals) is collected in real time by a pressure sensor installed at the outlet of the high-pressure pump, with a sampling frequency preferably of 100 Hz. According to the momentum theorem in fluid mechanics, the amplitude of the jet reaction force is:

[0096] in, Seawater density (unit: kg / m³, typical value is 1025 kg / m³, which can be corrected based on actual measurements from temperature and salinity sensors). Volumetric flow rate (unit: cubic meters per second). Nozzle outlet velocity (unit: meters per second). ,in The nozzle velocity coefficient (dimensionless, ranging from 0.95 to 0.99, determined by the nozzle geometry and internal friction characteristics, obtained through factory calibration). Volumetric flow rate. ,in This refers to the nozzle outlet cross-sectional area (unit: square meters, determined by the nozzle geometry). and Substitution The expression yields:

[0097] The direction of the jet reaction force is opposite to the direction of the jet. Feedforward compensation velocity. for:

[0098] in, This is the feedforward gain coefficient (dimensionless). This represents the robot's current hydrodynamic damping coefficient (unit: Newton-second / meter). It is a unit vector (dimensionless) in the direction of the jet. The value of is slightly less than 1 (preferably within the range of 0.85 to 0.95) in order to retain a certain control margin and avoid overcompensation that could lead to system oscillation. The basis for this determination is: under ideal circumstances It can completely offset the reaction force, but due to the hydrodynamic damping coefficient The estimate has an error, take This ensures system stability. During the system debugging phase, the first step is to... Set the value to 1 for a step response test. If overshoot or oscillation is observed, gradually decrease the value. Until the system response is smooth, at this point The value is the feedforward gain coefficient of the robot.

[0099] Hydrodynamic damping coefficient The method for obtaining this information is as follows: During the water tank calibration experiment before the robot leaves the factory, the relationship between the thruster thrust and the steady-state velocity at different speeds is measured. Specifically, the robot is fixed to a six-degree-of-freedom force sensor, and a known thrust is applied to each thruster. Measure the corresponding steady-state velocity ,but By performing linear regression on multiple sets of data, we can obtain... The estimated value. During the operation, The speed will vary, and real-time calculations can be performed using table lookup or polynomial fitting.

[0100] Calculation of obstacle avoidance speed term: Obstacle avoidance sensors (such as forward-looking sonar or optical cameras) on underwater robots detect obstacles in front of and around the robot, and output the distance of the obstacles relative to the robot. (Unit: meters) and direction. The preferred detection range of the obstacle avoidance sensor is 20 meters in front, with a horizontal scanning angle of 120 degrees and a vertical scanning angle of 60 degrees. When Less than the preset safe distance At that time, an obstacle avoidance speed term is generated that points away from the obstacle.

[0101] safe distance ,in The radius of the robot's outer envelope sphere (unit: meters). The safety factor (dimensionless, ranging from 3 to 5). The basis for its value is: when When the value is 3, the safe distance is 3 times the robot's radius. This provides the robot with sufficient reaction time for obstacle avoidance maneuvers, while preventing it from deviating too far from the mission path due to premature obstacle avoidance. However, when the obstacle environment is complex and dense... Option 5 is preferable, providing a greater safety margin.

[0102] The formula for calculating the obstacle avoidance speed term is:

[0103] in, This is the obstacle avoidance gain coefficient (unit: square meters / second). Let be the unit vector pointing from the obstacle to the robot (i.e., the direction away from the obstacle, dimensionless). The value of determines the intensity of the obstacle avoidance response, and the value ranges from 0.5 to 2.0 square meters per second. The criteria for determining this are: in a simulation environment, tests are conducted using typical obstacle scenarios (such as a single spherical obstacle located 5 meters directly in front of the robot), with the robot's ability to smoothly navigate around the obstacle without oscillations as the standard. When the value is below 0.5, the robot's obstacle avoidance response is slow, and it may collide with obstacles; when Above 2.0, the robot's obstacle avoidance response is overly aggressive, resulting in large swings. Within the range of 0.5 to 2.0, the obstacle avoidance response can balance timeliness and smoothness.

[0104] Will Decomposed into tangential components and normal components The normal component is used to correct the density flux correction term (preventing the robot from crossing obstacles), while the tangential component is used to correct the distributed drift velocity (guiding the robot to glide along the obstacle contour).

[0105] Speed ​​Fusion: The corrected density flux correction term and the corrected distributed drift velocity are vector-summed to generate an intermediate velocity vector, which is then vector-superimposed with the jet reaction force feedforward term.

[0106]

[0107] in, This is the distributed drift velocity after tangential component correction. This is the density flux correction term after correction of the normal component.

[0108] Step S108: Convert the resultant velocity vector into a thrust distribution command for the underwater robot thruster.

[0109] Underwater robots are typically equipped with multiple thrusters (such as horizontal thrusters, vertical thrusters, and lateral thrusters). By adjusting the rotational speed of each thruster, they generate thrust in different directions and magnitudes, thereby enabling the robot to move in three-dimensional space. (Resultant velocity vector) This is the speed (including magnitude and direction) that the robot's center of mass should achieve, but this speed command cannot be directly applied to the thrusters; it needs to be converted into specific rotational speed commands for each thruster. This step solves this conversion problem.

[0110] First, establish the thruster configuration matrix for the underwater robot. ,in This represents the number of thrusters. Each column of the matrix corresponds to one thruster, and each row corresponds to one degree of freedom (sway, roll, heave, pitch, yaw, bow roll). Matrix elements Indicates the first When the thruster produces unit thrust at the first The force or torque components generated in each degree of freedom direction.

[0111] For installation location (Unit: meters), direction of thrust is The j-th thruster of a unit vector (dimensionless) has the following configuration matrix with column j as follows:

[0112] The matrix consists of three rows representing the linear forces (in Newtons) generated by the thruster per unit force in three directions, and three rows representing the torque components (in Newton-meters) generated by the thrust relative to the robot's center of mass in three directions. Each column of the matrix has a different dimension; the first three rows are dimensionless (because thrust is divided by thrust), while the last three rows are in meters (lever arm). This dimensional difference is determined by physical principles (force and torque have different physical dimensions). In subsequent pseudo-inverse calculations, due to the different physical dimensions of each row, directly calculating the pseudo-inverse may lead to poor numerical condition numbers. In this embodiment, this problem is addressed through diagonal scaling preprocessing: The last three lines multiplied by the feature length (Unit: meters, taking the eigenvalue of the robot's body length) The result is dimensionless, the pseudo-inverse is calculated, and then the result is restored. Specifically, a scaling matrix is ​​defined. ,calculate (Dimensionless matrix), then This method ensures that the condition number of the matrix is ​​within a reasonable range, thus avoiding numerical instability.

[0113] The resultant velocity vector The robot's dynamic model is transformed into the desired generalized force vector. For underwater robots, the dynamic model is:

[0114] in, Mass matrix (including additional mass, unit: kilogram or kilogram·) ), The Coriolis force matrix (based on robot velocity) Decision (unit: kg / s) This is the damping matrix (related to velocity). The resultant force vector of gravity and buoyancy (and attitude angle) related). and , The results were obtained through a water tank system identification experiment conducted before the robot left the factory.

[0115] Desired acceleration (unit: meters per second) 2 ), obtained by time difference of the resultant velocity vector:

[0116] In actual implementation, and Identification was performed through a water tank test before the robot left the factory. Specifically, a known thrust excitation was applied to the robot, and the corresponding acceleration and velocity response were measured. The least squares system identification method was then used to estimate the... and The various elements.

[0117] The thrust distribution problem can be described as: finding the thrust of each thruster. , making .when When the number of thrusters exceeds the degrees of freedom (i.e., redundant configuration), the equation is underdetermined, and the solution is not unique. This embodiment uses the Moore-Penrose pseudo-inverse matrix to find the least-norm 2 solution:

[0118] The meaning of this solution is: in all cases where the expected generalized force can be generated... In the thrust distribution scheme, the scheme with the minimum sum of the squares of the thrust of each thruster is selected, which is the energy-optimal distribution scheme.

[0119] In addition, the physical constraints of the thrusters must also be considered: (The thruster can only generate positive thrust and cannot exceed the maximum thrust). If the pseudo-inverse solution exceeds the constraint range, a constrained quadratic programming method must be used to solve it, with the objective function being... , constraint is , This quadratic programming problem can be solved using the active set method or the interior point method.

[0120] The obtained thrust values ​​of each thruster Convert the hydrodynamic characteristic curve of the propeller into speed value (Unit: revolutions per minute). The relationship between thrust and rotational speed is usually approximated as follows: ,in For the first Thrust coefficient of each thruster (unit: Newton-minute) 2 / change 2 The speed is determined by the hydrodynamic characteristics of the thruster, obtained through pre-shipment water tank calibration tests. Therefore, the speed command is:

[0121] Step S109: Execute the thrust distribution command to drive the robot to move, and update the local neighborhood window using the position data after the movement.

[0122] Upon receiving a speed command, each thruster adjusts its speed accordingly, generating corresponding thrust to drive the robot to move in the direction of the resultant velocity vector. The robot completes one control cycle of movement in the water. (Unit: seconds) After that, it reaches a new position. Control cycle The value of should be less than 1 / 5 of the robot's dynamic response time constant to ensure the stability of the control system. Dynamic response time constant The definition of is: the time required for the robot's velocity to change from the initial value to 63.2% of the final value when the thruster thrust changes stepwise. It can be determined through a thruster step response experiment. The preferred value range is 0.1 to 0.5 seconds, based on the fact that the dynamic response time constant of a typical underwater robot is approximately 0.5 to 2.5 seconds. It can ensure that the speed change within the control cycle is approximately linear, and the discretization error is within an acceptable range.

[0123] At this point, the updated location data needs to be used to rebuild the local neighborhood window in preparation for density calculation and control decisions in the next control cycle.

[0124] The updating of the local neighborhood window involves two aspects: First, the center position of the window is updated to the new position after the robot's movement. Second, the spatial extent of the window (i.e., the bandwidth parameter) may need to be adjusted according to the actual situation. Specifically, if the number of neighbors around the robot is too small (indicating that the window extent is too small and fails to cover enough neighbors), the bandwidth parameter needs to be appropriately increased to expand the window extent; if the number of neighbors is too large (indicating that the window extent is too large and includes too many distant neighbors), the bandwidth parameter needs to be appropriately reduced to improve the local accuracy of density estimation. See Example 3 for details on the specific adaptive update method.

[0125] Through the cyclical execution of the above nine steps, the multi-robot swarm can continuously perform density perception, deviation calculation, potential field construction, flux correction, velocity fusion, thrust allocation, and position update under a distributed architecture, thereby achieving collaborative formation control.

[0126] Example 2 Based on the first embodiment described above, this embodiment provides a detailed explanation of the specific implementation of step S105, "solving the density flux correction term based on the normal flux constraint".

[0127] This step specifically includes the following sub-steps.

[0128] Step S201: Obtain the pump pressure data of the high-pressure water jet.

[0129] High-pressure water jet cleaning systems are one of the core actuators in underwater robot operations. This system uses a high-pressure pump to pressurize seawater to a preset pressure value, then ejects it at high speed through nozzles, utilizing the impact force of the water jet to remove marine debris from the manifold surface. Pump pressure data reflects the current pressure level of the water jet and is the fundamental input for calculating the jet reaction force.

[0130] Pump pressure data is acquired by using a pressure sensor installed at the outlet of the high-pressure pump to collect pressure signals in real time, which are then converted from analog to digital and sent to the robot's central controller. The sampling frequency of the pressure sensor is preferably 100 Hz, which is higher than the robot's control cycle update frequency (usually 2 to 10 Hz) to ensure that the controller can obtain real-time and continuous pump pressure information.

[0131] Step S202: Calculate the amplitude of the jet reaction force of the high-pressure water jet based on the pump pressure data.

[0132] After obtaining the pump pressure data, the magnitude of the jet reaction force needs to be calculated. The principle behind the generation of the jet reaction force is as follows: High-pressure water gains high pressure energy under the drive of the pump. When the water flows through the nozzle and is accelerated out, the pressure energy is converted into kinetic energy, and the water flows at high speed towards the target surface. According to the momentum theorem, the momentum of the water changes during the jetting process, and the force required for this change is provided by the nozzle wall; conversely, the water exerts a reaction force of equal magnitude and opposite direction on the robot.

[0133] The formula is used to calculate the amplitude of the jet reaction force. For the specific derivation process and the meaning of the parameters, please refer to step S107 of Example 1.

[0134] Step S203: Construct a feedforward gain mapping relationship with the amplitude of the reaction force as the input.

[0135] After the amplitude of the jet reaction force is calculated, it needs to be converted into a velocity-level feedforward compensation. The feedforward gain mapping relationship is the function that completes this conversion; it describes the correspondence between the reaction force amplitude and the velocity compensation.

[0136] The feedforward gain mapping relationship is constructed based on the dynamic model of the underwater robot. When the robot moves in water, the relationship between its velocity and the net force acting on it is described by the dynamic equations: the net force equals mass multiplied by acceleration, while it is subjected to water resistance (related to velocity). In steady state (i.e., when acceleration is zero), the thruster thrust equals the resultant force of water resistance and external disturbance forces (including jet reaction force). Therefore, to counteract the jet reaction force, the thruster needs to provide an additional thrust of equal magnitude but opposite direction; the velocity increment corresponding to this additional thrust is the feedforward compensation.

[0137] The feedforward gain mapping relationship is as follows: ,in This is the hydrodynamic damping coefficient. For a detailed derivation of this formula, please refer to step S107 of Example 1.

[0138] Step S204: Scale the gradient term of the density potential field function using the feedforward gain mapping relationship to generate a feedforward compensation component.

[0139] The speed compensation amount calculated in step S203 As an independent velocity component, the feedforward compensation component. The direction of this component is opposite to the direction of the jet reaction force, and its amplitude is determined by the feedforward gain mapping relationship.

[0140] Step S205: Vector superposition of the feedforward compensation component and the gradient term of the density potential field function to output the density flux correction term.

[0141] By superimposing the feedforward compensation component onto the gradient term of the density potential field function, we obtain the density flux correction term after reaction force compensation: This correction term incorporates both density bias-driven components (from the potential field gradient) and reaction force compensation components (from the feedforward compensation component), enabling the robot to resist the interference of high-pressure water jet reaction forces while approaching the desired density distribution.

[0142] Example 3 This embodiment, based on the above embodiment one or two, provides a detailed explanation of the specific implementation of "updating the local neighborhood window using the position data after movement" in step S109.

[0143] This step specifically includes the following sub-steps.

[0144] Step S301: Count the number of neighboring robots within the local neighborhood window and generate neighbor connectivity parameters.

[0145] After the robot moves to a new position, it is necessary to evaluate whether the current local neighborhood window setting is reasonable. One of the core evaluation indicators is the number of neighboring robots within the window. If there are too few neighbors within the window, it indicates that the window range may be too small, and the statistical significance of the density estimation is insufficient; if there are too many neighbors within the window, it indicates that the window range may be too large, and the density estimation is too smooth, losing local details.

[0146] The neighbor connectivity parameter is a quantitative representation of the number of neighboring robots within a window. Each robot broadcasts its location information to its neighbors via a communication link and simultaneously receives location information broadcast by its neighbors. The neighbor connectivity parameter can be obtained by counting the number m of neighbor location information received within the local neighborhood window.

[0147] Step S302: Compare the neighbor connectivity parameter with the preset connectivity lower limit threshold.

[0148] Connectivity lower limit threshold This is a preset value representing the minimum number of neighbors required to maintain an effective density estimate. If the number of neighbors is below this threshold, the kernel density estimate will be unreliable due to insufficient sample size, and the variance of the estimate will increase. If it is too large, it is easily affected by abnormal fluctuations in the location of individual neighbors.

[0149] The principle for determining the value of is to ensure that the relative standard error of the density estimation does not exceed the preset value εpsilon (dimensionless, ranging from 0.1 to 0.3). Should meet In practical applications, to simplify calculations, A fixed value is chosen, preferably between 5 and 10. The rationale for this value is as follows: when the number of neighbors is less than 5, the statistical significance of the kernel density estimation is insufficient and the estimation variance is too large; when the number of neighbors is greater than 10, the estimation accuracy requirement is usually met, and there is no need to increase the sample size.

[0150] Step S303: When the neighbor connectivity parameter is lower than the connectivity lower limit threshold, extract the bandwidth parameter currently used in the kernel density estimation method.

[0151] if This indicates that the current window size is too small and needs to be expanded to include more neighbors. In this case, the currently used bandwidth parameter is first extracted. That is, the radius parameter used to define the local neighborhood window in step S101.

[0152] Step S304: Correct the bandwidth parameters according to the preset amplification ratio to generate updated bandwidth parameters; set the bandwidth amplification factor. The value range is 1.1 to 1.3. If the neighbor connectivity parameter remains below the lower connectivity threshold for three consecutive control cycles, then... Temporarily increased to 1.5; bandwidth limit preset. ,in This refers to the initial bandwidth parameters.

[0153] After extracting the current bandwidth parameters, they are amplified and corrected according to a preset amplification ratio: ,in This is the magnification factor (dimensionless, greater than 1), with a value range of 1.1 to 1.3. The basis for this value is that a gradual adjustment strategy can avoid over-adjustment caused by excessive amplification at once. Choosing a value between 1.1 and 1.3 means amplifying by only 10% to 30% each time. Combined with the continuity of each control cycle, this allows the bandwidth parameter to smoothly converge to the minimum value that satisfies the connectivity requirement. If Below 1.1, the convergence speed is too slow, requiring multiple control cycles to reach the required window size; if A value higher than 1.3 may cause the window to expand too quickly, incorporating too many unnecessary distant neighbors.

[0154] Step S305: When When the updated bandwidth parameters are used to replace the original bandwidth parameters, and the spatial range of the local neighborhood window is redefined according to the updated bandwidth parameters, with the updated position data of each robot as the center; when At that time, the current bandwidth remains unchanged, and a local communication failure alarm is triggered.

[0155] The newly defined local neighborhood window will be used as the current local neighborhood window for the next control cycle.

[0156] Step S306: Use the redefined local neighborhood window as the current local neighborhood window for the next control cycle.

[0157] The newly defined local neighborhood window is used as the starting window for the next control cycle, and is used in step S101 to calculate the local density observations. Through this adaptive adjustment mechanism, the local neighborhood window of each robot can dynamically adjust its size according to the actual distribution of its neighbors, ensuring the statistical reliability of the density estimation.

[0158] Example 4 Based on the above embodiment three, this embodiment provides a detailed explanation of the specific implementation method of "calculating the deviation between the observed local density value and the expected local density value, and constructing the density potential field function" in step S103.

[0159] This step specifically includes the following sub-steps.

[0160] Step S401: Calculate the difference between the observed local density value and the expected local density value to generate a density bias.

[0161] The local density observation values ​​calculated in step S101 Subtract the expected local density value obtained in step S102 , to obtain density deviation The sign of the density deviation reflects the direction of the deviation of the current density from the desired density. A positive value indicates that the density is too high (too crowded), and a negative value indicates that the density is too low (too sparse).

[0162] Step S402: Set the local density safety threshold.

[0163] Local density safety threshold This is a preset density upper limit, representing the maximum allowable robot density within a local area. When the observed local density exceeds this threshold, it means that the area is already too crowded. Continuing to add robots may increase the risk of collisions between robots, or affect the operational efficiency and maneuverability of each robot due to excessive space congestion.

[0164] The calculation formula is For the specific meanings and values ​​of the parameters, please refer to step S103 of Example 1.

[0165] Step S403: Compare the observed local density value with the local density safety threshold.

[0166] The local density observation values ​​calculated in step S101 Compared with the local density safety threshold set in step S402 Compare the results to determine if the current density exceeds the limit.

[0167] Step S404: Extraction of Density Exceedance Deviation when When this occurs, it indicates that the current area is already too crowded. At this point, the density deviation calculated in step S401 is... Marked as density exceeding the limit: .

[0168] Step S405: Multiply the density excess deviation by a preset repulsion gain coefficient to generate a repulsion potential energy term.

[0169] Repulsive potential term ,in This is the repulsion gain coefficient. The physical meaning of the repulsion gain coefficient is: the magnitude of the repulsive force generated per unit of excess density when the density exceeds the limit. The value should be greater than This is to ensure that the repulsive force dominates when the density exceeds the limit. The preferred value range is 1.0 to 3.0, and the value is based on step S103 of Example 1.

[0170] Step S406: Using the density deviation as the input variable and combining it with the repulsive potential energy term, construct a density potential field function that includes the basic gravitational component and the additional repulsive component.

[0171] Desired local density value { The fundamental gravitational component drives the density toward the desired value. When the density is below the desired value, it generates an attractive force, drawing more robots in; when the density is above the desired value, it generates a repulsive force, driving away excess robots. The additional repulsive component provides an extra strong repulsive force when the density exceeds the limit, forcibly evacuating robots and ensuring that the density does not remain above the safe threshold for an extended period.

[0172] Step S407: Replace the original expected local density value stored in the previous control cycle with the expected local density value.

[0173] The newly constructed desired local density value is stored in the robot's memory, replacing the original desired local density value stored in the previous control cycle, for use in subsequent steps (such as solving the density flux correction term in step S105).

[0174] Example 5 Based on the first embodiment described above, this embodiment provides a detailed explanation of another implementation method for "solving the density flux correction term based on the normal flux constraint" in step S105.

[0175] This step specifically includes the following sub-steps.

[0176] Step S501: Define the graph Laplacian operator within the local neighborhood window.

[0177] In a distributed robot swarm, the information interaction relationships between robots can be abstracted as a graph. Each robot is a node in the graph, and the communication links between robots are edges of the graph. The graph Laplace operator is a discrete differential operator defined on this graph, which describes the weighted sum of the differences of some quantity between a node and its neighboring nodes.

[0178] Thulaplace operator Defined as ,in It is an adjacency matrix. It is a degree matrix.

[0179] Adjacency Matrix elements Represents robots With robots The communication link weight between them is defined as:

[0180] in The scale parameter (in meters) for adjacency weights takes values ​​of This value makes the distance close. The adjacency weights between neighbors approach zero, achieving a smooth truncation of interactions at the window boundaries. Adjacency weights Dimensionless.

[0181] Degree matrix It is a diagonal matrix, and its diagonal elements , indicating robot The sum of link weights with all neighbors.

[0182] Thulaplace operator Acting on the density vector The result is:

[0183] The physical meaning of this result is: Robot The sum of the weighted differences between a robot's density and the densities of its neighbors; if a robot's density is higher than the average density of its neighbors, then... It is positive.

[0184] The core function of the Graph Laplace operator is to quantify the spatial rate of change of density distribution at various locations within a local neighborhood window. The more drastic the density change, the larger the output value of the Graph Laplace operator. In this embodiment, the Graph Laplace operator is used to discretize continuous partial differential equation constraints (with zero normal flux) into algebraic constraints that can be locally computed at each robot node.

[0185] Step S502: Collect local density observations and relative position vectors of neighboring robots.

[0186] Each robot obtains the local density observations (i.e., the local density observations calculated by each neighboring robot) and relative position vectors of its neighboring robots through a communication link. (That is, the positional offset of neighboring robots relative to this robot). This information is the basic data for constructing the adjacency matrix and degree matrix.

[0187] Step S503: Construct an adjacency matrix using the relative position vector, and construct a degree matrix by combining the local density observations.

[0188] According to the definition in step S501, the relative position vector is used. Calculate Euclidean distance Substitute into the adjacency weight formula to calculate Construct an adjacency matrix Sum-degree matrix .

[0189] Step S504: Calculate the effect of the graph Laplacian operator based on the adjacency matrix and the degree matrix.

[0190] calculate The results of the Laplacian operator at each robot location are obtained. .

[0191] Step S505: Substitute the result of the graph Laplace operator as a constraint into the Poisson equation solver.

[0192] The constraint of zero normal flux is equivalent in discrete form to: at the robot on the window boundary, the normal component of the density flux correction term is zero. Combining this constraint with the effect of the graph Laplace operator, we construct the discrete Poisson equation:

[0193] in Let be the value vector of the modified potential function to be solved at each individual position. The boundary conditions of the equation are: for the robot located at the window boundary, The rate of change in the direction of the boundary normal is zero.

[0194] Step S506: Use the Poisson equation solver to output a density flux correction term that satisfies the normal flux constraint.

[0195] Solving the above system of linear equations, we obtain... The density flux correction term is: The gradient is obtained through the interaction between neighboring nodes. Value difference calculation: .

[0196] Through the above steps, the density flux correction term is solved under the constraint that the normal flux is zero, providing input for subsequent distributed drift velocity calculation.

[0197] Example 6 Based on the first embodiment described above, this embodiment provides a detailed explanation of the specific implementation method of "obtaining the operation area map of the underwater manifold and constructing the global desired density distribution" in step S102.

[0198] The specific implementation of this step has been fully described in step S102 of Embodiment 1, including five sub-steps: geometric centerline identification, path point extraction, normal section generation, Gaussian density profile definition, and global desired density distribution synthesis, as well as their detailed calculation formulas and parameter selection criteria.

[0199] Example 7 Based on the above embodiment one, this embodiment provides a detailed explanation of the specific implementation method of "integrating the distributed drift velocity, the jet reaction force feedforward term of the high-pressure water jet, and the obstacle avoidance velocity term to generate a resultant velocity vector" in step S107.

[0200] The specific implementation of this step has been fully described in step S107 of Embodiment 1, including the calculation of the feedforward term of the jet reaction force (including pump pressure acquisition, reaction force amplitude calculation, and feedforward gain mapping), the calculation of the obstacle avoidance speed term (including obstacle distance acquisition, safety distance setting, and normal / tangential decomposition), and the vector superposition and fusion of the three.

[0201] Example 8 Based on the above embodiment one, this embodiment provides a detailed explanation of the specific implementation method of "converting the resultant velocity vector into a thrust distribution command for the underwater robot thruster" in step S108.

[0202] The specific implementation of this step has been fully described in step S108 of Embodiment 1, including the establishment of the thruster configuration matrix, the acquisition of attitude sensor data, the calculation of gravity decomposition components, the pseudo-inverse matrix solution for thrust allocation, and the conversion of thrust to rotational speed.

[0203] Example 9 Based on Embodiment 1 above, this embodiment provides a detailed explanation of another implementation method for step S109, which involves "executing the thrust distribution command to drive the robot to move and updating the local neighborhood window using the position data after the movement".

[0204] This step specifically includes the following sub-steps.

[0205] Step 1: Record the actual trajectory point sequence during the underwater robot's execution of the thrust distribution command.

[0206] During the motion of the robot in executing thrust distribution commands, its position sensors (such as an underwater acoustic positioning system or an inertial navigation system) continuously record the robot's position data at a preset sampling frequency (preferably 10 Hz), forming a sequence of actual trajectory points. The actual trajectory point sequence reflects the robot's true motion path within that control cycle.

[0207] Step 2: Calculate the lateral deviation between the actual trajectory point sequence and the preset ideal cleaning path.

[0208] Ideal cleaning path It is a pre-planned target trajectory that the robot should follow. In manifold cleaning operations, the ideal cleaning path usually coincides with or is parallel to the geometric centerline of the manifold. The robot moves along the pipe direction while cleaning the pipe surface with high-pressure water jets.

[0209] lateral deviation Refers to the actual trajectory point Compared to the ideal cleaning path Deviation distance (in meters) perpendicular to the path direction. ,in exist The nearest projection point on the surface. The greater the lateral deviation, the further the robot deviates from the predetermined trajectory, and the worse the uniformity of the cleaning coverage.

[0210] Step 3: When the lateral deviation is greater than the preset trajectory correction threshold, a trajectory correction vector is generated.

[0211] Trajectory Correction Threshold (Unit: meters) is a preset upper limit for allowable deviation. When the lateral deviation exceeds this threshold, it indicates that the robot has deviated too far from the predetermined trajectory and needs to be corrected. The value is determined based on the uniformity requirement of the cleaning coverage. If the deviation exceeds this value, some areas on the pipe surface may not be cleaned sufficiently. The preferred value is half the effective cleaning width of the nozzle.

[0212] At this point, a trajectory correction vector is generated. ,in This is the trajectory correction gain coefficient (unit: 1 / second, value range: 0.1 to 0.5). To point from the actual location to the nearest point on the ideal path The unit vector. The basis for determination and Similarly, the convergence speed of the correction process and the absence of oscillations are used as criteria, and the results are determined through simulation and debugging.

[0213] Step 4: Construct the additional potential field term for trajectory correction and fine-tune the density potential field function. Construct the additional potential field term using the trajectory correction vector: in, The value is the trajectory correction gain coefficient, ranging from 0.1 N / m to 0.5 N / m. This represents the lateral deviation between the actual trajectory point and the ideal cleaning path. This additional potential field term is superimposed onto the original density potential field function to obtain the corrected density potential field function. This method does not directly modify the global expected density distribution, thus avoiding cascading interference with the density deviation judgments of other robots, while achieving trajectory regression without overshoot.

[0214] Step 5: Write the corrected density potential field function into the memory, replacing the original density potential field function.

[0215] The corrected density potential field function is stored in the register, replacing the original density potential field function. In the next control cycle, step S103 will use the corrected desired local density value to calculate the density deviation, thereby incorporating the trajectory correction requirement into the construction of the desired local density value.

[0216] Step 6: Update the local neighborhood window using the position data in the actual trajectory point sequence.

[0217] The final position data in the actual trajectory point sequence The position (i.e., the position after the robot finishes its movement) is used as the updated position. A new local neighborhood window is then established with this position as the center for use in the next control cycle.

[0218] Example 10 This application also provides a distributed control system for multi-robot swarm collaborative formation, which includes the following functional modules: The situational awareness module is used to establish local neighborhood windows centered on each robot's position, calculate the local density observation values ​​within the windows using kernel density estimation, acquire a map of the underwater manifold's operating area, construct a global desired density distribution, and query the corresponding desired local density value based on the current position. This module corresponds to steps S101 and S102 in Embodiment 1, and its specific implementation is described in detail above.

[0219] The decision-making and planning module is used to calculate the deviation between the observed local density value and the desired local density value, and construct a density potential field function. It introduces a local mass conservation constraint, setting the normal flux of the normal gradient of the density potential field function at the boundary of the neighborhood window to zero. Based on the normal flux constraint, it solves for a density flux correction term, which characterizes the incompressibility of the robot swarm at the microscale. This module corresponds to steps S103, S104, and S105 in Embodiment 1, and its specific implementation is described in detail above.

[0220] The motion calculation module is used to calculate the distributed drift velocity corresponding to each robot based on the density flux correction term; to fuse the distributed drift velocity, the jet reaction force feedforward term of the high-pressure water jet, and the obstacle avoidance velocity term to generate a resultant velocity vector; and to convert the resultant velocity vector into a thrust distribution command for the underwater robot's thrusters. This module corresponds to steps S106, S107, and S108 in Embodiment 1, and its specific implementation is described in detail above.

[0221] The execution feedback module is used to execute the thrust distribution command to drive the robot to move, and to update the local neighborhood window using the position data after the movement. This module corresponds to step S109 in Embodiment 1, and its specific implementation is described in detail above.

Claims

1. A distributed control method for collaborative formation of multiple robot swarms, characterized in that, The method includes: A local neighborhood window is established with each robot's position as the center, and the local density observation value within the window is calculated using the kernel density estimation method. Obtain a map of the operating area of ​​the underwater manifold, construct a global expected density distribution, and query the corresponding expected local density value based on the current location; Calculate the deviation between the observed local density value and the expected local density value, and construct the density potential field function; By introducing a local mass conservation constraint, the normal flux of the normal gradient of the density potential field function on the boundary of the neighborhood window is set to zero. Solve for the density flux correction term based on the normal flux constraint; Calculate the distributed drift velocity of each robot based on the density flux correction term; The resultant velocity vector is generated by integrating the distributed drift velocity, the jet reaction force feedforward term of the high-pressure water jet, and the obstacle avoidance velocity term. The resultant velocity vector is converted into a thrust distribution command for the underwater robot's thruster; The thrust distribution command is executed to drive the robot to move, and the local neighborhood window is updated using the position data after the movement.

2. The method according to claim 1, characterized in that, The process of solving for the density flux correction term based on the normal flux constraint includes: Obtain the pump pressure data of the high-pressure water jet; The amplitude of the jet reaction force of the high-pressure water jet is calculated based on the pump pressure data; Construct a feedforward gain mapping relationship with the amplitude of the reaction force as the input; The density flux correction term is scaled using the feedforward gain mapping relationship to generate a feedforward compensation component; The feedforward compensation component is superimposed on the density flux correction term, and the compensated density flux correction term is output.

3. The method according to claim 2, characterized in that, Updating the local neighborhood window using the position data after movement includes: Count the number of neighboring robots within the local neighborhood window and generate neighbor connectivity parameters; Compare the neighbor connectivity parameter with a preset connectivity lower limit threshold; When the neighbor connectivity parameter is lower than the connectivity lower limit threshold, the bandwidth parameter currently used in the kernel density estimation method is extracted. The bandwidth parameters are corrected according to a preset magnification ratio to generate updated bandwidth parameters; The original bandwidth parameters are replaced with the updated bandwidth parameters, and the spatial range of the local neighborhood window is redefined according to the updated bandwidth parameters, with the updated position data of each robot as the center. The newly defined local neighborhood window will be used as the current local neighborhood window for the next control cycle.

4. The method according to claim 3, characterized in that, The calculation of the deviation between the observed local density value and the expected local density value, and the construction of the density potential field function, includes: Calculate the difference between the observed local density value and the expected local density value to generate a density bias; Set a local density safety threshold; Compare the observed local density value with the local density safety threshold. When the observed local density value is greater than the local density safety threshold, the density deviation is extracted as the density over-limit deviation. Multiply the density excess deviation by a preset repulsion gain coefficient to generate a repulsion potential energy term; Using the density deviation as the input variable and combining it with the repulsive potential energy term, a density potential field function including the basic gravitational component and the additional repulsive component is constructed.

5. The method according to claim 1, characterized in that, The process of solving for the density flux correction term based on the normal flux constraint includes: Define the graph Laplace operator within the local neighborhood window; Collect local density observations and relative position vectors of neighboring robots; An adjacency matrix is ​​constructed using the relative position vectors, and a degree matrix is ​​constructed by combining the local density observations. The effect of the graph Laplacian operator is calculated based on the adjacency matrix and the degree matrix. The result of the graph Laplace operator is substituted into the Poisson equation solver as a constraint condition. The Poisson equation solver outputs a density flux correction term that satisfies the normal flux constraint.

6. The method according to claim 1, characterized in that, The process of acquiring a map of the underwater manifold's operational area and constructing a global desired density distribution includes: Identify the geometric centerline of the underwater manifold; Multiple path points are extracted along the geometric centerline to construct a path point sequence; Calculate the normal cross section at each point on the path, and generate a set of normal cross sections perpendicular to the geometric center line; A Gaussian density profile is defined on the set of normal sections, wherein the peak value of the Gaussian density profile is located on the geometric center line; Based on the Gaussian density profile and the path point sequence, a global desired density distribution is synthesized.

7. The method according to claim 1, characterized in that, The process of fusing the distributed drift velocity, the high-pressure water jet's jet reaction force feedforward term, and the obstacle avoidance velocity term to generate a resultant velocity vector includes: Obtain obstacle distance data collected by the obstacle avoidance sensors mounted on the underwater robot; When the obstacle distance data is less than the preset safe distance, an original obstacle avoidance vector pointing away from the obstacle is generated; Calculate the tangential and normal components of the original obstacle avoidance vector; The density flux correction term is corrected using the normal component to limit the robot's tendency to traverse obstacles; The distributed drift velocity is corrected using the tangential component to guide the robot to glide along the contour of the obstacle; The modified density flux correction term and the modified distributed drift velocity are vector-summed to generate an intermediate velocity vector; The intermediate velocity vector is vector-superimposed with the feedforward term of the jet reaction force of the high-pressure water jet to generate the resultant velocity vector.

8. The method according to claim 1, characterized in that, The step of converting the resultant velocity vector into a thrust distribution command for the underwater robot's thruster includes: Establish a thruster configuration matrix for an underwater robot, wherein the configuration matrix represents the geometric relationship between the installation position of each thruster and the thrust direction; Acquire the roll and pitch angle data collected by the attitude sensors of the underwater robot; Calculate the decomposition components of gravity in the body coordinate system based on the aforementioned roll and pitch angle data; The gravity decomposition components are superimposed on the resultant velocity vector to generate a total force vector including gravity compensation. Multiply the total force vector on the left by the pseudo-inverse of the thruster configuration matrix to solve for the required rotational speed of each thruster; The rotational speed value is used as the thrust distribution command.

9. The method according to claim 1, characterized in that, The process of executing the thrust distribution command to drive the robot to move, and updating the local neighborhood window using the position data after the movement, includes: Record the sequence of actual trajectory points during the underwater robot's execution of the thrust distribution command; Calculate the lateral deviation between the actual trajectory point sequence and the preset ideal cleaning path; When the lateral deviation is greater than a preset trajectory correction threshold, a trajectory correction vector is generated; An additional potential field term is constructed using the trajectory correction vector, and the additional potential field term is superimposed on the density potential field function to generate a corrected density potential field function. Write the modified density potential field function into the memory, replacing the original density potential field function; The local neighborhood window is updated using the position data in the actual trajectory point sequence.

10. A distributed control system for multi-robot swarm collaborative formation, characterized in that, The system includes: The situational awareness module is used to establish a local neighborhood window centered on each robot's position, calculate the local density observation value within the window using the kernel density estimation method, acquire the operation area map of the underwater manifold, construct the global expected density distribution, and query the corresponding expected local density value based on the current position. The decision planning module is used to calculate the deviation between the observed local density value and the desired local density value, and construct the density potential field function; introduce local mass conservation constraints, and set the normal flux of the normal gradient of the density potential field function at the boundary of the neighborhood window to zero; solve the density flux correction term based on the normal flux constraint, and the correction term characterizes the incompressible characteristics of the robot swarm at the microscale. The motion calculation module is used to calculate the distributed drift velocity corresponding to each robot based on the density flux correction term; to fuse the density flux correction term after high-pressure water jet reaction force feedforward compensation with the distributed drift velocity and obstacle avoidance velocity term to generate a resultant velocity vector; and to convert the resultant velocity vector into a thrust distribution command for the underwater robot thruster. The execution feedback module is used to execute the thrust distribution command to drive the robot to move; when the lateral deviation of the trajectory is greater than a preset threshold, an additional potential field term for trajectory correction is constructed and the density potential field function is updated; the local neighborhood window is updated using the position data after the movement.

Images