Modular Robot Formation Maintenance Method, System, Device and Storage Medium

Abstract

The present invention relates to the field of robot technology, and specifically provides a modular robot formation maintenance method, system, device, and storage medium, including: constructing a plurality of formation graphs according to task types, and assigning a modular formation composed of a plurality of robots to each formation graph; designating a leader robot for the modular formation corresponding to each formation graph, and sending the formation graph to the corresponding leader robot; the leader robot decomposes the formation graph into relative position parameters between robots, and sends the relative position parameters to the follower robots in the same modular formation; the follower robots collect the actual position parameters of adjacent robots, and adjust their own motion parameters according to the actual position parameters and the relative position parameters; the leader robot is used to perform path planning and move along the planned path. The present invention realizes that when robots cannot obtain global position data, the formation can still be maintained, improving the stability of the robot formation.

Description

Technical Field

[0001] The present invention belongs to the technical field of robotics, and particularly relates to a modular robot formation maintenance method, system, device, and storage medium. Background Art

[0002] Multi-robots often operate in formation, and the formation control technology for multi-robot systems has been intensively studied. For example, using a formation of warehouse robots to process and distribute packages, millions of products are shipped to consumers around the world every day. Multi-robot formations have been widely applied in the military, logistics, and industrial fields.

[0003] Current research on formation control is mainly based on the global coordinate system and uses the absolute position information of robots to achieve it.

[0004] However, in real life, robots cannot always continuously obtain their pose information in the global coordinate system, such as in an unknown indoor environment or a dense forest cave. In an environment where GPS signals are interfered, a distributed modular robot formation control solution based on relative pose estimation is proposed to achieve multi-module robot formation control in the local coordinate system of each robot. Summary of the Invention

[0005] In view of the above deficiencies of the prior art, the present invention provides a modular robot formation maintenance method, system, device, and storage medium to solve the above technical problems.

[0006] In a first aspect, the present invention provides a modular robot formation maintenance method, including:

[0007] Construct multiple formation graphs according to the task type, and assign a modular formation composed of multiple robots to each formation graph;

[0008] Designate a leader robot for the modular formation corresponding to each formation graph, and send the formation graph to the corresponding leader robot;

[0009] The leader robot decomposes the formation graph into relative position parameters between robots, and sends the relative position parameters to the follower robots in the same modular formation;

[0010] The follower robots collect the actual position parameters of adjacent robots, and adjust their own motion parameters according to the actual position parameters and the relative position parameters;

[0011] The leader robot is used to perform path planning and move along the planned path.

[0012] In an alternative embodiment, multiple formation graphs are constructed according to the task type, and a modular formation composed of multiple robots is assigned to each formation graph, including:

[0013] Determine the formation type according to the task type, and the formation type includes triangular formation, diamond formation, chain formation, star formation, parallel line formation;

[0014] Determine the topological structure of the modular formation according to the number of robots and the formation type, and the topological structure uses the robots as nodes;

[0015] Construct the formation graph of the modular formation according to the topological structure.

[0016] In an alternative embodiment, constructing the formation graph of the modular formation according to the topological structure includes:

[0017] Define the formation graph according to the topological structure:

[0018]

[0019] where is the set of vertices, representing the robot formation set, is the set of edges; the directed edge from robot to robot represents the relative pose estimation of robot in the coordinate system of robot . In this case, robot is called the neighbor robot of robot ; then the set of neighbor robots of robot can be denoted as:

[0020]

[0021] Use the Laplacian matrix to describe the characteristics of the formation graph, and denote the number of edges connected to each node as the degree , and the degree matrix D is a diagonal matrix, expressed as:

[0022]

[0023] The adjacency matrix represents the edges in the graph. Confirm that the formation graph is an undirected graph. If there is an edge between node and node , then ; otherwise , and the adjacency matrix is expressed as:

[0024]

[0025] Then the graph The Laplacian matrix of

[0026] is defined as:

[0027] In an alternative embodiment, for each module formation corresponding to the formation map, a leader robot is specified, and the formation map is sent to the corresponding leader robot, including:

[0028] Preset the maximum following number corresponding to each formation type, where the maximum following number is the maximum number of follower robots led by a single leader robot;

[0029] Determine the number of leader robots according to the formation type adopted by the formation map, the number of nodes of the formation map, and the corresponding maximum following number;

[0030] Specify the corresponding number of leader robots for the corresponding module formation according to the number of leader robots.

[0031] In an alternative embodiment, the leader robot decomposes the formation map into relative position parameters between robots and distributes the relative position parameters to the follower robots in the same module formation, including:

[0032] Partition the Laplacian matrix L of the formation map into blocks according to leader robots and follower robots:

[0033]

[0034] where is the connection between leader robots, is the connection between follower robots, is the connection between the first type of leader robot and follower robots, is the connection between the second type of leader robot and follower robots;

[0035] Make the formation satisfy the consistency condition: , where x is the robot position vector; divide the position vector into the leader robot position and the follower robot position , then there is:

[0036]

[0037] Then the follower position:

[0038]

[0039] The relative position parameter is relative to the offset of.

[0040] In an alternative embodiment, the follower robot acquires the actual position parameters of the neighboring robots and adjusts its own motion parameters according to the actual position parameters and the relative position parameters, including:

[0041] The follower robot detects the actual position parameters between the leader robot and the neighboring robots through a four-sided sensor;

[0042] The follower robot calculates the distance error and the angle error between the leader robot and the neighboring robots according to the actual position parameters and the relative position parameters and the angle error , if the robot and The distance error and the angle error are defined as follows:

[0043]

[0044] where , is the desired distance, is the desired angle, , is the actual distance, is the actual angle, and the positive direction of the included angle is defined as the forward direction of the robot;

