Modularized robot formation maintaining method, system and device and storage medium

Abstract

The invention relates to the technical field of robots, and particularly provides a modular robot formation maintenance method, system and device and a storage medium, and the method comprises the steps: constructing a plurality of formation diagrams according to task types, and distributing a module formation composed of a plurality of robots for each formation diagram; assigning a navigation robot for the module formation corresponding to each formation map, and sending the formation map to the corresponding navigation robot; the navigation robot decomposes the formation map into relative position parameters between the robots and issues the relative position parameters to the following robots in the same module formation; the following robot collects actual position parameters of the neighboring robot and adjusts motion parameters of the following robot according to the actual position parameters and the relative position parameters; the piloting robot is used for executing path planning and moving along a planned path. According to the invention, when the robot cannot acquire the global position data, the formation can still be maintained, and the stability of the robot formation is improved.

Description

Technical Field

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

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

[0003] Current research on formation control is mainly based on the global coordinate system and is implemented using the absolute position information of the robot.

[0004] However, in real life, robots cannot always obtain their own position information in the global coordinate system, such as in unknown indoor environments or dense forests and caves. In an environment where GPS signals are interfered, a distributed modular robot formation control solution based on relative position estimation is proposed to solve this problem, and multi-module robot formation control is realized in the local coordinate system of each robot. Summary of the invention

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

[0006] In a first aspect, the present invention provides a modular robot formation maintenance method, comprising: Construct multiple formation graphs according to the task type, and assign a module formation consisting of multiple robots to each formation graph; Assign a pilot robot to each module formation corresponding to the formation diagram, and send the formation diagram to the corresponding pilot robot; The leading robot decomposes the formation graph into relative position parameters between robots, and sends the relative position parameters to the following robots in the same module formation; The following robot collects actual position parameters of the neighboring robots, and adjusts its own motion parameters according to the actual position parameters and the relative position parameters; The pilot robot is used to perform path planning and move along the planned path.

[0007] In an optional embodiment, a plurality of formation graphs are constructed according to the task type, and a module formation consisting of a plurality of robots is assigned to each formation graph, including: Determine the formation type according to the task type, the formation types including triangle formation, diamond formation, chain formation, star formation, parallel line formation; Determine the topological structure of the module formation according to the number of robots and the type of formation, wherein the topological structure uses the robots as nodes; A formation graph of the module formation is constructed according to the topological structure.

[0008] In an optional implementation, constructing a formation graph of the module formation according to the topological structure includes: Define the formation graph according to the topology:

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

[0010] The Laplace matrix is ​​used to describe the characteristics of the formation graph, and the number of edges connected to each node is recorded as the degree , the degree matrix D is a diagonal matrix, expressed as:

[0011] Adjacency Matrix Represents the edges in the graph, confirming that the formation graph is an undirected graph. If the node and nodes If there is an edge between ;otherwise , the adjacency matrix is ​​expressed as:

[0012] Figure The Laplacian matrix of is defined as: .

[0013] In an optional embodiment, a pilot robot is designated for each module formation corresponding to a formation diagram, and the formation diagram is sent to the corresponding pilot robot, including: The maximum number of followers corresponding to each formation type is preset, and the maximum number of followers is the maximum number of follower robots led by a single leader robot; Determine the number of pilot robots according to the formation type adopted by the formation graph, the number of nodes of the formation graph, and the corresponding maximum number of followers; A corresponding number of pilot robots is assigned to a corresponding module formation according to the number of pilot robots.

[0014] In an optional embodiment, the leading robot decomposes the formation graph into relative position parameters between robots, and sends the relative position parameters to the following robots in the same module formation, including: Divide the Laplace matrix L of the formation graph into blocks according to the leader robot and the follower robot:

[0015] in, It is the connection between the pilot robots. is the connection between the following robots, It is the first type of connection between the pilot robot and the follower robot. It is the second type of connection between the leader robot and the follower robot; Let the formation satisfy the consistency condition: , where x is the robot position vector; the position vector is divided into the pilot robot position and follow the robot position , then:

[0016] Then the follower position:

[0017] The relative position parameter is Relative to The offset of

[0018] In an optional embodiment, the following robot collects actual position parameters of neighboring robots and adjusts its own motion parameters according to the actual position parameters and the relative position parameters, including: The following robot detects the actual position parameters between the leading robot and the neighboring robots through sensors on four sides; The following robot calculates the distance error between the leading robot and the neighboring robot according to the actual position parameter and the relative position parameter. and angular error , if the robot and The distance error and angle error are defined as follows:

[0019] in, , is the expected distance, is the desired angle, , is the actual distance, is the actual angle, and the positive direction of the angle is defined as the forward direction of the robot; Then the control law of the multi-robot system is expressed as:

[0020] in, and is the control gain, It's a robot Compared to robots The speed is used to adjust the robot speed so that it can communicate with the robot Maintain the desired relative speed; , are the control inputs, which are linear velocity and angular velocity respectively; From the robot Pointing robot The unit vector of , which determines how to adjust the robot's motion based on position and velocity;

[0021] In order to prevent speed saturation from causing the speed to exceed the upper limit, the speed limit is defined as shown in the following formula:

[0022] in , , , are the maximum and minimum values ​​of linear velocity and angular velocity respectively.

[0023] In an optional embodiment, the method further comprises: Defining Robots With a neighbor robot The potential function of :

[0024] robot For robots The force is the negative gradient of the potential function, that is:

[0025]

[0026] in, is the set collision distance, For a safe distance, For robots In Robot Relative position parameters in the coordinate system, , , is the proportionality coefficient; Considering the formation graph and the set of neighboring robots, the overall potential energy is:

[0027] The velocity is adjusted according to the overall potential energy using Newton's equations of motion:

[0028] Where m is the mass of the robot, To control the cycle; According to the relative posture relationship, if the distance is less than the threshold , providing an obstacle avoidance potential, allowing the robot Avoid robots A collision occurred.

[0029] In a second aspect, the present invention provides a modular robot formation maintenance system, comprising: A construction module, used for constructing a plurality of formation graphs according to the task type, and allocating a module formation consisting of a plurality of robots to each formation graph; A configuration module, used to designate a pilot robot for each module formation corresponding to a formation diagram, and send the formation diagram to the corresponding pilot robot; A first processing module is used for the leading robot to decompose the formation graph into relative position parameters between robots, and send the relative position parameters to the following robots in the same module formation; A second processing module is used for the following robot to collect actual position parameters of neighboring robots and adjust its own motion parameters according to the actual position parameters and the relative position parameters; The pilot robot is used to perform path planning and move along the planned path.

[0030] In a third aspect, a device is provided, including: A memory for storing a modular robot formation maintenance program; A processor is used to implement the steps of the modular robot formation maintenance method provided in the first aspect when executing the modular robot formation maintenance program.

[0031] In a fourth aspect, a computer-readable storage medium is provided, on which a modular robot formation maintenance program is stored, and 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.

[0032] The beneficial effects of the present invention are that the modular robot formation maintenance method, system, device and storage medium provided by the present invention reduce the size of the formation by constructing a modular formation, and then specify a pilot robot in the modular formation, and the rest are follower robots. The pilot robot can perform path planning, and the follower robot maintains the formation by detecting the actual position parameters between the robot and the neighbor robot. At this time, the follower robot does not need to obtain global position data to maintain the modular formation while following the pilot robot. In addition, by setting a unified obstacle avoidance potential function, the robot collision in the formation is avoided. The present invention realizes that when the robot cannot obtain global position data, the formation can still be maintained, thereby improving the stability of the robot formation.

[0033] In addition, the invention has a reliable design principle, a simple structure and a very broad application prospect. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0035] Figure 1 is a schematic flow chart of a method according to an embodiment of the present invention.

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

[0037] Figure 3 Schematic diagram of robot collision range of a method according to an embodiment of the present invention.

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

[0039] Figure 5 The figure is a robot formation obstacle avoidance effect diagram of a method according to an embodiment of the present invention.

[0040] Figure 6 is a schematic flow chart of a method according to another embodiment of the present invention.

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

[0042] Figure 8 A schematic diagram of the structure of a device provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0043] In order to enable those skilled in the art to better understand the technical solutions in the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

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

[0045] The modular robot formation maintenance method provided by the embodiment of the present invention is executed by a computer device, and accordingly, the modular robot formation maintenance system runs in the computer device.

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

[0047] like Figure 1 As shown, the method includes: S1. construct multiple formation graphs according to the task type, and assign a module formation consisting of multiple robots to each formation graph; S2. designate a pilot robot for each module formation corresponding to the formation diagram, and send the formation diagram to the corresponding pilot robot; S3. The pilot robot decomposes the formation diagram into relative position parameters between robots, and sends the relative position parameters to the follower robots in the same module formation; S4. The following robot collects 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; The pilot robot is used to perform path planning and move along the planned path.

[0048] In view of the fact that current research is mainly based on the global coordinate system, this paper proposes a behavior-based distributed formation control strategy, which uses the relative position information of the robots to achieve formation control. A distributed multi-robot obstacle avoidance method is proposed, which introduces relative posture perception information into the obstacle avoidance within the team. Finally, different basic behavior components are designed for the different roles of 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 posture perception information with behavior-based formation control.

[0049] The modular robot platform uses five Mecanum omnidirectional mobile modular robots. Each modular robot is equipped with a monocular camera and a cooperation mark on its front, back, left and right sides. ARTAG is used as the cooperation mark to define the identity ID of each modular robot. The relative pose estimation data between multiple robot members is obtained using UWB ranging sensors and ARTAG artificial marks.