[0045] Then the control law of the multi-robot system is expressed as:

[0046]

[0047] where and are control gains, is the velocity of robot relative to robot , which is used to adjust the velocity of robot so that it can maintain the desired relative velocity with robot ; , are control inputs, which are the linear velocity and the angular velocity respectively; is the unit vector pointing from robot to robot , which determines how to adjust the motion of the robot according to the position and the velocity;

[0048]

[0049] To prevent the velocity from saturating and exceeding the upper limit, a velocity limit is defined as shown in the following equation:

[0050]

[0051] wherein and and and are respectively the maximum and minimum values of the linear velocity and the angular velocity.

[0052] In an alternative embodiment, the method further comprises:

[0053] defining a potential function of the robot and a neighbor robot :

[0054]

[0055] The force of the robot on the robot is the negative gradient of the potential function, i.e.:

[0056]

[0057]

[0058] wherein is the set collision distance, is the safety distance, is the relative position parameter of the robot in the coordinate system of the robot ; and and are proportionality coefficients;

[0059] Considering the formation graph and the set of neighbor robots, the total potential energy is:

[0060]

[0061] Adjusting the velocity according to the total potential energy through Newton's equations of motion:

[0062]

[0063] wherein m is the mass of the robot, is the control period;

[0064] According to the relative pose relationship, if the distance is less than the threshold , a collision avoidance potential energy is provided to cause the robot to avoid colliding with the robot .

[0065] In a second aspect, the present invention provides a modular robot formation maintenance system, comprising:

[0066] A building module, configured to build multiple formation diagrams according to task types and assign a module formation composed of multiple robots to each formation diagram;

[0067] A configuration module, configured to specify a pilot robot for the module formation corresponding to each formation diagram and send the formation diagram to the corresponding pilot robot;

[0068] A first processing module, configured to decompose the formation diagram into relative position parameters between robots by the pilot robot and send the relative position parameters to follower robots in the same module formation;

[0069] A second processing module, configured to collect actual position parameters of the follower robots with adjacent robots and adjust their own motion parameters according to the actual position parameters and the relative position parameters;

[0070] The pilot robot is configured to perform path planning and move along the planned path.

[0071] In a third aspect, a device is provided, including:

[0072] A memory, configured to store a modular robot formation maintenance program;

[0073] A processor, configured to implement the steps of the modular robot formation maintenance method provided in the first aspect when executing the modular robot formation maintenance program.

[0074] In a fourth aspect, a computer-readable storage medium is provided, on which a modular robot formation maintenance program is stored. When the modular robot formation maintenance program is executed by a processor, the steps of the modular robot formation maintenance method provided in the first aspect are implemented.

[0075] The beneficial effects of the present invention are as follows. The modular robot formation maintenance method, system, device and storage medium provided by the present invention build a modular formation to reduce the formation scale, then specify a pilot robot in the module formation, and the rest are follower robots. The pilot robot can perform path planning, and the follower robots maintain the formation by detecting the actual position parameters between adjacent robots. At this time, the follower robots can maintain the module formation while moving following the pilot robot without obtaining global position data. In addition, by setting a unified obstacle avoidance potential function, collisions between robots in the formation are avoided. The present invention realizes that the formation can still be maintained when the robots cannot obtain global position data, improving the stability of the robot formation.

[0076] In addition, the design principle of the present invention is reliable and the structure is simple, having a very wide application prospect. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the accompanying drawings required for the description of the embodiments or the prior art. Obviously, for those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0078] Figure 1 It is a schematic flowchart of the method according to an embodiment of the present invention.

[0079] Figure 2 It is a schematic diagram of the formation of the method according to an embodiment of the present invention.

[0080] Figure 3 It is a schematic diagram of the collision range of the robot of the method according to an embodiment of the present invention.

[0081] Figure 4 It is a schematic diagram of obstacle avoidance for robot formation switching of the method according to an embodiment of the present invention.

[0082] Figure 5 It is an effect diagram of obstacle avoidance for robot formation of the method according to an embodiment of the present invention.

[0083] Figure 6 It is a schematic flowchart of the method according to another embodiment of the present invention.

[0084] Figure 7 It is a schematic block diagram of the system according to an embodiment of the present invention.

[0085] Figure 8 It is a schematic structural diagram of a device provided by an embodiment of the present invention. Detailed implementation manners

[0086] In order to enable those skilled in the art of the present technology to better understand the technical solutions in the present invention, the following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0087] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the technical field of the present invention. The terms used in the description of the present invention in this specification are only for the purpose of describing specific embodiments, and are not intended to limit the present invention.

[0088] The modular robot formation maintenance method provided by the embodiments of the present invention is executed by a computer device. Correspondingly, the modular robot formation maintenance system runs in the computer device.

[0089] Figure 1 It is a schematic flowchart of the method according to an embodiment of the present invention. Among them, Figure 1 The execution subject can be a modular robot formation maintenance system. According to different requirements, the order of the steps in this flowchart can be changed, and some can be omitted.

[0090] Such as Figure 1 shown, the method includes:

[0091] S1. Construct multiple formation maps according to the task type, and assign a modular formation composed of multiple robots to each formation map;

[0092] S2. Designate a leader robot for the modular formation corresponding to each formation map, and send the formation map to the corresponding leader robot;

[0093] S3. The leader robot decomposes the formation map into relative position parameters between robots, and distributes the relative position parameters to the follower robots in the same modular formation;

[0094] S4. The follower robots collect the actual position parameters of the neighboring robots, and adjust their own motion parameters according to the actual position parameters and the relative position parameters;

[0095] The leader robot is used to perform path planning and move along the planned path.

[0096] In view of the fact that the current research is mainly based on the global coordinate system, the present invention proposes a behavior-based distributed formation control strategy, which uses the relative position information of robots to achieve formation control. And a distributed multi-robot obstacle avoidance method is proposed, introducing relative pose perception information into in-team obstacle avoidance. Finally, different basic behavior components are designed for different roles of the members in the multi-robot system. The leader robot is responsible for navigation, and other robots can achieve multi-robot formation control in the local coordinate system when the global information is unknown, successfully combining relative pose perception information with behavior-based formation control.

[0097] Among them, the modular robot platform uses 5 Mecanum omnidirectional mobile modular robots. A monocular camera and a cooperative identifier are respectively installed on the front, back, left side, and right side of each modular robot. The cooperative identifier uses ARTAG to define the identity ID of each modular robot; the relative pose estimation data between multi-robot members is obtained by using UWB ranging sensors and ARTAG artificial identifiers.

[0098] On this basis, a formation control law based on relative pose is further proposed. Aiming at the problems existing in the traditional artificial potential field method, the repulsive force and gravitational function are improved and a dynamic adjustment coefficient is introduced. At the same time, collision-free formation of robots is achieved according to relative pose estimation. Finally, all sub-behaviors are weighted and fused to obtain the final behavior vector of the robot. In the present invention, all measurement and control signals are carried out within the local coordinate system of each robot.

[0099] In an embodiment of the present invention, based on step S1, a possible embodiment will be given below to non-restrictively elaborate on its specific implementation scheme.

[0100] S101. Determine the formation type according to the task type, and the formation type includes triangular formation, diamond formation, chain formation, star formation, and parallel line formation.

[0101] Please refer to Figure 2 , where the yellow robot is the leader. The multi-robot system can adjust the number of robots in the formation and the pose between robots according to the set formation library to adapt to different environments.

[0102] Establish a task attribute vector: [coverage range, collaboration intensity, movement speed, environmental complexity, fault tolerance requirement].

[0103] Example task characteristics:

[0104] Search and rescue: coverage range (high), collaboration intensity (medium), movement speed (medium), environmental complexity (high), fault tolerance requirement (high);

[0105] Transportation task: coverage range (low), collaboration intensity (high), movement speed (low), environmental complexity (low), fault tolerance requirement (medium);

[0106]

[0107] Adopt a fuzzy logic inference system:

[0108] Input: standardized value of the task feature vector;

[0109] Rule base: IF (coverage range is high AND collaboration intensity is medium) THEN star formation; IF (collaboration intensity is high AND environmental complexity is low) THEN triangular formation;

[0110] Output: formation type confidence matrix.

[0111] S102. Determine the topological structure of the modular formation according to the number of robots and the formation type, and the topological structure takes the robots as nodes.

[0112] Set the minimum number of robots: 3 triangle formations, 4 diamond formations, 2 chain formations, star formation, N ≥ 3 (center node + satellite node), 2 parallel line formations, each with ≥ 2 robots.

[0113] Set the fabric extension policy:

[0114] Chain expansion: When the number of robots is greater than 2, a segmented chain structure is adopted, with the interval between segments being L=1.5d (d is the robot diameter);

[0115] Star expansion: Number of central nodes: ceil (N / 5); Satellite node density: adjusted according to the communication radius R, the distance between adjacent satellites ≥ 2R;

[0116] Diamond expansion: The primitive structure includes 4 nodes forming a diamond with a side length of L. The expansion mode includes mirroring along the x / y axis with the center as the origin, and keeping the distance between adjacent diamonds ≥ 2R.

[0117] Calculation method of geometric parameters of formation:

[0118] Triangle formation:

[0119] Side length L = k * v max / ω max (k is the safety factor, v max / ω max is the maximum linear velocity / angular velocity);

[0120] Center coordinates: (Σx i / 3, Σy i / 3);

[0121] Parallel Line Formation:

[0122] Line spacing D = 2R + 0.5m;

[0123] The robot spacing per line is d = 1.2R.

[0124] The topological structure can be generated based on the geometric parameters.

[0125] S103: Construct a formation graph of the module formation according to the topological structure.

[0126] Define the formation graph based on the topology:

[0127]

[0128] in is a set of vertices, representing a set of robot formations, is a set of edges; from the robot To the robot The directed edges represent the robot In the robot Relative pose estimation in the coordinate system. In this case, the robot Is called the robot Of the neighbor robot; then the robot The set of neighbor robots can be denoted as:

[0129]

[0130] The Laplacian matrix is used to describe the characteristics of the formation graph. The number of edges connected to each node is denoted as the degree , and the degree matrix D is a diagonal matrix, denoted as:

[0131]