[0050] On this basis, a formation control law based on relative posture is further proposed. In view of the problems existing in the traditional artificial potential field method, the repulsion and attraction functions are improved and a dynamic adjustment coefficient is introduced. At the same time, collision-free formation between robots is realized based on relative posture 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 performed in the local coordinate system of each robot.

[0051] In an embodiment of the present invention, based on step S1, a possible embodiment is given below to illustrate its specific implementation scheme in a non-limiting manner.

[0052] S101. Determine a formation type according to a task type, wherein the formation types include a triangle formation, a diamond formation, a chain formation, a star formation, and a parallel line formation.

[0053] 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 positions between robots according to the set formation library to adapt to different environments.

[0054] Establish a task attribute vector: [coverage range, collaboration intensity, movement speed, environment complexity, fault tolerance requirements].

[0055] Example task characteristics: Search and rescue: coverage (high), collaboration intensity (medium), movement speed (medium), environmental complexity (high), fault tolerance requirements (high); Transportation tasks: coverage (low), collaboration intensity (high), movement speed (low), environmental complexity (low), fault tolerance requirements (medium);

[0056] Using fuzzy logic reasoning system: Input: normalized value of task feature vector; Rule base: IF (high coverage AND medium collaboration intensity) THEN star formation; IF (high collaboration intensity AND low environment complexity) THEN triangle formation; Output: Formation type confidence matrix.

[0057] S102. Determine a topological structure of a module formation according to the number of robots and the type of formation, wherein the topological structure uses robots as nodes.

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

[0059] To set the structure extension policy: Chain expansion: When the number of robots is >2, a segmented chain structure is adopted, and the interval between segments is L=1.5d (d is the diameter of the robot); 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; Diamond expansion: The primitive structure includes 4 nodes forming a diamond with a side length of L. The expansion mode includes mirror expansion along the x / y axis with the center as the origin, keeping the distance between adjacent diamonds ≥ 2R.

[0060] Calculation method of geometric parameters of formation: Triangle Formation: Side length L = k * v max / ω max (k is the safety factor, v max / ω max is the maximum linear velocity / angular velocity); Center coordinates: (Σx i / 3, Σy i / 3); Parallel Line Formation: Line spacing D = 2R + 0.5m; The robot spacing per line is d = 1.2R.

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

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

[0063] Define the formation graph according to the topology:

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

[0065] The Laplace matrix is ​​used to describe the characteristics of the formation graph, and the number of edges connected to each node is recorded as the degree , the degree matrix D is a diagonal matrix, expressed as:

[0066] Adjacency Matrix Represents the edges in the graph, confirming that the formation graph is an undirected graph. If the node and nodes If there is an edge between ;otherwise , the adjacency matrix is ​​expressed as:

[0067] Figure The Laplacian matrix of is defined as: .

[0068] In an embodiment of the present invention, based on step S2, a possible embodiment is given below to illustrate its specific implementation scheme in a non-limiting manner.

[0069] S201. Pre-set the maximum number of followers corresponding to each formation type, where the maximum number of followers is the maximum number of follower robots led by a single pilot robot.

[0070] The maximum number of followers is determined based on: Communication constraints: The control range of each pilot is ≤2R (R is the communication radius); Kinematic constraints: follower acceleration response delay Δt≤0.5s; Collaboration complexity: The task processing capability of each pilot is Q=0.8C (C is the computing power of a single node).

[0071] Formation type - Maximum number of followers comparison table:

[0072] The formula for dynamic adjustment of the maximum number of followers is: current maximum number of followers = basic maximum number of followers × environmental complexity coefficient × (1 - current task density ÷ maximum task density).

[0073] Parameter Description: Basic maximum number of followers: The default maximum number of followers corresponding to the formation type (such as 2 for triangle).

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

[0075] Current task density: The quantitative value of the current task load (0~1).

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

[0077] S202. Determine the number of leading robots according to the formation type adopted by the formation graph, the number of nodes in the formation graph, and the corresponding maximum number of followers.

[0078] Basic calculation formula: Formula 1: Calculate the initial number of pilot robots. Divide the total number of robots by (the maximum number of followers for a single pilot plus 1) and round up to get the initial number of pilot robots. Formula expression: Initial number of pilots = round up (total number of robots ÷ (maximum number of followers for a single pilot + 1)).

[0079] Formula 2: The number of pilots after structural correction, multiply the initial pilot number by the formation structure correction coefficient (0.8 for triangle, 1.2 for star, 1.0 for other types), and then round up to get the adjusted number of pilots. Formula expression: Adjusted number of pilots = round up (initial number of pilots × formation structure correction coefficient).