[0132] Adjacency matrix Represents the edges in the graph. It is confirmed that the formation graph is an undirected graph. If there is an edge between node And node [[ID=3'1]]Then ; Otherwise , the adjacency matrix is denoted as:

[0133]

[0134] Then the graph The Laplacian matrix is defined as:

[0135] .

[0136] In an embodiment of the present invention, based on step S2, a possible embodiment will be given below to non - restrictively elaborate on its specific implementation scheme.

[0137] S201. Preset the maximum following number corresponding to each formation type. The maximum following number is the maximum number of following robots led by a single leader robot.

[0138] Basis for determining the maximum following number:

[0139] Communication constraint: The control range of each leader ≤ 2R (R is the communication radius);

[0140] Kinematics constraint: The acceleration response delay of the follower Δt ≤ 0.5s;

[0141] Collaboration complexity: The task - processing ability of each leader Q = 0.8C (C is the single - node computing ability).

[0142] Formation type - maximum following number comparison table:

[0143]

[0144] Dynamic adjustment formula for the maximum following quantity: Current maximum following quantity = Basic maximum following quantity × Environmental complexity coefficient × (1 - Current task density ÷ Maximum task density).

[0145] Parameter description:

[0146] Basic maximum following quantity: The default maximum following quantity corresponding to the formation type (for example, 2 for a triangle).

[0147] Environmental complexity coefficient: 1.0 for a simple environment and 0.6 for a complex environment.

[0148] Current task density: A quantitative value of the current task load (0 - 1).

[0149] Maximum task density: The maximum task load that the system can handle.

[0150] S202. Determine the number of leader robots according to the formation type adopted in the formation diagram, the number of nodes in the formation diagram, and the corresponding maximum following quantity.

[0151] Basic calculation formula:

[0152] Formula 1: Preliminary calculation of the number of leaders. Round up the result of dividing the total number of robots by (the maximum following number of a single leader + 1) to obtain the preliminary number of leader robots. Formula expression: Preliminary number of leaders = Round up (Total number of robots ÷ (Maximum following number of a single leader + 1)).

[0153] Formula 2: The number of leaders after structural correction. Multiply the preliminary number of leaders by the formation structure correction coefficient (0.8 for a triangle, 1.2 for a star, and 1.0 for other types), and then round up to obtain the adjusted number of leaders. Formula expression: Adjusted number of leaders = Round up (Preliminary number of leaders × Formation structure correction coefficient).

[0154] Formula 3: Final number of leaders (including redundancy). Multiply the adjusted number of leaders by 1.15 (reserving 15% redundancy), and then round up to obtain the final number of leader robots. Formula expression: Final number of leaders = Round up (Adjusted number of leaders × 1.15).

[0155] Specifically, according to the given total number of nodes, the maximum number of following robots that a single leader robot can lead, and the formation type, calculate the required number of leader robots. The specific steps are as follows:

[0156] Set different formation structure correction coefficients according to different formation types. If the formation type is triangular, the correction coefficient is 0.8; if it is diamond, chain or parallel line formation, the correction coefficient is 1.0; if it is star formation, the correction coefficient is 1.2. Select the corresponding value from these coefficients as the formation structure correction coefficient for this calculation according to the input formation type.

[0157] Divide the total number of nodes by the result of adding 1 to the maximum number of follower robots led by a single leader robot, and then round up this division result to obtain a preliminary number of leader robots. For example, if the total number of nodes is 10 and a single leader robot can lead 3 follower robots, then it is 10 divided by (3 + 1), getting 2.5, and after rounding up, it is 3.

[0158] Then, multiply the preliminary number of leader robots obtained just now by the previously selected formation structure correction coefficient, and round up this multiplication result to obtain an adjusted number of leader robots.

[0159] Finally, considering that a 15% redundancy is required, multiply the adjusted number of leader robots by 1.15, and round up this result. The final obtained value is the required number of leader robots.

[0160] For example: 10 robots are in star formation, the basic maximum following number is 3, the environmental complexity is simple (coefficient 1.0), and the current task density is 0.4. Calculation steps:

[0161] Preliminary leader number: 10÷(3 + 1)=2.5 → Rounded up to 3.

[0162] Structure correction: 3×1.2 (star formation coefficient)=3.6 → Rounded up to 4.

[0163] Redundancy adjustment: 4×1.15=4.6 → Rounded up to 5. Final result: 5 leader robots are required.

[0164] S203. Designate the corresponding number of leader robots for the corresponding module formations according to the number of leader robots.

[0165] Allocation algorithm steps:

[0166] Step 1: Initialization

[0167] Broadcast the node ID and the initial position;

[0168] Calculate the node degree (the number of neighbors).

[0169] Step 2: Candidate screening

[0170] Select nodes with a degree ≥ k_min (k_min = 3);

[0171] Preferentially select nodes with energy ≥ E_threshold (E_threshold = 70%).

[0172] Step 3: Hierarchical election

[0173] Layer 1: Select the node with the largest coverage area as the main leader;

[0174] Layer 2: Select the node with the highest connectivity among the remaining nodes as the sub-leader;

[0175] Recursively until the number of L_final is satisfied.

[0176] Step 4: Dynamic adjustment

[0177] Adjust the leader robots in the formation graph to ensure the normal operation of the formation. The specific steps are as follows:

[0178] Traverse each node in the formation graph.

[0179] For each node, its energy condition will be checked. If the energy of this node is lower than 30% of the maximum energy, it means that the energy of this node is relatively low and it may not be able to continue to perform the duties of the leader robot well.

[0180] When it is found that the energy of a certain node is lower than 30% of the maximum energy, an operation of "promoting a backup node" will be executed, that is, selecting one from the pre-prepared backup nodes to replace this low-energy node as the new leader robot.

[0181] After traversing all the nodes in the formation graph, return the updated formation graph, in which the allocation of the leader robots in the updated formation graph has been adjusted according to the node energy conditions.

[0182] In addition, when specifying the leader robot, obtain the network communication quality of all the robots in the formation of the acquisition module, and select the robot with the best network communication quality as the leader robot.

[0183] In an embodiment of the present invention, based on step S3, a possible embodiment will be given below to non-restrictively elaborate on its specific implementation scheme.

[0184] Partition the Laplacian matrix L of the formation graph into blocks according to the leader robots and the follower robots:

[0185]

[0186] Among them, is the connection between the leader robots, is the connection between follower robots, is the connection between the first type of leader robot and follower robots, is the connection between the second type of leader robot and follower robots;

[0187] Let the formation satisfy the consensus condition: , where x is the robot position vector; the position vector is divided into the leader robot position and the follower robot position , then there is:

[0188]

[0189] Then the follower position:

[0190]

[0191] The relative position parameter is relative to the offset.

[0192] Taking the triangular formation as an example:

[0193] The formation topology includes: leader: robot 1 (R1), followers: robot 2 (R2) and robot 3 (R3).

[0194] The connection relationship is: R1 is connected to R2 and R3, and R2 is interconnected with R3.

[0195] The Laplacian matrix is:

[0196]

[0197] After partitioning:

[0198]

[0199] Calculate the relative position of the follower:

[0200] Assume the leader position is (the coordinate origin), then:

[0201]

[0202] R2 and R3 need to maintain the lateral and longitudinal relative positions with R1 as 1 / 3 unit (the specific unit is determined by the formation scaling factor), respectively.

[0203] In an embodiment of the present invention, based on step S4, a possible embodiment will be given below to non - restrictively elaborate on its specific implementation scheme.

[0204] The follower robot detects the actual position parameters between itself and the leader robot and neighbor robots through sensors on all four sides;

[0205] The follower robot calculates the distance error and angle error between itself and the leader robot and neighbor robots based on the actual position parameters and the relative position parameters and the angle error , if the robot and The distance error and angle error are defined as follows:

[0206]

[0207] where, , is the desired distance, is the desired angle, , is the actual distance, is the actual angle, and the positive direction of the included angle is defined as the forward direction of the robot;

[0208] Then the control law of the multi-robot system is expressed as:

[0209]

[0210] where, and are control gains, is the velocity of robot relative to robot , which is used to adjust the velocity of robot so that it can maintain the desired relative velocity with robot ; , are control inputs, which are the linear velocity and angular velocity respectively; is the unit vector from robot to robot , which determines how to adjust the motion of the robot according to the position and velocity;

[0211]

[0212] To prevent the velocity from saturating and exceeding the upper limit, the velocity limit is defined as shown in the following equation:

[0213]

[0214] where , , , are the maximum and minimum values of the linear velocity and angular velocity respectively.

[0215] This formation control law is based on the premise that the robot topological structure remains unchanged, only considering the formation and maintenance of the formation. Subsequently, obstacle avoidance and formation switching need to be further considered and superimposed on the distributed formation controller. Each robot only depends on the local information of relative pose estimation and simple control rules, and can achieve global consistency and stability of the entire multi-robot formation system. This method does not require global information, and all measurement and control signals are developed in the local coordinate system of each robot. By using local relative position measurements, global asymptotic convergence to the desired planar formation is achieved. Each robot independently calculates its own control input based on the relative pose, realizing distributed formation control.

[0216] Based on the above embodiments, in order to further improve the reliability of the module formation, as an implementable method, a unified obstacle avoidance rule is set, including:

[0217] During the process of the multi-robot system executing the formation task, each robot may have the risk of path crossing with other robots, resulting in internal collisions. To ensure the safe operation of the entire system, while avoiding external obstacles, the robot must take effective measures to avoid internal collisions. That is, when the robots approach each other, a mutually repulsive velocity is generated to prevent collisions.

[0218] When the distance between the robots shrinks to a preset safety threshold, the robot will immediately stop moving to ensure the safety and stability of the formation. Figure 3 Shows the collision range between robot and robot Among them, is the safety distance, is the collision distance.

[0219] Define a circle with the collision distance as the radius for each robot, and set it as the collision range of the robot. When other robots or obstacles enter this range, it is regarded as a potential collision, and corresponding obstacle avoidance strategies need to be taken. Drawing on the idea of the traditional artificial potential field method, a new potential energy function is proposed. This function takes the robot's own coordinate system as the center and constructs a virtual potential field around the robot. There is a virtual potential field around each robot in the formation, that is, the origin of the coordinate system of robot is the position of robot , and the position of robot is determined relative to the coordinate system of robot .

[0220] In formation control, to accurately simulate and regulate the interaction between robots, two basic virtual forces are introduced: the repulsive force between robots and the attractive force between robots. The repulsive force between robots causes adjacent robots to move away from each other, achieving the effect of avoiding collisions between robots; the attractive force between robots makes adjacent robots approach each other to prevent the formation from disbanding. These two forces can adjust the potential energy function according to the desired distance between robots.

[0221] Define the potential energy function of robot with a neighbor robot as:

[0222]

[0223] The force of robot on robot is the negative gradient of the potential energy function, that is:

[0224]

[0225]

[0226] where is the set collision distance, is the safety distance, is the relative position parameter of robot in the coordinate system of robot , , , are proportionality coefficients;

[0227] Considering the formation graph and the set of neighbor robots, the total potential energy is:

[0228]

[0229] Adjust the velocity according to the total potential energy through Newton's equations of motion:

[0230]

[0231] where m is the mass of the robot, is the control period;

[0232] According to the relative pose relationship, if the distance is less than the threshold , provide an obstacle avoidance potential energy so that robot avoids colliding with robot . Ensure that robot maintains a safe distance while also trying to maintain the desired distance from robot . This method has a small computational amount, good real-time performance, and a simple structure. While maintaining the formation shape, robots can also avoid colliding with each other.

[0233] This embodiment provides a path generation for a pilot robot;

[0234] RRT* has good performance, but in an unknown environment, the path optimization of RRT* may need to be updated frequently, thus increasing the computational burden, and the algorithm planning efficiency and real-time performance are low. To address the above problems, the Fast-RRT* algorithm is proposed. During the generation process, instead of randomly generating points across the entire map, a lazy sampling method is used to reduce invalid sampling points. During the sampling phase, each time before inserting a point, calculate the length from the insertion point to the root node and compare it with the previous optimal length. If it is less than the optimal length, then retain the insertion point. Compare the distance between the current sampling point and the previous sampling point to the target. If it is closer, then retain the sampling point and the sampling is successful. If not, continue sampling until the above constraints are met. The constraints are expressed as equations:

[0235]

[0236] Through the two constraints, useless sampling points can be filtered out, and the optimization and collision detection of these sampling points are also avoided, reducing the growth range of the overall random search tree, which can improve the algorithm convergence speed and has high real-time performance.

[0237] Multi-robot behavior decision-making design;

[0238] In the multi-robot formation control framework, refining complex tasks into multiple simple subtasks helps to complete the entire operation process more efficiently. Each subtask is responsible for generating a motion command and is assigned a corresponding priority according to the importance of the task. Among them, tasks with lower priorities are projected into the space of tasks with higher priorities to ensure that they do not conflict with tasks with higher priorities. The weighted average method is used to fuse the basic behaviors of the robots to determine the final behavior of the robots. In this method, the instruction vector generated by each subtask will be weighted according to its corresponding weight. Accumulate all the weighted vectors to form an overall behavior response vector . To ensure the consistency and effectiveness of the behavior, the accumulated result also needs to be normalized to ensure that the final behavior response vector conforms to the physical limitations of the actual motion. [[ID=X]]

[0239] The multi-robot formation of the present invention consists of a target navigation mode , a robot obstacle avoidance mode , and a formation maintenance mode Composition. The target navigation motion behavior refers to the robot controlling its movement towards the target according to the task target point. Generally, this behavior is triggered when the multi-robot system has formed the desired collective formation and needs to move in formation to the target point. After planning the robot's path, the path target points are sent to the robot, and the leader robot needs to calculate the target linear velocity and angular velocity of the robot based on these path points.

[0240] In addition to considering the robot obstacle avoidance and in-team obstacle avoidance mentioned above, the robot obstacle avoidance mode of the module formation also takes into account the robot avoiding obstacles through formation changes. If no obstacle is detected, the multi-robot system will form and maintain the predefined formation. When a robot encounters an obstacle, both the leader robot and the follower robots will perform robot obstacle avoidance, and the follower robots will use the traditional PID control algorithm to reconstruct the predefined formation. In order to enable the robots to better maintain the formation during obstacle avoidance, a formation switching method based on environmental changes is proposed to reconstruct the multi-robot formation and better pass through the current environment. When the road narrows, that is, when the terrain becomes a corridor type, if the leader robot or at least half of the followers detect the corridor, the formation will switch to a linear formation. When passing through the corridor, it will return to the original formation when starting, as Figure 4 shown.

[0241] For the obstacle avoidance effect of the module formation, please refer to Figure 5 . By adjusting the proportionality coefficients of each motion mode according to experience and setting the priority of multi-robot system obstacle avoidance higher than formation maintenance, the overall behavior of the robot is obtained, as shown in the following formula.

[0242]

[0243] where is the behavior decision-making quantity of the leader, is the behavior decision-making quantity of the followers, , , are the proportionality coefficients of target navigation, robot obstacle avoidance, and formation maintenance respectively.

[0244] Please refer to Figure 6 . This embodiment gives a method for maintaining the robot formation:

[0245] 1. Obtain the pose information between the module robots

[0246] Using UWB ranging sensors, ARTAG artificial markers, combined with the monocular cameras and cooperative markers installed on each side (front, rear, left, and right sides) of the module robots, obtain the relative pose estimation data between the multi-robot members.

[0247] 2. Construct the adjacency matrix

[0248] Based on the graph theory of multi-robot systems, a formation model is established. The robots are regarded as nodes, and the relative poses are regarded as edges to construct an adjacency matrix. At the same time, the Laplacian matrix is used to describe the characteristics of the graph (including the calculation of the degree matrix and the adjacency matrix), and the topological structure of the formation graph is defined.

[0249] 3. Determine whether it is a leader robot

[0250] If it is a leader robot: Execute leader path generation. Adopt the Fast-RRT * algorithm to generate an optimized path through constraints such as lazy sampling, comparison of the length between the inserted point and the root node, and comparison of the distance between the sampling point and the target.

[0251] If it is not a leader robot: Form a formation according to the formation control law based on relative poses, calculate the distance error and angle error from the leader robot and adjacent robots, adjust its own linear velocity and angular velocity, and at the same time constrain the speed range through the speed limit formula.

[0252] 4. Determine whether obstacle avoidance is required

[0253] If obstacle avoidance is required:

[0254] Adopt an improved APF obstacle avoidance algorithm to handle external obstacles;

[0255] Adopt an in-team obstacle avoidance strategy (calculate the repulsive force and gravitational force between robots based on the potential energy function to avoid robot collisions and dissolutions).

[0256] If obstacle avoidance is not required: Directly proceed to the next step.

[0257] 5. Behavior decision-making design

[0258] Split the task into subtasks such as target navigation, robot obstacle avoidance, and formation maintenance, and assign priorities (the priority of obstacle avoidance is higher than that of formation maintenance).

[0259] Target navigation: The leader robot calculates the target linear velocity and angular velocity according to the planned path.

[0260] Robot obstacle avoidance: The leader and follower robots perform obstacle avoidance, and the follower robots reconstruct the formation through PID control; when the environment changes (such as a corridor), the formation is switched (such as a linear formation).

[0261] Formation maintenance: Maintain or reconstruct the desired formation through the formation control law.

[0262] 6. Weighted fusion

[0263] Fusion the subtasks such as target navigation, obstacle avoidance, and formation maintenance by weighted according to the proportional coefficient, and consider the priority relationship to obtain the total behavior of the robot.

[0264] 7. Generate the final behavior vector

[0265] Process the result after weighted fusion to generate the final behavior vector of the robot, and drive the robot to execute corresponding movements.

[0266] In some embodiments, the modular robot formation maintenance system may include multiple functional modules composed of computer program segments. The computer programs of each program segment in the modular robot formation maintenance system may be stored in the memory of the computer device and executed by at least one processor to perform the functions of modular robot formation maintenance (see Figure 1 description).

[0267] In this embodiment, the modular robot formation maintenance system can be divided into multiple functional modules according to the functions it performs, such as Figure 7 shown. The functional modules of the system may include: a construction module, a configuration module, a first processing module, and a second processing module. The module referred to in the present invention means a series of computer program segments that can be executed by at least one processor and can complete fixed functions, and are stored in the memory. In this embodiment, the functions of each module will be described in detail in subsequent embodiments.

[0268] The construction module is used to construct multiple formation diagrams according to the task type and assign a module formation composed of multiple robots to each formation diagram;

[0269] The configuration module is used to specify a leader robot for the module formation corresponding to each formation diagram and send the formation diagram to the corresponding leader robot;

[0270] The first processing module is used for the leader robot to decompose the formation diagram into relative position parameters between robots and send the relative position parameters to the follower robots in the same module formation;

[0271] The second processing module is used for the follower robots to collect the actual position parameters of adjacent robots and adjust their own motion parameters according to the actual position parameters and the relative position parameters;

[0272] The leader robot is used to perform path planning and move along the planned path.

[0273] Figure 8The modular robot formation maintenance method provided by the embodiments of this application can be applied to devices. Those skilled in the art can understand that the device structure involved in the embodiments of the present invention does not constitute a limitation on the device. The device may include more or fewer components than shown in the figure, or combine some components, or have different component arrangements. In the embodiments of the present invention, the device includes, but is not limited to, laptop computers, desktop computers, workbenches, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The device may also represent various forms of mobile devices, such as personal digital processors, cellular phones, smart phones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the embodiments of this application described and / or claimed herein.

[0274] Among them, the device 800 may include: a processor 810, a memory 820, and a communication unit 830. These components communicate through one or more buses. Those skilled in the art can understand that the structure of the server shown in the figure does not constitute a limitation on the present invention. It can be a bus structure, a star structure, or may include more or fewer components than shown in the figure, or combine some components, or have different component arrangements.

[0275] Among them, the memory 820 can be used to store the execution instructions of the processor 810. The memory 820 can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic memory, flash memory, magnetic disk, or optical disk. When the execution instructions in the memory 820 are executed by the processor 810, the device 800 can execute some or all of the steps in the above method embodiments.

[0276] The processor 810 is the control center of the storage device, connecting various parts of the entire electronic device through various interfaces and circuits. By running or executing software programs and / or modules stored in the memory 820, and by calling data stored in the memory, it performs various functions of the electronic device and / or processes data. The processor may be composed of an integrated circuit (IC), for example, it may be composed of a single packaged IC, or it may be composed of multiple packaged ICs with the same or different functions connected together. For example, the processor 810 may only include a central processing unit (CPU). In the embodiments of the present invention, the CPU may be a single arithmetic core or may include multiple arithmetic cores.

[0277] The communication unit 830 is used to establish a communication channel so that the storage device can communicate with other devices. It receives user data sent by other devices or sends user data to other devices.

[0278] The present invention also provides a computer storage medium. Among them, the computer storage medium can store a program, and when the program is executed, it may include some or all of the steps in the embodiments provided by the present invention. The storage medium may be a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM), etc.

[0279] Those skilled in the art can clearly understand that the technology in the embodiments of the present invention can be implemented by means of software plus a necessary general hardware platform. Based on such an understanding, the technical solutions in the embodiments of the present invention, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk, etc., which can store program codes, and includes several instructions to enable a computer device (which may be a personal computer, a server, or a second device, a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention.

[0280] For the same or similar parts between the various embodiments in this specification, reference can be made to each other. In particular, for the device embodiments, since they are basically similar to the method embodiments, the description is relatively simple, and the relevant parts can be referred to the description in the method embodiments.

[0281] In several embodiments provided by the present invention, it should be understood that the disclosed systems and methods can be implemented in other ways. For example, the system embodiments described above are merely illustrative. For example, the division of the modules is only a logical function division. In actual implementation, there may be other division methods. For example, multiple modules or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed coupling or direct coupling or communication connection between each other can be through some interfaces. The indirect coupling or communication connection of the system or module can be in electrical, mechanical or other forms.

[0282] The modules described as separate components may or may not be physically separated. The components displayed as modules may or may not be physical modules, that is, they can be located in one place or distributed to multiple network modules. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0283] In addition, in each embodiment of the present invention, the functional modules can be integrated in a processing module, or each module can exist physically alone, or two or more modules can be integrated in one module.

[0284] Although the present invention has been described in detail by referring to the accompanying drawings and in combination with the preferred embodiments, the present invention is not limited thereto. Without departing from the spirit and essence of the present invention, those of ordinary skill in the art can make various equivalent modifications or substitutions to the embodiments of the present invention, and these modifications or substitutions should all be within the scope of the present invention. / Any person skilled in the art within the technical scope disclosed by the present invention can easily think of changes or substitutions, and they should all be covered within the protection scope of the present invention.

Claims

1. A modular robot formation maintenance method, characterized in that, Including: Construct multiple formation diagrams according to the task type, and assign a modular formation composed of multiple robots to each formation diagram; Designate a leader robot for the modular formation corresponding to each formation diagram, and send the formation diagram to the corresponding leader robot; The leader robot decomposes the formation diagram into relative position parameters between robots, and sends the relative position parameters to the follower robots in the same modular formation; The follower robots collect the actual position parameters of adjacent robots, and adjust their own motion parameters according to the actual position parameters and the relative position parameters; The leader robot is used to perform path planning and move along the planned path; The method further includes: Define the robot with a neighbor robot of the potential function: Robot For the robot the force is the negative gradient of the potential function, i.e.: Among them, is the set collision distance, is the safety distance, is the robot in the robot relative position parameter in the coordinate system, , , are proportionality coefficients; Considering the formation diagram and the set of neighbor robots, the total potential energy is: Adjust the speed according to the total potential energy through Newton's equations of motion: where m is the mass of the robot, is the control period; According to the relative pose relationship, if the distance is less than the threshold , provide an obstacle avoidance potential energy so that the robot avoids collision with the robot .

2. The method according to claim 1, characterized in that Construct multiple formation diagrams according to the task type, and assign a modular formation composed of multiple robots to each formation diagram, including: Determine the formation type according to the task type, and the formation type includes triangular formation, diamond formation, chain formation, star formation, parallel line formation; Determine the topological structure of the modular formation according to the number of robots and the formation type, and the topological structure takes robots as nodes; Construct the formation diagram of the modular formation according to the topological structure.

3. The method according to claim 2, characterized in that, Construct the formation diagram of the modular formation according to the topological structure, including: Define the formation diagram according to the topological structure: Among them is the set of vertices, representing the robot formation set, is the set of edges; the directed edge from robot to robot represents the relative pose estimation of robot in the coordinate system of robot ; in this case, robot is called the neighbor robot of robot ; then the set of neighbor robots of robot can be denoted as: Describe the characteristics of the formation graph using the Laplacian matrix, and denote the number of edges connected to each node as the degree , and the degree matrix D is a diagonal matrix, expressed as: Adjacency matrix Represents the edges in the graph. Confirm that the formation graph is an undirected graph. If there is an edge between node and node , then ; otherwise , the adjacency matrix is represented as: Then the figure The Laplacian matrix is defined as: 。 4. The method according to claim 1, characterized in that, Designate a leader robot for the modular formation corresponding to each formation diagram, and send the formation diagram to the corresponding leader robot, including: Preset the maximum following number corresponding to each formation type, and the maximum following number is the maximum number of follower robots led by a single leader robot; Determine the number of leader robots according to the formation type adopted by the formation diagram, the number of nodes of the formation diagram, and the corresponding maximum following number; Designate the corresponding number of leader robots for the corresponding modular formation according to the number of leader robots.

5. The method according to claim 1, characterized in that The leader robot decomposes the formation diagram into relative position parameters between robots, and sends the relative position parameters to the follower robots in the same modular formation, including: Block the Laplacian matrix L of the formation diagram according to the leader robot and the follower robot: Among them, is the connection between the leading robots, is the connection between the following robots, is the connection between the first type of leading robot and the following robot, is the connection between the second type of leading robot and the following robot; Let the formation satisfy the consistency condition: , where x is the robot position vector; divide the position vector into the position of the leader robot and the position of the follower robot , then we have: Then the follower position: The relative position parameter is the offset relative to .

6. The method according to claim 1, wherein The follower robots collect the actual position parameters of adjacent robots, and adjust their own motion parameters according to the actual position parameters and the relative position parameters, including: The follower robots detect the actual position parameters between the leader robot and the neighbor robots through four-sided sensors; The follower robot calculates the distance error and the angle error between the leader robot and the neighbor robots according to the actual position parameter and the relative position parameter and the angle error , if the robot and The distance error and the angle error are defined as follows: Among them, , is the desired distance, is the desired angle, , is the actual distance, is the actual angle, and the positive direction of the included angle is defined as the forward direction of the robot; Then the control law of the multi-robot system is expressed as: Among them, and are control gains, is the robot relative to the robot speed, used to adjust the speed of the robot so that it can maintain the desired relative speed with the robot ; , are control inputs, which are the linear velocity and angular velocity respectively; is the unit vector pointing from the robot to the robot , which determines how to adjust the motion of the robot according to the position and speed. In order to prevent speed saturation from causing the speed to exceed the upper limit, define the speed limit as shown in the following formula: Among them and and and are respectively the maximum and minimum values of the linear velocity and the angular velocity.

7. A modular robot formation maintenance system, characterized in that Including: A construction module for constructing multiple formation diagrams according to the task type, and assigning a modular formation composed of multiple robots to each formation diagram; A configuration module for designating a leader robot for the modular formation corresponding to each formation diagram, and sending the formation diagram to the corresponding leader robot; The first processing module is used for the leading robot to decompose the formation map into relative position parameters between robots and send the relative position parameters to the following robots in the same-module formation; The second processing module is used for the following robots to collect the actual position parameters of adjacent robots and adjust their own motion parameters according to the actual position parameters and the relative position parameters; The leading robot is used to execute path planning and move along the planned path; It further includes: Define a robot with a neighboring robot 's potential function: Robot For the robot the force is the negative gradient of the potential function, i.e.: Among them, is the set collision distance, is the safety distance, is the robot in the robot relative position parameters in the coordinate system, , , are proportionality coefficients; Considering the formation map and the set of neighbor robots, the total potential energy is: Adjust the speed according to the total potential energy through Newton's equations of motion: where m is the mass of the robot, is the control period; According to the relative pose relationship, if the distance is less than the threshold , a collision avoidance potential energy is provided so that the robot avoids colliding with the robot .

8. An apparatus, characterized in that, It includes: A memory for storing a modular robot formation maintenance program; A processor for implementing the steps of the modular robot formation maintenance method as described in any one of claims 1-6 when executing the modular robot formation maintenance program.

9. A computer-readable storage medium storing a computer program, characterized in that, The modular robot formation maintenance program is stored on the readable storage medium, and when the modular robot formation maintenance program is executed by the processor, the steps of the modular robot formation maintenance method as described in any one of claims 1-6 are implemented.

Images