[0080] Formula 3: Final number of pilots (including redundancy), multiply the adjusted number of pilots by 1.15 (reserving 15% redundancy), and then round up to get the final number of pilot robots. Formula expression: Final number of pilots = round up (adjusted number of pilots × 1.15).

[0081] Specifically, the number of pilot robots required is calculated based on the given total number of nodes, the maximum number of follower robots that a single pilot robot can lead, and the formation type. The specific steps are as follows: Set different formation structure correction coefficients according to different formation types. If the formation type is a triangle, the correction coefficient is 0.8; if it is a diamond, chain or parallel line formation, the correction coefficient is 1.0; if it is a star formation, the correction coefficient is 1.2. According to the input formation type, select the corresponding value from these coefficients as the formation structure correction coefficient for this calculation.

[0082] Divide the total number of nodes by the maximum number of follower robots that a single leader can lead plus 1, and then round up the result to get a preliminary number of leader robots. For example, if the total number of nodes is 10 and a single leader can lead 3 follower robots, then divide 10 by (3 + 1), which is 2.5, which is rounded up to 3.

[0083] Then, multiply the initial number of pilot robots just obtained by the previously selected formation structure correction coefficient, and then round up the multiplication result to obtain an adjusted number of pilot robots.

[0084] Finally, considering the need for 15% redundancy, multiply the adjusted number of pilot robots by 1.15, and then round up the result. The final value is the number of pilot robots required.

[0085] For example: 10 robots use a star formation, the basic maximum number of followers is 3, the environment complexity is simple (coefficient 1.0), and the current task density is 0.4. Calculation steps: Initial pilot number: 10÷(3+1)=2.5→round up to 3.

[0086] Structural correction: 3×1.2 (star coefficient) = 3.6 → round up to 4.

[0087] Redundancy adjustment: 4×1.15=4.6→round up to 5. Final result: 5 pilot robots are required.

[0088] S203. Assign a corresponding number of pilot robots to a corresponding module formation according to the number of pilot robots.

[0089] Allocation algorithm steps: Step 1: Initialization Broadcast node ID and initial position; Compute node degree (number of neighbors).

[0090] Step 2: Candidate Screening Filter nodes with degree ≥ k_min (k_min=3); Prioritize nodes with energy ≥ E_threshold (E_threshold=70%).

[0091] Step 3: Tiered Elections Layer 1: Select the node with the largest coverage as the main pilot; Layer 2: Select the node with the highest connectivity among the remaining nodes as the sub-pilot; Recurse until L_final quantity is met.

[0092] Step 4: Dynamic Adjustment Adjust the pilot robot in the formation diagram to ensure the normal operation of the formation. The specific steps are as follows: Traverse each node in the formation graph.

[0093] For each node, its energy status will be checked. If the energy of the node is less than 30% of the maximum energy, it means that the energy of the node is already low and may not be able to continue to perform the duties of the pilot robot well.

[0094] When it is found that the energy of a node is lower than 30% of the maximum energy, an operation of "promoting the backup node" will be performed, that is, one of the pre-prepared backup nodes will be selected to replace the low-energy node and become the new pilot robot.

[0095] After traversing all nodes in the formation graph, an updated formation graph is returned. The allocation of the leading robot in this updated formation graph has been adjusted according to the node energy situation.

[0096] In addition, when designating a pilot robot, the network communication quality of all robots in the module formation is obtained, and the robot with the best network communication quality is selected as the pilot robot.

[0097] In an embodiment of the present invention, based on step S3, a possible embodiment is given below to illustrate its specific implementation scheme in a non-limiting manner.

[0098] Divide the Laplace matrix L of the formation graph into blocks according to the leader robot and the follower robot:

[0099] in, It is the connection between the pilot robots. is the connection between the following robots, It is the first type of connection between the pilot robot and the follower robot. It is the second type of connection between the leader robot and the follower robot; Let the formation satisfy the consistency condition: , where x is the robot position vector; the position vector is divided into the pilot robot position and follow the robot position , then:

[0100] Then the follower position:

[0101] The relative position parameter is Relative to The offset of

[0102] Take the triangle formation as an example: The formation topology includes: leader: robot 1 (R1), followers: robot 2 (R2) and robot 3 (R3).

[0103] The connection relationship is: R1 connects R2 and R3, and R2 and R3 are interconnected.

[0104] The Laplace matrix is:

[0105] After chunking:

[0106] Calculate the follower relative position: Assume the pilot position is (is the coordinate origin), then:

[0107] R2 and R3 need to maintain a horizontal and vertical relative position of 1 / 3 unit with R1 respectively (the specific unit is determined by the formation scaling factor).

[0108] In an embodiment of the present invention, based on step S4, a possible embodiment is given below to illustrate its specific implementation scheme in a non-limiting manner.

[0109] The following robot detects the actual position parameters between the leading robot and the neighboring robots through sensors on four sides; The following robot calculates the distance error between the leading robot and the neighboring robot according to the actual position parameter and the relative position parameter. and angular error , if the robot and The distance error and angle error are defined as follows:

[0110] in, , is the expected distance, is the desired angle, , is the actual distance, is the actual angle, and the positive direction of the angle is defined as the forward direction of the robot; Then the control law of the multi-robot system is expressed as:

[0111] in, and is the control gain, It's a robot Compared to robots The speed is used to adjust the robot speed so that it can communicate with the robot Maintain the desired relative speed; , are the control inputs, which are linear velocity and angular velocity respectively; From the robot Pointing robot The unit vector of , which determines how to adjust the robot's motion based on position and velocity;

[0112] In order to prevent speed saturation from causing the speed to exceed the upper limit, the speed limit is defined as shown in the following formula:

[0113] in , , , are the maximum and minimum values ​​of linear velocity and angular velocity respectively.

[0114] The formation control law is based on the premise that the robot topology will not change, and only considers the formation and maintenance of the formation. Obstacle avoidance and formation switching need to be further considered in the future, and superimposed on the distributed formation controller. Each robot only relies on local information of relative pose estimation and simple control rules, which can achieve global consistency and stability of the entire multi-robot formation system. This method does not require global information. All measurement and control signals are developed in the local coordinate system of each robot. The global asymptotic convergence of the desired plane formation is achieved by using local relative position measurement. Each robot independently calculates its own control input based on the relative pose, realizing distributed formation control.

[0115] On the basis of the above embodiments, in order to further improve the reliability of the module formation, a feasible method is to set a unified obstacle avoidance rule, including: When a multi-robot system performs a formation task, each robot may cross paths with other robots, resulting in the risk of internal collision. In order to ensure the safe operation of the entire system, the robots must take effective measures to avoid internal collisions while avoiding external obstacles. That is, when the robots approach each other, they generate a mutually repelling speed to prevent collisions.

[0116] When the distance between the robots is reduced to a preset safety threshold, the robots will stop moving immediately to ensure the safety and stability of the formation. Figure 3 Demonstrated robot and robots The collision range between It's a safe distance. is the collision distance.

[0117] Define a collision distance for each robot A circle with a radius of is set as the robot's collision range. When other robots or obstacles enter this range, it is considered a potential collision and the corresponding obstacle avoidance strategy needs to be adopted. 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 robot The origin of the coordinate system is the robot location, and the robot The position is relative to the robot to be determined by the coordinate system.

[0118] In formation control, in order to accurately simulate and adjust the interaction between robots, two basic virtual forces are introduced: inter-robot repulsion and inter-robot attraction. Inter-robot repulsion will make adjacent robots move away from each other to avoid collisions; inter-robot attraction will make adjacent robots move closer to avoid formation dissolution. These two forces can adjust the potential energy function according to the desired distance between robots.

[0119] Defining Robots With a neighbor robot The potential function of :

[0120] robot For robots The force is the negative gradient of the potential function, that is:

[0121]

[0122] in, is the set collision distance, For a safe distance, For robots In Robot Relative position parameters in the coordinate system, , , is the proportionality coefficient; Considering the formation graph and the set of neighboring robots, the overall potential energy is:

[0123] The velocity is adjusted according to the overall potential energy using Newton's equations of motion:

[0124] Where m is the mass of the robot, To control the cycle; According to the relative posture relationship, if the distance is less than the threshold , providing an obstacle avoidance potential, allowing the robot Avoid robots A collision occurred. Make sure the robot While maintaining a safe distance, try to keep This method has small computational complexity, good real-time performance, simple structure, and can avoid collisions between robots while maintaining the formation.

[0125] This embodiment provides a pilot robot path generation; RRT* has good performance, but in unknown environments, the path optimization of RRT* may need to be updated frequently, which increases the computational burden and reduces the efficiency and real-time performance of the algorithm planning. In the process of random generation of the entire graph, lazy sampling is used to reduce invalid sampling points. Pre-calculate the insertion point to the root node length, and before If the optimal length is less than the optimal length, the insertion point is retained. Compare the current sampling point and the last sampling point If the distance to the target is closer, the sampling point is retained and the sampling is successful. If it is not satisfied, continue sampling until the above constraints are met. The constraint expression is:

[0126] The two constraints can filter out useless sampling points, avoid optimization and collision detection of these sampling points, reduce the growth range of the overall random search tree, improve the algorithm convergence speed, and have high real-time performance.

[0127] Multi-robot behavior decision design; In the multi-robot formation control framework, breaking down complex tasks into multiple simple subtasks helps to complete the entire work process more efficiently. Each subtask is responsible for generating a motion command and is assigned a corresponding priority based on the importance of the task. 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 robot to determine the final behavior of the robot. In this method, the instruction vector generated by each subtask is weighted according to its corresponding weight. All weighted vectors are accumulated to form an overall behavior response vector. In order to ensure the consistency and effectiveness of the behavior, the accumulated results also need to be normalized to ensure that the final behavior response vector conforms to the physical limitations of the actual movement.

[0128] The multi-robot formation of the present invention is based on the target navigation mode , robot obstacle avoidance mode And formation maintenance mode The target navigation motion behavior refers to the robot controlling the robot to move toward the target according to the mission target point. Generally, this behavior is triggered when the multi-robot system has formed the desired collective formation and needs to form a formation to move to the target point. After planning the robot's path, the path target points are sent to the robot. The pilot robot needs to calculate the robot's target linear velocity and angular velocity based on these path points.

[0129] In addition to considering the robot obstacle avoidance and intra-team obstacle avoidance mentioned above, the robot obstacle avoidance mode also takes into account the robot's obstacle avoidance through formation changes. If no obstacle is detected, the multi-robot system will form and maintain a predefined formation. When the robot encounters an obstacle, both the leading robot and the follower robot will perform robot obstacle avoidance, and the follower robot will use the traditional PID control algorithm to reconstruct the predefined formation. In order to enable the robot to better maintain the formation during the obstacle avoidance process, 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 leading robot or at least half of the followers detect a corridor, the formation will switch to a linear formation. When passing through the corridor, the linear formation will be restored to the starting formation, such as Figure 4 shown.

[0130] For the obstacle avoidance effect of module formation, please refer to Figure 5. Adjust the proportional coefficient of each motion mode based on experience, and set the priority of obstacle avoidance of the multi-robot system higher than formation maintenance, and obtain the robot The overall behavior of is shown below.

[0131]

[0132] in is the behavioral decision-making quantity of the navigator, is the follower’s behavior decision, , , They are the proportional coefficients of target navigation, robot obstacle avoidance and formation maintenance respectively.

[0133] Please refer to Figure 6 , this embodiment provides a robot formation maintenance method: 1. Obtain the position information between module robots The relative pose estimation data among the multi-robot members are obtained by using UWB ranging sensors, ARTAG artificial markers, and monocular cameras and cooperation markers installed on each side (front, back, left, and right sides) of the modular robot.

[0134] 2. Constructing the adjacency matrix A formation model is established based on the graph theory of multi-robot systems. The robots are regarded as nodes and their relative postures as edges, and an adjacency matrix is ​​constructed. At the same time, the Laplace matrix is ​​used to describe the characteristics of the graph (including the calculation of the degree matrix and the adjacency matrix) to define the topological structure of the formation graph.

[0135] 3. Determine whether it is a pilot robot If it is a pilot robot: execute pilot path generation, use Fast-RRT* algorithm, and generate the optimized path through constraints such as lazy sampling, comparison between insertion point and root node length, and comparison between sampling point and target distance.

[0136] If it is not the pilot robot: form a formation according to the formation control law based on relative posture, calculate the distance error and angle error with the pilot robot and neighboring robots, adjust its own linear speed and angular speed, and constrain the speed range through the speed limit formula.

[0137] 4. Determine whether obstacle avoidance is needed If you need to avoid obstacles: Use improved APF obstacle avoidance algorithm to handle external obstacles; Adopt an intra-team obstacle avoidance strategy (calculate the repulsion and attraction between computers based on the potential energy function to avoid robot collision and disbanding).

[0138] If obstacle avoidance is not required: Go directly to the next step.

[0139] 5. Behavioral decision design Split the task into subtasks such as target navigation, robot obstacle avoidance, and formation maintenance, and assign priorities (obstacle avoidance has a higher priority than formation maintenance).

[0140] Target navigation: The pilot robot calculates the target linear velocity and angular velocity based on the planned path.

[0141] Robot obstacle avoidance: The leading and following robots perform obstacle avoidance, and the following robots reconstruct the formation through PID control; when the environment changes (such as corridors), the formation is switched (such as linear formation).

[0142] Formation maintenance: Maintain or reconstruct the desired formation through formation control laws.

[0143] 6. Weighted Fusion The sub-behaviors such as target navigation, obstacle avoidance, and formation maintenance are weighted and fused according to the proportional coefficients, and the priority relationship is considered to obtain the overall behavior of the robot.

[0144] 7. Generate the final behavior vector The weighted fusion results are processed to generate the final behavior vector of the robot, which drives the robot to perform corresponding movements.

[0145] In some embodiments, the modular robot formation maintenance system may include a plurality of functional modules composed of computer program segments. The computer programs of the various program segments in the modular robot formation maintenance system may be stored in a memory of a computer device and executed by at least one processor to perform (see Figure 1 Description) Modular robotic formation maintenance capabilities.

[0146] 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 As 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 refers to a series of computer program segments that can be executed by at least one processor and can complete fixed functions, which are stored in a memory. In this embodiment, the functions of each module will be described in detail in subsequent embodiments.

[0147] A construction module, used for constructing a plurality of formation graphs according to the task type, and allocating a module formation consisting of a plurality of robots to each formation graph; A configuration module, used to designate a pilot robot for each module formation corresponding to a formation diagram, and send the formation diagram to the corresponding pilot robot; A first processing module is used for the leading robot to decompose the formation graph into relative position parameters between robots, and send the relative position parameters to the following robots in the same module formation; A second processing module is used for the following robot to collect actual position parameters of neighboring robots and adjust its own motion parameters according to the actual position parameters and the relative position parameters; The pilot robot is used to perform path planning and move along the planned path.

[0148] Figure 8 The modular robot formation maintenance method provided for the embodiment of the present application can be applied to equipment. It will be appreciated by those skilled in the art that the equipment structure involved in the embodiment of the present invention does not constitute a limitation on the equipment, and the equipment may include more or less components than shown, or combine certain components, or arrange different components. In an embodiment of the present invention, the equipment includes but is not limited to laptop computers, desktop computers, workbenches, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The equipment may also represent various forms of mobile devices, such as personal digital processing, 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 the present application described and / or required herein.

[0149] The device 800 may include: a processor 810, a memory 820 and a communication unit 830. These components communicate via one or more buses. Those skilled in the art will appreciate that the server structure shown in the figure does not limit the present invention, and it may be a bus structure or a star structure, and may include more or fewer components than shown in the figure, or combine certain components, or arrange the components differently.

[0150] 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 perform some or all of the steps in the following method embodiments.

[0151] The processor 810 is the control center of the storage device, and uses various interfaces and lines to connect various parts of the entire electronic device. It runs or executes software programs and / or modules stored in the memory 820, and calls data stored in the memory to perform various functions of the electronic device and / or process data. The processor can be composed of an integrated circuit (IC), for example, it can be composed of a single packaged IC, or it can be composed of a plurality of packaged ICs with the same or different functions. For example, the processor 810 can include only a central processing unit (CPU). In an embodiment of the present invention, the CPU can be a single computing core or multiple computing cores.

[0152] The communication unit 830 is used to establish a communication channel so that the storage device can communicate with other devices, receive user data sent by other devices or send user data to other devices.

[0153] The present invention also provides a computer storage medium, wherein the computer storage medium may store a program, and when the program is executed, the program may include some or all of the steps in each embodiment 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).

[0154] 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 this understanding, the technical solution 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, which 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 disk or an optical disk, and other media that can store program codes, including several instructions for enabling a computer device (which can be a personal computer, a server, or a second device, a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention.

[0155] In this specification, the same or similar parts between the various embodiments can be referred to each other. In particular, for the device embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the description in the method embodiment.

[0156] In the 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 only schematic. For example, the division of the modules is only a logical function division. There may be other division methods in actual implementation, such as 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 mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of systems or modules, which can be electrical, mechanical or other forms.

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

[0158] In addition, each functional module in each embodiment of the present invention may be integrated into one processing module, or each module may exist physically separately, or two or more modules may be integrated into one module.

[0159] Although the present invention has been described in detail with reference 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, a person of ordinary skill in the art may make various equivalent modifications or substitutions to the embodiments of the present invention, and these modifications or substitutions shall be within the scope of the present invention. Any person of ordinary skill in the art may easily think of changes or substitutions within the technical scope disclosed by the present invention, and these shall be within the scope of protection of the present invention.

Claims

1. A modular robot formation maintenance method, characterized in that: include: Construct multiple formation graphs according to the task type, and assign a module formation consisting of multiple robots to each formation graph; Assign a pilot robot to each module formation corresponding to the formation diagram, and send the formation diagram to the corresponding pilot robot; The leading robot decomposes the formation graph into relative position parameters between robots, and sends the relative position parameters to the following robots in the same module formation; The following robot collects actual position parameters of the neighboring robots, and adjusts its own motion parameters according to the actual position parameters and the relative position parameters; The pilot robot is used to perform path planning and move along the planned path.

2. The method according to claim 1, characterized in that Construct multiple formation graphs according to the task type, and assign a module formation consisting of multiple robots to each formation graph, including: Determine the formation type according to the task type, the formation types including triangle formation, diamond formation, chain formation, star formation, parallel line formation; Determine the topological structure of the module formation according to the number of robots and the type of formation, wherein the topological structure uses the robots as nodes; A formation graph of the module formation is constructed according to the topological structure.

3. The method according to claim 2, characterized in that Constructing a formation graph of the module formation according to the topological structure includes: Define the formation graph according to the topology: in is a set of vertices, representing a set of robot formations, is the set of edges; from the robot To the robot The directed edges represent robots In Robot Relative pose estimation in the coordinate system. In this case, the robot Known as a robot neighbor robot; then the robot The set of neighbor robots can be recorded as: The Laplace matrix is ​​used to describe the characteristics of the formation graph, and the number of edges connected to each node is recorded as the degree , the degree matrix D is a diagonal matrix, expressed as: Adjacency Matrix Represents the edges in the graph, confirming that the formation graph is an undirected graph. If the node and nodes If there is an edge between ;otherwise , the adjacency matrix is ​​expressed as: Figure The Laplacian matrix of is defined as: 。 4. The method according to claim 1, characterized in that: Assigning a pilot robot to each module formation corresponding to the formation diagram, and sending the formation diagram to the corresponding pilot robot, including: The maximum number of followers corresponding to each formation type is preset, and the maximum number of followers is the maximum number of follower robots led by a single leader robot; Determine the number of pilot robots according to the formation type adopted by the formation graph, the number of nodes of the formation graph, and the corresponding maximum number of followers; A corresponding number of pilot robots is assigned to a corresponding module formation according to the number of pilot robots.

5. The method according to claim 1, characterized in that The leading robot decomposes the formation graph into relative position parameters between robots, and sends the relative position parameters to the following robots in the same module formation, including: Divide the Laplace matrix L of the formation graph into blocks according to the leader robot and the follower robot: in, It is the connection between the pilot robots. is the connection between the following robots, It is the first type of connection between the pilot robot and the follower robot. It is the second type of connection between the leader robot and the follower robot; Let the formation satisfy the consistency condition: , where x is the robot position vector; the position vector is divided into the pilot robot position and follow the robot position , then: Then the follower position: The relative position parameter is Relative to The offset of 6. The method according to claim 1, characterized in that The following robot collects actual position parameters of neighboring robots and adjusts its own motion parameters according to the actual position parameters and the relative position parameters, including: The following robot detects the actual position parameters between the leading robot and the neighboring robots through sensors on four sides; The following robot calculates the distance error between the leading robot and the neighboring robot according to the actual position parameter and the relative position parameter. and angular error , if the robot and The distance error and angle error are defined as follows: in, , is the expected distance, is the desired angle, , is the actual distance, is the actual angle, and the positive direction of the angle is defined as the forward direction of the robot; Then the control law of the multi-robot system is expressed as: in, and is the control gain, It's a robot Compared to robots The speed is used to adjust the robot speed so that it can communicate with the robot Maintain the desired relative speed; , are the control inputs, which are linear velocity and angular velocity respectively; From the robot Pointing robot The unit vector of , which determines how to adjust the robot's motion based on position and velocity; In order to prevent speed saturation from causing the speed to exceed the upper limit, the speed limit is defined as shown in the following formula: in , , , are the maximum and minimum values ​​of linear velocity and angular velocity respectively.

7. The method according to claim 1, characterized in that The method further comprises: Defining Robots With a neighbor robot Potential function: robot For robots The force is the negative gradient of the potential function, that is: in, is the set collision distance, For a safe distance, For robots In Robot Relative position parameters in the coordinate system, , , is the proportionality coefficient; Considering the formation graph and the set of neighboring robots, the overall potential energy is: The velocity is adjusted according to the overall potential energy using Newton's equations of motion: Where m is the mass of the robot, To control the cycle; According to the relative posture relationship, if the distance is less than the threshold , providing an obstacle avoidance potential, allowing the robot Avoid robots A collision occurred.

8. A modular robot formation maintenance system, characterized in that: include: A construction module, used for constructing a plurality of formation graphs according to the task type, and allocating a module formation consisting of a plurality of robots to each formation graph; A configuration module, used to designate a pilot robot for each module formation corresponding to a formation diagram, and send the formation diagram to the corresponding pilot robot; A first processing module is used for the leading robot to decompose the formation graph into relative position parameters between robots, and send the relative position parameters to the following robots in the same module formation; A second processing module is used for the following robot to collect actual position parameters of neighboring robots and adjust its own motion parameters according to the actual position parameters and the relative position parameters; The pilot robot is used to perform path planning and move along the planned path.

9. A device, characterized in that: include: A memory for storing a modular robot formation maintenance program; A processor, used to implement the steps of the modular robot formation maintenance method as described in any one of claims 1-7 when executing the modular robot formation maintenance program.

10. A computer-readable storage medium storing a computer program, characterized in that: The readable storage medium stores a modular robot formation maintenance program, which, when executed by a processor, implements the steps of the modular robot formation maintenance method according to any one of claims 1 to 7.

Images